Skip to content

Commit 275d801

Browse files
jhavljhavl
committed
tests passing
1 parent 53455dd commit 275d801

File tree

2 files changed

+38
-36
lines changed

2 files changed

+38
-36
lines changed

‎tests/test_ERobot.py

Lines changed: 18 additions & 14 deletions
Original file line numberDiff line numberDiff line change
@@ -112,9 +112,11 @@ def test_init_elink_autoparent(self):
112112
def test_init_elink_branched(self):
113113
robot = ERobot([
114114
ELink(ETS.rz(), name='link1'),
115-
ELink(ETS.tx(1) * ETS.ty(-0.5) * ETS.rz(), name='link2', parent='link1'),
115+
ELink(ETS.tx(1) * ETS.ty(-0.5) * ETS.rz(),
116+
name='link2', parent='link1'),
116117
ELink(ETS.tx(1), name='ee_1', parent='link2'),
117-
ELink(ETS.tx(1) * ETS.ty(0.5) * ETS.rz(), name='link3', parent='link1'),
118+
ELink(ETS.tx(1) * ETS.ty(0.5) * ETS.rz(),
119+
name='link3', parent='link1'),
118120
ELink(ETS.tx(1), name='ee_2', parent='link3')
119121
])
120122
self.assertEqual(robot.n, 3)
@@ -283,7 +285,7 @@ def test_fkine_traj(self):
283285
def test_fkine_all(self):
284286
a1 = 1
285287
a2 = 1
286-
e = ETS2.r() * ETS2.tx(a1) * ETS2.r() * ETS2.tx(a2);
288+
e = ETS2.r() * ETS2.tx(a1) * ETS2.r() * ETS2.tx(a2)
287289
robot = ERobot2(e)
288290

289291
T = robot.fkine_all([0, 0])
@@ -297,9 +299,11 @@ def test_fkine_all(self):
297299

298300
robot = ERobot2([
299301
ELink2(ETS2.r(), name='link1'),
300-
ELink2(ETS2.tx(1.2) * ETS2.ty(-0.5) * ETS2.r(), name='link2', parent='link1'),
302+
ELink2(ETS2.tx(1.2) * ETS2.ty(-0.5) *
303+
ETS2.r(), name='link2', parent='link1'),
301304
ELink2(ETS2.tx(1), name='ee_1', parent='link2'),
302-
ELink2(ETS2.tx(0.6) * ETS2.ty(0.5) * ETS2.r(), name='link3', parent='link1'),
305+
ELink2(ETS2.tx(0.6) * ETS2.ty(0.5) * ETS2.r(),
306+
name='link3', parent='link1'),
303307
ELink2(ETS2.tx(1), name='ee_2', parent='link3')
304308
])
305309
T = robot.fkine_all([0, 0, 0])
@@ -598,15 +602,15 @@ def test_init(self):
598602
def test_dict(self):
599603
panda = rtb.models.Panda()
600604
panda.grippers[0].links[0].collision.append(gm.Box([1, 1, 1]))
601-
panda.to_dict()
605+
panda._to_dict()
602606

603607
wx = rtb.models.wx250s()
604-
wx.to_dict()
608+
wx._to_dict()
605609

606610
def test_fkdict(self):
607611
panda = rtb.models.Panda()
608612
panda.grippers[0].links[0].collision.append(gm.Box([1, 1, 1]))
609-
panda.fk_dict()
613+
panda._fk_dict()
610614

611615
def test_elinks(self):
612616
panda = rtb.models.Panda()
@@ -779,21 +783,21 @@ def test_dist(self):
779783
s1 = gm.Box([1, 1, 1], base=sm.SE3(3, 0, 0))
780784
p = rtb.models.Panda()
781785

782-
d0, _, _ = p.closest_point(s0)
783-
d1, _, _ = p.closest_point(s1, 5)
784-
d2, _, _ = p.closest_point(s1)
786+
d0, _, _ = p.closest_point(p.q, s0)
787+
d1, _, _ = p.closest_point(p.q, s1, 5)
788+
d2, _, _ = p.closest_point(p.q, s1)
785789

786790
self.assertAlmostEqual(d0, -0.5599999999995913)
787-
self.assertAlmostEqual(d1, 2.4275999999999995)
791+
self.assertAlmostEqual(d1, 2.362147178773918)
788792
self.assertAlmostEqual(d2, None)
789793

790794
def test_collided(self):
791795
s0 = gm.Box([1, 1, 1], base=sm.SE3(0, 0, 0))
792796
s1 = gm.Box([1, 1, 1], base=sm.SE3(3, 0, 0))
793797
p = rtb.models.Panda()
794798

795-
c0 = p.collided(s0)
796-
c1 = p.collided(s1)
799+
c0 = p.collided(p.q, s0)
800+
c1 = p.collided(p.q, s1)
797801

798802
self.assertTrue(c0)
799803
self.assertFalse(c1)

‎tests/test_Robot.py

Lines changed: 20 additions & 22 deletions
Original file line numberDiff line numberDiff line change
@@ -33,20 +33,14 @@ def test_configurations_str(self):
3333

3434
def test_dyntable(self):
3535
r = rp.models.DH.Puma560()
36-
r.dyntable()
36+
r.dynamics()
3737

3838
def test_linkcolormap(self):
3939
r = rp.models.DH.Puma560()
4040
r.linkcolormap()
4141

4242
r.linkcolormap(['r', 'r', 'r', 'r', 'r', 'r'])
4343

44-
def test_base_error(self):
45-
r = rp.models.DH.Puma560()
46-
47-
with self.assertRaises(ValueError):
48-
r.base = 2
49-
5044
def test_tool_error(self):
5145
r = rp.models.DH.Puma560()
5246

@@ -80,32 +74,37 @@ def test_ikine_LM(self):
8074
sol3 = panda.ikine_LM(T, q0=[0, 1.4, 0, 1, 0, 0.5, 1])
8175

8276
self.assertTrue(sol1.success)
83-
self.assertAlmostEqual(np.linalg.norm(T - panda.fkine(sol1.q)), 0, places=4)
77+
self.assertAlmostEqual(np.linalg.norm(
78+
T - panda.fkine(sol1.q)), 0, places=4)
8479

8580
self.assertTrue(sol2.success[0])
86-
self.assertAlmostEqual(np.linalg.norm(T - panda.fkine(sol2.q[0,:])), 0, places=4)
81+
self.assertAlmostEqual(np.linalg.norm(
82+
T - panda.fkine(sol2.q[0, :])), 0, places=4)
8783
self.assertTrue(sol2.success[0])
88-
self.assertAlmostEqual(np.linalg.norm(T - panda.fkine(sol2.q[1,:])), 0, places=4)
84+
self.assertAlmostEqual(np.linalg.norm(
85+
T - panda.fkine(sol2.q[1, :])), 0, places=4)
8986

9087
self.assertTrue(sol3.success)
91-
self.assertAlmostEqual(np.linalg.norm(T - panda.fkine(sol3.q)), 0, places=4)
88+
self.assertAlmostEqual(np.linalg.norm(
89+
T - panda.fkine(sol3.q)), 0, places=4)
9290

9391
with self.assertRaises(ValueError):
94-
panda.ikine_LM(T, q0=[1,2])
92+
panda.ikine_LM(T, q0=[1, 2])
9593

9694
def test_ikine_LM_mask(self):
9795

9896
# simple RR manipulator, solve with mask
9997

10098
l0 = rp.RevoluteDH(a=2.0)
10199
l1 = rp.RevoluteDH(a=1)
102-
100+
103101
r = rp.DHRobot([l0, l1]) # RR manipulator
104102
T = sm.SE3(-1, 2, 0)
105103
sol = r.ikine_LM(T, mask=[1, 1, 0, 0, 0, 0])
106104

107105
self.assertTrue(sol.success)
108-
self.assertAlmostEqual(np.linalg.norm(T.t[:2] - r.fkine(sol.q).t[:2]), 0, places=4)
106+
self.assertAlmostEqual(np.linalg.norm(
107+
T.t[:2] - r.fkine(sol.q).t[:2]), 0, places=4)
109108

110109
# test initial condition search, drop iteration limit so it has to do
111110
# some searching
@@ -114,7 +113,8 @@ def test_ikine_LM_mask(self):
114113
ilimit=8, search=True)
115114

116115
self.assertTrue(sol.success)
117-
self.assertAlmostEqual(np.linalg.norm(T.t[:2] - r.fkine(sol.q).t[:2]), 0, places=4)
116+
self.assertAlmostEqual(np.linalg.norm(
117+
T.t[:2] - r.fkine(sol.q).t[:2]), 0, places=4)
118118

119119
def test_ikine_LM_transpose(self):
120120
# need to test this on a robot with squarish Jacobian, choose Puma
@@ -127,7 +127,6 @@ def test_ikine_LM_transpose(self):
127127
self.assertTrue(sol.success)
128128
self.assertAlmostEqual(np.linalg.norm(T - r.fkine(sol.q)), 0, places=4)
129129

130-
131130
def test_ikine_con(self):
132131
panda = rp.models.DH.Panda()
133132
q = np.array([0, -0.3, 0, -2.2, 0, 2.0, np.pi / 4])
@@ -142,7 +141,8 @@ def test_ikine_con(self):
142141
sol2 = panda.ikine_min(Tt, qlim=True)
143142

144143
self.assertTrue(sol1.success)
145-
self.assertAlmostEqual(np.linalg.norm(T - panda.fkine(sol1.q)), 0, places=4)
144+
self.assertAlmostEqual(np.linalg.norm(
145+
T - panda.fkine(sol1.q)), 0, places=4)
146146
nt.assert_array_almost_equal(
147147
T.A - panda.fkine(sol1.q).A, np.zeros((4, 4)), decimal=4)
148148

@@ -153,7 +153,6 @@ def test_ikine_con(self):
153153
nt.assert_array_almost_equal(
154154
T.A - panda.fkine(sol2[1].q).A, np.zeros((4, 4)), decimal=4)
155155

156-
157156
def test_ikine_unc(self):
158157
puma = rp.models.DH.Puma560()
159158
q = puma.qn
@@ -170,7 +169,7 @@ def test_ikine_unc(self):
170169
self.assertTrue(sol1[1].success)
171170
nt.assert_array_almost_equal(
172171
T.A - puma.fkine(sol1[1].q).A, np.zeros((4, 4)), decimal=4)
173-
172+
174173
self.assertTrue(sol2.success)
175174
nt.assert_array_almost_equal(
176175
T.A - puma.fkine(sol2.q).A, np.zeros((4, 4)), decimal=4)
@@ -190,8 +189,8 @@ def test_ikine_unc(self):
190189
unittest.main()
191190
# pytest.main(['tests/test_SerialLink.py'])
192191

193-
# import roboticstoolbox as rtb
194-
# from spatialmath import SE3
192+
# import roboticstoolbox as rtb
193+
# from spatialmath import SE3
195194

196195
# l0 = rtb.RevoluteDH(a=2.0)
197196
# l1 = rtb.RevoluteDH(a=1)
@@ -211,7 +210,6 @@ def test_ikine_unc(self):
211210
# # print(sol)
212211
# # print(r1.fkine(sol.q))
213212

214-
215213
# sol = r1.ikine_LM(
216214
# T, mask=[1, 1, 0, 0, 0, 0],
217215
# ilimit=8, search=True)

0 commit comments

Comments
 (0)