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

[bornaparo]

In cumotion_planner.py I replaced

motion_gen_result = self.motion_gen.plan_single(
            start_state,
            goal_pose,
            MotionGenPlanConfig(max_attempts=5, enable_graph_attempt=1,
                                time_dilation_factor=time_dilation_factor),
        )

with

motion_gen_result = self.motion_gen.plan_single_js(
            start_state,
            goal_state,
            MotionGenPlanConfig(max_attempts=5, enable_graph_attempt=1,
                                time_dilation_factor=time_dilation_factor),
        )

so it does joint state to joint state planning, but the problem is the same as before?

[rhaschke]

Then, probably, your changed code is not used? Add a debug message before that line and check whether it pops up in the log.

[bornaparo]

This is the output of ros2 launch mtc_tutorial mtc_demo.launch.py, and this is the output of ros2 launch mtc_tutorial pick_place_demo.launch.py . I tried using self.get_logger().debug() but it didn't output anything, so I used print() statement. From the logs it looks that method is being called only after the keyboard interrupt. There were 18 unsuccessful move to pick plans and 59 unsuccessfull move to place plans. The method was called 77 times as it should, but why after closing the program?

[bornaparo]

I changed from print(...) to self.get_logger().info(...) and now message is being printed and method is called as it should. So I guess the problem isn't with cumotion but somewhere with connect stage?