def test_fk_dict2(self):
s1 = rp.Sphere(1)
ans = {
't': [0.0, 0.0, 0.0], 'q': [1, 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)
# Launch the simulator Swift env = rtb.backends.Swift() env.launch() # Create a Panda robot object panda = rtb.models.Panda() # Set joint angles to ready configuration panda.q = panda.qr # Number of joint in the panda which we are controlling n = 7 # Make two obstacles with velocities s0 = rtb.Sphere(radius=0.05, base=sm.SE3(0.52, 0.4, 0.3)) s0.v = [0, -0.2, 0, 0, 0, 0] s1 = rtb.Sphere(radius=0.05, base=sm.SE3(0.1, 0.35, 0.65)) s1.v = [0, -0.2, 0, 0, 0, 0] collisions = [s0, s1] # Make a target target = rtb.Sphere(radius=0.02, base=sm.SE3(0.6, -0.2, 0.0)) # Add the Panda and shapes to the simulator env.add(panda) env.add(s0) env.add(s1) env.add(target)
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))