From sdk-wiki
Introduction
This example demonstrates the usage of spawning objects in Gazebo, and using Inverse Kinematics calls to move the arm in position in order to pick up and block from the table, and place it back on the table. A list of gripper tip positions are hard-coded in the main() function and these are passed to the PickAndPlace() method. It is here that the joint positions are retrieved from the Inverse Kinematics calls and executed as move_to_joint() commands.
Code Walkthrough
Now, let's break down the code.
33 import argparse
34 import struct
35 import sys
36 import copy
37
38 import rospy
39 import rospkg
40
41 from gazebo_msgs.srv import (
42 SpawnModel,
43 DeleteModel,
44 )
45 from geometry_msgs.msg import (
46 PoseStamped,
47 Pose,
48 Point,
49 Quaternion,
50 )
51 from std_msgs.msg import (
52 Header,
53 Empty,
54 )
55
56 from baxter_core_msgs.srv import (
57 SolvePositionIK,
58 SolvePositionIKRequest,
59 )
60
61 import baxter_interface
This imports the baxter interface for accessing the limb and the gripper class.
63 class PickAndPlace(object):
64 def __init__(self, limb, hover_distance = 0.15, verbose=True):
65 self._limb_name = limb # string
66 self._hover_distance = hover_distance # in meters
67 self._verbose = verbose # bool
68 self._limb = baxter_interface.Limb(limb)
69 self._gripper = baxter_interface.Gripper(limb)
70 ns = "ExternalTools/" + limb + "/PositionKinematicsNode/IKService"
71 self._iksvc = rospy.ServiceProxy(ns, SolvePositionIK)
72 rospy.wait_for_service(ns, 5.0)
73 # verify robot is enabled
74 print("Getting robot state... ")
75 self._rs = baxter_interface.RobotEnable(baxter_interface.CHECK_VERSION)
76 self._init_state = self._rs.state().enabled
77 print("Enabling robot... ")
78 self._rs.enable()
79
80 def move_to_start(self, start_angles=None):
81 print("Moving the {0} arm to start pose...".format(self._limb_name))
82 if not start_angles:
83 start_angles = dict(zip(self._joint_names, [0]*7))
84 self._guarded_move_to_joint_position(start_angles)
85 self.gripper_open()
86 rospy.sleep(1.0)
87 print("Running. Ctrl-c to quit")
Action server client for the corresponding limb is created. The existence of action server for the limb is checked with a timeout of 5 seconds. The move_to_start will move the arm to a desired initial pose. If none are supplied, then the start pose is zero angle for every joint.
89 def ik_request(self, pose):
90 hdr = Header(stamp=rospy.Time.now(), frame_id='base')
91 ikreq = SolvePositionIKRequest()
92 ikreq.pose_stamp.append(PoseStamped(header=hdr, pose=pose))
93 try:
94 resp = self._iksvc(ikreq)
95 except (rospy.ServiceException, rospy.ROSException), e:
96 rospy.logerr("Service call failed: %s" % (e,))
97 return False
98 # Check if result valid, and type of seed ultimately used to get solution
99 # convert rospy's string representation of uint8[]'s to int's
100 resp_seeds = struct.unpack('<%dB' % len(resp.result_type), resp.result_type)
101 limb_joints = {}
102 if (resp_seeds[0] != resp.RESULT_INVALID):
103 seed_str = {
104 ikreq.SEED_USER: 'User Provided Seed',
105 ikreq.SEED_CURRENT: 'Current Joint Angles',
106 ikreq.SEED_NS_MAP: 'Nullspace Setpoints',
107 }.get(resp_seeds[0], 'None')
108 if self._verbose:
109 print("IK Solution SUCCESS - Valid Joint Solution Found from Seed Type: {0}".format(
110 (seed_str)))
111 # Format solution into Limb API-compatible dictionary
112 limb_joints = dict(zip(resp.joints[0].name, resp.joints[0].position))
113 if self._verbose:
114 print("IK Joint Solution:\n{0}".format(limb_joints))
115 print("------------------")
116 else:
117 rospy.logerr("INVALID POSE - No Valid Joint Solution Found.")
118 return False
119 return limb_joints
This method is the essential function for requesting an Inverse Kinematics solution with the desired end effector pose. ik_request() will return the solved joint angles if there was a solution, or false if there was a problem with the IK solver or no solution was found.
121 def _guarded_move_to_joint_position(self, joint_angles):
122 if joint_angles:
123 self._limb.move_to_joint_positions(joint_angles)
124 else:
125 rospy.logerr("No Joint Angles provided for move_to_joint_positions. Staying put.")
126
127 def gripper_open(self):
128 self._gripper.open()
129 rospy.sleep(1.0)
130
131 def gripper_close(self):
132 self._gripper.close()
133 rospy.sleep(1.0)
134
135 def _approach(self, pose):
136 approach = copy.deepcopy(pose)
137 # approach with a pose the hover-distance above the requested pose
138 approach.position.z = approach.position.z + self._hover_distance
139 joint_angles = self.ik_request(approach)
140 self._guarded_move_to_joint_position(joint_angles)
141
142 def _retract(self):
143 # retrieve current pose from endpoint
144 current_pose = self._limb.endpoint_pose()
145 ik_pose = Pose()
146 ik_pose.position.x = current_pose['position'].x
147 ik_pose.position.y = current_pose['position'].y
148 ik_pose.position.z = current_pose['position'].z + self._hover_distance
149 ik_pose.orientation.x = current_pose['orientation'].x
150 ik_pose.orientation.y = current_pose['orientation'].y
151 ik_pose.orientation.z = current_pose['orientation'].z
152 ik_pose.orientation.w = current_pose['orientation'].w
153 joint_angles = self.ik_request(ik_pose)
154 # servo up from current pose
155 self._guarded_move_to_joint_position(joint_angles)
156
157 def _servo_to_pose(self, pose):
158 # servo down to release
159 joint_angles = self.ik_request(pose)
160 self._guarded_move_to_joint_position(joint_angles)
161
162 def pick(self, pose):
163 # open the gripper
164 self.gripper_open()
165 # servo above pose
166 self._approach(pose)
167 # servo to pose
168 self._servo_to_pose(pose)
169 # close gripper
170 self.gripper_close()
171 # retract to clear object
172 self._retract()
173
174 def place(self, pose):
175 # servo above pose
176 self._approach(pose)
177 # servo to pose
178 self._servo_to_pose(pose)
179 # open the gripper
180 self.gripper_open()
181 # retract to clear object
182 self._retract()
These functions direct Baxter to move into the desired pose by requesting an Inverse Kinematics solution, retrieving the calculated joint angles, and servoing to those positions.
184 def load_gazebo_models(table_pose=Pose(position=Point(x=1.0, y=0.0, z=0.0)),
185 table_reference_frame="world",
186 block_pose=Pose(position=Point(x=0.6725, y=0.1265, z=0.7825)),
187 block_reference_frame="world"):
188 # Get Models' Path
189 model_path = rospkg.RosPack().get_path('baxter_sim_examples')+"/models/"
190 # Load Table SDF
191 table_xml = ''
192 with open (model_path + "cafe_table/model.sdf", "r") as table_file:
193 table_xml=table_file.read().replace('\n', '')
194 # Load Block URDF
195 block_xml = ''
196 with open (model_path + "block/model.urdf", "r") as block_file:
197 block_xml=block_file.read().replace('\n', '')
198 # Spawn Table SDF
199 rospy.wait_for_service('/gazebo/spawn_sdf_model')
200 try:
201 spawn_sdf = rospy.ServiceProxy('/gazebo/spawn_sdf_model', SpawnModel)
202 resp_sdf = spawn_sdf("cafe_table", table_xml, "/",
203 table_pose, table_reference_frame)
204 except rospy.ServiceException, e:
205 rospy.logerr("Spawn SDF service call failed: {0}".format(e))
206 # Spawn Block URDF
207 rospy.wait_for_service('/gazebo/spawn_urdf_model')
208 try:
209 spawn_urdf = rospy.ServiceProxy('/gazebo/spawn_urdf_model', SpawnModel)
210 resp_urdf = spawn_urdf("block", block_xml, "/",
211 block_pose, block_reference_frame)
212 except rospy.ServiceException, e:
213 rospy.logerr("Spawn URDF service call failed: {0}".format(e))
214
215 def delete_gazebo_models():
216 # This will be called on ROS Exit, deleting Gazebo models
217 # Do not wait for the Gazebo Delete Model service, since
218 # Gazebo should already be running. If the service is not
219 # available since Gazebo has been killed, it is fine to error out
220 try:
221 delete_model = rospy.ServiceProxy('/gazebo/delete_model', DeleteModel)
222 resp_delete = delete_model("cafe_table")
223 resp_delete = delete_model("block")
224 except rospy.ServiceException, e:
225 rospy.loginfo("Delete Model service call failed: {0}".format(e))
These functions wrap the service calls for spawning and destroying models in the Gazebo environment. Specifically, the table is spawned with a SDF file and the block with an URDF.
227 def main():
228 """RSDK Inverse Kinematics Pick and Place Example
229 A Pick and Place example using the Rethink Inverse Kinematics
230 Service which returns the joint angles a requested Cartesian Pose.
231 This ROS Service client is used to request both pick and place
232 poses in the /base frame of the robot.
233 Note: This is a highly scripted and tuned demo. The object location
234 is "known" and movement is done completely open loop. It is expected
235 behavior that Baxter will eventually mis-pick or drop the block. You
236 can improve on this demo by adding perception and feedback to close
237 the loop.
238 """
239 rospy.init_node("ik_pick_and_place_demo")
240 # Load Gazebo Models via Spawning Services
241 # Note that the models reference is the /world frame
242 # and the IK operates with respect to the /base frame
243 load_gazebo_models()
244 # Remove models from the scene on shutdown
245 rospy.on_shutdown(delete_gazebo_models)
246
247 # Wait for the All Clear from emulator startup
248 rospy.wait_for_message("/robot/sim/started", Empty)
249
250 limb = 'left'
251 hover_distance = 0.15 # meters
252 # Starting Joint angles for left arm
253 starting_joint_angles = {'left_w0': 0.6699952259595108,
254 'left_w1': 1.030009435085784,
255 'left_w2': -0.4999997247485215,
256 'left_e0': -1.189968899785275,
257 'left_e1': 1.9400238130755056,
258 'left_s0': -0.08000397926829805,
259 'left_s1': -0.9999781166910306}
260 pnp = PickAndPlace(limb, hover_distance)
261 # An orientation for gripper fingers to be overhead and parallel to the obj
262 overhead_orientation = Quaternion(
263 x=-0.0249590815779,
264 y=0.999649402929,
265 z=0.00737916180073,
266 w=0.00486450832011)
267 block_poses = list()
268 # The Pose of the block in its initial location.
269 # You may wish to replace these poses with estimates
270 # from a perception node.
271 block_poses.append(Pose(
272 position=Point(x=0.7, y=0.15, z=-0.129),
273 orientation=overhead_orientation))
274 # Feel free to add additional desired poses for the object.
275 # Each additional pose will get its own pick and place.
276 block_poses.append(Pose(
277 position=Point(x=0.75, y=0.0, z=-0.129),
278 orientation=overhead_orientation))
279 # Move to the desired starting angles
280 pnp.move_to_start(starting_joint_angles)
281 idx = 0
282 while not rospy.is_shutdown():
283 print("\nPicking...")
284 pnp.pick(block_poses[idx])
285 print("\nPlacing...")
286 idx = (idx+1) % len(block_poses)
287 pnp.place(block_poses[idx])
288 return 0
The main function instantiates the PickAndPlace class, spawns the models in Gazebo, and creates a list of poses to pick and place from. You can add to this list of poses by creating your own Point. You'll likely want to maintain the frontoparallel orientation of the gripper. Try adding an additional pose at Point(x=0.72, y=0.075, z=-0.129), and see how Baxter now attempts three pick and place locations.
292 if __name__ == '__main__':
293 sys.exit(main())
This is the function that gets executed if this file is executed as a Python script. It simply calls the main() function, then exits when main has finished.
