def test_collided(self):
s0 = gm.Box([1, 1, 1], base=sm.SE3(0, 0, 0))
s1 = gm.Box([1, 1, 1], base=sm.SE3(3, 0, 0))
p = rtb.models.Panda()
c0 = p.collided(s0)
c1 = p.collided(s1)
self.assertTrue(c0)
self.assertFalse(c1)
def test_collided(self):
s0 = gm.Box([1, 1, 1], base=sm.SE3(0, 0, 0))
s1 = gm.Box([1, 1, 1], base=sm.SE3(3, 0, 0))
p = rp.models.Panda()
link = p.links[3]
c0 = link.collided(s0)
c1 = link.collided(s1)
self.assertTrue(c0)
self.assertFalse(c1)
def test_dist(self):
s0 = gm.Box([1, 1, 1], base=sm.SE3(0, 0, 0))
s1 = gm.Box([1, 1, 1], base=sm.SE3(3, 0, 0))
p = rtb.models.Panda()
d0, _, _ = p.closest_point(s0)
d1, _, _ = p.closest_point(s1, 5)
d2, _, _ = p.closest_point(s1)
self.assertAlmostEqual(d0, -0.5599999999995913)
self.assertAlmostEqual(d1, 2.4275999999999995)
self.assertAlmostEqual(d2, None)
def test_dist(self):
s0 = gm.Box([1, 1, 1], base=sm.SE3(0, 0, 0))
s1 = gm.Box([1, 1, 1], base=sm.SE3(3, 0, 0))
p = rp.models.Panda()
link = p.links[3]
d0, _, _ = link.closest_point(s0)
d1, _, _ = link.closest_point(s1, 5)
d2, _, _ = link.closest_point(s1)
self.assertAlmostEqual(d0, -0.49)
self.assertAlmostEqual(d1, 2.44)
self.assertAlmostEqual(d2, None)
def test_dict(self):
panda = rtb.models.Panda()
panda.grippers[0].links[0].collision.append(gm.Box([1, 1, 1]))
panda.to_dict()
wx = rtb.models.wx250s()
wx.to_dict()
def test_collision_fail(self):
l0 = rp.ELink()
col = gm.Box([1, 1, 1])
l0.collision = col
with self.assertRaises(TypeError):
l0.collision = [1, 1, 1]
with self.assertRaises(TypeError):
l0.collision = 1
def test_geometry_fail(self):
l0 = rp.ELink()
col = gm.Box([1, 1, 1])
l0.geometry = col
l0.geometry = [col, col]
with self.assertRaises(TypeError):
l0.geometry = [1, 1, 1]
with self.assertRaises(TypeError):
l0.geometry = 1
def test_fkdict(self):
panda = rtb.models.Panda()
panda.grippers[0].links[0].collision.append(gm.Box([1, 1, 1]))
panda.fk_dict()