12
12
import os
13
13
import xml .etree .ElementTree as ETT
14
14
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
+ )
16
23
17
24
from io import BytesIO
18
25
from roboticstoolbox .tools .data import rtb_path_to_datafile
24
31
# Global variable for the base path of the robot meshes
25
32
_base_path = None
26
33
34
+
27
35
def rotation_fromVec_toVec (
28
36
from_this_vector : ArrayLike3 , to_this_vector : ArrayLike3
29
37
) -> SO3 :
@@ -71,6 +79,7 @@ def rotation_fromVec_toVec(
71
79
rotation_from_to = SO3 .AngVec (rotation_angle , rotation_plane )
72
80
return rotation_from_to
73
81
82
+
74
83
def _find_standard_joint (joint : "Joint" ) -> "rtb.ET | None" :
75
84
"""
76
85
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":
122
131
std_joint = rtb .ET .SE3 (SE3 ())
123
132
return std_joint
124
133
134
+
125
135
def _find_joint_ets (
126
136
joint : "Joint" ,
127
137
parent_from_Rx_to_axis : Optional [SE3 ] = None ,
@@ -136,7 +146,7 @@ def _find_joint_ets(
136
146
Attributes
137
147
----------
138
148
joint: Joint
139
- Joint from the urdf.
149
+ Joint from the urdf.
140
150
Used to deduce: transl(N), rot(N), axis(N), [Rx]
141
151
parent_from_Rx_to_axis: Optional[SE3]
142
152
SE3 resulting from the axis orientation of the parent joint
@@ -157,7 +167,7 @@ def _find_joint_ets(
157
167
joint_without_axis = SE3 (joint_trans ) * SE3 .RPY (joint_rot )
158
168
159
169
std_joint = _find_standard_joint (joint )
160
- is_simple_joint = std_joint is not None
170
+ is_simple_joint = std_joint is not None
161
171
162
172
if is_simple_joint :
163
173
from_Rx_to_axis = SE3 ()
@@ -173,7 +183,7 @@ def _find_joint_ets(
173
183
174
184
if joint .joint_type in ("revolute" , "continuous" ):
175
185
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
177
187
pure_joint = rtb .ET .tx (flip = 0 )
178
188
else :
179
189
pure_joint = rtb .ET .SE3 (SE3 ())
@@ -194,6 +204,7 @@ def _find_joint_ets(
194
204
195
205
return ets , from_Rx_to_axis
196
206
207
+
197
208
class URDFType (object ):
198
209
"""Abstract base class for all URDF types.
199
210
This has useful class methods for automatic parsing/unparsing
@@ -1836,9 +1847,11 @@ def __init__(
1836
1847
elink = rtb .Link (
1837
1848
name = link .name ,
1838
1849
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
+ ),
1842
1855
I = link .inertial .inertia ,
1843
1856
)
1844
1857
elinks .append (elink )
@@ -1870,8 +1883,8 @@ def __init__(
1870
1883
# constant part of link transform
1871
1884
trans = SE3 (joint .origin ).t
1872
1885
rot = joint .rpy
1873
-
1874
- # childlink.ets will be set later.
1886
+
1887
+ # childlink.ets will be set later.
1875
1888
# This is because the fully defined parent link is required
1876
1889
# and this loop does not follow the parent-child order.
1877
1890
0 commit comments