MoveIt! move_group_python_interface_tutorial.py fails to produce cartesian path

edit

Hi

I have the following setup:

• OS: Ubuntu 20.04

• Ros Distro: Noetic

One of my ultimate goals for the robot is to have its end-effector move through a series of waypoints using the blend_radius feature of the sequence capability provided by the Pilz industrial planner MoveIt! plugin

https://ros-planning.github.io/moveit...

To see how movement through waypoints look without the blending and to get familiar with the move_group_node I started following this tutorial:

https://ros-planning.github.io/moveit...

I am currently altering the move_group_python_interface_tutorial.py code to fit it to my robot.

Planning to a joint_goal or a pose_goal works fine with the above mentioned python script, at least it shows the desired robot movements in Rviz ( I have not tested it on the real robot yet but that is irrelevant for my question here).

I prepared 2 coordinates (w1,w2) and checked whether it was possible for MoveIt to find a trajectory for (w0,w1) and (w0,w2) with w0 being the starting position of the end-effector for the desired planning group. In both cases MoveIt planned and displayed a correct trajectory in rviz.

This led me to believe that it would also be possible to create an array with waypoints [w0,w1,w2] and to plan to a cartesian path going through these waypoints. But when I ask the move_group node to plan this cartesian path, it does not find one.

Here is the relevant code snippet which specifies the waypoints and tries to find a cartesian path:

 # Create waypoints 
 current_pose = tutorial.move_group.get_current_pose().pose
 w0 = copy.deepcopy(current_pose)
 w1 = copy.deepcopy(w0)
 w1.position.z += 0.05 
 w1.position.y += 0.07 
 w2 = copy.deepcopy(w0)
 w2.position.z += 0.09 
 w2.position.y += 0.08 
 w3 = copy.deepcopy(current_pose)
 waypoints = [w0,w1,w2]

 #plan 
 (plan, fraction) = tutorial.move_group.compute_cartesian_path(waypoints, 0.0005, 0.0)
 #display 
 display_trajectory = moveit_msgs.msg.DisplayTrajectory()
 display_trajectory.trajectory_start = tutorial.robot.get_current_state()
 display_trajectory.trajectory.append(plan)
 #publish
 tutorial.display_trajectory_publisher.publish(display_trajectory)
 #execute 
 tutorial.move_group.execute(plan, wait=True)

This snippet is an adapted version of the code from move_group_python_interface_tutorial.py using the exact same functions in the exact same order.

When the original move_group_python_interface_tutorial.py is executed with the panda robot it says in the terminal:

[ INFO] [1684139671.501745044]: Attempting to follow 3 waypoints for link 'panda_link8' using a step of 0.010000 m and jump threshold 0.000000 (in global reference frame)

[ INFO] [1684139671.507262405]: Computed Cartesian path with 44 points (followed 100.000000% of requested trajectory)

When I run move_group_python_interface_tutorial.py with the only modification being that my robots end-effector, planning group and lower step are used, then the terminal says:

[ INFO] [1684140333.395828072]: Attempting to follow 3 waypoints for link 'L_Hand_Link' using a step of 0.001000 m and jump threshold 0.000000 (in global reference frame)

[ INFO] [1684140333.401456556]: Computed Cartesian path with 1 points (followed 0.000000% of requested trajectory)

And in Rviz there is no movement, just a constant flickering of the robot (but only if ... (more)