Moveit: Linear Y twist command with MoveIt Servo and 4DoF arm

[dudasdavid]

Description

I'm trying to use the MoveIt Servo with an OpenMANIPULATOR-X which is a 4DoF manipulator from ROBOTIS.
I modified the open_manipulator_controller to listen to the JointTrajectory messages from moveit_servo so communication works properly. When sending linear x or z commands the manipulator works as expected. When sending linear y twist command moveit_servo cannot move the arm. With a 4 DoF manipulator, obviously, I don't expect a clean y translation, but moveit_commander is able to handle such a request in compute_cartesian_path, so I expect a similar behavior here.

Your environment

• ROS Distro: Melodic
• OS Version: Ubuntu 18.04
• Tried both master and melodic-devel branch from source

Steps to reproduce

You can reproduce the issue using Gazebo.

Use the moveit-servo-experiment branch from my fork for the modified open_manipulator_controller:
https://github.com/dudasdavid/open_manipulator/tree/moveit-servo-expriment

My config file for moveit_servo is:
https://gist.github.com/dudasdavid/61d815a318760cf71f7eb7168a263ed5

• roslaunch open_manipulator_gazebo open_manipulator_gazebo.launch
• roslaunch open_manipulator_controller open_manipulator_controller.launch use_platform:=false use_moveit:=true
• roslaunch moveit_servo spacenav_cpp.launch (without input device)
• send twis command:

rostopic pub -r 100 -s /servo_server/delta_twist_cmds geometry_msgs/TwistStamped "header: auto
twist:
  linear:
    x: 0.0
    y: 0.1
    z: 0.0
  angular:
    x: 0.0
    y: 0.0
    z: 0.0"

Expected behaviour

Similar behavior as compute_cartesian_path of moveit_commander for a 4DoF manipulator.

Actual behaviour

X and Z linear twist commands are fine, but Y doesn't work as expected.

[welcome[bot]]

Thanks for reporting an issue. Because we're a volunteer community, providing a pull request with suggested changes is always welcomed.

[AndyZe]

I took a shot at reproducing the issue but got this error upon roslaunch moveit_servo spacenav_cpp.launch:

ros.rosconsole_bridge.console_bridge: No name given for the robot.
ros.moveit_ros_planning.rdf_loader: Unable to parse URDF from parameter '/robot_description'

Do you see any warnings in terminal about singularities? If so, increase lower_singularity_threshold in the yaml.

You can also allow one or more dimensions to "drift" with a service call like this. I would recommend allowing two drift dimensions because the robot is underactuated by 2 DOF. This example will allow small pitch and yaw.

rosservice call /servo_server/change_drift_dimensions "drift_x_translation: false
drift_y_translation: false
drift_z_translation: false
drift_x_rotation: false
drift_y_rotation: true
drift_z_rotation: true
transform_jog_frame_to_drift_frame:
  translation: {x: 0.0, y: 0.0, z: 0.0}
  rotation: {x: 0.0, y: 0.0, z: 0.0, w: 0.0}"

[AndyZe]

I can see from the Gazebo sim that it's just not possible for the robot to translate along Y without some rotation. I'm not sure how Cartesian planning gets around that. Maybe loose tolerances? Or allowing a joint jump from one waypoint to the next?

[dudasdavid]

Thank you for your answer! I've no idea why do you have the above error, it was tested on more systems, I installed open_manipulator_gazebo with apt-get, only the controller and a few other nodes are running from my fork. The colors are also missing on your model:

I didn't need any trick to make Cartesian planning work, it was working out of the box, and it can execute 100% of trajectories, see my video, I'm using only Cartesian planning here.

I tried setting drift for y and z rotation with the service call, and it works for the y translation until it reaches y = 0 (middle), I get a Very close to a singularity, emergency stop warning there and servoing stops, of course, I can move it manually using the interactive marker of MoveIt so it's not a singularity of this arm. I tried to increase the singularity threshold without any luck. Do you have an idea of how could I adjust singularity detection for this arm?

[AndyZe]

Well, unfortunately there's nothing to be done other than what I already wrote. Increase the singularity thresholds or make the service call to enable drift. I think you will be pleasantly surprised by how "drift" helps.

[AndyZe]

Try enabling drift for all three degrees of rotation (roll, pitch, and yaw).

[dudasdavid]

Interestingly, setting the drift on x rotation solved the singularity issue. It's great that moveit_servo can work with such a low DoF arm after setting the drift, thanks for the help, I close this issue!

[AndyZe]

Cool video!