new_main.py

@@ -1,5 +1,4 @@
 #!/usr/bin/python
-from __future__ import print_function
 import argparse
 import struct
 import sys
@@ -27,7 +26,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 = False #Flag for asking confirmation before robot performs key maneuvers
+debug = True #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):
@@ -48,10 +47,6 @@ 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:
@@ -220,7 +215,6 @@ 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)
@@ -232,34 +226,24 @@ 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):
-    print('Picking up brick ', step['step'], '...')
+    if step['vertical'] == True:
+        V_Routine(limb)
+    else:
+        H_Routine(limb)
 
-    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'])
@@ -268,35 +252,23 @@ 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,67 +27,64 @@ instructions = [
 	{
 	'step':1,
 	'hover':B_1_A,
-	'place':B_1_P,
+	'pick':B_1_P,
 	'vertical':True,
 	'lastBrick':False
 	},
 	{
 	'step':2,
 	'hover':B_2_A,
-	'place':B_2_P,
+	'pick':B_2_P,
 	'vertical':True,
 	'lastBrick':False
 	},
 	{
 	'step':3,
 	'hover':B_3_A,
-	'place':B_3_P,
+	'pick':B_3_P,
 	'vertical':True,
 	'lastBrick':False
 	},
 	{
 	'step':4,
 	'hover':B_4_A,
-	'place':B_4_P,
+	'pick':B_4_P,
 	'vertical':False,
 	'lastBrick':False
 	},
 	{
 	'step':5,
 	'hover':B_5_A,
-	'place':B_5_P,
+	'pick':B_5_P,
 	'vertical':False,
 	'lastBrick':False
 	},
 	{
 	'step':6,
 	'hover':B_6_A,
-	'place':B_6_P,
+	'pick':B_6_P,
 	'vertical':True,
 	'lastBrick':False
 	},
 	{
 	'step':7,
 	'hover':B_7_A,
-	'place':B_7_P,
+	'pick':B_7_P,
 	'vertical':True,
 	'lastBrick':False
 	},
 	{
 	'step':8,
 	'hover':B_8_A,
-	'place':B_8_P,
+	'pick':B_8_P,
 	'vertical':False,
 	'lastBrick':False
 	},
 	{
 	'step':9,
 	'hover':B_9_A,
-	'place':B_9_P,
+	'pick':B_9_P,
 	'vertical':True,
-	'lastBrick':True,
+	'lastBrick':True
-	'last1':B_9_za,
-	'last2':B_9_zb,
-	'last3':H_pickup
 	}
 ]