pose.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 # take a set of joint positions from a yaml file and replay them,
00003 # or take two sets of joint positions and interpolate between them
00004 # 
00005 # This is written specifically to make photoshoots of the PR2 easier.
00006 #
00007 # One pose:
00008 #  pose.py [pose.yaml]
00009 #
00010 # Two poses:
00011 #  pose.py <1.yaml> <2.yaml> <steps>
00012 
00013 
00014 import roslib; roslib.load_manifest('pr2_joint_teleop')
00015 import rospy
00016 import actionlib
00017 import yaml
00018 import math
00019 
00020 from pr2_controllers_msgs.msg import JointTrajectoryGoal, JointTrajectoryAction
00021 from trajectory_msgs.msg import JointTrajectoryPoint
00022 from pr2_controllers_msgs.msg import Pr2GripperCommand
00023 from sensor_msgs.msg import Joy
00024 
00025 def pose_(position, joints, client):
00026    goal = JointTrajectoryGoal()
00027    goal.trajectory.joint_names = joints
00028 
00029    goal.trajectory.points = [ JointTrajectoryPoint() ]
00030 
00031    goal.trajectory.points[0].velocities = [0.0] * len(joints);
00032    goal.trajectory.points[0].positions = [0.0] * len(joints);
00033    for i, j in enumerate(joints):
00034       goal.trajectory.points[0].positions[i] = position[i]
00035 
00036    goal.trajectory.points[0].time_from_start = rospy.Duration.from_sec(1.0)
00037    client.send_goal(goal)
00038 
00039 def scale(axes, min_limit, max_limit):
00040    position = [ 0 ] * len(min_limit)
00041    for i in range(len(min_limit)):
00042       avg = (min_limit[i] + max_limit[i]) / 2.0
00043       r = (max_limit[i] - min_limit[i]) / 2.0
00044       deg = axes[i]*r + avg
00045       position[i] = deg * math.pi / 180.0
00046    return position
00047 
00048 def pose_r(axes):
00049    joints = ["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"]
00050    min_limit = [ -130, -30, -224, -130, -180, -130, -180 ]
00051    max_limit = [ 0, 80, 44, 0, 180, 0, 180 ]
00052    position = scale(axes, min_limit, max_limit)
00053    pose_(position, joints, traj_client_r)
00054 
00055 def pose_l(axes):
00056    joints = ["l_shoulder_pan_joint", "l_shoulder_lift_joint", "l_upper_arm_roll_joint", "l_elbow_flex_joint", "l_forearm_roll_joint", "l_wrist_flex_joint", "l_wrist_roll_joint"]
00057    min_limit = [0, -30, -44, -130, -180, -130, -180 ]
00058    max_limit = [130, 80, 224, 0, 180, 0, 180 ]
00059    position = scale(axes, min_limit, max_limit)
00060    pose_(position, joints, traj_client_l)
00061 
00062 def pose_head(axes):
00063    joints = [ 'head_pan_joint', 'head_tilt_joint' ]
00064    min_limit = [ -160, -30 ]
00065    max_limit = [ 160, 90 ]
00066    position = scale(axes, min_limit, max_limit)
00067    pose_(position, joints, traj_client_head)
00068 
00069 def pose_torso(position):
00070    joints = [ 'torso_lift_joint' ]
00071    pose_(position, joints, traj_client_torso)
00072 
00073 def pose_gripper_l(axes):
00074    control = Pr2GripperCommand(0.04, 0)
00075    control.max_effort = 10.0
00076    control.position = 0.08 * ((axes[0] + 1.0) / 2.0)
00077    gripper_l.publish(control)
00078 
00079 def pose_gripper_r(axes):
00080    control = Pr2GripperCommand(0.04, 0)
00081    control.max_effort = 10.0
00082    control.position = 0.08 * ((axes[0] + 1.0) / 2.0)
00083    gripper_r.publish(control)
00084 
00085 # move the robot to the specified pose
00086 def pose(position):
00087    pose_l(position)
00088    pose_r(position)
00089    pose_head(position)
00090    pose_torso(position)
00091 
00092 def TrajClient(t):
00093    c = actionlib.SimpleActionClient(t, JointTrajectoryAction)
00094    c.wait_for_server()
00095    return c
00096 
00097 def joy_callback(joy_msg):
00098    if len(joy_msg.buttons) >= 25:
00099       mode = joy_msg.buttons[24]
00100       right = []
00101       right.extend(joy_msg.axes)
00102       right[1] = -right[1]
00103       left = []
00104       left.extend(joy_msg.axes)
00105       left[0] = -left[0]
00106       left[1] = -left[1]
00107       left[2] = -left[2]
00108       left[4] = -left[4]
00109       left[6] = -left[6]
00110       if 0 == mode:
00111          # right arm
00112          pose_r(joy_msg.axes[0:7])
00113          pose_gripper_r(joy_msg.axes[7:8])
00114       elif 1 == mode:
00115          # left arm
00116          pose_l(left[0:7])
00117          pose_gripper_l(left[7:8])
00118       elif 2 == mode:
00119          # head
00120          pose_head(joy_msg.axes[0:2])
00121       else:
00122          # both arms mirror mode
00123          pose_r(right[0:7])
00124          pose_gripper_r(right[7:8])
00125          pose_l(left[0:7])
00126          pose_gripper_l(left[7:8])
00127 
00128 if __name__ == '__main__':
00129    rospy.init_node('pr2_joint_teleop')
00130    argv = rospy.myargv()
00131 
00132    traj_client_r = TrajClient("r_arm_controller/joint_trajectory_action")
00133    traj_client_l = TrajClient("l_arm_controller/joint_trajectory_action")
00134    traj_client_head = TrajClient('head_traj_controller/joint_trajectory_action')
00135    traj_client_torso = TrajClient('torso_controller/joint_trajectory_action')
00136    gripper_l = rospy.Publisher("l_gripper_controller/command", Pr2GripperCommand)
00137    gripper_r = rospy.Publisher("r_gripper_controller/command", Pr2GripperCommand)
00138 
00139    rospy.Subscriber("joy", Joy, joy_callback)
00140    rospy.spin()


pr2_joint_teleop
Author(s): Austin Hendrix
autogenerated on Wed Aug 26 2015 15:37:37