Are AD (CasADi)-interface capabilities possible in Pinocchio for Python?

[alejandroastudillo]

Dear all,

I have built a C++ "library" which generates CasADi's symbolic expressions for robot dynamics, kinematics, joint torque regressor, generalized gravity, etc, from a URDF file. This library depends on Eigen, Pinocchio and CasADi.

I was wondering if it is possible to create such "library/module" in Python 3, which depends on EigenPy, Pinocchio and Casadi?

I am asking this because I have started to build a Python module that eases the deployment of Model Predictive Control for Robots, and after several tests I have confirmed that Pinocchio is the way to go in order to easily generate computationally efficient functions for a robot.

[cmastalli]

Dear @alejandroastudillo

Inside Gepetto team and MEMMO project, we're working on very efficient optimal control for robots. The name of our library is Crocoddyl, and you can find it here: https://gepgitlab.laas.fr/loco-3d/crocoddyl

Crocoddyl has novel solvers and very efficient formulations. We solve optimal control problems for quadrupeds, humanoids, etc at very high frequencies (from 200-800 Hz). Furthermore, we can generate highly-dynamic manoeuvres within few iterations and milliseconds.

For more information you can also read the following submitted paper: https://cmastalli.github.io/publications/crocoddyl20unpub.html and contact me in private.

Best, Carlos

PD: currently I am integrating CodeGen with CppAD and multithreading. We expect to speed up between 2-4 times our computations.

[jcarpent]

Dear @alejandroastudillo,

First of all, thank you very much for your useful feedback on your current usage of Pinocchio.

I can confirm that I'm working on making AutoDiff and CodeGen accessible in Python. This is ongoing work, where I'm also trying to really expose Eigen::Matrix and Eigen::Vector with autodiff scalars in EigenPy. I also know that this feature would be essential for @andreadelprete and @thomasfla for instance.

If you want to join the task force, your help is very welcome and will be appreciated.

[alejandroastudillo]

Thank you for your fast reply.

@cmastalli your work is really interesting. I'll give it a try starting with the arm_manipulator example, since is the one of greatest interest for me. The idea of the module I am building is to fill a gap inside a more complex toolchain built in my research team, but Crocoddyl seems like a good complement for what the toolchain aims to do.

@jcarpent it's nice to read that you're already heading towards the AD access from Python's Pinocchio. I am open for joining the task force. I will try to help as much as I can.

[cmastalli]

If our tool is useful for you, you can help us to make better!

[alejandroastudillo]

@cmastalli yes it is. I will first focus on building my toolchain, but I'll check more about Crocoddyl later and see what can I improve there.

@jcarpent Are you working on AutoDiff/CodeGen in your own fork/branch of EigenPy/Pinocchio?

[xinsongyan]

It would be very nice to have such kind of interface!

[cmastalli]

It would be very nice to have such kind of interface!

The feature is targeted to v3.x!

[jcarpent]

@xinsongyan If you need this feature sooner, your help is very welcome to implement the feature ;)

[xinsongyan]

@xinsongyan If you need this feature sooner, your help is very welcome to implement the feature ;)

I am very happy to help, but how can I get involved?

[alejandroastudillo]

If possible I would also like to contribute to this feature, but don’t really know how @jcarpent @xinsongyan

[jmirabel]

If you want to help on this topic, the first thing you should do is to get familiar with the autodiff in C++. A lot of the autodiff logic happens at C++ compile time thanks to template programming. So there are two options to bind the auto-differentiation:

• define a set of function for which the automatic differentiation will be exposed.
• find a way of postponing the autodiff logic at runtime. I have no idea whether this is doable, and even if it is, if it is worth it.

[jmirabel]

* I have no idea whether this is doable, and even if it is, if it is worth it.

rephrasing: this is very likely doable but at what price on the computation time...

[jcarpent]

@alejandroastudillo @xinsongyan First of all, thanks a lot for your involvement in this project.

For Pinocchio I see the change as follows:

• change the whole c++ library to accept a macro default scalar type
• change the Python bindings to compile for any default scalar type to expose SE3, Motion everything inside a dedicated lib with a prefix (casadi, cppad, cppadcodegen, etc.). This Python lib will be a submodule of the main library which will remain with the default double type. So when doing from pinocchio import SE3, SE3 will be SE3<double> but when writting from pinocchio.casadi import SE3, it will use SE3<casadi_type>.
• expose thanks to eigenpy numpy.array to accept the AD type. It may be required to expose the AD type in Python (already done for casadi)

[cmastalli]

@jcarpent do we need to do the same at eigenpy level? I am asking this question for CppAD case too.

[jmirabel]

* change the whole c++ library to accept a macro default scalar type

If you allow C++ >= 11 features, then I think you can use template namespaces.

[jcarpent]

@jmirabel Do you know how this feature is really available? I've found only a little information on it on the web.

[jmirabel]

I will if I find it back, but I can't. From what I read on the web, it seems I misread something...

[jcarpent]

No problem. Just to be sure that I was not looking in the correct direction.

[cmastalli]

@jcarpent toward this milestone with especial focuses on CppAD.

Do we need to first create eigenpy objects that handle different scalars? I have in mind something like:

  q = np.zero((3,2), dtype=adouble)
  pinocchio.cppad.forwardKinematicas(model, data, q)

If my understanding is right, we need to define dtype for CppAD (see this code in pyadolc: https://github.com/b45ch1/pyadolc/blob/master/adolc/src/py_adolc.hpp#L400). It might needed to implement it inside eigenpy, not in a different project (e.g. PyCppAD), what do you think?

Then, we need that np.zero((3,2), dtype=adouble) have been interpreted as Eigen::MatrixBase<adouble, ..>. I don't have any idea about this part.

Am I correct? If not, could we elaborate a development strategy? It would help to bring people into the development of this feature.

[jcarpent]

Concerning CppAD, I'm implementing the Python bindings which will be sync with the official repository.

For the dtype argument, this is indeed the way to allocate a CppAD vector with numpy. The issue is that this thing also to be implemented inside EigenPy which only supports currently complex, float, double and long double types. This is the most blocking part currently.

[cmastalli]

Concerning CppAD, I'm implementing the Python bindings which will be sync with the official repository.

cool

For the dtype argument, this is indeed the way to allocate a CppAD vector with numpy. The issue is that this thing also to be implemented inside EigenPy which only supports currently complex, float, double and long double types. This is the most blocking part currently.

If you have a clear path of development, then we could have a chat to define a set of tasks. I would be happy to contribute on it.

[jcarpent]

The main question is how to create Python array for a default user type, because, by default, Numpy only declares a few common types.

[cmastalli]

The main question is how to create Python array for a default user type, because, by default, Numpy only declares a few common types.

Pyadolc is doing that. If I understood correctly the code, you need to define with all the methods that you can see here: https://github.com/b45ch1/pyadolc/blob/master/adolc/src/py_adolc.hpp#L339

[jcarpent]

No, what you want is that a numpy Array holds some type which are CppAD::AD, and the same for CppAD code gen. But the thing is that you need to register this type as a Python type and to let Numpy managing the memory and so on.

[traikodinev]

What is the status of this feature at the moment? The pinocchio-preview branch seems to be different from master, but devel and master are the same.

Also, how do I use this? I need to compile from source with Casadi enabled, right? Is it possible to do so without compiling Casadi itself (which I gather is not recommended)?

[jcarpent]

@proyan Could you guide @traikodinev?

[proyan]

Hi @traikodinev This feature is fully functional and will be released with pinocchio3. Also, a functioning beta version is also available in pinocchio3-preview branch. In order to install it, you would need to -> install casadi -> have the latest release of EigenPy, -> and install pinocchio3-preview from source with -DBUILD_WITH_CASADI=ON and -DBUILD_PYTHON_INTERFACE=ON in your cmake argument list.

Once it is installed, you can access pinocchio functions and classes in casadi scalar by calling from pinocchio.casadi module instead of pinocchio. So you would call

import pinocchio.casadi as cpin
cpin.rnea(...)

instead of

import pinocchio as pin
pin.rnea(...)

[jcarpent]

Done in Pinocchio-3x which should be released soon.