Compare commits
No commits in common. "7ffdec977c5ec9a96a77307775cf03fa4740f260" and "9a7b3797e2aae2142db81cfa624ee08ffe34b933" have entirely different histories.
7ffdec977c
...
9a7b3797e2
40
new_main.py
40
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("#################################################################################################################")
|
||||
|
||||
@ -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,
|
||||
'last1':B_9_za,
|
||||
'last2':B_9_zb,
|
||||
'last3':H_pickup
|
||||
'lastBrick':True
|
||||
}
|
||||
]
|
||||
Loading…
Reference in New Issue
Block a user