Update

new_main.py

@@ -1,4 +1,5 @@
 #!/usr/bin/python
+from __future__ import print_function
 import argparse
 import struct
 import sys
@@ -26,7 +27,7 @@ import target_angles as ta
 #^Importing essential libraries for ROS and motion planning and running for the main script^
 
 simulation = True #Flag for spawning objects in the gazebo if the script is running in a simulation
-debug = True #Flag for asking confirmation before robot performs key maneuvers
+debug = False #Flag for asking confirmation before robot performs key maneuvers
 
 class PickAndPlace(object): #class that handles moving the robot, gripper and IK
     def __init__(self, limb, hover_distance = 0.10, verbose=True, speed=0.2, accuracy=baxter_interface.settings.JOINT_ANGLE_TOLERANCE):
@@ -47,6 +48,10 @@ class PickAndPlace(object): #class that handles moving the robot, gripper and IK
         print("Enabling robot... ")
         self._rs.enable()
 
+    def calibrate_gripper(self):
+        '''Calibrated gripper'''
+        self._gripper.calibrate()
+
     def _guarded_move_to_joint_position(self, joint_angles):
         '''Private class to move robot limbs'''
         if joint_angles:
@@ -215,6 +220,7 @@ def V_Routine(limb):
     if debug: x = raw_input('Pick? ')
     limb.goto(ta.V_pickup)
     if debug: x = raw_input('Close gripper? ')
+    print('Closing gripper...')
     limb.gripper_close()
     gripperBrickChecker(limb)
     limb.goto(ta.V_approach)
@@ -226,24 +232,34 @@ def H_Routine(limb):
     if debug: x = raw_input('Pick? ')
     limb.goto(ta.H_pickup)
     if debug: x = raw_input('Close gripper? ')
+    print('Closing gripper...')
     limb.gripper_close()
     gripperBrickChecker(limb)
     limb.goto(ta.H_approach)
 
 def process(limb, step):
-    if step['vertical'] == True:
-        V_Routine(limb)
-    else:
-        H_Routine(limb)
+    print('Picking up brick ', step['step'], '...')
 
+    if step['vertical'] == True: V_Routine(limb)
+    else: H_Routine(limb)
+
+    print('Moving brick ', step['step'], ' to hover position...')
     limb.goto(step['hover'])
+
     if debug:x = raw_input('Place?: ')
+    print('Placing brick ', step['step'], '...')
     limb.goto(step['place'])
+
     if debug:x = raw_input('Open gripper?: ')
+    print('Opening gripper...')
+    time.sleep(0.5)
+    
     limb.gripper_open()
     if not step['lastBrick']:
+        print('Moving arm to hover ', step['step'], ' position...')
         limb.goto(step['hover'])
     else:
+        print('Moving arm to home position...')
         limb.goto(step['last1'])
         limb.goto(step['last2'])
         limb.goto(step['last3'])
@@ -252,23 +268,35 @@ def process(limb, step):
 
 
 rospy.init_node("MOTHER_CLUCKER")  #Initializes rospy node
+print('Robot initialized')
+print('Simulation: ', simulation)
+print('Debug: ', debug)
 
 if simulation:
     #Clean up any (known) preexisting objects in Gazebo and spawns the tables
     cleanup()
     spawn_tables()
 
+print('Initializing arms...')
 tuck_arms.init_arms() #Makes sure the arms are in known starting place
-tuck_arms.init_arms()
+
 left_pnp = PickAndPlace('left') #Limb initializer
 
+print('Calibrating gripper...')
+left_pnp.calibrate_gripper()
+
+print('Opening gripper...')
 left_pnp.gripper_open() #Ensures gripper is open before grabbing brick
 
 tower_instructions = ta.instructions
 
+print('Loaded tower instructions, bricks: ', len(tower_instructions))
+
 for step in tower_instructions:
+    print('Processing step: ', step['step'])
     process(left_pnp, step)
 
+
 time.sleep(1)
 
 print("#################################################################################################################")

target_angles.py

@@ -27,64 +27,67 @@ instructions = [
 	{
 	'step':1,
 	'hover':B_1_A,
-	'pick':B_1_P,
+	'place':B_1_P,
 	'vertical':True,
 	'lastBrick':False
 	},
 	{
 	'step':2,
 	'hover':B_2_A,
-	'pick':B_2_P,
+	'place':B_2_P,
 	'vertical':True,
 	'lastBrick':False
 	},
 	{
 	'step':3,
 	'hover':B_3_A,
-	'pick':B_3_P,
+	'place':B_3_P,
 	'vertical':True,
 	'lastBrick':False
 	},
 	{
 	'step':4,
 	'hover':B_4_A,
-	'pick':B_4_P,
+	'place':B_4_P,
 	'vertical':False,
 	'lastBrick':False
 	},
 	{
 	'step':5,
 	'hover':B_5_A,
-	'pick':B_5_P,
+	'place':B_5_P,
 	'vertical':False,
 	'lastBrick':False
 	},
 	{
 	'step':6,
 	'hover':B_6_A,
-	'pick':B_6_P,
+	'place':B_6_P,
 	'vertical':True,
 	'lastBrick':False
 	},
 	{
 	'step':7,
 	'hover':B_7_A,
-	'pick':B_7_P,
+	'place':B_7_P,
 	'vertical':True,
 	'lastBrick':False
 	},
 	{
 	'step':8,
 	'hover':B_8_A,
-	'pick':B_8_P,
+	'place':B_8_P,
 	'vertical':False,
 	'lastBrick':False
 	},
 	{
 	'step':9,
 	'hover':B_9_A,
-	'pick':B_9_P,
+	'place':B_9_P,
 	'vertical':True,
-	'lastBrick':True
+	'lastBrick':True,
+	'last1':B_9_za,
+	'last2':B_9_zb,
+	'last3':H_pickup
 	}
 ]