fix import error, fix missing methods

src/adam/casadi/__init__.py

@@ -4,3 +4,5 @@
 
 from .computations import KinDynComputations
 from .casadi_like import CasadiLike
+from .computations_parametric import KinDynComputationsParametric
+

src/adam/casadi/computations_parametric.py

@@ -34,7 +34,7 @@ def __init__(
         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)
+        factory = URDFParametricModelFactory(path=urdfstring, math=math,link_parametric_list=link_name_list,lenght_multiplier=self.length_multiplier, density=self.density)
         model = Model.build(factory=factory, joints_name_list=joints_name_list)
         self.rbdalgos = RBDAlgorithms(model=model, math=math)
         self.NDoF = self.rbdalgos.NDoF

src/adam/model/__init__.py

@@ -3,4 +3,7 @@
 from .std_factories.std_joint import StdJoint
 from .std_factories.std_link import StdLink
 from .std_factories.std_model import URDFModelFactory
+from .parametric_factories.parametric_joint import ParmetricJoint
+from .parametric_factories.parametric_link import ParametricLink
+from .parametric_factories.parametric_model import URDFParametricModelFactory
 from .tree import Node, Tree

src/adam/model/parametric_factories/parametric_joint.py

@@ -5,7 +5,7 @@
 
 from adam.core.spatial_math import SpatialMath
 from adam.model import Joint
-from adam.model.parametric_factories import link_parametric
+from adam.model.parametric_factories.parametric_link import ParametricLink
 
 class ParmetricJoint(Joint):
     """Parametric Joint class"""
@@ -14,34 +14,33 @@ def __init__(
         self,
         joint: urdf_parser_py.urdf.Joint,
         math: SpatialMath,
-        parent_link:link_parametric,
+        parent_link:ParametricLink,
         idx: Union[int, None] = None,
     ) -> None:
         self.math = math
         self.name = joint.name
-        self.parent = jparent_link
+        self.parent = parent_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.joint = joint
+        joint_offset = self.parent.compute_joint_offset(joint, self.parent.link_offset)
         self.offset = joint_offset
-        self.origin = self.modify(self.parent_link.offset)
+        self.origin = self.modify(self.parent.link_offset)
 
     def modify(self, parent_joint_offset):
-        length = self.parent_link.get_principal_lenght_parametric()
+        length = self.parent.get_principal_lenght_parametric()
         # Ack for avoiding depending on casadi 
-        vo = self.parent_link.origin[2]
+        vo = self.parent.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]
+        xyz_rpy[0] = self.joint.origin.xyz[0]
+        xyz_rpy[1] = self.joint.origin.xyz[1]
+        xyz_rpy[2] = self.joint.origin.xyz[2]
+        xyz_rpy[3] = self.joint.origin.rpy[0]
+        xyz_rpy[4] = self.joint.origin.rpy[1]
+        xyz_rpy[5] = self.joint.origin.rpy[2]
 
         if(xyz_rpy[2]<0): 
             xyz_rpy[2] = -length +parent_joint_offset - self.offset   

src/adam/model/parametric_factories/parametric_link.py

@@ -5,6 +5,8 @@
 from adam.core.spatial_math import SpatialMath
 from adam.model import Link
 
+import math
+
 class I_parametric():
     def __init__(self) -> None:
         self.ixx = 0.0
@@ -25,6 +27,7 @@ class Side(Enum):
     WIDTH = 1
     HEIGHT = 2
     DEPTH = 3
+# ['XML_REFL', '_Link__get_collision', '_Link__get_visual', '_Link__set_collision', '_Link__set_visual', '__class__', '__delattr__', '__dict__', '__dir__', '__doc__', '__eq__', '__format__', '__ge__', '__getattribute__', '__gt__', '__hash__', '__init__', '__init_subclass__', '__le__', '__lt__', '__module__', '__ne__', '__new__', '__reduce__', '__reduce_ex__', '__repr__', '__setattr__', '__sizeof__', '__str__', '__subclasshook__', '__weakref__', 'add_aggregate', 'add_aggregates_to_xml', 'aggregate_init', 'aggregate_order', 'aggregate_type', 'check_valid', 'collision', 'collisions', 'from_xml', 'from_xml_file', 'from_xml_string', 'get_aggregate_list', 'get_refl_vars', 'inertial', 'lump_aggregates', 'name', 'origin', 'parse', 'post_read_xml', 'pre_write_xml', 'read_xml', 'remove_aggregate', 'to_xml', 'to_xml_string', 'to_yaml', 'visual', 'visuals', 'write_xml']
 
 
 class ParametricLink(Link):
@@ -35,7 +38,8 @@ def __init__(self, link: urdf_parser_py.urdf.Link, math: SpatialMath, length_mul
         self.name = link.name
         self.length_multiplier = length_multiplier
         self.density = density
-        self.visuals = link.visuals
+        self.visuals = link.visual
+        print(self.get_geometry(self.visuals))
         self.geometry_type, self.visual_data = self.get_geometry(self.visuals)
         self.link = link
         self.link_offset = self.compute_offset()        
@@ -49,9 +53,8 @@ def __init__(self, link: urdf_parser_py.urdf.Link, math: SpatialMath, length_mul
             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) 
+    def get_principal_lenght(self):  
+        xyz_rpy =  [*self.visuals.origin.xyz, *self.visuals.origin.rpy]
         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 
@@ -66,8 +69,8 @@ def get_principal_lenght(self):
         return v_l 
 
     def get_principal_lenght_parametric(self): 
-        visual = self.get_visual()
-        xyz_rpy = matrix_to_xyz_rpy(visual.origin) 
+
+        xyz_rpy =  [*self.visuals.origin.xyz, *self.visuals.origin.rpy]
         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 
@@ -82,8 +85,7 @@ def get_principal_lenght_parametric(self):
         return v_l 
 
     def compute_offset(self): 
-        visual = self.get_visual()
-        xyz_rpy = matrix_to_xyz_rpy(visual.origin) 
+        xyz_rpy =  [*self.visuals.origin.xyz, *self.visuals.origin.rpy] 
         v_l=  self.get_principal_lenght()
         v_o = xyz_rpy[2]
         if(v_o<0):
@@ -94,10 +96,10 @@ def compute_offset(self):
 
     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) 
+        xyz_rpy =  [*self.visuals.origin.xyz, *self.visuals.origin.rpy]  
         v_l= self.get_principal_lenght()
-        j_0 = matrix_to_xyz_rpy(joint_i.origin)[2]
+        print(joint_i.origin.xyz)
+        j_0 = joint_i.origin.xyz[2]
         v_o = xyz_rpy[2]
         if(j_0<0):
             joint_offset_temp = -(v_l + j_0-parent_offset)
@@ -109,32 +111,38 @@ def compute_joint_offset(self,joint_i, parent_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]
+        print("visual obj",dir(visual_obj))
+        print(type(visual_obj.geometry))
+        print(visual_obj.name)
+        """Returns the geometry type and thez corresponding geometry object for a given visual"""
+        if type(visual_obj.geometry) is urdf_parser_py.urdf.Box:
+            print("IS A BOX")
+            return (Geometry.BOX, visual_obj.geometry)
+        if type(visual_obj.geometry) is urdf_parser_py.urdf.Cylinder:
+            print("IS A CYLINDER")
+            return (Geometry.CYLINDER, visual_obj.geometry)
+        if type(visual_obj.geometry) is urdf_parser_py.urdf.Sphere:
+            print("IS A SPHERE")
+            return (Geometry.SPHERE, visual_obj.geometry)
 
     """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]
+            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
             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]
+            visual_data_new[0] = self.visual_data.length * self.length_multiplier
+            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]
+            visual_data_new = self.visual_data.radius * self.length_multiplier
             volume = 4 * math.pi * visual_data_new ** 3 / 3
         return volume, visual_data_new
 
@@ -147,20 +155,18 @@ def compute_mass(self):
 
     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)
+        xyz_rpy =  [*self.visuals.origin.xyz, *self.visuals.origin.rpy] 
         v_o = xyz_rpy[2] 
         length = self.get_principal_lenght_parametric()
         if(v_o<0):
-            origin[2] = self.offset-length/2
+            origin[2] = self.link_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[2] = length/2 +self.link_offset
             origin[0] = xyz_rpy[0]
             origin[1] = xyz_rpy[1]
             origin[3] = xyz_rpy[3]
@@ -173,8 +179,7 @@ def modify_origin(self):
 
     def compute_inertia_parametric(self):
         I = I_parametric
-        visual = self.get_visual()
-        xyz_rpy = matrix_to_xyz_rpy(visual.origin) 
+        xyz_rpy =  [*self.visuals.origin.xyz, *self.visuals.origin.rpy] 
         """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:

src/adam/model/parametric_factories/parametric_model.py

@@ -4,7 +4,8 @@
 import urdf_parser_py.urdf
 
 from adam.core.spatial_math import SpatialMath
-from adam.model import ModelFactory, StdJoint, StdLink, Link, Joint, LinkParametric, JointParametric
+from adam.model import ModelFactory, StdJoint, StdLink, Link, Joint
+from adam.model import ParmetricJoint, ParametricLink
 
 
 class URDFParametricModelFactory(ModelFactory):
@@ -20,7 +21,7 @@ def __init__(self, path: str, math: SpatialMath, link_parametric_list:List, leng
             path = pathlib.Path(path)
         if not path.exists():
             raise FileExistsError(path)
-        self.link_parametric_list = list
+        self.link_parametric_list = link_parametric_list
         self.urdf_desc = urdf_parser_py.urdf.URDF.from_xml_file(path)
         self.name = self.urdf_desc.name
         self.lenght_multiplier = lenght_multiplier
@@ -60,8 +61,8 @@ def build_joint(self, joint: urdf_parser_py.urdf.Joint) -> Joint:
         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)
+            parent_link_parametric = ParametricLink(link_parent, self.math,self.lenght_multiplier[index_link], self.density[index_link]) 
+            return ParmetricJoint(joint, self.math, parent_link_parametric)
 
         return StdJoint(joint, self.math)
 
@@ -75,7 +76,7 @@ def build_link(self, link: urdf_parser_py.urdf.Link) -> Link:
         """
         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 ParametricLink(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):