def align_camera():
global img_w, obj_w, img_h, obj_h
global boRun
boAlign = True
speed = 0.0005
while boRun and boAlign:
x_move = 0.0
z_move = 0.0
if abs((img_w * 0.5) - obj_w) > 20:
if (img_w * 0.5) < obj_w:
x_move = speed
#cartesian_move_rel(limb="left", x=0.001*abs((img_w * 0.5) - obj_w), y=0.0, z=0.0)
elif (img_w * 0.5) > obj_w:
x_move = -speed
#cartesian_move_rel(limb="left", x=-0.001*abs((img_w * 0.5) - obj_w), y=0.0, z=0.0)
if abs((img_h * 0.5) - obj_h) > 20:
if abs((img_h * 0.5) - obj_h) > 20:
if (img_h * 0.5) < obj_h:
z_move = -speed
# cartesian_move_rel(limb="left", x=0.001*abs((img_w * 0.5) - obj_w), y=0.0, z=0.0)
# cartesian_move_rel(limb="left", x=0.0, y=0.0, z=0.0)
elif (img_h * 0.5) > obj_h:
z_move = speed
# cartesian_move_rel(limb="left", x=-0.001*abs((img_w * 0.5) - obj_w), y=0.0, z=0.0)
# cartesian_move_rel(limb="left", x=0.0, y=0.0, z=0.0)
print(abs((img_w * 0.5) - obj_w))
if x_move == 0.0 and z_move == 0.0:
time.sleep(0.5)
if x_move == 0.0 and z_move == 0.0:
print("centered")
move_list_smooth(arm="left",
p_list=[{
"left_w2": 0.2,
"left_w0": -1.35
}],
speed=0.8)
cartesian_move_rel(limb="left", x=0.0, y=-0.15, z=0.0)
thread_open_hand = threading.Thread(name='open_hand',
target=open_hand)
thread_open_hand.start()
boAlign = False
else:
#cartesian_move_rel(limb="left", x=x_move, y=0.0, z=z_move, threshold=0.1)
cartesian_move_rel(limb="left",
x=x_move * abs((img_w * 0.5) - obj_w),
y=0.0,
z=z_move * abs((img_h * 0.5) - obj_h),
threshold=0.1)
obj_w = img_w * 0.5
obj_h = img_h * 0.5
time.sleep(0.19)
def align_camera_to_corners():
global img_w, obj_w, img_h, obj_h
global boRun, boAlign
speed = 0.0005
x_move = 0.0008
while boRun:
if boAlign:
#cartesian_move_rel(limb="left", x=-x_move * abs((img_w * 0.5) - object_loc_arr[0]['x']), y=0.0, z=0.0)
cartesian_move_rel(limb="left",
x=-x_move *
abs((img_w * 0.5) - object_loc_arr[0]['x']),
y=0.0,
z=0.0)
boAlign = False
time.sleep(0.5)
def main():
arg_fmt = argparse.RawDescriptionHelpFormatter
parser = argparse.ArgumentParser(formatter_class=arg_fmt,
description=main.__doc__)
parser.add_argument('-l',
'--limb',
choices=['left', 'right'],
required=True,
help="the limb to move")
parser.add_argument('-x',
type=float,
required=False,
help="movement on the x-plane")
parser.add_argument('-y',
type=float,
required=False,
help="movement on the y-plane")
parser.add_argument('-z',
type=float,
required=False,
help="movement on the z-plane")
args = vars(parser.parse_args())
cartesian_move_rel(args['limb'], args['x'], args['y'], args['z'])
def align_camera():
global img_w, obj_w, img_h, obj_h
global boRun
speed = 0.0005
while boRun:
x_move = 0.0
z_move = 0.0
if abs((img_w * 0.5) - obj_w) > 20:
if (img_w * 0.5) < obj_w:
x_move = speed
#cartesian_move_rel(limb="left", x=0.001*abs((img_w * 0.5) - obj_w), y=0.0, z=0.0)
elif (img_w * 0.5) > obj_w:
x_move = -speed
#cartesian_move_rel(limb="left", x=-0.001*abs((img_w * 0.5) - obj_w), y=0.0, z=0.0)
if abs((img_h * 0.5) - obj_h) > 20:
if abs((img_h * 0.5) - obj_h) > 20:
if (img_h * 0.5) < obj_h:
z_move = -speed
# cartesian_move_rel(limb="left", x=0.001*abs((img_w * 0.5) - obj_w), y=0.0, z=0.0)
# cartesian_move_rel(limb="left", x=0.0, y=0.0, z=0.0)
elif (img_h * 0.5) > obj_h:
z_move = speed
# cartesian_move_rel(limb="left", x=-0.001*abs((img_w * 0.5) - obj_w), y=0.0, z=0.0)
# cartesian_move_rel(limb="left", x=0.0, y=0.0, z=0.0)
print(abs((img_w * 0.5) - obj_w))
if x_move == 0.0 and z_move == 0.0:
print("centered")
else:
#cartesian_move_rel(limb="left", x=x_move, y=0.0, z=z_move, threshold=0.1)
cartesian_move_rel(limb="left", x=x_move * abs((img_w * 0.5) - obj_w), y=0.0, z=z_move * abs((img_h * 0.5) - obj_h), threshold=0.1)
obj_w = img_w * 0.5
obj_h = img_h * 0.5
time.sleep(0.19)
def pump_and_up(x_diff=0.0, z_diff=0.0):
global start_Y_pos
grip_left.set_moving_force(90)
grip_left.set_velocity(10)
cartesian_move_rel(limb="left", y=-0.03, smooth=False)
grip_left.open(block=True, timeout=15.0)
time.sleep(8.0)
cartesian_move_rel(limb="left", y=-0.01, smooth=False)
grip_left.close(block=True, timeout=15.0)
time.sleep(1.0)
cartesian_move_rel(limb="left", y=0.04, smooth=False)
#shake()
time.sleep(1.0)
print(abs(start_Y_pos - get_cartesian_positions('left')['position y:']))
cartesian_move_rel(limb="left", x=x_diff, y=0.05, z=z_diff, smooth=False)
time.sleep(1.0)
def main():
cartesian_move_rel('left', x=-0.1)
def lower_hand():
cartesian_move_rel(limb="left", y=-0.05, threshold=0.005, smooth=False)
def shake():
cartesian_move_rel(limb="left", x=-0.01)
cartesian_move_rel(limb="left", x=0.02)
cartesian_move_rel(limb="left", x=-0.01)