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:
- Use RRT planning to plan and execute trajectory to pre-approach pose
- Use robotic arm servoing to slowly move arm to pre-manipulation pose
- 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 :)