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


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