# IK Pick and Place Demo - Code Walkthrough

## 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 (
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):
91         ikreq = SolvePositionIKRequest()
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/"
191     table_xml = ''
192     with open (model_path + "cafe_table/model.sdf", "r") as table_file:
195     block_xml = ''
196     with open (model_path + "block/model.urdf", "r") as block_file:
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
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
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),
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),
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.