PID control for robotic manipulator servoing

Question

I'm trying to enable robotic manipulator servoing using MoveIt servo implementation. However, I'm having some questions regarding reference following and error compensation.

The thing is, I roughly know pose of the object I need to manipulate, and as closer I get, my pose estimation is better, therefore I should converge to the wanted pose. Pipeline I'm having currently is:

1. Use RRT planning to plan and execute trajectory to pre-approach pose
2. Use robotic arm servoing to slowly move arm to pre-manipulation pose
3. Manipulate object (not important for know)

In my implementation, I'm using ROS ActionServers for different modes of control. The thing is 1. part of the pipeline works fine, and the pose estimation as soon as I'm close enough is really good. Servoing in MoveIt is realized by using 4 PIDs (3 for position/xyz, and 1 for orientation/quaternion). Error is generated as (e = p_wanted - p_current) and after going through PIDs we get linear and angular velocities of the end effector. Thing is, p_wanted is always the same, and p_current changes during servo execution. As far as I know, error should minimize, but in some cases, after certain time, servoing "diverges" and error begins increasing instead of decreasing.

I'm wondering how could I prevent that jump, and how can I tune PID parameters good enough to be able to minimize large errors (10 cm) as well as small errors (1 cm). Is it even possible? Would I get better preformance if I would concentrate on continuous reference change, compensation of 1cm of error at the time?

If more context is needed, or something isn't clear enough, let me know.

Thank you for your help :)

Answer 1

How exactly are you calculating the error? If you are combining position error and angular error, then that might make the error blow up. (Especially if the orientation error is calculated incorrectly). Calculating angular error can be tricky.

In the past when I've done tasks like this, I usually keep the end-effector's orientation constant and do translation only to slowly get the arm into the pre-manipulation pose. See this question for how to do that: How to get pure end-effector translation through Jacobian?

But if you do need to follow a trajectory with orientation included, see this question: Jacobian-based trajectory following

Answer 2

Angular error is calculated using dot product of quaternion and quaternion inverse as shown here :)

Basically, I've been using MoveIt servo.

And I've managed to grasp it following this article.

First of all, kudos to all the MoveIt contributors because they made it easy as possible.

Second of all, I had multiple problems regarding servoing which are in (priority order):

• badly tuned PID controllers for individual joints
• trying to servo manipulator at the edge of reachable space
• servoing 6DoFs with 6DoF manipulator (simply doesn't work well enough for larger Euclidean distances)

Some advices:

• tune PID controllers of each joint, and do it well. Make sure your robot is controllable
• Do not servo for large Euclidean distances
• Servo less DoFs than your manipulator has

Rest of the discussion.

Thank you for your answer. Resources are super helpful :)

Cheers.