diff --git a/README.md b/README.md index b191128..fa1b559 100644 --- a/README.md +++ b/README.md @@ -211,6 +211,20 @@ w_H_b = np.eye(4) joints = np.ones(len(joints_name_list)) M = kinDyn.mass_matrix_fun() print(M(w_H_b, joints)) + +# If you want to use the symbolic version +w_H_b = cs.SX.eye(4) +joints = cs.SX.sym('joints', len(joints_name_list)) +M = kinDyn.mass_matrix_fun() +print(M(w_H_b, joints)) + +# This is usable also with casadi.MX +w_H_b = cs.MX.eye(4) +joints = cs.MX.sym('joints', len(joints_name_list)) +M = kinDyn.mass_matrix_fun() +print(M(w_H_b, joints)) + + ``` ### PyTorch interface diff --git a/src/adam/casadi/casadi_like.py b/src/adam/casadi/casadi_like.py index 1a9ae88..b8a5be4 100644 --- a/src/adam/casadi/casadi_like.py +++ b/src/adam/casadi/casadi_like.py @@ -104,17 +104,16 @@ def T(self) -> "CasadiLike": class CasadiLikeFactory(ArrayLikeFactory): - def __init__(self, cs_type: Union[cs.SX, cs.DM]): - self.cs_type = cs_type - - def zeros(self, *x: int) -> "CasadiLike": + @staticmethod + def zeros(*x: int) -> "CasadiLike": """ Returns: CasadiLike: Matrix of zeros of dim *x """ - return CasadiLike(self.cs_type.zeros(*x)) + return CasadiLike(cs.SX.zeros(*x)) - def eye(self, x: int) -> "CasadiLike": + @staticmethod + def eye(x: int) -> "CasadiLike": """ Args: x (int): matrix dimension @@ -122,20 +121,21 @@ def eye(self, x: int) -> "CasadiLike": Returns: CasadiLike: Identity matrix """ - return CasadiLike(self.cs_type.eye(x)) + return CasadiLike(cs.SX.eye(x)) - def array(self, *x) -> "CasadiLike": + @staticmethod + def array(*x) -> "CasadiLike": """ Returns: CasadiLike: Vector wrapping *x """ - return CasadiLike(self.cs_type(*x)) + return CasadiLike(cs.SX(*x)) class SpatialMath(SpatialMath): - def __init__(self, cs_type: Union[cs.SX, cs.DM]): - super().__init__(CasadiLikeFactory(cs_type)) + def __init__(self): + super().__init__(CasadiLikeFactory) @staticmethod def skew(x: Union["CasadiLike", npt.ArrayLike]) -> "CasadiLike": diff --git a/src/adam/casadi/computations.py b/src/adam/casadi/computations.py index 679baf9..fd87532 100644 --- a/src/adam/casadi/computations.py +++ b/src/adam/casadi/computations.py @@ -22,7 +22,6 @@ def __init__( urdfstring: str, joints_name_list: list = None, root_link: str = "root_link", - cs_type: Union[cs.SX, cs.DM] = cs.SX, gravity: np.array = np.array([0.0, 0.0, -9.80665, 0.0, 0.0, 0.0]), f_opts: dict = dict(jit=False, jit_options=dict(flags="-Ofast"), cse=True), ) -> None: @@ -32,7 +31,7 @@ def __init__( joints_name_list (list): list of the actuated joints root_link (str, optional): the first link. Defaults to 'root_link'. """ - math = SpatialMath(cs_type) + math = SpatialMath() factory = URDFModelFactory(path=urdfstring, math=math) model = Model.build(factory=factory, joints_name_list=joints_name_list) self.rbdalgos = RBDAlgorithms(model=model, math=math) @@ -239,8 +238,13 @@ def mass_matrix( joint_positions (Union[cs.SX, cs.DM]): The joints position Returns: - M (jax): Mass Matrix + M (Union[cs.SX, cs.DM]): Mass Matrix """ + if isinstance(base_transform, cs.MX) and isinstance(joint_positions, cs.MX): + raise ValueError( + "You are using casadi MX. Please use the function KinDynComputations.mass_matrix_fun()" + ) + [M, _] = self.rbdalgos.crba(base_transform, joint_positions) return M.array @@ -256,6 +260,11 @@ def centroidal_momentum_matrix( Returns: Jcc (Union[cs.SX, cs.DM]): Centroidal Momentum matrix """ + if isinstance(base_transform, cs.MX) and isinstance(joint_positions, cs.MX): + raise ValueError( + "You are using casadi MX. Please use the function KinDynComputations.centroidal_momentum_matrix_fun()" + ) + [_, Jcm] = self.rbdalgos.crba(base_transform, joint_positions) return Jcm.array @@ -269,6 +278,11 @@ def relative_jacobian(self, frame: str, joint_positions: Union[cs.SX, cs.DM]): Returns: J (Union[cs.SX, cs.DM]): The Jacobian between the root and the frame """ + if isinstance(joint_positions, cs.MX): + raise ValueError( + "You are using casadi MX. Please use the function KinDynComputations.relative_jacobian_fun()" + ) + return self.rbdalgos.relative_jacobian(frame, joint_positions).array def jacobian_dot( @@ -291,6 +305,15 @@ def jacobian_dot( Returns: Jdot (Union[cs.SX, cs.DM]): The Jacobian derivative relative to the frame """ + if ( + isinstance(base_transform, cs.MX) + and isinstance(joint_positions, cs.MX) + and isinstance(base_velocity, cs.MX) + and isinstance(joint_velocities, cs.MX) + ): + raise ValueError( + "You are using casadi MX. Please use the function KinDynComputations.jacobian_dot_fun()" + ) return self.rbdalgos.jacobian_dot( frame, base_transform, joint_positions, base_velocity, joint_velocities ).array @@ -311,6 +334,11 @@ def forward_kinematics( Returns: H (Union[cs.SX, cs.DM]): The fk represented as Homogenous transformation matrix """ + if isinstance(base_transform, cs.MX) and isinstance(joint_positions, cs.MX): + raise ValueError( + "You are using casadi MX. Please use the function KinDynComputations.forward_kinematics_fun()" + ) + return self.rbdalgos.forward_kinematics( frame, base_transform, joint_positions ).array @@ -326,6 +354,11 @@ def jacobian(self, frame: str, base_transform, joint_positions): Returns: J_tot (Union[cs.SX, cs.DM]): The Jacobian relative to the frame """ + if isinstance(base_transform, cs.MX) and isinstance(joint_positions, cs.MX): + raise ValueError( + "You are using casadi MX. Please use the function KinDynComputations.jacobian_fun()" + ) + return self.rbdalgos.jacobian(frame, base_transform, joint_positions).array def bias_force( @@ -347,6 +380,16 @@ def bias_force( Returns: h (Union[cs.SX, cs.DM]): the bias force """ + if ( + isinstance(base_transform, cs.MX) + and isinstance(joint_positions, cs.MX) + and isinstance(base_velocity, cs.MX) + and isinstance(joint_velocities, cs.MX) + ): + raise ValueError( + "You are using casadi MX. Please use the function KinDynComputations.bias_force_fun()" + ) + return self.rbdalgos.rnea( base_transform, joint_positions, base_velocity, joint_velocities, self.g ).array @@ -370,6 +413,16 @@ def coriolis_term( Returns: C (Union[cs.SX, cs.DM]): the Coriolis term """ + if ( + isinstance(base_transform, cs.MX) + and isinstance(joint_positions, cs.MX) + and isinstance(base_velocity, cs.MX) + and isinstance(joint_velocities, cs.MX) + ): + raise ValueError( + "You are using casadi MX. Please use the function KinDynComputations.coriolis_term_fun()" + ) + return self.rbdalgos.rnea( base_transform, joint_positions, @@ -391,6 +444,11 @@ def gravity_term( Returns: G (Union[cs.SX, cs.DM]): the gravity term """ + if isinstance(base_transform, cs.MX) and isinstance(joint_positions, cs.MX): + raise ValueError( + "You are using casadi MX. Please use the function KinDynComputations.gravity_term_fun()" + ) + return self.rbdalgos.rnea( base_transform, joint_positions, @@ -411,4 +469,9 @@ def CoM_position( Returns: CoM (Union[cs.SX, cs.DM]): The CoM position """ + if isinstance(base_transform, cs.MX) and isinstance(joint_positions, cs.MX): + raise ValueError( + "You are using casadi MX. Please use the function KinDynComputations.CoM_position_fun()" + ) + return self.rbdalgos.CoM_position(base_transform, joint_positions).array diff --git a/src/adam/parametric/casadi/computations_parametric.py b/src/adam/parametric/casadi/computations_parametric.py index 85f92dd..1247d2c 100644 --- a/src/adam/parametric/casadi/computations_parametric.py +++ b/src/adam/parametric/casadi/computations_parametric.py @@ -24,7 +24,6 @@ def __init__( joints_name_list: list, links_name_list: list, root_link: str = "root_link", - cs_type: Union[cs.SX, cs.DM] = cs.SX, gravity: np.array = np.array([0.0, 0.0, -9.80665, 0.0, 0.0, 0.0]), f_opts: dict = dict(jit=False, jit_options=dict(flags="-Ofast")), ) -> None: @@ -35,7 +34,7 @@ def __init__( links_name_list (list): list of the parametrized links root_link (str, optional): the first link. Defaults to 'root_link'. """ - math = SpatialMath(cs_type) + math = SpatialMath() n_param_links = len(links_name_list) self.densities = cs.SX.sym("densities", n_param_links) self.length_multiplier = cs.SX.sym("length_multiplier", n_param_links)