Skip to content

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

@captain-yoshi

Description

@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.

Metadata

Metadata

Assignees

No one assigned

    Labels

    No labels
    No labels

    Type

    No type

    Projects

    No projects

    Milestone

    No milestone

    Relationships

    None yet

    Development

    No branches or pull requests

    Issue actions