Skip to content

Navigation Menu

Sign in
Appearance settings

Search code, repositories, users, issues, pull requests...

Provide feedback

We read every piece of feedback, and take your input very seriously.

Saved searches

Use saved searches to filter your results more quickly
Appearance settings

Commit b3d1509

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

File tree

Expand file treeCollapse file tree

2 files changed

+114
-23
lines changed
Filter options
Expand file treeCollapse file tree

2 files changed

+114
-23
lines changed

‎spatialmath/pose3d.py

Copy file name to clipboardExpand all lines: spatialmath/pose3d.py
+6-2Lines 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

Copy file name to clipboardExpand all lines: tests/test_pose3d.py
+108-21Lines 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 (0)
Morty Proxy This is a proxified and sanitized view of the page, visit original site.