Python transformations.quaternion_matrix函数代码示例

    def predictSinglePose(self, armName, curPose, prevPose, dt=1.0):
        if dt <= 0:
            print 'Error: Illegal timestamp'
            return None

        # Convert pose to numpy matrix
        curTrans = tft.translation_matrix([curPose.position.x, curPose.position.y, curPose.position.z])
        curRot = tft.quaternion_matrix([curPose.orientation.x, curPose.orientation.y ,curPose.orientation.z, curPose.orientation.w])
        curPoseMatrix = np.dot(curTrans, curRot)

        prevTrans = tft.translation_matrix([prevPose.position.x, prevPose.position.y, prevPose.position.z])
        prevRot = tft.quaternion_matrix([prevPose.orientation.x, prevPose.orientation.y ,prevPose.orientation.z, prevPose.orientation.w])
        prevPoseMatrix = np.dot(prevTrans, prevRot)
        
        deltaPoseMatrix = np.linalg.inv(prevPoseMatrix).dot(curPoseMatrix)
        deltaAngles = euler_from_matrix(deltaPoseMatrix[:3,:3])
        deltaPos = deltaPoseMatrix[:3,3]

        #deltaAngles = np.array([a / dt for a in deltaAngles])
        deltaPos = deltaPos / dt
        #deltaPoseMatrix = euler_matrix(deltaAngles[0], deltaAngles[1], deltaAngles[2])
        deltaPoseMatrix[:3,3] = deltaPos

        gpList, sysList = self.predict(armName, [curPoseMatrix], [deltaPoseMatrix])
        return tfx.pose(gpList[0], frame=curPose.frame, stamp=curPose.stamp), tfx.pose(sysList[0], frame=curPose.frame, stamp=curPose.stamp)

    def publish_state(self, t):
        state_msg = TransformStamped()
        t_ned_imu = tft.translation_matrix(self.model.get_position())
        R_ned_imu = tft.quaternion_matrix(self.model.get_orientation())
        T_ned_imu = tft.concatenate_matrices(t_ned_imu, R_ned_imu)
        #rospy.loginfo("T_ned_imu = \n" + str(T_ned_imu))
        if self.T_imu_vicon is None:
            # grab the static transform from imu to vicon frame from param server:
            frames = rospy.get_param("frames", None)
            ypr = frames['vicon_to_imu']['rotation']
            q_vicon_imu = tft.quaternion_from_euler(*[radians(x) for x in ypr], axes='rzyx') # xyzw
            R_vicon_imu = tft.quaternion_matrix(q_vicon_imu)
            t_vicon_imu = tft.translation_matrix(frames['vicon_to_imu']['translation'])
#            rospy.loginfo(str(R_vicon_imu))
#            rospy.loginfo(str(t_vicon_imu))
            self.T_vicon_imu = tft.concatenate_matrices(t_vicon_imu, R_vicon_imu)
            self.T_imu_vicon = tft.inverse_matrix(self.T_vicon_imu)
            self.T_enu_ned = tft.euler_matrix(radians(90), 0, radians(180), 'rzyx')
            rospy.loginfo(str(self.T_enu_ned))
            rospy.loginfo(str(self.T_imu_vicon))
            #rospy.loginfo(str(T_vicon_imu))
        # we have /ned -> /imu, need to output /enu -> /vicon:
        T_enu_vicon = tft.concatenate_matrices(self.T_enu_ned, T_ned_imu, self.T_imu_vicon )
        state_msg.header.stamp  = t
        state_msg.header.frame_id = "/enu"
        state_msg.child_frame_id = "/simflyer1/flyer_vicon"
        stt = state_msg.transform.translation
        stt.x, stt.y, stt.z = T_enu_vicon[:3,3]
        stro = state_msg.transform.rotation
        stro.x, stro.y, stro.z, stro.w = tft.quaternion_from_matrix(T_enu_vicon)
        
        self.pub_state.publish(state_msg)

    def Update(self, timeout=10):
        marker_message = rospy.wait_for_message(self.marker_topic,
                                                MarkerArray,
                                                timeout=timeout)
        
        added_kinbodies = []
        updated_kinbodies = []
        
        for marker in marker_message.markers:
            if marker.ns in self.marker_data:
                kinbody_file, kinbody_offset = self.marker_data[marker.ns]
                kinbody_offset = numpy.array(kinbody_offset)
                marker_pose = numpy.array(quaternion_matrix([
                        marker.pose.orientation.x,
                        marker.pose.orientation.y,
                        marker.pose.orientation.z,
                        marker.pose.orientation.w]))
                marker_pose[0,3] = marker.pose.position.x
                marker_pose[1,3] = marker.pose.position.y
                marker_pose[2,3] = marker.pose.position.z
                
                self.listener.waitForTransform(
                        self.detection_frame,
                        self.destination_frame,
                        rospy.Time(),
                        rospy.Duration(timeout))
                frame_trans, frame_rot = self.listener.lookupTransform(
#                        self.detection_frame,
                        self.destination_frame,
                        self.detection_frame,
                        rospy.Time(0))
                frame_offset = numpy.matrix(quaternion_matrix(frame_rot))
                frame_offset[0,3] = frame_trans[0]
                frame_offset[1,3] = frame_trans[1]
                frame_offset[2,3] = frame_trans[2]
                
                kinbody_pose = numpy.array(numpy.dot(numpy.dot(frame_offset,marker_pose),
                                                     kinbody_offset))
                
                kinbody_name = kinbody_file.replace('.kinbody.xml', '')
                kinbody_name = kinbody_name + str(marker.id)
                
                # load the object if it does not exist
                if self.env.GetKinBody(kinbody_name) is None:
                    new_body = prpy.rave.add_object(
                            self.env,
                            kinbody_name,
                            os.path.join(self.kinbody_directory, kinbody_file))
                    added_kinbodies.append(new_body)
                    self.generated_bodies.append(new_body)
                
                body = self.env.GetKinBody(kinbody_name)
                body.SetTransform(kinbody_pose)
                updated_kinbodies.append(body)
        
        return added_kinbodies, updated_kinbodies

	def object_pose_callback(self,msg):
		print 'object pose cb'
		(tf_trans,tf_rot) = self.pr2.tf_listener.lookupTransform(msg.header.frame_id,self.base_frame,rospy.Time(0))
		msg_tf = numpy.mat(numpy.dot(tft.translation_matrix(tf_trans),tft.quaternion_matrix(tf_rot)))
		
		q = numpy.array([msg.pose.orientation.x,msg.pose.orientation.y,msg.pose.orientation.z,msg.pose.orientation.w])
		p = numpy.array([msg.pose.position.x,msg.pose.position.y,msg.pose.position.z])
		rot = numpy.mat(tft.quaternion_matrix(q))
		trans = numpy.mat(tft.translation_matrix(p))
		self.object_pose = msg_tf * trans * rot

 def update(self, dt, desired, current):
     ((desired_p, desired_o), (desired_p_dot, desired_o_dot), (desired_p_dotdot, desired_o_dotdot)), ((p, o), (p_dot, o_dot)) = desired, current
     
     world_from_body = transformations.quaternion_matrix(o)[:3, :3]
     x_dot = numpy.concatenate([
         world_from_body.dot(p_dot),
         world_from_body.dot(o_dot),
     ])
     
     world_from_desiredbody = transformations.quaternion_matrix(desired_o)[:3, :3]
     desired_x_dot = numpy.concatenate([
         world_from_body.dot(desired_p_dot),
         world_from_body.dot(desired_o_dot),
     ])
     desired_x_dotdot = numpy.concatenate([
         world_from_body.dot(desired_p_dotdot),
         world_from_body.dot(desired_o_dotdot),
     ])
     
     error_position_world = numpy.concatenate([
         desired_p - p,
         quat_to_rotvec(transformations.quaternion_multiply(
             desired_o,
             transformations.quaternion_inverse(o),
         )),
     ])
     
     world_from_body2 = numpy.zeros((6, 6))
     world_from_body2[:3, :3] = world_from_body2[3:, 3:] = world_from_body
     body_gain = lambda x: world_from_body2.dot(x).dot(world_from_body2.T)
     
     
     error_velocity_world = (desired_x_dot + body_gain(numpy.diag(self.config['k'])).dot(error_position_world)) - x_dot
     
     pd_output = body_gain(numpy.diag(self.config['ks'])).dot(error_velocity_world)
     
     output = pd_output
     if self.config['use_rise']:
         rise_term_int = body_gain(numpy.diag(self.config['ks'] * self.config['alpha'])).dot(error_velocity_world) + \
                         body_gain(numpy.diag(self.config['beta'])).dot(numpy.sign(error_velocity_world))
         
         self._rise_term = self._rise_term + dt/2*(rise_term_int + self._rise_term_int_prev)
         self._rise_term_int_prev = rise_term_int
         
         output = output + self._rise_term
     else:
         # zero rise term so it doesn't wind up over time
         self._rise_term = numpy.zeros(6)
         self._rise_term_int_prev = numpy.zeros(6)
     output = output + body_gain(numpy.diag(self.config['accel_feedforward'])).dot(desired_x_dotdot)
     output = output + body_gain(numpy.diag(self.config['vel_feedforward'])).dot(desired_x_dot)
     
     wrench_from_vec = lambda output: (world_from_body.T.dot(output[0:3]), world_from_body.T.dot(output[3:6]))
     return wrench_from_vec(pd_output), wrench_from_vec(output)

    def recalculate_transform(self, currentTime):
        """
        Creates updated transform from /odom to /map given recent odometry and
        laser data.
        
        :Args:
            | currentTime (rospy.Time()): Time stamp for this update
         """
        
        transform = Transform()

        T_est = transformations.quaternion_matrix([self.estimatedpose.pose.pose.orientation.x,
                                                   self.estimatedpose.pose.pose.orientation.y,
                                                   self.estimatedpose.pose.pose.orientation.z,
                                                   self.estimatedpose.pose.pose.orientation.w])
        T_est[0, 3] = self.estimatedpose.pose.pose.position.x
        T_est[1, 3] = self.estimatedpose.pose.pose.position.y
        T_est[2, 3] = self.estimatedpose.pose.pose.position.z
        
        T_odom = transformations.quaternion_matrix([self.last_odom_pose.pose.pose.orientation.x,
                                                   self.last_odom_pose.pose.pose.orientation.y,
                                                   self.last_odom_pose.pose.pose.orientation.z,
                                                   self.last_odom_pose.pose.pose.orientation.w])
        T_odom[0, 3] = self.last_odom_pose.pose.pose.position.x
        T_odom[1, 3] = self.last_odom_pose.pose.pose.position.y
        T_odom[2, 3] = self.last_odom_pose.pose.pose.position.z
        T = np.dot(T_est, np.linalg.inv(T_odom))
        q = transformations.quaternion_from_matrix(T) #[:3, :3])

        transform.translation.x = T[0, 3] 
        transform.translation.y = T[1, 3] 
        transform.translation.z = T[2, 3] 
        transform.rotation.x = q[0]
        transform.rotation.y = q[1]
        transform.rotation.z = q[2]
        transform.rotation.w = q[3]
        

        # Insert new Transform into a TransformStamped object and add to the
        # tf tree
        new_tfstamped = TransformStamped()
        new_tfstamped.child_frame_id = "/odom"
        new_tfstamped.header.frame_id = "/map"
        new_tfstamped.header.stamp = currentTime
        new_tfstamped.transform = transform

        # Add the transform to the list of all transforms
        self.tf_message = tfMessage(transforms=[new_tfstamped])

    def cb(data):
        # rospy.loginfo("{0}".format(data))
        stamp = data.header.stamp
        tf_listener.waitForTransform(world_frame, link_frame, stamp, rospy.Duration(4.0))

        source_frame = world_frame
        target_frame = link_frame
        tf_link = tf_listener.lookupTransform(source_frame, target_frame, stamp)
        T_link = transformations.quaternion_matrix(tf_link[1])
        T_link[:3, 3] = tf_link[0]

        pose_chessboard = data.pose
        T_chessboard = pose_matrix(pose_chessboard)
        # T_chessboard =  numpy.linalg.inv(T_chessboard)

        yaml_data.append({"T_chessboard": sum(T_chessboard.tolist(), []), "T_link": sum(T_link.tolist(), [])})

        yf = open(out_fn, "w")
        yaml.dump(yaml_data, yf)

        print len(yaml_data)
        print stamp
        print T_chessboard
        print T_link
        print "---"
        yf.close()

def get_tag_obj_transforms(input_markers):

    output_markers = MarkerArray()
    tag_detections = {}

    for marker in input_markers.markers:
        tag_id = marker.id
        position_data = marker.pose.position
        orientation_data = marker.pose.orientation

        tag_pose = numpy.matrix(quaternion_matrix([orientation_data.x,
                                                   orientation_data.y,
                                                   orientation_data.z,
                                                   orientation_data.w]))
        tag_pose[0,3] = position_data.x
        tag_pose[1,3] = position_data.y
        tag_pose[2,3] = position_data.z

        tag_detections[tag_id] = tag_pose
    

    #Setup the data structure to dump into config file
    # config_file_data = {}
    # config_file_data['objects'] = {}
    # config_file_data['objects'][OBJECT_TO_ENTER] = {}
    # config_file_data['objects'][OBJECT_TO_ENTER]['tags'] = {}
    if TAG_AT_CENTRE in tag_detections:
        transform = tag_detections[TAG_AT_CENTRE]
        tag_transforms = {}
        for tag in tag_detections:
            tag_transforms[tag] = numpy.linalg.inv(tag_detections[tag])*transform

        print (tag_transforms)

def box_array_cb(box_array):
    polygon_array = PolygonArray()
    model_coefficients_array = ModelCoefficientsArray()
    for box in box_array.boxes:
        polygon_stamped = PolygonStamped()
        coefficients = ModelCoefficients()
        quaternion = np.array([box.pose.orientation.x, box.pose.orientation.y, box.pose.orientation.z, box.pose.orientation.w])
        rotation_matrix = transformations.quaternion_matrix(quaternion)
        ux = numpy.array([rotation_matrix[0][0], rotation_matrix[1][0], rotation_matrix[2][0]])
        uy = numpy.array([rotation_matrix[0][1], rotation_matrix[1][1], rotation_matrix[2][1]])
        uz = numpy.array([rotation_matrix[0][2], rotation_matrix[1][2], rotation_matrix[2][2]])
        dim_x = box.dimensions.x/2
        dim_y = box.dimensions.y/2
        dim_z = box.dimensions.z/2
        point = box.pose.position
        for x, y in [[-dim_x, -dim_y], [dim_x, -dim_y], [dim_x, dim_y], [-dim_x, dim_y]]:
            polygon_stamped.polygon.points.append(Point32(x*ux[0]+y*uy[0]-dim_z*uz[0]+point.x,x*ux[1]+y*uy[1]-dim_z*uz[1]+point.y,x*ux[2]+y*uy[2]-dim_z*uz[2]+point.z))
        polygon_stamped.header = box.header
        polygon_array.polygons.append(polygon_stamped)
        coefficients.values = [-uz[0], -uz[1], -uz[2], ((point.x-dim_z*uz[1])*uz[0]+(point.y-dim_z*uz[1])*uz[1]+(point.z-dim_z*uz[2])*uz[2])]
        coefficients.header = box.header
        model_coefficients_array.coefficients.append(coefficients)
    polygon_array.header = box_array.header
    PArrayPub.publish(polygon_array)
    model_coefficients_array.header = box_array.header
    MArrayPub.publish(model_coefficients_array)

def transformFt2Global(ftlist):
    global listener
    # transform ft data to global frame
    (pos_trasform, ori_trasform) = lookupTransform('base_link', 'link_ft', listener)
    rotmat = tfm.quaternion_matrix(ori_trasform)
    ft_global = np.dot(rotmat, ftlist + [1.0])
    return ft_global[0:3].tolist()

    def detect(self, timeout=10):
        marker_message = rospy.wait_for_message(self.marker_topic, MarkerArray, timeout=timeout)
        camera_message = rospy.wait_for_message(self.camera_topic, CameraInfo, timeout=timeout)
        camera_matrix = np.array([0])
        for marker in marker_message.markers:
            if marker.id == self.marker_number:
                marker_pose = np.array(
                    quaternion_matrix(
                        [
                            marker.pose.orientation.x,
                            marker.pose.orientation.y,
                            marker.pose.orientation.z,
                            marker.pose.orientation.w,
                        ]
                    )
                )
                marker_pose[0, 3] = marker.pose.position.x
                marker_pose[1, 3] = marker.pose.position.y
                marker_pose[2, 3] = marker.pose.position.z
                marker_pose = np.delete(marker_pose, 3, 0)

                camera_intrinsics = np.array(camera_message.K).reshape(3, 3)

                print camera_intrinsics
                print marker_pose
                camera_matrix = np.dot(camera_intrinsics, marker_pose)
                print camera_matrix
        return camera_matrix

    def execute(self, userdata):


        userdata.marker_pose.header.stamp = rospy.Time.now()
        pose = self.tf.transformPose('/base_link', userdata.marker_pose)
        p = [pose.pose.position.x, pose.pose.position.y, pose.pose.position.z]
        q = [pose.pose.orientation.x, pose.pose.orientation.y, pose.pose.orientation.z, pose.pose.orientation.w]
        rm = tfs.quaternion_matrix(q)
        # assemble a new coordinate frame that has z-axis aligned with
        # the vertical direction and x-axis facing the z-axis of the
        # original frame
        z = rm[:3, 2]
        z[2] = 0
        axis_x = tfs.unit_vector(z)
        axis_z = tfs.unit_vector([0, 0, 1])
        axis_y = np.cross(axis_x, axis_z)
        rm = np.vstack((axis_x, axis_y, axis_z)).transpose()
        # shift the pose along the x-axis
        p += np.dot(rm, [self.DISTANCE, 0, 0])[:3]
        userdata.goal_pose = pose
        userdata.goal_pose.pose.position.x = p[0]
        userdata.goal_pose.pose.position.y = p[1]
        userdata.goal_pose.pose.position.z = p[2]
        yaw = tfs.euler_from_matrix(rm)[2]
        q = tfs.quaternion_from_euler(0, 0, yaw - math.pi)
        userdata.goal_pose.pose.orientation.x = q[0]
        userdata.goal_pose.pose.orientation.y = q[1]
        userdata.goal_pose.pose.orientation.z = q[2]
        userdata.goal_pose.pose.orientation.w = q[3]
        return 'succeeded'

 def computePlaceToBaseMatrix(self, place):
     place_quaternion = place[3:]
     rotation_matrix = quaternion_matrix(place_quaternion)
     translation = place[0:3]
     rotation_matrix[[0, 1, 2], 3] = translation
     place_to_base_matrix = rotation_matrix
     return place_to_base_matrix

    def process_wrench(self, msg):
        cur_wrench = np.mat([msg.wrench.force.x, 
                             msg.wrench.force.y, 
                             msg.wrench.force.z, 
                             msg.wrench.torque.x, 
                             msg.wrench.torque.y, 
                             msg.wrench.torque.z]).T
        try:
            (ft_pos, ft_quat) = self.tf_list.lookupTransform(self.gravity_frame, 
                                                             self.netft_frame, rospy.Time(0))
        except:
            return
        rot_mat = np.mat(tf_trans.quaternion_matrix(ft_quat))[:3,:3]
        z_grav = self.react_mult * rot_mat.T * np.mat([0, 0, -1.]).T
        force_grav = np.mat(np.zeros((6, 1)))
        force_grav[:3, 0] = self.mass * g * z_grav
        torque_grav = np.mat(np.zeros((6, 1)))
        torque_grav[3:, 0] = np.mat(np.cross(self.com_pos.T.A[0], force_grav[:3, 0].T.A[0])).T
        zeroing_wrench = force_grav + torque_grav + self.wrench_zero
        zeroed_wrench = self.react_mult * (cur_wrench - zeroing_wrench)
        
        if not self.got_zero:
            self.wrench_zero = (cur_wrench - (force_grav + torque_grav))
            self.got_zero = True

        tf_zeroed_wrench = self.transform_wrench(zeroed_wrench)
        if tf_zeroed_wrench is None:
            return
        zero_msg = WrenchStamped(msg.header, 
                                 Wrench(Vector3(*tf_zeroed_wrench[:3,0]), Vector3(*tf_zeroed_wrench[3:,0])))
        self.zero_pub.publish(zero_msg)
        self.visualize_wrench(tf_zeroed_wrench)

	def localCb(self, data):
		self.localPose.setPoseStamped(data)
		
		if(not (self.enuTickPose.isNone() or self.offset is None)):
			t = self.localPose.getTranslation()
			q = self.enuTickPose.getQuaternion()
			
			q = quaternion_matrix(q)
			t = translation_matrix(t)
		
			self.localEnuPose.setMatrix(numpy.dot(q,t))
			
			t = self.localEnuPose.getTranslation()
			
			t[0] -= self.offset[0]
			t[1] -= self.offset[1]
			t[2] -= self.offset[2]
			
			q = self.localEnuPose.getQuaternion()
			
			self.localEnuPose.setTranslationQuaternion(t, q)
			
			p = PointStamped()
			p.point.x = self.offset[0]
			p.point.y = self.offset[1]
			p.point.z = self.offset[2]
			
			p.header = Header(0, rospy.rostime.get_rostime(), "world")
			
			self.offsetPub.publish(p)
			
			self.localEnuPub.publish(self.localEnuPose.getPoseStamped())