Set constraint link_name as the attached object

[captain-yoshi]

I want to add a constraint in one of my Connect stage. The constraint fails when the link_name is an attached object attached via the ModifyPlanningScene stage:

...
moveit_msgs::Constraints constraint;
constraint.name = "test";

Eigen::Quaterniond quat = Eigen::AngleAxisd(0, Eigen::Vector3d::UnitZ()) *
                          Eigen::AngleAxisd(M_PI_2, Eigen::Vector3d::UnitY()) *
                          Eigen::AngleAxisd(0, Eigen::Vector3d::UnitX());

moveit_msgs::OrientationConstraint oc;
oc.header.frame_id = "optical_table";
oc.link_name = "object_name";  // Object that is attached to the eef
oc.orientation.w = quat.w();
oc.orientation.x = quat.x();
oc.orientation.y = quat.y();
oc.orientation.z = quat.z();

oc.absolute_x_axis_tolerance = 2 * M_PI;
oc.absolute_y_axis_tolerance = 0.4;
oc.absolute_z_axis_tolerance = 0.4;
oc.weight = 100;
constraint.orientation_constraints.push_back(oc);

stage->setPathConstraints(std::move(constraint));
...

From moveit tutorials it is possible to set the end-effector link to be an object an even a subframe.

group.clearPoseTargets();
group.setEndEffectorLink(end_effector_link);
/*
group.setEndEffectorLink("cylinder/tip");    // Example 1
group.setEndEffectorLink("panda_hand");      // Example 2
*/
group.setStartStateToCurrentState();
group.setPoseTarget(pose);

When parsing the MTC library, I dont see the method setEndEffectorLink. Is this feature missing from MTC?

[v4hn]

Sorry for the late response.

Your code should "just work" but apparently it doesn't. This has nothing to do with any notion of "EndEffector" in the MoveGroupInterface code you quote. It might actually be a bug in your planner configuration. Do you have have the ResolveConstraintFrames adapter loaded in the planning pipeline?

[captain-yoshi]

My apologies. I made a mistake, the orientation constraint was not wrt the mesh. After the fix the planning solves correctly. But the console reports this errors on a successful solution for the connect stage:

[DEBUG] [1596556607.717987725]: Computing stage 'filtered'
[ INFO] [1596556607.719912078]: Planner configuration 'left_eef' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed.
[ INFO] [1596556607.720311533]: left_eef: Allocating specialized state sampler for state space
[ INFO] [1596556607.720422833]: left_eef/left_eef: Starting planning with 1 states already in datastructure
[ INFO] [1596556608.246790618]: left_eef/left_eef: Created 4 states (2 start + 2 goal)

[ INFO] [1596556608.246864900]: Solution found in 0.526790 seconds
[ INFO] [1596556608.450591070]: SimpleSetup: Path simplification took 0.203637 seconds and changed from 3 to 2 states
[ERROR] [1596556608.464468861]: Link 'cuvette' not found in model 'ur3_dual_eef'
[ WARN] [1596556608.464553573]: Could not find link model for link name cuvette

I guess that this is not really an error because a solution was found. But there are some solutions that does not respect the constraints. This happens when there appears to have multiple segments in one solution (See gif). I can open up another issue if you prefer.

The object cuvette mesh has the same orientation frame as the frame id optical_table (See pictures). Fixed code:

moveit_msgs::Constraints constraint;
constraint.name = "test";

Eigen::Quaterniond quat = Eigen::AngleAxisd(0, Eigen::Vector3d::UnitZ()) *
                          Eigen::AngleAxisd(0, Eigen::Vector3d::UnitY()) *
                          Eigen::AngleAxisd(0, Eigen::Vector3d::UnitX());

moveit_msgs::OrientationConstraint oc;
oc.header.frame_id = "optical_table";
oc.link_name = "cuvette";  // Object that is attached to the eef
oc.orientation.w = quat.w();
oc.orientation.x = quat.x();
oc.orientation.y = quat.y();
oc.orientation.z = quat.z();

oc.absolute_x_axis_tolerance = 0.4; // ~23 degrees
oc.absolute_y_axis_tolerance = 0.4; // ~23 degrees
oc.absolute_z_axis_tolerance = 2 * M_PI; // Represents the gravity axis (Pointing UP)
oc.weight = 100;
constraint.orientation_constraints.push_back(oc);

[v4hn]

If this actually talks about oc.link_name you are probably still missing the ResolveConstraintFrames adapter. To verify whether this is an MTC issue, try to reproduce this planning request in MoveIt (using the MoveGroupInterface). I would expect it will fail as well (which would mean this is not an issue with MTC, but with your setup or MoveIt general).

[captain-yoshi]

Do you have have the ResolveConstraintFrames adapter loaded in the planning pipeline?

Yes I do!

If this actually talks about oc.link_name

The only place I see not found in model when grepping the moveit repo is in the robot_model.cpp file. There is a good chance that this is related to theoc.link_name.

I would expect it will fail as well (which would mean this is not an issue with MTC, but with your setup or MoveIt general).

It does not fail with MTC. It gives good solutions. I will investigate this further and report back.

[v4hn]

There is a good chance that this is related to theoc.link_name.

You can easily check by changing the string and observing the warning. :)

It does not fail with MTC. It gives good solutions. I will investigate this further and report back.

If it also generates bad solutions it rather fails in my opinion. Of course it could also be some independent warning and the constraint planning works as it should...

[captain-yoshi]

If it also generates bad solutions it rather fails in my opinion.

I agree!

I would expect it will fail as well (which would mean this is not an issue with MTC, but with your setup or MoveIt general).

You are right! This has nothing to do with MTC. When adding path constraints in the Connect stage with a link_name from an attached object, MoveIt silence the ConstraintEvaluationResult to success. I will add a pull request by the end of the week. My initial fix fixes this issue, but I will do more testing.

On the other hand the ComputeIK stage can retrieve the attached object because it uses the getWorld function to calculate the transformation.

Backtrace for Connect stage

``` terminal (gdb) backtrace #0 moveit::core::RobotModel::getLinkModel (this=0x555556f0b730, name="cuvette", has_link=0x0) at /home/bob/ws/ros/catkin_ws/src/moveit/moveit_core/robot_model/src/robot_model.cpp:1162 #1 0x00007ffff3891c41 in moveit::core::RobotModel::getLinkModel (this=0x555556f0b730, name="cuvette", has_link=0x0) at /home/bob/ws/ros/catkin_ws/src/moveit/moveit_core/robot_model/src/robot_model.cpp:1137 #2 0x00007ffff2b3e686 in kinematic_constraints::OrientationConstraint::configure (this=0x55555d4f56d0, oc=..., tf=...) at /home/bob/ws/ros/catkin_ws/src/moveit/moveit_core/kinematic_constraints/src/kinematic_constraint.cpp:501 #3 0x00007ffff2b44f52 in kinematic_constraints::KinematicConstraintSet::add (this=0x7fffffff8880, oc=std::vector of length 1, capacity 1 = {...}, tf=...) at /home/bob/ws/ros/catkin_ws/src/moveit/moveit_core/kinematic_constraints/src/kinematic_constraint.cpp:1111 #4 0x00007ffff2b45241 in kinematic_constraints::KinematicConstraintSet::add (this=0x7fffffff8880, c=..., tf=...) at /home/bob/ws/ros/catkin_ws/src/moveit/moveit_core/kinematic_constraints/src/kinematic_constraint.cpp:1140 #5 0x00007ffff72d8da1 in planning_scene::PlanningScene::isPathValid (this=0x55555d005d30, trajectory=..., path_constraints=..., goal_constraints=std::vector of length 0, capacity 0, group="left_eef", verbose=false, invalid_index=0x7fffffff8a50) at /home/bob/ws/ros/catkin_ws/src/moveit/moveit_core/planning_scene/src/planning_scene.cpp:2170 #6 0x00007ffff72d941a in planning_scene::PlanningScene::isPathValid (this=0x55555d005d30, trajectory=..., path_constraints=..., group="left_eef", verbose=false, invalid_index=0x7fffffff8a50) at /home/bob/ws/ros/catkin_ws/src/moveit/moveit_core/planning_scene/src/planning_scene.cpp:2232 #7 0x00007fffe9e0465a in planning_pipeline::PlanningPipeline::generatePlan(std::shared_ptr const&, moveit_msgs::MotionPlanRequest_ > const&, planning_interface::MotionPlanResponse&, std::vector >&) const () from /home/bob/ws/ros/catkin_ws/devel/lib/libmoveit_planning_pipeline.so.1.1.0 #8 0x00007fffe9e057a1 in planning_pipeline::PlanningPipeline::generatePlan(std::shared_ptr const&, moveit_msgs::MotionPlanRequest_ > const&, planning_interface::MotionPlanResponse&) const () from /home/bob/ws/ros/catkin_ws/devel/lib/libmoveit_planning_pipeline.so.1.1.0 #9 0x00007fffedea4dba in moveit::task_constructor::solvers::PipelinePlanner::plan (this=0x5555570fa6e0, from=std::shared_ptr (use count 8, weak count 1) = {...}, to=std::shared_ptr (use count 3, weak count 1) = {...}, jmg=0x555556f0d310, timeout=5, result=std::shared_ptr (empty) = {...}, path_constraints=...) at /home/bob/ws/ros/catkin_ws/src/moveit_task_constructor/core/src/solvers/pipeline_planner.cpp:113 #10 0x00007fffed85e550 in moveit::task_constructor::stages::Connect::compute (this=0x55555718b0b0, from=..., to=...) at /home/bob/ws/ros/catkin_ws/src/moveit_task_constructor/core/src/stages/connect.cpp:157 #11 0x00007fffede31a8a in moveit::task_constructor::ConnectingPrivate::compute (this=0x5555570ee510) at /home/bob/ws/ros/catkin_ws/src/moveit_task_constructor/core/src/stage.cpp:672 #12 0x00007fffeddc4215 in moveit::task_constructor::StagePrivate::runCompute (this=0x5555570ee510) at /home/bob/ws/ros/catkin_ws/src/moveit_task_constructor/core/include/moveit/task_constructor/stage_p.h:152 #13 0x00007fffeddbe529 in moveit::task_constructor::WrapperBase::compute (this=0x555557100b30) at /home/bob/ws/ros/catkin_ws/src/moveit_task_constructor/core/src/container.cpp:696 #14 0x00007fffeddb9ac1 in moveit::task_constructor::ContainerBasePrivate::compute (this=0x5555570da480) at /home/bob/ws/ros/catkin_ws/src/moveit_task_constructor/core/src/container.cpp:106 #15 0x00007fffeddc4215 in moveit::task_constructor::StagePrivate::runCompute (this=0x5555570da480) at /home/bob/ws/ros/catkin_ws/src/moveit_task_constructor/core/include/moveit/task_constructor/stage_p.h:152 #16 0x00007fffeddbd113 in moveit::task_constructor::SerialContainer::compute (this=0x55555717f830) at /home/bob/ws/ros/catkin_ws/src/moveit_task_constructor/core/src/container.cpp:531 #17 0x00007fffeddb9ac1 in moveit::task_constructor::ContainerBasePrivate::compute (this=0x55555701a780) at /home/bob/ws/ros/catkin_ws/src/moveit_task_constructor/core/src/container.cpp:106 #18 0x00007fffeddc4215 in moveit::task_constructor::StagePrivate::runCompute (this=0x55555701a780) at /home/bob/ws/ros/catkin_ws/src/moveit_task_constructor/core/include/moveit/task_constructor/stage_p.h:152 #19 0x00007fffeddbd113 in moveit::task_constructor::SerialContainer::compute (this=0x555555f01c60) at /home/bob/ws/ros/catkin_ws/src/moveit_task_constructor/core/src/container.cpp:531 #20 0x00007fffeddb9ac1 in moveit::task_constructor::ContainerBasePrivate::compute (this=0x5555559da4d0) at /home/bob/ws/ros/catkin_ws/src/moveit_task_constructor/core/src/container.cpp:106 #21 0x00007fffeddc4215 in moveit::task_constructor::StagePrivate::runCompute (this=0x5555559da4d0) at /home/bob/ws/ros/catkin_ws/src/moveit_task_constructor/core/include/moveit/task_constructor/stage_p.h:152 #22 0x00007fffede48052 in moveit::task_constructor::Task::compute (this=0x5555570e9f80) at /home/bob/ws/ros/catkin_ws/src/moveit_task_constructor/core/src/task.cpp:285 #23 0x00007fffede48128 in moveit::task_constructor::Task::plan (this=0x5555570e9f80, max_solutions=30) at /home/bob/ws/ros/catkin_ws/src/moveit_task_constructor/core/src/task.cpp:297 #24 0x00007fffee365a18 in MtcPlanner::plan (this=0x7fffffffaa00, task_name="action0", c=..., max_solutions=30) at /home/bob/ws/ros/catkin_ws/src/bob/bob_planner/src/mtc/mtc_planner.cpp:655 #25 0x00007ffff5a92f49 in ProtocolPlanner::plan (this=0x7fffffffb640, action_block=std::vector of length 5, capacity 5 = {...}, max_solutions=30) at /home/bob/ws/ros/catkin_ws/src/bob/bob_planner/src/protocol_planner/protocol_planner.cpp:280 #26 0x00005555556657be in test_dual_eef_protocol2 () at /home/bob/ws/ros/catkin_ws/src/bob/bob_planner/src/pp.cpp:560 #27 0x000055555567fb82 in main (argc=1, argv=0x7fffffffc168) at /home/bob/ws/ros/catkin_ws/src/bob/bob_planner/src/pp.cpp:1970 ```

Backtrace for ComputeIk stage

``` terminal (gdb) backtrace #0 moveit::core::RobotModel::getLinkModel (this=0x555556f0b6f0, name="cuvette", has_link=0x7fffffff8b7e) at /home/bob/ws/ros/catkin_ws/src/moveit/moveit_core/robot_model/src/robot_model.cpp:1163 #1 0x00007ffff3891c41 in moveit::core::RobotModel::getLinkModel (this=0x555556f0b6f0, name="cuvette", has_link=0x7fffffff8b7e) at /home/bob/ws/ros/catkin_ws/src/moveit/moveit_core/robot_model/src/robot_model.cpp:1137 #2 0x00007ffff1b424e3 in moveit::core::RobotState::getFrameInfo (this=0x555557e38cb0, frame_id="cuvette", robot_link=@0x7fffffff8b80: 0x5555570a2670, frame_found=@0x7fffffff8b7e: false) at /home/bob/ws/ros/catkin_ws/src/moveit/moveit_core/robot_state/src/robot_state.cpp:1063 #3 0x00007ffff1b42118 in moveit::core::RobotState::getFrameTransform (this=0x555557e38cb0, frame_id="cuvette", frame_found=0x7fffffff8c4f) at /home/bob/ws/ros/catkin_ws/src/moveit/moveit_core/robot_state/src/robot_state.cpp:1040 #4 0x00007ffff72d6ff9 in planning_scene::PlanningScene::getFrameTransform (this=0x555556d1c290, state=..., frame_id="cuvette") at /home/bob/ws/ros/catkin_ws/src/moveit/moveit_core/planning_scene/src/planning_scene.cpp:1875 #5 0x00007ffff72d6f3e in planning_scene::PlanningScene::getFrameTransform (this=0x555556d1c290, frame_id="cuvette") at /home/bob/ws/ros/catkin_ws/src/moveit/moveit_core/planning_scene/src/planning_scene.cpp:1864 #6 0x00007fffed84ca2c in moveit::task_constructor::stages::ComputeIK::compute_ (this=0x55555712f720) at /home/bob/ws/ros/catkin_ws/src/moveit_task_constructor/core/src/stages/compute_ik.cpp:276 #7 0x00007fffed84ebdf in moveit::task_constructor::stages::ComputeIK::compute (this=0x55555712f720) at /home/bob/ws/ros/catkin_ws/src/moveit_task_constructor/core/src/stages/compute_ik.cpp:454 #8 0x00007fffeddb9ac1 in moveit::task_constructor::ContainerBasePrivate::compute (this=0x5555571b8530) at /home/bob/ws/ros/catkin_ws/src/moveit_task_constructor/core/src/container.cpp:106 #9 0x00007fffeddc4215 in moveit::task_constructor::StagePrivate::runCompute (this=0x5555571b8530) at /home/bob/ws/ros/catkin_ws/src/moveit_task_constructor/core/include/moveit/task_constructor/stage_p.h:152 #10 0x00007fffeddbe6eb in moveit::task_constructor::Alternatives::compute (this=0x55555719ff80) at /home/bob/ws/ros/catkin_ws/src/moveit_task_constructor/core/src/container.cpp:712 #11 0x00007fffeddb9ac1 in moveit::task_constructor::ContainerBasePrivate::compute (this=0x555557135d20) at /home/bob/ws/ros/catkin_ws/src/moveit_task_constructor/core/src/container.cpp:106 #12 0x00007fffeddc4215 in moveit::task_constructor::StagePrivate::runCompute (this=0x555557135d20) at /home/bob/ws/ros/catkin_ws/src/moveit_task_constructor/core/include/moveit/task_constructor/stage_p.h:152 #13 0x00007fffeddbd113 in moveit::task_constructor::SerialContainer::compute (this=0x5555570a0490) at /home/bob/ws/ros/catkin_ws/src/moveit_task_constructor/core/src/container.cpp:531 #14 0x00007fffeddb9ac1 in moveit::task_constructor::ContainerBasePrivate::compute (this=0x5555571c11c0) at /home/bob/ws/ros/catkin_ws/src/moveit_task_constructor/core/src/container.cpp:106 #15 0x00007fffeddc4215 in moveit::task_constructor::StagePrivate::runCompute (this=0x5555571c11c0) at /home/bob/ws/ros/catkin_ws/src/moveit_task_constructor/core/include/moveit/task_constructor/stage_p.h:152 #16 0x00007fffeddbd113 in moveit::task_constructor::SerialContainer::compute (this=0x555556d2a280) at /home/bob/ws/ros/catkin_ws/src/moveit_task_constructor/core/src/container.cpp:531 #17 0x00007fffeddb9ac1 in moveit::task_constructor::ContainerBasePrivate::compute (this=0x5555559da4d0) at /home/bob/ws/ros/catkin_ws/src/moveit_task_constructor/core/src/container.cpp:106 #18 0x00007fffeddc4215 in moveit::task_constructor::StagePrivate::runCompute (this=0x5555559da4d0) at /home/bob/ws/ros/catkin_ws/src/moveit_task_constructor/core/include/moveit/task_constructor/stage_p.h:152 #19 0x00007fffede48052 in moveit::task_constructor::Task::compute (this=0x555557196bf0) at /home/bob/ws/ros/catkin_ws/src/moveit_task_constructor/core/src/task.cpp:285 #20 0x00007fffede48128 in moveit::task_constructor::Task::plan (this=0x555557196bf0, max_solutions=30) at /home/bob/ws/ros/catkin_ws/src/moveit_task_constructor/core/src/task.cpp:297 #21 0x00007fffee365a18 in MtcPlanner::plan (this=0x7fffffffaa00, task_name="action0", c=..., max_solutions=30) at /home/bob/ws/ros/catkin_ws/src/bob/bob_planner/src/mtc/mtc_planner.cpp:655 #22 0x00007ffff5a92f49 in ProtocolPlanner::plan (this=0x7fffffffb640, action_block=std::vector of length 5, capacity 5 = {...}, max_solutions=30) at /home/bob/ws/ros/catkin_ws/src/bob/bob_planner/src/protocol_planner/protocol_planner.cpp:280 #23 0x00005555556657be in test_dual_eef_protocol2 () at /home/bob/ws/ros/catkin_ws/src/bob/bob_planner/src/pp.cpp:560 #24 0x000055555567fb82 in main (argc=1, argv=0x7fffffffc168) at /home/bob/ws/ros/catkin_ws/src/bob/bob_planner/src/pp.cpp:1970 ```

[v4hn]

I will add a pull request by the end of the week.

Thanks! Looking forward to review it.

On the other hand the ComputeIK stage can retrieve the attached object

yes, it's a very different code path.

[captain-yoshi]

I will add a pull request by the end of the week.

Basically what I wrote was something that resembles the ResolveConstraintFrames function before I fond out that the ResolveConstraintFrames is not always triggered! So for the same Connect stage the planning has completly different backtraces

Here is a backtrace that does not triggers the ResolveConstraintFrames

``` terminal (gdb) backtrace #0 moveit::core::RobotModel::getLinkModel (this=0x555556f0b730, name="cuvette", has_link=0x0) at /home/bob/ws/ros/catkin_ws/src/moveit/moveit_core/robot_model/src/robot_model.cpp:1162 #1 0x00007ffff3891c41 in moveit::core::RobotModel::getLinkModel (this=0x555556f0b730, name="cuvette", has_link=0x0) at /home/bob/ws/ros/catkin_ws/src/moveit/moveit_core/robot_model/src/robot_model.cpp:1137 #2 0x00007ffff2b3e686 in kinematic_constraints::OrientationConstraint::configure (this=0x55555d4f56d0, oc=..., tf=...) at /home/bob/ws/ros/catkin_ws/src/moveit/moveit_core/kinematic_constraints/src/kinematic_constraint.cpp:501 #3 0x00007ffff2b44f52 in kinematic_constraints::KinematicConstraintSet::add (this=0x7fffffff8880, oc=std::vector of length 1, capacity 1 = {...}, tf=...) at /home/bob/ws/ros/catkin_ws/src/moveit/moveit_core/kinematic_constraints/src/kinematic_constraint.cpp:1111 #4 0x00007ffff2b45241 in kinematic_constraints::KinematicConstraintSet::add (this=0x7fffffff8880, c=..., tf=...) at /home/bob/ws/ros/catkin_ws/src/moveit/moveit_core/kinematic_constraints/src/kinematic_constraint.cpp:1140 #5 0x00007ffff72d8da1 in planning_scene::PlanningScene::isPathValid (this=0x55555d005d30, trajectory=..., path_constraints=..., goal_constraints=std::vector of length 0, capacity 0, group="left_eef", verbose=false, invalid_index=0x7fffffff8a50) at /home/bob/ws/ros/catkin_ws/src/moveit/moveit_core/planning_scene/src/planning_scene.cpp:2170 #6 0x00007ffff72d941a in planning_scene::PlanningScene::isPathValid (this=0x55555d005d30, trajectory=..., path_constraints=..., group="left_eef", verbose=false, invalid_index=0x7fffffff8a50) at /home/bob/ws/ros/catkin_ws/src/moveit/moveit_core/planning_scene/src/planning_scene.cpp:2232 #7 0x00007fffe9e0465a in planning_pipeline::PlanningPipeline::generatePlan(std::shared_ptr const&, moveit_msgs::MotionPlanRequest_ > const&, planning_interface::MotionPlanResponse&, std::vector >&) const () from /home/bob/ws/ros/catkin_ws/devel/lib/libmoveit_planning_pipeline.so.1.1.0 #8 0x00007fffe9e057a1 in planning_pipeline::PlanningPipeline::generatePlan(std::shared_ptr const&, moveit_msgs::MotionPlanRequest_ > const&, planning_interface::MotionPlanResponse&) const () from /home/bob/ws/ros/catkin_ws/devel/lib/libmoveit_planning_pipeline.so.1.1.0 #9 0x00007fffedea4dba in moveit::task_constructor::solvers::PipelinePlanner::plan (this=0x5555570fa6e0, from=std::shared_ptr (use count 8, weak count 1) = {...}, to=std::shared_ptr (use count 3, weak count 1) = {...}, jmg=0x555556f0d310, timeout=5, result=std::shared_ptr (empty) = {...}, path_constraints=...) at /home/bob/ws/ros/catkin_ws/src/moveit_task_constructor/core/src/solvers/pipeline_planner.cpp:113 #10 0x00007fffed85e550 in moveit::task_constructor::stages::Connect::compute (this=0x55555718b0b0, from=..., to=...) at /home/bob/ws/ros/catkin_ws/src/moveit_task_constructor/core/src/stages/connect.cpp:157 #11 0x00007fffede31a8a in moveit::task_constructor::ConnectingPrivate::compute (this=0x5555570ee510) at /home/bob/ws/ros/catkin_ws/src/moveit_task_constructor/core/src/stage.cpp:672 #12 0x00007fffeddc4215 in moveit::task_constructor::StagePrivate::runCompute (this=0x5555570ee510) at /home/bob/ws/ros/catkin_ws/src/moveit_task_constructor/core/include/moveit/task_constructor/stage_p.h:152 #13 0x00007fffeddbe529 in moveit::task_constructor::WrapperBase::compute (this=0x555557100b30) at /home/bob/ws/ros/catkin_ws/src/moveit_task_constructor/core/src/container.cpp:696 #14 0x00007fffeddb9ac1 in moveit::task_constructor::ContainerBasePrivate::compute (this=0x5555570da480) at /home/bob/ws/ros/catkin_ws/src/moveit_task_constructor/core/src/container.cpp:106 #15 0x00007fffeddc4215 in moveit::task_constructor::StagePrivate::runCompute (this=0x5555570da480) at /home/bob/ws/ros/catkin_ws/src/moveit_task_constructor/core/include/moveit/task_constructor/stage_p.h:152 #16 0x00007fffeddbd113 in moveit::task_constructor::SerialContainer::compute (this=0x55555717f830) at /home/bob/ws/ros/catkin_ws/src/moveit_task_constructor/core/src/container.cpp:531 #17 0x00007fffeddb9ac1 in moveit::task_constructor::ContainerBasePrivate::compute (this=0x55555701a780) at /home/bob/ws/ros/catkin_ws/src/moveit_task_constructor/core/src/container.cpp:106 #18 0x00007fffeddc4215 in moveit::task_constructor::StagePrivate::runCompute (this=0x55555701a780) at /home/bob/ws/ros/catkin_ws/src/moveit_task_constructor/core/include/moveit/task_constructor/stage_p.h:152 #19 0x00007fffeddbd113 in moveit::task_constructor::SerialContainer::compute (this=0x555555f01c60) at /home/bob/ws/ros/catkin_ws/src/moveit_task_constructor/core/src/container.cpp:531 #20 0x00007fffeddb9ac1 in moveit::task_constructor::ContainerBasePrivate::compute (this=0x5555559da4d0) at /home/bob/ws/ros/catkin_ws/src/moveit_task_constructor/core/src/container.cpp:106 #21 0x00007fffeddc4215 in moveit::task_constructor::StagePrivate::runCompute (this=0x5555559da4d0) at /home/bob/ws/ros/catkin_ws/src/moveit_task_constructor/core/include/moveit/task_constructor/stage_p.h:152 #22 0x00007fffede48052 in moveit::task_constructor::Task::compute (this=0x5555570e9f80) at /home/bob/ws/ros/catkin_ws/src/moveit_task_constructor/core/src/task.cpp:285 #23 0x00007fffede48128 in moveit::task_constructor::Task::plan (this=0x5555570e9f80, max_solutions=30) at /home/bob/ws/ros/catkin_ws/src/moveit_task_constructor/core/src/task.cpp:297 #24 0x00007fffee365a18 in MtcPlanner::plan (this=0x7fffffffaa00, task_name="action0", c=..., max_solutions=30) at /home/bob/ws/ros/catkin_ws/src/bob/bob_planner/src/mtc/mtc_planner.cpp:655 #25 0x00007ffff5a92f49 in ProtocolPlanner::plan (this=0x7fffffffb640, action_block=std::vector of length 5, capacity 5 = {...}, max_solutions=30) at /home/bob/ws/ros/catkin_ws/src/bob/bob_planner/src/protocol_planner/protocol_planner.cpp:280 #26 0x00005555556657be in test_dual_eef_protocol2 () at /home/bob/ws/ros/catkin_ws/src/bob/bob_planner/src/pp.cpp:560 #27 0x000055555567fb82 in main (argc=1, argv=0x7fffffffc168) at /home/bob/ws/ros/catkin_ws/src/bob/bob_planner/src/pp.cpp:1970 ```

Here is a backtrace that triggers the ResolveConstraintFrames (Line 26)

``` terminal (gdb) backtrace #0 kinematic_constraints::OrientationConstraint::configure (this=0x55555d52edd0, state=..., oc=..., tf=...) at /home/bob/ws/ros/catkin_ws/src/moveit/moveit_core/kinematic_constraints/src/kinematic_constraint.cpp:539 #1 0x00007ffff2b50837 in kinematic_constraints::KinematicConstraintSet::add (this=0x55555d52ecc0, state=..., oc=std::vector of length 1, capacity 1 = {...}, tf=...) at /home/bob/ws/ros/catkin_ws/src/moveit/moveit_core/kinematic_constraints/src/kinematic_constraint.cpp:1176 #2 0x00007ffff2b50b37 in kinematic_constraints::KinematicConstraintSet::add (this=0x55555d52ecc0, state=..., c=..., tf=...) at /home/bob/ws/ros/catkin_ws/src/moveit/moveit_core/kinematic_constraints/src/kinematic_constraint.cpp:1206 #3 0x00007ffff72e432a in planning_scene::PlanningScene::isStateConstrained (this=0x55555d011f20, state=..., constr=..., verbose=false) at /home/bob/ws/ros/catkin_ws/src/moveit/moveit_core/planning_scene/src/planning_scene.cpp:2060 #4 0x00007ffff72e47b2 in planning_scene::PlanningScene::isStateValid (this=0x55555d011f20, state=..., constr=..., group="left_eef", verbose=false) at /home/bob/ws/ros/catkin_ws/src/moveit/moveit_core/planning_scene/src/planning_scene.cpp:2108 #5 0x00007fffa663ffeb in default_planner_request_adapters::FixStartStatePathConstraints::adaptAndPlan(boost::function const&, moveit_msgs::MotionPlanRequest_ > const&, planning_interface::MotionPlanResponse&)> const&, std::shared_ptr const&, moveit_msgs::MotionPlanRequest_ > const&, planning_interface::MotionPlanResponse&, std::vector >&) const () from /home/bob/ws/ros/catkin_ws/devel/lib/libmoveit_default_planning_request_adapter_plugins.so #6 0x00007fffe298ec43 in planning_request_adapter::PlanningRequestAdapter::adaptAndPlan (this=0x555556768a80, planner=warning: RTTI symbol not found for class 'std::_Sp_counted_deleter, std::allocator, (__gnu_cxx::_Lock_policy)2>' warning: RTTI symbol not found for class 'std::_Sp_counted_deleter, std::allocator, (__gnu_cxx::_Lock_policy)2>' std::shared_ptr (use count 2, weak count 0) = {...}, planning_scene=std::shared_ptr (use count 5, weak count 1) = {...}, req=..., res=..., added_path_index=std::vector of length 0, capacity 0) at /home/bob/ws/ros/catkin_ws/src/moveit/moveit_core/planning_request_adapter/src/planning_request_adapter.cpp:67 #7 0x00007fffe298edb2 in planning_request_adapter::(anonymous namespace)::callAdapter1 (adapter=0x555556768a80, planner=warning: RTTI symbol not found for class 'std::_Sp_counted_deleter, std::allocator, (__gnu_cxx::_Lock_policy)2>' warning: RTTI symbol not found for class 'std::_Sp_counted_deleter, std::allocator, (__gnu_cxx::_Lock_policy)2>' std::shared_ptr (use count 2, weak count 0) = {...}, planning_scene=std::shared_ptr (use count 5, weak count 1) = {...}, req=..., res=..., added_path_index=std::vector of length 0, capacity 0) at /home/bob/ws/ros/catkin_ws/src/moveit/moveit_core/planning_request_adapter/src/planning_request_adapter.cpp:90 #8 0x00007fffe2994ef0 in boost::_bi::list6, boost::_bi::value >, boost::arg<1>, boost::arg<2>, boost::arg<3>, boost::reference_wrapper > > >::operator() const&, std::shared_ptr const&, moveit_msgs::MotionPlanRequest_ > const&, planning_interface::MotionPlanResponse&, std::vector >&), boost::_bi::rrlist3 const&, moveit_msgs::MotionPlanRequest_ > const&, planning_interface::MotionPlanResponse&> > (this=0x55555d5256f8, f= @0x55555d5256f0: 0x7fffe298ed35 >&)>, a=...) at /usr/include/boost/bind/bind.hpp:592 #9 0x00007fffe29941d0 in boost::_bi::bind_t const&, std::shared_ptr const&, moveit_msgs::MotionPlanRequest_ > const&, planning_interface::MotionPlanResponse&, std::vector >&), boost::_bi::list6, boost::_bi::value >, boost::arg<1>, boost::arg<2>, boost::arg<3>, boost::reference_wrapper > > > >::operator() const&, moveit_msgs::MotionPlanRequest_ > const&, planning_interface::MotionPlanResponse&> (this=0x55555d5256f0, a1=std::shared_ptr (use count 5, weak count 1) = {...}, a2=..., a3=...) at /usr/include/boost/bind/bind.hpp:1330 #10 0x00007fffe29937a9 in boost::detail::function::function_obj_invoker3 const&, std::shared_ptr const&, moveit_msgs::MotionPlanRequest_ > const&, planning_interface::MotionPlanResponse&, std::vector >&), boost::_bi::list6, boost::_bi::value >, boost::arg<1>, boost::arg<2>, boost::arg<3>, boost::reference_wrapper > > > >, bool, std::shared_ptr const&, moveit_msgs::MotionPlanRequest_ > const&, planning_interface::MotionPlanResponse&>::invoke (function_obj_ptr=..., a0=std::shared_ptr (use count 5, weak count 1) = {...}, a1=..., a2=...) at /usr/include/boost/function/function_template.hpp:138 #11 0x00007fffa6638964 in default_planner_request_adapters::FixStartStateCollision::adaptAndPlan(boost::function const&, moveit_msgs::MotionPlanRequest_ > const&, planning_interface::MotionPlanResponse&)> const&, std::shared_ptr const&, moveit_msgs::MotionPlanRequest_ > const&, planning_interface::MotionPlanResponse&, std::vector >&) const () from /home/bob/ws/ros/catkin_ws/devel/lib/libmoveit_default_planning_request_adapter_plugins.so #12 0x00007fffe298f0fa in planning_request_adapter::(anonymous namespace)::callAdapter2 (adapter=0x5555571b0c20, planner=..., planning_scene=std::shared_ptr (use count 5, weak count 1) = {...}, req=..., res=..., added_path_index=std::vector of length 0, capacity 0) at /home/bob/ws/ros/catkin_ws/src/moveit/moveit_core/planning_request_adapter/src/planning_request_adapter.cpp:108 #13 0x00007fffe2995c50 in boost::_bi::list6, boost::_bi::value const&, moveit_msgs::MotionPlanRequest_ > const&, planning_interface::MotionPlanResponse&)> >, boost::arg<1>, boost::arg<2>, boost::arg<3>, boost::reference_wrapper > > >::operator() const&, moveit_msgs::MotionPlanRequest_ > const&, planning_interface::MotionPlanResponse&)> const&, std::shared_ptr const&, moveit_msgs::MotionPlanRequest_ > const&, planning_interface::MotionPlanResponse&, std::vector >&), boost::_bi::rrlist3 const&, moveit_msgs::MotionPlanRequest_ > const&, planning_interface::MotionPlanResponse&> >(boost::_bi::type, bool (*&)(planning_request_adapter::PlanningRequestAdapter const*, boost::function const&, moveit_msgs::MotionPlanRequest_ > const&, planning_interface::MotionPlanResponse&)> const&, std::shared_ptr const&, moveit_msgs::MotionPlanRequest_ > const&, planning_interface::MotionPlanResponse&, std::vector >&), boost::_bi::rrlist3 const&, moveit_msgs::MotionPlanRequest_ > const&, planning_interface::Mo---Type to continue, or q to quit--- tionPlanResponse&>&, long) (this=0x55555d526238, f= @0x55555d526230: 0x7fffe298f078 >&)>, a=...) at /usr/include/boost/bind/bind.hpp:592 #14 0x00007fffe2995098 in boost::_bi::bind_t const&, moveit_msgs::MotionPlanRequest_ > const&, planning_interface::MotionPlanResponse&)> const&, std::shared_ptr const&, moveit_msgs::MotionPlanRequest_ > const&, planning_interface::MotionPlanResponse&, std::vector >&), boost::_bi::list6, boost::_bi::value const&, moveit_msgs::MotionPlanRequest_ > const&, planning_interface::MotionPlanResponse&)> >, boost::arg<1>, boost::arg<2>, boost::arg<3>, boost::reference_wrapper > > > >::operator() const&, moveit_msgs::MotionPlanRequest_ > const&, planning_interface::MotionPlanResponse&>(std::shared_ptr const&, moveit_msgs::MotionPlanRequest_ > const&, planning_interface::MotionPlanResponse&) (this=0x55555d526230, a1=std::shared_ptr (use count 5, weak count 1) = {...}, a2=..., a3=...) at /usr/include/boost/bind/bind.hpp:1330 #15 0x00007fffe2994447 in boost::detail::function::function_obj_invoker3 const&, moveit_msgs::MotionPlanRequest_ > const&, planning_interface::MotionPlanResponse&)> const&, std::shared_ptr const&, moveit_msgs::MotionPlanRequest_ > const&, planning_interface::MotionPlanResponse&, std::vector >&), boost::_bi::list6, boost::_bi::value const&, moveit_msgs::MotionPlanRequest_ > const&, planning_interface::MotionPlanResponse&)> >, boost::arg<1>, boost::arg<2>, boost::arg<3>, boost::reference_wrapper > > > >, bool, std::shared_ptr const&, moveit_msgs::MotionPlanRequest_ > const&, planning_interface::MotionPlanResponse&>::invoke(boost::detail::function::function_buffer&, std::shared_ptr const&, moveit_msgs::MotionPlanRequest_ > const&, planning_interface::MotionPlanResponse&) ( function_obj_ptr=..., a0=std::shared_ptr (use count 5, weak count 1) = {...}, a1=..., a2=...) at /usr/include/boost/function/function_template.hpp:138 #16 0x00007fffa6634d80 in default_planner_request_adapters::FixStartStateBounds::adaptAndPlan(boost::function const&, moveit_msgs::MotionPlanRequest_ > const&, planning_interface::MotionPlanResponse&)> const&, std::shared_ptr const&, moveit_msgs::MotionPlanRequest_ > const&, planning_interface::MotionPlanResponse&, std::vector >&) const () from /home/bob/ws/ros/catkin_ws/devel/lib/libmoveit_default_planning_request_adapter_plugins.so #17 0x00007fffe298f0fa in planning_request_adapter::(anonymous namespace)::callAdapter2 (adapter=0x5555571923a0, planner=..., planning_scene=std::shared_ptr (use count 5, weak count 1) = {...}, req=..., res=..., added_path_index=std::vector of length 0, capacity 0) at /home/bob/ws/ros/catkin_ws/src/moveit/moveit_core/planning_request_adapter/src/planning_request_adapter.cpp:108 #18 0x00007fffe2995c50 in boost::_bi::list6, boost::_bi::value const&, moveit_msgs::MotionPlanRequest_ > const&, planning_interface::MotionPlanResponse&)> >, boost::arg<1>, boost::arg<2>, boost::arg<3>, boost::reference_wrapper > > >::operator() const&, moveit_msgs::MotionPlanRequest_ > const&, planning_interface::MotionPlanResponse&)> const&, std::shared_ptr const&, moveit_msgs::MotionPlanRequest_ > const&, planning_interface::MotionPlanResponse&, std::vector >&), boost::_bi::rrlist3 const&, moveit_msgs::MotionPlanRequest_ > const&, planning_interface::MotionPlanResponse&> >(boost::_bi::type, bool (*&)(planning_request_adapter::PlanningRequestAdapter const*, boost::function const&, moveit_msgs::MotionPlanRequest_ > const&, planning_interface::MotionPlanResponse&)> const&, std::shared_ptr const&, moveit_msgs::MotionPlanRequest_ > const&, planning_interface::MotionPlanResponse&, std::vector >&), boost::_bi::rrlist3 const&, moveit_msgs::MotionPlanRequest_ > const&, planning_interface::MotionPlanResponse&>&, long) (this=0x55555d5261f8, f= @0x55555d5261f0: 0x7fffe298f078 >&)>, a=...) at /usr/include/boost/bind/bind.hpp:592 #19 0x00007fffe2995098 in boost::_bi::bind_t const&, moveit_msgs::MotionPlanRequest_ > const&, planning_interface::MotionPlanResponse&)> const&, std::shared_ptr const&, moveit_msgs::MotionPlanRequest_ > const&, planning_interface::MotionPlanResponse&, std::vector >&), boost::_bi::list6, boost::_bi::value const&, moveit_msgs::MotionPlanRequest_ > const&, planning_interface::MotionPlanResponse&)> >, boost::arg<1>, boost::arg<2>, boost::arg<3>, boost::reference_wrapper > > > >::operator() const&, moveit_msgs::MotionPlanRequest_ > const&, planning_interface::MotionPlanResponse&>(std::shared_ptr const&, moveit_msgs::MotionPlanRequest_ > const&, planning_interface::MotionPlanResponse&) (this=0x55555d5261f0, a1=std::shared_ptr (use count 5, weak count 1) = {...}, a2=..., a3=...) at /usr/include/boost/bind/bind.hpp:1330 #20 0x00007fffe2994447 in boost::detail::function::function_obj_invoker3 const&, moveit_msgs::MotionPlanRequest_ > const&, planning_interface::MotionPlanResponse&)> const&, std::shared_ptr const&, moveit_msgs::MotionPlanRequest_ > const&, planning_interface::MotionPlanResponse&, std::vector >&), boost::_bi::list6, boost::_bi::value const&, moveit_msgs::MotionPlanRequest_ > const&, planning_interface::MotionPlanResponse&)> >, boost::arg<1>, boost::arg<2>, boost::arg<3>, boost::reference_wrapper > > > >, bool, std::shared_ptr const&, moveit_msgs::MotionPlanRequest_ > const&, planning_interface::MotionPlanResponse&>::invoke(boost::detail::function::function_buffer&, std::shared_ptr const&, moveit_msgs::MotionPlanRequest_ > const&, planning_interface::MotionPlanResponse&) ( function_obj_ptr=..., a0=std::shared_ptr (use count 5, weak count 1) = {...}, a1=..., a2=...) at /usr/include/boost/function/function_template.hpp:138 ---Type to continue, or q to quit--- #21 0x00007fffa6643136 in default_planner_request_adapters::FixWorkspaceBounds::adaptAndPlan(boost::function const&, moveit_msgs::MotionPlanRequest_ > const&, planning_interface::MotionPlanResponse&)> const&, std::shared_ptr const&, moveit_msgs::MotionPlanRequest_ > const&, planning_interface::MotionPlanResponse&, std::vector >&) const () from /home/bob/ws/ros/catkin_ws/devel/lib/libmoveit_default_planning_request_adapter_plugins.so #22 0x00007fffe298f0fa in planning_request_adapter::(anonymous namespace)::callAdapter2 (adapter=0x555557191eb0, planner=..., planning_scene=std::shared_ptr (use count 5, weak count 1) = {...}, req=..., res=..., added_path_index=std::vector of length 0, capacity 0) at /home/bob/ws/ros/catkin_ws/src/moveit/moveit_core/planning_request_adapter/src/planning_request_adapter.cpp:108 #23 0x00007fffe2995c50 in boost::_bi::list6, boost::_bi::value const&, moveit_msgs::MotionPlanRequest_ > const&, planning_interface::MotionPlanResponse&)> >, boost::arg<1>, boost::arg<2>, boost::arg<3>, boost::reference_wrapper > > >::operator() const&, moveit_msgs::MotionPlanRequest_ > const&, planning_interface::MotionPlanResponse&)> const&, std::shared_ptr const&, moveit_msgs::MotionPlanRequest_ > const&, planning_interface::MotionPlanResponse&, std::vector >&), boost::_bi::rrlist3 const&, moveit_msgs::MotionPlanRequest_ > const&, planning_interface::MotionPlanResponse&> >(boost::_bi::type, bool (*&)(planning_request_adapter::PlanningRequestAdapter const*, boost::function const&, moveit_msgs::MotionPlanRequest_ > const&, planning_interface::MotionPlanResponse&)> const&, std::shared_ptr const&, moveit_msgs::MotionPlanRequest_ > const&, planning_interface::MotionPlanResponse&, std::vector >&), boost::_bi::rrlist3 const&, moveit_msgs::MotionPlanRequest_ > const&, planning_interface::MotionPlanResponse&>&, long) (this=0x55555d5261b8, f= @0x55555d5261b0: 0x7fffe298f078 >&)>, a=...) at /usr/include/boost/bind/bind.hpp:592 #24 0x00007fffe2995098 in boost::_bi::bind_t const&, moveit_msgs::MotionPlanRequest_ > const&, planning_interface::MotionPlanResponse&)> const&, std::shared_ptr const&, moveit_msgs::MotionPlanRequest_ > const&, planning_interface::MotionPlanResponse&, std::vector >&), boost::_bi::list6, boost::_bi::value const&, moveit_msgs::MotionPlanRequest_ > const&, planning_interface::MotionPlanResponse&)> >, boost::arg<1>, boost::arg<2>, boost::arg<3>, boost::reference_wrapper > > > >::operator() const&, moveit_msgs::MotionPlanRequest_ > const&, planning_interface::MotionPlanResponse&>(std::shared_ptr const&, moveit_msgs::MotionPlanRequest_ > const&, planning_interface::MotionPlanResponse&) (this=0x55555d5261b0, a1=std::shared_ptr (use count 5, weak count 1) = {...}, a2=..., a3=...) at /usr/include/boost/bind/bind.hpp:1330 #25 0x00007fffe2994447 in boost::detail::function::function_obj_invoker3 const&, moveit_msgs::MotionPlanRequest_ > const&, planning_interface::MotionPlanResponse&)> const&, std::shared_ptr const&, moveit_msgs::MotionPlanRequest_ > const&, planning_interface::MotionPlanResponse&, std::vector >&), boost::_bi::list6, boost::_bi::value const&, moveit_msgs::MotionPlanRequest_ > const&, planning_interface::MotionPlanResponse&)> >, boost::arg<1>, boost::arg<2>, boost::arg<3>, boost::reference_wrapper > > > >, bool, std::shared_ptr const&, moveit_msgs::MotionPlanRequest_ > const&, planning_interface::MotionPlanResponse&>::invoke(boost::detail::function::function_buffer&, std::shared_ptr const&, moveit_msgs::MotionPlanRequest_ > const&, planning_interface::MotionPlanResponse&) ( function_obj_ptr=..., a0=std::shared_ptr (use count 5, weak count 1) = {...}, a1=..., a2=...) at /usr/include/boost/function/function_template.hpp:138 #26 0x00007fffa664679f in default_planner_request_adapters::ResolveConstraintFrames::adaptAndPlan(boost::function const&, moveit_msgs::MotionPlanRequest_ > const&, planning_interface::MotionPlanResponse&)> const&, std::shared_ptr const&, moveit_msgs::MotionPlanRequest_ > const&, planning_interface::MotionPlanResponse&, std::vector >&) const () from /home/bob/ws/ros/catkin_ws/devel/lib/libmoveit_default_planning_request_adapter_plugins.so #27 0x00007fffe298f0fa in planning_request_adapter::(anonymous namespace)::callAdapter2 (adapter=0x55555659cfe0, planner=..., planning_scene=std::shared_ptr (use count 5, weak count 1) = {...}, req=..., res=..., added_path_index=std::vector of length 0, capacity 0) at /home/bob/ws/ros/catkin_ws/src/moveit/moveit_core/planning_request_adapter/src/planning_request_adapter.cpp:108 #28 0x00007fffe2995c50 in boost::_bi::list6, boost::_bi::value const&, moveit_msgs::MotionPlanRequest_ > const&, planning_interface::MotionPlanResponse&)> >, boost::arg<1>, boost::arg<2>, boost::arg<3>, boost::reference_wrapper > > >::operator() const&, moveit_msgs::MotionPlanRequest_ > const&, planning_interface::MotionPlanResponse&)> const&, std::shared_ptr const&, moveit_msgs::MotionPlanRequest_ > const&, planning_interface::MotionPlanResponse&, std::vector >&), boost::_bi::rrlist3 const&, moveit_msgs::MotionPlanRequest_ > const&, planning_interface::MotionPlanResponse&> >(boost::_bi::type, bool (*&)(planning_request_adapter::PlanningRequestAdapter const*, boost::function const&, moveit_msgs::MotionPlanRequest_ > const&, planning_interface::MotionPlanResponse&)> const&, std::shared_ptr const&, moveit_msgs::MotionPlanRequest_ > const&, planning_interface::MotionPlanResponse&, std::vector >&), boost::_bi::rrlist3 const&, moveit_msgs::MotionPlanRequest_ > const&, planning_interface::MotionPlanResponse&>&, long) (this=0x55555d526178, f= @0x55555d526170: 0x7fffe298f078 >&)>, a=...) at /usr/include/boost/bind/bind.hpp:592 ---Type to continue, or q to quit--- #29 0x00007fffe2995098 in boost::_bi::bind_t const&, moveit_msgs::MotionPlanRequest_ > const&, planning_interface::MotionPlanResponse&)> const&, std::shared_ptr const&, moveit_msgs::MotionPlanRequest_ > const&, planning_interface::MotionPlanResponse&, std::vector >&), boost::_bi::list6, boost::_bi::value const&, moveit_msgs::MotionPlanRequest_ > const&, planning_interface::MotionPlanResponse&)> >, boost::arg<1>, boost::arg<2>, boost::arg<3>, boost::reference_wrapper > > > >::operator() const&, moveit_msgs::MotionPlanRequest_ > const&, planning_interface::MotionPlanResponse&>(std::shared_ptr const&, moveit_msgs::MotionPlanRequest_ > const&, planning_interface::MotionPlanResponse&) (this=0x55555d526170, a1=std::shared_ptr (use count 5, weak count 1) = {...}, a2=..., a3=...) at /usr/include/boost/bind/bind.hpp:1330 #30 0x00007fffe2994447 in boost::detail::function::function_obj_invoker3 const&, moveit_msgs::MotionPlanRequest_ > const&, planning_interface::MotionPlanResponse&)> const&, std::shared_ptr const&, moveit_msgs::MotionPlanRequest_ > const&, planning_interface::MotionPlanResponse&, std::vector >&), boost::_bi::list6, boost::_bi::value const&, moveit_msgs::MotionPlanRequest_ > const&, planning_interface::MotionPlanResponse&)> >, boost::arg<1>, boost::arg<2>, boost::arg<3>, boost::reference_wrapper > > > >, bool, std::shared_ptr const&, moveit_msgs::MotionPlanRequest_ > const&, planning_interface::MotionPlanResponse&>::invoke(boost::detail::function::function_buffer&, std::shared_ptr const&, moveit_msgs::MotionPlanRequest_ > const&, planning_interface::MotionPlanResponse&) ( function_obj_ptr=..., a0=std::shared_ptr (use count 5, weak count 1) = {...}, a1=..., a2=...) at /usr/include/boost/function/function_template.hpp:138 #31 0x00007fffa6645150 in default_planner_request_adapters::AddTimeOptimalParameterization::adaptAndPlan(boost::function const&, moveit_msgs::MotionPlanRequest_ > const&, planning_interface::MotionPlanResponse&)> const&, std::shared_ptr const&, moveit_msgs::MotionPlanRequest_ > const&, planning_interface::MotionPlanResponse&, std::vector >&) const () from /home/bob/ws/ros/catkin_ws/devel/lib/libmoveit_default_planning_request_adapter_plugins.so #32 0x00007fffe298f0fa in planning_request_adapter::(anonymous namespace)::callAdapter2 (adapter=0x555556480110, planner=..., planning_scene=std::shared_ptr (use count 5, weak count 1) = {...}, req=..., res=..., added_path_index=std::vector of length 0, capacity 0) at /home/bob/ws/ros/catkin_ws/src/moveit/moveit_core/planning_request_adapter/src/planning_request_adapter.cpp:108 #33 0x00007fffe2995c50 in boost::_bi::list6, boost::_bi::value const&, moveit_msgs::MotionPlanRequest_ > const&, planning_interface::MotionPlanResponse&)> >, boost::arg<1>, boost::arg<2>, boost::arg<3>, boost::reference_wrapper > > >::operator() const&, moveit_msgs::MotionPlanRequest_ > const&, planning_interface::MotionPlanResponse&)> const&, std::shared_ptr const&, moveit_msgs::MotionPlanRequest_ > const&, planning_interface::MotionPlanResponse&, std::vector >&), boost::_bi::rrlist3 const&, moveit_msgs::MotionPlanRequest_ > const&, planning_interface::MotionPlanResponse&> >(boost::_bi::type, bool (*&)(planning_request_adapter::PlanningRequestAdapter const*, boost::function const&, moveit_msgs::MotionPlanRequest_ > const&, planning_interface::MotionPlanResponse&)> const&, std::shared_ptr const&, moveit_msgs::MotionPlanRequest_ > const&, planning_interface::MotionPlanResponse&, std::vector >&), boost::_bi::rrlist3 const&, moveit_msgs::MotionPlanRequest_ > const&, planning_interface::MotionPlanResponse&>&, long) (this=0x55555d526138, f= @0x55555d526130: 0x7fffe298f078 >&)>, a=...) at /usr/include/boost/bind/bind.hpp:592 #34 0x00007fffe2995098 in boost::_bi::bind_t const&, moveit_msgs::MotionPlanRequest_ > const&, planning_interface::MotionPlanResponse&)> const&, std::shared_ptr const&, moveit_msgs::MotionPlanRequest_ > const&, planning_interface::MotionPlanResponse&, std::vector >&), boost::_bi::list6, boost::_bi::value const&, moveit_msgs::MotionPlanRequest_ > const&, planning_interface::MotionPlanResponse&)> >, boost::arg<1>, boost::arg<2>, boost::arg<3>, boost::reference_wrapper > > > >::operator() const&, moveit_msgs::MotionPlanRequest_ > const&, planning_interface::MotionPlanResponse&>(std::shared_ptr const&, moveit_msgs::MotionPlanRequest_ > const&, planning_interface::MotionPlanResponse&) (this=0x55555d526130, a1=std::shared_ptr (use count 5, weak count 1) = {...}, a2=..., a3=...) at /usr/include/boost/bind/bind.hpp:1330 #35 0x00007fffe2994447 in boost::detail::function::function_obj_invoker3 const&, moveit_msgs::MotionPlanRequest_ > const&, planning_interface::MotionPlanResponse&)> const&, std::shared_ptr const&, moveit_msgs::MotionPlanRequest_ > const&, planning_interface::MotionPlanResponse&, std::vector >&), boost::_bi::list6, boost::_bi::value const&, moveit_msgs::MotionPlanRequest_ > const&, planning_interface::MotionPlanResponse&)> >, boost::arg<1>, boost::arg<2>, boost::arg<3>, boost::reference_wrapper > > > >, bool, std::shared_ptr const&, moveit_msgs::MotionPlanRequest_ > const&, planning_interface::MotionPlanResponse&>::invoke(boost::detail::function::function_buffer&, std::shared_ptr const&, moveit_msgs::MotionPlanRequest_ > const&, planning_interface::MotionPlanResponse&) ( function_obj_ptr=..., a0=std::shared_ptr (use count 5, weak count 1) = {...}, a1=..., a2=...) at /usr/include/boost/function/function_template.hpp:138 #36 0x00007fffe299075c in boost::function3 const&, moveit_msgs::MotionPlanRequest_ > const&, planning_interface::MotionPlanResponse&>::operator() (this=0x7fffffff8940, a0=std::shared_ptr (use count 5, weak count 1) = {...}, a1=..., a2=...) at /usr/include/boost/function/function_template.hpp:760 #37 0x00007fffe298f783 in planning_request_adapter::PlanningRequestAdapterChain::adaptAndPlan (this=0x5555567687d0, planner=warning: RTTI symbol not found for class 'std::_Sp_counted_deleter, std::allocator, (__gnu_cxx::_Lock_policy)2>' warning: RTTI symbol not found for class 'std::_Sp_counted_deleter, std::allocator, (__gnu_cxx::_Lock_policy)2>' std::shared_ptr (use count 2, weak count 0) = {...}, planning_scene=std::shared_ptr (use count 5, weak count 1) = {...}, req=..., res=..., added_path_index=std::vector of length 0, capacity 0) at /home/bob/ws/ros/catkin_ws/src/moveit/moveit_core/planning_request_adapter/src/planning_request_adapter.cpp:152 #38 0x00007fffe9e0dfb9 in planning_pipeline::PlanningPipeline::generatePlan(std::shared_ptr const&, moveit_msgs::MotionPlanRequest_ > const&, planning_interface::MotionPlanResponse&, std::vector >&) const () from /home/bob/ws/ros/catkin_ws/devel/lib/libmoveit_planning_pipeline.so.1.1.0 #39 0x00007fffe9e0f7a1 in planning_pipeline::PlanningPipeline::generatePlan(std::shared_ptr const&, moveit_msgs::MotionPlanRequest_ > const&, planning_interface::MotionPlanResponse&) const () from /home/bob/ws/ros/catkin_ws/devel/lib/libmoveit_planning_pipeline.so.1.1.0 #40 0x00007fffedeaede4 in moveit::task_constructor::solvers::PipelinePlanner::plan (this=0x5555571677b0, from=std::shared_ptr (use count 5, weak count 1) = {...}, to=std::shared_ptr (use count 3, weak count 1) = {...}, jmg=0x555556cf9000, timeout=5, result=std::shared_ptr (empty) = {...}, path_constraints=...) at /home/bob/ws/ros/catkin_ws/src/moveit_task_constructor/core/src/solvers/pipeline_planner.cpp:113 #41 0x00007fffed868550 in moveit::task_constructor::stages::Connect::compute (this=0x555556f92af0, from=..., to=...) at /home/bob/ws/ros/catkin_ws/src/moveit_task_constructor/core/src/stages/connect.cpp:157 #42 0x00007fffede3ba9a in moveit::task_constructor::ConnectingPrivate::compute (this=0x5555570c51e0) at /home/bob/ws/ros/catkin_ws/src/moveit_task_constructor/core/src/stage.cpp:672 #43 0x00007fffeddce225 in moveit::task_constructor::StagePrivate::runCompute (this=0x5555570c51e0) at /home/bob/ws/ros/catkin_ws/src/moveit_task_constructor/core/include/moveit/task_constructor/stage_p.h:152 #44 0x00007fffeddc8539 in moveit::task_constructor::WrapperBase::compute (this=0x55555716dc00) at /home/bob/ws/ros/catkin_ws/src/moveit_task_constructor/core/src/container.cpp:696 #45 0x00007fffeddc3ad1 in moveit::task_constructor::ContainerBasePrivate::compute (this=0x55555719e2b0) at /home/bob/ws/ros/catkin_ws/src/moveit_task_constructor/core/src/container.cpp:106 #46 0x00007fffeddce225 in moveit::task_constructor::StagePrivate::runCompute (this=0x55555719e2b0) at /home/bob/ws/ros/catkin_ws/src/moveit_task_constructor/core/include/moveit/task_constructor/stage_p.h:152 #47 0x00007fffeddc7123 in moveit::task_constructor::SerialContainer::compute (this=0x555556cf3860) at /home/bob/ws/ros/catkin_ws/src/moveit_task_constructor/core/src/container.cpp:531 #48 0x00007fffeddc3ad1 in moveit::task_constructor::ContainerBasePrivate::compute (this=0x555557192be0) at /home/bob/ws/ros/catkin_ws/src/moveit_task_constructor/core/src/container.cpp:106 #49 0x00007fffeddce225 in moveit::task_constructor::StagePrivate::runCompute (this=0x555557192be0) at /home/bob/ws/ros/catkin_ws/src/moveit_task_constructor/core/include/moveit/task_constructor/stage_p.h:152 #50 0x00007fffeddc7123 in moveit::task_constructor::SerialContainer::compute (this=0x555556fad4f0) at /home/bob/ws/ros/catkin_ws/src/moveit_task_constructor/core/src/container.cpp:531 #51 0x00007fffeddc3ad1 in moveit::task_constructor::ContainerBasePrivate::compute (this=0x5555559da4d0) at /home/bob/ws/ros/catkin_ws/src/moveit_task_constructor/core/src/container.cpp:106 #52 0x00007fffeddce225 in moveit::task_constructor::StagePrivate::runCompute (this=0x5555559da4d0) at /home/bob/ws/ros/catkin_ws/src/moveit_task_constructor/core/include/moveit/task_constructor/stage_p.h:152 #53 0x00007fffede52062 in moveit::task_constructor::Task::compute (this=0x5555571a7a50) at /home/bob/ws/ros/catkin_ws/src/moveit_task_constructor/core/src/task.cpp:285 #54 0x00007fffede52138 in moveit::task_constructor::Task::plan (this=0x5555571a7a50, max_solutions=30) at /home/bob/ws/ros/catkin_ws/src/moveit_task_constructor/core/src/task.cpp:297 #55 0x00007fffee36fe0c in MtcPlanner::plan (this=0x7fffffffaa00, task_name="action0", c=..., max_solutions=30) at /home/bob/ws/ros/catkin_ws/src/bob/bob_planner/src/mtc/mtc_planner.cpp:663 #56 0x00007ffff5a9c339 in ProtocolPlanner::plan (this=0x7fffffffb640, action_block=std::vector of length 5, capacity 5 = {...}, max_solutions=30) at /home/bob/ws/ros/catkin_ws/src/bob/bob_planner/src/protocol_planner/protocol_planner.cpp:280 #57 0x00005555556657be in test_dual_eef_protocol2 () at /home/bob/ws/ros/catkin_ws/src/bob/bob_planner/src/pp.cpp:560 #58 0x000055555567fb82 in main (argc=1, argv=0x7fffffffc168) at /home/bob/ws/ros/catkin_ws/src/bob/bob_planner/src/pp.cpp:1970 ```

It seems that the adaptAndPlan is not always triggered depending on the adapter_chain_ variable...

Do you know why there are times that the ResolveConstraintFrames is not always triggered for the same Connect stage?

[v4hn]

Do you know why there are times that the ResolveConstraintFrames is not always triggered for the same Connect stage?

No. The adapter_chain_ is loaded from ROS parameters and should not change at any point in the lifetime of the pipeline. I suspect an issue in your setup (possibly you have multiple pipelines and one does not define a chain?) but there might be some underlying issue in MoveIt.

[captain-yoshi]

Do you know why there are times that the ResolveConstraintFrames is not always triggered for the same Connect stage?

The adapter_chain_ is loaded correctly. My mistake, the adaptAndPlan is always triggered.

There are 2 paths when planning using generatePlan:

1. isStateValid -> The OrientationConstraint msg link_name and orientation will be changed if the link_name is a sub-frame.
2. isPathValid -> The OrientationConstraint msg is not changed

After validating the State, it tries to validate the Path but if the OrientationConstraint link_name is a sub-frame, it fails to retrieve it.

For State validation the resolvedConstraints is applied, but for the Path validation it is not. Is the above the intended behavior?

[v4hn]

O.o No!

EDIT:

you might have found a shortcoming in the ResolveConstraintFrames adapter that should be fixed in the upstream package. The original authors likely forgot to apply and verify path constraints when they added the plugin.

Looking at the sources though, at first glance it seems to transform path constraints as expected.

[captain-yoshi]

Looking at the sources though, at first glance it seems to transform path constraints as expected.

Yes that is correct. But the planning_scene->isPathValid does not pass through the adapter.

[v4hn]

But planning_scene->isPathValid does not pass through the adapter.

That's true. Implementing the frame resolver here will also render the ResolveConstraintsFrame request adapter obsolete, because if you have to transform the request in the scope of that method anyway, you might as well do it before passing them into the pipeline...

Could you please create an issue and/or pull-request in the MoveIt repository?

[captain-yoshi]

Could you please create an issue and/or pull-request in the MoveIt repository?

Yes first thing in the morning!