Solution trajectory does not include time parameterization

[Carson-Stark]

I am following the moveit tutorial here with the MTC humble branch.

I am able to get a successful plan for the pick operation, but the "move to pick" connection stage fails when executing. [ur_ros2_control_node-1] [ERROR] [1720466677.224820132] [scaled_joint_trajectory_controller]: Time between points 0 and 1 is not strictly increasing, it is 0.000000 and 0.000000 respectively

I inspected the trajectory published to /solution, and it seems like time_from_start, velocity, and acceleration are all 0 for every point. I suspect that I am missing something in my config or a property when the planner is initialized. I am able to visualize the solution in rviz without any issues.

[rhaschke]

It's a problem of the ROS controller that such a trajectory is not executed. Are the two corresponding waypoints identical?

[Carson-Stark]

Here is a snippet from the solution trajectory. The waypoints show different positions but they are all at time 0.

trajectory: joint_trajectory: header: stamp: sec: 0 nanosec: 0 frame_id: world joint_names:

• shoulder_pan_joint
• shoulder_lift_joint
• elbow_joint
• wrist_1_joint
• wrist_2_joint
• wrist_3_joint points:
• positions:
  • 0.9762626886367798
  • -2.148520294819967
  • 2.2753353118896484
  • -0.6215809027301233
  • 1.728919267654419
  • -0.0956500212298792 velocities:
  • 0.0
  • 0.0
  • 0.0
  • 0.0
  • 0.0
  • 0.0 accelerations:
  • 0.0
  • 0.0
  • 0.0
  • 0.0
  • 0.0
  • 0.0 effort: [] time_from_start: sec: 0 nanosec: 0
• positions:
  • 1.0087883660924717
  • -2.109131459563662
  • 2.250216577809731
  • -0.7033602355167735
  • 1.7916161806890842
  • 0.019589622960529046 velocities:
  • 0.0
  • 0.0
  • 0.0
  • 0.0
  • 0.0
  • 0.0 accelerations:
  • 0.0
  • 0.0
  • 0.0
  • 0.0
  • 0.0
  • 0.0 effort: [] time_from_start: sec: 0 nanosec: 0
• positions:
  • 1.0413140435481634
  • -2.0697426243073567
  • 2.2250978437298143
  • -0.7851395683034237
  • 1.8543130937237495
  • 0.1348292671509373 velocities:
  • 0.0
  • 0.0
  • 0.0
  • 0.0
  • 0.0
  • 0.0 accelerations:
  • 0.0
  • 0.0
  • 0.0
  • 0.0
  • 0.0
  • 0.0 effort: [] time_from_start: sec: 0 nanosec: 0

[rhaschke]

Sorry, I didn't read your initial issue report carefully enough. Indeed, your trajectory is missing the time parameterization. This is added via a planning request adapter, that needs to be configured in your ompl_planning_pipeline.launch.xml as follows:

  <!-- The request adapters (plugins) used when planning with OMPL. ORDER MATTERS! -->
  <arg name="planning_adapters"
       default="default_planner_request_adapters/AddTimeParameterization
                default_planner_request_adapters/ResolveConstraintFrames
                default_planner_request_adapters/FixWorkspaceBounds
                default_planner_request_adapters/FixStartStateBounds
                default_planner_request_adapters/FixStartStateCollision
                default_planner_request_adapters/FixStartStatePathConstraints"
                />

See the tutorial for details: https://moveit.github.io/moveit_tutorials/doc/time_parameterization/time_parameterization_tutorial.html

[Carson-Stark]

That worked. I was missing moveit_config.planning_pipelines in my launch file so the ompl_planning.yaml file was not being loaded properly.

pick_place_task = Node(
        package="pick_and_place",
        executable="pick_and_place_node",
        output="screen",
        parameters=[
            moveit_config.joint_limits,
            moveit_config.planning_pipelines,
            moveit_config.robot_description,
            moveit_config.robot_description_semantic,
            moveit_config.robot_description_kinematics,
        ],
    )