diff --git a/roboticstoolbox/tools/urdf/urdf.py b/roboticstoolbox/tools/urdf/urdf.py index 2069af158..af3e7ca1b 100644 --- a/roboticstoolbox/tools/urdf/urdf.py +++ b/roboticstoolbox/tools/urdf/urdf.py @@ -4,14 +4,19 @@ @author (Adapted by) Jesse Haviland """ +from typing import Optional, Tuple import numpy as np import roboticstoolbox as rtb import spatialgeometry as gm import copy import os import xml.etree.ElementTree as ETT -from spatialmath import SE3 -from spatialmath.base import unitvec_norm, angvec2r, tr2rpy +from spatialmath import SE3, SO3 +from spatialmath.base import ( + ArrayLike3, + getvector, + unitvec, +) from io import BytesIO from roboticstoolbox.tools.data import rtb_path_to_datafile @@ -24,6 +29,179 @@ _base_path = None +def rotation_fromVec_toVec( + from_this_vector: ArrayLike3, to_this_vector: ArrayLike3 +) -> SO3: + """ + Computes the rotation matrix from the first to the second vector. + + Attributes + ---------- + from_this_vector: ArrayLike3 + to_this_vector: ArrayLike3 + + Returns + ------- + rotation_from_to: SO3 + Rotation matrix + + Notes + ----- + Vector length is irrelevant. + """ + from_this_vector = getvector(from_this_vector) + to_this_vector = getvector(to_this_vector) + + is_zero = np.all(np.isclose(from_this_vector, 0)) + if is_zero: + target_axis = to_this_vector + else: + target_axis = unitvec(from_this_vector) + + dt = np.dot(target_axis, to_this_vector) + crss = np.cross(target_axis, to_this_vector) + + is_parallel = np.all(np.isclose(crss, 0)) + if is_parallel: + rotation_plane = unitvec( + np.cross(target_axis, to_this_vector + np.array([1, 1, 1])) + ) + else: + rotation_plane = unitvec(crss) + + x = dt + y = np.linalg.norm(crss) + rotation_angle = np.arctan2(y, x) + + rotation_from_to = SO3.AngVec(rotation_angle, rotation_plane) + return rotation_from_to + + +def _find_standard_joint(joint: "Joint") -> "rtb.ET | None": + """ + Finds a pure rtb.ET joint corresponding to the URDF joint axis. + + If urdf joint is fixed, returns an empty rtb.ET.SE3. + If none exists (axis is not +- 1 over x, y or z) returns None. + + Attributes + ---------- + joint + joint object read from the urdf. + + Returns + ------- + std_joint: rtb.ET or None + if a rtb.ET joint exists: + Returns rtb.ET of type joint. This is rtb.ET.[SE(), Rx(), Ry(), ..., tz]. + else: + Returns None. + """ + std_joint = None + if joint.joint_type in ("revolute", "continuous"): # pragma nocover # noqa + if joint.axis[0] == 1: + std_joint = rtb.ET.Rx() + elif joint.axis[0] == -1: + std_joint = rtb.ET.Rx(flip=True) + elif joint.axis[1] == 1: + std_joint = rtb.ET.Ry() + elif joint.axis[1] == -1: + std_joint = rtb.ET.Ry(flip=True) + elif joint.axis[2] == 1: + std_joint = rtb.ET.Rz() + elif joint.axis[2] == -1: + std_joint = rtb.ET.Rz(flip=True) + elif joint.joint_type == "prismatic": # pragma nocover + if joint.axis[0] == 1: + std_joint = rtb.ET.tx() + elif joint.axis[0] == -1: + std_joint = rtb.ET.tx(flip=True) + elif joint.axis[1] == 1: + std_joint = rtb.ET.ty() + elif joint.axis[1] == -1: + std_joint = rtb.ET.ty(flip=True) + elif joint.axis[2] == 1: + std_joint = rtb.ET.tz() + elif joint.axis[2] == -1: + std_joint = rtb.ET.tz(flip=True) + elif joint.joint_type == "fixed": + std_joint = rtb.ET.SE3(SE3()) + return std_joint + + +def _find_joint_ets( + joint: "Joint", + parent_from_Rx_to_axis: Optional[SE3] = None, +) -> Tuple["rtb.ETS", SE3]: + """ + Finds the ETS of a urdf joint object to be used by a Link. + + This is based on the following fomula: + ets(N) = axis(N-1).inv() * transl(N) * rot(N) * axis(N) * [Rx] + where N is the current joint, and (N-1) the parent joint. + + Attributes + ---------- + joint: Joint + Joint from the urdf. + Used to deduce: transl(N), rot(N), axis(N), [Rx] + parent_from_Rx_to_axis: Optional[SE3] + SE3 resulting from the axis orientation of the parent joint + Used to deduce: axis(N-1) + + Returns + ------- + ets: ETS + ETS representing the joint. It ends with a joint. + from_Rx_to_axis: SE3 + SE3 representing the rotation of the axis attribute of the joint. + """ + joint_trans = SE3(joint.origin).t + joint_rot = joint.rpy + if parent_from_Rx_to_axis is None: + parent_from_Rx_to_axis = SE3() + + joint_without_axis = SE3(joint_trans) * SE3.RPY(joint_rot) + + std_joint = _find_standard_joint(joint) + is_simple_joint = std_joint is not None + + if is_simple_joint: + from_Rx_to_axis = SE3() + pure_joint = std_joint + else: # rotates a Rx joint onto right axis + joint_axis = joint.axis + axis_of_Rx = np.array([1, 0, 0], dtype=float) + + rotation_from_Rx_to_axis = rotation_fromVec_toVec( + from_this_vector=axis_of_Rx, to_this_vector=joint_axis + ) + from_Rx_to_axis = SE3(rotation_from_Rx_to_axis) + + if joint.joint_type in ("revolute", "continuous"): + pure_joint = rtb.ET.Rx(flip=0) + elif joint.joint_type == "prismatic": # I cannot test this case + pure_joint = rtb.ET.tx(flip=0) + else: + pure_joint = rtb.ET.SE3(SE3()) + + listET = [] + emptySE3 = SE3() + + # skips empty SE3 + if not parent_from_Rx_to_axis == emptySE3: + listET.append(rtb.ET.SE3(parent_from_Rx_to_axis.inv())) + listET.append(rtb.ET.SE3(joint_without_axis)) + if not from_Rx_to_axis == emptySE3: + listET.append(rtb.ET.SE3(from_Rx_to_axis)) + if not joint.joint_type == "fixed": + listET.append(pure_joint) + + ets = rtb.ETS(listET) + + return ets, from_Rx_to_axis + + class URDFType(object): """Abstract base class for all URDF types. This has useful class methods for automatic parsing/unparsing @@ -1666,9 +1844,11 @@ def __init__( elink = rtb.Link( name=link.name, m=link.inertial.mass, - r=link.inertial.origin[:3, 3] - if link.inertial.origin is not None - else None, + r=( + link.inertial.origin[:3, 3] + if link.inertial.origin is not None + else None + ), I=link.inertial.inertia, ) elinks.append(elink) @@ -1691,100 +1871,114 @@ def __init__( # connect the links using joint info for joint in self._joints: # get references to joint's parent and child - childlink = elinkdict[joint.child] + childlink: "rtb.Link" = elinkdict[joint.child] parentlink = elinkdict[joint.parent] childlink._parent = parentlink # connect child link to parent childlink._joint_name = joint.name + # Link precise definition will be done recursively later + self.elinks = elinks - # constant part of link transform - trans = SE3(joint.origin).t - rot = joint.rpy + # TODO, why did you put the base_link on the end? + # easy to do it here - # Check if axis of rotation/tanslation is not 1 - if np.count_nonzero(joint.axis) < 2: - ets = rtb.ET.SE3(SE3(trans) * SE3.RPY(rot)) - else: - # Normalise the joint axis to be along or about z axis - # Convert rest to static ETS - v = joint.axis - u, n = unitvec_norm(v) - R = angvec2r(n, u) - - R_total = SE3.RPY(joint.rpy) * R - rpy = tr2rpy(R_total) - - ets = rtb.ET.SE3(SE3(trans) * SE3.RPY(rpy)) - - joint.axis = [0, 0, 1] - - # variable part of link transform - var = None - if joint.joint_type in ("revolute", "continuous"): # pragma nocover # noqa - if joint.axis[0] == 1: - var = rtb.ET.Rx() - elif joint.axis[0] == -1: - var = rtb.ET.Rx(flip=True) - elif joint.axis[1] == 1: - var = rtb.ET.Ry() - elif joint.axis[1] == -1: - var = rtb.ET.Ry(flip=True) - elif joint.axis[2] == 1: - var = rtb.ET.Rz() - elif joint.axis[2] == -1: - var = rtb.ET.Rz(flip=True) - elif joint.joint_type == "prismatic": # pragma nocover - if joint.axis[0] == 1: - var = rtb.ET.tx() - elif joint.axis[0] == -1: - var = rtb.ET.tx(flip=True) - elif joint.axis[1] == 1: - var = rtb.ET.ty() - elif joint.axis[1] == -1: - var = rtb.ET.ty(flip=True) - elif joint.axis[2] == 1: - var = rtb.ET.tz() - elif joint.axis[2] == -1: - var = rtb.ET.tz(flip=True) - elif joint.joint_type == "fixed": - var = None - - if var is not None: - ets = ets * var - - if isinstance(ets, rtb.ET): - ets = rtb.ETS(ets) - - childlink.ets = ets - - # joint limit - try: - if childlink.isjoint: - childlink.qlim = [joint.limit.lower, joint.limit.upper] - except AttributeError: - # no joint limits provided - pass + # the childlink.ets and other info is set recursively here + self._recursive_axis_definition() + return - # joint friction - try: - if joint.dynamics.friction is not None: - childlink.B = joint.dynamics.friction + def _recursive_axis_definition( + self, + parentname: Optional[str] = None, + parent_from_Rx_to_axis: Optional[SE3] = None, + ) -> None: + """ + Recursively sets the ets of all elinks (in place). + + The ets of a link depends on the previous joint axis orientation. + In a URDF a joint is defined as the following ets: + ets = translation * rotation * axis * [Rx] * axis.inv() + where Rx is the variable joint ets, and "axis" rotates the variable joint + axis, BUT NOT the next link. Hence why Rx is rotated onto the axis, then + the rotation is canceled by axis.inv(). + + A Link is requiered to end with a variable ets -- this is our [Rx]. + The previous formula must therefore be changed and requires recursion: + ets(N) = axis(N-1).inv() * transl(N) * rot(N) * axis(N) * [Rx] + where N is the current joint, and (N-1) the parent joint. + Chaining the ets of the second formula is equivalent to the first formula. + + Attributes + ---------- + parentname: Optional[str] + Name of the parent link. + parent_from_Rx_to_axis: Optional[SE3] + SE3 resulting from the axis orientation of the parent joint + """ + if parentname is None: + # starts again with all orphan links + for link in self.elinks: + if link.parent is None: + self._recursive_axis_definition( + parentname=link.name, parent_from_Rx_to_axis=None + ) + if parent_from_Rx_to_axis is None: + parent_from_Rx_to_axis = SE3() - # TODO Add damping - # joint.dynamics.damping - except AttributeError: - pass + for joint in self._joints: # search all joint with identical parent + is_parent = joint.parent == parentname + if not is_parent: + continue # skips to next joint + + ets, from_Rx_to_axis = _find_joint_ets(joint, parent_from_Rx_to_axis) - # joint gear ratio - # TODO, not sure if t.joint.name is a thing - for t in self.transmissions: # pragma nocover - if t.name == joint.name: - childlink.G = t.actuators[0].mechanicalReduction + for childlink in self.elinks: # search all link with identical child + is_child = joint.child == childlink.name + if not is_child: + continue # skips to next link - self.elinks = elinks + childlink.ets = ets # sets the ets of the joint + self.finalize_linking(childlink, joint) - # TODO, why did you put the base_link on the end? - # easy to do it here + self._recursive_axis_definition( + parentname=childlink.name, parent_from_Rx_to_axis=from_Rx_to_axis + ) + + def finalize_linking(self, childlink: "rtb.Link", joint: "Joint"): + """ + Finalize the linking process after the link ets is set. + + This directly changes childlink in place. + The ets of childlink must be defined prior to this. + + Attributes + ---------- + childlink: rtb.Link + Link to finalize the definition of. + joint: Joint + Joint used to define the link. + """ + try: + if childlink.isjoint: + childlink.qlim = [joint.limit.lower, joint.limit.upper] + except AttributeError: + # no joint limits provided + pass + + # joint friction + try: + if joint.dynamics.friction is not None: + childlink.B = joint.dynamics.friction + + # TODO Add damping + # joint.dynamics.damping + except AttributeError: + pass + + # joint gear ratio + # TODO, not sure if t.joint.name is a thing + for t in self.transmissions: # pragma nocover + if t.name == joint.name: + childlink.G = t.actuators[0].mechanicalReduction @property def name(self):