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