added model parametric and first version of casadi computation parame…

…tric

src/adam/casadi/computations_parametric.py

@@ -0,0 +1,179 @@
+# Copyright (C) 2021 Istituto Italiano di Tecnologia (IIT). All rights reserved.
+# This software may be modified and distributed under the terms of the
+# GNU Lesser General Public License v2.1 or any later version.
+
+import casadi as cs
+import numpy as np
+
+from adam.casadi.casadi_like import SpatialMath
+from adam.core import RBDAlgorithms
+from adam.model import Model, URDFModelFactory, URDFParametricModelFactory
+
+
+class KinDynComputationsParametric:
+    """This is a small class that retrieves robot quantities represented in a symbolic fashion using CasADi
+    in mixed representation, for Floating Base systems - as humanoid robots.
+    """
+
+    def __init__(
+        self,
+        urdfstring: str,
+        joints_name_list: list,
+        link_name_list:list,  
+        root_link: str = "root_link",
+        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:
+        """
+        Args:
+            urdfstring (str): path of the urdf
+            joints_name_list (list): list of the actuated joints
+            root_link (str, optional): the first link. Defaults to 'root_link'.
+        """
+        math = SpatialMath()
+        n_param_link = len(link_name_list)
+        self.density = cs.SX.sym("density", n_param_link)
+        self.length_multiplier = cs.SX.sym("length_multiplier",n_param_link)
+        factory = URDFParametricModelFactory(path=urdfstring, math=math,link_parametric_list=link_name_list,lenght_multiplier=lenght_multiplier, density=density)
+        model = Model.build(factory=factory, joints_name_list=joints_name_list)
+        self.rbdalgos = RBDAlgorithms(model=model, math=math)
+        self.NDoF = self.rbdalgos.NDoF
+        self.g = gravity
+        self.f_opts = f_opts
+
+    def mass_matrix_fun(self) -> cs.Function:
+        """Returns the Mass Matrix functions computed the CRBA
+
+        Returns:
+            M (casADi function): Mass Matrix
+        """
+        T_b = cs.SX.sym("T_b", 4, 4)
+        s = cs.SX.sym("s", self.NDoF)
+        [M, _] = self.rbdalgos.crba(T_b, s)
+        return cs.Function("M", [T_b, s, self.length_multiplier, self.density], [M.array], self.f_opts)
+
+    def centroidal_momentum_matrix_fun(self) -> cs.Function:
+        """Returns the Centroidal Momentum Matrix functions computed the CRBA
+
+        Returns:
+            Jcc (casADi function): Centroidal Momentum matrix
+        """
+        T_b = cs.SX.sym("T_b", 4, 4)
+        s = cs.SX.sym("s", self.NDoF)
+        [_, Jcm] = self.rbdalgos.crba(T_b, s)
+        return cs.Function("Jcm", [T_b, s,self.length_multiplier, self.density], [Jcm.array], self.f_opts)
+
+    def forward_kinematics_fun(self, frame: str) -> cs.Function:
+        """Computes the forward kinematics relative to the specified frame
+
+        Args:
+            frame (str): The frame to which the fk will be computed
+
+        Returns:
+            T_fk (casADi function): The fk represented as Homogenous transformation matrix
+        """
+        s = cs.SX.sym("s", self.NDoF)
+        T_b = cs.SX.sym("T_b", 4, 4)
+        T_fk = self.rbdalgos.forward_kinematics(frame, T_b, s)
+        return cs.Function("T_fk", [T_b, s,self.length_multiplier, self.density], [T_fk.array], self.f_opts)
+
+    def jacobian_fun(self, frame: str) -> cs.Function:
+        """Returns the Jacobian relative to the specified frame
+
+        Args:
+            frame (str): The frame to which the jacobian will be computed
+
+        Returns:
+            J_tot (casADi function): The Jacobian relative to the frame
+        """
+        s = cs.SX.sym("s", self.NDoF)
+        T_b = cs.SX.sym("T_b", 4, 4)
+        J_tot = self.rbdalgos.jacobian(frame, T_b, s)
+        return cs.Function("J_tot", [T_b, s,self.length_multiplier, self.density], [J_tot.array], self.f_opts)
+
+    def relative_jacobian_fun(self, frame: str) -> cs.Function:
+        """Returns the Jacobian between the root link and a specified frame frames
+
+        Args:
+            frame (str): The tip of the chain
+
+        Returns:
+            J (casADi function): The Jacobian between the root and the frame
+        """
+        s = cs.SX.sym("s", self.NDoF)
+        J = self.rbdalgos.relative_jacobian(frame, s)
+        return cs.Function("J", [s,self.length_multiplier, self.density], [J.array], self.f_opts)
+
+    def CoM_position_fun(self) -> cs.Function:
+        """Returns the CoM positon
+
+        Returns:
+            com (casADi function): The CoM position
+        """
+        s = cs.SX.sym("s", self.NDoF)
+        T_b = cs.SX.sym("T_b", 4, 4)
+        com_pos = self.rbdalgos.CoM_position(T_b, s)
+        return cs.Function("CoM_pos", [T_b, s,self.length_multiplier, self.density], [com_pos.array], self.f_opts)
+
+    def bias_force_fun(self) -> cs.Function:
+        """Returns the bias force of the floating-base dynamics equation,
+        using a reduced RNEA (no acceleration and external forces)
+
+        Returns:
+            h (casADi function): the bias force
+        """
+        T_b = cs.SX.sym("T_b", 4, 4)
+        s = cs.SX.sym("s", self.NDoF)
+        v_b = cs.SX.sym("v_b", 6)
+        s_dot = cs.SX.sym("s_dot", self.NDoF)
+        h = self.rbdalgos.rnea(T_b, s, v_b, s_dot, self.g)
+        return cs.Function("h", [T_b, s, v_b, s_dot,self.length_multiplier, self.density], [h.array], self.f_opts)
+
+    def coriolis_term_fun(self) -> cs.Function:
+        """Returns the coriolis term of the floating-base dynamics equation,
+        using a reduced RNEA (no acceleration and external forces)
+
+        Returns:
+            C (casADi function): the Coriolis term
+        """
+        T_b = cs.SX.sym("T_b", 4, 4)
+        q = cs.SX.sym("q", self.NDoF)
+        v_b = cs.SX.sym("v_b", 6)
+        q_dot = cs.SX.sym("q_dot", self.NDoF)
+        # set in the bias force computation the gravity term to zero
+        C = self.rbdalgos.rnea(T_b, q, v_b, q_dot, np.zeros(6))
+        return cs.Function("C", [T_b, q, v_b, q_dot,self.length_multiplier, self.density], [C.array], self.f_opts)
+
+    def gravity_term_fun(self) -> cs.Function:
+        """Returns the gravity term of the floating-base dynamics equation,
+        using a reduced RNEA (no acceleration and external forces)
+
+        Returns:
+            G (casADi function): the gravity term
+        """
+        T_b = cs.SX.sym("T_b", 4, 4)
+        q = cs.SX.sym("q", self.NDoF)
+        # set in the bias force computation the velocity to zero
+        G = self.rbdalgos.rnea(T_b, q, np.zeros(6), np.zeros(self.NDoF), self.g)
+        return cs.Function("G", [T_b, q,self.length_multiplier, self.density], [G.array], self.f_opts)
+
+    def forward_kinematics(self, frame, T_b, s) -> cs.Function:
+        """Computes the forward kinematics relative to the specified frame
+
+        Args:
+            frame (str): The frame to which the fk will be computed
+
+        Returns:
+            T_fk (casADi function): The fk represented as Homogenous transformation matrix
+        """
+
+        return self.rbdalgos.forward_kinematics(frame, T_b, s,self.length_multiplier, self.density)
+
+    #TODO 
+    def get_total_mass(self) -> float:
+        """Returns the total mass of the robot
+
+        Returns:
+            mass: The total mass
+        """
+        return self.rbdalgos.get_total_mass()

src/adam/model/parametric_factories/parametric_joint.py

@@ -0,0 +1,129 @@
+from typing import Union
+
+import numpy.typing as npt
+import urdf_parser_py.urdf
+
+from adam.core.spatial_math import SpatialMath
+from adam.model import Joint
+from adam.model.parametric_factories import link_parametric
+
+class ParmetricJoint(Joint):
+    """Parametric Joint class"""
+
+    def __init__(
+        self,
+        joint: urdf_parser_py.urdf.Joint,
+        math: SpatialMath,
+        parent_link:link_parametric,
+        idx: Union[int, None] = None,
+    ) -> None:
+        self.math = math
+        self.name = joint.name
+        self.parent = jparent_link
+        self.child = joint.child
+        self.type = joint.joint_type
+        self.axis = joint.axis
+        self.limit = joint.limit
+        self.idx = idx
+
+        joint_offset = self.parent_link.compute_joint_offset(joint, self.parent_link.offset)
+        self.offset = joint_offset
+        self.origin = self.modify(self.parent_link.offset)
+
+    def modify(self, parent_joint_offset):
+        length = self.parent_link.get_principal_lenght_parametric()
+        # Ack for avoiding depending on casadi 
+        vo = self.parent_link.origin[2]
+        xyz_rpy = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
+        xyz_rpy[0] = self.joint.origin[0,3]
+        xyz_rpy[1] = self.joint.origin[1,3]
+        xyz_rpy[2] = self.joint.origin[2,3]
+        xyz_rpy_temp=  matrix_to_xyz_rpy(self.joint.origin)
+        xyz_rpy[3] = xyz_rpy_temp[3]
+        xyz_rpy[4] = xyz_rpy_temp[4]
+        xyz_rpy[5] = xyz_rpy_temp[5]
+
+        if(xyz_rpy[2]<0): 
+            xyz_rpy[2] = -length +parent_joint_offset - self.offset   
+        else:
+            xyz_rpy[2] = vo+ length/2 - self.offset
+        return xyz_rpy
+
+    def homogeneous(self, q: npt.ArrayLike) -> npt.ArrayLike:
+        """
+        Args:
+            q (npt.ArrayLike): joint value
+
+        Returns:
+            npt.ArrayLike: the homogenous transform of a joint, given q
+        """
+
+        if self.type == "fixed":
+            xyz = self.origin.xyz
+            rpy = self.origin.rpy
+            return self.math.H_from_Pos_RPY(xyz, rpy)
+        elif self.type in ["revolute", "continuous"]:
+            return self.math.H_revolute_joint(
+                self.origin.xyz,
+                self.origin.rpy,
+                self.axis,
+                q,
+            )
+        elif self.type in ["prismatic"]:
+            return self.math.H_prismatic_joint(
+                self.origin.xyz,
+                self.origin.rpy,
+                self.axis,
+                q,
+            )
+
+    def spatial_transform(self, q: npt.ArrayLike) -> npt.ArrayLike:
+        """
+        Args:
+            q (npt.ArrayLike): joint motion
+
+        Returns:
+            npt.ArrayLike: spatial transform of the joint given q
+        """
+        if self.type == "fixed":
+            return self.math.X_fixed_joint(self.origin[:3], self.origin[3:])
+        elif self.type in ["revolute", "continuous"]:
+            return self.math.X_revolute_joint(
+                self.origin[:3], self.origin[3:], self.axis, q
+            )
+        elif self.type in ["prismatic"]:
+            return self.math.X_prismatic_joint(
+                self.origin[:3],
+                self.origin[3:],
+                self.axis,
+                q,
+            )
+
+    def motion_subspace(self) -> npt.ArrayLike:
+        """
+        Args:
+            joint (Joint): Joint
+
+        Returns:
+            npt.ArrayLike: motion subspace of the joint
+        """
+        if self.type == "fixed":
+            return self.math.vertcat(0, 0, 0, 0, 0, 0)
+        elif self.type in ["revolute", "continuous"]:
+            return self.math.vertcat(
+                0,
+                0,
+                0,
+                self.axis[0],
+                self.axis[1],
+                self.axis[2],
+            )
+        elif self.type in ["prismatic"]:
+            return self.math.vertcat(
+                self.axis[0],
+                self.axis[1],
+                self.axis[2],
+                0,
+                0,
+                0,
+            )