def test_fk_dict(self):
s1 = rp.Cylinder(1, 1)
ans = {
't': [0.0, 0.0, 0.0],
'q': [0.7071067811865476, 0.7071067811865475, 0.0, 0.0]}
self.assertEqual(s1.fk_dict(), ans)
def test_closest(self):
s0 = rp.Box([1, 1, 1], sm.SE3(0, 0, 0))
s1 = rp.Cylinder(1, 1, sm.SE3(2, 0, 0))
s2 = rp.Sphere(1, sm.SE3(4, 0, 0))
d0, _, _ = s0.closest_point(s1, 10)
d1, _, _ = s1.closest_point(s2, 10)
d2, _, _ = s2.closest_point(s0, 10)
d3, _, _ = s2.closest_point(s0)
self.assertAlmostEqual(d0, 0.5)
self.assertAlmostEqual(d1, 4.698463840213662e-13)
self.assertAlmostEqual(d2, 2.5)
self.assertAlmostEqual(d3, None)
def test_to_dict(self):
s1 = rp.Cylinder(1, 1)
ans = {
'stype': 'cylinder',
'scale': [1.0, 1.0, 1.0],
'filename': None,
'radius': 1.0,
'length': 1.0,
't': [0.0, 0.0, 0.0],
'q': [0.7071067811865476, 0.7071067811865475, 0.0, 0.0],
'v': [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]}
self.assertEqual(s1.to_dict(), ans)
def test_init(self):
rp.Box([1, 1, 1], sm.SE3(0, 0, 0))
rp.Cylinder(1, 1, sm.SE3(2, 0, 0))
rp.Sphere(1, sm.SE3(4, 0, 0))
