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]))