mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
ComputeIK: forward child solution's comment
This commit is contained in:
parent
c70db65578
commit
4860462076
@ -298,7 +298,7 @@ void ComputeIK::onNewSolution(const SolutionBase &s)
|
|||||||
std::copy(failure_markers.begin(), failure_markers.end(), std::back_inserter(solution.markers()));
|
std::copy(failure_markers.begin(), failure_markers.end(), std::back_inserter(solution.markers()));
|
||||||
solution.markAsFailure();
|
solution.markAsFailure();
|
||||||
// TODO: visualize collisions
|
// TODO: visualize collisions
|
||||||
solution.setComment("eef in collision: " + listCollisionPairs(collisions.contacts, ", "));
|
solution.setComment(s.comment() + " eef in collision: " + listCollisionPairs(collisions.contacts, ", "));
|
||||||
spawn(InterfaceState(sandbox_scene), std::move(solution));
|
spawn(InterfaceState(sandbox_scene), std::move(solution));
|
||||||
return;
|
return;
|
||||||
} else
|
} else
|
||||||
@ -351,6 +351,7 @@ void ComputeIK::onNewSolution(const SolutionBase &s)
|
|||||||
// create a new scene for each solution as they will have different robot states
|
// create a new scene for each solution as they will have different robot states
|
||||||
planning_scene::PlanningScenePtr scene = s.start()->scene()->diff();
|
planning_scene::PlanningScenePtr scene = s.start()->scene()->diff();
|
||||||
SubTrajectory solution;
|
SubTrajectory solution;
|
||||||
|
solution.setComment(s.comment());
|
||||||
|
|
||||||
// frames at target pose and ik frame
|
// 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");
|
||||||
@ -384,7 +385,7 @@ void ComputeIK::onNewSolution(const SolutionBase &s)
|
|||||||
SubTrajectory solution;
|
SubTrajectory solution;
|
||||||
|
|
||||||
solution.markAsFailure();
|
solution.markAsFailure();
|
||||||
solution.setComment("no IK found");
|
solution.setComment(s.comment() + " no IK found");
|
||||||
|
|
||||||
// ik target link placement
|
// ik target link placement
|
||||||
std::copy(failure_markers.begin(), failure_markers.end(), std::back_inserter(solution.markers()));
|
std::copy(failure_markers.begin(), failure_markers.end(), std::back_inserter(solution.markers()));
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user