diff --git a/new_main.py b/new_main.py index 48348a2..6903abd 100755 --- a/new_main.py +++ b/new_main.py @@ -37,7 +37,6 @@ class PickAndPlace(object): #class that handles moving the robot, gripper and IK self._verbose = verbose # bool self._limb = baxter_interface.Limb(limb) self._gripper = baxter_interface.Gripper(limb) - self._gripper.calibrate() #Calibrating gripper ns = "ExternalTools/" + limb + "/PositionKinematicsNode/IKService" self._iksvc = rospy.ServiceProxy(ns, SolvePositionIK) rospy.wait_for_service(ns, 5.0) @@ -48,6 +47,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: @@ -216,6 +219,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) @@ -227,24 +231,30 @@ 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) - + 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...') 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']) @@ -253,22 +263,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 + 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(instructions)) + for step in tower_instructions: + print('Processing step: ', step['step']) process(left_pnp, step) + time.sleep(1) print("#################################################################################################################")