ik_trajectory_test.py

Go to the documentation of this file.

#!/usr/bin/env python
# Software License Agreement (BSD License)
#
# Copyright (c) 2008, Willow Garage, Inc.  All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions are met:
#
#  * Redistributions of source code must retain the above copyright notice,
#  this list of conditions and the following disclaimer.  * Redistributions in
#  binary form must reproduce the above copyright notice, this list of
#  conditions and the following disclaimer in the documentation and/or other
#  materials provided with the distribution.  * Neither the name of the Willow
#  Garage nor the names of its contributors may be used to endorse or promote
#  products derived from this software without specific prior written
#  permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
#

#test client for ik_trajectory_test

import roslib
roslib.load_manifest('pr2_plugs_gazebo_demo')
import rospy
from pr2_plugs_gazebo_demo.srv import ExecuteCartesianIKTrajectory
from geometry_msgs.msg import Pose, PoseWithCovarianceStamped
from nav_msgs.msg import Odometry
import time
import sys
import pdb
import tf
import tf.transformations as tft

import actionlib
from pr2_controllers_msgs.msg import Pr2GripperCommandAction,Pr2GripperCommandGoal

plug_p = [0,0,0]
plug_q = [0,0,0,1]
plug_e = [0,0,0]
got_plug = False

#execute a Cartesian trajectory defined by a root frame, a list of 
#positions (x,y,z), and a list of orientations (quaternions: x,y,z,w)

gripper_error = [1 1 1 1 1 1 1]

#pretty-print list to string
def gotToIKPose(position,orientation):
    ik_pub = rospy.Publisher('plugs_ik_node/pose_command', PoseWithCovarianceStamped)
    p = PoseWithCovarianceStamped()
    p.header.frame_id = 'base_link'
    p.header.stamp = rospy.get_rostime()
    p.pose.pose = Pose()
    p.pose.pose.position.x = position[0]
    p.pose.pose.position.y = position[1]
    p.pose.pose.position.z = position[2]
    p.pose.pose.orientation.x = orientation[0]
    p.pose.pose.orientation.y = orientation[1]
    p.pose.pose.orientation.z = orientation[2]
    p.pose.pose.orientation.w = orientation[3]
    
    ik_pub.publish(p)
    (trans, rot) = tf_listener.lookupTransform('base_link', 'r_wrist_roll_link', rospy.Time(0))

    #check the final pose
    (trans, rot) = tf_listener.lookupTransform('base_link', 'r_wrist_roll_link', rospy.Time(0))
    while abs(trans[0]-position[0]) > 0.001 \
       or abs(trans[1]-position[1]) > 0.001 \
       or abs(trans[2]-position[2]) > 0.001 \
       or abs(rot[0]-orientation[0]) > 0.001:
        (trans, rot) = tf_listener.lookupTransform('base_link', 'r_wrist_roll_link', rospy.Time(0))
        print "goal: ",position[0],position[1],position[2],orientation[0],orientation[1],orientation[2],orientation[3]
        print "curr: ",trans[0],trans[1],trans[2],rot[0],rot[1],rot[2],rot[3]
        time.sleep(1)

    return

def  plugP3DInput(p3d):
    global plug_p, plug_q, plug_e, got_plug
    plug_p = [p3d.pose.pose.position.x, p3d.pose.pose.position.y, p3d.pose.pose.position.z]
    plug_q = [p3d.pose.pose.orientation.x, p3d.pose.pose.orientation.y, p3d.pose.pose.orientation.z, p3d.pose.pose.orientation.w]
    plug_e = tft.euler_from_quaternion(plug_q)
    got_plug = True

#print out the positions, velocities, and efforts of the right arm joints
if __name__ == "__main__":
    rospy.init_node("test_cartesian_ik_trajectory_executer")
    rospy.Subscriber("/plug/plug_pose_ground_truth", Odometry, plugP3DInput)
    tf_listener = tf.TransformListener()

    # wait for plug pose
    while not got_plug:
      print "waiting for plug pose: ",got_plug
      time.sleep(1.) #give the transform listener time to get some frames

    # open gripper
    client = actionlib.SimpleActionClient('r_gripper_controller/gripper_action',Pr2GripperCommandAction)
    print "waiting for gripper_action server"
    client.wait_for_server()
    gripper_goal = Pr2GripperCommandGoal()
    gripper_goal.command.position = 0.08
    gripper_goal.command.max_effort = 10.0
    client.send_goal(gripper_goal)
    client.wait_for_result()

    joint_names = ["r_shoulder_pan_joint",
                   "r_shoulder_lift_joint",
                   "r_upper_arm_roll_joint",
                   "r_elbow_flex_joint",
                   "r_forearm_roll_joint",
                   "r_wrist_flex_joint",
                   "r_wrist_roll_joint"]

    # fill in targets with plug_pose - base_pose
    #positions = [[0.76, -0.2, 0.83] \
    #            ,[plug_p[0] - base_p[0],plug_p[1] - base_p[1],plug_p[2] - base_p[2]]]
    #            ,[0.76, -0.2, 0.8]]
    #positions = [[.76, -.19, .83], [0.59, -0.36, 0.93]]
    #orientations = [[.02, -.09, 0.0, 1.0], [0.65, -0.21, .38, .62]]
    #positions = [[plug_p[0] - base_p[0],plug_p[1] - base_p[1],plug_p[2] - base_p[2]]]
    #orientations = [[plug_q[0] - base_q[0],plug_q[1] - base_q[1],plug_q[2] - base_q[2],plug_q[3] - base_q[3]]]
    #orientations = [[.0, .0, 0.7071, 0.7071],[.0, .0, 0.7071, 0.7071]] # orientation for grasping
    position = [plug_p[0] ,plug_p[1] - 0.18 ,plug_p[2]-0.04 ]
    orientation = [.0, .0, 0.7071, 0.7071]
    print "goal: ",position[0],position[1],position[2],orientation[0],orientation[1],orientation[2],orientation[3]

    gotToIKPose(position,orientation)


    # close gripper
    client = actionlib.SimpleActionClient('r_gripper_controller/gripper_action',Pr2GripperCommandAction)
    client.wait_for_server()
    gripper_goal = Pr2GripperCommandGoal()
    gripper_goal.command.position = 0.0
    gripper_goal.command.max_effort = 100.0
    client.send_goal(gripper_goal)
    #client.wait_for_result()

    position = [.76, -.19, .83]
    position = [plug_p[0],plug_p[1] - 0.2 ,plug_p[2]-0.2 ]
    orientation = [.0, .0, 0.7071, 0.7071]
    orientation = [.0, .0, 0.0, 1.0]

    gotToIKPose(position,orientation)