Exemplo n.º 1
0
Arquivo: param.py Projeto: c-l/planopt
 def get_axises(self, pose):
     Rz, Ry, Rx = _axis_rot_matrices(pose)
     axis_x = np.dot(Rz, np.dot(Ry, [1,0,0]))
     # axis_y = np.dot(Rx, [0,1,0])
     axis_y = np.dot(Rz, [0,1,0])
     # axis_z = np.dot(np.dot(Ry, Rx), [0,0,1])
     axis_z = [0,0,1]
     # ipdb.set_trace()
     # return (axis_z, axis_y, [1,0,0])
     return (axis_z, axis_y, axis_x)
Exemplo n.º 2
0
def test_obj_pose():
    from transformations import rotation_matrix, concatenate_matrices, euler_from_matrix
    from utils import _axis_rot_matrices, _ypr_from_rot_matrix
    alpha, beta, gamma = 0.123, -1.234, 2.345
    origin, xaxis, yaxis, zaxis = (0, 0, 0), (1, 0, 0), (0, 1, 0), (0, 0, 1)
    # I = identity_matrix()
    Rx_tf = rotation_matrix(gamma, xaxis)
    Ry_tf = rotation_matrix(beta, yaxis)
    Rz_tf = rotation_matrix(alpha, zaxis)

    obj = Obj("obj")
    Rz, Ry, Rx = _axis_rot_matrices([alpha, beta, gamma, 0., 0., 0.])
    assert np.allclose(Rx_tf[:3,:3], Rx)
    assert np.allclose(Ry_tf[:3,:3], Ry)
    assert np.allclose(Rz_tf[:3,:3], Rz)
    R = concatenate_matrices(Rz_tf, Ry_tf, Rx_tf)
    rot_mat = np.dot(Rz, np.dot(Ry, Rx))
    euler = euler_from_matrix(R, 'sxyz')
    assert np.allclose(R[:3,:3], rot_mat)
    assert np.allclose([gamma, beta, alpha], euler)
    assert np.allclose([alpha, beta, gamma], _ypr_from_rot_matrix(R[:3,:3]))