From 2c0dd5b0fe2d8ee21752c5bdb20badeddd20f942 Mon Sep 17 00:00:00 2001 From: elian-WSL22H Date: Sun, 19 May 2024 11:30:52 +0900 Subject: [PATCH 1/6] Helper function _find_standard_joint(joint) This will be used heavily later but can already replace some ugly code. Signed-off-by: elian-WSL22H --- roboticstoolbox/tools/urdf/urdf.py | 86 +++++++++++++++++++----------- 1 file changed, 54 insertions(+), 32 deletions(-) diff --git a/roboticstoolbox/tools/urdf/urdf.py b/roboticstoolbox/tools/urdf/urdf.py index 2069af158..924da6102 100644 --- a/roboticstoolbox/tools/urdf/urdf.py +++ b/roboticstoolbox/tools/urdf/urdf.py @@ -23,6 +23,56 @@ # Global variable for the base path of the robot meshes _base_path = None +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 class URDFType(object): """Abstract base class for all URDF types. @@ -1718,38 +1768,10 @@ def __init__( 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: + var = _find_standard_joint(joint) + + is_not_fixed = joint.joint_type != "fixed" + if var is not None and is_not_fixed: ets = ets * var if isinstance(ets, rtb.ET): From 307fad9b216c21832cb2e661c1a5252d83b6ef7b Mon Sep 17 00:00:00 2001 From: elian-WSL22H Date: Sun, 19 May 2024 11:42:53 +0900 Subject: [PATCH 2/6] Math function for rotation between two vectors Signed-off-by: elian-WSL22H --- roboticstoolbox/tools/urdf/urdf.py | 51 ++++++++++++++++++++++++++++-- 1 file changed, 49 insertions(+), 2 deletions(-) diff --git a/roboticstoolbox/tools/urdf/urdf.py b/roboticstoolbox/tools/urdf/urdf.py index 924da6102..5f86d1832 100644 --- a/roboticstoolbox/tools/urdf/urdf.py +++ b/roboticstoolbox/tools/urdf/urdf.py @@ -10,8 +10,8 @@ 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, unitvec_norm, angvec2r, tr2rpy from io import BytesIO from roboticstoolbox.tools.data import rtb_path_to_datafile @@ -23,6 +23,53 @@ # Global variable for the base path of the robot meshes _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. From 671fbbfd9d66ad4d72acfb4c577e37c14a350f01 Mon Sep 17 00:00:00 2001 From: elian-WSL22H Date: Sun, 19 May 2024 13:30:37 +0900 Subject: [PATCH 3/6] Correct joint and link definition Sorry this is a very big commit, I couldn't think of how to simplify it. Signed-off-by: elian-WSL22H --- roboticstoolbox/tools/urdf/urdf.py | 163 ++++++++++++++++++++++++----- 1 file changed, 135 insertions(+), 28 deletions(-) diff --git a/roboticstoolbox/tools/urdf/urdf.py b/roboticstoolbox/tools/urdf/urdf.py index 5f86d1832..2ebb0f66e 100644 --- a/roboticstoolbox/tools/urdf/urdf.py +++ b/roboticstoolbox/tools/urdf/urdf.py @@ -4,6 +4,7 @@ @author (Adapted by) Jesse Haviland """ +from typing import Optional, Tuple import numpy as np import roboticstoolbox as rtb import spatialgeometry as gm @@ -121,6 +122,78 @@ def _find_standard_joint(joint: "Joint") -> "rtb.ET | None": 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 @@ -1797,34 +1870,10 @@ def __init__( # constant part of link transform trans = SE3(joint.origin).t rot = joint.rpy - - # 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] - - var = _find_standard_joint(joint) - - is_not_fixed = joint.joint_type != "fixed" - if var is not None and is_not_fixed: - ets = ets * var - - if isinstance(ets, rtb.ET): - ets = rtb.ETS(ets) - - childlink.ets = ets + + # childlink.ets will be set later. + # This is because the fully defined parent link is required + # and this loop does not follow the parent-child order. # joint limit try: @@ -1855,6 +1904,64 @@ def __init__( # TODO, why did you put the base_link on the end? # easy to do it here + # the childlink.ets will be set in there + self._recursive_axis_definition() + return + + 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: + base_link_exists = "base_link" in [j.name for j in self.elinks] + if base_link_exists: + parentname = "base_link" + else: + parentname = self.elinks[0].name + if parent_from_Rx_to_axis is None: + parent_from_Rx_to_axis = SE3() + + 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) + + for child in self.elinks: # search all link with identical child + is_child = joint.child == child.name + if not is_child: + continue # skips to next link + + child.ets = ets # sets the ets of the joint + self._recursive_axis_definition( + parentname=child.name, parent_from_Rx_to_axis=from_Rx_to_axis + ) + @property def name(self): """str : The name of the URDF.""" From 6eebbd3f751db1df1c4ec0f8b9c0190c49976cbf Mon Sep 17 00:00:00 2001 From: elian-WSL22H Date: Sun, 19 May 2024 13:40:33 +0900 Subject: [PATCH 4/6] Black formatting Signed-off-by: elian-WSL22H --- roboticstoolbox/tools/urdf/urdf.py | 31 +++++++++++++++++++++--------- 1 file changed, 22 insertions(+), 9 deletions(-) diff --git a/roboticstoolbox/tools/urdf/urdf.py b/roboticstoolbox/tools/urdf/urdf.py index 2ebb0f66e..17e0f2334 100644 --- a/roboticstoolbox/tools/urdf/urdf.py +++ b/roboticstoolbox/tools/urdf/urdf.py @@ -12,7 +12,14 @@ import os import xml.etree.ElementTree as ETT from spatialmath import SE3, SO3 -from spatialmath.base import ArrayLike3, getvector, unitvec, unitvec_norm, angvec2r, tr2rpy +from spatialmath.base import ( + ArrayLike3, + getvector, + unitvec, + unitvec_norm, + angvec2r, + tr2rpy, +) from io import BytesIO from roboticstoolbox.tools.data import rtb_path_to_datafile @@ -24,6 +31,7 @@ # Global variable for the base path of the robot meshes _base_path = None + def rotation_fromVec_toVec( from_this_vector: ArrayLike3, to_this_vector: ArrayLike3 ) -> SO3: @@ -71,6 +79,7 @@ def rotation_fromVec_toVec( 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. @@ -122,6 +131,7 @@ def _find_standard_joint(joint: "Joint") -> "rtb.ET | None": std_joint = rtb.ET.SE3(SE3()) return std_joint + def _find_joint_ets( joint: "Joint", parent_from_Rx_to_axis: Optional[SE3] = None, @@ -136,7 +146,7 @@ def _find_joint_ets( Attributes ---------- joint: Joint - Joint from the urdf. + 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 @@ -157,7 +167,7 @@ def _find_joint_ets( joint_without_axis = SE3(joint_trans) * SE3.RPY(joint_rot) std_joint = _find_standard_joint(joint) - is_simple_joint = std_joint is not None + is_simple_joint = std_joint is not None if is_simple_joint: from_Rx_to_axis = SE3() @@ -173,7 +183,7 @@ def _find_joint_ets( if joint.joint_type in ("revolute", "continuous"): pure_joint = rtb.ET.Rx(flip=0) - elif joint.joint_type == "prismatic": # I cannot test this case + elif joint.joint_type == "prismatic": # I cannot test this case pure_joint = rtb.ET.tx(flip=0) else: pure_joint = rtb.ET.SE3(SE3()) @@ -194,6 +204,7 @@ def _find_joint_ets( 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 @@ -1836,9 +1847,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) @@ -1870,8 +1883,8 @@ def __init__( # constant part of link transform trans = SE3(joint.origin).t rot = joint.rpy - - # childlink.ets will be set later. + + # childlink.ets will be set later. # This is because the fully defined parent link is required # and this loop does not follow the parent-child order. From 187589acdb11216e0963487d77beb0e537b09c83 Mon Sep 17 00:00:00 2001 From: elian-WSL22H Date: Sun, 19 May 2024 15:04:48 +0900 Subject: [PATCH 5/6] Fixing start link Signed-off-by: elian-WSL22H --- roboticstoolbox/tools/urdf/urdf.py | 14 ++++++-------- 1 file changed, 6 insertions(+), 8 deletions(-) diff --git a/roboticstoolbox/tools/urdf/urdf.py b/roboticstoolbox/tools/urdf/urdf.py index 17e0f2334..b5cf1a626 100644 --- a/roboticstoolbox/tools/urdf/urdf.py +++ b/roboticstoolbox/tools/urdf/urdf.py @@ -16,9 +16,6 @@ ArrayLike3, getvector, unitvec, - unitvec_norm, - angvec2r, - tr2rpy, ) from io import BytesIO @@ -1950,11 +1947,12 @@ def _recursive_axis_definition( SE3 resulting from the axis orientation of the parent joint """ if parentname is None: - base_link_exists = "base_link" in [j.name for j in self.elinks] - if base_link_exists: - parentname = "base_link" - else: - parentname = self.elinks[0].name + # 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() From 8094b1d5ac30420d94356624d384cb5f11009a2b Mon Sep 17 00:00:00 2001 From: elian-WSL22H Date: Sun, 19 May 2024 15:05:36 +0900 Subject: [PATCH 6/6] Fix link attribute definition order The link ETS needs to be set before some operations. This is now done in the right order and tests passes. Signed-off-by: elian-WSL22H --- roboticstoolbox/tools/urdf/urdf.py | 91 ++++++++++++++++-------------- 1 file changed, 49 insertions(+), 42 deletions(-) diff --git a/roboticstoolbox/tools/urdf/urdf.py b/roboticstoolbox/tools/urdf/urdf.py index b5cf1a626..af3e7ca1b 100644 --- a/roboticstoolbox/tools/urdf/urdf.py +++ b/roboticstoolbox/tools/urdf/urdf.py @@ -1871,50 +1871,18 @@ 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 - # childlink.ets will be set later. - # This is because the fully defined parent link is required - # and this loop does not follow the parent-child order. - - # joint limit - 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 - - self.elinks = elinks - - # TODO, why did you put the base_link on the end? - # easy to do it here - - # the childlink.ets will be set in there + # the childlink.ets and other info is set recursively here self._recursive_axis_definition() return @@ -1963,16 +1931,55 @@ def _recursive_axis_definition( ets, from_Rx_to_axis = _find_joint_ets(joint, parent_from_Rx_to_axis) - for child in self.elinks: # search all link with identical child - is_child = joint.child == child.name + 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 - child.ets = ets # sets the ets of the joint + childlink.ets = ets # sets the ets of the joint + self.finalize_linking(childlink, joint) + self._recursive_axis_definition( - parentname=child.name, parent_from_Rx_to_axis=from_Rx_to_axis + 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): """str : The name of the URDF."""