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()