Different frame_id when calculating Pilz CIRC distance between start-center and goal-center

[captain-yoshi]

I have this MoveTo sequence using Pilz with a CIRC (center) motion

{
    pilz_planner->setPlannerId("CIRC");
    pilz_planner->setProperty("max_acceleration_scaling_factor", max_acceleration_scaling_factor);
    pilz_planner->setProperty("max_velocity_scaling_factor", max_velocity_scaling_factor);

    auto stage = std::make_unique<stages::MoveTo>("pilz", pilz_planner);
    stage->setGroup(ARM_GROUP_NAME);
    stage->setGoal(goal);
    stage->setPathConstraints(path_constraints);
    t.add(std::move(stage));
}

with this goal

pose_goal:
  header:
    frame_id: "newport_tabletop"
  pose:
    position: [-0.38752, -0.23781, 0.3799]
    orientation: [0.1727, 0.85905, -0.30889, 0.36982]

path_constraints:
  name: "center"
  position_constraints:
    - header:
        frame_id: "newport_tabletop"
      link_name: "robotiq_2f140_tcp"
      constraint_region:
        primitive_poses:
          - position: [-0.28223, -0.23781, 0.28258]
            orientation: [0, 0, 0, 1]

and some TF and visualizations.

The goal pose goal frame_id will be changed from newport_tabletop to tabletop which is constructed from the constructGoalConstraints function. But the path constraints frame_id will remain to newport_tabletop. Pilz will then compare the distance from the center of circle with the goal and the start, but not in the same frame, thus throwing this error : Distances between start-center and goal-center are different. A circle cannot be created.

This error does not necessarily belong to MTC, but if you construct your goals wrt. the root of the robot model, should the path constraints also be changed to the same frame ? Maybe this change should be done in MoveIt. At least checking that each frame is wrt. the same frames should be done in the pilz_industrial_motion_planner package.

[captain-yoshi]

Seems to be related to moveit/moveit#3035.

[DaniGarciaLopez]

Hi @captain-yoshi, I'm getting the same error with a CIRC motion when setting the constraint in a different frame. Any updates?

[captain-yoshi]

Sorry for the delay... Getting back from a long internship!

I think the best approach would be to fix the behavior in the Pilz Industrial Motion Planner. Will make a PR in the weeks to come.

[rhaschke]

@captain-yoshi, I think moveit/moveit#3519 tackles that?

[captain-yoshi]

Ah yes! It's only missing the CIRC implementation. I will test the PR once it is finished.

[rhaschke]

I think he attempts to do the CIRC implementation as well. At least I have seen changes to corresponding files too.

[captain-yoshi]

After reading into moveit/moveit#3519, I think it does not does not tackle this bug.

Here is where the error is triggered:

KDL::Path PathCircleGenerator::circleFromCenter(
                const KDL::Frame& start_pose,    // wrt. the planning frame
                const KDL::Frame& goal_pose,     // wrt. the planning frame
                const KDL::Vector& center_point, // potentially wrt. to an alternate frame
                double eqradius)
{
  double a = (start_pose.p - center_point).Norm();
  double b = (goal_pose.p - center_point).Norm();
  double c = (start_pose.p - goal_pose.p).Norm();

  if (fabs(a - b) > MAX_RADIUS_DIFF)
  {
    throw ErrorMotionPlanningCenterPointDifferentRadius();
  }
  ...

When the center of circle (center point) is wrt. the planning frame it succeeds. If not, then you could have something like this which should work but does not.

The solution would be to check if the start, goal and center point are all wrt. the same frame (should be the planning frame). If not, change the frame to be wrt. the planning frame.

[rhaschke]

When the center of circle (center point) is wrt. the planning frame it succeeds. If not, then you could have something like this which should work but does not.

The method assumes that all entities (start_pose, goal_pose, and center_point) are represented w.r.t. the same frame.
This cannot work if they are represented in different frames.

[captain-yoshi]

The method assumes that all entities (start_pose, goal_pose, and center_point) are represented w.r.t. the same frame.
This cannot work if they are represented in different frames.

They are represented in the same frame (not the planning frame) at the user level ! Down the road the start and goal are changed to the planning frame, but not the center point.

I consider this a bug. Either we add a check/transform in the Pilz section or we transform the path constraints frames wrt. the planning frame somewhere in MoveIt and/or MTC ?

[rhaschke]

They are represented in the same frame (not the planning frame) at the user level! Down the road the start and goal are changed to the planning frame, but not the center point.

If the frames are converted, the center point should be too. Did you find where the frames are converted?

[captain-yoshi]

Yup. Only the start pose is transformed in the Pilz for cartesian constraints. The center point is never transformed in Pilz, the planning frame is assumed. The goal pose is never transformed in Pilz. Obviously, there is no need to transform from a link that can change after computing the goal joints with the IK. But all the other transforms, that are fixed and before the first active joint, are perfectly valid.

The goal pose is transformed in the MTC move_to stage wrt. the planning frame.

Have already fixed these issues, will add tests this weekend.

[captain-yoshi]

@DaniGarciaLopez This should now work as expected. It is working on my end.

[rhaschke]

Fixed via moveit/moveit#3522