Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014 import roslib; roslib.load_manifest('pr2_pose')
00015 import rospy
00016 import actionlib
00017 import yaml
00018
00019 from pr2_controllers_msgs.msg import JointTrajectoryGoal, JointTrajectoryAction
00020 from trajectory_msgs.msg import JointTrajectoryPoint
00021
00022 def pose_(position, joints, client):
00023 goal = JointTrajectoryGoal()
00024 goal.trajectory.joint_names = joints
00025
00026 goal.trajectory.points = [ JointTrajectoryPoint() ]
00027
00028 goal.trajectory.points[0].velocities = [0.0] * len(joints);
00029 goal.trajectory.points[0].positions = [0.0] * len(joints);
00030 for i, j in enumerate(joints):
00031 goal.trajectory.points[0].positions[i] = position[j]
00032
00033 goal.trajectory.points[0].time_from_start = rospy.Duration.from_sec(1.0)
00034 client.send_goal(goal)
00035
00036 def pose_r(position):
00037 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"]
00038 pose_(position, joints, traj_client_r)
00039
00040 def pose_l(position):
00041 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"]
00042 pose_(position, joints, traj_client_l)
00043
00044 def pose_head(position):
00045 joints = [ 'head_pan_joint', 'head_tilt_joint' ]
00046 pose_(position, joints, traj_client_head)
00047
00048 def pose_torso(position):
00049 joints = [ 'torso_lift_joint' ]
00050 pose_(position, joints, traj_client_torso)
00051
00052
00053 def pose(position):
00054 pose_l(position)
00055 pose_r(position)
00056 pose_head(position)
00057 pose_torso(position)
00058
00059 def TrajClient(t):
00060 c = actionlib.SimpleActionClient(t, JointTrajectoryAction)
00061 c.wait_for_server()
00062 return c
00063
00064 if __name__ == '__main__':
00065 rospy.init_node('pose')
00066 argv = rospy.myargv()
00067
00068 traj_client_r = TrajClient("r_arm_controller/joint_trajectory_action")
00069 traj_client_l = TrajClient("l_arm_controller/joint_trajectory_action")
00070 traj_client_head = TrajClient('head_traj_controller/joint_trajectory_action')
00071 traj_client_torso = TrajClient('torso_controller/joint_trajectory_action')
00072
00073 fname1 = 'pose.yaml'
00074 if len(argv) > 1:
00075 fname1 = argv[1]
00076
00077 p1 = yaml.load(file(fname1, 'r'))
00078
00079 if len(argv) == 4:
00080 fname2= argv[2]
00081 steps = int(argv[3])
00082
00083 p2 = yaml.load(file(fname2, 'r'))
00084 p = dict(p1)
00085 for i in range(steps):
00086 for j in p1:
00087 p[j] = ((steps - i)*p1[j] + i*p2[j]) / steps
00088 pose(p)
00089 print yaml.dump(p)
00090 raw_input("%d > "%i)
00091 print steps
00092 pose(p2)
00093 print yaml.dump(p2)
00094 elif len(argv) < 3:
00095 pose(p1)
00096 else:
00097 print "Improper number of arguments"
00098 print "Usage"
00099 print " pose.py [pose.yaml]"
00100 print " pose.py <1.yaml> <2.yaml> <steps>"