ami-iit / adam

adam implements a collection of algorithms for calculating rigid-body dynamics in Jax, CasADi, PyTorch, and Numpy.
https://adam-docs.readthedocs.io/en/latest/
BSD 3-Clause "New" or "Revised" License
131 stars 20 forks source link

Compatibility with MX variables #81

Closed AFusco99 closed 4 months ago

AFusco99 commented 4 months ago

Hello, I wanted to know if you had any suggestion on how to make this library compatible with casadi MX symbolic variables. The joint angles of my manipulator are currently only available in this format and I would like to use your library for the computation of forward and inverse kynematics. Thank you very much

DanielePucci commented 4 months ago

CC @Giulero

Giulero commented 4 months ago

Hi @AFusco99! Can you provide a snippet of code on how you're using the library? Thanks!

AFusco99 commented 4 months ago

Yes, thanks

import casadi as cs
import numpy as np
model_path = "panda.urdf"
import adam
from adam.casadi import KinDynComputations

joints_name_list = [
           'panda_joint1', 'panda_joint2', 'panda_joint3', 'panda_joint4',
           'panda_joint5', 'panda_joint6', 'panda_joint7'
            ]
joint_angles=cs.MX.sym('q', 7)
root_link = 'panda_link0'
w_H_b = np.eye(4)
 kinDyn = KinDynComputations(model_path, joints_name_list, root_link)
frame="panda_hand"
Tmat=(kinDyn.forward_kinematics(frame,w_H_b,joint_angles))

This code is functional as long as joint_angles is defined as an SX symbolic variable however in my case I need to define it as an MX symbolic variable as I am using external functions and CasADI only supports MX variables in these cases.

Giulero commented 4 months ago

Got it! It shouldn't be possible to convert SX to MX and vice-versa (it seems open https://github.com/casadi/casadi/issues/1870).

I created a branch with some modifications in order to let adam accept also MX types.

I opened this PR: https://github.com/ami-iit/adam/pull/84. You might test changing the branch!

Giulero commented 4 months ago

Fyi: PR #84 merged

Giulero commented 4 months ago

Hi @AFusco99 ! I just noticed that you are using the forward_kinematics function.

I guess also that you could use the function forward_kinematics_fun function. This function returns the casadi function directly, and it should be possible to use it with SX, MX and DM (and numpy).

Your snippet would become

import casadi as cs
import numpy as np
model_path = "panda.urdf"
import adam
from adam.casadi import KinDynComputations

joints_name_list = [
           'panda_joint1', 'panda_joint2', 'panda_joint3', 'panda_joint4',
           'panda_joint5', 'panda_joint6', 'panda_joint7'
            ]
joint_angles=cs.MX.sym('q', 7)
root_link = 'panda_link0'
w_H_b = np.eye(4)
 kinDyn = KinDynComputations(model_path, joints_name_list, root_link)
frame="panda_hand"
T_mat_fun = kinDyn.forward_kinematics_fun(frame)
Tmat=T_mat_fun(w_H_b,joint_angles)
Giulero commented 4 months ago

@AFusco99, please have a look at #89. Anyway, using the *fun version of the function, you should be able to use MX!

AFusco99 commented 4 months ago

Thank you I just tested both options and I was able to work with Mx variables