diff --git a/src/adam/parametric/casadi/computations_parametric.py b/src/adam/parametric/casadi/computations_parametric.py index ef922a36..7cc42656 100644 --- a/src/adam/parametric/casadi/computations_parametric.py +++ b/src/adam/parametric/casadi/computations_parametric.py @@ -1,6 +1,7 @@ # 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. +from typing import List import casadi as cs import numpy as np @@ -9,7 +10,7 @@ from adam.core import RBDAlgorithms from adam.core.constants import Representations from adam.model import Model -from adam.parametric.model import URDFParametricModelFactory +from adam.parametric.model import URDFParametricModelFactory, ParametricLink class KinDynComputationsParametric: @@ -37,6 +38,7 @@ def __init__( 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) + self.links_name_list = links_name_list factory = URDFParametricModelFactory( path=urdfstring, math=math, @@ -258,3 +260,17 @@ def get_total_mass(self) -> cs.Function: return cs.Function( "m", [self.length_multiplier, self.densities], [m], self.f_opts ) + + def get_original_densities(self) -> List[float]: + """Returns the original densities of the links + + Returns: + densities: The original densities + """ + densities = [] + model = self.rbdalgos.model + for name in self.links_name_list: + link = model.links[name] + assert isinstance(link, ParametricLink) + densities.append(link.original_density) + return densities diff --git a/src/adam/parametric/jax/computations_parametric.py b/src/adam/parametric/jax/computations_parametric.py index fb0823c9..483f1129 100644 --- a/src/adam/parametric/jax/computations_parametric.py +++ b/src/adam/parametric/jax/computations_parametric.py @@ -2,6 +2,8 @@ # This software may be modified and distributed under the terms of the # GNU Lesser General Public License v2.1 or any later version. +from typing import List + import jax.numpy as jnp import numpy as np from jax import grad, jit, vmap @@ -10,7 +12,7 @@ from adam.core.constants import Representations from adam.jax.jax_like import SpatialMath from adam.model import Model -from adam.parametric.model import URDFParametricModelFactory +from adam.parametric.model import URDFParametricModelFactory, ParametricLink class KinDynComputationsParametric: @@ -446,3 +448,17 @@ def get_total_mass( self.rbdalgos.set_frame_velocity_representation(self.representation) self.NDoF = self.rbdalgos.NDoF return self.rbdalgos.get_total_mass() + + def get_original_densities(self) -> List[float]: + """Returns the original densities of the links + + Returns: + densities: The original densities + """ + densities = [] + model = self.rbdalgos.model + for name in self.links_name_list: + link = model.links[name] + assert isinstance(link, ParametricLink) + densities.append(link.original_density) + return densities diff --git a/src/adam/parametric/model/parametric_factories/parametric_link.py b/src/adam/parametric/model/parametric_factories/parametric_link.py index d37f9699..c9ae02be 100644 --- a/src/adam/parametric/model/parametric_factories/parametric_link.py +++ b/src/adam/parametric/model/parametric_factories/parametric_link.py @@ -43,8 +43,12 @@ def __init__( self.original_visual = copy.deepcopy(link.visual) self.visuals = [copy.deepcopy(link.visual)] self.geometry_type, self.visual_data = self.get_geometry(self.original_visual) + original_volume, _ = self.compute_volume(length_multiplier=1.0) + self.original_density = link.inertial.mass / original_volume self.link_offset = self.compute_offset() - (self.volume, self.visual_data_new) = self.compute_volume() + (self.volume, self.visual_data_new) = self.compute_volume( + length_multiplier=self.length_multiplier + ) self.mass = self.compute_mass() self.inertial = Inertial(self.mass) self.inertial.mass = self.mass @@ -139,7 +143,7 @@ def get_geometry(visual_obj): f"The visual type {visual_obj.geometry.__class__} is not supported" ) - def compute_volume(self): + def compute_volume(self, length_multiplier): """ Returns: (npt.ArrayLike, npt.ArrayLike): the volume and the dimension parametric @@ -150,21 +154,20 @@ def compute_volume(self): if self.geometry_type == Geometry.BOX: 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 + visual_data_new[2] = self.visual_data.size[2] * 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] = self.visual_data.length * self.length_multiplier + visual_data_new[0] = self.visual_data.length * 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 = self.visual_data.radius * self.length_multiplier + visual_data_new = self.visual_data.radius * length_multiplier volume = 4 * math.pi * visual_data_new**3 / 3 return volume, visual_data_new - """Function that computes the mass starting from the densities, the length multiplier and the link""" - def compute_mass(self): """ + Function that computes the mass starting from the densities, and the link volume Returns: (npt.ArrayLike): the link mass """ diff --git a/src/adam/parametric/numpy/computations_parametric.py b/src/adam/parametric/numpy/computations_parametric.py index e4338251..9bb685a1 100644 --- a/src/adam/parametric/numpy/computations_parametric.py +++ b/src/adam/parametric/numpy/computations_parametric.py @@ -3,11 +3,12 @@ # GNU Lesser General Public License v2.1 or any later version. import numpy as np +from typing import List from adam.core.rbd_algorithms import RBDAlgorithms from adam.core.constants import Representations from adam.model import Model -from adam.parametric.model import URDFParametricModelFactory +from adam.parametric.model import URDFParametricModelFactory, ParametricLink from adam.numpy.numpy_like import SpatialMath @@ -441,3 +442,17 @@ def get_total_mass( self.rbdalgos.set_frame_velocity_representation(self.representation) self.NDoF = model.NDoF return self.rbdalgos.get_total_mass() + + def get_original_densities(self) -> List[float]: + """Returns the original densities of the links + + Returns: + densities: The original densities + """ + densities = [] + model = self.rbdalgos.model + for name in self.links_name_list: + link = model.links[name] + assert isinstance(link, ParametricLink) + densities.append(link.original_density) + return densities diff --git a/src/adam/parametric/pytorch/computations_parametric.py b/src/adam/parametric/pytorch/computations_parametric.py index 531124f7..3bc44610 100644 --- a/src/adam/parametric/pytorch/computations_parametric.py +++ b/src/adam/parametric/pytorch/computations_parametric.py @@ -4,11 +4,12 @@ import numpy as np import torch +from typing import List from adam.core.rbd_algorithms import RBDAlgorithms from adam.core.constants import Representations -from adam.model import Model, URDFModelFactory -from adam.parametric.model import URDFParametricModelFactory +from adam.model import Model +from adam.parametric.model import URDFParametricModelFactory, ParametricLink from adam.pytorch.torch_like import SpatialMath @@ -445,3 +446,17 @@ def get_total_mass( self.rbdalgos.set_frame_velocity_representation(self.representation) self.NDoF = self.rbdalgos.NDoF return self.rbdalgos.get_total_mass() + + def get_original_densities(self) -> List[float]: + """Returns the original densities of the links + + Returns: + densities: The original densities + """ + densities = [] + model = self.rbdalgos.model + for name in self.links_name_list: + link = model.links[name] + assert isinstance(link, ParametricLink) + densities.append(link.original_density) + return densities