diff --git a/src/adam/casadi/casadi_like.py b/src/adam/casadi/casadi_like.py index 9ee0ce02..fbdee660 100644 --- a/src/adam/casadi/casadi_like.py +++ b/src/adam/casadi/casadi_like.py @@ -93,6 +93,12 @@ def __getitem__(self, idx) -> "CasadiLike": """Overrides get item operator""" return CasadiLike(self.array[idx]) + def __eq__(self, other: Union["CasadiLike", npt.ArrayLike]) -> bool: + """Overrides == operator""" + if type(self) is not type(other): + return self.array == other + return self.array == other.array + @property def T(self) -> "CasadiLike": """ @@ -149,6 +155,39 @@ def skew(x: Union["CasadiLike", npt.ArrayLike]) -> "CasadiLike": else: return CasadiLike(cs.skew(x)) + @staticmethod + def vee(x: Union["CasadiLike", npt.ArrayLike]) -> "CasadiLike": + """ + Args: + x (Union[CasadiLike, npt.ArrayLike]): 3x3 matrix + + Returns: + CasadiLike: the vector from the skew symmetric matrix x + """ + vee = lambda x: cs.vertcat(x[2, 1], x[0, 2], x[1, 0]) + if isinstance(x, CasadiLike): + return CasadiLike(vee(x.array)) + else: + return CasadiLike(vee(x)) + + @staticmethod + def inv(x: Union["CasadiLike", npt.ArrayLike]) -> "CasadiLike": + """ + Args: + x (Union[CasadiLike, npt.ArrayLike]): matrix + + Returns: + CasadiLike: inverse of x + """ + if isinstance(x, CasadiLike): + return CasadiLike(cs.inv(x.array)) + else: + return ( + CasadiLike(cs.inv(x)) + if x.size1() <= 5 + else CasadiLike(self.solve(x, self.eye(x.size1()))) + ) + @staticmethod def sin(x: npt.ArrayLike) -> "CasadiLike": """ @@ -206,6 +245,18 @@ def horzcat(*x) -> "CasadiLike": y = [xi.array if isinstance(xi, CasadiLike) else xi for xi in x] return CasadiLike(cs.horzcat(*y)) + @staticmethod + def solve(A: "CasadiLike", b: "CasadiLike") -> "CasadiLike": + """ + Args: + A (CasadiLike): matrix + b (CasadiLike): vector + + Returns: + CasadiLike: solution of A*x=b + """ + return CasadiLike(cs.solve(A.array, b.array)) + if __name__ == "__main__": math = SpatialMath() diff --git a/src/adam/casadi/computations.py b/src/adam/casadi/computations.py index eab62718..64a45e69 100644 --- a/src/adam/casadi/computations.py +++ b/src/adam/casadi/computations.py @@ -411,3 +411,33 @@ def CoM_position( CoM (Union[cs.SX, cs.DM]): The CoM position """ return self.rbdalgos.CoM_position(base_transform, joint_positions).array + + def forward_dynamics( + self, + base_transform: Union[cs.SX, cs.DM], + base_velocity: Union[cs.SX, cs.DM], + joint_positions: Union[cs.SX, cs.DM], + joint_velocities: Union[cs.SX, cs.DM], + joint_torques: Union[cs.SX, cs.DM], + ) -> Union[cs.SX, cs.DM]: + """Returns base and joints accelerations of the floating-base dynamics equation + + Args: + base_transform (Union[cs.SX, cs.DM]): The homogenous transform from base to world frame + base_velocity (Union[cs.SX, cs.DM]): The base velocity in mixed representation + joint_positions (Union[cs.SX, cs.DM]): The joints position + joint_velocities (Union[cs.SX, cs.DM]): The joints velocity + joint_torques (Union[cs.SX, cs.DM]): The joints torque + + Returns: + base_acceleration (Union[cs.SX, cs.DM]): The base acceleration in mixed representation + joint_accelerations (Union[cs.SX, cs.DM]): The joints acceleration + """ + return self.rbdalgos.aba( + base_transform, + base_velocity, + joint_positions, + joint_velocities, + joint_torques, + self.g, + ) diff --git a/src/adam/core/rbd_algorithms.py b/src/adam/core/rbd_algorithms.py index ba68391f..a1ef1027 100644 --- a/src/adam/core/rbd_algorithms.py +++ b/src/adam/core/rbd_algorithms.py @@ -5,7 +5,7 @@ from adam.core.constants import Representations from adam.core.spatial_math import SpatialMath -from adam.model import Model, Node +from adam.model import Model, Node, Link class RBDAlgorithms: @@ -457,5 +457,170 @@ def rnea( tau[:6] = B_X_BI.T @ tau[:6] return tau - def aba(self): - raise NotImplementedError + def aba( + self, + base_transform: npt.ArrayLike, + base_velocity: npt.ArrayLike, + joint_positions: npt.ArrayLike, + joint_velocities: npt.ArrayLike, + tau: npt.ArrayLike, + g: npt.ArrayLike, + ) -> npt.ArrayLike: + """Implementation of Articulated Body Algorithm + + Args: + base_transform (T): The homogenous transform from base to world frame + base_velocity (T): The base velocity in mixed representation + joint_positions (T): The joints position + joint_velocities (T): The joints velocity + tau (T): The generalized force variables + g (T): The 6D gravity acceleration + + Returns: + base_acceleration (T): The base acceleration in mixed representation + joint_accelerations (T): The joints acceleration + """ + model = self.model.reduce(self.model.actuated_joints) + joints = list(model.joints.values()) + + NB = model.N + + i_X_pi = self.math.factory.zeros(NB, 6, 6) + v = self.math.factory.zeros(NB, 6, 1) + c = self.math.factory.zeros(NB, 6, 1) + pA = self.math.factory.zeros(NB, 6, 1) + IA = self.math.factory.zeros(NB, 6, 6) + U = self.math.factory.zeros(NB, 6, 1) + D = self.math.factory.zeros(NB, 1, 1) + u = self.math.factory.zeros(NB, 1, 1) + + a = self.math.factory.zeros(NB, 6, 1) + sdd = self.math.factory.zeros(NB, 1, 1) + B_X_W = self.math.adjoint_mixed(base_transform) + + if model.floating_base: + IA[0] = model.tree.get_node_from_name(self.root_link).link.spatial_inertia() + v[0] = B_X_W @ base_velocity + pA[0] = ( + self.math.spatial_skew_star(v[0]) @ IA[0] @ v[0] + ) # - self.math.adjoint_inverse(B_X_W).T @ f_ext[0] + + def get_tree_transform(self, joints) -> "Array": + """returns the tree transform + + Returns: + Array: the tree transform + """ + relative_transform = lambda j: self.math.inv( + model.tree.graph[j.child].parent_arc.spatial_transform(0) + ) @ j.spatial_transform(0) + + return self.math.vertcat( + [self.math.factory.eye(6).array] + + list( + map( + lambda j: ( + relative_transform(j).array + if j.parent != self.root_link + else self.math.factory.eye(6).array + ), + joints, + ) + ) + ) + + tree_transform = get_tree_transform(self, joints) + # p = lambda i: list(model.tree.graph).index(joints[i].parent) + + # def p(i): + # jp = joints[i].parent + + # try: + # if model.tree.graph[jp].parent_arc.name in model.actuated_joints: + # return model.tree.graph[jp].parent_arc.idx + # else: + # return p(model.tree.graph[jp].parent_arc.parent.idx) + # except AttributeError: + # if model.tree.graph[jp].parent_arc is None: + # return -1 # root link + # else: + # return p(model.tree.graph[jp].parent_arc.idx) + + # Pass 1 + for i, node in enumerate(model.tree): + if node.name == self.root_link: + continue + # link, parent_arc, parent + link_i, joint_i, link_pi = node.get_elements() + pi = model.tree.get_idx_from_name(link_pi.name) + + q = joint_positions[i - 1] + q_dot = joint_velocities[i - 1] + + # Parent-child transform + i_X_pi[i] = tree_transform[i] @ joint_i.spatial_transform(q) + v_J = joint_i.motion_subspace() * q_dot + + v[i] = i_X_pi[i] @ v[pi] + v_J + c[i] = i_X_pi[i] @ c[pi] + self.math.spatial_skew(v[i]) @ v_J + + IA[i] = link_i.spatial_inertia() + + pA[i] = IA[i] @ c[i] + self.math.spatial_skew_star(v[i]) @ IA[i] @ v[i] + + LUMPED = False + + # Pass 2 + for i, node in enumerate(reversed(model.tree)): + if node.name == self.root_link: + continue + + link_i, joint_i, link_pi = node.get_elements() + pi = model.tree.get_idx_from_name(link_pi.name) + + U[i] = IA[i] @ joint_i.motion_subspace() + D[i] = joint_i.motion_subspace().T @ U[i] + u[i] = self.math.vertcat(tau[i - 1]) - joint_i.motion_subspace().T @ pA[i] + + Ia = IA[i] - U[i] / D[i] @ U[i].T + pa = pA[i] + Ia @ c[i] + U[i] * u[i] / D[i] + + if joint_i.parent != self.root_link or not model.floating_base: + IA[pi] += i_X_pi[i].T @ Ia @ i_X_pi[i] + pA[pi] += i_X_pi[i].T @ pa + continue + + a[0] = B_X_W @ g if model.floating_base else self.math.solve(-IA[0], pA[0]) + + # Pass 3 + for i, node in enumerate(model.tree): + if node.name == self.root_link: + continue + + link_i, joint_i, link_pi = node.get_elements() + pi = model.tree.get_idx_from_name(link_pi.name) + + sdd[i - 1] = (u[i] - U[i].T @ a[i]) / D[i] + + a[i] += i_X_pi[i].T @ a[pi] + joint_i.motion_subspace() * sdd[i - 1] + c[i] + + # Squeeze sdd + s_ddot = self.math.vertcat(*[sdd[i] for i in range(sdd.shape[0])]) + + if ( + self.frame_velocity_representation + == Representations.BODY_FIXED_REPRESENTATION + ): + return (a[0], s_ddot) + + elif self.frame_velocity_representation == Representations.MIXED_REPRESENTATION: + return ( + self.math.horzcat( + ( + (self.math.solve(B_X_W, a[0]) + g)[:, None] + if model.floating_base + else self.math.zeros(6, 1) + ), + ), + s_ddot, + ) diff --git a/src/adam/core/spatial_math.py b/src/adam/core/spatial_math.py index 3e1a0f8c..778257ee 100644 --- a/src/adam/core/spatial_math.py +++ b/src/adam/core/spatial_math.py @@ -158,6 +158,26 @@ def cos(x: npt.ArrayLike) -> npt.ArrayLike: def skew(x): pass + @abc.abstractmethod + def vee(x): + pass + + @abc.abstractmethod + def inv(x): + pass + + @abc.abstractmethod + def solve(A: npt.ArrayLike, b: npt.ArrayLike) -> npt.ArrayLike: + """ + Args: + A (npt.ArrayLike): matrix + b (npt.ArrayLike): vector + + Returns: + npt.ArrayLike: solution of the linear system Ax=b + """ + pass + def R_from_axis_angle(self, axis: npt.ArrayLike, q: npt.ArrayLike) -> npt.ArrayLike: """ Args: diff --git a/src/adam/jax/computations.py b/src/adam/jax/computations.py index 2f68d092..e99a2848 100644 --- a/src/adam/jax/computations.py +++ b/src/adam/jax/computations.py @@ -239,3 +239,35 @@ def get_total_mass(self) -> float: mass: The total mass """ return self.rbdalgos.get_total_mass() + + def forward_dynamics( + self, + base_transform: jnp.array, + base_velocity: jnp.array, + joint_positions: jnp.array, + joint_velocities: jnp.array, + joint_torques: jnp.array, + ) -> jnp.array: + """Returns base and joints accelerations of the floating-base dynamics equation + + Args: + base_transform (jnp.array): The homogenous transform from base to world frame + base_velocity (jnp.array): The base velocity in mixed representation + joint_positions (jnp.array): The joints position + joint_velocities (jnp.array): The joints velocity + joint_torques (jnp.array): The joints torques + + Returns: + base_acceleration (jnp.array): The base acceleration in mixed representation + joint_accelerations (jnp.array): The joints acceleration + """ + base_acceleration, joint_accelerations = self.rbdalgos.aba( + base_transform, + base_velocity, + joint_positions, + joint_velocities, + joint_torques, + self.g, + ) + + return base_acceleration.array.squeeze(), joint_accelerations.array.squeeze() diff --git a/src/adam/jax/jax_like.py b/src/adam/jax/jax_like.py index 23ddc2a1..89b95d79 100644 --- a/src/adam/jax/jax_like.py +++ b/src/adam/jax/jax_like.py @@ -107,6 +107,12 @@ def __neg__(self) -> "JaxLike": """Overrides - operator""" return JaxLike(-self.array) + def __eq__(self, other: Union["JaxLike", npt.ArrayLike]) -> bool: + """Overrides == operator""" + if type(self) is not type(other): + return self.array.squeeze() == other.squeeze() + return self.array.squeeze() == other.array.squeeze() + class JaxLikeFactory(ArrayLikeFactory): @staticmethod @@ -188,6 +194,31 @@ def skew(x: Union["JaxLike", npt.ArrayLike]) -> "JaxLike": x = x.array return JaxLike(-jnp.cross(jnp.array(x), jnp.eye(3), axisa=0, axisb=0)) + @staticmethod + def vee(x: Union["JaxLike", npt.ArrayLike]) -> "JaxLike": + """ + Args: + x (Union[JaxLike, npt.ArrayLike]): matrix + + Returns: + JaxLike: the vee operator from x + """ + if not isinstance(x, JaxLike): + return JaxLike(jnp.array([x[2, 1], x[0, 2], x[1, 0]])) + x = x.array + return JaxLike(jnp.array([x[2, 1], x[0, 2], x[1, 0]])) + + @staticmethod + def inv(x: "JaxLike") -> "JaxLike": + """ + Args: + x (JaxLike): Matrix + + Returns: + JaxLike: Inverse of x + """ + return JaxLike(jnp.linalg.inv(x.array)) + @staticmethod def vertcat(*x) -> "JaxLike": """ @@ -211,3 +242,15 @@ def horzcat(*x) -> "JaxLike": else: v = jnp.hstack([x[i] for i in range(len(x))]) return JaxLike(v) + + @staticmethod + def solve(A: "JaxLike", b: "JaxLike") -> "JaxLike": + """ + Args: + A (JaxLike): Matrix + b (JaxLike): Vector + + Returns: + JaxLike: Solution of Ax=b + """ + return JaxLike(jnp.linalg.solve(A.array, b.array)) diff --git a/src/adam/model/abc_factories.py b/src/adam/model/abc_factories.py index 9e406026..8789a045 100644 --- a/src/adam/model/abc_factories.py +++ b/src/adam/model/abc_factories.py @@ -92,6 +92,19 @@ def homogeneous(self) -> npt.ArrayLike: """ pass + @abc.abstractmethod + def lump(self, other: "Link", relative_transform: npt.ArrayLike) -> "Link": + """lump two links together + + Args: + other (Link): the other link + relative_transform (npt.ArrayLike): the transform between the two links + + Returns: + Link: the lumped link + """ + pass + @dataclasses.dataclass class ModelFactory(abc.ABC): diff --git a/src/adam/model/model.py b/src/adam/model/model.py index 0f51306f..b2224f7b 100644 --- a/src/adam/model/model.py +++ b/src/adam/model/model.py @@ -18,6 +18,7 @@ class Model: tree: Tree NDoF: int actuated_joints: List[str] + floating_base: bool = True def __post_init__(self): """set the "length of the model as the number of links""" @@ -61,10 +62,12 @@ def build(factory: ModelFactory, joints_name_list: List[str] = None) -> "Model": tree = Tree.build_tree(links=links, joints=joints) + floating_base = tree.is_floating_base() + # generate some useful dict - joints: Dict(str, Joint) = {joint.name: joint for joint in joints} - links: Dict(str, Link) = {link.name: link for link in links} - frames: Dict(str, Link) = {frame.name: frame for frame in frames} + joints: Dict[str, Joint] = {joint.name: joint for joint in joints} + links: Dict[str, Link] = {link.name: link for link in links} + frames: Dict[str, Link] = {frame.name: frame for frame in frames} return Model( name=factory.name, @@ -74,6 +77,40 @@ def build(factory: ModelFactory, joints_name_list: List[str] = None) -> "Model": tree=tree, NDoF=len(joints_name_list), actuated_joints=joints_name_list, + floating_base=floating_base, + ) + + def reduce(self, joints_name_list: List[str]) -> "Model": + """reduce the model to a subset of joints + + Args: + joints_name_list (List[str]): the list of the joints to keep + + Returns: + Model: the reduced model + """ + + tree = self.tree.reduce(joints_name_list) + + links = {node.name: node.link for node in tree.graph.values()} + joints = { + joint.name: joint + for joint in self.joints.values() + if joint.name in joints_name_list + } + for link in self.links: + if link not in tree.graph: + self.frames[link] = link + + return Model( + name=self.name, + links=links, + frames=self.frames, + joints=joints, + tree=tree, + NDoF=len(joints_name_list), + actuated_joints=joints_name_list, + floating_base=self.floating_base, ) def get_joints_chain(self, root: str, target: str) -> List[Joint]: @@ -87,24 +124,24 @@ def get_joints_chain(self, root: str, target: str) -> List[Joint]: List[Joint]: the list of the joints """ - if target not in list(self.links) and target not in list(self.frames): + if target not in list(self.frames) and target not in list(self.tree.graph): raise ValueError(f"{target} is not not in the robot model.") if target == root: return [] chain = [] - current_node = [ + current_joint = [ joint for joint in self.joints.values() if joint.child == target ][0] - chain.insert(0, current_node) - while current_node.parent != root: - current_node = [ + chain.insert(0, current_joint) + while current_joint.parent != root: + current_joint = [ joint for joint in self.joints.values() - if joint.child == current_node.parent + if joint.child == current_joint.parent ][0] - chain.insert(0, current_node) + chain.insert(0, current_joint) return chain def get_total_mass(self) -> float: diff --git a/src/adam/model/std_factories/std_joint.py b/src/adam/model/std_factories/std_joint.py index a72787d8..3043b795 100644 --- a/src/adam/model/std_factories/std_joint.py +++ b/src/adam/model/std_factories/std_joint.py @@ -104,3 +104,6 @@ def motion_subspace(self) -> npt.ArrayLike: 0, 0, ) + + def __hash__(self) -> int: + return hash(self.name) diff --git a/src/adam/model/std_factories/std_link.py b/src/adam/model/std_factories/std_link.py index cefac67c..29bc70f9 100644 --- a/src/adam/model/std_factories/std_link.py +++ b/src/adam/model/std_factories/std_link.py @@ -44,3 +44,45 @@ def homogeneous(self) -> npt.ArrayLike: self.inertial.origin.xyz, self.inertial.origin.rpy, ) + + def lump(self, other: "StdLink", relative_transform: npt.ArrayLike) -> "StdLink": + """lump two links together + + Args: + other (StdLink): the other link + relative_transform (npt.ArrayLike): the transform between the two links + + Returns: + StdLink: the lumped link + """ + # compute origin from inertia + origin = self.math.vee(self.spatial_inertia()[3:, :3]) / self.inertial.mass + + # compute rotation matrix from RPY + R = self.math.R_from_RPY(self.inertial.origin.rpy) + + other_inertia = ( + relative_transform.T @ other.spatial_inertia() @ relative_transform + ) + + # lump the inertial properties + lumped_mass = self.inertial.mass + other.inertial.mass + lumped_inertia = self.spatial_inertia() + other_inertia + + inertia_matrix = ( + lumped_inertia[3:, :3] + - lumped_mass * self.math.skew(origin) @ self.math.skew(origin).T + ) + + self.inertial.mass = lumped_mass + self.inertial.inertia.ixx = inertia_matrix[0, 0].array + self.inertial.inertia.ixy = inertia_matrix[0, 1].array + self.inertial.inertia.ixz = inertia_matrix[0, 2].array + self.inertial.inertia.iyy = inertia_matrix[1, 1].array + self.inertial.inertia.iyz = inertia_matrix[1, 2].array + self.inertial.inertia.izz = inertia_matrix[2, 2].array + + return self + + def __hash__(self): + return hash(self.name) diff --git a/src/adam/model/std_factories/std_model.py b/src/adam/model/std_factories/std_model.py index ffeff713..bf3c3e75 100644 --- a/src/adam/model/std_factories/std_model.py +++ b/src/adam/model/std_factories/std_model.py @@ -17,10 +17,7 @@ def urdf_remove_sensors_tags(xml_string): for sensors_tag in root.findall("sensor"): root.remove(sensors_tag) - # Convert the modified XML back to a string - modified_xml_string = ET.tostring(root) - - return modified_xml_string + return ET.tostring(root) class URDFModelFactory(ModelFactory): @@ -44,9 +41,8 @@ def __init__(self, path: str, math: SpatialMath): # to have a useless and noisy warning, let's remove before hands all the sensor elements, # that anyhow are not parser by urdf_parser_py or adam # See https://github.com/ami-iit/ADAM/issues/59 - xml_file = open(path, "r") - xml_string = xml_file.read() - xml_file.close() + with open(path, "r") as xml_file: + xml_string = xml_file.read() xml_string_without_sensors_tags = urdf_remove_sensors_tags(xml_string) self.urdf_desc = urdf_parser_py.urdf.URDF.from_xml_string( xml_string_without_sensors_tags diff --git a/src/adam/model/tree.py b/src/adam/model/tree.py index dc96bfe5..5b79de5f 100644 --- a/src/adam/model/tree.py +++ b/src/adam/model/tree.py @@ -1,5 +1,7 @@ import dataclasses -from typing import Dict, Iterable, List, Tuple, Union +import logging + +from typing import Dict, Iterable, List, Tuple, Union, Set, Iterator from adam.model.abc_factories import Joint, Link @@ -31,7 +33,7 @@ def get_elements(self) -> Tuple[Link, Joint, Link]: class Tree(Iterable): """The directed tree class""" - graph: Dict + graph: Dict[str, Node] root: str def __post_init__(self): @@ -48,7 +50,7 @@ def build_tree(links: List[Link], joints: List[Joint]) -> "Tree": Returns: Tree: the directed tree """ - nodes: Dict(str, Node) = { + nodes: Dict[str, Node] = { l.name: Node( name=l.name, link=l, arcs=[], children=[], parent=None, parent_arc=None ) @@ -71,6 +73,78 @@ def build_tree(links: List[Link], joints: List[Joint]) -> "Tree": raise ValueError("The model has more than one root link") return Tree(nodes, root_link[0]) + def reduce(self, considered_joint_names: List[Joint]) -> "Tree": + """reduces the tree to the considered joints + + Args: + considered_joint_names (List[Joint]): the list of the considered joints + + Returns: + Tree: the reduced tree + """ + + relative_transform = lambda node: ( + node.link.math.inv( + self.graph[node.parent.name].parent_arc.spatial_transform(0) + ) + @ node.parent_arc.spatial_transform(0) + if node.parent.name != self.root + else node.parent_arc.spatial_transform(0) + ) + + # find the fixed joints using the considered_joint_names + fixed_joints = [ + joint + for joint in self.get_joint_list() + if joint.name not in considered_joint_names + ] + # set fixed joints to fixed + for joint in fixed_joints: + joint.type = "fixed" + + for fixed_j in fixed_joints: + saved_node = self.graph[fixed_j.parent] + removing_node = self.graph[fixed_j.child] + + saved_node.children.remove(removing_node) + saved_node.children.extend(removing_node.children) + # update the arcs + saved_node.arcs.remove(fixed_j) + saved_node.arcs.extend(removing_node.arcs) + + # saved_node.link = saved_node.link.lump( + # other=removing_node.link, joint=fixed_j + # ) + + # merged_joint = saved_node.parent_arc + # removed_joint = removing_node.parent_arc + # update the parent arc of the merged node + # saved_node.parent_arc = saved_node.parent_arc.lump(removed_joint) + + # we need to updated the parents and child on the joints in fixed_joints + for joint in self.get_joint_list(): + if joint.parent == removing_node.name: + joint.parent = saved_node.name + if joint.child == removing_node.name: + joint.child = saved_node.name + + for child in saved_node.children: + child.parent = saved_node.link + child.parent_arc = saved_node.parent_arc + + self.graph.pop(removing_node.name) + self.graph[saved_node.name] = saved_node + + if {joint.name for joint in self.get_joint_list()} != set( + considered_joint_names + ): + raise ValueError( + "The joints remaining in the graph are not equal to the considered joints" + ) + tree = Tree(self.graph, self.root) + tree.print(self.root) + return tree + def print(self, root) -> str: """prints the tree @@ -94,8 +168,7 @@ def get_ordered_nodes_list(self, start: str) -> List[str]: self.get_children(self.graph[start], ordered_list) return ordered_list - @classmethod - def get_children(cls, node: Node, list: List): + def get_children(self, node: Node, list: List): """Recursive method that finds children of child of child Args: node (Node): the analized node @@ -104,7 +177,7 @@ def get_children(cls, node: Node, list: List): if node.children is not []: for child in node.children: list.append(child.name) - cls.get_children(child, list) + self.get_children(child, list) def get_idx_from_name(self, name: str) -> int: """ @@ -136,7 +209,21 @@ def get_node_from_name(self, name: str) -> Node: """ return self.graph[name] - def __iter__(self) -> Node: + def is_floating_base(self) -> bool: + """ + Returns: + bool: True if the model is floating base + """ + return len(self.graph[self.root].children) > 1 + + def get_joint_list(self) -> Set[Joint]: + """ + Returns: + Set[Joint]: the set of the joints + """ + return {arc for node in self.graph.values() for arc in node.arcs} + + def __iter__(self) -> Iterator[Node]: """This method allows to iterate on the model Returns: Node: the node istance @@ -146,7 +233,7 @@ def __iter__(self) -> Node: """ yield from [self.graph[name] for name in self.ordered_nodes_list] - def __reversed__(self) -> Node: + def __reversed__(self) -> Iterator[Node]: """ Returns: Node @@ -154,7 +241,7 @@ def __reversed__(self) -> Node: Yields: Iterator[Node]: the reversed nodes list """ - yield from reversed(self) + yield from reversed([self.graph[name] for name in self.ordered_nodes_list]) def __getitem__(self, key) -> Node: """get the item at key in the model diff --git a/src/adam/numpy/computations.py b/src/adam/numpy/computations.py index ad531f08..b6bd14a9 100644 --- a/src/adam/numpy/computations.py +++ b/src/adam/numpy/computations.py @@ -244,3 +244,33 @@ def get_total_mass(self) -> float: mass: The total mass """ return self.rbdalgos.get_total_mass() + + def forward_dynamics( + self, + base_transform: np.ndarray, + base_velocity: np.ndarray, + joint_positions: np.ndarray, + joint_velocities: np.ndarray, + joint_torques: np.ndarray, + ) -> np.ndarray: + """Returns base and joints accelerations of the floating-base dynamics equation + + Args: + base_transform (np.ndarray): The homogenous transform from base to world frame + base_velocity (np.ndarray): The base velocity in mixed representation + joint_positions (np.ndarray): The joints position + joint_velocities (np.ndarray): The joint velocities + joint_torques (np.ndarray): The joint torques + + Returns: + base_acceleration (np.ndarray): the base acceleration + joint_accelerations (np.ndarray): the joint accelerations + """ + return self.rbdalgos.aba( + base_transform, + base_velocity, + joint_positions, + joint_velocities, + joint_torques, + self.g, + ) diff --git a/src/adam/numpy/numpy_like.py b/src/adam/numpy/numpy_like.py index ddfac73a..4a761f2c 100644 --- a/src/adam/numpy/numpy_like.py +++ b/src/adam/numpy/numpy_like.py @@ -106,6 +106,13 @@ def __neg__(self): """Overrides - operator""" return NumpyLike(-self.array) + def __eq__(self, other: Union["NumpyLike", npt.ArrayLike]) -> bool: + """Overrides == operator""" + if type(self) is type(other): + return self.array == other.array + else: + return self.array == other + class NumpyLikeFactory(ArrayLikeFactory): @staticmethod @@ -213,3 +220,42 @@ def skew(x: Union["NumpyLike", npt.ArrayLike]) -> "NumpyLike": return -np.cross(np.array(x), np.eye(3), axisa=0, axisb=0) x = x.array return NumpyLike(-np.cross(np.array(x), np.eye(3), axisa=0, axisb=0)) + + @staticmethod + def vee(x: Union["NumpyLike", npt.ArrayLike]) -> "NumpyLike": + """ + Args: + x (Union[NumpyLike, npt.ArrayLike]): skew symmetric matrix + + Returns: + NumpyLike: vector from the skew symmetric matrix + """ + if not isinstance(x, NumpyLike): + return np.array([x[2, 1], x[0, 2], x[1, 0]]) + return np.array([x.array[2, 1], x.array[0, 2], x.array[1, 0]]) + + @staticmethod + def inv(x: Union["NumpyLike", npt.ArrayLike]) -> "NumpyLike": + """ + Args: + x (Union["NumpyLike", npt.ArrayLike]): matrix + + Returns: + NumpyLike: inverse of x + """ + if isinstance(x, NumpyLike): + return NumpyLike(np.linalg.inv(x.array)) + else: + return NumpyLike(np.linalg.inv(x)) + + @staticmethod + def solve(A: "NumpyLike", b: "NumpyLike") -> "NumpyLike": + """ + Args: + A (NumpyLike): matrix + b (NumpyLike): vector + + Returns: + NumpyLike: solution of Ax=b + """ + return NumpyLike(np.linalg.solve(A.array, b.array)) diff --git a/src/adam/pytorch/computations.py b/src/adam/pytorch/computations.py index e81c48be..07a1c96e 100644 --- a/src/adam/pytorch/computations.py +++ b/src/adam/pytorch/computations.py @@ -252,3 +252,33 @@ def get_total_mass(self) -> float: mass: The total mass """ return self.rbdalgos.get_total_mass() + + def forward_dynamic( + self, + base_transform: torch.Tensor, + base_velocity: torch.Tensor, + joint_positions: torch.Tensor, + joint_velocities: torch.Tensor, + joint_torques: torch.Tensor, + ) -> torch.Tensor: + """Returns base and joints accelerations of the floating-base dynamics equation + + Args: + base_transform (torch.tensor): The homogenous transform from base to world frame + base_velocity (torch.tensor): The base velocity in mixed representation + joint_positions (torch.tensor): The joints positions + joint_velocities (torch.tensor): The joints velocities + joint_torques (torch.tensor): The joints torques + + Returns: + base_acceleration (torch.tensor): The base acceleration in mixed representation + joint_accelerations (torch.tensor): The joints accelerations + """ + return self.rbdalgos.aba( + base_transform, + base_velocity, + joint_positions, + joint_velocities, + joint_torques, + self.g, + ).array.squeeze() diff --git a/src/adam/pytorch/torch_like.py b/src/adam/pytorch/torch_like.py index 71962f65..c9d7091a 100644 --- a/src/adam/pytorch/torch_like.py +++ b/src/adam/pytorch/torch_like.py @@ -122,6 +122,13 @@ def __neg__(self) -> "TorchLike": """Overrides - operator""" return TorchLike(-self.array) + def __eq__(self, other: Union["TorchLike", ntp.ArrayLike]) -> bool: + """Overrides == operator""" + if type(self) is type(other): + return self.array == other.array + else: + return self.array == other + class TorchLikeFactory(ArrayLikeFactory): @staticmethod @@ -213,6 +220,36 @@ def skew(x: Union["TorchLike", ntp.ArrayLike]) -> "TorchLike": torch.tensor([[0, -x[2], x[1]], [x[2], 0, -x[0]], [-x[1], x[0], 0]]) ) + @staticmethod + def vee(x: Union["TorchLike", ntp.ArrayLike]) -> "TorchLike": + """ + Args: + x (Union["TorchLike", ntp.ArrayLike]): matrix + + Returns: + TorchLike: vector from skew matrix x + """ + if isinstance(x, TorchLike): + return TorchLike( + torch.tensor([x.array[2, 1], x.array[0, 2], x.array[1, 0]]) + ) + else: + return TorchLike(torch.tensor([x[2, 1], x[0, 2], x[1, 0]])) + + @staticmethod + def inv(x: Union["TorchLike", ntp.ArrayLike]) -> "TorchLike": + """ + Args: + x (Union["TorchLike", ntp.ArrayLike]): matrix + + Returns: + TorchLike: inverse of x + """ + if isinstance(x, TorchLike): + return TorchLike(torch.inverse(x.array)) + else: + return TorchLike(torch.inverse(torch.tensor(x))) + @staticmethod def vertcat(*x: ntp.ArrayLike) -> "TorchLike": """ @@ -236,3 +273,15 @@ def horzcat(*x: ntp.ArrayLike) -> "TorchLike": else: v = torch.tensor(x) return TorchLike(v) + + @staticmethod + def solve(A: "TorchLike", b: "TorchLike") -> "TorchLike": + """ + Args: + A (TorchLike): matrix + b (TorchLike): vector + + Returns: + TorchLike: solution of Ax = b + """ + return TorchLike(torch.linalg.solve(A.array, b.array)) diff --git a/tests/body_fixed/test_CasADi_body_fixed.py b/tests/body_fixed/test_CasADi_body_fixed.py index d9686912..7ee75b47 100644 --- a/tests/body_fixed/test_CasADi_body_fixed.py +++ b/tests/body_fixed/test_CasADi_body_fixed.py @@ -239,3 +239,17 @@ def test_relative_jacobian(): J_test2 = cs.DM(comp.relative_jacobian("l_sole", joints_val)) assert iDynNumpyRelativeJ - J_test == pytest.approx(0.0, abs=1e-4) assert iDynNumpyRelativeJ - J_test2 == pytest.approx(0.0, abs=1e-4) + + +def test_fd(): + joint_torques = np.random.rand(n_dofs) + joints_vel = np.random.rand(n_dofs) + reference_acc = np.linalg.inv(comp.mass_matrix(H_b, joints_val)) @ ( + np.concatenate((np.zeros(6), joint_torques)) + - comp.bias_force(H_b, joints_val, base_vel, joints_vel) + ) + base_acc, joint_acc = comp.forward_dynamics( + H_b, base_vel, joints_vel, joints_vel, joint_torques + ) + assert base_acc - reference_acc[:6] == pytest.approx(0.0, abs=1e-4) + assert joint_acc - reference_acc[6:] == pytest.approx(0.0, abs=1e-4) diff --git a/tests/body_fixed/test_Jax_body_fixed.py b/tests/body_fixed/test_Jax_body_fixed.py index 10330947..72d33537 100644 --- a/tests/body_fixed/test_Jax_body_fixed.py +++ b/tests/body_fixed/test_Jax_body_fixed.py @@ -6,7 +6,6 @@ import icub_models import idyntree.swig as idyntree -import jax.numpy as jnp import numpy as np import pytest from jax import config @@ -192,3 +191,17 @@ def test_gravity_term(): ) G_test = comp.gravity_term(H_b, joints_val) assert G_iDyn_np - G_test == pytest.approx(0.0, abs=1e-4) + + +def test_fd(): + joint_torques = np.random.rand(n_dofs) + joints_vel = np.random.rand(n_dofs) + reference_acc = np.linalg.inv(comp.mass_matrix(H_b, joints_val)) @ ( + np.concatenate((np.zeros(6), joint_torques)) + - comp.bias_force(H_b, joints_val, base_vel, joints_vel) + ) + base_acc, joint_acc = comp.forward_dynamics( + H_b, base_vel, joints_vel, joints_vel, joint_torques + ) + assert base_acc - reference_acc[:6] == pytest.approx(0.0, abs=1e-4) + assert joint_acc - reference_acc[6:] == pytest.approx(0.0, abs=1e-4) diff --git a/tests/body_fixed/test_NumPy_body_fixed.py b/tests/body_fixed/test_NumPy_body_fixed.py index 5704c87d..0325be62 100644 --- a/tests/body_fixed/test_NumPy_body_fixed.py +++ b/tests/body_fixed/test_NumPy_body_fixed.py @@ -8,14 +8,19 @@ import idyntree.swig as idyntree import numpy as np import pytest - +from pathlib import Path from adam import Representations from adam.geometry import utils from adam.numpy import KinDynComputations np.random.seed(42) - +# model_path = str(icub_models.get_model_file("iCubGazeboV2_5")) +# import robot_descriptions.ur10_description + +# model_path = str(robot_descriptions.ur10_description.URDF_PATH) +# model_path = "/home/flferretti/git/element_rl-for-codesign/assets/model/Cartpole.urdf" + joints_name_list = [ "torso_pitch", @@ -42,6 +47,7 @@ "r_ankle_pitch", "r_ankle_roll", ] +# joints_name_list = ["linear", "pivot"] def H_from_Pos_RPY_idyn(xyz, rpy): @@ -189,3 +195,46 @@ def test_gravity_term(): ) G_test = comp.gravity_term(H_b, joints_val) assert G_iDyn_np - G_test == pytest.approx(0.0, abs=1e-4) + + +def test_fd(): + joint_torques = np.random.rand(n_dofs) + joints_vel = np.zeros(n_dofs) + joints_val = np.zeros(n_dofs) + base_vel = np.zeros(6) + + M = comp.mass_matrix(base_transform=H_b, joint_positions=joints_vel) + h = comp.bias_force( + base_transform=H_b, + joint_positions=joints_val, + base_velocity=base_vel, + joint_velocities=joints_vel, + ) + kinDyn.setRobotState(H_b, joints_val, base_vel, joints_vel, g) + + mass_idyn = idyntree.MatrixDynSize() + kinDyn.getFreeFloatingMassMatrix(mass_idyn) + M_iDyn = mass_idyn.toNumPy() + + h_iDyn = idyntree.FreeFloatingGeneralizedTorques(kinDyn.model()) + assert kinDyn.generalizedBiasForces(h_iDyn) + h_iDyn_np = np.concatenate( + (h_iDyn.baseWrench().toNumPy(), h_iDyn.jointTorques().toNumPy()) + ) + + reference_acc = np.linalg.inv(M_iDyn) @ ( + np.concatenate((np.zeros(6), joint_torques)) - h_iDyn_np + ) + base_acc, joint_acc = comp.forward_dynamics( + base_transform=H_b, + base_velocity=base_vel, + joint_positions=joints_val, + joint_velocities=joints_vel, + joint_torques=joint_torques, + ) + + assert base_acc - reference_acc[:6] == pytest.approx(0.0, abs=1e-4) + assert joint_acc - reference_acc[6:] == pytest.approx(0.0, abs=1e-4) + + +test_fd() diff --git a/tests/body_fixed/test_pytorch_body_fixed.py b/tests/body_fixed/test_pytorch_body_fixed.py index daa3a142..27aa569b 100644 --- a/tests/body_fixed/test_pytorch_body_fixed.py +++ b/tests/body_fixed/test_pytorch_body_fixed.py @@ -205,3 +205,17 @@ def test_gravity_term(): ) G_test = comp.gravity_term(H_b, joints_val) assert G_iDyn_np - np.asarray(G_test) == pytest.approx(0.0, abs=1e-4) + + +def test_fd(): + joint_torques = np.random.rand(n_dofs) + joints_vel = np.random.rand(n_dofs) + reference_acc = np.linalg.inv(comp.mass_matrix(H_b, joints_val)) @ ( + np.concatenate((np.zeros(6), joint_torques)) + - comp.bias_force(H_b, joints_val, base_vel, joints_vel) + ) + base_acc, joint_acc = comp.forward_dynamics( + H_b, base_vel, joints_vel, joints_vel, joint_torques + ) + assert base_acc - reference_acc[:6] == pytest.approx(0.0, abs=1e-4) + assert joint_acc - reference_acc[6:] == pytest.approx(0.0, abs=1e-4) diff --git a/tests/mixed/test_CasADi_mixed.py b/tests/mixed/test_CasADi_mixed.py index 60c1c102..3779f995 100644 --- a/tests/mixed/test_CasADi_mixed.py +++ b/tests/mixed/test_CasADi_mixed.py @@ -239,3 +239,17 @@ def test_relative_jacobian(): J_test2 = cs.DM(comp.relative_jacobian("l_sole", joints_val)) assert iDynNumpyRelativeJ - J_test == pytest.approx(0.0, abs=1e-4) assert iDynNumpyRelativeJ - J_test2 == pytest.approx(0.0, abs=1e-4) + + +def test_fd(): + joint_torques = np.random.rand(n_dofs) + joints_vel = np.random.rand(n_dofs) + reference_acc = np.linalg.inv(comp.mass_matrix(H_b, joints_val)) @ ( + np.concatenate((np.zeros(6), joint_torques)) + - comp.bias_force(H_b, joints_val, base_vel, joints_vel) + ) + base_acc, joint_acc = comp.forward_dynamics( + H_b, base_vel, joints_vel, joints_vel, joint_torques + ) + assert base_acc - reference_acc[:6] == pytest.approx(0.0, abs=1e-4) + assert joint_acc - reference_acc[6:] == pytest.approx(0.0, abs=1e-4) diff --git a/tests/mixed/test_Jax_mixed.py b/tests/mixed/test_Jax_mixed.py index 636a3391..93eb750e 100644 --- a/tests/mixed/test_Jax_mixed.py +++ b/tests/mixed/test_Jax_mixed.py @@ -6,7 +6,6 @@ import icub_models import idyntree.swig as idyntree -import jax.numpy as jnp import numpy as np import pytest from jax import config @@ -192,3 +191,17 @@ def test_gravity_term(): ) G_test = comp.gravity_term(H_b, joints_val) assert G_iDyn_np - G_test == pytest.approx(0.0, abs=1e-4) + + +def test_fd(): + joint_torques = np.random.rand(n_dofs) + joints_vel = np.random.rand(n_dofs) + reference_acc = np.linalg.inv(comp.mass_matrix(H_b, joints_val)) @ ( + np.concatenate((np.zeros(6), joint_torques)) + - comp.bias_force(H_b, joints_val, base_vel, joints_vel) + ) + base_acc, joint_acc = comp.forward_dynamics( + H_b, base_vel, joints_vel, joints_vel, joint_torques + ) + assert base_acc - reference_acc[:6] == pytest.approx(0.0, abs=1e-4) + assert joint_acc - reference_acc[6:] == pytest.approx(0.0, abs=1e-4) diff --git a/tests/mixed/test_NumPy_mixed.py b/tests/mixed/test_NumPy_mixed.py index d4710441..2e5f6516 100644 --- a/tests/mixed/test_NumPy_mixed.py +++ b/tests/mixed/test_NumPy_mixed.py @@ -188,3 +188,17 @@ def test_gravity_term(): ) G_test = comp.gravity_term(H_b, joints_val) assert G_iDyn_np - G_test == pytest.approx(0.0, abs=1e-4) + + +def test_fd(): + joint_torques = np.random.rand(n_dofs) + joints_vel = np.random.rand(n_dofs) + reference_acc = np.linalg.inv(comp.mass_matrix(H_b, joints_val)) @ ( + np.concatenate((np.zeros(6), joint_torques)) + - comp.bias_force(H_b, joints_val, base_vel, joints_vel) + ) + base_acc, joint_acc = comp.forward_dynamics( + H_b, base_vel, joints_vel, joints_vel, joint_torques + ) + assert base_acc - reference_acc[:6] == pytest.approx(0.0, abs=1e-4) + assert joint_acc - reference_acc[6:] == pytest.approx(0.0, abs=1e-4) diff --git a/tests/mixed/test_pytorch_mixed.py b/tests/mixed/test_pytorch_mixed.py index 32a72702..1e475942 100644 --- a/tests/mixed/test_pytorch_mixed.py +++ b/tests/mixed/test_pytorch_mixed.py @@ -205,3 +205,17 @@ def test_gravity_term(): ) G_test = comp.gravity_term(H_b, joints_val) assert G_iDyn_np - np.asarray(G_test) == pytest.approx(0.0, abs=1e-4) + + +def test_fd(): + joint_torques = np.random.rand(n_dofs) + joints_vel = np.random.rand(n_dofs) + reference_acc = np.linalg.inv(comp.mass_matrix(H_b, joints_val)) @ ( + np.concatenate((np.zeros(6), joint_torques)) + - comp.bias_force(H_b, joints_val, base_vel, joints_vel) + ) + base_acc, joint_acc = comp.forward_dynamics( + H_b, base_vel, joints_vel, joints_vel, joint_torques + ) + assert base_acc - reference_acc[:6] == pytest.approx(0.0, abs=1e-4) + assert joint_acc - reference_acc[6:] == pytest.approx(0.0, abs=1e-4)