Implement the Inverse kinematics in the repository

[GiulioRomualdi]

This PR implements the InverseKinematics component in the repository.

In details, it introduces the class task and the interface inverse kinematics,
The task describes a generic task that can be handled in inverse kinematics. I also implemented 4 concrete classes that describe different tasks Namelly:

• SE3Task: a task that ensures the tracking of a cartesian trajectory
• SO3Task: it ensures the tracking of the desired orientation
• JointsTrackingTask: it ensures the tracking of a desired joint configuration. (You can use it as a regularization factor)
• CoMTask : it ensures the tracking of the CoM

Once the user has a list of tasks the user can build the inverse kinematics with the InverseKinematics interface.

I also implemented a QPInverseKinematics class. This is a concrete implementation of an integration base IK. In this specific implementation, I used osqp-eigen to solve the ik problem.

I also implemented some tests.

The PR is ready to be reviewed @traversaro @S-Dafarra.

[DanielePucci]

IB-IK in BLF @lrapetti @paolo-viceconte @Yeshasvitvs @claudia-lat

[GiulioRomualdi]

Implementation finished! The PR is ready to be reviewed

[S-Dafarra]

Some initial comments

[S-Dafarra]

Some other comments

[S-Dafarra]

Still reviewing

[S-Dafarra]

WIP

[S-Dafarra]

Maybe it is worth adding an assert, in the end, to check that no constraint has changed size.

[S-Dafarra]

Note that it would be enough to check if at the end index is equal to the total number of constraints. You cannot know which constraint increased in dimension, but you know that at least one did.

[traversaro]

This seems something that gives in output the robot velocity, so could it make sense to call it VelocityInverseKinematics , to distinguish it from the position-based inverse kinematics?

[S-Dafarra]

Last comments

[S-Dafarra]

To be honest, I am not sure if this is checking if the QPIK is working. You are initializing it already in the solution when setting the robot state in line 149, right?

[GiulioRomualdi]

Right. Do you have in mind a possible idea to write a better test?

[S-Dafarra]

It really depends on what you want to test 😉

[GiulioRomualdi]

As discussed F2F I reimplemented the test in df1e11f

Here I run the IK sequentially and I check that the solution converged to the desired one. The tests run in three different models automatically generated by iDynTree.

While I was writing the test I noticed that osqp-eigen does not support unconstrained optimization problems. This is actually a limitation since we may want to consider all the tasks in the cost function (If I'm not mistaken the AnDy ik works in this way @lrapetti )

[S-Dafarra]

Is it a limitation of osqp or osqp-eigen? It may be worked around with some box constraints on the control variables, eventually with large (but not infinite) bounds.

[kouroshD]

While I was writing the test I noticed that osqp-eigen does not support unconstrained optimization problems. This is actually a limitation since we may want to consider all the tasks in the cost function (If I'm not mistaken the AnDy ik works in this way @lrapetti )

I am not sure why you are making this decision to avoid using constraint in IK.
Since a constraint is ensured to be respected (with a good sampling time), while a soft constraint added to the task can be violated. Anyhow, as far as I remember and logically, you should be able to solve the optimization problem without constraints as well.

[S-Dafarra]

That's just a possible implementation. It has pros and cons. For example, you may decide to use no constraints so that you are sure you always get a solution, even if you have two completely contradictory tasks.

[kouroshD]

Note that in a manifold, if you have a set of constraints such that the possible space of your solution is not empty, it should retrieve a best solution as a result, and it is the task of the designer to do not fix such contradictory constraints!

[S-Dafarra]

and it is the task of the designer to do not fix such contradictory constraints!

You should not trust anyone 😁

Jokes apart, one thing is what you should do or not, another thing is a limitation introduced by the software. You should not justify a flaw in the design with the excuse that the corresponding use cases are not good enough.

[GiulioRomualdi]

gbionics/osqp-eigen#84

[traversaro]

To be honest I found a bit difficult to understand what the InverseKinematics interface was just from the docs, so I tried to sketch a diagram (always remembering that every system can be thought in terms of inputs/outpus/state/parameters) of a possible use of this class (possible because the use can be different in other contexts, for example I guess @lrapetti use case is different and he just integrates the IK output to obtain the position). For the task I made an example of just a single non-Lie group task, but I think that is ok for keeping the diagram small.

(raw diagram:
ExplanationWalkingIK.zip
)

In this diagram, the logic that is exposed by the BipedalLocomotion::IK::InverseKinematics interface and the BipedalLocomotion::IK::QPInverseKinematics is the one contained in the yellow block.

My doubts are the following:

• The name InverseKinematics for something that takes in input task positions (and optionally task velocities) and outputs desired robot state may be misleading, especially as we also used quite a lot the Ipopt-based inverse kinematics in which the input are also task positions, and the outputs are instead robot state positions (something that can be obtained also in this case by attaching to the proposed system an integrator). Even VelocityInverseKinematics may be misleading. To be honest I can't think about a good name, so feel free to proceed as otherwise we risk to just loose time bikeshedding, but I just to point out this. Probably it is just sufficient to briefly describe what this interface is (i.e. what are the inputs and what are the outputs) in the interface descriptions.
• What I indicate in the schema as "Velocity Inverse Kinematics" as far as I understand can't be used decouple from the tasks, i.e. injecting directly the velocity that you want. That make sense if it simplifies implementation and directly setting the task velocity is not needed, but I wanted to double check this two hypothesis with you.
• The setKinDyn method used by Tasks and by the IK in general is used for two different goals: one is set the "Kinematic Model" parameter of the system, and the other is to pass the $s^m$ input to the inverse kinematics system. If this is convenient for the implementation it make sense, but I would perhaps mention it in the setKinDyn docs as it may be confusing for users (for example, one user may want to use as feedback in the taks controllers the output of the integrator $s^d$ instead of the measured position $s^m$ or the other way around, and how this is done is a bit criptic.
• Not relevant for the PR, but just for me to understand: in the diagram I indicated a block as ??? as I don't know how you go from $\nu^d$ to $\dot{s}^d$, which logic is there?

[GiulioRomualdi]

Hi @traversaro

1. The name InverseKinematics for something that takes in input task positions (and optionally task velocities) and outputs desired robot state may be misleading, especially as we also used quite a lot the Ipopt-based inverse kinematics in which the input are also task positions, and the outputs are instead robot state positions (something that can be obtained also in this case by attaching to the proposed system an integrator). Even VelocityInverseKinematics may be misleading. To be honest I can't think about a good name, so feel free to proceed as otherwise we risk to just loose time bikeshedding, but I just to point out this. Probably it is just sufficient to briefly describe what this interface is (i.e. what are the inputs and what are the outputs) in the interface descriptions.

We can also rename it in IntegrationBaseInverseKinematics, what do you think?

2. What I indicate in the schema as "Velocity Inverse Kinematics" as far as I understand can't be used decouple from the tasks, i.e. injecting directly the velocity that you want. That make sense if it simplifies implementation and directly setting the task velocity is not needed, but I wanted to double check this two hypothesis with you.

The inverse kinematics is strictly related to task. If we need a pure velocity task we can easily implement it.

3. The setKinDyn method used by Tasks and by the IK in general is used for two different goals: one is set the "Kinematic Model" parameter of the system, and the other is to pass the $s^m$ input to the inverse kinematics system. If this is convenient for the implementation it make sense, but I would perhaps mention it in the setKinDyn docs as it may be confusing for users (for example, one user may want to use as feedback in the taks controllers the output of the integrator $s^d$ instead of the measured position $s^m$ or the other way around, and how this is done is a bit cryptic.

The kinDyn object is used in the tasks just as a container of information (e.g. joint position or frame pose). The user should feed the kindyn object from the outside. So in the case of IK, the kindyn should contain desired values evaluated at the previous step (and integrated) otherwise if the user wants to use the IK class as a velocity controller kindyn should contain measured values.

4. Not relevant for the PR, but just for me to understand: in the diagram I indicated a block as ??? as I don't know how you go from $\nu^d$ to $\dot{s}^d$, which logic is there?

In the QPInverseKinematics $\nu = [\dot{p}; \omega \dot{s}]$

[traversaro]

1. What I indicate in the schema as "Velocity Inverse Kinematics" as far as I understand can't be used decouple from the tasks, i.e. injecting directly the velocity that you want. That make sense if it simplifies implementation and directly setting the task velocity is not needed, but I wanted to double check this two hypothesis with you.

The inverse kinematics is strictly related to task. If we need a pure velocity task we can easily implement it.

Good point!

1. The setKinDyn method used by Tasks and by the IK in general is used for two different goals: one is set the "Kinematic Model" parameter of the system, and the other is to pass the sms^m input to the inverse kinematics system. If this is convenient for the implementation it make sense, but I would perhaps mention it in the setKinDyn docs as it may be confusing for users (for example, one user may want to use as feedback in the taks controllers the output of the integrator sds^d instead of the measured position sms^m or the other way around, and how this is done is a bit cryptic.

The kinDyn object is used in the tasks just as a container of information (e.g. joint position or frame pose). The user should feed the kindyn object from the outside. So in the case of IK, the kindyn should contain desired values evaluated at the previous step (and integrated) otherwise if the user wants to use the IK class as a velocity controller kindyn should contain measured values.

Yes, I think that a description like this in the description of some setKinDyn method would be great.

We can also rename it in IntegrationBaseInverseKinematics, what do you think?

Good idea, it clearly conveys the message that the output is output is the robot velocities that needs to be integrated.

[traversaro]

1. Not relevant for the PR, but just for me to understand: in the diagram I indicated a block as ??? as I don't know how you go from νd\nu^d to s˙d\dot{s}^d, which logic is there?

In the QPInverseKinematics ν=[p˙;ωs˙]

As per t2t discussion, tipically only the joint part of the vector is taken and the base is ignored, because the tasks of the QP are such that the velocities are compatible with the active contact constraints.

I updated the diagram accordingly:

ExplanationWalkingIK.zip

[GiulioRomualdi]

For the sake of completeness, I add the two schemes that explain how the class can be used.

The IK component can actually be used as Velocity controller or real IK. Indeed it is important to notice that IntegrationBasedIKState is a struct containing the joints velocity. When a robot velocity controller is available, one can set these joint velocities to the low-level robot controller. In this case, the $t ^ d$ quantities in the following figures can be evaluated by using robot sensor feedback, and the robot is said to be velocity controlled. On the other hand, if the robot velocity control
is not available, one may integrate the outcome of IntegrationBasedIK to obtain the desired joint position to be set to a low-level robot position controller. In this case, the $t ^d$ quantities can be evaluated by using the desired integrated quantities instead of
sensor feedback, and the block behaves as an inverse kinematics module, and the robot is said to be position controlled.

Here you can find an example of the IK component used as a velocity controller

Here you can find an example of the IK component used as inverse kinematics. In this case, the System::FloatingBasedKinematicsSystem and System::Integrator classes can be used to integrate the output of the IK taking into account the geometrical structure of the configuration space ($ R^3 \times SO(3) \times R^n$)

Here the updated blocks ExplanationWalkingIK.zip

I added this explanation in the doxygen documentation 4f8b58e

cc @traversaro

[GiulioRomualdi]

Here two updated images:

ExplanationWalkingIK.zip