def parse_origin(node): """Find the ``origin`` subelement of an XML node and convert it into a 4x4 homogenous transformation matrix. Parameters ---------- node : :class`lxml.etree.Element` An XML node which (optionally) has a child node with the ``origin`` tag. Returns ------- matrix : (4,4) float The 4x4 homogneous transform matrix that corresponds to this node's ``origin`` child. Defaults to the identity matrix if no ``origin`` child was found. """ matrix = sm.SE3() rpy = np.array([0, 0, 0]) origin_node = node.find('origin') if origin_node is not None: if 'xyz' in origin_node.attrib: t = np.fromstring(origin_node.attrib['xyz'], sep=' ') matrix = sm.SE3(t) if 'rpy' in origin_node.attrib: rpy = np.fromstring(origin_node.attrib['rpy'], sep=' ') matrix.A[:3, :3] = sm.SE3.RPY(rpy).R return matrix.A, rpy
def test_jacob0_4(self): r = rtb.models.Panda() q = np.array([1.0, 2, 3, 4, 5, 6, 7]) nt.assert_almost_equal( r.jacob0(q, end=r.links[4], tool=sm.SE3(0.1, 0, 0)), r.jacob0(q, end=r.links[4], tool=sm.SE3(0.1, 0, 0).A, fast=True))
def test_A(self): l0 = rp.DHLink(sigma=0) l1 = rp.DHLink(sigma=1) l2 = rp.DHLink(sigma=0, mdh=0) l3 = rp.DHLink(sigma=1, mdh=1) l4 = rp.DHLink(flip=True) T0 = sm.SE3(np.array([ [-1, 0, 0, 0], [0, -1, 0, 0], [0, 0, 1, 0], [0, 0, 0, 1] ])) T1 = sm.SE3(np.array([ [1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, np.pi], [0, 0, 0, 1] ])) nt.assert_array_almost_equal(l0.A(np.pi).A, T0.A) nt.assert_array_almost_equal(l1.A(np.pi).A, T1.A) nt.assert_array_almost_equal(l2.A(np.pi).A, T0.A) nt.assert_array_almost_equal(l3.A(np.pi).A, T1.A) nt.assert_array_almost_equal(l4.A(np.pi).A, T0.A)
def test_collision(self): s0 = rp.Box([1, 1, 1], sm.SE3(0, 0, 0)) s1 = rp.Box([1, 1, 1], sm.SE3(0.5, 0, 0)) s2 = rp.Box([1, 1, 1], sm.SE3(3, 0, 0)) c0 = s0.collided(s1) c1 = s0.collided(s2) 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_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_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_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_ikine_unc(self): puma = rp.models.DH.Puma560() q = puma.qn T = puma.fkine(q) Tt = sm.SE3([T, T]) sol1 = puma.ikine_min(Tt) sol2 = puma.ikine_min(T.A) sol3 = puma.ikine_min(T) self.assertTrue(sol1[0].success) nt.assert_array_almost_equal(T.A - puma.fkine(sol1[0].q).A, np.zeros((4, 4)), decimal=4) self.assertTrue(sol1[1].success) nt.assert_array_almost_equal(T.A - puma.fkine(sol1[1].q).A, np.zeros((4, 4)), decimal=4) self.assertTrue(sol2.success) nt.assert_array_almost_equal(T.A - puma.fkine(sol2.q).A, np.zeros((4, 4)), decimal=4) self.assertTrue(sol3.success) nt.assert_array_almost_equal(T.A - puma.fkine(sol3.q).A, np.zeros((4, 4)), decimal=4)
def test_ikine_con(self): panda = rp.models.DH.Panda() q = np.array([0, -0.3, 0, -2.2, 0, 2.0, np.pi / 4]) T = panda.fkine(q) Tt = sm.SE3([T, T, T]) # qr = [7.69161412e-04, 9.01409257e-01, -1.46372859e-02, # -6.98000000e-02, 1.38978915e-02, 9.62104811e-01, # 7.84926515e-01] sol1 = panda.ikine_min(T.A, qlim=True, q0=np.zeros(7)) sol2 = panda.ikine_min(Tt, qlim=True) self.assertTrue(sol1.success) self.assertAlmostEqual(np.linalg.norm(T - panda.fkine(sol1.q)), 0, places=4) nt.assert_array_almost_equal(T.A - panda.fkine(sol1.q).A, np.zeros((4, 4)), decimal=4) self.assertTrue(sol2[0].success) nt.assert_array_almost_equal(T.A - panda.fkine(sol2[0].q).A, np.zeros((4, 4)), decimal=4) self.assertTrue(sol2[1].success) nt.assert_array_almost_equal(T.A - panda.fkine(sol2[1].q).A, np.zeros((4, 4)), decimal=4)
def prepare_trajectory(trajectory: np.ndarray, robot_pose: Pose, robot_model: rtb.DHRobot, model_pose: np.ndarray) -> np.ndarray: robot_orientation = Quaternion(robot_pose.orientation[0], robot_pose.orientation[1], robot_pose.orientation[2], robot_pose.orientation[3]) robot_position = Point(robot_pose.position[0], robot_pose.position[1], robot_pose.position[2]) robot_euler = T.euler_from_quaternion( (robot_orientation.x, robot_orientation.y, robot_orientation.z, robot_orientation.w)) robot_transformation = T.euler_matrix(robot_euler[0], robot_euler[1], robot_euler[2]) robot_transformation[0, 3] = robot_position.x robot_transformation[1, 3] = robot_position.y robot_transformation[2, 3] = robot_position.z robot_transformation = np.linalg.inv(robot_transformation) transformed_trajectory = [] for i in range(len(trajectory)): projected_point = robot_transformation.dot( np.concatenate((trajectory[i], 1), axis=None)) / 1000 projected_point = model_pose.dot(projected_point)[:-1] ik_result, fail, err = robot_model.ikine( sm.SE3(projected_point[0], projected_point[1], projected_point[2])) transformed_trajectory += [ik_result] return np.array(transformed_trajectory)
def test_ikine6s_fail(self): l0 = rp.Revolute(alpha=np.pi / 2) l1 = rp.Revolute(d=1.0) l2 = rp.Revolute(alpha=np.pi / 2) l3 = rp.Revolute(alpha=-np.pi / 2) l4a = rp.Revolute(alpha=np.pi / 2) l4b = rp.Revolute() l5 = rp.Revolute() l6 = rp.Revolute(mdh=1) r0 = rp.SerialLink([l0, l1, l2, l3, l4a, l5]) r1 = rp.SerialLink([l0, l1, l2, l3, l4b, l5]) r2 = rp.SerialLink([l1, l2, l3]) r3 = rp.SerialLink([l6, l6, l6, l6, l6, l6]) puma = rp.Puma560() T = sm.SE3(0, 10, 10) puma.ikine6s(T) q = [1, 1, 1, 1, 1, 1] T = r0.fkine(q) with self.assertRaises(ValueError): r0.ikine6s(T) with self.assertRaises(ValueError): r1.ikine6s(T) with self.assertRaises(ValueError): r2.ikine6s(T) with self.assertRaises(ValueError): r3.ikine6s(T)
def plotting_func(queue, ok_queue): sys.stderr = None sys.stdout = None # Make and instance of the Swift simulator and open it from roboticstoolbox.backends.Swift import Swift env = Swift(realtime=False) env.launch() # Make a robot model and set its joint angles to the ready joint configuration robot = rtb.models.UR5() robot.q = robot.qr robot.base = sm.SE3(0, 0, 0.8) # Add the robot to the simulator env.add(robot) #ok_queue.put(None) while True: q_or_end = queue.get() if q_or_end is None: return robot.q, step_size = q_or_end # Step the simulator by step_size env.step(step_size)
def test_color(self): shape = rp.Box([1, 1, 1], sm.SE3(0, 0, 0)) shape.color = [0.1, 0.2, 0.3] self.assertEqual(shape.color[0], 0.1) self.assertEqual(shape.color[1], 0.2) self.assertEqual(shape.color[2], 0.3) self.assertEqual(shape.color[3], 1) shape.color = [0.1, 0.2, 0.3, 0.5] self.assertEqual(shape.color[0], 0.1) self.assertEqual(shape.color[1], 0.2) self.assertEqual(shape.color[2], 0.3) self.assertEqual(shape.color[3], 0.5) shape.color = (0.1, 0.2, 0.3) self.assertEqual(shape.color[0], 0.1) self.assertEqual(shape.color[1], 0.2) self.assertEqual(shape.color[2], 0.3) self.assertEqual(shape.color[3], 1) shape.color = (100, 200, 250, 100) self.assertAlmostEqual(shape.color[0], 100 / 255) self.assertAlmostEqual(shape.color[1], 200 / 255) self.assertAlmostEqual(shape.color[2], 250 / 255) self.assertEqual(shape.color[3], 100 / 255)
def test_ikine_LM(self): panda = rp.models.DH.Panda() q = np.array([0, -0.3, 0, -2.2, 0, 2.0, np.pi / 4]) T = panda.fkine(q) Tt = sm.SE3([T, T]) qr = [0.0342, 1.6482, 0.0312, 1.2658, -0.0734, 0.4836, 0.7489] sol1 = panda.ikine_LM(T) sol2 = panda.ikine_LM(Tt) sol3 = panda.ikine_LM(T, q0=[0, 1.4, 0, 1, 0, 0.5, 1]) self.assertTrue(sol1.success) self.assertAlmostEqual(np.linalg.norm(T - panda.fkine(sol1.q)), 0, places=4) self.assertTrue(sol2.success[0]) self.assertAlmostEqual(np.linalg.norm(T - panda.fkine(sol2.q[0, :])), 0, places=4) self.assertTrue(sol2.success[0]) self.assertAlmostEqual(np.linalg.norm(T - panda.fkine(sol2.q[1, :])), 0, places=4) self.assertTrue(sol3.success) self.assertAlmostEqual(np.linalg.norm(T - panda.fkine(sol3.q)), 0, places=4) with self.assertRaises(ValueError): panda.ikine_LM(T, q0=[1, 2])
def test_color(self): shape = rp.Box([1, 1, 1], sm.SE3(0, 0, 0)) shape.color = [1, 2, 3] self.assertEquals(shape.color[0], 1) self.assertEquals(shape.color[1], 2) self.assertEquals(shape.color[2], 3) self.assertEquals(shape.color[3], 1) shape.color = [1, 2, 3, 0.5] self.assertEquals(shape.color[0], 1) self.assertEquals(shape.color[1], 2) self.assertEquals(shape.color[2], 3) self.assertEquals(shape.color[3], 0.5) shape.color = (1, 2, 3) self.assertEquals(shape.color[0], 1) self.assertEquals(shape.color[1], 2) self.assertEquals(shape.color[2], 3) self.assertEquals(shape.color[3], 1) shape.color = (1, 2, 3, 0.5) self.assertEquals(shape.color[0], 1) self.assertEquals(shape.color[1], 2) self.assertEquals(shape.color[2], 3) self.assertEquals(shape.color[3], 0.5)
def test_ikine_LM(self): panda = rp.models.DH.Panda() q = np.array([0, -0.3, 0, -2.2, 0, 2.0, np.pi / 4]) T = panda.fkine(q) Tt = sm.SE3([T, T]) l0 = rp.RevoluteDH(d=2.0) l1 = rp.PrismaticDH(theta=1.0) l2 = rp.PrismaticDH(theta=1, qlim=[0, 2]) r0 = rp.DHRobot([l0, l1]) r1 = rp.DHRobot([l0, l2]) qr = [0.0342, 1.6482, 0.0312, 1.2658, -0.0734, 0.4836, 0.7489] sol1 = panda.ikine_LM(T) sol2 = panda.ikine_LM(Tt) sol3 = panda.ikine_LM(T, q0=[0, 1.4, 0, 1, 0, 0.5, 1]) # Untested sol5 = r0.ikine_LM(T.A, mask=[1, 1, 0, 0, 0, 0], transpose=5, ilimit=5) sol6 = r0.ikine_LM(T, mask=[1, 1, 0, 0, 0, 0]) sol7 = r0.ikine_LM(T, mask=[1, 1, 0, 0, 0, 0], ilimit=1) sol8 = r1.ikine_LM(T, mask=[1, 1, 0, 0, 0, 0], ilimit=1, search=True, slimit=1) self.assertTrue(sol1.success) self.assertAlmostEqual(np.linalg.norm(T - panda.fkine(sol1.q)), 0, places=4) self.assertTrue(sol2[0].success) self.assertAlmostEqual(np.linalg.norm(T - panda.fkine(sol2[0].q)), 0, places=4) self.assertTrue(sol2[0].success) self.assertAlmostEqual(np.linalg.norm(T - panda.fkine(sol2[1].q)), 0, places=4) self.assertTrue(sol3.success) self.assertAlmostEqual(np.linalg.norm(T - panda.fkine(sol3.q)), 0, places=4) with self.assertRaises(ValueError): panda.ikine_LM(T, q0=[1, 2]) with self.assertRaises(ValueError): r0.ikine_LM(T, mask=[1, 1, 0, 0, 0, 0], ilimit=1, search=True, slimit=1)
def test_base(self): panda = rtb.models.ETS.Panda() pose = sm.SE3() panda.base = pose.A nt.assert_array_almost_equal(np.eye(4), panda.base.A) panda.base = pose nt.assert_array_almost_equal(np.eye(4), panda.base.A)
def _step_shapes(self, dt): for shape in self.shapes: T = shape.base t = T.t r = T.rpy('rad') t += shape.v[:3] * (dt) r += shape.v[3:] * (dt) shape.base = sm.SE3(t) * sm.SE3.RPY(r)
def test_ikine6s_traj(self): r0 = rp.Puma560() q = r0.qr T = r0.fkine(q) Tt = sm.SE3([T, T, T]) qr0 = [0.2689, 1.5708, -1.4768, -3.1416, 0.0940, 2.8726] q0, _ = r0.ikine6s(Tt) nt.assert_array_almost_equal(q0[:, 0], qr0, decimal=4) nt.assert_array_almost_equal(q0[:, 1], qr0, decimal=4) nt.assert_array_almost_equal(q0[:, 2], qr0, decimal=4)
def _step_shapes(self, dt): for shape in self.shapes: if shape is not None: T = shape.base t = T.t.astype('float64') r = T.rpy('rad').astype('float64') t += shape.v[:3] * dt r += shape.v[3:] * dt shape.base = sm.SE3(t) * sm.SE3.RPY(r)
def test_ikine6s_traj(self): self.skipTest("error introduced with DHLink change") r0 = rp.models.DH.Puma560() q = r0.qr T = r0.fkine(q) Tt = sm.SE3([T, T, T]) qr0 = [0.2689, 1.5708, -1.4768, -3.1416, 0.0940, 2.8726] q0, _ = r0.ikine6s(Tt) nt.assert_array_almost_equal(q0[0, :], qr0, decimal=4) nt.assert_array_almost_equal(q0[1, :], qr0, decimal=4) nt.assert_array_almost_equal(q0[2, :], qr0, decimal=4)
def test_ikine(self): panda = rp.PandaMDH() q = np.array([0, -0.3, 0, -2.2, 0, 2.0, np.pi / 4]) T = panda.fkine(q) Tt = sm.SE3([T, T]) l0 = rp.Revolute(d=2.0) l1 = rp.Prismatic(theta=1.0) l2 = rp.Prismatic(theta=1, qlim=[0, 2]) r0 = rp.SerialLink([l0, l1]) r1 = rp.SerialLink([l0, l2]) qr = [0.0342, 1.6482, 0.0312, 1.2658, -0.0734, 0.4836, 0.7489] qa, success, err = panda.ikine(T) qa2, success, err = panda.ikine(Tt) qa3, success, err = panda.ikine(Tt, q0=np.zeros((7, 2))) qa4, success, err = panda.ikine(T, q0=np.zeros(7)) # Untested qa5, success, err = r0.ikine(T.A, mask=[1, 1, 0, 0, 0, 0], transpose=5, ilimit=5) qa5, success, err = r0.ikine(T, mask=[1, 1, 0, 0, 0, 0]) qa6, success, err = r0.ikine(T, mask=[1, 1, 0, 0, 0, 0], ilimit=1) qa7, success, err = r1.ikine(T, mask=[1, 1, 0, 0, 0, 0], ilimit=1, search=True, slimit=1) nt.assert_array_almost_equal(qa, qr, decimal=4) nt.assert_array_almost_equal(qa2[:, 0], qr, decimal=4) nt.assert_array_almost_equal(qa2[:, 1], qr, decimal=4) nt.assert_array_almost_equal(qa3[:, 1], qr, decimal=4) nt.assert_array_almost_equal(qa4, qr, decimal=4) with self.assertRaises(ValueError): panda.ikine(Tt, q0=np.zeros(7)) with self.assertRaises(ValueError): r0.ikine(T) with self.assertRaises(ValueError): r0.ikine(T, mask=[1, 1, 0, 0, 0, 0], ilimit=1, search=True, slimit=1)
def test_ikinem(self): puma = rp.models.DH.Puma560() q = puma.qr T = puma.fkine(q) Tt = sm.SE3([T, T]) q0, _, _ = puma.ikinem(Tt) q1, success, _ = puma.ikinem(T.A, qlimits=False) q2, success, _ = puma.ikinem(T, qlimits=False, stiffness=0.1, ilimit=1) # print(np.sum(np.abs(T.A - puma.fkine(q0[:, 0]).A))) self.assertTrue(np.sum(np.abs(T.A - puma.fkine(q0[0, :]).A)) < 0.7) self.assertTrue(np.sum(np.abs(T.A - puma.fkine(q0[1, :]).A)) < 0.7) self.assertTrue(np.sum(np.abs(T.A - puma.fkine(q1).A)) < 0.7)
def test_ikine_min(self): puma = rp.models.DH.Puma560() q = puma.qn T = puma.fkine(q) Tt = sm.SE3([T, T]) sol0 = puma.ikine_min(Tt) sol1 = puma.ikine_min(T.A, qlimits=False) sol2 = puma.ikine_min(T, qlimits=False, stiffness=0.1, ilimit=1) # print(np.sum(np.abs(T.A - puma.fkine(q0[:, 0]).A))) self.assertTrue(sol0[0].success) self.assertAlmostEqual(np.linalg.norm(T - puma.fkine(sol0[0].q)), 0, places=4)
def test_p_servo(self): a = sm.SE3() b = sm.SE3.Rx(0.7) * sm.SE3.Tx(1) c = sm.SE3.Tz(0.59) v0, arrived0 = rp.p_servo(a, b) v1, _ = rp.p_servo(a.A, b.A) _, arrived1 = rp.p_servo(a, c, threshold=0.6) ans = np.array([2, 0, 0, 1.4, -0, 0]) nt.assert_array_almost_equal(v0, ans, decimal=4) nt.assert_array_almost_equal(v1, ans, decimal=4) self.assertFalse(arrived0) self.assertTrue(arrived1)
def __init__(self, elinks, name='', tool=None): self._n = 0 self.name = name if tool is None: self.tool = sm.SE3() else: self.tool = tool for link in elinks: if link.isjoint: self._n += 1 self.q = np.zeros(self.n) self._links = elinks # assign the joint indices if all([link.jindex is None for link in elinks]): jindex = [0] # "mutable integer" hack def visit_link(link, jindex): # if it's a joint, assign it a jindex and increment it if link.isjoint: link.jindex = jindex[0] jindex[0] += 1 # visit all links in DFS order self.dfs_links(elinks[0], lambda link: visit_link(link, jindex)) elif all([link.jindex is not None for link in elinks]): # jindex set on all, check they are unique and sequential jset = set(range(self.n)) for link in elinks: if link.jindex not in jset: raise ValueError('gripper joint index {link.jindex} was ' 'repeated or out of range') jset -= set([link.jindex]) if len(jset) > 0: # pragma nocover # is impossible raise ValueError('gripper joints {jset} were not assigned') else: # must be a mixture of ELinks with/without jindex raise ValueError( 'all gripper links must have a jindex, or none have a jindex')
def test_ikunc(self): puma = rp.models.DH.Puma560() q = puma.qr T = puma.fkine(q) Tt = sm.SE3([T, T]) q0, _, _ = puma.ikunc(Tt) q1, success, _ = puma.ikunc(T.A) q2, success, _ = puma.ikunc(T, ilimit=1) nt.assert_array_almost_equal( T.A - puma.fkine(q0[0, :]).A, np.zeros((4, 4)), decimal=4) nt.assert_array_almost_equal( T.A - puma.fkine(q0[1, :]).A, np.zeros((4, 4)), decimal=4) nt.assert_array_almost_equal( T.A - puma.fkine(q1).A, np.zeros((4, 4)), decimal=4)
def test_ikine3(self): l0 = rp.RevoluteDH(alpha=np.pi / 2) l1 = rp.RevoluteDH(a=0.4318) l2 = rp.RevoluteDH(d=0.15005, a=0.0203, alpha=-np.pi / 2) l3 = rp.PrismaticDH() l4 = rp.PrismaticMDH() r0 = rp.DHRobot([l0, l1, l2]) r1 = rp.DHRobot([l3, l3]) r2 = rp.DHRobot([l3, l3, l3]) r3 = rp.DHRobot([l4, l4, l4]) q = [1, 1, 1] r0.q = q T = r0.fkine(q) # T2 = r1.fkine(q) Tt = sm.SE3([T, T]) res = [2.9647, 1.7561, 0.2344] res2 = [1.0000, 0.6916, 0.2344] res3 = [2.9647, 2.4500, 3.1762] res4 = [1.0000, 1.3855, 3.1762] q0 = r0.ikine3(T.A) q1 = r0.ikine3(Tt) q2 = r0.ikine3(T, left=False, elbow_up=False) q3 = r0.ikine3(T, elbow_up=False) q4 = r0.ikine3(T, left=False) nt.assert_array_almost_equal(q0, res, decimal=4) nt.assert_array_almost_equal(q1[0, :], res, decimal=4) nt.assert_array_almost_equal(q1[1, :], res, decimal=4) nt.assert_array_almost_equal(q2, res2, decimal=4) nt.assert_array_almost_equal(q3, res3, decimal=4) nt.assert_array_almost_equal(q4, res4, decimal=4) with self.assertRaises(ValueError): r1.ikine3(T) with self.assertRaises(ValueError): r2.ikine3(T) with self.assertRaises(ValueError): r3.ikine3(T)