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.""" pFad - Phonifier reborn

Pfad - The Proxy pFad of © 2024 Garber Painting. All rights reserved.

Note: This service is not intended for secure transactions such as banking, social media, email, or purchasing. Use at your own risk. We assume no liability whatsoever for broken pages.


Alternative Proxies:

Alternative Proxy

pFad Proxy

pFad v3 Proxy

pFad v4 Proxy