Skip to content

Commit b3d1509

Browse files
petercorkepetercorke
committed
fix bug in passing matrix to RPY
add tests for this case for RPY and Eul
1 parent e02e1ed commit b3d1509

2 files changed

Lines changed: 114 additions & 23 deletions

File tree

‎spatialmath/pose3d.py‎

Lines changed: 6 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -497,9 +497,13 @@ def RPY(cls, *angles, unit='rad', order='zyx', ):
497497
if len(angles) == 1:
498498
angles = angles[0]
499499

500-
angles = base.getmatrix(angles, (None, 3))
501-
return cls(base.rpy2r(angles, order=order, unit=unit), check=False)
500+
# angles = base.getmatrix(angles, (None, 3))
501+
# return cls(base.rpy2r(angles, order=order, unit=unit), check=False)
502502

503+
if base.isvector(angles, 3):
504+
return cls(base.rpy2r(angles, unit=unit, order=order), check=False)
505+
else:
506+
return cls([base.rpy2r(a, unit=unit, order=order) for a in angles], check=False)
503507

504508
@classmethod
505509
def OA(cls, o, a):

‎tests/test_pose3d.py‎

Lines changed: 108 additions & 21 deletions
Original file line numberDiff line numberDiff line change
@@ -66,12 +66,35 @@ def test_constructor(self):
6666
array_compare(R, rotz(0.2))
6767
self.assertIsInstance(R, SO3)
6868

69-
# triple angle
69+
# OA
70+
R = SO3.OA([0, 1, 0], [0, 0, 1])
71+
nt.assert_equal(len(R), 1)
72+
array_compare(R, np.eye(3))
73+
self.assertIsInstance(R, SO3)
74+
75+
# random
76+
R = SO3.Rand()
77+
nt.assert_equal(len(R), 1)
78+
self.assertIsInstance(R, SO3)
79+
80+
# copy constructor
81+
R = SO3.Rx(pi / 2)
82+
R2 = SO3(R)
83+
R = SO3.Ry(pi / 2)
84+
array_compare(R2, rotx(pi / 2))
85+
86+
def test_constructor_Eul(self):
87+
7088
R = SO3.Eul([0.1, 0.2, 0.3])
7189
nt.assert_equal(len(R), 1)
7290
array_compare(R, eul2r([0.1, 0.2, 0.3]))
7391
self.assertIsInstance(R, SO3)
7492

93+
R = SO3.Eul(0.1, 0.2, 0.3)
94+
nt.assert_equal(len(R), 1)
95+
array_compare(R, eul2r([0.1, 0.2, 0.3]))
96+
self.assertIsInstance(R, SO3)
97+
7598
R = SO3.Eul(np.r_[0.1, 0.2, 0.3])
7699
nt.assert_equal(len(R), 1)
77100
array_compare(R, eul2r([0.1, 0.2, 0.3]))
@@ -82,26 +105,105 @@ def test_constructor(self):
82105
array_compare(R, eul2r([10, 20, 30], unit='deg'))
83106
self.assertIsInstance(R, SO3)
84107

108+
R = SO3.Eul(10, 20, 30, unit='deg')
109+
nt.assert_equal(len(R), 1)
110+
array_compare(R, eul2r([10, 20, 30], unit='deg'))
111+
self.assertIsInstance(R, SO3)
112+
113+
# matrix input
114+
115+
angles = np.array([
116+
[0.1, 0.2, 0.3],
117+
[0.2, 0.3, 0.4],
118+
[0.3, 0.4, 0.5],
119+
[0.4, 0.5, 0.6]
120+
])
121+
R = SO3.Eul(angles)
122+
self.assertIsInstance(R, SO3)
123+
nt.assert_equal(len(R), 4)
124+
for i in range(4):
125+
array_compare(R[i], eul2r(angles[i,:]))
126+
127+
angles *= 10
128+
R = SO3.Eul(angles, unit='deg')
129+
self.assertIsInstance(R, SO3)
130+
nt.assert_equal(len(R), 4)
131+
for i in range(4):
132+
array_compare(R[i], eul2r(angles[i,:], unit='deg'))
133+
134+
135+
def test_constructor_RPY(self):
136+
137+
R = SO3.RPY(0.1, 0.2, 0.3, order='zyx')
138+
nt.assert_equal(len(R), 1)
139+
array_compare(R, rpy2r([0.1, 0.2, 0.3], order='zyx'))
140+
self.assertIsInstance(R, SO3)
141+
142+
R = SO3.RPY(10, 20, 30, unit='deg', order='zyx')
143+
nt.assert_equal(len(R), 1)
144+
array_compare(R, rpy2r([10, 20, 30], order='zyx', unit='deg'))
145+
self.assertIsInstance(R, SO3)
146+
147+
R = SO3.RPY([0.1, 0.2, 0.3], order='zyx')
148+
nt.assert_equal(len(R), 1)
149+
array_compare(R, rpy2r([0.1, 0.2, 0.3], order='zyx'))
150+
self.assertIsInstance(R, SO3)
151+
152+
R = SO3.RPY(np.r_[0.1, 0.2, 0.3], order='zyx')
153+
nt.assert_equal(len(R), 1)
154+
array_compare(R, rpy2r([0.1, 0.2, 0.3], order='zyx'))
155+
self.assertIsInstance(R, SO3)
156+
157+
# check default
85158
R = SO3.RPY([0.1, 0.2, 0.3])
86159
nt.assert_equal(len(R), 1)
87-
array_compare(R, rpy2r([0.1, 0.2, 0.3]))
160+
array_compare(R, rpy2r([0.1, 0.2, 0.3], order='zyx'))
88161
self.assertIsInstance(R, SO3)
89162

90-
R = SO3.RPY(np.r_[0.1, 0.2, 0.3])
163+
# XYZ order
164+
165+
R = SO3.RPY(0.1, 0.2, 0.3, order='xyz')
91166
nt.assert_equal(len(R), 1)
92-
array_compare(R, rpy2r([0.1, 0.2, 0.3]))
167+
array_compare(R, rpy2r([0.1, 0.2, 0.3], order='xyz'))
93168
self.assertIsInstance(R, SO3)
94169

95-
R = SO3.RPY([10, 20, 30], unit='deg')
170+
R = SO3.RPY(10, 20, 30, unit='deg', order='xyz')
96171
nt.assert_equal(len(R), 1)
97-
array_compare(R, rpy2r([10, 20, 30], unit='deg'))
172+
array_compare(R, rpy2r([10, 20, 30], order='xyz', unit='deg'))
98173
self.assertIsInstance(R, SO3)
99174

100175
R = SO3.RPY([0.1, 0.2, 0.3], order='xyz')
101176
nt.assert_equal(len(R), 1)
102177
array_compare(R, rpy2r([0.1, 0.2, 0.3], order='xyz'))
103178
self.assertIsInstance(R, SO3)
104179

180+
R = SO3.RPY(np.r_[0.1, 0.2, 0.3], order='xyz')
181+
nt.assert_equal(len(R), 1)
182+
array_compare(R, rpy2r([0.1, 0.2, 0.3], order='xyz'))
183+
self.assertIsInstance(R, SO3)
184+
185+
# matrix input
186+
187+
angles = np.array([
188+
[0.1, 0.2, 0.3],
189+
[0.2, 0.3, 0.4],
190+
[0.3, 0.4, 0.5],
191+
[0.4, 0.5, 0.6]
192+
])
193+
R = SO3.RPY(angles, order='zyx')
194+
self.assertIsInstance(R, SO3)
195+
nt.assert_equal(len(R), 4)
196+
for i in range(4):
197+
array_compare(R[i], rpy2r(angles[i,:], order='zyx'))
198+
199+
angles *= 10
200+
R = SO3.RPY(angles, unit='deg', order='zyx')
201+
self.assertIsInstance(R, SO3)
202+
nt.assert_equal(len(R), 4)
203+
for i in range(4):
204+
array_compare(R[i], rpy2r(angles[i,:], unit='deg', order='zyx'))
205+
206+
def test_constructor_AngVec(self):
105207
# angvec
106208
R = SO3.AngVec(0.2, [1, 0, 0])
107209
nt.assert_equal(len(R), 1)
@@ -113,22 +215,7 @@ def test_constructor(self):
113215
array_compare(R, roty(0.3))
114216
self.assertIsInstance(R, SO3)
115217

116-
# OA
117-
R = SO3.OA([0, 1, 0], [0, 0, 1])
118-
nt.assert_equal(len(R), 1)
119-
array_compare(R, np.eye(3))
120-
self.assertIsInstance(R, SO3)
121-
122-
# random
123-
R = SO3.Rand()
124-
nt.assert_equal(len(R), 1)
125-
self.assertIsInstance(R, SO3)
126218

127-
# copy constructor
128-
R = SO3.Rx(pi / 2)
129-
R2 = SO3(R)
130-
R = SO3.Ry(pi / 2)
131-
array_compare(R2, rotx(pi / 2))
132219

133220
def test_shape(self):
134221
a = SO3()

0 commit comments

Comments
 (0)