Test Cartesian control algorithm starting from singularity

[Woolfrey]

Plan:

The control barrier function (CBF) assumes the robot starts in a safe set (i.e. not singular). It is possible that the robot starts from a singular configuration. There is a condition in the code for handling singular cases, but we need to check that it is numerically stable.

1. Start the controller in a singular configuration (i.e. $\mathbf{q = 0}$.
2. Run the Cartesian trajectory tracking.
3. Plot joint velocities vs. time.

Do:

Observations	Thoughts, implications
The QP solver failed a couple of times when testing the Sawyer robot model.	This means the constraints do not form a convex set.
The UR5 did not move, and the QP solver did not throw any errors.	This is good because its safe, but it's not very useful.
The UR5 spat out a nan for the manipulability for $\mathbf{q = 0}$.	This might be because of numerical error leading to the square root of a negative number. We should check further, because it affects the gradient calculation.

Study:

Results for the Sawyer robot for 2 different tests:

Results for the UR5:

Upon reviewing the code, the control problem for the singular case is:

\begin{align}
\min_{\mathbf{\dot{q}}} \frac{1}{2}\| \mathbf{\dot{x} - J\dot{q}} \|^2 \\
\text{subject to: } \mathbf{\dot{q}}^\mathrm{T}\frac{\partial\mu}{\partial\mathbf{q}} > \mathbf{0}
\end{align}

But when singular, $\frac{\partial\mu}{\partial\mathbf{q}} = \mathbf{0}$. This would explain (part of) the problem.

Act:

We need to think of an alternative problem that can handle singular cases. A quick thought is:

\min_{\mathbf{\dot{q}}} \frac{1}{2}\| \mathbf{\dot{q} - J^\mathrm{T}\dot{x}}\|^2

but I will need to do some more math.

[Woolfrey]

Plan:

To test the following optimization for singular cases:

\min_{\mathbf{\dot{q}}} \frac{1}{2}\|\mathbf{\dot{q} - J^\mathrm{T}\dot{x}}\|^2

If $\mathbf{J}$ is singular, then $\exists\mathbf{\dot{x}\ne 0}$ such that $\mathbf{J^\mathrm{T}\dot{x} = 0}$. This avoids inversion and should be numerically stable.

The process is:

1. Start the robot in $\mathbf{q = 0}$ (singular),
2. Create a random Cartesian trajectory,
3. Run the Cartesian trajectory tracking.
4. Plot the joint velocities vs. time.

Do:

Observations	Thoughts, intuitions, implications
The QP solver often failed with the UR5 model.	I have no immediate thoughts on this.
Every case of the Sawyer was unstable. Only 1 test for the UR5 was semi-stable.	It is not clear how this difference is affecting things.

Study:

Sawyer model, 1st test:

Sawyer model, 2nd test:

---

UR5 model, 1st test:

UR5 2nd test:

Act:

This optimization method does not lead to numerically stable results.

It is difficult to judge why, since these are only numerical simulations. It might be more evident to do a proper 3D simulation to see what's happening.

For now I think it is best to just cease all Cartesian control in a singularity until a better method can be found.

[Woolfrey]

Plan:

Test the optimisation problem:

\begin{align}
\min_\mathbf{\dot{q}} \frac{1}{2}\mathbf{\dot{q}^\mathrm{T}\dot{q}} \\
\text{subject to: } \mathbf{\dot{q}}_{min} \le \mathbf{\dot{q}} \le \mathbf{\dot{q}}_{max}
\end{align}

for singular configurations. This should drive the robot to zero velocity as fast as possible, whilst satisfying joint limits.

Do:

Observations	Thoughts
There's always 1 joint which suddenly accelerations and decelerates.	Somehow one of the joint velocities must be set incorrectly. Not sure where or why this is happening.

Study:

Results are good, except for 1 joint:

Act:

The reason for the 1 joint suddenly acceleration is not clear. It could be a problem with the code. We will need to test on a real robot.