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_tutorials/doc/pilz_industrial_motion_planner/pilz_industrial_motion_planner.html

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

https://ros-planning.github.io/moveit_tutorials/doc/move_group_python_interfacemove_group_python_interface_tutorial.html#start-rviz-and-movegroup-node

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

Planning to a jointgoal or a posegoal 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 movegrouppythoninterfacetutorial.py using the exact same functions in the exact same order.

When the original movegrouppythoninterfacetutorial.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 movegrouppythoninterfacetutorial.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 the alpha is set to 0.5, for a robot alpha of 1 the robot does not flicker).

Weirdly enough the terminal also says:

[ INFO] [1684140598.326323861]: Execution request received

[ INFO] [1684140598.686430742]: Completed trajectory execution with status SUCCEEDED ...

[ INFO] [1684140598.686487111]: Execution completed: SUCCEEDED

even though nothing moves in Rviz.

One last thing I ought to mention. My robot has less then 3DOF. It is roughly humanoid and I am working with the left arm at the moment, which has certain joint limits which dictate a certain reachable space. I am rather new to robotics but I think this amounts to less than 3 DOF.

Does anyone have a theory for why the planning to a cartesian path fails?

I am grateful for any input and am happy to provide more details about my setup and terminal outputs.