def forceMomentTransformation(inputFrame, outputFrame):
"""
Utility getting the force-moment transformation for converting wrenches from
one frame to another. Ff wrench is 6 x 1 (moment, force) expressed in input frame,
then corresponding wrench in output frame would be numpy.dot(FM, wrench) = wrench_in_output_frame.
:param inputFrame: A vtkTransform defining the input frame
:param outputFrame: A vtkTransform defining the output frame
:return: 4x4 matrix that transform a wrench from the input frame to the output frame
"""
FM = np.zeros((6, 6))
inputToOutputFrame = copyFrame(inputFrame)
inputToOutputFrame.PostMultiply()
inputToOutputFrame.Concatenate(outputFrame.GetLinearInverse())
position, quaternion = poseFromTransform(inputToOutputFrame)
inputToOutputRotationMatrix = transformations.quaternion_matrix(
quaternion)[:3, :3]
FM[0:3, 0:3] = inputToOutputRotationMatrix
FM[3:, 3:] = inputToOutputRotationMatrix
cross = crossProductMatrix(position)
FM[0:3, 3:] = np.dot(cross, inputToOutputRotationMatrix)
return FM
def transformFromPose(position, quaternion):
'''
Returns a vtkTransform
'''
mat = transformations.quaternion_matrix(quaternion)
mat[:3, 3] = position
return getTransformFromNumpy(mat)
def forceMomentTransformation(inputFrame, outputFrame):
"""
Utility getting the force-moment transformation for converting wrenches from
one frame to another. Ff wrench is 6 x 1 (moment, force) expressed in input frame,
then corresponding wrench in output frame would be numpy.dot(FM, wrench) = wrench_in_output_frame.
:param inputFrame: A vtkTransform defining the input frame
:param outputFrame: A vtkTransform defining the output frame
:return: 4x4 matrix that transform a wrench from the input frame to the output frame
"""
FM = np.zeros((6,6))
inputToOutputFrame = copyFrame(inputFrame)
inputToOutputFrame.PostMultiply()
inputToOutputFrame.Concatenate(outputFrame.GetLinearInverse())
position, quaternion = poseFromTransform(inputToOutputFrame)
inputToOutputRotationMatrix = transformations.quaternion_matrix(quaternion)[:3,:3]
FM[0:3, 0:3] = inputToOutputRotationMatrix
FM[3:,3:] = inputToOutputRotationMatrix
cross = crossProductMatrix(position)
FM[0:3,3:] = np.dot(cross,inputToOutputRotationMatrix)
return FM
def transformFromPose(position, quaternion):
'''
Returns a vtkTransform
'''
mat = transformations.quaternion_matrix(quaternion)
mat[:3,3] = position
return getTransformFromNumpy(mat)
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 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 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 isQuatEqual(quatA, quatB):
matA = transformations.quaternion_matrix(quatA)
matB = transformations.quaternion_matrix(quatB)
return transformations.is_same_transform(matA, matB)
def isQuatEqual(quatA, quatB):
matA = transformations.quaternion_matrix(quatA)
matB = transformations.quaternion_matrix(quatB)
return transformations.is_same_transform(matA, matB)
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)
