Remote controlling of a UR5e arm using servo limitations

[BrocoliAsesino]

Description

Hello community!

I ve been working for the past few weeks on controlling a UR5e remotely and I have reached the point where I don´t know if I am doing something wrong or simply what I am trying to achieve just doesn´t work/doesn´t make sense.

The idea is that I have an Optitrack that can record the poses of a mock gripper, it is these movements that I want the UR5e to follow. Since the poses are in the Optitrack's absolute frame, before passing them to my implementaion of PoseTracking, I find the difference between consecutive poses, transform it to be in the gripper s frame and publish them to /target_pose (poses wrt Tool0).

The movements done with the mock gripper under the Optitrack's recording are not particulary fast, they are at an average human arm speed. The issue that I am seing is that the highest /target_pose publishing rate is, the movements that I see out of the robot are in the correct direction, but are tremendously scaled down. We are talking publishing rates between 30 to 50 Hz. To compensate that, I need to set very large proportional gains (in the order of 100) however this does not seem appropiate. I am using the forward_position_controller, in Moveit(1) I had the same issue and tried the JointTrajectoryController and forward_velocity_controller with the same results.

So my question is, is this a problem of the controllers that are limiting the commands published by moveit servo or, is it that when the poses keep changing very often and thus the twist commands are updated frequently, servo fails to generate the appropiate commands. I know servo in itself is quite simple and that PoseTracking is simply a PID, but the fact that increasing the proportional gain makes it track the movements better makes me question if it comes from there.

Any help or guidance into the source of the issue would be greatly appreciated!

ROS Distro

Humble

OS and version

Ubuntu 22.04

Source or binary build?

Source

If binary, which release version?

No response

If source, which branch?

Humble

Which RMW are you using?

None

Steps to Reproduce

Only way to reproduce it would be with a UR5e.

Expected behavior

Correct tracking of the mock gripper.

Actual behavior

Highly scaled down behavior.

Backtrace or Console output

No response

[sea-bass]

Do you know if this scaling down is due to hitting joint velocity/acceleration limits, or is the robot within limits but the output is wrong? I believe MoveIt Servo publishes a status enum to help investigate this.

If it's the former, then you may want to look at the velocity scaling parameters in Servo and/or the joint limits defined for the robot.

If it's the latter, you may want to try the new version of MoveIt Servo, not the Humble one, as this can directly take pose commands instead of going through the PID controller demo node that converts from poses to twists.