Description

When planning a LIN motion using a subframe as the link_name in constraints, the motion taken by the end link is linear, rather than the motion taken by the subframe (which is often nonlinear)

Your environment

• ROS Distro: Noetic
• OS Version: e.g. Ubuntu 20.04 (focal)
• Source or Binary build? Source (noetic-devel) or Docker Image (moveit/moveit:noetic-release)

Steps to reproduce

The problem can be reproduced as follows:

• Add the default_planner_request_adapters/ResolveConstraintFrames adapter to pilz_industrial_motion_planner.yaml
• Load a robot
• Attach a collision object to the robot
• Add a subframe to the collision object
• Build a GetMotionSequence request with the following properties:
  • First point as the current pose
  • Second point with a position constraint and orientation constraint using the subframe as the link_name
    • I used a 0.1m offset along x and -90 degree rotation around x relative to the subframe
  • Planner: pilz_industrial_motion_planner, LIN
• Call the PlanMotionSequence service

Expected behaviour

The motion should move the subframe from the start pose to the end pose with the subframe taking a linear path. I managed to emulate the desired behaviour by planning a PTP path between the same start and end goal, inserting 100 extra samples of the linear path as waypoints:

Actual behaviour

The motion moves the subframe from the start to the end pose, but the motion the subframe takes is nonlinear. Instead, the motion that takes end link to the poses needed to put the subframe in the right start and end poses is linear instead:

Backtrace or Console output

Motion plan constraints for the goal point as sent to PlanMotionSequence:

[18:15:29.721657] [/fake/motion_server]: name: 
joint_constraints[]
position_constraints[]
  position_constraints[0]: 
    header: 
      seq: 0
      stamp: 0.000000000
      frame_id: world
    link_name: tb_1_1/disc_tip
    target_point_offset: 
      x: 0
      y: 0
      z: 0
    constraint_region: 
      primitives[]
        primitives[0]: 
                type: 2
          dimensions[]
      primitive_poses[]
        primitive_poses[0]: 
                position: 
            x: 0.463583
            y: 0.000855646
            z: 0.394315
          orientation: 
            x: 0.707088
            y: 0.00515615
            z: 0.000812115
            w: 0.707106
      meshes[]
      mesh_poses[]
    weight: 1
orientation_constraints[]
  orientation_constraints[0]: 
    header: 
      seq: 0
      stamp: 0.000000000
      frame_id: world
    orientation: 
      x: 0.707088
      y: 0.00515615
      z: 0.000812115
      w: 0.707106
    link_name: tb_1_1/disc_tip
    absolute_x_axis_tolerance: 0
    absolute_y_axis_tolerance: 0
    absolute_z_axis_tolerance: 0
    parameterization: 0
    weight: 1
visibility_constraints[]

Motion plan constraints for the goal point as received by LIN:

[18:15:29.979323] [/fake/move_group]: name: 
joint_constraints[]
position_constraints[]
  position_constraints[0]: 
    header: 
      seq: 0
      stamp: 0.000000000
      frame_id: world
    link_name: axia80_mate
    target_point_offset: 
      x: -0.160755
      y: -1.16121e-05
      z: 0.120096
    constraint_region: 
      primitives[]
        primitives[0]: 
                type: 2
          dimensions[]
      primitive_poses[]
        primitive_poses[0]: 
                position: 
            x: 0.463583
            y: 0.000855646
            z: 0.394315
          orientation: 
            x: 0.707088
            y: 0.00515615
            z: 0.000812115
            w: 0.707106
      meshes[]
      mesh_poses[]
    weight: 1
orientation_constraints[]
  orientation_constraints[0]: 
    header: 
      seq: 0
      stamp: 0.000000000
      frame_id: world
    orientation: 
      x: 0.49578
      y: 0.503044
      z: 0.50422
      w: 0.496901
    link_name: axia80_mate
    absolute_x_axis_tolerance: 0
    absolute_y_axis_tolerance: 0
    absolute_z_axis_tolerance: 0
    parameterization: 0
    weight: 1
visibility_constraints[]

I tried turning off the default_planner_request_adapters/ResolveConstraintFrames adapter, which (not unexpectedly) causes the two plans to agree. I believed that some parts of MoveIt are subframe aware e.g. RobotState::getFrameTransform(), so I thought the MoveIt / IK calls might be able to handle the tranforms between the subframe and end link behind-the-scenes, however, this leads to IK related failures:

[18:23:08.760746] [/fake/move_group]: This is the line that fails using a subframe as the target
[18:23:08.760860] [/fake/move_group]: File: ${WORKSPACE}/src/moveit/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator.cpp, Line: 209
[18:23:08.761253] [/fake/move_group]: No IK solver available for link: "tb_1_1/disc_tip"
[18:23:08.761532] [/fake/move_group]: Planner threw an exception (error code: -31): Could not solve request

I'm wondering if the default_planner_request_adapters/ResolveConstraintFrames adapter should pass the entire frame of the target point relative to the end link. That way, LIN can build a KDF::Line subframe start and goal, sample a bunch of poses transformed into the end link frame, then pass that end link path to the IK solver?

Alternatively, the tranformation between poses in the path could be done in the same method that IK is called over the path e.g. for each pose, perform IK if the link is not a subframe, else transform and perform IK (to avoid resampling the path twice and to ensure the sample rates match up)

Then again, if the intention is for that adapter to hide any subframe details from the planner, this seems like it would defeat the purpose. I don't see any way of allowing LIN to achieve this without requiring subframe knowledge though, since it's all points along the path that need to satify a certain subframe property, and the points generated in the middle are an implementation detail of the planner which a pre-processer couldn't account for (also this adapter already seems to pass some information through using the target_point_offset field)

Any ideas would be much appreciated!