mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
frame marker at ik frame
This commit is contained in:
parent
5b6a02d105
commit
c98f10478e
@ -226,6 +226,7 @@ void ComputeIK::onNewSolution(const SolutionBase &s)
|
|||||||
|
|
||||||
// determine IK link from ik_frame
|
// determine IK link from ik_frame
|
||||||
const robot_model::LinkModel* link = nullptr;
|
const robot_model::LinkModel* link = nullptr;
|
||||||
|
geometry_msgs::PoseStamped ik_pose_msg;
|
||||||
const boost::any& value = props.get("ik_frame");
|
const boost::any& value = props.get("ik_frame");
|
||||||
if (value.empty()) { // property undefined
|
if (value.empty()) { // property undefined
|
||||||
// determine IK link from eef/group
|
// determine IK link from eef/group
|
||||||
@ -234,8 +235,10 @@ void ComputeIK::onNewSolution(const SolutionBase &s)
|
|||||||
ROS_WARN_STREAM_NAMED("ComputeIK", "Failed to derive IK target link");
|
ROS_WARN_STREAM_NAMED("ComputeIK", "Failed to derive IK target link");
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
|
ik_pose_msg.header.frame_id = link->getName();
|
||||||
|
ik_pose_msg.pose.orientation.w = 1.0;
|
||||||
} else {
|
} else {
|
||||||
geometry_msgs::PoseStamped ik_pose_msg = boost::any_cast<geometry_msgs::PoseStamped>(value);
|
ik_pose_msg = boost::any_cast<geometry_msgs::PoseStamped>(value);
|
||||||
Eigen::Affine3d ik_pose;
|
Eigen::Affine3d ik_pose;
|
||||||
tf::poseMsgToEigen(ik_pose_msg.pose, ik_pose);
|
tf::poseMsgToEigen(ik_pose_msg.pose, ik_pose);
|
||||||
if (!(link = robot_model->getLinkModel(ik_pose_msg.header.frame_id))) {
|
if (!(link = robot_model->getLinkModel(ik_pose_msg.header.frame_id))) {
|
||||||
@ -255,25 +258,29 @@ void ComputeIK::onNewSolution(const SolutionBase &s)
|
|||||||
|
|
||||||
robot_state::RobotState& sandbox_state = sandbox_scene->getCurrentStateNonConst();
|
robot_state::RobotState& sandbox_state = sandbox_scene->getCurrentStateNonConst();
|
||||||
|
|
||||||
|
// markers used for failures
|
||||||
|
std::deque<visualization_msgs::Marker> failure_markers;
|
||||||
|
// frames at target pose and ik frame
|
||||||
|
rviz_marker_tools::appendFrame(failure_markers, target_pose_msg, 0.1, "ik frame");
|
||||||
|
rviz_marker_tools::appendFrame(failure_markers, ik_pose_msg, 0.1, "ik frame");
|
||||||
// visualize placed end-effector
|
// visualize placed end-effector
|
||||||
std::deque<visualization_msgs::Marker> placed_link_markers;
|
auto appender = [&failure_markers](visualization_msgs::Marker& marker, const std::string& name) {
|
||||||
auto appender = [&placed_link_markers](visualization_msgs::Marker& marker, const std::string& name) {
|
|
||||||
marker.ns = "ik target";
|
marker.ns = "ik target";
|
||||||
marker.color.a *= 0.5;
|
marker.color.a *= 0.5;
|
||||||
placed_link_markers.push_back(marker);
|
failure_markers.push_back(marker);
|
||||||
};
|
};
|
||||||
const auto& visualize_links = moveit::core::RobotModel::getRigidlyConnectedParentLinkModel(link)
|
const auto& link_to_visualize = moveit::core::RobotModel::getRigidlyConnectedParentLinkModel(link)
|
||||||
->getParentJointModel()->getDescendantLinkModels();
|
->getParentJointModel()->getDescendantLinkModels();
|
||||||
if (colliding) {
|
if (colliding) {
|
||||||
SubTrajectory solution;
|
SubTrajectory solution;
|
||||||
generateCollisionMarkers(sandbox_state, appender, visualize_links);
|
generateCollisionMarkers(sandbox_state, appender, link_to_visualize);
|
||||||
std::copy(placed_link_markers.begin(), placed_link_markers.end(), std::back_inserter(solution.markers()));
|
std::copy(failure_markers.begin(), failure_markers.end(), std::back_inserter(solution.markers()));
|
||||||
solution.setCost(std::numeric_limits<double>::infinity()); // mark solution as failure
|
solution.setCost(std::numeric_limits<double>::infinity()); // mark solution as failure
|
||||||
solution.setName("eef in collision");
|
solution.setName("eef in collision");
|
||||||
spawn(InterfaceState(sandbox_scene), std::move(solution));
|
spawn(InterfaceState(sandbox_scene), std::move(solution));
|
||||||
return;
|
return;
|
||||||
} else
|
} else
|
||||||
generateVisualMarkers(sandbox_state, appender, visualize_links);
|
generateVisualMarkers(sandbox_state, appender, link_to_visualize);
|
||||||
|
|
||||||
|
|
||||||
// determine joint values of robot pose to compare IK solution with for costs
|
// determine joint values of robot pose to compare IK solution with for costs
|
||||||
@ -323,8 +330,9 @@ void ComputeIK::onNewSolution(const SolutionBase &s)
|
|||||||
planning_scene::PlanningScenePtr scene = s.start()->scene()->diff();
|
planning_scene::PlanningScenePtr scene = s.start()->scene()->diff();
|
||||||
SubTrajectory solution;
|
SubTrajectory solution;
|
||||||
|
|
||||||
// frame at target pose
|
// frames at target pose and ik frame
|
||||||
rviz_marker_tools::appendFrame(solution.markers(), target_pose_msg, 0.1, "ik frame");
|
rviz_marker_tools::appendFrame(solution.markers(), target_pose_msg, 0.1, "ik frame");
|
||||||
|
rviz_marker_tools::appendFrame(solution.markers(), ik_pose_msg, 0.1, "ik frame");
|
||||||
|
|
||||||
if (succeeded)
|
if (succeeded)
|
||||||
// compute cost as distance to compare_pose
|
// compute cost as distance to compare_pose
|
||||||
@ -348,15 +356,12 @@ void ComputeIK::onNewSolution(const SolutionBase &s)
|
|||||||
planning_scene::PlanningScenePtr scene = s.start()->scene()->diff();
|
planning_scene::PlanningScenePtr scene = s.start()->scene()->diff();
|
||||||
SubTrajectory solution;
|
SubTrajectory solution;
|
||||||
|
|
||||||
// frame at target pose
|
|
||||||
rviz_marker_tools::appendFrame(solution.markers(), target_pose_msg, 0.1, "ik frame");
|
|
||||||
|
|
||||||
// mark solution as invalid
|
// mark solution as invalid
|
||||||
solution.setCost(std::numeric_limits<double>::infinity());
|
solution.setCost(std::numeric_limits<double>::infinity());
|
||||||
solution.setName("no IK found");
|
solution.setName("no IK found");
|
||||||
|
|
||||||
// ik target link placement
|
// ik target link placement
|
||||||
std::copy(placed_link_markers.begin(), placed_link_markers.end(), std::back_inserter(solution.markers()));
|
std::copy(failure_markers.begin(), failure_markers.end(), std::back_inserter(solution.markers()));
|
||||||
|
|
||||||
spawn(InterfaceState(scene), std::move(solution));
|
spawn(InterfaceState(scene), std::move(solution));
|
||||||
}
|
}
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user