From b876aa511f84daf5c4de37ce1f285f16a4bb83a6 Mon Sep 17 00:00:00 2001 From: Carlotta Date: Wed, 11 Oct 2023 12:25:12 +0200 Subject: [PATCH] added model parametric and first version of casadi computation parametric --- src/adam/casadi/computations_parametric.py | 179 ++++++++++++++ .../model/parametric_factories/__init__.py | 0 .../parametric_factories/parametric_joint.py | 129 ++++++++++ .../parametric_factories/parametric_link.py | 229 ++++++++++++++++++ .../parametric_factories/parametric_model.py | 87 +++++++ 5 files changed, 624 insertions(+) create mode 100644 src/adam/casadi/computations_parametric.py create mode 100644 src/adam/model/parametric_factories/__init__.py create mode 100644 src/adam/model/parametric_factories/parametric_joint.py create mode 100644 src/adam/model/parametric_factories/parametric_link.py create mode 100644 src/adam/model/parametric_factories/parametric_model.py diff --git a/src/adam/casadi/computations_parametric.py b/src/adam/casadi/computations_parametric.py new file mode 100644 index 00000000..515e38a3 --- /dev/null +++ b/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() diff --git a/src/adam/model/parametric_factories/__init__.py b/src/adam/model/parametric_factories/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/src/adam/model/parametric_factories/parametric_joint.py b/src/adam/model/parametric_factories/parametric_joint.py new file mode 100644 index 00000000..4c086cbe --- /dev/null +++ b/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, + ) diff --git a/src/adam/model/parametric_factories/parametric_link.py b/src/adam/model/parametric_factories/parametric_link.py new file mode 100644 index 00000000..ab4564fa --- /dev/null +++ b/src/adam/model/parametric_factories/parametric_link.py @@ -0,0 +1,229 @@ +import numpy.typing as npt +import urdf_parser_py.urdf +from enum import Enum + +from adam.core.spatial_math import SpatialMath +from adam.model import Link + +class I_parametric(): + def __init__(self) -> None: + self.ixx = 0.0 + self.ixy = 0.0 + self.ixz = 0.0 + self.iyy = 0.0 + self.iyz = 0.0 + self.izz = 0.0 + +class Geometry(Enum): + """The different types of geometries that constitute the URDF""" + BOX = 1 + CYLINDER = 2 + SPHERE = 3 + +class Side(Enum): + """The possible sides of a box geometry""" + WIDTH = 1 + HEIGHT = 2 + DEPTH = 3 + + +class ParametricLink(Link): + """Parametric Link class""" + + def __init__(self, link: urdf_parser_py.urdf.Link, math: SpatialMath, length_multiplier, density): + self.math = math + self.name = link.name + self.length_multiplier = length_multiplier + self.density = density + self.visuals = link.visuals + self.geometry_type, self.visual_data = self.get_geometry(self.visuals) + self.link = link + self.link_offset = self.compute_offset() + (self.volume, self.visual_data_new) = self.compute_volume() + self.mass = self.compute_mass() + self.I = self.compute_inertia_parametric() + self.origin = self.modify_origin() + + # if the link has inertial properties, but the origin is None, let's add it + if link.inertial is not None and link.inertial.origin is None: + link.inertial.origin.xyz = [0, 0, 0] + link.inertial.origin.rpy = [0, 0, 0] + + def get_principal_lenght(self): + visual = self.get_visual() + xyz_rpy = matrix_to_xyz_rpy(visual.origin) + if self.geometry_type == Geometry.CYLINDER: + if(xyz_rpy[3] < 0.0 or xyz_rpy[4] > 0.0): + v_l = 2*self.visual_data.radius # returning the diameter, since the orientation of the shape is such that the radius is the principal lenght + else: + v_l=self.visual_data.length # returning the lenght, since the orientation of the shape is such that the radius is the principal lenght + elif(self.geometry_type == Geometry.SPHERE): + v_l = self.visual_data.radius + elif(self.geometry_type == Geometry.BOX): + v_l= self.visual_data.size[2] + else: + raise Exception(f"THE GEOMETRY IS NOT SPECIFIED") + return v_l + + def get_principal_lenght_parametric(self): + visual = self.get_visual() + xyz_rpy = matrix_to_xyz_rpy(visual.origin) + if self.geometry_type == Geometry.CYLINDER: + if(xyz_rpy[3] < 0.0 or xyz_rpy[4] > 0.0): + v_l = 2*self.visual_data_new[1] # returning the diameter, since the orientation of the shape is such that the radius is the principal lenght + else: + v_l=self.visual_data_new[0] # returning the lenght, since the orientation of the shape is such that the radius is the principal lenght + elif(self.geometry_type == Geometry.SPHERE): + v_l = self.visual_data_new + elif(self.geometry_type == Geometry.BOX): + v_l= self.visual_data_new[2] + else: + raise Exception(f"THE GEOMETRY IS NOT SPECIFIED") + return v_l + + def compute_offset(self): + visual = self.get_visual() + xyz_rpy = matrix_to_xyz_rpy(visual.origin) + v_l= self.get_principal_lenght() + v_o = xyz_rpy[2] + if(v_o<0): + link_offset = v_l/2 + v_o + else: + link_offset = (v_o - v_l/2) + return link_offset + + def compute_joint_offset(self,joint_i, parent_offset): + # Taking the principal direction i.e. the length + visual = self.get_visual() + xyz_rpy = matrix_to_xyz_rpy(visual.origin) + v_l= self.get_principal_lenght() + j_0 = matrix_to_xyz_rpy(joint_i.origin)[2] + v_o = xyz_rpy[2] + if(j_0<0): + joint_offset_temp = -(v_l + j_0-parent_offset) + joint_offset = joint_offset_temp + else: + joint_offset_temp = v_l + parent_offset - j_0 + joint_offset = joint_offset_temp + return joint_offset + + @staticmethod + def get_geometry(visual_obj): + """Returns the geometry type and the corresponding geometry object for a given visual""" + if visual_obj.geometry.box is not None: + return [Geometry.BOX, visual_obj.geometry.box] + if visual_obj.geometry.cylinder is not None: + return [Geometry.CYLINDER, visual_obj.geometry.cylinder] + if visual_obj.geometry.sphere is not None: + return [Geometry.SPHERE, visual_obj.geometry.sphere] + + """Function that starting from a multiplier and link visual characteristics computes the link volume""" + def compute_volume(self): + volume = 0.0 + """Modifies a link's volume by a given multiplier, in a manner that is logical with the link's geometry""" + if self.geometry_type == Geometry.BOX: + visual_data_new =[0.0, 0.0, 0.0] + visual_data_new[0] = self.visual_data.size[0] * self.length_multiplier[0] + visual_data_new[1] = self.visual_data.size[1] * self.length_multiplier[1] + visual_data_new[2] = self.visual_data.size[2] * self.length_multiplier[2] + volume = visual_data_new[0] * visual_data_new[1] * visual_data_new[2] + elif self.geometry_type == Geometry.CYLINDER: + visual_data_new = [0.0, 0.0] + visual_data_new[0] = self.visual_data.length * self.length_multiplier[0] + visual_data_new[1] = self.visual_data.radius * self.length_multiplier[1] + volume = math.pi * visual_data_new[1] ** 2 * visual_data_new[0] + elif self.geometry_type == Geometry.SPHERE: + visual_data_new = 0.0 + visual_data_new = self.visual_data.radius * self.length_multiplier[0] + volume = 4 * math.pi * visual_data_new ** 3 / 3 + return volume, visual_data_new + + """Function that computes the mass starting from the density, the length multiplier and the link""" + def compute_mass(self): + """Changes the mass of a link by preserving a given density.""" + mass = 0.0 + mass = self.volume * self.density + return mass + + def modify_origin(self): + origin = [0.0,0.0,0.0,0.0,0.0,0.0] + visual = self.get_visual() + """Modifies the position of the origin by a given amount""" + xyz_rpy = matrix_to_xyz_rpy(visual.origin) + v_o = xyz_rpy[2] + length = self.get_principal_lenght_parametric() + if(v_o<0): + origin[2] = self.offset-length/2 + origin[0] = xyz_rpy[0] + origin[1] = xyz_rpy[1] + origin[3] = xyz_rpy[3] + origin[4] = xyz_rpy[4] + origin[5] = xyz_rpy[5] + else: + origin[2] = length/2 +self.offset + origin[0] = xyz_rpy[0] + origin[1] = xyz_rpy[1] + origin[3] = xyz_rpy[3] + origin[4] = xyz_rpy[4] + origin[5] = xyz_rpy[5] + if self.geometry_type == Geometry.SPHERE: + "in case of a sphere the origin of the framjoint_name_list[0]:link_parametric.JointCharacteristics(0.0295),e does not change" + origin = xyz_rpy + return origin + + def compute_inertia_parametric(self): + I = I_parametric + visual = self.get_visual() + xyz_rpy = matrix_to_xyz_rpy(visual.origin) + """Calculates inertia (ixx, iyy and izz) with the formula that corresponds to the geometry + Formulas retrieved from https://en.wikipedia.org/wiki/List_of_moments_of_inertia""" + if self.geometry_type == Geometry.BOX: + I.ixx = self.mass * (self.visual_data_new[1] ** 2+ self.visual_data_new[2] ** 2)/12 + I.iyy = self.mass* (self.visual_data_new[0]**2 + self.visual_data_new[2]**2)/12 + I.izz = self.mass * (self.visual_data_new[0]**2 + self.visual_data_new[1]**2)/12 + elif self.geometry_type == Geometry.CYLINDER: + i_xy_incomplete = ( + 3 *(self.visual_data_new[1] ** 2) + self.visual_data_new[0] ** 2 + ) / 12 + I.ixx = self.mass * i_xy_incomplete + I.iyy = self.mass * i_xy_incomplete + I.izz = self.mass * self.visual_data_new[1] ** 2 / 2 + + if(xyz_rpy[3]>0 and xyz_rpy[4] == 0.0 and xyz_rpy[5] == 0.0): + itemp = I.izz + I.iyy = itemp + I.izz = I.ixx + elif(xyz_rpy[4]>0.0): + itemp = I.izz + I.ixx = itemp + I.izz = I.iyy + return I + elif self.geometry_type == Geometry.SPHERE: + I.ixx = 2 * self.mass * self.visual_data_new** 2 / 5 + I.iyy = I.ixx + I.izz = I.ixx + return I + + def spatial_inertia(self) -> npt.ArrayLike: + """ + Args: + link (Link): Link + + Returns: + npt.ArrayLike: the 6x6 inertia matrix expressed at the origin of the link (with rotation) + """ + I = self.I + mass = self.mass + o = self.origin[:3] + rpy = self.origin[3:] + return self.math.spatial_inertia(I, mass, o, rpy) + + def homogeneous(self) -> npt.ArrayLike: + """ + Returns: + npt.ArrayLike: the homogeneus transform of the link + """ + return self.math.H_from_Pos_RPY( + self.inertial.origin.xyz, + self.inertial.origin.rpy, + ) diff --git a/src/adam/model/parametric_factories/parametric_model.py b/src/adam/model/parametric_factories/parametric_model.py new file mode 100644 index 00000000..96556042 --- /dev/null +++ b/src/adam/model/parametric_factories/parametric_model.py @@ -0,0 +1,87 @@ +import pathlib +from typing import List + +import urdf_parser_py.urdf + +from adam.core.spatial_math import SpatialMath +from adam.model import ModelFactory, StdJoint, StdLink, Link, Joint, LinkParametric, JointParametric + + +class URDFParametricModelFactory(ModelFactory): + """This factory generates robot elements from urdf_parser_py + + Args: + ModelFactory: the Model factory + """ + + def __init__(self, path: str, math: SpatialMath, link_parametric_list:List, lenght_multiplier, density): + self.math = math + if type(path) is not pathlib.Path: + path = pathlib.Path(path) + if not path.exists(): + raise FileExistsError(path) + self.link_parametric_list = list + self.urdf_desc = urdf_parser_py.urdf.URDF.from_xml_file(path) + self.name = self.urdf_desc.name + self.lenght_multiplier = lenght_multiplier + self.density = density + + def get_joints(self) -> List[Joint]: + """ + Returns: + List[StdJoint]: build the list of the joints + """ + return [self.build_joint(j) for j in self.urdf_desc.joints] + + def get_links(self) -> List[Link]: + """ + Returns: + List[StdLink]: build the list of the links + """ + return [ + self.build_link(l) for l in self.urdf_desc.links if l.inertial is not None + ] + + def get_frames(self) -> List[StdLink]: + """ + Returns: + List[StdLink]: build the list of the links + """ + return [self.build_link(l) for l in self.urdf_desc.links if l.inertial is None] + + def build_joint(self, joint: urdf_parser_py.urdf.Joint) -> Joint: + """ + Args: + joint (Joint): the urdf_parser_py joint + + Returns: + StdJoint: our joint representation + """ + if(joint.parent in self.link_parametric_list): + index_link = self.link_parametric_list.index(joint.parent) + link_parent = self.get_element_by_name(joint.parent) + parent_link_parametric = LinkParametric(link_parent, self.math,self.lenght_multiplier[index_link], self.density[index_link]) + return JointParametric(joint, self.math, parent_link_parametric) + + return StdJoint(joint, self.math) + + def build_link(self, link: urdf_parser_py.urdf.Link) -> Link: + """ + Args: + link (Link): the urdf_parser_py link + + Returns: + Link: our link representation + """ + if(link.name in self.link_parametric_list): + index_link = self.link_parametric_list.index(link.name) + return LinkParametric(link, self.math, self.lenght_multiplier[index_link], self.density[index_link]) + return StdLink(link, self.math) + + def get_element_by_name(self,link_name): + """Explores the robot looking for the link whose name matches the first argument""" + link_list = [corresponding_link for corresponding_link in self.urdf_desc.links if corresponding_link.name == link_name] + if len(link_list) != 0: + return link_list[0] + else: + return None \ No newline at end of file