diff --git a/src/adam/casadi/casadi_like.py b/src/adam/casadi/casadi_like.py index 8d131f9f..6ce21615 100644 --- a/src/adam/casadi/casadi_like.py +++ b/src/adam/casadi/casadi_like.py @@ -380,12 +380,15 @@ def mxv(m: CasadiLike, v: CasadiLike) -> CasadiLike: def vxs(v: CasadiLike, c: CasadiLike) -> CasadiLike: """ Vector times scalar multiplication for CasADi. + For multi-DOF joints (v has more than 1 column), performs matrix multiply. Args: - v: Vector (CasadiLike) - c: Scalar (CasadiLike) + v: Vector or matrix (CasadiLike) + c: Scalar or vector (CasadiLike) Returns: - CasadiLike: Result of vector times scalar + CasadiLike: Result of v * c (1-DOF) or v @ c (multi-DOF) """ + if v.array.shape[1] > 1: + return CasadiLike(cs.mtimes(v.array, c.array)) return CasadiLike(v.array * c.array) diff --git a/src/adam/casadi/computations.py b/src/adam/casadi/computations.py index 3191e96a..10b626bb 100644 --- a/src/adam/casadi/computations.py +++ b/src/adam/casadi/computations.py @@ -4,12 +4,14 @@ import casadi as cs import numpy as np +import idyntree.bindings as iDynTree from adam.casadi.casadi_like import SpatialMath from adam.core import RBDAlgorithms from adam.core.constants import Representations from adam.model import Model, build_model_factory from adam.model.kindyn_mixin import KinDynFactoryMixin +from adam.model.conversions import from_idyntree_model class KinDynComputations(KinDynFactoryMixin): @@ -22,6 +24,7 @@ def __init__( root_link: str = None, 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), + convert_revolute_to_spherical: bool = False, ) -> None: """ Args: @@ -29,10 +32,33 @@ def __init__( NOTE: The parameter name `urdfstring` is deprecated and will be renamed to `model` in a future release. joints_name_list (list): list of the actuated joints root_link (str, optional): Deprecated. The root link is automatically chosen as the link with no parent in the URDF. Defaults to None. + convert_revolute_to_spherical (bool, optional): If True, converts revolute joints to spherical joints. Defaults to False. """ math = SpatialMath() - factory = build_model_factory(description=urdfstring, math=math) - model = Model.build(factory=factory, joints_name_list=joints_name_list) + # if convert_revolute_to_spherical: + # model_loader = iDynTree.ModelLoader() + # options = model_loader.parsingOptions() + # options.convertThreeRevoluteJointsToSphericalJoint = ( + # convert_revolute_to_spherical + # ) + # model_loader.setParsingOptions(options) + # model_loader.loadReducedModelFromFile(str(urdfstring), joints_name_list) + # model_idy = model_loader.model() + # model = from_idyntree_model(model_idy, math=math) + # else: + # factory = build_model_factory(description=urdfstring, math=math) + # model = Model.build(factory=factory, joints_name_list=joints_name_list) + + model_loader = iDynTree.ModelLoader() + options = model_loader.parsingOptions() + options.convertThreeRevoluteJointsToSphericalJoint = ( + convert_revolute_to_spherical + ) + model_loader.setParsingOptions(options) + model_loader.loadReducedModelFromFile(str(urdfstring), joints_name_list) + model_idyntree = model_loader.model() + model = from_idyntree_model(model_idyntree, math=math) + self.rbdalgos = RBDAlgorithms(model=model, math=math) self.NDoF = self.rbdalgos.NDoF self.g = gravity diff --git a/src/adam/casadi/inverse_kinematics.py b/src/adam/casadi/inverse_kinematics.py index 019c3c97..39278c9f 100644 --- a/src/adam/casadi/inverse_kinematics.py +++ b/src/adam/casadi/inverse_kinematics.py @@ -43,6 +43,7 @@ def __init__( joints_list: list[str], joint_limits_active: bool = True, solver_settings: dict[str, Any] = None, + convert_revolute_to_spherical: bool = False, ): """Initialize the InverseKinematics solver. @@ -52,9 +53,13 @@ def __init__( joint_limits_active (bool, optional): If True, enforces joint limits. Defaults to True. solver_settings (dict[str, ], optional): Settings for the solver. Defaults to None. """ - self.kd = KinDynComputations(urdf_path, joints_list) - self.ndof = len(joints_list) - self.joints_list = joints_list + self.kd = KinDynComputations( + urdf_path, + joints_list, + convert_revolute_to_spherical=convert_revolute_to_spherical, + ) + self.ndof = self.kd.NDoF + self.joints_list = self.kd.rbdalgos.model.actuated_joints # Opti variables -------------------------------------------------- self.opti = cs.Opti() diff --git a/src/adam/core/rbd_algorithms.py b/src/adam/core/rbd_algorithms.py index 85e2dfa2..20073f6d 100644 --- a/src/adam/core/rbd_algorithms.py +++ b/src/adam/core/rbd_algorithms.py @@ -131,13 +131,16 @@ def tile_batch(arr): Xup[idx], ) - def block_index(node_idx: int) -> int | None: + def block_info(node_idx: int) -> list[tuple[int, int]] | None: + """Return list of (block_index, local_dof_offset) for a node, or None.""" if node_idx == root_idx: - return 0 + return [(0, 0)] joint_idx = joint_indices[node_idx] if joint_idx is None: return None - return 1 + int(joint_idx) + if isinstance(joint_idx, tuple): + return [(1 + j, k) for k, j in enumerate(joint_idx)] + return [(1 + int(joint_idx), 0)] sizes = [6] + [1] * n blocks: list[list[npt.ArrayLike | None]] = [ @@ -148,30 +151,38 @@ def block_index(node_idx: int) -> int | None: Phi_i = Phi[idx] Phi_T = math.swapaxes(Phi_i, -2, -1) F = math.mtimes(Ic_comp[idx], Phi_i) - ri = block_index(idx) + ri_info = block_info(idx) if idx == root_idx: blocks[0][0] = math.mtimes(Phi_T, F) continue - if ri is not None: - blocks[ri][ri] = math.mtimes(Phi_T, F) + if ri_info is not None: + diag = math.mtimes(Phi_T, F) + for ri, oi in ri_info: + for rj, oj in ri_info: + blocks[ri][rj] = diag[ + ..., oi : oi + sizes[ri], oj : oj + sizes[rj] + ] current = idx F_path = F while current != root_idx: parent = parent_indices[current] F_path = math.mtimes(Xup_T[current], F_path) - rj = block_index(parent) - if rj is None: + rj_info = block_info(parent) + if rj_info is None: current = parent continue - Bij = math.mtimes(math.swapaxes(F_path, -2, -1), Phi[parent]) - if ri is None: + Bij_full = math.mtimes(math.swapaxes(F_path, -2, -1), Phi[parent]) + if ri_info is None: current = parent continue - blocks[ri][rj] = Bij - blocks[rj][ri] = math.swapaxes(Bij, -2, -1) + for ri, oi in ri_info: + for rj, oj in rj_info: + sub = Bij_full[..., oi : oi + sizes[ri], oj : oj + sizes[rj]] + blocks[ri][rj] = sub + blocks[rj][ri] = math.swapaxes(sub, -2, -1) current = parent row_tensors = [] @@ -206,14 +217,17 @@ def block_index(node_idx: int) -> int | None: ) zero_col = math.factory.zeros(batch_shape + (6, 1)) + joint_local_dof = self._joint_index_to_local_dof joint_cols = [] for jidx in range(n): node_idx = joint_to_node.get(jidx) if node_idx is not None: - col = math.mtimes( + full_col = math.mtimes( math.swapaxes(X_G[node_idx], -2, -1), math.mtimes(Ic_comp[node_idx], Phi[node_idx]), ) + local = joint_local_dof.get(jidx, 0) + col = full_col[..., :, local : local + 1] else: col = zero_col joint_cols.append(col) @@ -547,8 +561,8 @@ def jacobian_dot( a = a + self.math.vxs(J_dot_j, q_dot) if joint.idx is not None: - cols[joint.idx] = J_j - cols_dot[joint.idx] = J_dot_j + self._scatter_cols(cols, joint.idx, J_j) + self._scatter_cols(cols_dot, joint.idx, J_dot_j) zero_col = self.math.factory.zeros(batch_size + (6, 1)) cols = [zero_col if c is None else c for c in cols] @@ -829,7 +843,8 @@ def rnea( else: joint_idx = joint_indices[idx] if joint_idx is not None: - tau_joint_cols[joint_idx] = math.mxv(Phi_T, Fi) + tau_val = math.mxv(Phi_T, Fi) + self._scatter_entries(tau_joint_cols, joint_idx, tau_val) parent = parent_indices[idx] if parent >= 0: f[parent] = f[parent] + math.mxv( @@ -1035,8 +1050,12 @@ def tile_batch(arr): U_i = math.mtimes(IA[idx], Si) d_i = math.mtimes(math.swapaxes(Si, -2, -1), U_i) joint_idx = joint_indices[idx] - tau_vec = joint_torques_eff[..., joint_idx] - Si_T_pA = math.mxv(math.swapaxes(Si, -2, -1), pA[idx])[..., 0] + if isinstance(joint_idx, tuple): + tau_slice = slice(joint_idx[0], joint_idx[-1] + 1) + else: + tau_slice = slice(joint_idx, joint_idx + 1) + tau_vec = joint_torques_eff[..., tau_slice] + Si_T_pA = math.mxv(math.swapaxes(Si, -2, -1), pA[idx]) u_i = tau_vec - Si_T_pA d_list[idx] = d_i @@ -1080,14 +1099,15 @@ def tile_batch(arr): if Si is not None and joint_idx is not None: U_i = U_list[idx] - U_T_rel_acc = math.mxv(math.swapaxes(U_i, -2, -1), rel_acc)[..., 0] + U_T_rel_acc = math.mxv(math.swapaxes(U_i, -2, -1), rel_acc) num = u_list[idx] - U_T_rel_acc inv_d = inv_d_list[idx] num_expanded = expand_to_match(num, inv_d) gain_qdd = math.mtimes(inv_d, num_expanded) qdd_col = gain_qdd[..., 0] - if joint_idx < n: - qdd_entries[joint_idx] = qdd_col + max_idx = max(joint_idx) if isinstance(joint_idx, tuple) else joint_idx + if max_idx < n: + self._scatter_entries(qdd_entries, joint_idx, qdd_col) a_correction_vec = math.mxv(Si, qdd_col) a[idx] = a_pre + a_correction_vec else: @@ -1223,12 +1243,35 @@ def _joints_jacobian_from_traversal( continue L_H_j = L_H_B @ B_H_j S = joint.motion_subspace() - cols[joint.idx] = self.math.adjoint(L_H_j) @ S + J_block = self.math.adjoint(L_H_j) @ S + self._scatter_cols(cols, joint.idx, J_block) zero_col = self.math.factory.zeros(batch_size + (6, 1)) cols = [zero_col if col is None else col for col in cols] return self.math.concatenate(cols, axis=-1) + @staticmethod + def _scatter_cols( + cols: list, idx: int | tuple[int, ...], block: npt.ArrayLike + ) -> None: + """Assign columns of *block* to *cols* at the DOF positions given by *idx*.""" + if isinstance(idx, tuple): + for i, j in enumerate(idx): + cols[j] = block[..., :, i : i + 1] + else: + cols[idx] = block + + @staticmethod + def _scatter_entries( + entries: list, idx: int | tuple[int, ...], vec: npt.ArrayLike + ) -> None: + """Assign elements of *vec* to *entries* at the DOF positions given by *idx*.""" + if isinstance(idx, tuple): + for i, j in enumerate(idx): + entries[j] = vec[..., i : i + 1] + else: + entries[idx] = vec + def _default_joint_value( self, joint_positions: npt.ArrayLike, fallback: npt.ArrayLike | None = None ) -> npt.ArrayLike: @@ -1266,11 +1309,14 @@ def _prepare_tree_cache(self) -> None: self._node_indices = tuple(range(node_count)) self._rev_node_indices = tuple(reversed(self._node_indices)) self._parent_indices = [-1] * node_count - self._joint_indices_per_node: list[int | None] = [None] * node_count + self._joint_indices_per_node: list[int | tuple[int, ...] | None] = [ + None + ] * node_count self._motion_subspaces: list[npt.ArrayLike] = [None] * node_count self._spatial_inertias: list[npt.ArrayLike] = [None] * node_count self._joints_per_node: list[Joint | None] = [None] * node_count self._joint_index_to_node: dict[int, int] = {} + self._joint_index_to_local_dof: dict[int, int] = {} self._joint_by_child: dict[str, Joint] = {} self._joint_chain_cache: dict[str, tuple[Joint, ...]] = { self.root_link: tuple() @@ -1293,7 +1339,13 @@ def _prepare_tree_cache(self) -> None: self._motion_subspaces[idx] = joint.motion_subspace() self._joint_indices_per_node[idx] = joint.idx if joint.idx is not None: - self._joint_index_to_node[int(joint.idx)] = idx + if isinstance(joint.idx, tuple): + for local, global_idx in enumerate(joint.idx): + self._joint_index_to_node[global_idx] = idx + self._joint_index_to_local_dof[global_idx] = local + else: + self._joint_index_to_node[int(joint.idx)] = idx + self._joint_index_to_local_dof[int(joint.idx)] = 0 for joint in self.model.joints.values(): self._joint_by_child[joint.child] = joint diff --git a/src/adam/core/spatial_math.py b/src/adam/core/spatial_math.py index 1d1fdde1..1dac5dec 100644 --- a/src/adam/core/spatial_math.py +++ b/src/adam/core/spatial_math.py @@ -401,6 +401,45 @@ def H_revolute_joint( R = R_rpy @ R_axis return self.homogeneous(R, xyz) + def H_spherical_joint( + self, xyz: npt.ArrayLike, rpy: npt.ArrayLike, q: npt.ArrayLike + ) -> npt.ArrayLike: + """ + Args: + xyz (npt.ArrayLike): joint origin in the urdf + rpy (npt.ArrayLike): joint orientation in the urdf + q (npt.ArrayLike): [roll, pitch, yaw] representing joint rotation + Returns: + npt.ArrayLike: Homogeneous transform + """ + # CasADi matrices are always 2-D. A 3-element slice of a column + # vector arrives as (3, 1); transpose it to (1, 3) so that + # q[..., i] correctly selects each angle. + if q.ndim == 2 and q.shape[0] != 1 and q.shape[1] == 1: + q = q.T + + if q.shape[-1] != 3: + raise ValueError( + f"Spherical joints expect rotation [roll, pitch, yaw] (3 values), received shape {q.shape}" + ) + + if q.ndim > 1 and q.shape[0] != 1: + batch_shape = q.shape[:-1] + xp = self._xp(q.array) + if xyz.ndim == 1: + xyz = self.factory.asarray( + xp.broadcast_to(xyz.array, batch_shape + xyz.array.shape) + ) + if rpy.ndim == 1: + rpy = self.factory.asarray( + xp.broadcast_to(rpy.array, batch_shape + rpy.array.shape) + ) + + R_rpy = self.R_from_RPY(rpy) + R_joint = self.R_from_RPY(q) + R = R_rpy @ R_joint + return self.homogeneous(R, xyz) + def homogeneous(self, R, p): # Ensure p has the right shape for concatenation if p.ndim == R.ndim - 1: @@ -439,7 +478,7 @@ def H_from_Pos_RPY(self, xyz: npt.ArrayLike, rpy: npt.ArrayLike) -> npt.ArrayLik rpy (npt.ArrayLike): rotation as rpy angles Returns: - npt.ArrayLike: Homegeneous transform + npt.ArrayLike: Homogeneous transform """ R = self.R_from_RPY(rpy) return self.homogeneous(R, xyz) @@ -477,6 +516,26 @@ def X_revolute_joint( p = self.mxv(-R, T[..., :3, 3]) return self.spatial_transform(R, p) + def X_spherical_joint( + self, + xyz: npt.ArrayLike, + rpy: npt.ArrayLike, + q: npt.ArrayLike, + ) -> npt.ArrayLike: + """ + Args: + xyz (npt.ArrayLike): joint origin in the urdf + rpy (npt.ArrayLike): joint orientation in the urdf + q (npt.ArrayLike): [roll, pitch, yaw] representing joint rotation + + Returns: + npt.ArrayLike: Spatial transform of a spherical joint given its rotation angles + """ + T = self.H_spherical_joint(xyz, rpy, q) + R = self.swapaxes(T[..., :3, :3], -1, -2) + p = self.mxv(-R, T[..., :3, 3]) + return self.spatial_transform(R, p) + def X_prismatic_joint( self, xyz: npt.ArrayLike, @@ -679,15 +738,17 @@ def mxv(self, m: npt.ArrayLike, v: npt.ArrayLike) -> npt.ArrayLike: def vxs(self, v: npt.ArrayLike, s: npt.ArrayLike) -> npt.ArrayLike: """ Args: - v (npt.ArrayLike): Vector - s (npt.ArrayLike): Scalar + v (npt.ArrayLike): Vector or matrix (motion subspace) + s (npt.ArrayLike): Scalar or vector (joint value / velocity) Returns: - npt.ArrayLike: Result of vector cross product with scalar multiplication + npt.ArrayLike: v * s for 1-DOF, v @ s for multi-DOF """ if v.shape[-1] == 1: v = v[..., 0] - s = s[..., None] # Add extra dimension - return v * s + s = s[..., None] # Add extra dimension + return v * s + # Multi-DOF: matrix-vector product + return self.mxv(v, s) def adjoint_inverse(self, H: npt.ArrayLike) -> npt.ArrayLike: """ diff --git a/src/adam/model/abc_factories.py b/src/adam/model/abc_factories.py index 28908299..ebb847dd 100644 --- a/src/adam/model/abc_factories.py +++ b/src/adam/model/abc_factories.py @@ -91,7 +91,8 @@ class Joint(abc.ABC): axis: npt.ArrayLike origin: Pose limit: Limits - idx: int + idx: int | tuple[int, ...] + dofs: int """ Abstract base class for all joints. """ diff --git a/src/adam/model/conversions/__init__.py b/src/adam/model/conversions/__init__.py index 39459511..f3d8d07c 100644 --- a/src/adam/model/conversions/__init__.py +++ b/src/adam/model/conversions/__init__.py @@ -1,4 +1,10 @@ -from .idyntree import to_idyntree_model +from .idyntree import from_idyntree_model, to_idyntree_model from .usd import model_to_usd, model_to_usd_stage, urdf_to_usd -__all__ = ["to_idyntree_model", "model_to_usd_stage", "model_to_usd", "urdf_to_usd"] +__all__ = [ + "from_idyntree_model", + "to_idyntree_model", + "model_to_usd_stage", + "model_to_usd", + "urdf_to_usd", +] diff --git a/src/adam/model/conversions/idyntree.py b/src/adam/model/conversions/idyntree.py index 6b721d9d..3a08a76a 100644 --- a/src/adam/model/conversions/idyntree.py +++ b/src/adam/model/conversions/idyntree.py @@ -3,9 +3,11 @@ import urdf_parser_py.urdf import casadi as cs +from adam.core.spatial_math import ArrayLike, SpatialMath from adam.model.abc_factories import Joint, Link from adam.model.model import Model -from adam.core.spatial_math import ArrayLike +from adam.model.std_factories.std_joint import StdJoint +from adam.model.std_factories.std_link import StdLink def _to_sequence(x) -> list[float]: @@ -225,9 +227,34 @@ def to_idyntree_model(model: Model) -> idyntree.bindings.Model: for node in model.tree: for j in node.arcs: assert j.name not in model.frames - joint = to_idyntree_joint(j, links_map[j.parent], links_map[j.child]) - joint_index = output.addJoint(j.name, joint) - assert output.isValidJointIndex(joint_index) + if j.type == "spherical": + parent_idx = links_map[j.parent] + child_idx = links_map[j.child] + + # Create rest transform from joint origin + rest_position = idyntree.bindings.Position.FromPython( + _to_sequence(j.origin.xyz) + ) + rest_rotation = idyntree.bindings.Rotation.RPY( + *_to_sequence(j.origin.rpy) + ) + rest_transform = idyntree.bindings.Transform() + rest_transform.setRotation(rest_rotation) + rest_transform.setPosition(rest_position) + + # Create SphericalJoint with rest transform + spherical_joint = idyntree.bindings.SphericalJoint(rest_transform) + spherical_joint.setAttachedLinks(parent_idx, child_idx) + + # Set the joint center at the joint origin (relative to parent link) + spherical_joint.setJointCenter(parent_idx, rest_position) + + joint_index = output.addJoint(j.name, spherical_joint) + assert output.isValidJointIndex(joint_index) + else: + joint = to_idyntree_joint(j, links_map[j.parent], links_map[j.child]) + joint_index = output.addJoint(j.name, joint) + assert output.isValidJointIndex(joint_index) frames_list = [f + "_fixed_joint" for f in model.frames] for name in model.joints: @@ -256,3 +283,321 @@ def to_idyntree_model(model: Model) -> idyntree.bindings.Model: assert output_reduced.isValid() return output_reduced + + +class _URDFOrigin: + """Lightweight stand-in for urdf_parser_py.urdf.Pose.""" + + def __init__(self, xyz, rpy): + self.xyz = xyz + self.rpy = rpy + + +class _URDFInertia: + """Lightweight stand-in for urdf_parser_py.urdf.Inertia.""" + + def __init__(self, ixx, ixy, ixz, iyy, iyz, izz): + self.ixx = ixx + self.ixy = ixy + self.ixz = ixz + self.iyy = iyy + self.iyz = iyz + self.izz = izz + + +class _URDFInertial: + """Lightweight stand-in for urdf_parser_py.urdf.Inertial.""" + + def __init__(self, mass, origin, inertia): + self.mass = mass + self.origin = origin + self.inertia = inertia + + +class _URDFLimit: + """Lightweight stand-in for urdf_parser_py.urdf.JointLimit.""" + + def __init__(self, lower, upper, effort=0.0, velocity=0.0): + self.lower = lower + self.upper = upper + self.effort = effort + self.velocity = velocity + + +class _URDFLinkProxy: + """Lightweight proxy mimicking urdf_parser_py.urdf.Link for StdLink.""" + + def __init__(self, name, inertial, visuals=None, collisions=None): + self.name = name + self.inertial = inertial + self.visuals = visuals or [] + self.collisions = collisions or [] + + +class _URDFJointProxy: + """Lightweight proxy mimicking urdf_parser_py.urdf.Joint for StdJoint.""" + + _IDYNTREE_JOINT_TYPE_MAP = { + "FixedJoint": "fixed", + "RevoluteJoint": "revolute", + "PrismaticJoint": "prismatic", + "SphericalJoint": "spherical", + } + + def __init__(self, name, joint_type, parent, child, origin, axis=None, limit=None): + self.name = name + self.joint_type = joint_type + self.parent = parent + self.child = child + self.origin = origin + self.axis = axis + self.limit = limit + + +def _idyntree_rotation_to_rpy( + rotation: idyntree.bindings.Rotation, +) -> list[float]: + """Extract RPY angles from an iDynTree Rotation.""" + return list(rotation.asRPY().toNumPy()) + + +def _idyntree_position_to_xyz( + position: idyntree.bindings.Position, +) -> list[float]: + """Extract xyz from an iDynTree Position.""" + return list(position.toNumPy()) + + +def _extract_link_proxy( + idyn_model: idyntree.bindings.Model, link_index: int +) -> _URDFLinkProxy: + """Build a _URDFLinkProxy from an iDynTree link.""" + link_name = idyn_model.getLinkName(link_index) + idyn_link = idyn_model.getLink(link_index) + sp_inertia = idyn_link.getInertia() + + mass = sp_inertia.getMass() + com = _idyntree_position_to_xyz(sp_inertia.getCenterOfMass()) + + # getRotationalInertiaWrtCenterOfMass returns the 3x3 inertia at the COM + rot_inertia = sp_inertia.getRotationalInertiaWrtCenterOfMass() + ixx = rot_inertia.getVal(0, 0) + ixy = rot_inertia.getVal(0, 1) + ixz = rot_inertia.getVal(0, 2) + iyy = rot_inertia.getVal(1, 1) + iyz = rot_inertia.getVal(1, 2) + izz = rot_inertia.getVal(2, 2) + + inertia = _URDFInertia(ixx, ixy, ixz, iyy, iyz, izz) + origin = _URDFOrigin(xyz=com, rpy=[0.0, 0.0, 0.0]) + inertial = _URDFInertial(mass=mass, origin=origin, inertia=inertia) + + return _URDFLinkProxy(name=link_name, inertial=inertial) + + +def _infer_idyntree_joint_type(idyn_joint) -> str: + """Infer the adam joint type string from an iDynTree IJoint. + + SWIG may return the base ``IJoint`` wrapper even for concrete + subclasses, so ``type().__name__`` is unreliable. We fall back to + ``isinstance`` checks and ``getNrOfDOFs()`` when the class name is + not recognised. + """ + # Fast path: class name matches a known concrete type + class_name = type(idyn_joint).__name__ + known = _URDFJointProxy._IDYNTREE_JOINT_TYPE_MAP.get(class_name) + if known is not None: + return known + + # Try isinstance checks (works when SWIG preserves the hierarchy) + for cls_name, type_str in _URDFJointProxy._IDYNTREE_JOINT_TYPE_MAP.items(): + cls = getattr(idyntree.bindings, cls_name, None) + if cls is not None and isinstance(idyn_joint, cls): + return type_str + + # Fall back to DOF-based classification + ndofs = idyn_joint.getNrOfDOFs() + if ndofs == 0: + return "fixed" + if ndofs >= 3: + return "spherical" + # 1-DOF: try to distinguish revolute vs prismatic via isinstance + prism_cls = getattr(idyntree.bindings, "PrismaticJoint", None) + if prism_cls is not None and isinstance(idyn_joint, prism_cls): + return "prismatic" + # Default to revolute for any remaining 1-DOF joint + return "revolute" + + +def _extract_joint_proxy( + idyn_model: idyntree.bindings.Model, joint_index: int +) -> _URDFJointProxy: + """Build a _URDFJointProxy from an iDynTree joint.""" + joint_name = idyn_model.getJointName(joint_index) + idyn_joint = idyn_model.getJoint(joint_index) + + joint_type = _infer_idyntree_joint_type(idyn_joint) + + # Parent / child link names + first_idx = idyn_joint.getFirstAttachedLink() + second_idx = idyn_joint.getSecondAttachedLink() + + # Determine parent and child using the model's traversal + traversal = idyntree.bindings.Traversal() + idyn_model.computeFullTreeTraversal(traversal) + + parent_idx, child_idx = first_idx, second_idx + for i in range(traversal.getNrOfVisitedLinks()): + visited = traversal.getLink(i) + visited_idx = visited.getIndex() + if visited_idx == first_idx or visited_idx == second_idx: + # The first one encountered in traversal is the parent + parent_idx = visited_idx + child_idx = second_idx if visited_idx == first_idx else first_idx + break + + parent_name = idyn_model.getLinkName(parent_idx) + child_name = idyn_model.getLinkName(child_idx) + + # Rest transform: parent -> child + rest_transform = idyn_joint.getRestTransform(parent_idx, child_idx) + xyz = _idyntree_position_to_xyz(rest_transform.getPosition()) + rpy = _idyntree_rotation_to_rpy(rest_transform.getRotation()) + origin = _URDFOrigin(xyz=xyz, rpy=rpy) + + axis = None + limit = None + if joint_type in ("revolute", "prismatic"): + # SWIG returns the base IJoint wrapper which lacks getAxis(). + # Downcast to the concrete type to access it. + concrete = None + if joint_type == "revolute": + concrete = idyn_joint.asRevoluteJoint() + elif joint_type == "prismatic": + concrete = idyn_joint.asPrismaticJoint() + + if concrete is not None: + idyn_axis = concrete.getAxis(child_idx) + direction = idyn_axis.getDirection() + axis = [direction.getVal(0), direction.getVal(1), direction.getVal(2)] + else: + # Last resort: extract from the 6-D motion subspace vector + msv = idyn_joint.getMotionSubspaceVector(0, child_idx, parent_idx) + msv_np = msv.toNumPy().flatten() + axis = [float(msv_np[3]), float(msv_np[4]), float(msv_np[5])] + + if idyn_joint.hasPosLimits(): + lower = idyn_joint.getMinPosLimit(0) + upper = idyn_joint.getMaxPosLimit(0) + limit = _URDFLimit(lower=lower, upper=upper) + + return _URDFJointProxy( + name=joint_name, + joint_type=joint_type, + parent=parent_name, + child=child_name, + origin=origin, + axis=axis, + limit=limit, + ) + + +def from_idyntree_model( + idyn_model: idyntree.bindings.Model, + joints_name_list: list[str] | None = None, + math: SpatialMath | None = None, +) -> Model: + """Convert an iDynTree Model to an adam Model. + + Args: + idyn_model: the iDynTree model to convert. + joints_name_list: the list of actuated joint names. If ``None``, + all non-fixed joints present in the iDynTree model are used. + math: the SpatialMath backend to use. Defaults to the NumPy backend. + + Returns: + Model: the adam model. + """ + if math is None: + from adam.numpy.numpy_like import SpatialMath as NumpySpatialMath + + math = NumpySpatialMath() + + # -- Build links ---------------------------------------------------------- + links: list[StdLink] = [] + for link_idx in range(idyn_model.getNrOfLinks()): + proxy = _extract_link_proxy(idyn_model, link_idx) + links.append(StdLink(proxy, math)) + + # -- Build joints --------------------------------------------------------- + joints: list[StdJoint] = [] + for joint_idx in range(idyn_model.getNrOfJoints()): + proxy = _extract_joint_proxy(idyn_model, joint_idx) + joints.append(StdJoint(proxy, math)) + + # -- Build frames (additional frames in iDynTree) ------------------------- + frames: list[StdLink] = [] + for frame_idx in range(idyn_model.getNrOfFrames()): + # iDynTree frame indices start after link indices; skip link frames + if idyn_model.isValidLinkIndex(frame_idx): + continue + frame_name = idyn_model.getFrameName(frame_idx) + # Skip frames that correspond to links (link-frames share the index) + if any(l.name == frame_name for l in links): + continue + # Create a zero-inertia link proxy for the frame + frame_proxy = _URDFLinkProxy(name=frame_name, inertial=None) + frames.append(StdLink(frame_proxy, math)) + # Also create a fixed joint connecting the frame to its parent link + parent_link_idx = idyn_model.getFrameLink(frame_idx) + parent_link_name = idyn_model.getLinkName(parent_link_idx) + frame_transform = idyn_model.getFrameTransform(frame_idx) + frame_xyz = _idyntree_position_to_xyz(frame_transform.getPosition()) + frame_rpy = _idyntree_rotation_to_rpy(frame_transform.getRotation()) + frame_joint_proxy = _URDFJointProxy( + name=frame_name + "_fixed_joint", + joint_type="fixed", + parent=parent_link_name, + child=frame_name, + origin=_URDFOrigin(xyz=frame_xyz, rpy=frame_rpy), + ) + joints.append(StdJoint(frame_joint_proxy, math)) + + # -- Determine actuated joints -------------------------------------------- + if joints_name_list is None: + joints_name_list = [j.name for j in joints if j.type != "fixed"] + + # -- Assemble the Model using factory-compatible path --------------------- + from adam.model.tree import Tree + + # Assign DOF indices to actuated joints + current_pos_idx = 0 + for joint_name in joints_name_list: + for j in joints: + if j.name != joint_name: + continue + dofs = getattr(j, "dofs", 1) + if j.type == "fixed" or dofs == 0: + j.idx = None + elif dofs == 1: + j.idx = current_pos_idx + else: + j.idx = tuple(range(current_pos_idx, current_pos_idx + dofs)) + current_pos_idx += dofs + break + + tree = Tree.build_tree(links=links, joints=joints) + + joints_dict: dict[str, Joint] = {j.name: j for j in joints} + links_dict: dict[str, Link] = {l.name: l for l in links} + frames_dict: dict[str, Link] = {f.name: f for f in frames} + + return Model( + name=idyn_model.toString(), + links=links_dict, + frames=frames_dict, + joints=joints_dict, + tree=tree, + NDoF=current_pos_idx, + actuated_joints=joints_name_list, + ) diff --git a/src/adam/model/model.py b/src/adam/model/model.py index 7bd9d2dc..4ca2c76f 100644 --- a/src/adam/model/model.py +++ b/src/adam/model/model.py @@ -60,10 +60,23 @@ def build(factory: ModelFactory, joints_name_list: list[str] = None) -> "Model": ) # set idx to the actuated joints - for [idx, joint_str] in enumerate(joints_name_list): + current_pos_idx = 0 + for joint_str in joints_name_list: for joint in joints_list: - if joint.name == joint_str: - joint.idx = idx + if joint.name != joint_str: + continue + dofs = getattr(joint, "dofs", None) + if dofs is None: + dofs = 1 + if joint.type == "fixed" or dofs == 0: + joint.idx = None + dofs = 0 + elif dofs == 1: + joint.idx = current_pos_idx + else: + joint.idx = tuple(range(current_pos_idx, current_pos_idx + dofs)) + current_pos_idx += dofs + break tree = Tree.build_tree(links=links_list, joints=joints_list) @@ -78,7 +91,7 @@ def build(factory: ModelFactory, joints_name_list: list[str] = None) -> "Model": frames=frames, joints=joints, tree=tree, - NDoF=len(joints_name_list), + NDoF=current_pos_idx, actuated_joints=joints_name_list, ) diff --git a/src/adam/model/std_factories/std_joint.py b/src/adam/model/std_factories/std_joint.py index 12b335ff..3e12b2fc 100644 --- a/src/adam/model/std_factories/std_joint.py +++ b/src/adam/model/std_factories/std_joint.py @@ -26,6 +26,15 @@ def __init__( self.origin = self._set_origin(joint.origin) self.limit = self._set_limits(joint.limit) self.idx = idx + self.dofs = self._infer_dofs() + + def _infer_dofs(self) -> int: + """Number of joint's DOFs.""" + if self.type == "fixed": + return 0 + if self.type == "spherical": + return 3 + return 1 def _set_axis(self, axis: npt.ArrayLike) -> npt.ArrayLike: """ @@ -92,6 +101,12 @@ def homogeneous(self, q: npt.ArrayLike) -> npt.ArrayLike: self.axis, q, ) + elif self.type == "spherical": + return self.math.H_spherical_joint( + self.origin.xyz, + self.origin.rpy, + q, + ) def spatial_transform(self, q: npt.ArrayLike) -> npt.ArrayLike: """ @@ -114,6 +129,12 @@ def spatial_transform(self, q: npt.ArrayLike) -> npt.ArrayLike: self.axis, q, ) + elif self.type == "spherical": + return self.math.X_spherical_joint( + self.origin.xyz, + self.origin.rpy, + q, + ) def motion_subspace(self) -> npt.ArrayLike: """ @@ -133,3 +154,7 @@ def motion_subspace(self) -> npt.ArrayLike: axis = self.axis z = self.math.zeros(1) return self.math.vertcat(axis[0], axis[1], axis[2], z, z, z) + elif self.type == "spherical": + z = self.math.zeros(3, 3) + eye = self.math.eye(3) + return self.math.vertcat(z, eye) diff --git a/src/adam/model/std_factories/std_model.py b/src/adam/model/std_factories/std_model.py index a130224e..797f630c 100644 --- a/src/adam/model/std_factories/std_model.py +++ b/src/adam/model/std_factories/std_model.py @@ -73,6 +73,11 @@ def __init__(self, path: str, math: SpatialMath): # See https://github.com/ami-iit/ADAM/issues/59 xml_string_without_sensors_tags = urdf_remove_sensors_tags(xml_string) + + # Add "spherical" to the list of allowed joint types as urdf_parser_py does not natively support it + if "spherical" not in urdf_parser_py.urdf.Joint.TYPES: + urdf_parser_py.urdf.Joint.TYPES.append("spherical") + self.urdf_desc = urdf_parser_py.urdf.URDF.from_xml_string( xml_string_without_sensors_tags ) diff --git a/src/adam/parametric/model/parametric_factories/parametric_joint.py b/src/adam/parametric/model/parametric_factories/parametric_joint.py index 68ac7312..520a79de 100644 --- a/src/adam/parametric/model/parametric_factories/parametric_joint.py +++ b/src/adam/parametric/model/parametric_factories/parametric_joint.py @@ -35,6 +35,14 @@ def __init__( ) self.offset = joint_offset self.origin = self.modify(self.parent_parametric.link_offset) + self.dofs = self._infer_dofs() + + def _infer_dofs(self) -> int: + if self.type == "fixed": + return 0 + if self.type == "spherical": + return 3 + return 1 def _set_axis(self, axis: npt.ArrayLike) -> npt.ArrayLike: """ @@ -118,6 +126,12 @@ def homogeneous(self, q: npt.ArrayLike) -> npt.ArrayLike: self.axis, q, ) + elif self.type in ["spherical"]: + return self.math.H_spherical_joint( + self.origin.xyz, + self.origin.rpy, + q, + ) def spatial_transform(self, q: npt.ArrayLike) -> npt.ArrayLike: """ @@ -140,6 +154,12 @@ def spatial_transform(self, q: npt.ArrayLike) -> npt.ArrayLike: self.axis, q, ) + elif self.type in ["spherical"]: + return self.math.X_spherical_joint( + self.origin.xyz, + self.origin.rpy, + q, + ) def motion_subspace(self) -> npt.ArrayLike: """ @@ -159,3 +179,7 @@ def motion_subspace(self) -> npt.ArrayLike: axis = self.axis z = self.math.zeros(1) return self.math.vertcat(axis[0], axis[1], axis[2], z, z, z) + elif self.type in ["spherical"]: + zeros = self.math.zeros(3, 3) + eye = self.math.eye(3) + return self.math.vertcat(zeros, eye)