Test Cartesian velocity control algorithm

[Woolfrey]

I'm currently working on a Cartesian velocity control test executable.

I am sometimes getting very nice results:

---

This one starts with a high joint velocity, which corresponds to a high initial orientation error of the endpoint:

There is obviously something wrong with the orientation error calculation as the initial pose of the robot is used as the start point of the trajectory generator (it should have zero error at the start).

On the plus side, the QP solver is obeying the joint limits very nicely!

[Woolfrey]

I fixed the problems with the orientation error. It was inside the CartesianTrajectory class itself.

Here's an example of a numerical simulation with good results:

Here is a numerical simulation with "bad" results:

:+1: The good thing is, the QP solver is obeying the joint limits! :-1: The output for singularity avoidance is unstable. We need to work on it.

I still need to test a 6DOF robot as well.

[Woolfrey]

I tried numerous methods of singularity avoidance, including fancy methods of momentum, QR decomposition, etc. etc. I settled on 2 things:

1. Reducing the max joint acceleration in the SerialLinkBase class, and
2. Increasing the damping in a classical DLS for the SerialKinematicControl class.

Results are satisfactory, but not ideal:

It's hard to say what's realistic since I'm just running random numbers; I can't if the robot is in a feasible position, if the trajectories are sensible, etc etc.

But numerically it looks "good enough" compared to previous results.

I still want to try 1 more method before moving on and settling with this for short-term implementation.

---

EDIT: I will test the Control Barrier Function for singularity avoidance. On the ergoCub robot it would always violate constraints a tiny bit, and my QP solver would fail. But here the QP solver is obeying joint limits perfectly :+1:

[ssutjipto]

Results look reasonable and your plan sounds good.

Also, do you have the python scripts for plotting in another repository?

[Woolfrey]

@ssutjipto, the Python scrips are in the test folder:

https://github.com/Woolfrey/software_robot_library/tree/control/Control/test

[Woolfrey]

Here are results using the UR5 model:

---

Here is a "bad" case where the robot hits a singularity:

The QP solver satisfied the constraint on manipulability nicely.

(I've also seen some crazy / unstable results, but its hard to ascertain if these are sensible without doing a proper 3D simulation.)