Investigate why Integration-based IK returns high joint velocity with the robot model

[kouroshD]

There is this problem that the HumanStateProvider when using Integration based-IK with icub model, some times it returns values that does not represent what the human has been performed. This behavior has been examined and observed specifically with robot shoulder.

In these cases, the joint velocities reaches high values:

I have put a saturation for the joint velocities for that, when the returned velocity is higher than ±10 rad/sec:

 l_shoulder_yaw  :  84.3201 
l_shoulder_pitch  :  -83.5096 
************
 l_shoulder_yaw  :  144.197
l_shoulder_pitch  :  -143.37 
************
l_shoulder_yaw  :  235.524
l_shoulder_pitch  :  -234.658
************
l_shoulder_yaw  :  -12.9854 
l_shoulder_pitch  :  13.7822

The maximum joint velocity limit as mentioned here is 1000 deg/sec=17.4533 rad/sec

This behavior probably happen when the rotation matrix reached a sort of singularity. @lrapetti @Yeshasvitvs Does this behavior happen with the human model as well?!

[lrapetti]

This behavior probably happen when the rotation matrix reached a sort of singularity.

I agree the problem may be related to the singularity of the spherical joint. In order to investigate that you can try to check the Jacobian when this problem happens (check here where the Jacobian is prepared).

In order to prevent the singularity, you may also try to play with the cost of the regularization, that is used in the pseudoinverse.

[DanielePucci]

As we discussed F2F, the discontinuity of the velocity may be due to essentially two facts:

• The jacobian getting singular
• A discontinuity in the measured velocity

We also agreed that @kouroshD will check them both, starting by checking the singular values of the jacobian.

[kouroshD]

This morning I have investigated the reason why we see this behavior:

• Velocities: It seems that, the link and base velocities we use to find the joint velocities are fine, these are the link velocities when we integral IK returns a 1270.16 rad/sec l_shoulder_pitch joint velocity:

-0.00181016 0.00563969 0.0019123 -0.0313407 0.0196375 0.00568333 -0.036022 0.0236513 -0.0271193 0.0557748 0.14846 -0.048796 -0.0250652 0.0483587 -0.072875 0.0275612 -0.247219 0.389204 0.2698 0.184134 0.0567466 -0.634626 -1.05252 0.557032 -2.02192 -2.08467 -0.978867 -2.25763 -1.87731 -1.21863 0.0453237 0.11989 -0.236694 -0.038093 -0.0485296 -0.281606 -0.0086942 -0.0843336 0.537537 -0.05445 -0.074917 0.0237207 0.021458 0.0481577 0.0869072 0.0258679 0.0144267 -0.0786234

• Jacobian: The problem seems to be from the Jacobian singularity: Here is the result of SVD of the jacobian in this case: 4.9748 4.8690 4.7261 2.7598 2.5028 2.3668 2.3234 2.2348 2.1624 1.8475 1.7354 1.7007 1.3681 1.2628 1.1544 1.0000 1.0000 1.0000 0.9326 0.8265 0.8116 0.8024 0.7998 0.7821 0.7654 0.7580 0.7081 0.6123 0.5997 0.5790 0.5678 0.5554 0.5090 0.4846 0.4706 0.4238 0.4216 0.0003 and the determinant and condition number of the jacobians are 1.4182e-04 and 1.7118e+04.

For a normal case, the determinant and condition numbers are: 237.7862 and 13.8951. Also, the 5 lowest SVD are 6.2381 7.7290 21.9953 23.5863 25.2472.

Theoretically when the human stays in T-pose with the robot model, the configuration of joint are singular.

Moreover, when the human stays in T-pose with the human model, and rotate shoulder 90 deg along pitch axis, can reach a singular configuration.

[kouroshD]

I have added a commit in order to resolve the problem of the singularity here.

The computational time to compute the IK with the robot model increased from 5-6 msec to 7-9 msec.

It should be tested.

[DanielePucci]

Not really clear to me why it should take longer, but we accept life as it is

[lrapetti]

I have added a commit in order to resolve the problem of the singularity here.

What is the solution you found to solve the problem?

[kouroshD]

By investigating yesterday and today on the HDE, I understood that cost regularization in inegtarion-based-IK was not changing even if I was changing the costRegularization in the config file. The problem is now fixed with this commit in HumanStateProvider.cpp. We were not updating the regularization, and it was the default value always 1e-8.

[DanielePucci]

@kouroshD nice did you by chance try to check if with higher regularisation terms the problem gets mitigated?

[kouroshD]

Also, I have applied in this commit, InverseVelocityKinematics.cpp a number of Eigen matrix pseudo inverse methods for to inverse the Jacobian Matrix. I did not check the accuracy of the results. Following I show the computational time to solve the IK problem using different methods:

MethodName	ave Computational Time
CompleteOrthogonalDecomposition	7-9 ms
default inverse()	5-6 msec
Least Square Solving	50-60 msec
Robust Cholesky decomposition	3-4 msec
Standard Cholesky decomposition	2-3 msec
Sparse Robust Cholesky decomposition	4-5 msec
Sparse Standard Cholesky decomposition	3 msec

The value obtained using the icub robot model.

A detailed description can be found here: https://eigen.tuxfamily.org/dox/group__TutorialLinearAlgebra.html and http://eigen.tuxfamily.org/dox-devel/group__TopicSparseSystems.html.

[kouroshD]

@kouroshD nice did you by chance try to check if with higher regularisation terms the problem gets mitigated?

I'm putting it now the results with different regularization terms!

[kouroshD]

The collected data video (human data):

[kouroshD]

Cost Regularization: 1e-3

qdot:

q:

[kouroshD]

In the experiment, we have Xsens retargeting + walking-controller. The only limit, in all the pipelines are the joint limits.

[kouroshD]

Cost Regularization: 1e-2

qdot:

q:

[kouroshD]

Cost Regularization: 1e-1

q:

dq:

[kouroshD]

cost regularization =1

dq:

[kouroshD]

cost regularization =10

qdot:

q:

[DanielePucci]

Really nice @kouroshD, I think we are there ;-) Well done

Some observations:

1. legends and labels - with unit of measurements - are missing in all plots, and this renders data difficult to read/interpret for people not knowing exactly what these plots are about. I would also increase the font of the pictures for better visualisation. If you could address all these little issues and replace the above pictures would be perfect
2. For joint angles and velocities, we use $s$ and $\dot{s}$, not $q$ and $\dot{q}$. Please change notation accordingly
3. I really like the fact that you added also the GIF of the human, it helped a lot. The only issue is that the human motions are shown in a comment much more above than the GIFs showing the robot motions. Could it be possible to always put the GIF of the human in each comment just close to that of the robot? If you could also sync them, it would be definitely better for visualisation comparison
4. It seems to me that in all the above comments, see e.g. this one, the important quantity is the joint velocity, and that you basically showed that by changing the regularisation term of the IK, the problem of the velocity spikes is solved. We may then add a sort of recap comment having:
   • For each joint, a single figure showing the different joint velocities obtained with different regularisation values - include also the value for which we have the problem. Here, all different joint velocities may be depicted in a single plot (i.e. in the same Matlab axis). Please add here legends, labels, etc (see point 1. and 2.). Clearly, different colours associated with each velocity will help the reading and comparison of the different regularisation term choices, since we will have several lines in the same axis
   • For each joint, another single figure showing the different joint angles associated with different regularisation values, all depicted in a single plot (i.e. in the same Matlab axis). Here I would plot only the outcome of the IK (there are always two lines, black and red, associated with the joint positions, that I guess are the desired joint values and the robot value )
   • Several pairs of GIFs showing Human-Robot motions (each pair of GIFs Human-Robot associated with a choice of the regularisation term). These $n$ pairs of GIFs, with $n$ the number of choices for the regularisation term, show visually what is going on
   • Then, @kouroshD may suggest a value of the regularisation term, open a PR to master/devel containing the modification for correct loading of the regularisation term, and even close this issue. Here, @lrapetti input may be important

[kouroshD]

After doing some experiments and trials with regularization terms, I think a cost regularization of value 1.0 is good enough both in terms of performance to follow the sensory data, and close to singularity does not return high velocities. In the Following figures, I disable the velocity saturation(in General we added 10.0 is the value for the velocity saturation). In the following figures, I manually tried to synchronize the data of experiments with different regularization values.

color	cost regularization value
black	0.1
blue	0.5
red	1.0
green	3.0

[kouroshD]

I put the dumped data here as a reference:

human_dump_singularity_test.zip

we play the data with the following option: yarpdataplayer --withExtraTimeCol 1

[kouroshD]

Following I tried to synchronize the human and robot videos (bu playing with robot video speed and starting time of them, as the real time factor was not equal in two cases):

Human	Robot(cost regularization value=1.0)

[DanielePucci]

Nice @kouroshD the fact that the GIFs are not sync complexifies a little the reading, but really nice, I think you got it

[kouroshD]

CC @lrapetti @DanielePucci

If you agree as we addressed the issue we had, I close this issue. Moreover, I think It can be nice to add manipulability terms in intg-IK so that while we have good performance, we avoid returning high velocities when we are close to singularity. Also, it can enhance a possible future publication and scalability of the software for different robots.