def test_joint_angle_limit_weight(self):
j1 = RotationalJoint(
child_link=make_coords(),
max_angle=np.deg2rad(32.3493),
min_angle=np.deg2rad(-122.349))
j1.joint_angle(np.deg2rad(-60.0))
testing.assert_almost_equal(
joint_angle_limit_weight([j1]),
3.1019381e-01)
j2 = RotationalJoint(
child_link=make_coords(),
max_angle=np.deg2rad(74.2725),
min_angle=np.deg2rad(-20.2598))
j2.joint_angle(np.deg2rad(74.0))
testing.assert_almost_equal(
joint_angle_limit_weight([j2]),
1.3539208e+03)
j3 = RotationalJoint(
child_link=make_coords(),
max_angle=float('inf'),
min_angle=-float('inf'))
j3.joint_angle(np.deg2rad(-20.0))
testing.assert_almost_equal(
joint_angle_limit_weight([j3]),
0.0)
def test_orient_with_matrix(self):
coords = make_coords()
rotation_matrix = np.array([[0, -1, 0], [1, 0, 0], [0, 0, 1]])
coords.orient_with_matrix(rotation_matrix, wrt='local')
coords.orient_with_matrix(rotation_matrix, wrt='world')
wrt = make_coords().rotate(np.pi / 2.0, 'z')
coords.orient_with_matrix(rotation_matrix, wrt=wrt)
def test_difference_rotation(self):
coord1 = make_coords()
coord2 = make_coords(rot=rpy_matrix(pi / 2.0, pi / 3.0, pi / 5.0))
dif_rot = coord1.difference_rotation(coord2)
testing.assert_almost_equal(dif_rot,
[-0.32855112, 1.17434985, 1.05738936])
dif_rot = coord1.difference_rotation(coord2, False)
testing.assert_almost_equal(dif_rot, [0, 0, 0])
dif_rot = coord1.difference_rotation(coord2, 'x')
testing.assert_almost_equal(dif_rot, [0.0, 1.36034952, 0.78539816])
dif_rot = coord1.difference_rotation(coord2, 'y')
testing.assert_almost_equal(dif_rot, [0.35398131, 0.0, 0.97442695])
dif_rot = coord1.difference_rotation(coord2, 'z')
testing.assert_almost_equal(dif_rot, [-0.88435715, 0.74192175, 0.0])
dif_rot = coord1.difference_rotation(coord2, 'xx')
testing.assert_almost_equal(dif_rot, [0.0, -1.36034952, -0.78539816])
dif_rot = coord1.difference_rotation(coord2, 'yy')
testing.assert_almost_equal(dif_rot, [0.35398131, 0.0, 0.97442695])
dif_rot = coord1.difference_rotation(coord2, 'zz')
testing.assert_almost_equal(dif_rot, [-0.88435715, 0.74192175, 0.0])
coord1 = make_coords()
coord2 = make_coords().rotate(pi, 'x')
dif_rot = coord1.difference_rotation(coord2, 'xm')
testing.assert_almost_equal(dif_rot, [0, 0, 0])
coord2 = make_coords().rotate(pi / 2.0, 'x')
dif_rot = coord1.difference_rotation(coord2, 'xm')
testing.assert_almost_equal(dif_rot, [-pi / 2.0, 0, 0])
def test_transform_vector(self):
pos = [0.13264493, 0.05263172, 0.93042636]
q = [-0.20692513, 0.50841015, 0.82812527, 0.1136206]
coord = make_coords(pos=pos, rot=q)
testing.assert_almost_equal(
coord.transform_vector([0.2813606, 0.97762403, 0.83617263]),
[0.70004566, 1.05660075, 0.29465928])
coord = make_coords(pos=[0, 0, 1])
testing.assert_almost_equal(
coord.transform_vector([0, 0, 1]),
[0, 0, 2])
coord = make_coords(pos=[1, 1, 1])
testing.assert_almost_equal(
coord.transform_vector([-1, -1, -1]),
[0, 0, 0])
# batch transform
coord = make_coords(pos=[1, 1, 1])
coord.rotate(pi, 'z')
testing.assert_almost_equal(
coord.transform_vector(((0, 1, 2),
(2, 3, 4),
(5, 6, 7))),
((1, 0, 3),
(-1, -2, 5),
(-4, -5, 8)))
def test_transform(self):
coord = make_coords()
coord.transform(make_coords(pos=[1, 2, 3]))
testing.assert_array_equal(coord.translation, [1, 2, 3])
original_id = hex(id(coord))
coord.transform(make_coords(pos=[1, 2, 3]), coord)
self.assertEqual(original_id, hex(id(coord)))
def test_inverse_transform_vector(self):
pos = [0.13264493, 0.05263172, 0.93042636]
q = [-0.20692513, 0.50841015, 0.82812527, 0.1136206]
coord = make_coords(pos=pos, rot=q)
testing.assert_almost_equal(
coord.inverse_transform_vector(
[0.2813606, 0.97762403, 0.83617263]),
[0.63310725, 0.55723807, 0.41865477])
coord = make_coords(pos=[0, 0, 1])
testing.assert_almost_equal(
coord.inverse_transform_vector([0, 0, 1]),
[0, 0, 0])
coord = make_coords(pos=[1, 1, 1])
testing.assert_almost_equal(
coord.inverse_transform_vector([-1, -1, -1]),
[-2, -2, -2])
# batch transform
coord = make_coords(pos=(1, 1, 1))
coord.rotate(pi, 'z')
testing.assert_almost_equal(
coord.inverse_transform_vector(((0, 1, 2),
(2, 3, 4),
(5, 6, 7))),
((1, 0, 1),
(-1, -2, 3),
(-4, -5, 6)))
def test_midcoords(self):
a = make_coords(pos=[1.0, 1.0, 1.0])
b = make_coords()
c = midcoords(0.5, a, b)
testing.assert_array_equal(c.worldpos(), [0.5, 0.5, 0.5])
testing.assert_array_equal(matrix2quaternion(c.worldrot()),
[1, 0, 0, 0])
def test_difference_rotation(self):
dq1 = make_coords(pos=[1.0, 1.0, 1.0]).dual_quaternion
dq2 = make_coords(pos=[0.0, 0.0, 0.0]).dual_quaternion
testing.assert_almost_equal(dq1.difference_rotation(dq2), 0.0)
dq1 = make_coords(rot=[1, 0, 0, 0]).dual_quaternion
dq2 = make_coords(rot=[0, 0, 0, 1]).dual_quaternion
testing.assert_almost_equal(dq1.difference_rotation(dq2), np.pi)
def test_move_coords(self):
coord = make_coords()
target_coord = make_coords(pos=(1, 2, 3), rot=(0, 0, 1, 0))
local_coord = make_coords(pos=(-2, -2, -1))
coord.move_coords(target_coord, local_coord)
result = coord.copy_worldcoords().transform(local_coord)
testing.assert_almost_equal(result.translation, (1, 2, 3))
testing.assert_almost_equal(result.quaternion, (0, 0, 1, 0))
def test___init__(self):
coord = make_coords(pos=[1, 1, 1])
testing.assert_array_equal(coord.translation, [1, 1, 1])
coord = make_coords(pos=[1, 0, 1], rot=[pi, 0, 0])
testing.assert_array_equal(coord.translation, [1, 0, 1])
testing.assert_almost_equal(coord.rotation,
[[-1, 0, 0], [0, -1, 0], [0, 0, 1]])
coord = make_coords(
[[-1, 0, 0, 1], [0, -1, 0, 0], [0, 0, 1, -1], [0, 0, 0, 1]],
rot=[pi, 0, 0])
testing.assert_array_equal(coord.translation, [1, 0, -1])
testing.assert_almost_equal(coord.rotation,
[[-1, 0, 0], [0, -1, 0], [0, 0, 1]])
def test_y_axis(self):
coord = make_coords()
testing.assert_array_equal(coord.y_axis, [0, 1, 0])
y_axis = coord.y_axis
y_axis[0] = 10
testing.assert_array_equal(coord.y_axis, [0, 1, 0])
def test_z_axis(self):
coord = make_coords()
testing.assert_array_equal(coord.z_axis, [0, 0, 1])
z_axis = coord.z_axis
z_axis[0] = 10
testing.assert_array_equal(coord.z_axis, [0, 0, 1])
def midcoords(p, c1, c2):
"""Returns mid (or p) coordinates of given two coordinates c1 and c2.
Parameters
----------
p : float
ratio of c1:c2
c1 : skrobot.coordinates.Coordinates
Coordinates
c2 : skrobot.coordinates.Coordinates
Coordinates
Returns
-------
coordinates : skrobot.coordinates.Coordinates
midcoords
Examples
--------
>>> from skrobot.coordinates import Coordinates
>>> from skrobot.coordinates.geo import midcoords
>>> c1 = Coordinates()
>>> c2 = Coordinates(pos=[0.1, 0, 0])
>>> c = midcoords(0.5, c1, c2)
>>> c.translation
array([0.05, 0. , 0. ])
"""
return make_coords(pos=midpoint(p, c1.worldpos(), c2.worldpos()),
rot=midrot(p, c1.worldrot(), c2.worldrot()))
def test_x_axis(self):
coord = make_coords()
testing.assert_array_equal(coord.x_axis, [1, 0, 0])
x_axis = coord.x_axis
x_axis[0] = 10
testing.assert_array_equal(coord.x_axis, [1, 0, 0])
def test_transform(self):
a = make_cascoords(rot=rotation_matrix(pi / 3, 'x'),
pos=[0.1, 0, 0])
b = make_cascoords(rot=rotation_matrix(pi / 3, 'y'),
pos=[0.1, 0, 0.2])
a.assoc(b)
testing.assert_almost_equal(
b.copy_worldcoords().transform(
make_cascoords(pos=(-0.1, -0.2, -0.3)), 'local').translation,
(-0.20980762113533155, -0.1999999999999999, 0.13660254037844383))
testing.assert_almost_equal(
b.copy_worldcoords().transform(
make_cascoords(pos=(-0.1, -0.2, -0.3)), 'world').translation,
(0, -0.2, -0.1))
c = make_coords(pos=(-0.2, -0.3, -0.4)).rotate(pi / 2, 'y')
b.transform(
make_cascoords(pos=(-0.1, -0.2, -0.3)), c)
testing.assert_almost_equal(
b.translation, (-0.3, 0.15980762113533148, 0.32320508075688775))
testing.assert_almost_equal(
b.copy_worldcoords().translation,
(-0.2, -0.2, 0.3))
wrts = ['local', 'world', 'parent']
for wrt in wrts:
b.transform(
make_cascoords(pos=(-0.1, -0.2, -0.3)), wrt=wrt)
def test_transformation(self):
coord_a = make_coords(pos=[0, 0, 1])
coord_b = make_coords(pos=[1, 0, 0])
testing.assert_almost_equal(
coord_a.transformation(coord_b).worldpos(), [1, 0, -1])
testing.assert_almost_equal(
coord_a.transformation(coord_b).quaternion, [1, 0, 0, 0])
testing.assert_almost_equal(
coord_b.transformation(coord_a).worldpos(), [-1, 0, 1])
testing.assert_almost_equal(
coord_b.transformation(coord_a).quaternion, [1, 0, 0, 0])
c = make_coords(rot=[deg2rad(10), 0, 0])
d = make_coords(rot=[deg2rad(20), 0, 0])
testing.assert_almost_equal(
c.transformation(d).worldrot(),
make_coords(rot=[deg2rad(10), 0, 0]).worldrot())
def rotate(self):
c = make_coords(pos=[1, 2, 3])
c.rotate(pi / 7.0, 'y', 'world')
c.rotate(pi / 11.0, 'x', 'local')
testing.assert_almost_equal(
c.worldrot(),
[[0.900969, 0.122239, 0.416308], [0.0, 0.959493, -0.281733],
[-0.433884, 0.253832, 0.864473]])
def test_quaternion(self):
c = make_coords()
testing.assert_almost_equal(
c.quaternion, [1, 0, 0, 0])
c.rotate(pi / 3, 'y').rotate(pi / 5, 'z')
testing.assert_almost_equal(
c.quaternion,
[0.8236391, 0.1545085, 0.47552826, 0.26761657])
def test_translate(self):
c = make_coords()
testing.assert_almost_equal(
c.translation, [0, 0, 0])
c.translate([0.1, 0.2, 0.3])
testing.assert_almost_equal(
c.translation, [0.1, 0.2, 0.3])
c = make_coords().rotate(pi / 2.0, 'y')
c.translate([0.1, 0.2, 0.3], 'local')
testing.assert_almost_equal(
c.translation, [0.3, 0.2, -0.1])
c = make_coords().rotate(pi / 2.0, 'y')
c.translate([0.1, 0.2, 0.3], 'world')
testing.assert_almost_equal(
c.translation, [0.1, 0.2, 0.3])
c = make_coords()
c2 = make_coords().translate([0.1, 0.2, 0.3])
c.translate([0.1, 0.2, 0.3], c2)
testing.assert_almost_equal(
c.translation, [0.1, 0.2, 0.3])
c = make_coords().rotate(pi / 3.0, 'z')
c2 = make_coords().rotate(pi / 2.0, 'y')
c.translate([0.1, 0.2, 0.3], c2)
testing.assert_almost_equal(
c.translation, [0.3, 0.2, -0.1])
testing.assert_almost_equal(
c.rpy_angle()[0], [pi / 3.0, 0, 0])
def test_inverse_transformation(self):
coord_a = make_coords(pos=[1, 1, 1])
testing.assert_almost_equal(
coord_a.inverse_transformation().worldpos(), [-1, -1, -1])
pos = [0.13264493, 0.05263172, 0.93042636]
q = [-0.20692513, 0.50841015, 0.82812527, 0.1136206]
coord_b = make_coords(pos=pos, rot=q)
testing.assert_almost_equal(
coord_b.inverse_transformation().worldpos(),
[-0.41549991, -0.12132025, 0.83588229])
testing.assert_almost_equal(
coord_b.inverse_transformation().quaternion,
[0.20692513, 0.50841015, 0.82812527, 0.1136206])
# check inverse of transformation(worldcoords)
testing.assert_almost_equal(
coord_a.inverse_transformation().worldpos(),
coord_a.transformation(make_coords()).worldpos())
def test_difference_position(self):
coord1 = make_coords()
coord2 = make_coords().translate([1, 2, 3])
dif_pos = coord1.difference_position(coord2)
testing.assert_almost_equal(dif_pos, [1, 2, 3])
dif_pos = coord1.difference_position(coord2, translation_axis=False)
testing.assert_almost_equal(dif_pos, [0, 0, 0])
dif_pos = coord1.difference_position(coord2, translation_axis='x')
testing.assert_almost_equal(dif_pos, [0, 2, 3])
dif_pos = coord1.difference_position(coord2, translation_axis='y')
testing.assert_almost_equal(dif_pos, [1, 0, 3])
dif_pos = coord1.difference_position(coord2, translation_axis='z')
testing.assert_almost_equal(dif_pos, [1, 2, 0])
dif_pos = coord1.difference_position(coord2, translation_axis='xy')
testing.assert_almost_equal(dif_pos, [0, 0, 3])
dif_pos = coord1.difference_position(coord2, translation_axis='yz')
testing.assert_almost_equal(dif_pos, [1, 0, 0])
dif_pos = coord1.difference_position(coord2, translation_axis='zx')
testing.assert_almost_equal(dif_pos, [0, 2, 0])
def test_orient_coords_to_axis(self):
target_coords = make_coords(pos=[1.0, 1.0, 1.0])
orient_coords_to_axis(target_coords, [0, 1, 0])
testing.assert_array_equal(target_coords.worldpos(), [1, 1, 1])
testing.assert_array_almost_equal(target_coords.rpy_angle()[0],
[0, 0, -1.57079633])
# case of rot_angle_cos == 1.0
target_coords = make_coords(pos=[1.0, 1.0, 1.0])
orient_coords_to_axis(target_coords, [0, 0, 1])
testing.assert_array_equal(target_coords.worldpos(), [1, 1, 1])
testing.assert_array_almost_equal(target_coords.rpy_angle()[0],
[0, 0, 0])
# case of rot_angle_cos == -1.0
target_coords = make_coords()
orient_coords_to_axis(target_coords, [0, 0, -1])
testing.assert_array_equal(target_coords.worldpos(), [0, 0, 0])
testing.assert_array_almost_equal(target_coords.rpy_angle()[0],
[0, 0, pi])
def test_transform(self):
coord = make_coords()
coord.transform(make_coords(pos=[1, 2, 3]))
testing.assert_array_equal(coord.translation, [1, 2, 3])
original_id = hex(id(coord))
coord.transform(make_coords(pos=[1, 2, 3]), coord)
self.assertEqual(original_id, hex(id(coord)))
coord = make_coords().rotate(pi / 2.0, 'y')
coord.transform(make_coords(pos=[1, 2, 3]), 'world')
testing.assert_array_equal(coord.translation, [1, 2, 3])
wrt = make_coords().rotate(pi / 2.0, 'y')
coord = make_coords()
coord.transform(make_coords(pos=[1, 2, 3]), wrt)
testing.assert_almost_equal(coord.translation, [3.0, 2.0, -1.0])
def test_is_relevant(self):
fetch = self.fetch
self.assertTrue(fetch._is_relevant(
fetch.shoulder_pan_joint, fetch.wrist_roll_link))
self.assertFalse(fetch._is_relevant(
fetch.shoulder_pan_joint, fetch.base_link))
self.assertTrue(fetch._is_relevant(
fetch.shoulder_pan_joint, fetch.rarm_end_coords))
co = make_coords()
with self.assertRaises(AssertionError):
fetch._is_relevant(fetch.shoulder_pan_joint, co)
# if it's not connceted to the robot,
casco = CascadedCoords()
with self.assertRaises(AssertionError):
fetch._is_relevant(fetch.shoulder_pan_joint, casco)
# but, if casco connects to the robot,
fetch.rarm_end_coords.assoc(casco)
self.assertTrue(fetch._is_relevant(
fetch.shoulder_pan_joint, casco))
def test_transform_vector_and_rotate_vector(self):
# see test_transform_vector for these values
pos = [0.13264493, 0.05263172, 0.93042636]
q = [-0.20692513, 0.50841015, 0.82812527, 0.1136206]
coords = make_coords(pos=pos, rot=q)
tf = coords.get_transform()
pt_original = np.array([0.2813606, 0.97762403, 0.83617263])
pt_transformed = tf.transform_vector(pt_original)
pt_ground_truth = coords.transform_vector(pt_original)
testing.assert_equal(pt_transformed, pt_ground_truth)
pt_transformed = tf.rotate_vector(pt_original)
pt_ground_truth = coords.rotate_vector(pt_original)
testing.assert_equal(pt_transformed, pt_ground_truth)
tf.transform_vector(np.zeros((100, 3))) # ok
with self.assertRaises(AssertionError):
tf.transform_vector(np.zeros((100, 100, 3))) # ng
tf.rotate_vector(np.zeros((100, 3))) # ok
with self.assertRaises(AssertionError):
tf.rotate_vector(np.zeros((100, 100, 3))) # ng
def test_difference_position(self):
dq1 = make_coords(pos=[1.0, 1.0, 1.0]).dual_quaternion
dq2 = make_coords(pos=[0.0, 0.0, 0.0]).dual_quaternion
testing.assert_almost_equal(dq1.difference_position(dq2), np.sqrt(3.0))
def test_difference_rotation(self):
coord1 = make_coords()
coord2 = make_coords(rot=rpy_matrix(pi / 2.0, pi / 3.0, pi / 5.0))
dif_rot = coord1.difference_rotation(coord2)
testing.assert_almost_equal(dif_rot,
[-0.32855112, 1.17434985, 1.05738936])
dif_rot = coord1.difference_rotation(coord2, False)
testing.assert_almost_equal(dif_rot,
[0, 0, 0])
dif_rot = coord1.difference_rotation(coord2, 'x')
testing.assert_almost_equal(dif_rot,
[0.0, 1.36034952, 0.78539816])
dif_rot = coord1.difference_rotation(coord2, 'y')
testing.assert_almost_equal(dif_rot,
[0.35398131, 0.0, 0.97442695])
dif_rot = coord1.difference_rotation(coord2, 'z')
testing.assert_almost_equal(dif_rot,
[-0.88435715, 0.74192175, 0.0])
# TODO(iory) This case's rotation_axis='xx' is unstable
# due to float point
dif_rot = coord1.difference_rotation(coord2, 'xx')
testing.assert_almost_equal(dif_rot[0], 0)
testing.assert_almost_equal(abs(dif_rot[1]), 1.36034952)
testing.assert_almost_equal(abs(dif_rot[2]), 0.78539816)
testing.assert_almost_equal(sign(dif_rot[1]) * sign(dif_rot[2]), 1)
dif_rot = coord1.difference_rotation(coord2, 'yy')
testing.assert_almost_equal(
dif_rot, [0.35398131, 0.0, 0.97442695])
dif_rot = coord1.difference_rotation(coord2, 'zz')
testing.assert_almost_equal(
dif_rot, [-0.88435715, 0.74192175, 0.0])
coord1 = make_coords()
coord2 = make_coords().rotate(pi, 'x')
dif_rot = coord1.difference_rotation(coord2, 'xm')
testing.assert_almost_equal(dif_rot, [0, 0, 0])
coord2 = make_coords().rotate(pi / 2.0, 'x')
dif_rot = coord1.difference_rotation(coord2, 'xm')
testing.assert_almost_equal(dif_rot, [-pi / 2.0, 0, 0])
# corner case
coord1 = make_coords()
coord2 = make_coords().rotate(0.2564565431501872, 'y')
dif_rot = coord1.difference_rotation(coord2, 'zy')
testing.assert_almost_equal(dif_rot, [0, 0, 0])
# norm == 0 case
coord1 = make_coords()
coord2 = make_coords()
dif_rot = coord1.difference_rotation(coord2, 'xy')
testing.assert_almost_equal(dif_rot, [0, 0, 0])
coord1 = make_coords()
coord2 = make_coords().rotate(pi / 2, 'x').rotate(pi / 2, 'y')
dif_rot = coord1.difference_rotation(coord2, 'xy')
testing.assert_almost_equal(dif_rot, [0, 0, pi / 2])
dif_rot = coord1.difference_rotation(coord2, 'yx')
testing.assert_almost_equal(dif_rot, [0, 0, pi / 2])
coord1 = make_coords()
coord2 = make_coords().rotate(pi / 2, 'y').rotate(pi / 2, 'z')
dif_rot = coord1.difference_rotation(coord2, 'yz')
testing.assert_almost_equal(dif_rot, [pi / 2, 0, 0])
dif_rot = coord1.difference_rotation(coord2, 'zy')
testing.assert_almost_equal(dif_rot, [pi / 2, 0, 0])
coord1 = make_coords()
coord2 = make_coords().rotate(pi / 2, 'z').rotate(pi / 2, 'x')
dif_rot = coord1.difference_rotation(coord2, 'zx')
testing.assert_almost_equal(dif_rot, [0, pi / 2, 0])
dif_rot = coord1.difference_rotation(coord2, 'xz')
testing.assert_almost_equal(dif_rot, [0, pi / 2, 0])
def test_inverse_kinematics(self):
kuka = self.kuka
move_target = kuka.rarm.end_coords
link_list = kuka.rarm.link_list
kuka.reset_manip_pose()
target_coords = kuka.rarm.end_coords.copy_worldcoords().translate(
[0.100, -0.100, 0.100], 'local')
kuka.inverse_kinematics(target_coords,
move_target=move_target,
link_list=link_list,
translation_axis=True,
rotation_axis=True)
dif_pos = kuka.rarm.end_coords.difference_position(target_coords, True)
dif_rot = kuka.rarm.end_coords.difference_rotation(target_coords, True)
self.assertLess(np.linalg.norm(dif_pos), 0.001)
self.assertLess(np.linalg.norm(dif_rot), np.deg2rad(1))
target_coords = kuka.rarm.end_coords.copy_worldcoords().\
rotate(- np.pi / 6.0, 'y', 'local')
for rotation_axis in [
True, 'x', 'y', 'z', 'xx', 'yy', 'zz', 'xm', 'ym', 'zm'
]:
kuka.reset_manip_pose()
kuka.inverse_kinematics(target_coords,
move_target=move_target,
link_list=link_list,
translation_axis=True,
rotation_axis=rotation_axis)
dif_pos = kuka.rarm.end_coords.difference_position(
target_coords, True)
dif_rot = kuka.rarm.end_coords.difference_rotation(
target_coords, rotation_axis)
self.assertLess(np.linalg.norm(dif_pos), 0.001)
self.assertLess(np.linalg.norm(dif_rot), np.deg2rad(1))
# ik failed case
av = kuka.reset_manip_pose()
target_coords = kuka.rarm.end_coords.copy_worldcoords().\
translate([10000, 0, 0], 'local')
ik_result = kuka.inverse_kinematics(target_coords,
move_target=move_target,
link_list=link_list,
translation_axis=True,
rotation_axis=True)
self.assertEqual(ik_result, False)
testing.assert_array_equal(av, kuka.angle_vector())
# inverse kinematics with linear joint
fetch = self.fetch
fetch.reset_manip_pose()
target_coords = make_coords(pos=[1.0, 0, 1.5])
fetch.inverse_kinematics(target_coords,
move_target=fetch.rarm.end_coords,
link_list=[fetch.torso_lift_link] +
fetch.rarm.link_list,
stop=200)
dif_pos = fetch.rarm.end_coords.difference_position(
target_coords, True)
dif_rot = fetch.rarm.end_coords.difference_rotation(
target_coords, True)
self.assertLess(np.linalg.norm(dif_pos), 0.001)
self.assertLess(np.linalg.norm(dif_rot), np.deg2rad(1))
