Adapt iDynTree to use automatic differentiation

[GiulioRomualdi]

iDynTree is a wonderful library and it allows us to efficiently retrieve all the robot kinematics and dynamics quantities. However using, at the current stage is not possible to use automatic differentiation-algorithms for computing the derivatives of given quantities. Having such derivatives may allow the user to design complex control/estimation algorithms.

In the past @S-Dafarra implemented a Semi-automatic differentiation library named levi, however I don't know the current status and if it is easy to use it inside iDynTree.
Another interesting framework is pinocchio where the support of modern Automatic Differentiation frameworks like CppAD or CasADi is guaranteed.

This issue is just for discussing proposals and ideas. I know that this a moonshot and adapting iDynTree for enabling the automatic differentiation is a really long and complex task, however I think this is the right moment for starting the discussion.

[S-Dafarra]

In the past @S-Dafarra implemented a Semi-automatic differentiation library named levi, however I don't know the current status and if it is easy to use it inside iDynTree.

I haven't worked on it for a while now. But it should still be working. Notice though that levi is more like symbolic differentiation, not AD. The performance gap between the levi method and AD is not yet studied. From an application point of view, it is pretty easy to use iDynTree within levi. I used it to compute a bunch of derivatives involving Jacobians and transformation matrices.

This issue is just for discussing proposals and ideas. I know that this a moonshot and adapting iDynTree for enabling the automatic differentiation is a really long and complex task

I remember some discussion with @traversaro where the main idea was basically to make at least KinDynComputations compatible with AD software. The major problem right now is that AD breaks in case of if in the code. There should not be any conditional branching in the code where you want to use AD. Right now this is the case. For example, when you request a Jacobian, there may be different branches depending on the type of frame (link, fixed frame, base, ecc.) and the chosen trivialization.
Another possibility we discussed was to make KinDynComputations only an interface, implementing different backends, maybe exploiting directly pinocchio. At this stage, we would need to have the same functionalities we have in iDynTree: different trivializations handling, handling of reduced models, easy change of base, so on and so forth.

Another thing to take into considerations is the use of the vectors and matrices to be used. For AD to work, it may be necessary to redefine the primitive types. Hence, VectorDynSize and MatrixDynSize cannot be used as they are right now, since they are arrays of "classical" double.

Having said this, I strongly believe this is an interesting direction 👍

[traversaro]

This issue is just for discussing proposals and ideas. I know that this a moonshot and adapting iDynTree for enabling the automatic differentiation is a really long and complex task, however I think this is the right moment for starting the discussion.

Thanks for opening the issue! It is not so complex or long, it just needs some dedicated and focus work.

I think there are basic two main possible non-conflicting paths (that have been already mentioned by @S-Dafarra, I just mention them separately for clarity):

Modify iDynTree core data structures and algorithms to enable autodiff

In a nutshell, it boils down to modify all the data structures and functions from:

class VectorDynSize

to

template<typename T> class VectorDynSizeTempl {
...
};

typedef VectorDynSizeTempl<double> VectorDynSize;

And then try to instantiate all the algoritms with an appropriate autodiff scalar, and deal with all the problem you will find. : )

Note that given that with C++17 and Eigen 3.4 all the issue related to EIGEN_MAKE_ALIGNED_OPERATOR_NEW have been fixed (see http://eigen.tuxfamily.org/index.php?title=3.4#Alignment) it may be an option to just use Eigen matrices in iDynTree datatypes, using Eigen in public headers. Unfortunately Ubuntu 20.04 still ships with Eigen 3.3, so if we want to do so we need either to wait for Ubuntu 22.04 or depend on a non-system Eigen (or just temporary accept all the downside of the EIGEN_MAKE_ALIGNED_OPERATOR_NEW-related problems).

However, if you are interested in minimizing computational speed, that may not be the best options as due to some internal choices, for sure iDynTree is not the fastest possible implementation of rigid body dynamics algorithms. For this reason, I think the next options is appealing.

Create a KinDynComputations interface to support different KinDynComputations implementations

As 95% of the users of iDynTree actually use the KinDynComputations class that embeds a lot of functionalities we are used to (not last the specific kind of floating base dynamics equation that we have used in almost all our papers and software for the last 5 years), a possible way to proceed is to create a KinDynComputations interface, that exposes the same functions of KinDynComputations, but offloading the actual computations to another library, for example pinocchio. Note some functions of the interface could be marked as "optional", and could be implemented or not depending on the different implementation. This would permit to add new functions that exploit autodiff features that would be implemented only on the pinocchio backend, and at the same time disable for new backends some seldom used features (such as setFrameVelocityRepresentation ) that may be more complex to implement.

The main advantage of this approach is that we would automatically get a much faster KinDynComputations for all the applications that benefit from it (such as optimization/learning), but maintaining all the nice features that we are used to (for example Matlab/Simulink bindings, that is not something usual in other Rigid Body Dynamics libraries).

Implementation detail on a pinocchio-based KinDynComputations backend

In here I add a few notes regarding the implementation of a pinocchio-based KinDynComputations backend.

First of all, to avoid I would start to try start writing a function that converts a iDynTree::Model to a pinocchio::ModelTmpl. In this way, we bypass any kind of different behaviour between pinocchio's URDF parser and iDynTree's URDF parser, and we make sure that the backend works correctly if in the future we add more parsers (such as SDF parser as discussed in #481).

Then, I am afraid that Pinocchio is not as flexible as iDynTree regarding JointIndeces, so we need to make sure that Jacobian, matrices and other quantities serialized according to the joint and link order are properly converted when passing from Pinocchio to iDynTree.

Finally, I would look into developing some functions that convert the quantities from the convention used in Pinocchio to the one used in iDynTree. After writing down the mathematical details of the convention used in Pinocchio and the one used in iDynTree (that are the one we used in all our papers, at least when using the defautl "mixed" velocity representation). As a first step, I would just target the mixed representation that is the one used by 99% of KinDynComputations users. The logic of conversions would probably me similar to the existing process and convert methods already present in KinDynComputations to convert from BODY_FIXED (the convention used in all Featherstone algorithms and that is used internally also in iDynTree, and so I guess used also in Pinocchio) to MIXED :

• idyntree/src/high-level/src/KinDynComputations.cpp

  Line 1488 in d5bbcea

  // Process right side of the jacobian

• idyntree/src/high-level/src/KinDynComputations.cpp

  Line 759 in d5bbcea

  // Account for the different possible representations

• idyntree/src/high-level/src/KinDynComputations.cpp

  Line 1884 in d5bbcea

  // Convert input base acceleration

  
  You can find more info about this in Computation of "Classic" Cartesian velocity of a frame stack-of-tasks/pinocchio#1140 . Note that it is important that also this conversion functions are templated, as if you want to compute the autodiff of the massmatrix in the iDynTree convention, you need to take in consideration also the effect of this transformations.

Once you have done so, testing is fortunatly easy as we already have an existing KinDynComputations that can serve as a reference, and so we can just test the methods of KinDynComputations for the two backends on an arbitrary model and arbitrary inputs, and verify that the one computed by a pinocchio-backend are faster.

[traversaro]

I think that developing a pinocchio-based KinDynComputations backend is probably the best option at the moment, and I don't think it requires a crazy amount of time. If anyone wants to work on it, I would be happy to provide support and handle the CMake and CI part, if necessary.

[GiulioRomualdi]

What do you think to schedule a meeting for discussing this? @traversaro @S-Dafarra

[traversaro]

What do you think to schedule a meeting for discussing this? @traversaro @S-Dafarra

When you want, those are the kind of topics over I can also have beer meetings.

[jcarpent]

Looks very promising. If from the pinocchio's side, we can provide any help, it would be with great pleasure (and much more around a beer ;))

[traversaro]

Today we had a brief discussion with @S-Dafarra @GiulioRomualdi and @diegoferigo .

Main points:

• Having a new KinDynComputations backend is not interesting, as the main use case is to have a KinDynComputations-like object that does not return numerical quantities (vectors, matrices, tensors) given the input parameters (position, velocities, acceleration) but rather returns data structures that can them-self be further combined (for example to compute the cost function of an optimization problem). So the idea is to build a KinDynComputations-like class, but that instead of returning actual vector and matrices returns directly CasADi data structures that can be further processed. So while a new KinDynComputations backend, we are still interested in making sure that the new class have exactly the same semantics of the original KinDynComputations , taking as input a iDynTree::Model (for this we have a separate issue: Document semantics difference between Pinocchio quantities and iDynTree KinDynComputations quantities  #674).
• As the new class will return CasADi data structures, apparently there is not a lot of interested in having them exposedd in WB-Toolbox. On the other hand, it may be of interest to make sure that it interoperates correctly at the Matlab and Python level with the corresponding CasADi bindings. This is however something that can be checked later, as it may be tricky.

Given that the discussion has been done, I think we can close this issue and open new issues on the specific issues as soon as they are clear (I already opened #674 for example ).

[traversaro]

While I guess that CasADi + Pinocchio may be a good way to go for us, for the logistics of dealing with Ipopt/CasADi on Windows/macOS/Linux and on C++/Python/Matlab, it may be worth to check https://github.com/opensim-org/opensim-moco .

[GiulioRomualdi]

Regarding the discussion that we had yesterday @DanielePucci @Giulero