test_grasp.py

Go to the documentation of this file.

#!/usr/bin/env python

# set pr2 to a pose for grasping

PKG = 'pr2_pickup_object_demo'

import roslib; roslib.load_manifest(PKG)
import sys
import rospy
import os
import numpy
import time
from sensor_msgs.msg import LaserScan, Image, PointCloud
from gazebo.msg import *
from gazebo.srv import *
from geometry_msgs.msg import Point, Pose, Quaternion, Twist, Wrench, Vector3


import actionlib
from trajectory_msgs.msg import *
from kinematics_msgs.msg import *
from kinematics_msgs.srv import *
from pr2_controllers_msgs.msg import *
from pr2_controllers_msgs.srv import *

import rospy
#   header: 
#     seq: 10043
#     stamp: 
#       secs: 100
#       nsecs: 764000000
#     frame_id: ''
#   name: ['fl_caster_rotation_joint', 'fl_caster_l_wheel_joint', 'fl_caster_r_wheel_joint', 'fr_caster_rotation_joint', 'fr_caster_l_wheel_joint', 'fr_caster_r_wheel_joint', 'bl_caster_rotation_joint', 'bl_caster_l_wheel_joint', 'bl_caster_r_wheel_joint', 'br_caster_rotation_joint', 'br_caster_l_wheel_joint', 'br_caster_r_wheel_joint', 'head_pan_joint', 'head_tilt_joint', 'laser_tilt_mount_joint', 'r_upper_arm_roll_joint', 'r_shoulder_pan_joint', 'r_shoulder_lift_joint', 'r_forearm_roll_joint', 'r_elbow_flex_joint', 'r_wrist_flex_joint', 'r_wrist_roll_joint', 'r_gripper_joint', 'r_gripper_l_finger_tip_joint', 'r_gripper_r_finger_tip_joint', 'r_gripper_l_finger_joint', 'r_gripper_r_finger_joint', 'l_upper_arm_roll_joint', 'l_shoulder_pan_joint', 'l_shoulder_lift_joint', 'l_forearm_roll_joint', 'l_elbow_flex_joint', 'l_wrist_flex_joint', 'l_wrist_roll_joint', 'l_gripper_joint', 'l_gripper_l_finger_tip_joint', 'l_gripper_r_finger_tip_joint', 'l_gripper_l_finger_joint', 'l_gripper_r_finger_joint']
#   position: [-1.2374515368840378e-05, 13.849011208135753, 13.854646834822727, -2.0182888337494376e-05, 13.867926659094984, 13.87465036000275, -3.0273366892430431e-05, 13.798391413070624, 13.803469733109218, -8.9169276339617909e-05, 13.763956446381279, 13.785982540514944, -0.028014984291522715, 0.522937305713703, 0.23430921400735369, 0.00031428941291178347, -0.40039691465768534, 1.0001709932508449, -0.0016722329173761707, -2.0498051406658426, -0.10004495846217587, -0.0011961845468899668, 0.021801808285971121, 0.01090090414298556, -0.01090090414298556, 0.17441446628776897, -0.17441446628776897, 0.63569515764238282, 0.77977415758334701, 1.0364984164611073, -3.9495641888643505, -1.7985134583697606, -0.28943329621010871, 3.6907943582700078, 0.085993919065644644, 0.042996959532822322, -0.042996959532822322, 0.68795135252515716, -0.68795135252515716]
#   velocity: [-0.0015232128535343742, -0.0040391975586448773, -0.0044194835971378254, 0.0003320293140202554, 0.00023643758321290499, 0.0019627090569989025, 0.00054137492103062759, -0.0050486235200869782, -0.003401071121264383, -0.0026104110533784848, 0.00081900272215129296, -0.00041862974404447491, -0.0025013433752263396, -0.0013589969993489071, -0.98516886635338308, -0.002014049475062488, -0.0017729315393138145, -0.0014735383851937115, 0.001438797501675931, -1.8165655290624822e-05, -0.001130025859087349, -0.00041673830176579166, 2.0795385802584405e-05, 1.0397692901292203e-05, -1.0397692901292203e-05, 0.00016636308642067524, -0.00016636308642067524, 0.0011424596745216919, -0.0006822000575323516, -4.1183240355585453e-05, -0.0017336646404559018, -0.0014154375940461891, 2.463187280165929e-05, 0.0019698240389872403, 3.4160440164151104e-05, 1.7080220082075552e-05, -1.7080220082075552e-05, 0.00027328352131320883, -0.00027328352131320883]
#   effort: [0.0024740927445187876, 0.011566463875487246, 0.0086316794539931553, 0.015685307250347107, 0.0027755948521506176, 0.0014800183369710325, 0.026422425218831386, 0.0043179555843298511, 0.01536352427659657, -0.055467835158435244, 0.010990770216869028, -0.0152601416701998, 0.0021626517099224648, -1.3876620065962568, 0.080921492974234843, -0.0025236471815478314, 0.0029353377903397378, 0.00567705611571921, 0.00066284095537457377, 0.010040540671223164, 0.0046029901638574115, 0.00079873211410497712, -0.0, -0.0, 0.0, -0.0, 0.0, 0.043849783788818909, -0.082580302711595208, -0.01672623402298886, -0.0081631184410976138, -0.025819476720384517, -0.0036219894326636175, -0.0053667326151033545, 6.1492552416997359e-05, 3.074627620849868e-05, -3.074627620849868e-05, 0.00049194041933597887, -0.00049194041933597887]

def set_model_configuration_client(model_name, urdf_param_name, joint_names, joint_positions):
    print "Setting model configuration"
    rospy.wait_for_service('/gazebo/set_model_configuration')
    try:
      set_model_configuration = rospy.ServiceProxy('/gazebo/set_model_configuration',SetModelConfiguration)
      resp = set_model_configuration(model_name, urdf_param_name, joint_names, joint_positions)
      print "set configuration status: ", resp.status_message
      return resp.success
    except rospy.ServiceException, e:
      print "Service call failed: %s"%e

def set_model_state_client(model_name, pose, twist, reference_frame):
    print "Setting ",model_name," model state"
    rospy.wait_for_service('/gazebo/set_model_state')
    try:
      set_model_state = rospy.ServiceProxy('/gazebo/set_model_state',SetModelState)
      resp = set_model_state(ModelState(model_name, pose, twist, reference_frame))
      print "set state status: ", resp.status_message
      return resp.success
    except rospy.ServiceException, e:
      print "Service call failed: %s"%e

def arm_joint_trajectory_action_client(arm,trajectory):
    print "sending arm joint trajectory to ",arm
    client = actionlib.SimpleActionClient(arm,JointTrajectoryAction)
    client.wait_for_server()
    client.send_goal(trajectory)
    client.wait_for_result()
    return client.get_result()

def gripper_command_action_client(command):
    print "sending gripper command"
    client = actionlib.SimpleActionClient('l_gripper_controller/gripper_action',Pr2GripperCommandAction)
    client.wait_for_server()
    client.send_goal(command)
    client.wait_for_result()
    return client.get_result()

if __name__ == '__main__':
    rospy.init_node('gazebo_test_grasp')

    joint_names = ['fl_caster_rotation_joint', 'fl_caster_l_wheel_joint', 'fl_caster_r_wheel_joint', 'fr_caster_rotation_joint', 'fr_caster_l_wheel_joint', 'fr_caster_r_wheel_joint', 'bl_caster_rotation_joint', 'bl_caster_l_wheel_joint', 'bl_caster_r_wheel_joint', 'br_caster_rotation_joint', 'br_caster_l_wheel_joint', 'br_caster_r_wheel_joint', 'head_pan_joint', 'head_tilt_joint', 'laser_tilt_mount_joint', 'r_upper_arm_roll_joint', 'r_shoulder_pan_joint', 'r_shoulder_lift_joint', 'r_forearm_roll_joint', 'r_elbow_flex_joint', 'r_wrist_flex_joint', 'r_wrist_roll_joint', 'r_gripper_joint', 'r_gripper_l_finger_tip_joint', 'r_gripper_r_finger_tip_joint', 'r_gripper_l_finger_joint', 'r_gripper_r_finger_joint', 'l_upper_arm_roll_joint', 'l_shoulder_pan_joint', 'l_shoulder_lift_joint', 'l_forearm_roll_joint', 'l_elbow_flex_joint', 'l_wrist_flex_joint', 'l_wrist_roll_joint', 'l_gripper_joint', 'l_gripper_l_finger_tip_joint', 'l_gripper_r_finger_tip_joint', 'l_gripper_l_finger_joint', 'l_gripper_r_finger_joint']
    joint_positions = [-1.2374515368840378e-05, 13.849011208135753, 13.854646834822727, -2.0182888337494376e-05, 13.867926659094984, 13.87465036000275, -3.0273366892430431e-05, 13.798391413070624, 13.803469733109218, -8.9169276339617909e-05, 13.763956446381279, 13.785982540514944, -0.028014984291522715, 0.522937305713703, 0.23430921400735369, 0.00031428941291178347, -0.40039691465768534, 1.0001709932508449, -0.0016722329173761707, -2.0498051406658426, -0.10004495846217587, -0.0011961845468899668, 0.021801808285971121, 0.01090090414298556, -0.01090090414298556, 0.17441446628776897, -0.17441446628776897, 0.63569515764238282, 0.77977415758334701, 1.0364984164611073, -3.9495641888643505, -1.7985134583697606, -0.28943329621010871, 3.6907943582700078, 0.085993919065644644, 0.042996959532822322, -0.042996959532822322, 0.68795135252515716, -0.68795135252515716]
    #success = set_model_configuration_client("pr2","robot_description", joint_names, joint_positions)

    l_arm_joint_names = ['l_upper_arm_roll_joint', 'l_shoulder_pan_joint', 'l_shoulder_lift_joint', 'l_forearm_roll_joint', 'l_elbow_flex_joint', 'l_wrist_flex_joint', 'l_wrist_roll_joint']
    l_arm_joint_positions = [0.63569515764238282, 0.77977415758334701, 1.0364984164611073, -3.9495641888643505, -1.7985134583697606, -0.80, 3.5]
    l_arm_trajectory = JointTrajectoryGoal()
    l_arm_trajectory.trajectory.joint_names = l_arm_joint_names
    point = JointTrajectoryPoint()
    point.positions = l_arm_joint_positions
    point.velocities = numpy.zeros(len(l_arm_joint_positions))
    point.time_from_start = rospy.Duration.from_sec(5)
    l_arm_trajectory.trajectory.points.append(point)
    result = arm_joint_trajectory_action_client('l_arm_controller/joint_trajectory_action',l_arm_trajectory)
    print "arm trajectory result: ",result

    r_arm_joint_names = [ 'r_upper_arm_roll_joint', 'r_shoulder_pan_joint', 'r_shoulder_lift_joint', 'r_forearm_roll_joint', 'r_elbow_flex_joint', 'r_wrist_flex_joint', 'r_wrist_roll_joint']
    r_arm_joint_positions = [ 0.00031428941291178347, -0.40039691465768534, 1.0001709932508449, -0.0016722329173761707, -2.0498051406658426, -0.10004495846217587, -0.0011961845468899668]
    r_arm_trajectory = JointTrajectoryGoal()
    r_arm_trajectory.trajectory.joint_names = r_arm_joint_names
    point = JointTrajectoryPoint()
    point.positions = r_arm_joint_positions
    point.velocities = numpy.zeros(len(r_arm_joint_positions))
    point.time_from_start = rospy.Duration.from_sec(5)
    r_arm_trajectory.trajectory.points.append(point)
    result = arm_joint_trajectory_action_client('r_arm_controller/joint_trajectory_action',r_arm_trajectory)
    print "arm trajectory result: ",result

    gripper_command = Pr2GripperCommandGoal()
    gripper_command.command.position = 0.08
    gripper_command.command.max_effort = 100
    result = gripper_command_action_client(gripper_command)
    print "gripper command result: ",result
    time.sleep(3)


    model_name = 'pr2'
    pose = Pose(Point(1.030, 0, 0.0),Quaternion(0 ,0 ,0 , 1))
    twist = Twist(Vector3(0,0,0),Vector3(0,0,0))
    reference_frame = 'world'
    set_model_state_client(model_name, pose, twist, reference_frame)


    gripper_pub = rospy.Publisher('/l_gripper_controller/command', Pr2GripperCommand)
    gripper_command = Pr2GripperCommand()
    gripper_command.position = 0.0
    gripper_command.max_effort = 200.0
    timeout = time.time() + 5
    while (time.time() < timeout):
      gripper_pub.publish(gripper_command)
      time.sleep(1)

    model_name = 'table_1'
    pose = Pose(Point(3,0,0.01),Quaternion(0,0,0,1))
    twist = Twist(Vector3(0,0,0),Vector3(0,0,0))
    reference_frame = 'world'
    set_model_state_client(model_name, pose, twist, reference_frame)

    l_arm_joint_names = ['l_upper_arm_roll_joint', 'l_shoulder_pan_joint', 'l_shoulder_lift_joint', 'l_forearm_roll_joint', 'l_elbow_flex_joint', 'l_wrist_flex_joint', 'l_wrist_roll_joint']
    l_arm_joint_positions = [0.3, 0.8, -0.3, 1.0, -1.0, 0.0, 0.0]
    l_arm_trajectory = JointTrajectoryGoal()
    l_arm_trajectory.trajectory.joint_names = l_arm_joint_names
    point = JointTrajectoryPoint()
    point.positions = l_arm_joint_positions
    point.velocities = numpy.zeros(len(l_arm_joint_positions))
    point.time_from_start = rospy.Duration.from_sec(5)
    l_arm_trajectory.trajectory.points.append(point)
    result = arm_joint_trajectory_action_client('l_arm_controller/joint_trajectory_action',l_arm_trajectory)
    print "arm trajectory result: ",result