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)
