Skip to content

Commit 6eebbd3

Browse files
committed
Black formatting
Signed-off-by: elian-WSL22H <elian.neppel@posteo.eu>
1 parent 671fbbf commit 6eebbd3

File tree

1 file changed

+22
-9
lines changed

1 file changed

+22
-9
lines changed

roboticstoolbox/tools/urdf/urdf.py

Lines changed: 22 additions & 9 deletions
Original file line numberDiff line numberDiff line change
@@ -12,7 +12,14 @@
1212
import os
1313
import xml.etree.ElementTree as ETT
1414
from spatialmath import SE3, SO3
15-
from spatialmath.base import ArrayLike3, getvector, unitvec, unitvec_norm, angvec2r, tr2rpy
15+
from spatialmath.base import (
16+
ArrayLike3,
17+
getvector,
18+
unitvec,
19+
unitvec_norm,
20+
angvec2r,
21+
tr2rpy,
22+
)
1623

1724
from io import BytesIO
1825
from roboticstoolbox.tools.data import rtb_path_to_datafile
@@ -24,6 +31,7 @@
2431
# Global variable for the base path of the robot meshes
2532
_base_path = None
2633

34+
2735
def rotation_fromVec_toVec(
2836
from_this_vector: ArrayLike3, to_this_vector: ArrayLike3
2937
) -> SO3:
@@ -71,6 +79,7 @@ def rotation_fromVec_toVec(
7179
rotation_from_to = SO3.AngVec(rotation_angle, rotation_plane)
7280
return rotation_from_to
7381

82+
7483
def _find_standard_joint(joint: "Joint") -> "rtb.ET | None":
7584
"""
7685
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":
122131
std_joint = rtb.ET.SE3(SE3())
123132
return std_joint
124133

134+
125135
def _find_joint_ets(
126136
joint: "Joint",
127137
parent_from_Rx_to_axis: Optional[SE3] = None,
@@ -136,7 +146,7 @@ def _find_joint_ets(
136146
Attributes
137147
----------
138148
joint: Joint
139-
Joint from the urdf.
149+
Joint from the urdf.
140150
Used to deduce: transl(N), rot(N), axis(N), [Rx]
141151
parent_from_Rx_to_axis: Optional[SE3]
142152
SE3 resulting from the axis orientation of the parent joint
@@ -157,7 +167,7 @@ def _find_joint_ets(
157167
joint_without_axis = SE3(joint_trans) * SE3.RPY(joint_rot)
158168

159169
std_joint = _find_standard_joint(joint)
160-
is_simple_joint = std_joint is not None
170+
is_simple_joint = std_joint is not None
161171

162172
if is_simple_joint:
163173
from_Rx_to_axis = SE3()
@@ -173,7 +183,7 @@ def _find_joint_ets(
173183

174184
if joint.joint_type in ("revolute", "continuous"):
175185
pure_joint = rtb.ET.Rx(flip=0)
176-
elif joint.joint_type == "prismatic": # I cannot test this case
186+
elif joint.joint_type == "prismatic": # I cannot test this case
177187
pure_joint = rtb.ET.tx(flip=0)
178188
else:
179189
pure_joint = rtb.ET.SE3(SE3())
@@ -194,6 +204,7 @@ def _find_joint_ets(
194204

195205
return ets, from_Rx_to_axis
196206

207+
197208
class URDFType(object):
198209
"""Abstract base class for all URDF types.
199210
This has useful class methods for automatic parsing/unparsing
@@ -1836,9 +1847,11 @@ def __init__(
18361847
elink = rtb.Link(
18371848
name=link.name,
18381849
m=link.inertial.mass,
1839-
r=link.inertial.origin[:3, 3]
1840-
if link.inertial.origin is not None
1841-
else None,
1850+
r=(
1851+
link.inertial.origin[:3, 3]
1852+
if link.inertial.origin is not None
1853+
else None
1854+
),
18421855
I=link.inertial.inertia,
18431856
)
18441857
elinks.append(elink)
@@ -1870,8 +1883,8 @@ def __init__(
18701883
# constant part of link transform
18711884
trans = SE3(joint.origin).t
18721885
rot = joint.rpy
1873-
1874-
# childlink.ets will be set later.
1886+
1887+
# childlink.ets will be set later.
18751888
# This is because the fully defined parent link is required
18761889
# and this loop does not follow the parent-child order.
18771890

0 commit comments

Comments
 (0)