work in progress
Please ask about problems and questions regarding this tutorial on answers.ros.org. Don't forget to include in your question the link to this page, the versions of your OS & ROS, and also add appropriate tags. |
Moving an Arm
Description: This tutorial shows step-by-step how to move an ArbotiX-based arm controlled by the simple arm server.Tutorial Level: INTERMEDIATE
Prerequisites
This tutorial assumes that you have an ArbotiX/Dynamixel-based arm, with a trajectory controller and URDF properly configured.
Overview
The simple arm server allows you to move an arm through a series of goals. The MoveArm request contains a header and an array of ArmAction goals:
Header header ArmAction[] goals
The header is used to specify the frame_id of the goals, while each ArmAction goal consists of several fields:
byte type # move the arm or the gripper? geometry_msgs/Pose goal # goal for an arm float64 command # goal for a gripper duration move_time
Arm Position Goals
Most goals will move the arm to a position. In this case, we fill out the pose goal and set type to MOVE_ARM, a constant defined in the ArmAction message.
Gripper Goals
Goals can also set the commanded position of the gripper. In this case, we set type equal to MOVE_GRIPPER and set the command field of the request.
Example Code
The multi_waypoint_test.py example in simple_arm_server shows how to move the arm through a small trajectory, picking up a block, and then dropping it in a different location:
1 import roslib; roslib.load_manifest('simple_arm_server')
2 import rospy
3 import sys
4
5 import tf
6 from tf.transformations import euler_from_quaternion, quaternion_from_euler
7
8 from simple_arm_server.msg import *
9 from simple_arm_server.srv import *
10
11 if __name__ == '__main__':
12 rospy.init_node('simple_arm_server_test')
13 rospy.wait_for_service('simple_arm_server/move')
14 move_srv = rospy.ServiceProxy('simple_arm_server/move', MoveArm)
15
16 req = MoveArmRequest()
17 req.header.frame_id = "base_link"
18
19 action = ArmAction()
20 action.type = ArmAction.MOVE_ARM
21 action.goal.position.x = 0.2
22 action.goal.position.y = -0.09
23 action.goal.position.z = .1
24 q = quaternion_from_euler(0.0, 1.57, 0.0, 'sxyz')
25 action.goal.orientation.x = q[0]
26 action.goal.orientation.y = q[1]
27 action.goal.orientation.z = q[2]
28 action.goal.orientation.w = q[3]
29 action.move_time = rospy.Duration(5.0)
30 req.goals.append(action)
31
32 action = ArmAction()
33 action.type = ArmAction.MOVE_GRIPPER
34 action.command = 0.03
35 action.move_time = rospy.Duration(1.0)
36 req.goals.append(action)
37
38 action = ArmAction()
39 action.type = ArmAction.MOVE_ARM
40 action.goal.position.x = 0.2
41 action.goal.position.y = -0.09
42 action.goal.position.z = .04
43 q = quaternion_from_euler(0.0, 1.57, 0.0, 'sxyz')
44 action.goal.orientation.x = q[0]
45 action.goal.orientation.y = q[1]
46 action.goal.orientation.z = q[2]
47 action.goal.orientation.w = q[3]
48 action.move_time = rospy.Duration(1.0)
49 req.goals.append(action)
50
51 action = ArmAction()
52 action.type = ArmAction.MOVE_GRIPPER
53 action.command = 0.0254
54 action.move_time = rospy.Duration(2.0)
55 req.goals.append(action)
56
57 action = ArmAction()
58 action.type = ArmAction.MOVE_ARM
59 action.goal.position.x = 0.2
60 action.goal.position.y = -0.09
61 action.goal.position.z = .1
62 q = quaternion_from_euler(0.0, 1.57, 0.0, 'sxyz')
63 action.goal.orientation.x = q[0]
64 action.goal.orientation.y = q[1]
65 action.goal.orientation.z = q[2]
66 action.goal.orientation.w = q[3]
67 action.move_time = rospy.Duration(1.0)
68 req.goals.append(action)
69
70 action = ArmAction()
71 action.type = ArmAction.MOVE_ARM
72 action.goal.position.x = 0.2
73 action.goal.position.y = 0.09
74 action.goal.position.z = .1
75 q = quaternion_from_euler(0.0, 1.57, 0.0, 'sxyz')
76 action.goal.orientation.x = q[0]
77 action.goal.orientation.y = q[1]
78 action.goal.orientation.z = q[2]
79 action.goal.orientation.w = q[3]
80 action.move_time = rospy.Duration(5.0)
81 req.goals.append(action)
82
83 action = ArmAction()
84 action.type = ArmAction.MOVE_GRIPPER
85 action.command = 0.03
86 action.move_time = rospy.Duration(2.0)
87 req.goals.append(action)
88
89 action = ArmAction()
90 action.type = ArmAction.MOVE_GRIPPER
91 action.command = 0.0254
92 action.move_time = rospy.Duration(1.0)
93 req.goals.append(action)
94
95 try:
96 r = move_srv(req)
97 print r
98 except rospy.ServiceException, e:
99 print "Service did not process request: %s"%str(e)
Summary
This tutorial has shown how to move the arm through a series of poses. This can be used for simple pick and place, although the server offers no collision checking. Please see the Moving the Arm tutorial for Mini Maxwell for a specific example.