mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
cleanup ComputeIK
- correctly check collision for target pose before doing IK - visualize failed collision check / failed IK
This commit is contained in:
parent
deb09b7dfd
commit
dd0d004052
@ -70,54 +70,33 @@ void ComputeIK::setIgnoreCollisions(bool flag)
|
||||
typedef std::vector<std::vector<double>> IKSolutions;
|
||||
|
||||
namespace {
|
||||
bool isValid(planning_scene::PlanningSceneConstPtr scene,
|
||||
bool ignore_collisions,
|
||||
IKSolutions* ik_solutions,
|
||||
robot_state::RobotState* state,
|
||||
const robot_model::JointModelGroup* jmg,
|
||||
const double* joint_positions){
|
||||
for (const auto& sol : *ik_solutions){
|
||||
if (jmg->distance(joint_positions, sol.data()) < 0.1)
|
||||
return false; // to close to already found solution
|
||||
}
|
||||
state->setJointGroupPositions(jmg, joint_positions);
|
||||
ik_solutions->emplace_back();
|
||||
state->copyJointGroupPositions(jmg, ik_solutions->back());
|
||||
|
||||
return ignore_collisions || !scene->isStateColliding(*state, jmg->getName());
|
||||
}
|
||||
|
||||
bool isTargetPoseColliding(const planning_scene::PlanningSceneConstPtr& scene,
|
||||
const robot_model::JointModelGroup* jmg,
|
||||
const Eigen::Affine3d &pose,
|
||||
Eigen::Affine3d pose,
|
||||
const std::string &link_name)
|
||||
{
|
||||
planning_scene::PlanningScenePtr sandbox_scene = scene->diff();
|
||||
robot_state::RobotState& robot_state = sandbox_scene->getCurrentStateNonConst();
|
||||
robot_state.updateStateWithLinkAt(link_name, pose, true);
|
||||
|
||||
// disable collision checking for parent links (except fixed links)
|
||||
auto& acm = sandbox_scene->getAllowedCollisionMatrixNonConst();
|
||||
// consider all rigidly connected parent links as well
|
||||
const robot_model::LinkModel* link = robot_state.getLinkModel(link_name);
|
||||
const robot_model::LinkModel* parent_link = link->getParentLinkModel();
|
||||
const robot_model::JointModel* joint = link->getParentJointModel();
|
||||
const robot_model::LinkModel* parent = robot_model::RobotModel::getRigidlyConnectedParentLinkModel(link);
|
||||
if (parent != link) // transform pose into pose suitable to place parent
|
||||
pose = pose * robot_state.getGlobalLinkTransform(link).inverse() * robot_state.getGlobalLinkTransform(parent);
|
||||
|
||||
// keep links rigidly attached to link_name
|
||||
std::vector<const std::string*> fixed_eef_links;
|
||||
while (parent_link && joint->getType() == robot_model::JointModel::FIXED) {
|
||||
fixed_eef_links.push_back(&parent_link->getName());
|
||||
link = parent_link;
|
||||
joint = link->getParentJointModel();
|
||||
parent_link = joint->getParentLinkModel();
|
||||
}
|
||||
// now parent_link is the first link that is not rigidly attached to link_name
|
||||
// place link at given pose
|
||||
robot_state.updateStateWithLinkAt(parent, pose);
|
||||
robot_state.updateCollisionBodyTransforms();
|
||||
|
||||
std::vector<const std::string*> pending_links;
|
||||
while (parent_link) {
|
||||
pending_links.push_back(&parent_link->getName());
|
||||
link = parent_link;
|
||||
joint = link->getParentJointModel();
|
||||
parent_link = joint->getParentLinkModel();
|
||||
// disable collision checking for parent links (except links fixed to root)
|
||||
auto& acm = sandbox_scene->getAllowedCollisionMatrixNonConst();
|
||||
std::vector<const std::string*> pending_links; // parent link names that might be rigidly connected to root
|
||||
while (parent) {
|
||||
pending_links.push_back(&parent->getName());
|
||||
link = parent;
|
||||
const robot_model::JointModel* joint = link->getParentJointModel();
|
||||
parent = joint->getParentLinkModel();
|
||||
|
||||
if (joint->getType() != robot_model::JointModel::FIXED) {
|
||||
for (const std::string* name : pending_links)
|
||||
@ -128,9 +107,8 @@ bool isTargetPoseColliding(const planning_scene::PlanningSceneConstPtr& scene,
|
||||
|
||||
// check collision with the world using the padded version
|
||||
collision_detection::CollisionRequest req;
|
||||
req.group_name = jmg->getName();
|
||||
collision_detection::CollisionResult res;
|
||||
scene->getCollisionWorld()->checkRobotCollision(req, res, *scene->getCollisionRobot(), robot_state, acm);
|
||||
scene->checkCollision(req, res, robot_state, acm);
|
||||
return res.collision;
|
||||
}
|
||||
|
||||
@ -157,28 +135,51 @@ void ComputeIK::onNewSolution(const SolutionBase &s)
|
||||
const moveit::core::JointModelGroup* jmg = group.empty() ? robot_model->getJointModelGroup(eef_jmg->getEndEffectorParentGroup().first)
|
||||
: robot_model->getJointModelGroup(group);
|
||||
|
||||
robot_state::RobotState& sandbox_state = sandbox_scene->getCurrentStateNonConst();
|
||||
const robot_model::LinkModel* link = sandbox_state.getLinkModel(link_name);
|
||||
if (!link) throw std::runtime_error("requested link '" + link_name + "' does not exist");
|
||||
|
||||
// compute target pose w.r.t. link_name
|
||||
geometry_msgs::PoseStamped target_pose_msg = props.get<geometry_msgs::PoseStamped>("target_pose");
|
||||
Eigen::Affine3d target_pose;
|
||||
tf::poseMsgToEigen(target_pose_msg.pose, target_pose);
|
||||
if (!target_pose_msg.header.frame_id.empty() && target_pose_msg.header.frame_id != link_name) {
|
||||
const Eigen::Affine3d& ref_pose = sandbox_scene->getFrameTransform(props.get<std::string>(target_pose_msg.header.frame_id));
|
||||
if(ref_pose.matrix().cwiseEqual(Eigen::Affine3d::Identity().matrix()).all())
|
||||
ROS_WARN("requested reference frame '%s' for target pose does not exist", target_pose_msg.header.frame_id.c_str());
|
||||
const Eigen::Affine3d& link_pose = sandbox_scene->getFrameTransform(link_name);
|
||||
if(link_pose.matrix().cwiseEqual(Eigen::Affine3d::Identity().matrix()).all())
|
||||
ROS_WARN("requested link frame '%s' does not exist", link_name.c_str());
|
||||
const robot_model::LinkModel* ref_link = sandbox_state.getLinkModel(target_pose_msg.header.frame_id);
|
||||
if (!ref_link) throw std::runtime_error("requested reference frame '" + target_pose_msg.header.frame_id + "' is not a robot link");
|
||||
|
||||
const Eigen::Affine3d link_pose = sandbox_state.getGlobalLinkTransform(link_name);
|
||||
const Eigen::Affine3d ref_pose = sandbox_state.getGlobalLinkTransform(target_pose_msg.header.frame_id);
|
||||
// transform target pose such that the link frame will reach there
|
||||
target_pose = target_pose * ref_pose.inverse() * link_pose;
|
||||
}
|
||||
|
||||
// validate ee group for collision
|
||||
if (isTargetPoseColliding(sandbox_scene, eef_jmg, target_pose, link_name)) {
|
||||
// validate placed link for collisions
|
||||
bool colliding = isTargetPoseColliding(sandbox_scene, eef_jmg, target_pose, link_name);
|
||||
if (colliding && !storeFailures()) {
|
||||
ROS_ERROR("eeg in collision");
|
||||
return;
|
||||
}
|
||||
|
||||
// visualize placed end-effector
|
||||
std::deque<visualization_msgs::Marker> placed_link_markers;
|
||||
auto appender = [&placed_link_markers](visualization_msgs::Marker& marker, const std::string& name) {
|
||||
marker.ns = "ik target";
|
||||
marker.color.a *= 0.5;
|
||||
placed_link_markers.push_back(marker);
|
||||
};
|
||||
const auto& visualize_links = moveit::core::RobotModel::getRigidlyConnectedParentLinkModel(link)
|
||||
->getParentJointModel()->getDescendantLinkModels();
|
||||
if (colliding) {
|
||||
SubTrajectory solution;
|
||||
generateCollisionMarkers(sandbox_state, appender, visualize_links);
|
||||
std::copy(placed_link_markers.begin(), placed_link_markers.end(), std::back_inserter(solution.markers()));
|
||||
solution.setCost(std::numeric_limits<double>::infinity()); // mark solution as failure
|
||||
spawn(InterfaceState(sandbox_scene), std::move(solution));
|
||||
return;
|
||||
} else
|
||||
generateVisualMarkers(sandbox_state, appender, visualize_links);
|
||||
|
||||
|
||||
// determine joint values of robot pose to compare IK solution with for costs
|
||||
std::vector<double> compare_pose;
|
||||
const std::string &compare_pose_name = props.get<std::string>("default_pose");
|
||||
@ -189,17 +190,23 @@ void ComputeIK::onNewSolution(const SolutionBase &s)
|
||||
} else
|
||||
sandbox_scene->getCurrentState().copyJointGroupPositions(jmg, compare_pose);
|
||||
|
||||
IKSolutions ik_solutions;
|
||||
robot_state::RobotState& sandbox_state = sandbox_scene->getCurrentStateNonConst();
|
||||
// prepare for marker frame usage
|
||||
target_pose_msg.header.frame_id = s.start()->scene()->getPlanningFrame();
|
||||
|
||||
const moveit::core::GroupStateValidityCallbackFn is_valid =
|
||||
std::bind(&isValid,
|
||||
sandbox_scene,
|
||||
props.get<bool>("ignore_collisions"),
|
||||
&ik_solutions,
|
||||
std::placeholders::_1,
|
||||
std::placeholders::_2,
|
||||
std::placeholders::_3);
|
||||
IKSolutions ik_solutions;
|
||||
bool ignore_collisions = props.get<bool>("ignore_collisions");
|
||||
auto isValid = [sandbox_scene, ignore_collisions, &ik_solutions]
|
||||
(robot_state::RobotState* state, const robot_model::JointModelGroup* jmg, const double* joint_positions) {
|
||||
for (const auto& sol : ik_solutions){
|
||||
if (jmg->distance(joint_positions, sol.data()) < 0.1)
|
||||
return false; // too close to already found solution
|
||||
}
|
||||
state->setJointGroupPositions(jmg, joint_positions);
|
||||
ik_solutions.emplace_back();
|
||||
state->copyJointGroupPositions(jmg, ik_solutions.back());
|
||||
|
||||
return ignore_collisions || !sandbox_scene->isStateColliding(*state, jmg->getName());
|
||||
};
|
||||
|
||||
uint32_t max_ik_solutions = props.get<uint32_t>("max_ik_solutions");
|
||||
bool tried_current_state_as_seed = false;
|
||||
@ -211,46 +218,52 @@ void ComputeIK::onNewSolution(const SolutionBase &s)
|
||||
sandbox_state.setToRandomPositions(jmg);
|
||||
tried_current_state_as_seed= true;
|
||||
|
||||
bool succeeded = sandbox_state.setFromIK(jmg, target_pose, link_name, 1, remaining_time, is_valid);
|
||||
size_t previous = ik_solutions.size();
|
||||
bool succeeded = sandbox_state.setFromIK(jmg, target_pose, link_name, 1, remaining_time, isValid);
|
||||
|
||||
auto now = std::chrono::steady_clock::now();
|
||||
remaining_time -= std::chrono::duration<double>(now - start_time).count();
|
||||
start_time = now;
|
||||
|
||||
planning_scene::PlanningSceneConstPtr scene = s.start()->scene();
|
||||
SubTrajectory solution;
|
||||
|
||||
// include markers from original solution
|
||||
std::copy(s.markers().begin(), s.markers().end(), std::back_inserter(solution.markers()));
|
||||
|
||||
// frame at target pose
|
||||
target_pose_msg.header.frame_id = scene->getPlanningFrame();
|
||||
rviz_marker_tools::appendFrame(solution.markers(), target_pose_msg, 0.1, "ik frame");
|
||||
|
||||
if (succeeded) {
|
||||
solution.setCost(s.cost() + jmg->distance(ik_solutions.back().data(), compare_pose.data()));
|
||||
if (succeeded || (storeFailures() && ik_solutions.size() > previous)) {
|
||||
// create a new scene for each solution as they will have different robot states
|
||||
planning_scene::PlanningScenePtr new_scene = s.start()->scene()->diff();
|
||||
robot_state::RobotState& robot_state = new_scene->getCurrentStateNonConst();
|
||||
robot_state.setJointGroupPositions(jmg, ik_solutions.back().data());
|
||||
scene = new_scene;
|
||||
planning_scene::PlanningScenePtr scene = s.start()->scene()->diff();
|
||||
SubTrajectory solution;
|
||||
|
||||
// robot model
|
||||
robot_state.updateLinkTransforms();
|
||||
auto appender = [&solution](visualization_msgs::Marker& marker, const std::string& name) {
|
||||
marker.ns = "ik solution";
|
||||
marker.color.a *= 0.5;
|
||||
solution.markers().push_back(marker);
|
||||
};
|
||||
generateVisualMarkers(robot_state, appender, jmg->getLinkModelNames());
|
||||
} else {
|
||||
solution.setCost(std::numeric_limits<double>::infinity());
|
||||
// frame at target pose
|
||||
rviz_marker_tools::appendFrame(solution.markers(), target_pose_msg, 0.1, "ik frame");
|
||||
|
||||
if (succeeded)
|
||||
// compute cost as distance to compare_pose
|
||||
solution.setCost(s.cost() + jmg->distance(ik_solutions.back().data(), compare_pose.data()));
|
||||
else // found an IK solution, but this was not valid
|
||||
solution.setCost(std::numeric_limits<double>::infinity());
|
||||
|
||||
// set scene's robot state
|
||||
robot_state::RobotState& robot_state = scene->getCurrentStateNonConst();
|
||||
robot_state.setJointGroupPositions(jmg, ik_solutions.back().data());
|
||||
|
||||
spawn(InterfaceState(scene), std::move(solution));
|
||||
}
|
||||
|
||||
spawn(InterfaceState(scene), std::move(solution));
|
||||
|
||||
if (!succeeded && max_ik_solutions == 1)
|
||||
break; // first and only attempt failed
|
||||
break; // first and only attempt failed
|
||||
}
|
||||
|
||||
if (ik_solutions.empty() && storeFailures()) { // failed to find any solution
|
||||
planning_scene::PlanningScenePtr scene = s.start()->scene()->diff();
|
||||
SubTrajectory solution;
|
||||
|
||||
// frame at target pose
|
||||
rviz_marker_tools::appendFrame(solution.markers(), target_pose_msg, 0.1, "ik frame");
|
||||
|
||||
// mark solution as invalid
|
||||
solution.setCost(std::numeric_limits<double>::infinity());
|
||||
|
||||
// ik target link placement
|
||||
std::copy(placed_link_markers.begin(), placed_link_markers.end(), std::back_inserter(solution.markers()));
|
||||
|
||||
spawn(InterfaceState(scene), std::move(solution));
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
Loading…
Reference in New Issue
Block a user