def pose_msg2homogeneous(pose):
trans = transformations.translation_matrix(
(pose.position.x, pose.position.y, pose.position.z))
rot = transformations.quaternion_matrix(
(pose.orientation.x, pose.orientation.y, pose.orientation.z,
pose.orientation.w))
return transformations.concatenate_matrices(trans, rot)
def testTransform():
'''
test transformFromPose --> getAxesFromTransform is same as quat --> matrix
'''
quat = transformations.random_quaternion()
pos = np.random.rand(3)
frame = transformUtils.transformFromPose(pos, quat)
axes = transformUtils.getAxesFromTransform(frame)
mat = transformUtils.getNumpyFromTransform(frame)
assert np.allclose(mat[:3, :3], np.array(axes).transpose())
mat2 = transformations.quaternion_matrix(quat)
mat2[:3, 3] = pos
print mat
print mat2
assert np.allclose(mat, mat2)
def testEulerBotpy():
'''
Test some quaternion and euler conversions with botpy
'''
quat = transformations.random_quaternion()
rpy = transformations.euler_from_quaternion(quat)
rpy2 = botpy.quat_to_roll_pitch_yaw(quat)
quat2 = botpy.roll_pitch_yaw_to_quat(rpy)
mat = transformations.quaternion_matrix(quat)
frame = transformUtils.getTransformFromNumpy(mat)
rpy3 = transformUtils.rollPitchYawFromTransform(frame)
print quat, quat2
print rpy, rpy2, rpy3
assert isQuatEqual(quat, quat2)
assert np.allclose(rpy, rpy2)
assert np.allclose(rpy2, rpy3)
def pose_msg2homogeneous(pose): trans = transformations.translation_matrix((pose.position.x, pose.position.y, pose.position.z)) rot = transformations.quaternion_matrix((pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w)) return transformations.concatenate_matrices(trans, rot)
def isQuatEqual(quatA, quatB):
matA = transformations.quaternion_matrix(quatA)
matB = transformations.quaternion_matrix(quatB)
return transformations.is_same_transform(matA, matB)
