def main():
print 'Baxter Organizer initiated.'
initial_setup_baxter()
initial_setup_camera()
calibrate_homography()
get_original_positions()
print 'Now you can move the object at any time and wait.'
while True:
print 'Checking objects.'
if not check_positions():
print 'What a mess! >:('
organize()
else:
print 'Everything ok! :)'
time.sleep(10)
done = False
print 'Esc to exit.'
while not done and not rospy.is_shutdown():
c = iodevices.getch()
if c:
# Get 'esc' or 'ctrl+c'
if c in ['\x1b', '\x03']:
done = True
#disable_baxter()
print 'Baxter Organizer finished.'
def map_file(filename, loops=1):
""" Loops through csv file
Does not loop indefinitely, but only until the file is read
and processed. Reads each line, split up in columns and
formats each line into a controller command in the form of
name/value pairs. Names come from the column headers
first column is the time stamp
@param filename - the file to play
@param loops - number of times to loop
values < 0 mean 'infinite'
"""
left = baxter_interface.Limb('left')
right = baxter_interface.Limb('right')
grip_left = baxter_interface.Gripper('left')
grip_right = baxter_interface.Gripper('right')
rate = rospy.Rate(1000)
start_time = rospy.get_time()
print("Playing back: %s" % (filename, ))
with open(filename, 'r') as f:
lines = f.readlines()
keys = lines[0].rstrip().split(',')
i = 0
l = 0
# If specified, repeat the file playback 'loops' number of times
while loops < 1 or l < loops:
l = l + 1
print("Moving to start position...")
cmd_start, lcmd_start, rcmd_start, raw_start = clean_line(
lines[1], keys)
left.move_to_joint_positions(lcmd_start)
right.move_to_joint_positions(rcmd_start)
for values in lines[1:]:
i = i + 1
loopstr = str(loops) if loops > 0 else "forever"
sys.stdout.write("\r Record %d of %d, loop %d of %s" \
% (i, len(lines)-1,l,loopstr))
sys.stdout.flush()
cmd, lcmd, rcmd, values = clean_line(values, keys)
#command this set of commands until the next frame
while (rospy.get_time() - start_time) < values[0]:
if rospy.is_shutdown():
print("\n ROS shutdown")
return False
if iodevices.getch():
print("\n stopped")
return False
if len(lcmd):
left.set_joint_positions(lcmd)
if len(rcmd):
right.set_joint_positions(rcmd)
if 'left_gripper' in cmd:
grip_left.set_position(cmd['left_gripper'])
if 'left_gripper' in cmd:
grip_right.set_position(cmd['right_gripper'])
rate.sleep()
print
return True
def wobble(self):
self.set_neutral()
"""
Performs the wobbling of both arms
"""
rate = rospy.Rate(100)
start = rospy.Time.now()
def make_v_func():
"""
returns a randomly parameterized cos function to control a specific joint
"""
period_factor = random.uniform(0.3, 0.5)
amplitude_factor = random.uniform(0.1, 0.2)
def v_func(elapsed):
return math.cos(period_factor * elapsed.to_sec() * math.pi *
2) * amplitude_factor
return v_func
v_funcs = [make_v_func() for x in range(len(self._right_joint_names))]
done = False
print("Wobbling. Press any key to stop...")
while not done and not rospy.is_shutdown():
if iodevices.getch():
done = True
else:
self._pub_rate.publish(100)
elapsed = rospy.Time.now() - start
cmd = dict(
zip(self._left_joint_names, [
v_funcs[i](elapsed)
for i in range(len(self._left_joint_names))
]))
self._left_arm.set_joint_velocities(cmd)
cmd = dict(
zip(self._right_joint_names, [
-v_funcs[i](elapsed)
for i in range(len(self._right_joint_names))
]))
self._right_arm.set_joint_velocities(cmd)
rate.sleep()
rate = rospy.Rate(100)
if not rospy.is_shutdown():
for i in range(100):
if rospy.is_shutdown():
return False
self._left_arm.set_joint_position_mode()
self._right_arm.set_joint_position_mode()
self._pub_rate.publish(100)
rate.sleep()
#return to normal
self.set_neutral()
return True
def done(self):
"""
Return whether or not recording is done
"""
if rospy.is_shutdown():
self.stop()
elif iodevices.getch(0.9 / self._raw_rate):
self.stop()
return self._done
def done(self):
"""
Return whether or not recording is done
"""
if rospy.is_shutdown():
self.stop()
elif iodevices.getch(0.9 / self._raw_rate):
self.stop()
return self._done
def map_keyboard():
left = baxter_interface.Gripper('left')
right = baxter_interface.Gripper('right')
bindings = {
# key: (function, args, description)
'r': (left.reboot, [], "left: reset"),
'R': (right.reboot, [], "right: reset"),
'c': (left.calibrate, [], "left: calibrate"),
'C': (right.calibrate, [], "right: calibrate"),
'q': (left.close, [], "left: close"),
'Q': (right.close, [], "right: close"),
'w': (left.open, [], "left: open"),
'W': (right.open, [], "right: open"),
'[': (left.set_velocity, [100.0], "left: set 100% velocity"),
'{': (right.set_velocity, [100.0], "right: set 100% velocity"),
']': (left.set_velocity, [30.0], "left: set 30% velocity"),
'}': (right.set_velocity, [30.0], "right: set 30% velocity"),
's': (left.stop, [], "left: stop"),
'S': (right.stop, [], "right: stop"),
'z': (left.inc_dead_band, [-1.0], "left: decrease dead band"),
'Z': (right.inc_dead_band, [-1.0], "right: decrease dead band"),
'x': (left.inc_dead_band, [1.0], "left: increase dead band"),
'X': (right.inc_dead_band, [1.0], "right: increase dead band"),
'f': (left.inc_force, [-5.0], "left: decrease moving force"),
'F': (right.inc_force, [-5.0], "right: decrease moving force"),
'g': (left.inc_force, [5.0], "left: increase moving force"),
'G': (right.inc_force, [5.0], "right: increase moving force"),
'h': (left.inc_holding_force, [-5.0], "left: decrease holding force"),
'H': (right.inc_holding_force, [-5.0], "right: decrease holding force"),
'j': (left.inc_holding_force, [5.0], "left: increase holding force"),
'J': (right.inc_holding_force, [5.0], "right: increase holding force"),
'v': (left.inc_velocity, [-5.0], "left: decrease velocity"),
'V': (right.inc_velocity, [-5.0], "right: decrease velocity"),
'b': (left.inc_velocity, [5.0], "left: increase velocity"),
'B': (right.inc_velocity, [5.0], "right: increase velocity"),
'u': (left.inc_position, [-5.0], "left: decrease position"),
'U': (right.inc_position, [-5.0], "right: decrease position"),
'i': (left.inc_position, [5.0], "left: increase position"),
'I': (right.inc_position, [5.0], "right: increase position"),
}
done = False
while not done and not rospy.is_shutdown():
c = iodevices.getch()
if c:
if c in ['\x1b', '\x03']:
done = True
elif c in bindings:
cmd = bindings[c]
cmd[0](*cmd[1])
print("command: %s" % (cmd[2],))
else:
print("key bindings: ")
print(" Esc: Quit")
print(" ?: Help")
for key, val in sorted(bindings.items(), key=lambda x: x[1][2]):
print(" %s: %s" % (key, val[2]))
def put_it_all_together(self):
"""
Calls all functions and waits for termination input
"""
self.set_arm()
self.prepare_cameras()
print("Press any key to stop...")
done = False
while not done and not rospy.is_shutdown():
if iodevices.getch():
done = True
self.left_cam.close()
def wave(self):
self.set_neutral()
"""
Performs the wobbling of both arms
"""
rate = rospy.Rate(1000);
start = rospy.Time.now()
def make_v_func():
"""
returns a randomly parameterized cos function to control a
specific joint
"""
period_factor = random.uniform(0.3, 0.5)
amplitude_factor = random.uniform(0.2, 0.9)
def v_func(elapsed):
return math.cos(period_factor * elapsed.to_sec() * math.pi * 2) * amplitude_factor
return v_func
v_funcs = [make_v_func() for x in range(len(self._joint_names))]
done = False
print("Waving. Press any key to stop...")
while not done and not rospy.is_shutdown():
if iodevices.getch():
done = True
else:
self._pub_rate.publish(1000)
elapsed = rospy.Time.now() - start
cmd = dict(zip(self._joint_names, [-v_funcs[i](elapsed) for i in range(len(self._joint_names))]))
cmd['s0'] = 0
cmd['s1'] = 0
cmd['e0'] = 0
cmd['w1'] = 0
self._right_arm.set_joint_velocities(cmd)
rate.sleep()
rate = rospy.Rate(100);
if not rospy.is_shutdown():
for i in range(100):
if rospy.is_shutdown():
return False
self._left_arm.set_joint_position_mode()
self._right_arm.set_joint_position_mode()
self._pub_rate.publish(100)
rate.sleep()
#return to normal
self.revert_relax()
return True
def puppet(self):
"""
"""
self.set_neutral()
rate = rospy.Rate(100)
start = rospy.Time.now()
control_joint_names = self._control_arm.joint_names()
puppet_joint_names = self._puppet_arm.joint_names()
done = False
print("Puppeting. Press any key to stop...")
while not done and not rospy.is_shutdown():
if iodevices.getch():
done = True
else:
elapsed = rospy.Time.now() - start
cmd = {}
for idx, name in enumerate(puppet_joint_names):
v = self._control_arm.joint_velocity(
control_joint_names[idx])
if name[-2:] in ('s0', 'e0', 'w0', 'w2'):
v = -v
cmd[name] = v * self._amp
self._puppet_arm.set_joint_velocities(cmd)
rate.sleep()
rate = rospy.Rate(100)
if not rospy.is_shutdown():
for i in range(100):
if rospy.is_shutdown():
return False
self._control_arm.set_joint_position_mode()
self._puppet_arm.set_joint_position_mode()
rate.sleep()
#return to normal
self.set_neutral()
return True
def map_joystick(joystick):
"""
maps joystick input to joint position commands
@param joystick - an instance of a Joystick
"""
left = baxter_interface.Limb('left')
right = baxter_interface.Limb('right')
grip_left = baxter_interface.Gripper('left')
grip_right = baxter_interface.Gripper('right')
lcmd = {}
rcmd = {}
#available joints
lj = left.joint_names()
rj = right.joint_names()
#abbreviations
jhi = lambda s: joystick.stick_value(s) > 0
jlo = lambda s: joystick.stick_value(s) < 0
bdn = joystick.button_down
bup = joystick.button_up
def print_help(bindings_list):
print("press any keyboard key to quit.")
for bindings in bindings_list:
for (test, cmd, doc) in bindings:
if callable(doc):
doc = doc()
print("%s: %s" % (str(test[1][0]), doc))
bindings_list = []
bindings = (
((bdn, ['rightTrigger']), (grip_left.close, []), "left gripper close"),
((bup, ['rightTrigger']), (grip_left.open, []), "left gripper open"),
((bdn, ['leftTrigger']), (grip_right.close, []), "right gripper close"),
((bup, ['leftTrigger']), (grip_right.open, []), "right gripper open"),
((jlo, ['leftStickHorz']), (set_j, [rcmd, right, rj, 0, 0.1]), lambda i=0:"right inc "+rj[i]),
((jhi, ['leftStickHorz']), (set_j, [rcmd, right, rj, 0, -0.1]), lambda i=0:"right dec "+rj[i]),
((jlo, ['rightStickHorz']), (set_j, [lcmd, left, lj, 0, 0.1]), lambda i=0:"left inc "+lj[i]),
((jhi, ['rightStickHorz']), (set_j, [lcmd, left, lj, 0, -0.1]), lambda i=0:"left dec "+lj[i]),
((jlo, ['leftStickVert']), (set_j, [rcmd, right, rj, 1, 0.1]), lambda i=1:"right inc "+rj[i]),
((jhi, ['leftStickVert']), (set_j, [rcmd, right, rj, 1, -0.1]), lambda i=1:"right dec "+rj[i]),
((jlo, ['rightStickVert']), (set_j, [lcmd, left, lj, 1, 0.1]), lambda i=1:"left inc "+lj[i]),
((jhi, ['rightStickVert']), (set_j, [lcmd, left, lj, 1, -0.1]), lambda i=1:"left dec "+lj[i]),
((bdn, ['rightBumper']), (rotate, [lj]), "left: cycle joint"),
((bdn, ['leftBumper']), (rotate, [rj]), "right: cycle joint"),
((bdn, ['btnRight']), (grip_left.calibrate, []), "left calibrate"),
((bdn, ['btnLeft']), (grip_right.calibrate, []), "right calibrate"),
((bdn, ['function1']), (print_help, [bindings_list]), "help"),
((bdn, ['function2']), (print_help, [bindings_list]), "help"),
)
bindings_list.append(bindings)
rate = rospy.Rate(100)
print_help(bindings_list)
print("press any key to stop. ")
while not rospy.is_shutdown():
c = iodevices.getch()
if c:
if c == '?':
print_help(bindings_list)
else:
return True
for (test, cmd, doc) in bindings:
if test[0](*test[1]):
cmd[0](*cmd[1])
if callable(doc):
print(doc())
else:
print(doc)
if len(lcmd):
left.set_joint_positions(lcmd)
lcmd.clear()
if len(rcmd):
right.set_joint_positions(rcmd)
rcmd.clear()
rate.sleep()
return False
def map_joystick(joystick):
"""
maps joystick input to gripper commands
@param joystick - an instance of a Joystick
"""
left = baxter_interface.Gripper('left')
right = baxter_interface.Gripper('right')
#abbreviations
jhi = lambda s: joystick.stick_value(s) > 0
jlo = lambda s: joystick.stick_value(s) < 0
bdn = joystick.button_down
bup = joystick.button_up
def print_help(bindings_list):
print("press any keyboard key to quit.")
for bindings in bindings_list:
for (test, cmd, doc) in bindings:
if callable(doc):
doc = doc()
print("%s %s: %s" % (test[0].__name__, str(test[1]), doc))
bindings_list = []
bindings = (
((bdn, ['btnDown']), (left.reboot, []), "left: reset"),
((bdn, ['btnLeft']), (right.reboot, []), "right: reset"),
((bdn, ['btnRight']), (left.calibrate, []), "left: calibrate"),
((bdn, ['btnUp']), (right.calibrate, []), "right: calibrate"),
((bdn, ['rightTrigger']), (left.close, []), "left: close"),
((bdn, ['leftTrigger']), (right.close, []), "right: close"),
((bup, ['rightTrigger']), (left.open, []), "left: open"),
((bup, ['leftTrigger']), (right.open, []), "right: open"),
((bdn, ['rightBumper']), (left.stop, []), "left: stop"),
((bdn, ['leftBumper']), (right.stop, []), "right: stop"),
((jlo, ['rightStickHorz']), (left.inc_position, [-5.0]),
"left: decrease position"),
((jlo, ['leftStickHorz']), (right.inc_position, [-5.0]),
"right: decrease position"),
((jhi, ['rightStickHorz']), (left.inc_position, [5.0]),
"left: increase position"),
((jhi, ['leftStickHorz']), (right.inc_position, [5.0]),
"right: increase position"),
((jlo, ['rightStickVert']), (left.inc_holding_force, [-5.0]),
"left: decrease holding force"),
((jlo, ['leftStickVert']), (right.inc_holding_force, [-5.0]),
"right: decrease holding force"),
((jhi, ['rightStickVert']), (left.inc_holding_force, [5.0]),
"left: increase holding force"),
((jhi, ['leftStickVert']), (right.inc_holding_force, [5.0]),
"right: increase holding force"),
((bdn, ['dPadDown']), (left.inc_velocity, [-5.0]),
"left: decrease velocity"),
((bdn, ['dPadLeft']), (right.inc_velocity, [-5.0]),
"right: decrease velocity"),
((bdn, ['dPadRight']), (left.inc_velocity, [5.0]),
"left: increase velocity"),
((bdn, ['dPadUp']), (right.inc_velocity, [5.0]),
"right: increase velocity"),
((bdn, ['function1']), (print_help, [bindings_list]), "help"),
((bdn, ['function2']), (print_help, [bindings_list]), "help"),
)
bindings_list.append(bindings)
rate = rospy.Rate(100)
print_help(bindings_list)
print("press any key to stop...")
while not rospy.is_shutdown():
c = iodevices.getch()
if c:
if c == '?':
print_help(bindings_list)
else:
return True
for (test, cmd, doc) in bindings:
if test[0](*test[1]):
cmd[0](*cmd[1])
if callable(doc):
print(doc())
else:
print(doc)
rate.sleep()
return False
def bringToAngles(self, limb, e0b, e1b, s0b, s1b, w0b, w1b, w2b):
#self.set_neutral()
"""
Brings left arm to the desired angles
"""
rate = rospy.Rate(1000);
start = rospy.Time.now()
done = False
print("Starting to move. Press any key to stop...")
# while not done and not rospy.is_shutdown():
# c = iodevices.getch()
# if c:
# done = True
# else:
# self._pub_rate.publish(1000)
# elapsed = rospy.Time.now() - start
# cmd = dict(zip(self._joint_names, [v_funcs[i](elapsed) for i in range(len(self._joint_names))]))
# self._left_arm.set_velocities(cmd)
# rate.sleep()
# self._left_arm.set_pose( dict(zip(['e0', 'e1', 's0', 's1', 'w0', 'w1', 'w2'], [-1.15, 1.32, -0.11, -0.62, 0.80, 1.27, 2.39])) )
self._left_arm.move_to_joint_positions(dict(zip(['e0', 'e1', 's0', 's1', 'w0', 'w1', 'w2'], [float(e0b), float(e1b), float(s0b), float(s1b), float(w0b), float(w1b), float(w2b)])))
#self._left_arm.move_to_joint_positions( dict(zip(['e0', 'e1', 's0', 's1', 'w0', 'w1', 'w2'], [-0.893885199733119,1.2679789443546783, -0.6385064965107536, -0.3050569420347243, 1.0130868024843116, 1.0695686598843879, -0.8670164430795886])) )
grip_left = baxter_interface.Gripper('left')
grip_right = baxter_interface.Gripper('right')
bindings = {
# key: (function, args, description)
',': (grip_left.close, [], "left: gripper close"),
'm': (grip_left.open, [], "left: gripper open"),
'/': (grip_left.calibrate, [], "left: gripper calibrate"),
}
if not done and not rospy.is_shutdown():
c = iodevices.getch()
if c:
#catch Esc or ctrl-c
if c in ['\x1b', '\x03']:
done = True
elif c in bindings:
cmd = bindings[c]
#expand binding to something like "set_j(right, 's0', 0.1)"
cmd[0](*cmd[1])
print("command: %s" % (cmd[2],))
else:
print("key bindings: ")
print(" Esc: Quit")
print(" ?: Help")
for key, val in sorted(bindings.items(), key=lambda x: x[1][2]):
print(" %s: %s" % (key, val[2]))
print("IDK")
#raw_input()
#time.sleep(6)
rate = rospy.Rate(100);
if not rospy.is_shutdown():
for i in range(100):
if rospy.is_shutdown():
return False
self._left_arm.set_position_mode()
self._pub_rate.publish(100)
rate.sleep()
#return to normal
self.set_neutral()
return True
def map_keyboard():
left = baxter_interface.Limb('left')
right = baxter_interface.Limb('right')
grip_left = baxter_interface.Gripper('left')
grip_right = baxter_interface.Gripper('right')
lj = left.joint_names()
rj = right.joint_names()
def set_j(limb, joint_name, delta):
current_position = limb.joint_angle(joint_name)
joint_command = {joint_name: current_position + delta}
limb.set_joint_positions(joint_command)
bindings = {
# key: (function, args, description)
'9': (set_j, [left, lj[0], 0.1], "left_s0 increase"),
'6': (set_j, [left, lj[0], -0.1], "left_s0 decrease"),
'8': (set_j, [left, lj[1], 0.1], "left_s1 increase"),
'7': (set_j, [left, lj[1], -0.1], "left_s1 decrease"),
'o': (set_j, [left, lj[2], 0.1], "left_e0 increase"),
'y': (set_j, [left, lj[2], -0.1], "left_e0 decrease"),
'i': (set_j, [left, lj[3], 0.1], "left_e1 increase"),
'u': (set_j, [left, lj[3], -0.1], "left_e1 decrease"),
'l': (set_j, [left, lj[4], 0.1], "left_w0 increase"),
'h': (set_j, [left, lj[4], -0.1], "left_w0 decrease"),
'k': (set_j, [left, lj[5], 0.1], "left_w1 increase"),
'j': (set_j, [left, lj[5], -0.1], "left_w1 decrease"),
'.': (set_j, [left, lj[6], 0.1], "left_w2 increase"),
'n': (set_j, [left, lj[6], -0.1], "left_w2 decrease"),
',': (grip_left.close, [], "left: gripper close"),
'm': (grip_left.open, [], "left: gripper open"),
'/': (grip_left.calibrate, [], "left: gripper calibrate"),
'4': (set_j, [right, rj[0], 0.1], "right_s0 increase"),
'1': (set_j, [right, rj[0], -0.1], "right_s0 decrease"),
'3': (set_j, [right, rj[1], 0.1], "right_s1 increase"),
'2': (set_j, [right, rj[1], -0.1], "right_s1 decrease"),
'r': (set_j, [right, rj[2], 0.1], "right_e0 increase"),
'q': (set_j, [right, rj[2], -0.1], "right_e0 decrease"),
'e': (set_j, [right, rj[3], 0.1], "right_e1 increase"),
'w': (set_j, [right, rj[3], -0.1], "right_e1 decrease"),
'f': (set_j, [right, rj[4], 0.1], "right_w0 increase"),
'a': (set_j, [right, rj[4], -0.1], "right_w0 decrease"),
'd': (set_j, [right, rj[5], 0.1], "right_w1 increase"),
's': (set_j, [right, rj[5], -0.1], "right_w1 decrease"),
'v': (set_j, [right, rj[6], 0.1], "right_w2 increase"),
'z': (set_j, [right, rj[6], -0.1], "right_w2 decrease"),
'c': (grip_right.close, [], "right: gripper close"),
'x': (grip_right.open, [], "right: gripper open"),
'b': (grip_right.calibrate, [], "right: gripper calibrate"),
}
done = False
print("Controlling joints. Press ? for help, Esc to quit.")
while not done and not rospy.is_shutdown():
c = iodevices.getch()
if c:
#catch Esc or ctrl-c
if c in ['\x1b', '\x03']:
done = True
elif c in bindings:
cmd = bindings[c]
#expand binding to something like "set_j(right, 's0', 0.1)"
cmd[0](*cmd[1])
print("command: %s" % (cmd[2], ))
else:
print("key bindings: ")
print(" Esc: Quit")
print(" ?: Help")
for key, val in sorted(bindings.items(),
key=lambda x: x[1][2]):
print(" %s: %s" % (key, val[2]))
def map_keyboard():
left = baxter_interface.Limb('left')
right = baxter_interface.Limb('right')
grip_left = baxter_interface.Gripper('left')
grip_right = baxter_interface.Gripper('right')
lj = left.joint_names()
rj = right.joint_names()
def set_j(limb, joint_name, delta):
current_position = limb.joint_angle(joint_name)
joint_command = {joint_name: current_position + delta}
limb.set_joint_positions(joint_command)
bindings = {
# key: (function, args, description)
'9': (set_j, [left, lj[0], 0.1], "left_s0 increase"),
'6': (set_j, [left, lj[0], -0.1], "left_s0 decrease"),
'8': (set_j, [left, lj[1], 0.1], "left_s1 increase"),
'7': (set_j, [left, lj[1], -0.1], "left_s1 decrease"),
'o': (set_j, [left, lj[2], 0.1], "left_e0 increase"),
'y': (set_j, [left, lj[2], -0.1], "left_e0 decrease"),
'i': (set_j, [left, lj[3], 0.1], "left_e1 increase"),
'u': (set_j, [left, lj[3], -0.1], "left_e1 decrease"),
'l': (set_j, [left, lj[4], 0.1], "left_w0 increase"),
'h': (set_j, [left, lj[4], -0.1], "left_w0 decrease"),
'k': (set_j, [left, lj[5], 0.1], "left_w1 increase"),
'j': (set_j, [left, lj[5], -0.1], "left_w1 decrease"),
'.': (set_j, [left, lj[6], 0.1], "left_w2 increase"),
'n': (set_j, [left, lj[6], -0.1], "left_w2 decrease"),
',': (grip_left.close, [], "left: gripper close"),
'm': (grip_left.open, [], "left: gripper open"),
'/': (grip_left.calibrate, [], "left: gripper calibrate"),
'4': (set_j, [right, rj[0], 0.1], "right_s0 increase"),
'1': (set_j, [right, rj[0], -0.1], "right_s0 decrease"),
'3': (set_j, [right, rj[1], 0.1], "right_s1 increase"),
'2': (set_j, [right, rj[1], -0.1], "right_s1 decrease"),
'r': (set_j, [right, rj[2], 0.1], "right_e0 increase"),
'q': (set_j, [right, rj[2], -0.1], "right_e0 decrease"),
'e': (set_j, [right, rj[3], 0.1], "right_e1 increase"),
'w': (set_j, [right, rj[3], -0.1], "right_e1 decrease"),
'f': (set_j, [right, rj[4], 0.1], "right_w0 increase"),
'a': (set_j, [right, rj[4], -0.1], "right_w0 decrease"),
'd': (set_j, [right, rj[5], 0.1], "right_w1 increase"),
's': (set_j, [right, rj[5], -0.1], "right_w1 decrease"),
'v': (set_j, [right, rj[6], 0.1], "right_w2 increase"),
'z': (set_j, [right, rj[6], -0.1], "right_w2 decrease"),
'c': (grip_right.close, [], "right: gripper close"),
'x': (grip_right.open, [], "right: gripper open"),
'b': (grip_right.calibrate, [], "right: gripper calibrate"),
}
done = False
print("Controlling joints. Press ? for help, Esc to quit.")
while not done and not rospy.is_shutdown():
c = iodevices.getch()
if c:
#catch Esc or ctrl-c
if c in ['\x1b', '\x03']:
done = True
elif c in bindings:
cmd = bindings[c]
#expand binding to something like "set_j(right, 's0', 0.1)"
cmd[0](*cmd[1])
print("command: %s" % (cmd[2],))
else:
print("key bindings: ")
print(" Esc: Quit")
print(" ?: Help")
for key, val in sorted(bindings.items(), key=lambda x: x[1][2]):
print(" %s: %s" % (key, val[2]))
def map_joystick(joystick):
"""
maps joystick input to gripper commands
@param joystick - an instance of a Joystick
"""
left = baxter_interface.Gripper("left")
right = baxter_interface.Gripper("right")
# abbreviations
jhi = lambda s: joystick.stick_value(s) > 0
jlo = lambda s: joystick.stick_value(s) < 0
bdn = joystick.button_down
bup = joystick.button_up
def print_help(bindings_list):
print("press any keyboard key to quit.")
for bindings in bindings_list:
for (test, cmd, doc) in bindings:
if callable(doc):
doc = doc()
print("%s %s: %s" % (test[0].__name__, str(test[1]), doc))
bindings_list = []
bindings = (
((bdn, ["btnDown"]), (left.reboot, []), "left: reset"),
((bdn, ["btnLeft"]), (right.reboot, []), "right: reset"),
((bdn, ["btnRight"]), (left.calibrate, []), "left: calibrate"),
((bdn, ["btnUp"]), (right.calibrate, []), "right: calibrate"),
((bdn, ["rightTrigger"]), (left.close, []), "left: close"),
((bdn, ["leftTrigger"]), (right.close, []), "right: close"),
((bup, ["rightTrigger"]), (left.open, []), "left: open"),
((bup, ["leftTrigger"]), (right.open, []), "right: open"),
((bdn, ["rightBumper"]), (left.stop, []), "left: stop"),
((bdn, ["leftBumper"]), (right.stop, []), "right: stop"),
((jlo, ["rightStickHorz"]), (left.inc_position, [-5.0]), "left: decrease position"),
((jlo, ["leftStickHorz"]), (right.inc_position, [-5.0]), "right: decrease position"),
((jhi, ["rightStickHorz"]), (left.inc_position, [5.0]), "left: increase position"),
((jhi, ["leftStickHorz"]), (right.inc_position, [5.0]), "right: increase position"),
((jlo, ["rightStickVert"]), (left.inc_holding_force, [-5.0]), "left: decrease holding force"),
((jlo, ["leftStickVert"]), (right.inc_holding_force, [-5.0]), "right: decrease holding force"),
((jhi, ["rightStickVert"]), (left.inc_holding_force, [5.0]), "left: increase holding force"),
((jhi, ["leftStickVert"]), (right.inc_holding_force, [5.0]), "right: increase holding force"),
((bdn, ["dPadDown"]), (left.inc_velocity, [-5.0]), "left: decrease velocity"),
((bdn, ["dPadLeft"]), (right.inc_velocity, [-5.0]), "right: decrease velocity"),
((bdn, ["dPadRight"]), (left.inc_velocity, [5.0]), "left: increase velocity"),
((bdn, ["dPadUp"]), (right.inc_velocity, [5.0]), "right: increase velocity"),
((bdn, ["function1"]), (print_help, [bindings_list]), "help"),
((bdn, ["function2"]), (print_help, [bindings_list]), "help"),
)
bindings_list.append(bindings)
rate = rospy.Rate(100)
print_help(bindings_list)
print("press any key to stop...")
while not rospy.is_shutdown():
c = iodevices.getch()
if c:
if c == "?":
print_help(bindings_list)
else:
return True
for (test, cmd, doc) in bindings:
if test[0](*test[1]):
cmd[0](*cmd[1])
if callable(doc):
print(doc())
else:
print(doc)
rate.sleep()
return False
def bringToAngles(self, limb, e0b, e1b, s0b, s1b, w0b, w1b, w2b):
#self.set_neutral()
"""
Brings left arm to the desired angles
"""
rate = rospy.Rate(1000)
start = rospy.Time.now()
done = False
print("Starting to move. Press any key to stop...")
# while not done and not rospy.is_shutdown():
# c = iodevices.getch()
# if c:
# done = True
# else:
# self._pub_rate.publish(1000)
# elapsed = rospy.Time.now() - start
# cmd = dict(zip(self._joint_names, [v_funcs[i](elapsed) for i in range(len(self._joint_names))]))
# self._left_arm.set_velocities(cmd)
# rate.sleep()
# self._left_arm.set_pose( dict(zip(['e0', 'e1', 's0', 's1', 'w0', 'w1', 'w2'], [-1.15, 1.32, -0.11, -0.62, 0.80, 1.27, 2.39])) )
self._left_arm.move_to_joint_positions(
dict(
zip(['e0', 'e1', 's0', 's1', 'w0', 'w1', 'w2'], [
float(e0b),
float(e1b),
float(s0b),
float(s1b),
float(w0b),
float(w1b),
float(w2b)
])))
#self._left_arm.move_to_joint_positions( dict(zip(['e0', 'e1', 's0', 's1', 'w0', 'w1', 'w2'], [-0.893885199733119,1.2679789443546783, -0.6385064965107536, -0.3050569420347243, 1.0130868024843116, 1.0695686598843879, -0.8670164430795886])) )
grip_left = baxter_interface.Gripper('left')
grip_right = baxter_interface.Gripper('right')
bindings = {
# key: (function, args, description)
',': (grip_left.close, [], "left: gripper close"),
'm': (grip_left.open, [], "left: gripper open"),
'/': (grip_left.calibrate, [], "left: gripper calibrate"),
}
if not done and not rospy.is_shutdown():
c = iodevices.getch()
if c:
#catch Esc or ctrl-c
if c in ['\x1b', '\x03']:
done = True
elif c in bindings:
cmd = bindings[c]
#expand binding to something like "set_j(right, 's0', 0.1)"
cmd[0](*cmd[1])
print("command: %s" % (cmd[2], ))
else:
print("key bindings: ")
print(" Esc: Quit")
print(" ?: Help")
for key, val in sorted(bindings.items(),
key=lambda x: x[1][2]):
print(" %s: %s" % (key, val[2]))
print("IDK")
#raw_input()
#time.sleep(6)
rate = rospy.Rate(100)
if not rospy.is_shutdown():
for i in range(100):
if rospy.is_shutdown():
return False
self._left_arm.set_position_mode()
self._pub_rate.publish(100)
rate.sleep()
#return to normal
self.set_neutral()
return True
def map_joystick(joystick):
"""
maps joystick input to joint position commands
@param joystick - an instance of a Joystick
"""
left = baxter_interface.Limb('left')
right = baxter_interface.Limb('right')
grip_left = baxter_interface.Gripper('left')
grip_right = baxter_interface.Gripper('right')
lcmd = {}
rcmd = {}
#available joints
lj = left.joint_names()
rj = right.joint_names()
#abbreviations
jhi = lambda s: joystick.stick_value(s) > 0
jlo = lambda s: joystick.stick_value(s) < 0
bdn = joystick.button_down
bup = joystick.button_up
def print_help(bindings_list):
print("press any keyboard key to quit.")
for bindings in bindings_list:
for (test, cmd, doc) in bindings:
if callable(doc):
doc = doc()
print("%s: %s" % (str(test[1][0]), doc))
bindings_list = []
bindings = (
((bdn, ['rightTrigger']), (grip_left.close, []), "left gripper close"),
((bup, ['rightTrigger']), (grip_left.open, []), "left gripper open"),
((bdn, ['leftTrigger']), (grip_right.close, []),
"right gripper close"),
((bup, ['leftTrigger']), (grip_right.open, []), "right gripper open"),
((jlo, ['leftStickHorz']), (set_j, [rcmd, right, rj, 0, 0.1]),
lambda i=0: "right inc " + rj[i]),
((jhi, ['leftStickHorz']), (set_j, [rcmd, right, rj, 0, -0.1]),
lambda i=0: "right dec " + rj[i]),
((jlo, ['rightStickHorz']), (set_j, [lcmd, left, lj, 0, 0.1]),
lambda i=0: "left inc " + lj[i]),
((jhi, ['rightStickHorz']), (set_j, [lcmd, left, lj, 0, -0.1]),
lambda i=0: "left dec " + lj[i]),
((jlo, ['leftStickVert']), (set_j, [rcmd, right, rj, 1, 0.1]),
lambda i=1: "right inc " + rj[i]),
((jhi, ['leftStickVert']), (set_j, [rcmd, right, rj, 1, -0.1]),
lambda i=1: "right dec " + rj[i]),
((jlo, ['rightStickVert']), (set_j, [lcmd, left, lj, 1, 0.1]),
lambda i=1: "left inc " + lj[i]),
((jhi, ['rightStickVert']), (set_j, [lcmd, left, lj, 1, -0.1]),
lambda i=1: "left dec " + lj[i]),
((bdn, ['rightBumper']), (rotate, [lj]), "left: cycle joint"),
((bdn, ['leftBumper']), (rotate, [rj]), "right: cycle joint"),
((bdn, ['btnRight']), (grip_left.calibrate, []), "left calibrate"),
((bdn, ['btnLeft']), (grip_right.calibrate, []), "right calibrate"),
((bdn, ['function1']), (print_help, [bindings_list]), "help"),
((bdn, ['function2']), (print_help, [bindings_list]), "help"),
)
bindings_list.append(bindings)
rate = rospy.Rate(100)
print_help(bindings_list)
print("press any key to stop. ")
while not rospy.is_shutdown():
c = iodevices.getch()
if c:
if c == '?':
print_help(bindings_list)
else:
return True
for (test, cmd, doc) in bindings:
if test[0](*test[1]):
cmd[0](*cmd[1])
if callable(doc):
print(doc())
else:
print(doc)
if len(lcmd):
left.set_joint_positions(lcmd)
lcmd.clear()
if len(rcmd):
right.set_joint_positions(rcmd)
rcmd.clear()
rate.sleep()
return False