| Note: This tutorial assumes that you have completed the previous tutorials: ROS tutorials. |
Using the move_arm_service package
Description: Explains how to use the services to make simpler calls to move_arm and interpolated_ik_motion_planner.Tutorial Level: BEGINNER
Downloading the Package
If you have already checked out the experimental version of the mit-ros-pkg, you can skip this step.
To check out the move_arm_service package, first navigate to a directory on your ROS_PACKAGE_PATH in which you wish to have the package. Within this directory, check out the latest version of the package using:
svn co `roslocate svn move_arm_service`
The Code
First, create a package for this tutorial. Open a new shell, navigate to a directory on your ROS_PACKAGE_PATH in which you wish to do the tutorial and type
roscreate-pkg move_arm_tutorial rospy roscpp move_arm_service
Within that package, create the src/move_arm_tutorial/try_ik.py file and paste the following inside it:
1 #!/usr/bin/env python
2
3 '''
4 A Python test program for the service.
5
6 This test program shows how to call the move_arm service and the interpolated
7 IK service from Python. It also has example comments for documenting Python
8 code with Epydoc.
9
10 Technically, this belongs in the scripts directory since it is not code
11 anyone using the package (rather than working on it) would need. However,
12 I have put in the the src directory so that it could serve as example
13 documentation.
14 '''
15
16 import roslib; roslib.load_manifest('move_arm_service')
17 import rospy
18 import move_arm_service.srv
19 from geometry_msgs.msg import PoseStamped
20 import copy
21
22 class ArmMovements:
23 '''
24 A class that contains functions for moving the arm
25 using move_arm and interpolated IK.
26
27 Probably a class
28 is overkill for these simple functions, but I wanted
29 to show how to comment a class.
30 '''
31
32 def __init__(self, arm_name):
33 '''
34 Constructor for ArmMovements
35
36 @type arm_name: string
37 @param arm_name: The name of the arm this
38 class should move ('right_arm' or 'left_arm')
39 '''
40 self.arm_name = arm_name
41 '''
42 The name of the arm this class moves
43 '''
44
45 def move_arm(self, pose_stamped):
46 '''
47 A function to move the arm to a particular pose by calling the
48 move_arm_service.
49
50 @type pose_stamped: geometry_msgs.msg.PoseStamped
51 @param pose_stamped: The pose to move the arm to
52 @rtype: move_arm_service.srv.MoveArmResponse
53 @returns: The response from calling the move_arm_service
54 '''
55 move_call = move_arm_service.srv.MoveArmRequest()
56 move_call.pose_stamped = pose_stamped
57 move_call.arm = self.arm_name
58 print 'Calling move_arm'
59 move_arm_srv = rospy.ServiceProxy('move_arm_to_pose', move_arm_service.srv.MoveArm)
60 move_resp = move_arm_srv(move_call)
61 return move_resp
62
63 def interpolated_ik(self, start_pose, end_pose):
64 '''
65 Function for moving from start_pose to end_pose using interpolated IK.
66
67 @type start_pose: geometry_msgs.msg.PoseStamped
68 @param start_pose: The pose the arm will start in
69 @type end_pose: geometry_msgs.msg.PoseStamped
70 @param end_pose: The pose the arm will end in
71 @rtype: move_arm_service.srv.InterpolatedIKResponse
72 @returns: The response from the interpolated IK service
73 '''
74 ik_call = move_arm_service.srv.InterpolatedIKRequest()
75 ik_call.start_pose = start_pose
76 ik_call.end_pose = end_pose
77 ik_call.arm = self.arm_name
78 print 'Calling ik'
79 ik_srv = rospy.ServiceProxy('run_interpolated_ik',
80 move_arm_service.srv.InterpolatedIK)
81 ik_resp = ik_srv(ik_call)
82 return ik_resp
83
84
85 def main():
86 '''
87 The main function
88 '''
89 rospy.init_node('test_ik_node')
90 mover = ArmMovements('left_arm')
91 pose_stamped = PoseStamped()
92 pose_stamped.pose.position.x = 0
93 pose_stamped.pose.position.y = 0.7
94 pose_stamped.pose.position.z = 0
95 pose_stamped.pose.orientation.x = 0.707
96 pose_stamped.pose.orientation.y = 0
97 pose_stamped.pose.orientation.z = 0
98 pose_stamped.pose.orientation.w = 0.707
99 pose_stamped.header.frame_id = 'torso_lift_link'
100 pose_stamped.header.stamp = rospy.Time.now()
101 move_resp = mover.move_arm(pose_stamped)
102 print 'move_arm returned with response', move_resp.error_code.val
103 end_pose = copy.deepcopy(pose_stamped)
104 end_pose.pose.position.x = 0.1
105 ik_resp = mover.interpolated_ik(pose_stamped, end_pose)
106 print 'ik returned with response', ik_resp.error_code.val
107
108 if __name__ == '__main__':
109 main()
Don't forget to make it an executable:
chmod +x try_ik.py
Running the Code
First you should start the robot. You can run this in simulation:
roslaunch pr2_gazebo pr2_emptyworld.launch
or on the actual robot:
sudo robot start
The move_arm_service requires the move_arm action and the interpolated_ik_motion_planner service. Launch those using:
roslaunch pr2_object_manipulation_launch pr2_manipulation_prerequisites.launch
Then make the move_arm_service package:
rosmake move_arm_service
and start it
roslaunch move_arm_service move_arm_service.launch
You should now be able to run your test file:
rosrun move_arm_tutorial try_ik.py
The robot's left arm should move to its side (using move_arm) and then slowly extend 10cm forward (using interpolated IK).