def test_rotation_angle(self):
rot = rpy_matrix(-1.220e-08, -5.195e-09, 1.333e-09)
with self.assertRaises(ValueError):
rotation_angle(rot)
rot = rpy_matrix(-1.220e-08, -5.195e-09, -1.333e-09)
with self.assertRaises(ValueError):
rotation_angle(rot)
self.assertEqual(rotation_angle(np.eye(3)), None)
def test_rpy_matrix(self):
testing.assert_almost_equal(rpy_matrix(0, 0, 0), np.eye(3))
testing.assert_almost_equal(rpy_matrix(pi / 6, pi / 5, pi / 3),
np.array([[0.700629, 0.190839, 0.687531],
[0.404508, 0.687531, -0.603054],
[-0.587785, 0.700629,
0.404508]]),
decimal=5)
testing.assert_almost_equal(
rpy_matrix(-pi, 0, pi / 2),
np.array([[-1, 0, 0], [0, 0, 1], [0, 1, 0]]))
testing.assert_almost_equal(rpy_matrix(0, 0, 0), np.eye(3))
def test__mull__(self):
trans12 = np.array([0, 0, 1])
rot12 = rpy_matrix(pi / 2.0, 0, 0)
tf12 = Transform(trans12, rot12)
trans23 = np.array([1, 0, 0])
rot23 = rpy_matrix(0, 0, pi / 2.0)
tf23 = Transform(trans23, rot23)
tf13 = tf12 * tf23
# from principle
testing.assert_almost_equal(
tf13.translation, rot23.dot(trans12) + trans23)
testing.assert_almost_equal(tf13.rotation, rot23.dot(rot12))
def test_rodrigues(self):
mat = rpy_matrix(pi / 6, pi / 5, pi / 3)
theta, axis = rotation_angle(mat)
rec_mat = rodrigues(axis, theta)
testing.assert_array_almost_equal(mat, rec_mat)
mat = rpy_matrix(-pi / 6, -pi / 5, -pi / 3)
theta, axis = rotation_angle(mat)
rec_mat = rodrigues(axis, theta)
testing.assert_array_almost_equal(mat, rec_mat)
# case of theta is None
axis = np.array([np.pi, 0, 0], 'f')
rec_mat = rodrigues(axis)
testing.assert_array_almost_equal(
rpy_angle(rec_mat)[0], np.array([0.0, 0.0, -np.pi], 'f'))
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 set_robot_config(robot_model, joint_list, av, with_base=False):
"""A utility function for setting robot state
Parameters
----------
robot_model : skrobot.model.CascadedLink
robot model
joint_list : list[skrobot.model.Joint]
joint list to be set
av : numpy.ndarray[float](n_dof,)
angle vector which has n_dof dims.
with_base : bool
If `with_base=False`, `n_dof` is the number of joints `n_joint`,
but if `with_base=True`, `n_dof = n_joint + 3`.
"""
if with_base:
assert len(joint_list) + 3 == len(av)
else:
assert len(joint_list) == len(av)
if with_base:
av_joint, av_base = av[:-3], av[-3:]
x, y, theta = av_base
co = Coordinates(pos=[x, y, 0.0], rot=rpy_matrix(theta, 0.0, 0.0))
robot_model.newcoords(co)
else:
av_joint = av
for joint, angle in zip(joint_list, av_joint):
joint.joint_angle(angle)
def test_rpy_angle(self):
a, b = rpy_angle(rpy_matrix(pi / 6, pi / 5, pi / 3))
testing.assert_almost_equal(a, np.array([pi / 6, pi / 5, pi / 3]))
testing.assert_almost_equal(
b, np.array([3.66519143, 2.51327412, -2.0943951]))
rot = np.array([[0, 0, 1], [0, -1, 0], [1, 0, 0]])
testing.assert_almost_equal(
rpy_angle(rot)[0], np.array([0, -pi / 2.0, pi]))
def test_inverse_transformation(self):
# this also checks __mull__
trans = np.array([1, 1, 1])
rot = rpy_matrix(pi / 2.0, pi / 3.0, pi / 5.0)
tf = Transform(trans, rot)
tf_inv = tf.inverse_transformation()
tf_id = tf * tf_inv
testing.assert_almost_equal(tf_id.translation, np.zeros(3))
def test_rotation_distance(self):
mat1 = np.eye(3)
mat2 = np.eye(3)
diff_theta = rotation_distance(mat1, mat2)
self.assertEqual(diff_theta, 0.0)
mat1 = rpy_matrix(0, 0, np.pi)
mat2 = np.eye(3)
diff_theta = rotation_distance(mat1, mat2)
self.assertEqual(diff_theta, np.pi)
def test_rotation_matrix_from_rpy(self):
testing.assert_almost_equal(
rotation_matrix_from_rpy([-pi, 0, pi / 2]),
np.array([[-1, 0, 0], [0, 0, 1], [0, 1, 0]]))
testing.assert_almost_equal(rotation_matrix_from_rpy([0, 0, 0]),
np.eye(3))
# rotation_matrix_from_rpy and rpy_matrix should be same.
for i in range(100):
r = np.random.random()
p = np.random.random()
y = np.random.random()
testing.assert_almost_equal(rpy_matrix(y, p, r),
rotation_matrix_from_rpy([y, p, r]))
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])
import skrobot
import numpy as np
from skrobot.coordinates.math import rpy_matrix
from skrobot.coordinates import Coordinates
robot_model = skrobot.models.urdf.RobotModelFromURDF(
urdf_file=skrobot.data.pr2_urdfpath())
rarm_end_coords = skrobot.coordinates.CascadedCoords(
parent=robot_model.r_gripper_tool_frame, name='rarm_end_coords')
print(rarm_end_coords.worldpos())
co = Coordinates(pos=[1.0, 1.0, 1.0], rot=rpy_matrix(0.3, 0.3, 0.3))
robot_model.newcoords(co)
print(rarm_end_coords.worldpos())
mylink = Link(pos=[0.1, 0.1, 0.1], rot=[0.1, 0.2, 0.3], name="mylink")
robot_model.r_upper_arm_link.assoc(mylink, mylink)
link_list = [
robot_model.r_shoulder_pan_link, robot_model.r_shoulder_lift_link,
robot_model.r_upper_arm_roll_link, robot_model.r_elbow_flex_link,
robot_model.r_forearm_roll_link, robot_model.r_wrist_flex_link,
robot_model.r_wrist_roll_link, robot_model.base_link,
robot_model.r_upper_arm_link, mylink
]
joint_angles = [0.564, 0.35, -0.74, -0.7, -0.7, -0.17, -0.63]
for j, a in zip(joint_list, joint_angles):
j.joint_angle(a)
x, y, theta = 0.3, 0.5, 0.7
co = Coordinates(pos=[x, y, 0.0], rot=rpy_matrix(theta, 0.0, 0.0))
robot_model.newcoords(co)
angle_vector = joint_angles + [x, y, theta]
# compute pose of links
world_coordinate = CascadedCoords()
def extract_pose(co):
return np.hstack((co.worldpos(), co.worldcoords().rpy_angle()[0]))
pose_list = [extract_pose(co).tolist() for co in link_list]
# compute jacobian of links
def test_rpy_angle(self):
a, b = rpy_angle(rpy_matrix(pi / 6, pi / 5, pi / 3))
testing.assert_almost_equal(a, np.array([pi / 6, pi / 5, pi / 3]))
testing.assert_almost_equal(
b, np.array([3.66519143, 2.51327412, -2.0943951]))
