trajectory.py

Go to the documentation of this file.

#!/usr/bin/env python

import roslib; roslib.load_manifest('zyonz_apps_base')
import rospy
import smach
import smach_ros
import tf
import geometry_msgs
import numpy
import math

from smach import CBState
from iri_common_smach.utils_msg          import build_pose_stamped_msg
from iri_wam_smach.sm_wam_move_from_pose import SM_WAM_MoveFromPose
from geometry_msgs.msg import PoseStamped
from tf.transformations import *
from pprint import pprint

def pose_st_from_matrix(matrix, frame_id):
    """ Returns geometry_msgs.msg.Pose() from transformation.matrix

    """
    quaternion = quaternion_from_matrix(matrix)
    position = translation_from_matrix(matrix)

    pose_st = PoseStamped() 
    pose_st.header.frame_id = frame_id 
    pose_st.pose.position.x = position[0]
    pose_st.pose.position.y = position[1] 
    pose_st.pose.position.z = position[2] 
    pose_st.pose.orientation.x = quaternion[0] 
    pose_st.pose.orientation.y = quaternion[1] 
    pose_st.pose.orientation.z = quaternion[2] 
    pose_st.pose.orientation.w = quaternion[3] 
    
    return pose_st

def trajectory_around_view(pose_st, distance, number_of_poses):
    """ 
        Returns a list of poses around and arc. The arc is defined by
        the current pose, the distance to the center of the arc from the
        current pose and the number of next poses required

    """
    # This is the beginning of the FUNCTION 
    origin, xaxis, yaxis, zaxis = (0,0,0), (1,0,0), (0,1,0), (0,0,1)
    T = translation_matrix((pose_st.pose.position.x, pose_st.pose.position.y, pose_st.pose.position.z))
    R = quaternion_matrix((pose_st.pose.orientation.x, pose_st.pose.orientation.y, pose_st.pose.orientation.z, pose_st.pose.orientation.w))
    H = concatenate_matrices(T,R)
    frame_id = pose_st.header.frame_id 

    # Center of Rotation
    cr = concatenate_matrices(H, translation_matrix((0.0, 0.0, distance)))
    cr = concatenate_matrices(cr, rotation_matrix(-math.pi/2 ,zaxis), rotation_matrix(-math.pi, xaxis))
    print cr
    
    # Computing the new views around the arc
    lim_low = math.pi/4
    lim_high = -math.pi/4

    ang_y = 0
    pose_st_list = []
    """ 
        Uncomment if you want to have a sphere instead of an arc
    """
    #for ang_y in numpy.linspace(lim_low, lim_high, number_of_poses):
    for ang_x in numpy.linspace(lim_low, lim_high, number_of_poses):
        next_pose = concatenate_matrices(cr, concatenate_matrices(rotation_matrix(ang_y, yaxis), rotation_matrix(ang_x, xaxis), inverse_matrix(cr),H))
        pose_st_list.append(pose_st_from_matrix(next_pose, frame_id))
        
    return pose_st_list
 
class ZYONZGetArcTrajectory(smach.State):
    """
         
    """
    def __init__(self):
        smach.State.__init__(self, outcomes = ['success','fail'],
                                   input_keys = ['pose_st', 'distance', 'num_views'],
                                   output_keys = ['pose_st_list'])

    def execute(self, userdata):
        try:
            response = trajectory_around_view(userdata.pose_st, userdata.distance, userdata.num_views)        
        except rospy.ServiceException, e:
            return 'fail'
        
        if (not response):
            return 'fail'
        
        pprint(response)

        userdata.pose_st_list = response
        return 'success'