isaac_ros_cumotion pipeline planner in Connect stage suddenly jerks and changes joint values at last waypoint of the trajectory

[bornaparo]

Hi, I created pick and place task with moveit task constructor following the tutorial, and it works. Then I wanted to try using it with NVIDIA's isaac ros cumotion planner but it doesn't work. Connect stage creates trajectory that at last waypoint suddenly changes joint values. I tried changing a lot of properties of the connect stages and of pipeline planner, also have tried changing a lot of parameters in isaac_ros_cumotion/cumotion_planner.py but nothing helped. When I use isaac_ros_cumotion planner just for MoveTo stage then it works without the mentioned problems, so I guess the problem has to be with Connect stage but I cannot figure out where.

Here is a video of the problem: https://github.com/user-attachments/assets/66b01b1b-e40b-4955-af3d-a0f2dd40a24d

Here is an output of mtc node where it says Trajectory success! so I guess it is not a problem with cumotion. Also cumotion works correctly when I tried using it in RViz to plan around obstacles.

I am using moveit2 from humble branch and ros2 humble in docker.

To reproduce this error:

• Create the same package with the same files as in tutorial, or modify the already created files in moveit2_tutorials/doc/tutorials/pick_and_place_with_moveit_task_constructor/
• install isaac_ros_cumotion moveit plugin from here, I installed it from source in my own docker container
• copy isaac_ros_cumotion_moveit/config/isaac_ros_cumotion_planning.yaml to moveit_resources/panda_moveit_config/config/
• in mtc_demo.launch.py:
  • replace pipelines=["ompl", "chomp", "pilz_industrial_motion_planner"] with pipelines=["ompl", "chomp", "pilz_industrial_motion_planner", "isaac_ros_cumotion"]
  • add curobo planner node:

    curobo_planner_node = Node( 
    package="isaac_ros_cumotion",
    executable="cumotion_planner_node",
    output="screen",
    parameters=[
        {"robot": "franka.xrdf"},
        {"urdf_path": os.path.join(get_package_share_directory("moveit_resources_panda_description"), "urdf", "panda.urdf")},
    ]
    )

    and add it to the LaunchDescription

• in mtc_node.cpp replace auto sampling_planner = std::make_shared<mtc::solvers::PipelinePlanner>(node_); with auto sampling_planner = std::make_shared<mtc::solvers::PipelinePlanner>(node_, "isaac_ros_cumotion");
• build the package and run it:
  • ros2 launch mtc_tutorial mtc_demo.launch.py and wait for cumotion to warmup
  • ros2 launch mtc_tutorial pick_place_demo.launch.py

Any help is appreciated!

[rhaschke]

Looks like cumotion only cares about the Cartesian end-effector pose, but ignores the joint pose: As you can see from the rviz visualization, the generated paths reach the end-effector pose and then jump to a different joint-pose. This also explains, why it works for MoveTo but not for Connect. The latter requires planning from a joint-pose to another joint-pose. I suggest to file an issue with cumotion to check whether this mode of operation is supported somehow.

[bornaparo]

@rhaschke I see that curobo has an implementation for joint-pose to joint-pose planning. How can it be utilized so the Connect stage uses that method?

[rhaschke]

Looks like ros_cumotion doesn't use this joint-space planner. Instead, a joint-space pose is transformed into the Cartesian pose. Hence, you should adapt ros_cumotion to use plan_single_js in that case (and create a PR).