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):
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