def main():
rospy.init_node("release_cup")
rs = baxter_interface.RobotEnable(CHECK_VERSION)
init_state = rs.state().enabled
left = baxter_interface.Gripper('left', CHECK_VERSION)
# right = baxter_interface.Gripper('right', CHECK_VERSION)
rs.enable()
rospy.Subscriber("/robot/limb/left/endpoint_state",
EndpointState,
get_present_state,
left,
queue_size=1)
gripper_pub = rospy.Publisher('cup_taken', Bool, queue_size=10)
done = False
while not done and not rospy.is_shutdown():
c = baxter_external_devices.getch()
if c:
if c in ['\x1b', '\x03']:
done = True
if c == 'w':
left.open()
elif c == 'q':
left.close()
def map_keyboard():
bindings = {
# key: (function, args, description)
'a': (move_end_effector, ['right', 'Y', 0.1], "Y increase"),
'd': (move_end_effector, ['right', 'Y', -0.1], "Y decrease"),
'w': (move_end_effector, ['right', 'X', 0.1], "x increase"),
's': (move_end_effector, ['right', 'X', -0.1], "X decrease"),
}
done = False
print("Controlling robot. Esc to quit.")
while not done and not rospy.is_shutdown():
c = baxter_external_devices.getch()
if c:
#catch Esc or ctrl-c
if c in ['\x1b', '\x03']:
done = True
rospy.signal_shutdown("Example finished.")
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 adjust_yn():
def yes():
done = True
rospy.signal_shutdown("Shutting Down")
def no():
main_menu()
bindings = {
# key: (function, args, description)
'y': (yes,[], "Let Robot Continue"),
'n': (no, [], "Enter Control Mode"),
}
done = False
while not done and not rospy.is_shutdown():
c = baxter_external_devices.getch()
if c:
#catch Esc or ctrl-c
if c in ['\x1b', '\x03']:
done = True
rospy.signal_shutdown("Shutting Down.")
elif c in bindings:
cmd = bindings[c]
cmd[0](*cmd[1])
else:
print("key bindings: ")
for key, val in sorted(bindings.items(),
key=lambda x: x[1][2]):
print(" %s: %s" % (key, val[2]))
def map_keyboard():
bindings = {
# key: (function, args, description)
'a': (move_end_effector, ['right', 'Y', 0.1], "Y increase"),
'd': (move_end_effector, ['right', 'Y', -0.1], "Y decrease"),
'w': (move_end_effector, ['right', 'X', 0.1], "x increase"),
's': (move_end_effector, ['right', 'X', -0.1], "X decrease"),
}
done = False
print("Controlling robot. Esc to quit.")
while not done and not rospy.is_shutdown():
c = baxter_external_devices.getch()
if c:
#catch Esc or ctrl-c
if c in ['\x1b', '\x03']:
done = True
rospy.signal_shutdown("Example finished.")
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 grids_location_recorder(self):
done_flag = False
#x_counter = 0
#y_counter = 0
counter = 0
self.GridLocations = []
print "Enter Keyboard Command:"
while not done_flag and not rospy.is_shutdown():
c = baxter_external_devices.getch()
if c:
if c in ['q']:
print "Grid Location Recorder Stops"
done_flag = True
elif c in ['r']:
cur_pose = self.current_poses[self.arm]
self.GridLocations.append(cur_pose) # will be 9 in total
print "Current Pose", cur_pose
print len(self.GridLocations
), " number of grid location recorded"
elif c in ['u']:
pass
#rospy.sleep(0.1)
print "Saving Grid Location Data"
self.save_grid_locations()
def rotation_keyboard():
left = baxter_interface.Limb('left')
right = baxter_interface.Limb('right')
grip_left = baxter_interface.Gripper('left', CHECK_VERSION)
grip_right = baxter_interface.Gripper('right', CHECK_VERSION)
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)
def complete():
done = True
rospy.signal_shutdown("Shutting Down")
bindings = {
# key: (function, args, description)
'k': (set_j, [left, lj[6], 0.1], "left wrist increase"),
'l': (set_j, [left, lj[6], -0.1], "left wrist decrease"),
'a': (set_j, [right, rj[6], 0.1], "right wrist increase"),
's': (set_j, [right, rj[6], -0.1], "right wrist decrease"),
'd': (complete,[], "Quit"),
'm': (main_menu,[], "Return To Main Menu"),
}
done = False
print("Controlling Rotational Orientation")
print("key bindings: ")
print(" ?: Help")
for key, val in sorted(bindings.items(),
key=lambda x: x[1][2]):
print(" %s: %s" % (key, val[2]))
#os.system('rosrun baxter_examples xdisplay_image.py --file ~/Pictures/key_bindings.jpg')
while not done and not rospy.is_shutdown():
c = baxter_external_devices.getch()
if c:
#catch Esc or ctrl-c
if c in ['\x1b', '\x03']:
done = True
rospy.signal_shutdown("Program finished.")
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" % (
[2],))
else:
print("key bindings: ")
print(" d: Quit")
print(" ?: Help")
for key, val in sorted(bindings.items(),
key=lambda x: x[1][2]):
print(" %s: %s" % (key, val[2]))
def adjust_anchor(anchor):
old_point = anchor['position']
point = Point(x=old_point.x, y=old_point.y, z=old_point.z)
anchor['position'] = point
old_color = anchor['color']
anchor['color'] = (0, 1, 0, 1)
static_pub.publish("/alignment_markers", create_marker_array(anchor))
STEP = 0.01
print(
"Move the green dot so that it is in the same position relative to the robot model as the actual marker on the physical"
)
print("robot. Use the following keyboard to adjust the dot:")
print("")
print(" w/s - Up/Down")
print(" a/d - Left/Right")
print(" q/e - Forward/Backward")
print(" 1 - Set step size to 10cm")
print(" 2 - Set step size to 1cm")
print(" 3 - Set step size to 1mm")
print(" Esc - Cancel")
print(" Enter - Save")
while True:
c = baxter_external_devices.getch()
if c:
#catch Esc or ctrl-c
if c in ['\x1b', '\x03']:
anchor['position'] = old_point
break
elif c == 'w':
point.y = point.y - STEP
elif c == 's':
point.y = point.y + STEP
elif c == 'a':
point.x = point.x - STEP
elif c == 'd':
point.x = point.x + STEP
elif c == 'q':
point.z = point.z + STEP
elif c == 'e':
point.z = point.z - STEP
elif c == '1':
STEP = 0.1
print("Step set to 10cm.")
elif c == '2':
STEP = 0.01
print("Step set to 1cm.")
elif c == '3':
STEP = 0.001
print("Step set to 1mm.")
elif c in ['\x0d']:
break
anchor['color'] = old_color
def map_keyboard():
left = baxter_interface.Limb('left')
# right = baxter_interface.Limb('right')
grip_left = baxter_interface.Gripper('left', CHECK_VERSION)
# grip_right = baxter_interface.Gripper('right', CHECK_VERSION)
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")
}
done = False
print("Controlling joints. Press ? for help, Esc to quit.")
while not done and not rospy.is_shutdown():
c = baxter_external_devices.getch()
if c:
#catch Esc or ctrl-c
if c in ['\x1b', '\x03']:
done = True
rospy.signal_shutdown("Example finished.")
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():
printHelper()
rospy.loginfo('press ? to print help')
while not rospy.is_shutdown():
c = baxter_external_devices.getch()
if c:
#catch Esc or ctrl-c
if c in ['\x1b', '\x03']:
rospy.signal_shutdown("Finished.Exiting...")
return
checkCommand(c)
def map_keyboard():
printHelper()
rospy.loginfo('press ? to print help')
while not rospy.is_shutdown():
c = baxter_external_devices.getch()
if c:
#catch Esc or ctrl-c
if c in ['\x1b', '\x03']:
rospy.signal_shutdown("Finished.Exiting...")
return
checkCommand(c)
def print_endpoint_effort(self):
limb = baxter_interface.Limb(self.limb_side)
done = False
while not done and not rospy.is_shutdown():
c = baxter_external_devices.getch()
wrench = limb.endpoint_effort()
print('Force (x,y,z): ' + str(wrench['force']))
time.sleep(0.5)
if c:
if c in ['\x1b', '\x03']: #check if ESC of CTRL-C pressed
done = True
def main_menu():
def complete():
done = True
rospy.signal_shutdown("Shutting Down")
def rotate():
rotation_keyboard()
def xpos():
xpos = raw_input("Enter x adjustment (positive or negative): ")
xpos ='x: '+xpos
msg_publisher(xpos)
def ypos():
ypos = raw_input("Enter y adjustment (positive or negative): ")
ypos ='y: '+ypos
msg_publisher(ypos)
def zpos():
zpos = raw_input("Enter z adjustment (positive or negative): ")
zpos ='z: '+zpos
msg_publisher(zpos)
bindings = {
# key: (function, args, description)
'd': (complete,[], "Quit"),
'r': (rotate,[], "Adjust Rotational Orientation"),
'x': (xpos,[],"Input X Position Adjustment"),
'y': (ypos,[],"Input Y Position Adjustment"),
'z': (zpos,[],"Input Z Position Adjustment"),
}
print("Main Menu for Control Mode")
print("key bindings: ")
for key, val in sorted(bindings.items(),
key=lambda x: x[1][2]):
print(" %s: %s" % (key, val[2]))
print(" ?: Help")
done = False
while not done and not rospy.is_shutdown():
c = baxter_external_devices.getch()
if c:
#catch Esc or ctrl-c
if c in ['\x1b', '\x03']:
done = True
rospy.signal_shutdown("Shutting Down.")
elif c in bindings:
cmd = bindings[c]
cmd[0](*cmd[1])
else:
print("key bindings: ")
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', CHECK_VERSION)
grip_right = baxter_interface.Gripper('right', CHECK_VERSION)
lj = left.joint_names()
rj = right.joint_names()
bhc = BaxterHapticControl('left', 0)
bindings = {
# key: (function, args, description)
'k': (bhc._velocity, [0, 0, 10, 0, 0,
0], "Velocity Controller Activated"),
'l': (bhc._velocity, [10, 0, 0, 0, 0,
0], "Velocity Controller Activated"),
'x': (bhc._delta_x_position, [0.1], "Pos X Increasing"),
'c': (bhc._delta_x_position, [-0.1], "Pos X Decreasing"),
'j': (bhc._determinant, [], "Determinant"),
'f': (bhc._set_point, [
0.75, 0.15, -0.129,
[[-0.9987068, -0.0499725, 0.0093573],
[-0.0498289, 0.9986452, 0.0149960],
[-0.0100940, 0.0145103, -0.9998438]]
], "Setting Pose"),
'd': (bhc._starting_position, [], "Starting Pose"),
'e': (bhc._get_current_endpose, [], "Current EE Pose"),
'm': (bhc._create_planned_trajectory, [], "Created Trajectory"),
'r': (bhc._neutral_position, [], "Resetting")
}
done = False
print("Controlling joints. Press ? for help, Esc to quit.")
rate = rospy.Rate(1000) #Controls rate of the loop below.
while not done and not rospy.is_shutdown():
c = baxter_external_devices.getch()
if c:
#catch Esc or ctrl-c
if c in ['\x1b', '\x03']:
done = True
rospy.signal_shutdown("Example finished.")
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(self):
# initialize interfaces
print("Getting robot state... ")
rs = baxter_interface.RobotEnable(CHECK_VERSION)
init_state = rs.state().enabled
limb_0 = baxter_interface.Gripper(self.limb_name, CHECK_VERSION)
done = False
print("Enabling robot... ")
rs.enable()
print("Controlling grippers. Press ? for help, Esc to quit.")
while not done and not rospy.is_shutdown():
c = baxter_external_devices.getch()
if c:
if c in ['\x1b', '\x03']:
done = True
elif c == 'a':
rospy.loginfo("%s is pressed" %c)
self.move((0,-1,0),0.05)
elif c == 'd':
rospy.loginfo("%s is pressed" %c)
self.move((0,1,0),0.05)
elif c == 'w':
rospy.loginfo("%s is pressed" %c)
self.move((-1,0,0),0.05)
elif c == 's':
rospy.loginfo("%s is pressed" %c)
self.move((1,0,0),0.05)
elif c == 'z':
rospy.loginfo("%s is pressed" %c)
self.move((0,0,-1),0.05)
elif c == 'x':
rospy.loginfo("%s is pressed" %c)
self.move((0,0,1),0.05)
else:
print("Not implement it yet...")
# cmd = self.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(self.bindings.items(),
# key=lambda x: x[1][2]):
# print(" %s: %s" % (key, val[2]))
# force shutdown call if caught by key handler
rospy.signal_shutdown("Move.py finished.")
def execute(self, userdata):
if self._init:
self.move_pose = copy.deepcopy(userdata.pick_object_pose)
self._init = False
def set_pose(direction, delta):
move_pose = copy.deepcopy(self.move_pose)
move_scale = self.move_scale
if direction == 'f':
move_scale = move_scale + 1
if move_scale == 11:
move_scale = 10
print move_scale
elif direction == 'b':
move_scale = move_scale - 1
if move_scale == 0:
move_scale = 1
print move_scale
elif direction == 'x':
move_pose.position.x = move_pose.position.x + delta * move_scale
elif direction == 'y':
move_pose.position.y = move_pose.position.y + delta * move_scale
elif direction == 'z':
move_pose.position.z = move_pose.position.z + delta * move_scale
self.move_pose = copy.deepcopy(move_pose)
userdata.move_pose = copy.deepcopy(move_pose)
self.move_scale = move_scale
bindings = {
'2': (set_pose, ['x', -0.01], "x decrease 0.01!"),
'8': (set_pose, ['x', 0.01], "x increase 0.01!"),
'4': (set_pose, ['y', -0.01], "y decrease 0.01!"),
'6': (set_pose, ['y', 0.01], "y increase 0.01!"),
's': (set_pose, ['z', -0.01], "z decrease 0.01!"),
'w': (set_pose, ['z', 0.01], "z increase 0.01!"),
'1': (set_pose, ['f', 1], "Multiply scale * increase 1!"),
'3': (set_pose, ['b', 1], "Multiply scale * decrease 1!")
}
done = False
while not done and not rospy.is_shutdown():
c = baxter_external_devices.getch()
if c:
if c in bindings:
cmd = bindings[c]
cmd[0](*cmd[1])
print("command: %s" % cmd[2])
done = True
return 'Succeed'
def handle_keyboard(engine, mgame):
'Funcion for allowing us to interface with processes via the keyboard'
while not rospy.is_shutdown():
# Get keyboard input only when it changes (-1 input facilitates this)
c = baxter_external_devices.getch(-1)
if c:
if c == 'c':
engine._clapDetected('right')
if c == 'x':
engine._clapDetected('left')
if c == 's':
mgame.waiting = False
if c == 'f':
os._exit(0)
if c == 'q':
mgame.stop()
def talker():
# function menu
print("Use 1-6 to control the increment of left arm")
print("Use Control + (1-6) to control the negative increment of left arm")
print("Use q-y to control the right arm")
print("Use Control + (q-y) to control the negative increment of right arm")
#initialize node and topic
pub = rospy.Publisher('ik', String, queue_size=10)
rospy.init_node('ik_talker', anonymous=True)
# mapping from key to function
bindings = {
'left_arm_increment': ['1', '2', '3', '4', '5', '6'],
'left_arm_decrement': ['!', '@', '#', '$', '%', '^'],
'right_arm_increment': ['q', 'w', 'e', 'r', 't', 'y'],
'right_arm_decrement': ['Q', 'W', 'E', 'R', 'T', 'Y']
}
done = False
message = ''
while not done and not rospy.is_shutdown():
c = baxter_external_devices.getch()
if c:
#catch Esc or ctrl-c
if c in ['\x1b', '\x03']:
done = True
rospy.signal_shutdown("Example finished.")
elif c in bindings['left_arm_increment']:
message = "left_joint_increse " + c
elif c in bindings['left_arm_decrement']:
message = "left_joint_decrese " + c
elif c in bindings['right_arm_increment']:
message = "right_joint_increase " + c
elif c in bindings['right_arm_decrement']:
message = "right_joint_decrease " + c
# test begin
elif c == 'z':
message = "left limb " + c
# test end
else:
rospy.loginfo('key error!')
if message != '':
#publish message to topic
rospy.loginfo(message)
pub.publish(message)
def map_keyboard():
pub = rospy.Publisher("end_effector_command", String, queue_size=10)
keyboard_binding = {}
keyboard_binding["w"] = "up"
keyboard_binding["s"] = "down"
keyboard_binding["a"] = "left"
keyboard_binding["d"] = "right"
keyboard_binding["q"] = "backward"
keyboard_binding["e"] = "forward"
keyboard_binding["f"] = "switch limb to "
keyboard_binding["z"] = "orientation_x"
keyboard_binding[" "] = "keep"
keyboard_binding["k"] = "further"
keyboard_binding["l"] = "closer"
limb = ["right", "left"]
current_limb = 0
command = list()
rate = rospy.Rate(6)
print limb[current_limb] + " limb under control..."
while not rospy.is_shutdown():
c = baxter_external_devices.getch()
if c:
#catch Esc or ctrl-c
if c in ['\x1b', '\x03']:
rospy.signal_shutdown("Finished.Exiting...")
if (c in keyboard_binding.keys()):
if (c == "f"):
current_limb = 1 - current_limb
print "control switch to " + limb[current_limb]
pub.publish(
String(keyboard_binding[c] + limb[current_limb]))
else:
print "sending command: " + limb[
current_limb] + " limb " + keyboard_binding[c]
#pub.publish(String(keyboard_binding[c]))
command = list()
for s in range(0, 1):
command.append(keyboard_binding[c])
else:
print "invalid command: " + c
#print limb[current_limb] + " limb under control..."
# new control type
if (len(command)):
pub.publish(String(command.pop()))
rate.sleep()
def map_keyboard():
pub = rospy.Publisher("end_effector_command", String, queue_size=10)
keyboard_binding = {}
keyboard_binding["w"] = "up"
keyboard_binding["s"] = "down"
keyboard_binding["a"] = "left"
keyboard_binding["d"] = "right"
keyboard_binding["q"] = "backward"
keyboard_binding["e"] = "forward"
keyboard_binding["f"] = "switch limb to "
keyboard_binding["z"] = "orientation_x"
keyboard_binding[" "] = "keep"
keyboard_binding["k"] = "further"
keyboard_binding["l"] = "closer"
limb = ["right", "left"]
current_limb = 0
command = list()
rate = rospy.Rate(6)
rospy.loginfo(limb[current_limb] + " limb under control...")
while not rospy.is_shutdown():
c = baxter_external_devices.getch()
if c:
# catch Esc or ctrl-c
if c in ["\x1b", "\x03"]:
rospy.signal_shutdown("Finished.Exiting...")
if c in keyboard_binding.keys():
if c == "f":
current_limb = 1 - current_limb
rospy.loginfo("control switch to " + limb[current_limb])
pub.publish(String(keyboard_binding[c] + limb[current_limb]))
else:
rospy.loginfo("sending command: " + limb[current_limb] + " limb " + keyboard_binding[c])
# pub.publish(String(keyboard_binding[c]))
command = list()
for s in range(0, 1):
command.append(keyboard_binding[c])
else:
rospy.loginfo("invalid command: " + c)
# print limb[current_limb] + " limb under control..."
# new control type
if len(command):
pub.publish(String(command.pop()))
rate.sleep()
def map_keyboard():
pub = rospy.Publisher("end_effector_command", String, queue_size=10)
keyboard_binding = {}
keyboard_binding["w"] = "up"
keyboard_binding["s"] = "down"
keyboard_binding["a"] = "left"
keyboard_binding["d"] = "right"
keyboard_binding["q"] = "backward"
keyboard_binding["e"] = "forward"
keyboard_binding["f"] = "switch limb "
keyboard_binding["z"] = "orientation_x"
keyboard_binding[" "] = "keep"
keyboard_binding["k"] = "further"
keyboard_binding["l"] = "closer"
limb = ["right", "left"]
current_limb = 0
rospy.loginfo(limb[current_limb] + " limb under control...")
rospy.loginfo('press ? to print help')
while not rospy.is_shutdown():
c = baxter_external_devices.getch()
if c:
#catch Esc or ctrl-c
if c in ['\x1b', '\x03']:
rospy.signal_shutdown("Finished.Exiting...")
return
if c == '?':
printHelper(keyboard_binding)
if (c in keyboard_binding.keys()):
if (c == "f"):
current_limb = 1 - current_limb
rospy.loginfo("control switch to " + limb[current_limb])
pub.publish(
String(keyboard_binding[c] + limb[current_limb]))
else:
rospy.loginfo("sending command: " + limb[current_limb] +
" limb " + keyboard_binding[c])
pub.publish(String(keyboard_binding[c]))
else:
rospy.loginfo("invalid command: " + c)
rospy.loginfo('press ? to print help')
def select_from_list(lst, allow_cancel=True):
if allow_cancel:
print("Select one (Esc to cancel)-")
else:
print("Select one-")
for i in range(len(lst)):
print("\t" + str(i) + ": " + str(lst[i]))
while True:
c = baxter_external_devices.getch()
if c:
#catch Esc or ctrl-c
if c in ['\x1b', '\x03']:
if allow_cancel:
return -1
elif c >= '0' and c <= '9':
print("Selected " + str(lst[int(c)]))
return int(c)
else:
rospy.sleep(0.001)
def map_keyboard():
pub = rospy.Publisher("end_effector_command", String, queue_size=10);
keyboard_binding = {}
keyboard_binding["w"] = "up"
keyboard_binding["s"] = "down"
keyboard_binding["a"] = "left"
keyboard_binding["d"] = "right"
keyboard_binding["q"] = "backward"
keyboard_binding["e"] = "forward"
keyboard_binding["f"] = "switch limb "
keyboard_binding["z"] = "orientation_x"
keyboard_binding[" "] = "keep"
keyboard_binding["k"] = "further"
keyboard_binding["l"] = "closer"
limb = ["right", "left"]
current_limb = 0
rospy.loginfo(limb[current_limb] + " limb under control...")
rospy.loginfo('press ? to print help')
while not rospy.is_shutdown():
c = baxter_external_devices.getch()
if c:
#catch Esc or ctrl-c
if c in ['\x1b', '\x03']:
rospy.signal_shutdown("Finished.Exiting...")
return
if c == '?':
printHelper(keyboard_binding)
if(c in keyboard_binding.keys()):
if (c == "f"):
current_limb = 1 - current_limb
rospy.loginfo("control switch to " + limb[current_limb])
pub.publish(String(keyboard_binding[c] + limb[current_limb]))
else:
rospy.loginfo("sending command: " + limb[current_limb] + " limb " + keyboard_binding[c])
pub.publish(String(keyboard_binding[c]))
else:
rospy.loginfo("invalid command: " + c)
rospy.loginfo('press ? to print help')
def map_keyboard():
left = baxter_interface.Limb('left')
right = baxter_interface.Limb('right')
grip_left = baxter_interface.Gripper('left', CHECK_VERSION)
grip_right = baxter_interface.Gripper('right', CHECK_VERSION)
lj = left.joint_names()
rj = right.joint_names()
pnp = PickAndPlace('left', 0)
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': (handle_turtle_pose, [], "increase z"),
}
done = False
print("Controlling joints. Press ? for help, Esc to quit.")
while not done and not rospy.is_shutdown():
c = baxter_external_devices.getch()
if c:
#catch Esc or ctrl-c
if c in ['\x1b', '\x03']:
done = True
rospy.signal_shutdown("Example finished.")
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(self):
# initialize interfaces
print("Getting robot state... ")
rs = baxter_intercontrollerface.RobotEnable(CHECK_VERSION)
init_state = rs.state().enabled
limb_0 = baxter_interface.Gripper(self.limb_name, CHECK_VERSION)
done = False
print("Enabling robot... ")
rs.enable()
print("Controlling grippers. Press ? for help, Esc to quit.")
while not done and not rospy.is_shutdown():
c = baxter_external_devices.getch()
if c:
if c in ['\x1b', '\x03']:
done = True
elif c == 'a':
rospy.loginfo("%s is pressed" % c)
self.move((0, -1, 0), 0.05)
elif c == 'd':
rospy.loginfo("%s is pressed" % c)
self.move((0, 1, 0), 0.05)
elif c == 'w':
rospy.loginfo("%s is pressed" % c)
self.move((-1, 0, 0), 0.05)
elif c == 's':
rospy.loginfo("%s is pressed" % c)
self.move((1, 0, 0), 0.05)
elif c == 'z':
rospy.loginfo("%s is pressed" % c)
self.move((0, 0, -1), 0.05)
elif c == 'x':
rospy.loginfo("%s is pressed" % c)
self.move((0, 0, 1), 0.05)
else:
print("Not implement it yet...")
rospy.signal_shutdown("Move.py finished.")
def map_keyboard(self):
# initialize interfaces
print("Getting robot state... ")
rs = baxter_intercontrollerface.RobotEnable(CHECK_VERSION)
init_state = rs.state().enabled
limb_0 = baxter_interface.Gripper(self.limb_name, CHECK_VERSION)
done = False
print("Enabling robot... ")
rs.enable()
print("Controlling grippers. Press ? for help, Esc to quit.")
while not done and not rospy.is_shutdown():
c = baxter_external_devices.getch()
if c:
if c in ['\x1b', '\x03']:
done = True
elif c == 'a':
rospy.loginfo("%s is pressed" %c)
self.move((0,-1,0),0.05)
elif c == 'd':
rospy.loginfo("%s is pressed" %c)
self.move((0,1,0),0.05)
elif c == 'w':
rospy.loginfo("%s is pressed" %c)
self.move((-1,0,0),0.05)
elif c == 's':
rospy.loginfo("%s is pressed" %c)
self.move((1,0,0),0.05)
elif c == 'z':
rospy.loginfo("%s is pressed" %c)
self.move((0,0,-1),0.05)
elif c == 'x':
rospy.loginfo("%s is pressed" %c)
self.move((0,0,1),0.05)
else:
print("Not implement it yet...")
rospy.signal_shutdown("Move.py finished.")
def ask(self):
done = False
printed_objects = None
while not done and not rospy.is_shutdown():
if printed_objects != self.objects:
printed_objects = self.objects
print "Which object?"
for i, o in enumerate(self.objects):
print "%d.)" % (i + 1), o
c = baxter_external_devices.getch()
if c:
if c in ['\x1b', '\x03']:
done = True
else:
try:
idx = int(c)
except ValueError:
idx = 0
if idx != None and idx > 0 and idx <= len(self.objects):
print "Sending", self.objects[idx - 1]
self.command_publisher.publish(self.objects[idx - 1])
def ask(self):
done = False
printed_objects = None
while not done and not rospy.is_shutdown():
if printed_objects != self.objects:
printed_objects = self.objects
print "Which object?"
for i, o in enumerate(self.objects):
print "%d.)" % (i + 1), o
c = baxter_external_devices.getch()
if c:
if c in ['\x1b', '\x03']:
done = True
else:
try:
idx = int(c)
except ValueError:
idx = 0
if idx != None and idx > 0 and idx <= len(self.objects):
print "Sending", self.objects[idx - 1]
self.command_publisher.publish(self.objects[idx - 1])
def input_control(speed, accuracy):
"""
Takes user input to move the limbs
"""
left = baxter_interface.Limb('left')
right = baxter_interface.Limb('right')
grip_left = baxter_interface.Gripper('left', CHECK_VERSION)
grip_right = baxter_interface.Gripper('right', CHECK_VERSION)
lj = left.joint_names()
rj = right.joint_names()
# Set the speeds for the limbs to move
left.set_joint_position_speed(speed)
right.set_joint_position_speed(speed)
done = False
print("Controlling joints. Press ? for help, Esc to quit.")
while not done and not rospy.is_shutdown():
c = baxter_external_devices.getch()
if c:
#catch Esc or ctrl-c
if c in ['\x1b', '\x03']:
done = True
rospy.signal_shutdown("Example finished.")
elif c == '?':
"""
TODO FILL THIS OUT!!!!
"""
print("Enter joints and their values in corresponding order: ")
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')
# left = BaxterArm('left')
right = BaxterArm('right')
kin = baxter_kinematics('left')
grip_left = baxter_interface.Gripper('left', CHECK_VERSION)
grip_right = baxter_interface.Gripper('right', CHECK_VERSION)
lj = left.joint_names()
rj = right.joint_names()
leftdes = []
leftacc = []
lefterr = []
delta = 0.075 #was 0.05
gain = 1.
def IK(limb, movement):
# current_limb= limb.angles()
# joint_command = {joint_name: current_position + delta}
# current_pos, curr_ori = limb.ee_pose()
# leftacc = np.append(current_pos,quaternion.as_euler_angles(curr_ori))
# print leftacc.shape
joint_names = limb.joint_names()
# curr_q = limb.joint_angles()
def to_list(ls):
return [ls[n] for n in joint_names]
curr_q = np.asarray(to_list(limb.joint_angles())).reshape([7, 1])
# print curr_q.shape, 'shape'
# print 'ee pose', limb.endpoint_pose()
pos = np.array(limb.endpoint_pose()['position'])
#ori = limb.endpoint_pose()['orientation']
#pose = np.append(pos, quaternion.as_euler_angles(quaternion.quaternion(ori.w,ori.x,ori.y,ori.z))).reshape([6,1])
#print pose
# print pos, ori
# sldfjsl
# raw_input()
JT = kin.jacobian_transpose()
lefterr = np.append(movement, [0, 0, 0]).reshape([6, 1])
#lefterr = np.array(movement).reshape([3,1])
# leftdes = leftacc+lefterr
# print leftacc, lefterr, leftdes
#JT = np.linalg.pinv(limb.jacobian())
# JT = limb.jacobian().T
des_q = curr_q + gain * JT.dot(lefterr)
#des_q = JT.dot(lefterr + pose)
print des_q.shape, 'shape2'
q_dict = {}
for i in range(len(joint_names)):
q_dict[joint_names[i]] = des_q[i, :][0]
# print joint_names[i], q_dict[joint_names[i]]
# dsflasjkds
# joint_command = current_limb + JT.dot(lefterr)
# joint_command = JT.dot(leftdes)
# print joint_command
# print JT.dot(lefterr)
# limb.exec_position_cmd(joint_command)
# joint_command = {joint_name: curr_q + JT.dot(lefterr)}
# print q_dict
limb.move_to_joint_positions(q_dict)
#print (pos[1]-np.array(limb.endpoint_pose()['position'])[1])/delta 0.91 for x, 0.82 for z
print(pos[2] - np.array(limb.endpoint_pose()['position'])[2]) / delta
bindings = {
# key: (function, args, description)
# 's': (IK, [left, [delta,delta/100,delta/100] ], "xinc"),
#'d': (IK, [left, [-delta,-delta/100,-delta/100] ], "xdec"),
's': (IK, [left, [delta * 0.8, 0, 0]], "xinc"),
'd': (IK, [left, [-delta * 0.8, 0, 0]], "xdec"),
'w': (IK, [left, [0, delta * 0.5, 0]], "yinc"),
'e': (IK, [left, [0, -delta * 0.5, 0]], "ydec"),
#'w': (IK, [left, [-delta/20,delta,-delta/20] ], "yinc"),
# 'e': (IK, [left, [delta/20,-delta,delta/20] ], "ydec"),
'x': (IK, [left, [0, 0, delta]], "zinc"),
'c': (IK, [left, [0, 0, -delta]], "zdec"),
'k': (IK, [left, [0, 0, 0]], "nothing"),
'l': (IK, [left, [delta / 100, delta, delta / 100]], "allincy"),
';': (IK, [left, [-delta, -delta, -delta]], "alldec"),
#'n': (set_j, [left, lj[6], -0.1], "left_w2 decrease"),
'b': (grip_left.close, [], "left: gripper close"),
'n': (grip_left.open, [], "left: gripper open"),
'm': (grip_left.calibrate, [], "left: gripper calibrate") #comma here?
}
done = False
print("Controlling joints. Press ? for help, Esc to quit.")
while not done and not rospy.is_shutdown():
c = baxter_external_devices.getch()
if c:
#catch Esc or ctrl-c
if c in ['\x1b', '\x03']:
done = True
rospy.signal_shutdown("Example finished.")
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 chose_configuration():
# initialize interfaces
print("Getting robot state... ")
rs = baxter_interface.RobotEnable(CHECK_VERSION)
init_state = rs.state().enabled
left = baxter_interface.Gripper('left', CHECK_VERSION)
right = baxter_interface.Gripper('right', CHECK_VERSION)
def clean_shutdown():
global out_file
if not init_state:
print("Disabling robot...")
rs.disable()
out_file.close()
print("Exiting manipulations script.")
rospy.on_shutdown(clean_shutdown)
def move_left_flat_up():
global out_file
out_file.write("Time: "+ str(rospy.Time.now()) + ": starting right_reach_center\n")
os.system("rosrun baxter_examples joint_trajectory_file_playback_modified.py -f /home/berto/ros_ws/src/articulated_object_manipulation/actions/right_reach_center.rec")
right.close([])
out_file.write("Time: "+ str(rospy.Time.now()) + ": starting left_reach_left_flat\n")
os.system("rosrun baxter_examples joint_trajectory_file_playback_modified.py -f /home/berto/ros_ws/src/articulated_object_manipulation/actions/left_reach_left_flat.rec")
out_file.write("Time: "+ str(rospy.Time.now()) + ": starting left_pushrotate_left_flat_up\n")
os.system("rosrun baxter_examples joint_trajectory_file_playback_modified.py -f /home/berto/ros_ws/src/articulated_object_manipulation/actions/left_pushrotate_left_flat_up.rec")
out_file.write("Time: "+ str(rospy.Time.now()) + ": starting left_leave_left_up\n")
os.system("rosrun baxter_examples joint_trajectory_file_playback_modified.py -f /home/berto/ros_ws/src/articulated_object_manipulation/actions/left_leave_left_up.rec")
right.open([])
out_file.write("Time: "+ str(rospy.Time.now()) + ": starting right_leave_center\n")
os.system("rosrun baxter_examples joint_trajectory_file_playback_modified.py -f /home/berto/ros_ws/src/articulated_object_manipulation/actions/right_leave_center.rec")
newConfig( a,b,d,f,e,h )
def move_left_up_flat():
global out_file
out_file.write("Time: "+ str(rospy.Time.now()) + ": starting right_reach_center\n")
os.system("rosrun baxter_examples joint_trajectory_file_playback_modified.py -f /home/berto/ros_ws/src/articulated_object_manipulation/actions/right_reach_center.rec")
right.close([])
out_file.write("Time: "+ str(rospy.Time.now()) + ": starting left_reach_left_up\n")
os.system("rosrun baxter_examples joint_trajectory_file_playback_modified.py -f /home/berto/ros_ws/src/articulated_object_manipulation/actions/left_reach_left_up.rec")
out_file.write("Time: "+ str(rospy.Time.now()) + ": starting left_pullrotate_left_up_flat\n")
os.system("rosrun baxter_examples joint_trajectory_file_playback_modified.py -f /home/berto/ros_ws/src/articulated_object_manipulation/actions/left_pullrotate_left_up_flat.rec")
out_file.write("Time: "+ str(rospy.Time.now()) + ": starting left_leave_left_flat\n")
os.system("rosrun baxter_examples joint_trajectory_file_playback_modified.py -f /home/berto/ros_ws/src/articulated_object_manipulation/actions/left_leave_left_flat.rec")
right.open([])
out_file.write("Time: "+ str(rospy.Time.now()) + ": starting right_leave_center\n")
os.system("rosrun baxter_examples joint_trajectory_file_playback_modified.py -f /home/berto/ros_ws/src/articulated_object_manipulation/actions/right_leave_center.rec")
newConfig( b,f,h,a,d,e )
def move_left_flat_down():
global out_file
out_file.write("Time: "+ str(rospy.Time.now()) + ": starting right_reach_center\n")
os.system("rosrun baxter_examples joint_trajectory_file_playback_modified.py -f /home/berto/ros_ws/src/articulated_object_manipulation/actions/right_reach_center.rec")
right.close([])
out_file.write("Time: "+ str(rospy.Time.now()) + ": starting left_reach_left_flat\n")
os.system("rosrun baxter_examples joint_trajectory_file_playback_modified.py -f /home/berto/ros_ws/src/articulated_object_manipulation/actions/left_reach_left_flat.rec")
left.close([])
out_file.write("Time: "+ str(rospy.Time.now()) + ": starting left_rotate_left_flat_down\n")
os.system("rosrun baxter_examples joint_trajectory_file_playback_modified.py -f /home/berto/ros_ws/src/articulated_object_manipulation/actions/left_rotate_left_flat_down.rec")
left.open([])
out_file.write("Time: "+ str(rospy.Time.now()) + ": starting left_leave_left_down\n")
os.system("rosrun baxter_examples joint_trajectory_file_playback_modified.py -f /home/berto/ros_ws/src/articulated_object_manipulation/actions/left_leave_left_down.rec")
right.open([])
out_file.write("Time: "+ str(rospy.Time.now()) + ": starting right_leave_center\n")
os.system("rosrun baxter_examples joint_trajectory_file_playback_modified.py -f /home/berto/ros_ws/src/articulated_object_manipulation/actions/right_leave_center.rec")
newConfig( a,d,e,c,i,g )
def move_left_down_flat():
global out_file
out_file.write("Time: "+ str(rospy.Time.now()) + ": starting right_reach_center\n")
os.system("rosrun baxter_examples joint_trajectory_file_playback_modified.py -f /home/berto/ros_ws/src/articulated_object_manipulation/actions/right_reach_center.rec")
right.close([])
out_file.write("Time: "+ str(rospy.Time.now()) + ": starting left_reach_left_down\n")
os.system("rosrun baxter_examples joint_trajectory_file_playback_modified.py -f /home/berto/ros_ws/src/articulated_object_manipulation/actions/left_reach_left_down.rec")
left.close([])
out_file.write("Time: "+ str(rospy.Time.now()) + ": starting left_rotate_left_down_flat\n")
os.system("rosrun baxter_examples joint_trajectory_file_playback_modified.py -f /home/berto/ros_ws/src/articulated_object_manipulation/actions/left_rotate_left_down_flat.rec")
left.open([])
out_file.write("Time: "+ str(rospy.Time.now()) + ": starting left_leave_left_flat\n")
os.system("rosrun baxter_examples joint_trajectory_file_playback_modified.py -f /home/berto/ros_ws/src/articulated_object_manipulation/actions/left_leave_left_flat.rec")
right.open([])
out_file.write("Time: "+ str(rospy.Time.now()) + ": starting right_leave_center\n")
os.system("rosrun baxter_examples joint_trajectory_file_playback_modified.py -f /home/berto/ros_ws/src/articulated_object_manipulation/actions/right_leave_center.rec")
newConfig( c,i,g,a,d,e )
def move_right_flat_up():
global out_file
out_file.write("Time: "+ str(rospy.Time.now()) + ": starting left_reach_center\n")
os.system("rosrun baxter_examples joint_trajectory_file_playback_modified.py -f /home/berto/ros_ws/src/articulated_object_manipulation/actions/left_reach_center.rec")
left.close([])
out_file.write("Time: "+ str(rospy.Time.now()) + ": starting right_reach_right_flat\n")
os.system("rosrun baxter_examples joint_trajectory_file_playback_modified.py -f /home/berto/ros_ws/src/articulated_object_manipulation/actions/right_reach_right_flat.rec")
out_file.write("Time: "+ str(rospy.Time.now()) + ": starting right_pushrotate_right_flat_up\n")
os.system("rosrun baxter_examples joint_trajectory_file_playback_modified.py -f /home/berto/ros_ws/src/articulated_object_manipulation/actions/right_pushrotate_right_flat_up.rec")
out_file.write("Time: "+ str(rospy.Time.now()) + ": starting right_leave_right_up\n")
os.system("rosrun baxter_examples joint_trajectory_file_playback_modified.py -f /home/berto/ros_ws/src/articulated_object_manipulation/actions/right_leave_right_up.rec")
left.open([])
out_file.write("Time: "+ str(rospy.Time.now()) + ": starting left_leave_center\n")
os.system("rosrun baxter_examples joint_trajectory_file_playback_modified.py -f /home/berto/ros_ws/src/articulated_object_manipulation/actions/left_leave_center.rec")
newConfig( a,b,c,d,f,i )
def move_right_up_flat():
global out_file
out_file.write("Time: "+ str(rospy.Time.now()) + ": starting left_reach_center\n")
os.system("rosrun baxter_examples joint_trajectory_file_playback_modified.py -f /home/berto/ros_ws/src/articulated_object_manipulation/actions/left_reach_center.rec")
left.close([])
out_file.write("Time: "+ str(rospy.Time.now()) + ": starting right_reach_right_up\n")
os.system("rosrun baxter_examples joint_trajectory_file_playback_modified.py -f /home/berto/ros_ws/src/articulated_object_manipulation/actions/right_reach_right_up.rec")
out_file.write("Time: "+ str(rospy.Time.now()) + ": starting right_pullrotate_right_up_flat\n")
os.system("rosrun baxter_examples joint_trajectory_file_playback_modified.py -f /home/berto/ros_ws/src/articulated_object_manipulation/actions/right_pullrotate_right_up_flat.rec")
out_file.write("Time: "+ str(rospy.Time.now()) + ": starting right_leave_right_flat\n")
os.system("rosrun baxter_examples joint_trajectory_file_playback_modified.py -f /home/berto/ros_ws/src/articulated_object_manipulation/actions/right_leave_right_flat.rec")
left.open([])
out_file.write("Time: "+ str(rospy.Time.now()) + ": starting left_leave_center\n")
os.system("rosrun baxter_examples joint_trajectory_file_playback_modified.py -f /home/berto/ros_ws/src/articulated_object_manipulation/actions/left_leave_center.rec")
newConfig( d,f,i,a,b,c )
def move_right_flat_down():
global out_file
out_file.write("Time: "+ str(rospy.Time.now()) + ": starting left_reach_center\n")
os.system("rosrun baxter_examples joint_trajectory_file_playback_modified.py -f /home/berto/ros_ws/src/articulated_object_manipulation/actions/left_reach_center.rec")
left.close([])
out_file.write("Time: "+ str(rospy.Time.now()) + ": starting right_reach_right_flat\n")
os.system("rosrun baxter_examples joint_trajectory_file_playback_modified.py -f /home/berto/ros_ws/src/articulated_object_manipulation/actions/right_reach_right_flat.rec")
right.close([])
out_file.write("Time: "+ str(rospy.Time.now()) + ": starting right_rotate_right_flat_down\n")
os.system("rosrun baxter_examples joint_trajectory_file_playback_modified.py -f /home/berto/ros_ws/src/articulated_object_manipulation/actions/right_rotate_right_flat_down.rec")
right.open([])
out_file.write("Time: "+ str(rospy.Time.now()) + ": starting right_leave_right_down\n")
os.system("rosrun baxter_examples joint_trajectory_file_playback_modified.py -f /home/berto/ros_ws/src/articulated_object_manipulation/actions/right_leave_right_down.rec")
left.open([])
out_file.write("Time: "+ str(rospy.Time.now()) + ": starting left_leave_center\n")
os.system("rosrun baxter_examples joint_trajectory_file_playback_modified.py -f /home/berto/ros_ws/src/articulated_object_manipulation/actions/left_leave_center.rec")
newConfig( a,b,c,e,h,g )
def move_right_down_flat():
global out_file
out_file.write("Time: "+ str(rospy.Time.now()) + ": starting left_reach_center\n")
os.system("rosrun baxter_examples joint_trajectory_file_playback_modified.py -f /home/berto/ros_ws/src/articulated_object_manipulation/actions/left_reach_center.rec")
left.close([])
out_file.write("Time: "+ str(rospy.Time.now()) + ": starting right_reach_right_down\n")
os.system("rosrun baxter_examples joint_trajectory_file_playback_modified.py -f /home/berto/ros_ws/src/articulated_object_manipulation/actions/right_reach_right_down.rec")
right.close([])
out_file.write("Time: "+ str(rospy.Time.now()) + ": starting right_rotate_right_down_flat\n")
os.system("rosrun baxter_examples joint_trajectory_file_playback_modified.py -f /home/berto/ros_ws/src/articulated_object_manipulation/actions/right_rotate_right_down_flat.rec")
right.open([])
out_file.write("Time: "+ str(rospy.Time.now()) + ": starting right_leave_right_flat\n")
os.system("rosrun baxter_examples joint_trajectory_file_playback_modified.py -f /home/berto/ros_ws/src/articulated_object_manipulation/actions/right_leave_right_flat.rec")
left.open([])
out_file.write("Time: "+ str(rospy.Time.now()) + ": starting left_leave_cente\n")
os.system("rosrun baxter_examples joint_trajectory_file_playback_modified.py -f /home/berto/ros_ws/src/articulated_object_manipulation/actions/left_leave_center.rec")
newConfig( e,h,g,a,b,c )
configurations_from_a = {
# key: (function, args, description)
'b': (move_left_flat_up, [], "b"),
'c': (move_left_flat_down, [], "c"),
'd': (move_right_flat_up, [], "d"),
'e': (move_right_flat_down, [], "e"),
}
configurations_from_b = {
# key: (function, args, description)
'a': (move_left_up_flat, [], "a"),
'f': (move_right_flat_up, [], "f"),
'h': (move_right_flat_down, [], "h"),
}
configurations_from_c = {
# key: (function, args, description)
'a': (move_left_down_flat, [], "a"),
'g': (move_right_flat_down, [], "g"),
'i': (move_right_flat_up, [], "i"),
}
configurations_from_d = {
# key: (function, args, description)
'a': (move_right_up_flat, [], "a"),
'f': (move_left_flat_up, [], "f"),
'i': (move_left_flat_down, [], "i"),
}
configurations_from_e = {
# key: (function, args, description)
'a': (move_right_down_flat, [], "a"),
'g': (move_left_flat_down, [], "g"),
'h': (move_left_flat_up, [], "h"),
}
configurations_from_f = {
# key: (function, args, description)
'b': (move_right_up_flat, [], "b"),
'd': (move_left_up_flat, [], "d"),
}
configurations_from_g = {
# key: (function, args, description)
'c': (move_right_down_flat, [], "c"),
'e': (move_left_down_flat, [], "e"),
}
configurations_from_h = {
# key: (function, args, description)
'b': (move_right_down_flat, [], "b"),
'e': (move_left_up_flat, [], "e"),
}
configurations_from_i = {
# key: (function, args, description)
'c': (move_right_up_flat, [], "c"),
'd': (move_left_down_flat, [], "d"),
}
done = False
print("Enabling robot... ")
rs.enable()
print("=================================================================================")
print("Current configuration is 'a', insert the goal configuration or")
print("press ? to see the reachable configurations, Esc to quit.")
print("=================================================================================")
while not done and not rospy.is_shutdown():
first_input = baxter_external_devices.getch()
if first_input:
if current_configuration == a:
if first_input in ['\x1b', '\x03']:
done = True
elif first_input in configurations_from_a:
cmd = configurations_from_a[first_input]
print("Move to configuration: %s" % (cmd[2],))
cmd[0](*cmd[1])
else:
print("key bindings: ")
print(" Esc: Quit")
print(" ?: Help")
for key, val in sorted(configurations_from_a.items(), key=lambda x: x[1][2]):
print(" %s: %s" % (key, val[2]))
elif current_configuration == b:
if first_input in ['\x1b', '\x03']:
done = True
elif first_input in configurations_from_b:
cmd = configurations_from_b[first_input]
print("Move to configuration: %s" % (cmd[2],))
cmd[0](*cmd[1])
else:
print("key bindings: ")
print(" Esc: Quit")
print(" ?: Help")
for key, val in sorted(configurations_from_b.items(), key=lambda x: x[1][2]):
print(" %s: %s" % (key, val[2]))
elif current_configuration == c:
if first_input in ['\x1b', '\x03']:
done = True
elif first_input in configurations_from_c:
cmd = configurations_from_c[first_input]
print("Move to configuration: %s" % (cmd[2],))
cmd[0](*cmd[1])
else:
print("key bindings: ")
print(" Esc: Quit")
print(" ?: Help")
for key, val in sorted(configurations_from_c.items(), key=lambda x: x[1][2]):
print(" %s: %s" % (key, val[2]))
elif current_configuration == d:
if first_input in ['\x1b', '\x03']:
done = True
elif first_input in configurations_from_d:
cmd = configurations_from_d[first_input]
print("Move to configuration: %s" % (cmd[2],))
cmd[0](*cmd[1])
else:
print("key bindings: ")
print(" Esc: Quit")
print(" ?: Help")
for key, val in sorted(configurations_from_d.items(), key=lambda x: x[1][2]):
print(" %s: %s" % (key, val[2]))
elif current_configuration == e:
if first_input in ['\x1b', '\x03']:
done = True
elif first_input in configurations_from_e:
cmd = configurations_from_e[first_input]
print("Move to configuration: %s" % (cmd[2],))
cmd[0](*cmd[1])
else:
print("key bindings: ")
print(" Esc: Quit")
print(" ?: Help")
for key, val in sorted(configurations_from_e.items(), key=lambda x: x[1][2]):
print(" %s: %s" % (key, val[2]))
elif current_configuration == f:
if first_input in ['\x1b', '\x03']:
done = True
elif first_input in configurations_from_f:
cmd = configurations_from_f[first_input]
print("Move to configuration: %s" % (cmd[2],))
cmd[0](*cmd[1])
else:
print("key bindings: ")
print(" Esc: Quit")
print(" ?: Help")
for key, val in sorted(configurations_from_f.items(), key=lambda x: x[1][2]):
print(" %s: %s" % (key, val[2]))
elif current_configuration == g:
if first_input in ['\x1b', '\x03']:
done = True
elif first_input in configurations_from_g:
cmd = configurations_from_g[first_input]
print("Move to configuration: %s" % (cmd[2],))
cmd[0](*cmd[1])
else:
print("key bindings: ")
print(" Esc: Quit")
print(" ?: Help")
for key, val in sorted(configurations_from_g.items(), key=lambda x: x[1][2]):
print(" %s: %s" % (key, val[2]))
elif current_configuration == h:
if first_input in ['\x1b', '\x03']:
done = True
elif first_input in configurations_from_h:
cmd = configurations_from_h[first_input]
print("Move to configuration: %s" % (cmd[2],))
cmd[0](*cmd[1])
else:
print("key bindings: ")
print(" Esc: Quit")
print(" ?: Help")
for key, val in sorted(configurations_from_h.items(), key=lambda x: x[1][2]):
print(" %s: %s" % (key, val[2]))
elif current_configuration == i:
if first_input in ['\x1b', '\x03']:
done = True
elif first_input in configurations_from_i:
cmd = configurations_from_i[first_input]
print("Move to configuration: %s" % (cmd[2],))
cmd[0](*cmd[1])
else:
print("key bindings: ")
print(" Esc: Quit")
print(" ?: Help")
for key, val in sorted(configurations_from_i.items(), key=lambda x: x[1][2]):
print(" %s: %s" % (key, val[2]))
# force shutdown call if caught by key handler
rospy.signal_shutdown("Manipulations finished.")
def map_keyboard():
left = baxter_interface.Limb('left')
right = baxter_interface.Limb('right')
grip_left = baxter_interface.Gripper('left', CHECK_VERSION)
grip_right = baxter_interface.Gripper('right', CHECK_VERSION)
#These are from PyKDL and are needed for the Jacobian
left_kin = baxter_kinematics('left')
right_kin = baxter_kinematics('right')
#Connect with IK service
right_iksvc, right_ns = ik_command.connect_service('right')
left_iksvc, left_ns = ik_command.connect_service('left')
def command_jacobian(side, direction):
#UNUSED
if side == 'left':
limb = left
kin = left_kin
elif side == 'right':
limb = right
kin = right_kin
else:
raise Exception("Got wrong side in command_jacobian")
# current is the current position of end effector
current_p = np.array(limb.endpoint_pose()['position']+limb.endpoint_pose()['orientation'])
current_p = current_q.reshape((7, 1))
current = np.zeros((6, 1))
quaternion_to_euler(current_p, current)
# We need to convert the direction from the world frame to the hand frame
# Rotate direction by the inverse of the rotation matrix from the angular pose in current
R = tf.transformations.quaternion_matrix(current_p[3:7].flatten().tolist())
R_inv = tf.transformations.inverse_matrix(R)
print R_inv[0:3, 0:3]
direction = np.array(direction).reshape((6, 1))
print direction[0:3]
dir_rot = np.dot(R_inv[0:3, 0:3], direction[0:3])
print dir_rot
#direction[0:3] = dir_rot
# Goal is the goal position, found by adding the requested direction from the user
goal = current + direction
current_angles = np.array([limb.joint_angles()[name] for name in limb.joint_names()])
# Get the Jacobian inverse and solve for the necessary change in joint angles
jacobian_inv = kin.jacobian_transpose()
delta = jacobian_inv*(goal-current)
commands = current_angles.flatten() + delta.flatten()
# Command the joints
command_list = commands.tolist()
joint_command = dict(zip(limb.joint_names(), command_list[0]))
limb.set_joint_positions(joint_command)
def command_ik(side, direction):
"""Use the Rethink IK service to figure out a desired joint position"""
if side == 'left':
limb = left
iksvc = left_iksvc
ns = left_ns
else:
limb = right
iksvc = right_iksvc
ns = right_ns
current_p = np.array(limb.endpoint_pose()['position']+limb.endpoint_pose()['orientation'])
direction = np.array(direction)
desired_p = current_p + direction
ik_command.service_request(iksvc, desired_p, side)
zeros = [0]*4
inc = 0.1
bindings = {
'q': (command_ik, ['right', [inc, 0, 0]+zeros], "increase right x"),
'a': (command_ik, ['right', [-inc, 0, 0]+zeros], "decrease right x"),
'w': (command_ik, ['right', [0, inc, 0]+zeros], "increase right y"),
's': (command_ik, ['right', [0, -inc, 0]+zeros], "decrease right y"),
'e': (command_ik, ['right', [0, 0, inc]+zeros], "increase right z"),
'd': (command_ik, ['right', [0, 0, -inc]+zeros], "decrease right z"),
'u': (command_ik, ['left', [inc, 0, 0]+zeros], "increase left x"),
'j': (command_ik, ['left', [-inc, 0, 0]+zeros], "decrease left x"),
'i': (command_ik, ['left', [0, inc, 0]+zeros], "increase left y"),
'k': (command_ik, ['left', [0, -inc, 0]+zeros], "decrease left y"),
'o': (command_ik, ['left', [0, 0, inc]+zeros], "increase left z"),
'l': (command_ik, ['left', [0, 0, -inc]+zeros], "decrease left z"),
'z': (grip_right.close, [], "right: gripper close"),
'x': (grip_right.open, [], "right: gripper open"),
'c': (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 = baxter_external_devices.getch()
if c:
#catch Esc or ctrl-c
if c in ['\x1b', '\x03']:
done = True
rospy.signal_shutdown("Example finished.")
elif c in bindings:
cmd = bindings[c]
#expand binding to something like "command_jacobian(left, [0.1, 0, 0])"
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():
global desired_angles, inc, angle_inc
left = baxter_interface.Limb('left')
grip_left = baxter_interface.Gripper('left', CHECK_VERSION)qqqqqqqqqqqa
#These are from PyKDL and are needed for the Jacobian
left_kin = baxter_kinematics('left')
#Connect with IK service
left_iksvc, left_ns = ik_command.connect_service('left')
#Initialize desired angles from current angles
init_orient = left.endpoint_pose()['orientation']
quat = [0,0,0,0]
quat[3] = init_orient.w
quat[0] = init_orient.x
quat[1] = init_orient.y
quat[2] = init_orient.z
desired_angles = tf.transformations.euler_from_quaternion(quat)
#print init_orient
def command_jacobian(side, direction):
#UNUSED
if side == 'left':
limb = left
kin = left_kin
else:
raise Exception("Got wrong side in command_jacobian")
# current is the current position of end effector
current_p = np.array(limb.endpoint_pose()['position']+limb.endpoint_pose()['orientation'])
current_p = current_q.reshape((7, 1))
current = np.zeros((6, 1))
quaternion_to_euler(current_p, current)
# We need to convert the direction from the world frame to the hand frame
# Rotate direction by the inverse of the rotation matrix from the angular pose in current
R = tf.transformations.quaternion_matrix(current_p[3:7].flatten().tolist())
R_inv = tf.transformations.inverse_matrix(R)
print R_inv[0:3, 0:3]
direction = np.array(direction).reshape((6, 1))
print direction[0:3]
dir_rot = np.dot(R_inv[0:3, 0:3], direction[0:3])
print dir_rot
#direction[0:3] = dir_rot
# Goal is the goal position, found by adding the requested direction from the user
goal = current + direction
current_angles = np.array([limb.joint_angles()[name] for name in limb.joint_names()])
# Get the Jacobian inverse and solve for the necessary change in joint angles
jacobian_inv = kin.jacobian_transpose()
delta = jacobian_inv*(goal-current)
commands = current_angles.flatten() + delta.flatten()
# Command the joints
command_list = commands.tolist()
joint_command = dict(zip(limb.joint_names(), command_list[0]))
limb.set_joint_positions(joint_command)
def command_ik(side, direction):
"""Use the Rethink IK service to figure out a desired joint position"""
if side == 'left':
limb = left
iksvc = left_iksvc
ns = left_ns
current_p = np.array(limb.endpoint_pose()['position']+limb.endpoint_pose()['orientation'])
direction = np.array(direction)
print direction
desired_p = current_p + direction
ik_command.service_request(iksvc, desired_p, side)
zeros = [0]*4
def update_angles(change_angles):
global desired_angles
current_angles = np.array(desired_angles)
change_angles = np.array(change_angles)
#Note that adding np arrays does element-wise summation, but adding python lists concatenates them
new_angles = current_angles + change_angles
#Check that the angles are in bounds, angles are in radians
#for value in new_angles:
#Wrap crossing zero towards 2*pi
# if value < 0:
# value = (2* np.pi) + value
#Wrap crossing 2*pi towards zero
# if value > (2 * np.pi):
# value = value - (2*np.pi)
#Convert to a python array and store in the desired angles
desired_angles = new_angles.tolist()
#Convert the desired angles to a quaternion
quat = tf.transformations.quaternion_from_euler(desired_angles[0], desired_angles[1], desired_angles[2])
#This only does the left side, because the scooter only has a left arm
limb = left
current_p = np.array(limb.endpoint_pose()['position']+limb.endpoint_pose()['orientation'])
current_p[3:7] = quat
ik_command.service_request(left_iksvc, current_p, "left")
def update_inc(isSmall = True):
global inc, angle_inc
if isSmall:
inc = 0.1
angle_inc = 0.1
else:
inc = 0.5
angle_inc = 0.2
def toggle_gripper():
if grip_left.position() > 95:
#gripper is open, so close it. Can be confused by a big object, though.
print "Closing gripper"
grip_left.close()
elif grip_left.gripping() or grip_left.position() < 5:
print "Opening gripper"
#gripper is closed on something, or on nothing
grip_left.open()
else:
print "Unknown state, opening gripper"
#Unknown state, so open the gripper
grip_left.open()
done = False
print("Controlling joints. Press ? for help, Esc to quit.")
while not done and not rospy.is_shutdown():
#This had to be moved into the loop so the increment could be rebound when
#the -/= keys are used to change the increment between small and large
bindings = {
'u': (command_ik, ['left', [inc, 0, 0]+zeros], "increase left x"),
'j': (command_ik, ['left', [-inc, 0, 0]+zeros], "decrease left x"),
'i': (command_ik, ['left', [0, inc, 0]+zeros], "increase left y"),
'k': (command_ik, ['left', [0, -inc, 0]+zeros], "decrease left y"),
'o': (command_ik, ['left', [0, 0, inc]+zeros], "increase left z"),
'l': (command_ik, ['left', [0, 0, -inc]+zeros], "decrease left z"),
'q': (update_angles, [[angle_inc, 0, 0]], "increase left roll"),
'a': (update_angles, [[-angle_inc, 0, 0]], "decrease left roll"),
'w': (update_angles, [[0,angle_inc,0]], "increase left pitch"),
's': (update_angles, [[0,-angle_inc,0]], "decrease left pitch"),
'e': (update_angles, [[0,0,angle_inc]], "increase left yaw"),
'd': (update_angles, [[0,0,-angle_inc]], "decrease left yaw"),
'-': (update_inc, [True], "small motion increments"),
'=': (update_inc, [False], "large motion increments"),
' ': (toggle_gripper, [], "toggle gripper state")
}
c = baxter_external_devices.getch()
if c:
#catch Esc or ctrl-c
if c in ['\x1b', '\x03']:
done = True
rospy.signal_shutdown("Example finished.")
elif c in bindings:
cmd = bindings[c]
#expand binding to something like "command_jacobian(left, [0.1, 0, 0])"
cmd[0](*cmd[1])
print("command: %s with params %s" % (cmd[2], cmd[1]))
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 run_control(joystick):
# Initialise parameters
kbOnly = True if joystick == None else False
print('kbOnly: ', kbOnly)
#abbreviations
if kbOnly == False:
jhi = lambda s: joystick.stick_value(s) > 0
jlo = lambda s: joystick.stick_value(s) < 0
bdn = joystick.button_down
bup = joystick.button_up
zeros = [0] * 4
initial_inc = 0.03
inc = initial_inc
delta = 0.01
incU = 0.05
incL = 0.01
syncErrThresh = 0.01
vrepZdelta = 1.08220972
# Service to call functions in V-rep (e.g. set/get joint positions)
vrepScriptFunc = rospy.ServiceProxy('vrep/simRosCallScriptFunction',
simRosCallScriptFunction)
# Get robot interface
robot = RobotArm(vrepScriptFunc)
robot.updPos('left')
# Get appropriate bindings
kb_b = js_b = False
def gen_js_bindings(inc):
bindings = (
((bdn, ['btnUp']), (robot.grip_l_close, []), "left gripper"),
((jhi, ['rightTrigger']), (inc_delta, [inc, delta, incU,
incL]), "Speed increased"),
((jhi, ['leftTrigger']), (inc_delta, [inc, -delta, incU,
incL]), "Speed decreased"),
((bdn, ['leftBumper']), (robot.pos_to_ori, ['down']),
"Switched to orientation"),
((bup, ['leftBumper']), (robot.pos_to_ori, ['up']),
"Switched to position"),
((bdn, ['rightBumper']), (robot.world_to_ee, ['ee']),
"Switched to tool frame"),
((bup, ['rightBumper']), (robot.world_to_ee, ['world']),
"Switched to world frame"),
((jlo, ['leftStickHorz']), (robot.command_ik,
['left', [0, -inc,
0]]), lambda: "left y dec "),
((jhi, ['leftStickHorz']),
(robot.command_ik, ['left', [0, inc, 0]]), lambda: "left y inc "),
((jlo, ['leftStickVert']), (robot.command_ik,
['left', [-inc, 0,
0]]), lambda: "left x dec "),
((jhi, ['leftStickVert']),
(robot.command_ik, ['left', [inc, 0, 0]]), lambda: "left x inc "),
((jlo, ['rightStickVert']),
(robot.command_ik, ['left', [0, 0,
-inc]]), lambda: "left z dec "),
((jhi, ['rightStickVert']),
(robot.command_ik, ['left', [0, 0, inc]]), lambda: "left z inc "),
((bdn, ['function1']), (print_js_bindings, [js_b]), "help"),
((bdn, ['function2']), (print_js_bindings, [js_b]), "help"),
)
return bindings
def gen_kb_bindings(inc):
bindings = {
'9': (inc_delta, [inc, delta, incU, incL], "Speed increased"),
'0': (inc_delta, [inc, -delta, incU, incL], "Speed decreased"),
'i': (robot.pos_to_ori, [], "Switch position/orientation"),
# 'p': (robot.pos_to_ori, ['up'], "Switched to position"),
'o': (robot.world_to_ee, [], "Switch world/tool frame"),
# 'i': (robot.world_to_ee, ['world'], "Switched to world frame"),
'w': (robot.command_ik, ['left', [inc, 0, 0]], "increase left x"),
's': (robot.command_ik, ['left', [-inc, 0, 0]], "decrease left x"),
'a': (robot.command_ik, ['left', [0, inc, 0]], "increase left y"),
'd': (robot.command_ik, ['left', [0, -inc, 0]], "decrease left y"),
'q': (robot.command_ik, ['left', [0, 0, inc]], "increase left z"),
'e': (robot.command_ik, ['left', [0, 0, -inc]], "decrease left z"),
'p': (robot.grip_l_close, [], "Gripper open/close"),
'r': (robot.recordPos, ['left'], "Recorded position"),
# 'l': (robot.grip_l_open, [], "left: gripper open"),
# 'c': (grip_left.calibrate, [], "left: gripper calibrate")
}
return bindings
if kbOnly == False:
js_b = []
bindings = gen_js_bindings(inc)
js_b.append(bindings)
kb_b = gen_kb_bindings(inc)
# Print bindings
if kbOnly == False:
print_js_bindings(js_b)
else:
print_kb_bindings(kb_b)
# Setup data recording
endpoint_state = EndpointState
def on_endpoint_states(msg, endpoint_state):
endpoint_state.header = msg.header
endpoint_state.pose = msg.pose
endpoint_state.twist = msg.twist
endpoint_state.wrench = msg.wrench
recording = False
done_recording = False
def record_data(filename):
# Subscribe to endpoint state
# _cartesian_state_sub = rospy.Subscriber(
# '/robot/limb/left/endpoint_state',
# EndpointState,
# on_endpoint_states,
# endpoint_state,
# queue_size=1,
# tcp_nodelay=True)
# bag = rosbag.Bag(filename, 'w')
def terminate_process_and_children(s):
list_cmd = subprocess.Popen("rosnode list",
shell=True,
stdout=subprocess.PIPE)
list_output = list_cmd.stdout.read()
retcode = list_cmd.wait()
assert retcode == 0, "List command returned %d" % retcode
for str in list_output.split("\n"):
if (str.startswith(s)):
os.system("rosnode kill " + str)
# while not done_recording:
# if recording==True:
# print('Endpoint actual: ',endpoint_state)
# bag.write('endpoint_state',endpoint_state)
rt = rospy.Rate(100)
while not recording:
rt.sleep()
p = subprocess.Popen(
'rosbag record -j -O test.bag /robot/limb/left/endpoint_state '
'/cameras/head_camera/image',
stdin=subprocess.PIPE,
shell=True,
cwd='/home/talha/test_ws/src/baxter_ik/scripts/')
# print 'Finishing recording'
# bag.close()
while not done_recording:
rt.sleep()
print 'Finishing recording'
terminate_process_and_children("/record")
# thread.start_new_thread(record_data, ('test.bag',))
# Move to start position
move_to_start_pos(robot, syncErrThresh)
vrepScriptFunc.call('set_ik_mode@Baxter_leftArm_target', 1, [], [], [], '')
rate = rospy.Rate(100)
# while loop
while not rospy.is_shutdown():
btnPressed = False
autoMove = False
# inputControl()
if kbOnly == True:
c = baxter_external_devices.getch(1)
if c:
# print('Pressed: ',c)
recording = True
#catch Esc or ctrl-c
if c in ['\x1b', '\x03']:
# terminate_process_and_children(p)
rospy.signal_shutdown("Example finished.")
if c == '\r':
done_recording = True
if c == 'f':
autoMove = True
# Keyboard control
if c in kb_b:
btnPressed = True
cmd = kb_b[c]
if cmd[0] == inc_delta:
inc = cmd[0](*cmd[1])
kb_b = gen_kb_bindings(inc)
elif cmd[0] == robot.pos_to_ori:
kb_b = gen_kb_bindings(initial_inc)
cmd[0](*cmd[1])
else:
cmd[0](*cmd[1])
# print("command: %s" % (cmd[2],))
else:
c = baxter_external_devices.getch()
if c == '\r':
done_recording = True
# Joystick control
for (test, cmd, doc) in bindings:
if test[0](*test[1]):
recording = True
if cmd[0] == inc_delta:
inc = cmd[0](*cmd[1])
bindings = gen_js_bindings(inc)
js_b[0] = bindings
elif cmd[0] == robot.pos_to_ori:
bindings = gen_js_bindings(initial_inc)
js_b[0] = bindings
cmd[0](*cmd[1])
else:
cmd[0](*cmd[1])
if callable(doc):
print(doc())
else:
print(doc)
if autoMove == True:
wp1, wp2, wp3 = auto_move_generate_wp(robot, robot.recordedPose)
print('Waypoint 1:', wp1)
robot.auto_ik('left', wp1, vrepZdelta)
resp = vrepScriptFunc.call('ikSuccess@Baxter_leftArm_target', 1,
[], [], [], '')
# Get resulting joint positions from V-rep
resp = vrepScriptFunc.call('get_jnt_pos@Baxter_leftArm_target', 1,
[], [], [], '')
# Wrap and send joint positions to Baxter
limb_joints = dict(
zip(robot.left._joint_names['left'], resp.outputFloats))
robot.left.move_to_joint_positions(limb_joints)
robot.auto_ik('left', wp2, vrepZdelta)
resp = vrepScriptFunc.call('ikSuccess@Baxter_leftArm_target', 1,
[], [], [], '')
# Get resulting joint positions from V-rep
resp = vrepScriptFunc.call('get_jnt_pos@Baxter_leftArm_target', 1,
[], [], [], '')
# Wrap and send joint positions to Baxter
limb_joints = dict(
zip(robot.left._joint_names['left'], resp.outputFloats))
robot.left.move_to_joint_positions(limb_joints)
robot.auto_ik('left', wp3, vrepZdelta)
resp = vrepScriptFunc.call('ikSuccess@Baxter_leftArm_target', 1,
[], [], [], '')
# Get resulting joint positions from V-rep
resp = vrepScriptFunc.call('get_jnt_pos@Baxter_leftArm_target', 1,
[], [], [], '')
# Wrap and send joint positions to Baxter
limb_joints = dict(
zip(robot.left._joint_names['left'], resp.outputFloats))
robot.left.move_to_joint_positions(limb_joints)
# Check if IK was a success in V-rep
resp = vrepScriptFunc.call('ikSuccess@Baxter_leftArm_target', 1, [],
[], [], '')
# print 'Ik Success: ', resp.outputInts
# If successful, receive & send joints to Baxter
if resp.outputInts[0] == -1 and btnPressed == True:
# Get resulting joint positions from V-rep
resp = vrepScriptFunc.call('get_jnt_pos@Baxter_leftArm_target', 1,
[], [], [], '')
# Get Baxter joints
curr_jnts = np.array([
robot.left.joint_angles()[name]
for name in robot.left.joint_names()
])
# Compare joints
norm = np.linalg.norm(curr_jnts)
err = math.fabs(
np.linalg.norm(np.array(resp.outputFloats) - curr_jnts))
err = err / norm
# print('Error: ',err)
# limb_joints = dict(zip(robot.left._joint_names['left'], resp.outputFloats))
# robot.left.set_joint_positions(limb_joints)
# syncPos(left, vrepScriptFunc)
if err > abs(syncErrThresh):
# Wrap and send joint positions to Baxter
limb_joints = dict(
zip(robot.left._joint_names['left'], resp.outputFloats))
robot.left.set_joint_positions(limb_joints)
# print('V-rep Joints: ',limb_joints)
# resp = vrepScriptFunc.call('get_pose@Baxter_leftArm_target',1,[],[],[],'')
# print 'V-rep pose: ',resp.outputFloats
# print('---------------------------------------------------------------------------------'/0)
# print 'Finish pose: ',current_p
# print('Actual joints: ', left.joint_angles())
# print('---------------------------------------------------------------------------------')
else:
print('Synching')
left_arm_sync(robot, syncErrThresh)
vrepScriptFunc.call('set_ik_mode@Baxter_leftArm_target', 1, [], [],
[], '')
rate.sleep()
def get_char():
"""Gets the character typed in the terminal. Useful for keyboard control."""
return baxter_external_devices.getch()
def gauge_obj_center(init_pos, vis_pub, obj_height, obj_radius_x,
obj_radius_y):
center = np.array(init_pos)
# 1 mm
DELTA = 0.001
X_NEG = np.array([-DELTA, 0, 0])
X_POS = np.array([DELTA, 0, 0])
Y_NEG = np.array([0, -DELTA, 0])
Y_POS = np.array([0, DELTA, 0])
Z_NEG = np.array([0, 0, -DELTA])
Z_POS = np.array([0, 0, DELTA])
done = False
# A dictionary, mapping from a character to a tuple.
# First element in tuple is a function, second is arguments to be
# unpacked and passed to the function, third is the string to display
# to user.
bindings = {
# key: (function, args, description)
'h': (X_NEG, 'Move X by -DELTA'),
'l': (X_POS, 'Move X by +DELTA'),
'j': (Y_NEG, 'Move Y by -DELTA'),
'k': (Y_POS, 'Move Y by +DELTA'),
'm': (Z_NEG, 'Move Z by -DELTA'),
'i': (Z_POS, 'Move Z by +DELTA'),
}
#####
# Main UI loop
#####
print('Welcome to interactive keyboard UI for gauging object center.')
print('Press ? for help, Esc to quit.')
print('')
while not done and not rospy.is_shutdown():
# From baxter_examples joint_position_keyboard.py
c = baxter_external_devices.getch()
if c:
# Catch Esc or ctrl-c
if c in ['\x1b', '\x03']:
done = True
print ('Done. Returning object center %f %f %f' % \
(center[0], center[1], center[2]))
break
elif c in bindings:
cmd = bindings[c]
print("command: %s" % (cmd[1], ))
# Expand binding to something like "set_j(right, 's0', 0.1)"
center = move_delta(center, vis_pub, cmd[0], obj_height,
obj_radius_x, obj_radius_y)
print('New center: %f %f %f' %
(center[0], center[1], center[2]))
else:
print("key bindings: ")
print(" Esc: Quit")
print(" ?: Help")
for key, val in sorted(bindings.items(), key=lambda x: x[0]):
print(" %s: %s" % (key, val[1]))
return center
right_pose = PoseStamped()
right_pose.header = hdr
right_pose.pose.position.x = 0.3
right_pose.pose.position.y = -0.7
right_pose.pose.position.z = 0.2
right_pose.pose.orientation.x = 0
right_pose.pose.orientation.y = 1
right_pose.pose.orientation.z = 0
right_pose.pose.orientation.w = 0
done = False
while not done and not rospy.is_shutdown():
c = baxter_external_devices.getch()
if c:
#catch Esc or ctrl-c
if c in ['\x1b', '\x03']:
done = True
# rospy.signal_shutdown("Example finished.")
elif c == 'l':
print "move left arm"
move_to_pose(left_pose = left_pose, timeout = 4)
elif c == 'r':
print "move right arm"
move_to_pose(right_pose = right_pose, timeout = 4)
elif c == 's':
request_object()
def map_keyboard():
# left = baxter_interface.Limb('left')
print "this"
left = BaxterArm('left')
right = BaxterArm('right')
kin = baxter_kinematics('left')
config = ALL_CONFIGS['position_baxter']
ctrlr = OSPositionController(left, config)
ctrlr.set_active(True)
grip_left = baxter_interface.Gripper('left', CHECK_VERSION)
grip_right = baxter_interface.Gripper('right', CHECK_VERSION)
lj = left.joint_names()
rj = right.joint_names()
leftdes = []
leftacc = []
lefterr = []
delta = 0.05 #was 0.05
gain = 1.
def IK(limb, movement):
current_pos, curr_ori = limb.ee_pose()
goal_pos = current_pos + movement
goal_ori = curr_ori
ctrlr.set_goal(goal_pos=goal_pos,
goal_ori=goal_ori,
orientation_ctrl=True)
lin_error, ang_error, success, time_elapsed = ctrlr.wait_until_goal_reached(
timeout=1.0)
#print (pos[1]-np.array(limb.endpoint_pose()['position'])[1])/delta 0.91 for x, 0.82 for z
# print (pos[2]-np.array(limb.endpoint_pose()['position'])[2])/delta
bindings = {
# key: (function, args, description)
's': (IK, [left, [delta, 0, 0]], "xinc"),
'd': (IK, [left, [-delta, 0, 0]], "xdec"),
'w': (IK, [left, [0, delta, 0]], "yinc"),
'e': (IK, [left, [0, -delta, 0]], "ydec"),
'x': (IK, [left, [0, 0, delta]], "zinc"),
'c': (IK, [left, [0, 0, -delta]], "zdec"),
#'n': (set_j, [left, lj[6], -0.1], "left_w2 decrease"),
'b': (grip_left.close, [], "left: gripper close"),
'n': (grip_left.open, [], "left: gripper open"),
'm': (grip_left.calibrate, [], "left: gripper calibrate") #comma here?
}
done = False
print("Controlling joints. Press ? for help, Esc to quit.")
while not done and not rospy.is_shutdown():
c = baxter_external_devices.getch()
if c:
#catch Esc or ctrl-c
if c in ['\x1b', '\x03']:
done = True
rospy.signal_shutdown("Example finished.")
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 __move(self, wp, accuracy=1.0):
rate = rospy.Rate(10.0)
if self.left_arm_enabled:
if wp.lgrip < 50:
self.left_grip.close()
else:
self.left_grip.open()
if self.right_arm_enabled:
if wp.rgrip < 50:
self.right_grip.close()
else:
self.right_grip.open()
done = False
if self.left_arm_enabled:
self.left_arm.set_joint_position_speed(
max(min(Script.MAX_VEL * wp.vel / 100.0, 1.0),
0.0)) #default 0.3, range 0.0 - 1.0
if self.right_arm_enabled:
self.right_arm.set_joint_position_speed(
max(min(Script.MAX_VEL * wp.vel / 100.0, 1.0),
0.0)) #default 0.3, range 0.0 - 1.0
dd = [9999.0, 9999.0, 9999.0, 9999.0, 9999.0]
while not done:
if self.cancel_move:
break
if self.left_arm_enabled:
self.left_arm.set_joint_positions(wp.ljpos)
if self.right_arm_enabled:
self.right_arm.set_joint_positions(wp.rjpos)
dist = 0.0
if self.left_arm_enabled:
for j in self.left_arm.joint_names():
dist = dist + (wp.ljpos[j] - self.left_arm.joint_angle(
j)) * (wp.ljpos[j] - self.left_arm.joint_angle(j))
if self.right_arm_enabled:
for j in self.right_arm.joint_names():
dist = dist + (wp.rjpos[j] - self.right_arm.joint_angle(
j)) * (wp.rjpos[j] - self.right_arm.joint_angle(j))
dist = math.sqrt(dist)
ss = 0.0
for d in dd:
ss = ss + math.fabs(d - dist)
if ss / (wp.vel / 100.0) < 0.1 / accuracy:
done = True
elif rospy.is_shutdown():
print("ROS shutdown, cancelled movement")
done = True
elif baxter_external_devices.getch() in ['\x1b', '\x03']:
print("stopped")
done = True
else:
dd.append(dist)
del dd[0]
rate.sleep()
if self.left_arm_enabled:
self.left_arm.set_joint_position_speed(0.3)
if self.right_arm_enabled:
self.right_arm.set_joint_position_speed(0.3)
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 = baxter_external_devices.getch()
if c:
# catch Esc or ctrl-c
if c in ["\x1b", "\x03"]:
done = True
rospy.signal_shutdown("Example finished.")
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():
# initialize interfaces
print("Getting robot state... ")
rs = baxter_interface.RobotEnable(CHECK_VERSION)
init_state = rs.state().enabled
left = baxter_interface.Gripper('left', CHECK_VERSION)
right = baxter_interface.Gripper('right', CHECK_VERSION)
def clean_shutdown():
if not init_state:
print("Disabling robot...")
rs.disable()
print("Exiting example.")
rospy.on_shutdown(clean_shutdown)
def capability_warning(gripper, cmd):
msg = ("%s %s - not capable of '%s' command" %
(gripper.name, gripper.type(), cmd))
rospy.logwarn(msg)
def offset_position(gripper, offset):
if gripper.type() != 'electric':
capability_warning(gripper, 'command_position')
return
current = gripper.position()
gripper.command_position(current + offset)
def offset_holding(gripper, offset):
if gripper.type() != 'electric':
capability_warning(gripper, 'set_holding_force')
return
current = gripper.parameters()['holding_force']
gripper.set_holding_force(current + offset)
def offset_moving(gripper, offset):
if gripper.type() != 'electric':
capability_warning(gripper, 'set_moving_force')
return
current = gripper.parameters()['moving_force']
gripper.set_moving_force(current + offset)
def offset_velocity(gripper, offset):
if gripper.type() != 'electric':
capability_warning(gripper, 'set_velocity')
return
current = gripper.parameters()['velocity']
gripper.set_velocity(current + offset)
def offset_dead_band(gripper, offset):
if gripper.type() != 'electric':
capability_warning(gripper, 'set_dead_band')
return
current = gripper.parameters()['dead_zone']
gripper.set_dead_band(current + offset)
bindings = {
# key: (function, args, description)
'r': (left.reboot, [], "left: reboot"),
'R': (right.reboot, [], "right: reboot"),
'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': (offset_dead_band, [left, -1.0], "left: decrease dead band"),
'Z': (offset_dead_band, [right, -1.0], "right: decrease dead band"),
'x': (offset_dead_band, [left, 1.0], "left: increase dead band"),
'X': (offset_dead_band, [right, 1.0], "right: increase dead band"),
'f': (offset_moving, [left, -5.0], "left: decrease moving force"),
'F': (offset_moving, [right, -5.0], "right: decrease moving force"),
'g': (offset_moving, [left, 5.0], "left: increase moving force"),
'G': (offset_moving, [right, 5.0], "right: increase moving force"),
'h': (offset_holding, [left, -5.0], "left: decrease holding force"),
'H': (offset_holding, [right, -5.0], "right: decrease holding force"),
'j': (offset_holding, [left, 5.0], "left: increase holding force"),
'J': (offset_holding, [right, 5.0], "right: increase holding force"),
'v': (offset_velocity, [left, -5.0], "left: decrease velocity"),
'V': (offset_velocity, [right, -5.0], "right: decrease velocity"),
'b': (offset_velocity, [left, 5.0], "left: increase velocity"),
'B': (offset_velocity, [right, 5.0], "right: increase velocity"),
'u': (offset_position, [left, -15.0], "left: decrease position"),
'U': (offset_position, [right, -15.0], "right: decrease position"),
'i': (offset_position, [left, 15.0], "left: increase position"),
'I': (offset_position, [right, 15.0], "right: increase position"),
}
done = False
print("Enabling robot... ")
rs.enable()
print("Controlling grippers. Press ? for help, Esc to quit.")
while not done and not rospy.is_shutdown():
c = baxter_external_devices.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]))
# force shutdown call if caught by key handler
rospy.signal_shutdown("Example finished.")
def run_control(joystick):
# Initialise parameters
kbOnly = True if joystick==None else False
print('kbOnly: ', kbOnly)
#abbreviations
if kbOnly == False:
jhi = lambda s: joystick.stick_value(s) > 0
jlo = lambda s: joystick.stick_value(s) < 0
bdn = joystick.button_down
bup = joystick.button_up
zeros = [0]*4
initial_inc = 0.03
inc = initial_inc
delta = 0.01
incU = 0.02
incL = 0.01
syncErrThresh = 0.003
vrepZdelta = 1.08220972
# Service to call functions in V-rep (e.g. set/get joint positions)
vrepScriptFunc = rospy.ServiceProxy('vrep/simRosCallScriptFunction', simRosCallScriptFunction)
# Get robot interface
robot = RobotArm(vrepScriptFunc)
robot.updPos('left')
# Get appropriate bindings
kb_b= js_b = False
def gen_js_bindings(inc):
bindings = (
((bdn, ['btnUp']),
(robot.grip_l_close, []), "left gripper"),
((jhi, ['rightTrigger']),
(inc_delta, [inc, delta, incU, incL]), "Speed increased"),
((jhi, ['leftTrigger']),
(inc_delta, [inc, -delta, incU, incL]), "Speed decreased"),
((bdn, ['leftBumper']),
(robot.pos_to_ori, ['down']), "Switched to orientation"),
((bup, ['leftBumper']),
(robot.pos_to_ori, ['up']), "Switched to position"),
((bdn, ['rightBumper']),
(robot.world_to_ee, ['ee']), "Switched to tool frame"),
((bup, ['rightBumper']),
(robot.world_to_ee, ['world']), "Switched to world frame"),
((jlo, ['leftStickHorz']),
(robot.command_ik, ['left', [0, -inc, 0]]), lambda: "left y dec "),
((jhi, ['leftStickHorz']),
(robot.command_ik, ['left', [0, inc, 0]]), lambda: "left y inc "),
((jlo, ['leftStickVert']),
(robot.command_ik, ['left', [-inc, 0, 0]]), lambda: "left x dec "),
((jhi, ['leftStickVert']),
(robot.command_ik, ['left', [inc, 0, 0]]), lambda: "left x inc "),
((jlo, ['rightStickVert']),
(robot.command_ik, ['left', [0, 0, -inc]]), lambda: "left z dec "),
((jhi, ['rightStickVert']),
(robot.command_ik, ['left', [0, 0, inc]]), lambda: "left z inc "),
((bdn, ['function1']),
(print_js_bindings, [js_b]), "help"),
((bdn, ['function2']),
(print_js_bindings, [js_b]), "help"),
)
return bindings
def gen_kb_bindings(inc):
bindings = {
'9': (inc_delta, [inc, delta, incU, incL], "Speed increased"),
'0': (inc_delta, [inc, -delta, incU, incL], "Speed decreased"),
'i': (robot.pos_to_ori, [], "Switch position/orientation"),
# 'o': (robot.world_to_ee, [], "Switch world/tool frame"),
'w': (robot.command_ik, ['left', [inc, 0, 0]], "increase left x"),
's': (robot.command_ik, ['left', [-inc, 0, 0]], "decrease left x"),
'a': (robot.command_ik, ['left', [0, inc, 0]], "increase left y"),
'd': (robot.command_ik, ['left', [0, -inc, 0]], "decrease left y"),
'q': (robot.command_ik, ['left', [0, 0, inc]], "increase left z"),
'e': (robot.command_ik, ['left', [0, 0, -inc]], "decrease left z"),
'p': (robot.grip_l_close, [], "Gripper open/close"),
# 'l': (robot.grip_l_open, [], "left: gripper open"),
# 'c': (grip_left.calibrate, [], "left: gripper calibrate")
}
return bindings
if kbOnly == False:
js_b = []
bindings = gen_js_bindings(inc)
js_b.append(bindings)
kb_b = gen_kb_bindings(inc)
# Print bindings
if kbOnly == False:
print_js_bindings(js_b)
else:
print_kb_bindings(kb_b)
# Setup data recording
endpoint_state=EndpointState()
def on_endpoint_states(msg,endpoint_state):
endpoint_state.header= msg.header
endpoint_state.pose= msg.pose
endpoint_state.twist= msg.twist
endpoint_state.wrench= msg.wrench
recording=False
done_recording=False
def record_data(filename):
# Subscribe to endpoint state
# _cartesian_state_sub = rospy.Subscriber(
# '/robot/limb/left/endpoint_state',
# EndpointState,
# on_endpoint_states,
# endpoint_state,
# queue_size=1,
# tcp_nodelay=True)
# bag = rosbag.Bag(filename, 'w')
def terminate_process_and_children(s):
list_cmd = subprocess.Popen("rosnode list", shell=True, stdout=subprocess.PIPE)
list_output = list_cmd.stdout.read()
retcode = list_cmd.wait()
assert retcode == 0, "List command returned %d" % retcode
for str in list_output.split("\n"):
if (str.startswith(s)):
os.system("rosnode kill " + str)
# while not done_recording:
# if recording==True:
# print('Endpoint actual: ',endpoint_state)
# bag.write('endpoint_state',endpoint_state)
rt = rospy.Rate(100)
while not recording:
rt.sleep()
p = subprocess.Popen('rosbag record -j -O 5.bag /robot/limb/left/endpoint_state '
'/camera1/rgb/image_color /camera2/rgb/image_color ',
# '/cameras/left_hand_camera/image',
stdin=subprocess.PIPE,shell=True, cwd='/home/domenico/talha_ws/src/baxter_ik/data/id_7/')
# /home/talha/test_ws/src/baxter_ik/scripts/
# print 'Finishing recording'
# bag.close()
while not done_recording:
rt.sleep()
print 'Finishing recording'
terminate_process_and_children("/record")
# thread.start_new_thread(record_data, ('test.bag',))
then = rospy.get_rostime()
# Move to start position
move_to_start_pos(robot,syncErrThresh)
vrepScriptFunc.call('set_ik_mode@Baxter_leftArm_target',1,[],[],[],'')
prevPosVrep = None
rate = rospy.Rate(100)
# while loop
while not rospy.is_shutdown():
# inputControl()
if kbOnly == True:
c = baxter_external_devices.getch(-1)
if c:
# print('Pressed: ',c)
recording=True
#catch Esc or ctrl-c
if c in ['\x1b', '\x03']:
# terminate_process_and_children(p)
rospy.signal_shutdown("Example finished.")
if c == '\r':
done_recording=True
# Keyboard control
if c in kb_b:
cmd = kb_b[c]
if cmd[0] == inc_delta:
inc = cmd[0](*cmd[1])
kb_b = gen_kb_bindings(inc)
elif cmd[0] == robot.pos_to_ori:
# kb_b = gen_kb_bindings(initial_inc)
cmd[0](*cmd[1])
else:
cmd[0](*cmd[1])
# print("command: %s" % (cmd[2],))
else:
# Joystick control
for (test, cmd, doc) in bindings:
then = rospy.get_rostime()
if test[0](*test[1]):
recording=True
if cmd[0] == inc_delta:
inc = cmd[0](*cmd[1])
bindings = gen_js_bindings(inc)
js_b[0] = bindings
elif cmd[0] == robot.pos_to_ori:
# bindings = gen_js_bindings(initial_inc)
# js_b[0] = bindings
cmd[0](*cmd[1])
else:
cmd[0](*cmd[1])
if callable(doc):
print(doc())
else:
print(doc)
# Check if IK was a success in V-rep
resp = vrepScriptFunc.call('ikSuccess@Baxter_leftArm_target',1,[],[],[],'')
# print 'Ik Success: ', resp.outputInts
# If successful, receive & send joints to Baxter
if resp.outputInts[0] == -1:
# Get resulting joint positions from V-rep
resp = vrepScriptFunc.call('get_jnt_pos@Baxter_leftArm_target',1,[],[],[],'')
if prevPosVrep is None:
prevPosVrep = [0,0,0,0,0,0,0]
err2 = np.linalg.norm(np.array(resp.outputFloats) - np.array(prevPosVrep))
# print(resp.outputFloats)
# print(prevPosVrep)
prevPosVrep = copy.deepcopy(resp.outputFloats)
# Get Baxter joints
curr_jnts = np.array([robot.left.joint_angles()[name] for name in robot.left.joint_names()])
# Compare joints
norm = np.linalg.norm(curr_jnts)
err = math.fabs(np.linalg.norm(np.array(resp.outputFloats)-curr_jnts))
err = err/norm
# print('Error: ',err)
# print('ErrorVrep: ', err2)
# limb_joints = dict(zip(robot.left._joint_names['left'], resp.outputFloats))
# robot.left.set_joint_positions(limb_joints)
# robot.syncPos('left')
if err > abs(syncErrThresh):
if err2 > 0.0001:
# print('Position updated')
# Wrap and send joint positions to Baxter
limb_joints = dict(zip(robot.left._joint_names['left'], resp.outputFloats))
diff = rospy.get_rostime() - then
# print ('Time taken:', diff.to_sec())
robot.left.set_joint_positions(limb_joints)
curr_jnts = np.array([robot.left.joint_angles()[name] for name in robot.left.joint_names()])
jnt_err = np.zeros((7, 1))
for x in range(curr_jnts.size):
jnt_err[x] = curr_jnts[x] - resp.outputFloats[x]
# print('Joint difference: ', jnt_err.tolist())
# print('V-rep Joints: ',limb_joints)
# resp = vrepScriptFunc.call('get_pose@Baxter_leftArm_target',1,[],[],[],'')
# print 'V-rep pose: ',resp.outputFloats
# print('---------------------------------------------------------------------------------'/0)
# print 'Finish pose: ',current_p
# print('Actual joints: ', robot.left.joint_angles())
# print('---------------------------------------------------------------------------------')
rate.sleep()
def map_keyboard():
#user set joint angles
theta={}
theta['right_s0'] = np.float32(raw_input("Enter joint1: "))
theta['right_s1'] = np.float32(raw_input("Enter joint2: "))
theta['right_e0'] = np.float32(raw_input("Enter joint3: "))
theta['right_e1'] = np.float32(raw_input("Enter joint4: "))
theta['right_w0'] = np.float32(raw_input("Enter joint5: "))
theta['right_w1'] = np.float32(raw_input("Enter joint6: "))
theta['right_w2'] = np.float32(raw_input("Enter joint7: "))
def judge(dict):
if (abs(limb.joint_angle(joints[0])-theta['right_s0'])>0.1 or
abs(limb.joint_angle(joints[1])-theta['right_s1'])>0.1 or
abs(limb.joint_angle(joints[2])-theta['right_e0'])>0.1 or
abs(limb.joint_angle(joints[3])-theta['right_e1'])>0.1 or
abs(limb.joint_angle(joints[4])-theta['right_w0'])>0.1 or
abs(limb.joint_angle(joints[5])-theta['right_w1'])>0.1 or
abs(limb.joint_angle(joints[6])-theta['right_w2'])>0.1):
return True
else:
return False
while(judge(theta)):
limb.set_joint_positions(theta)
time.sleep(0.01)
limb = intera_interface.Limb(side)
left = baxter_interface.Limb('left')
right = baxter_interface.Limb('right')
grip_left = baxter_interface.Gripper('left', CHECK_VERSION)
grip_right = baxter_interface.Gripper('right', CHECK_VERSION)
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 = baxter_external_devices.getch()
if c:
#catch Esc or ctrl-c
if c in ['\x1b', '\x03']:
done = True
rospy.signal_shutdown("Example finished.")
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', CHECK_VERSION)
grip_right = baxter_interface.Gripper('right', CHECK_VERSION)
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.005], "left_s0 increase"),
'6': (set_j, [left, lj[0], -0.005], "left_s0 decrease"),
'8': (set_j, [left, lj[1], 0.005], "left_s1 increase"),
'7': (set_j, [left, lj[1], -0.005], "left_s1 decrease"),
'o': (set_j, [left, lj[2], 0.005], "left_e0 increase"),
'y': (set_j, [left, lj[2], -0.005], "left_e0 decrease"),
'i': (set_j, [left, lj[3], 0.005], "left_e1 increase"),
'u': (set_j, [left, lj[3], -0.005], "left_e1 decrease"),
'l': (set_j, [left, lj[4], 0.005], "left_w0 increase"),
'h': (set_j, [left, lj[4], -0.005], "left_w0 decrease"),
'k': (set_j, [left, lj[5], 0.005], "left_w1 increase"),
'j': (set_j, [left, lj[5], -0.005], "left_w1 decrease"),
'.': (set_j, [left, lj[6], 0.005], "left_w2 increase"),
'n': (set_j, [left, lj[6], -0.005], "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.005], "right_s0 increase"),
'1': (set_j, [right, rj[0], -0.005], "right_s0 decrease"),
'3': (set_j, [right, rj[1], 0.005], "right_s1 increase"),
'2': (set_j, [right, rj[1], -0.005], "right_s1 decrease"),
'r': (set_j, [right, rj[2], 0.005], "right_e0 increase"),
'q': (set_j, [right, rj[2], -0.005], "right_e0 decrease"),
'e': (set_j, [right, rj[3], 0.005], "right_e1 increase"),
'w': (set_j, [right, rj[3], -0.005], "right_e1 decrease"),
'f': (set_j, [right, rj[4], 0.005], "right_w0 increase"),
'a': (set_j, [right, rj[4], -0.005], "right_w0 decrease"),
'd': (set_j, [right, rj[5], 0.005], "right_w1 increase"),
's': (set_j, [right, rj[5], -0.005], "right_w1 decrease"),
'v': (set_j, [right, rj[6], 0.005], "right_w2 increase"),
'z': (set_j, [right, rj[6], -0.005], "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 = baxter_external_devices.getch()
if c:
#catch Esc or ctrl-c
if c in ['\x1b', '\x03']:
done = True
rospy.signal_shutdown("Example finished.")
elif c in bindings:
cmd = bindings[c]
#expand binding to something like "set_j(right, 's0', 0.005)"
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', CHECK_VERSION)
grip_right = baxter_interface.Gripper('right', CHECK_VERSION)
#These are from PyKDL and are needed for the Jacobian
left_kin = baxter_kinematics('left')
right_kin = baxter_kinematics('right')
#Connect with IK service
right_iksvc, right_ns = ik_command.connect_service('right')
left_iksvc, left_ns = ik_command.connect_service('left')
def command_jacobian(side, direction):
#UNUSED
if side == 'left':
limb = left
kin = left_kin
elif side == 'right':
limb = right
kin = right_kin
else:
raise Exception("Got wrong side in command_jacobian")
# current is the current position of end effector
current_p = np.array(limb.endpoint_pose()['position'] +
limb.endpoint_pose()['orientation'])
current_p = current_q.reshape((7, 1))
current = np.zeros((6, 1))
quaternion_to_euler(current_p, current)
# We need to convert the direction from the world frame to the hand frame
# Rotate direction by the inverse of the rotation matrix from the angular pose in current
R = tf.transformations.quaternion_matrix(
current_p[3:7].flatten().tolist())
R_inv = tf.transformations.inverse_matrix(R)
print R_inv[0:3, 0:3]
direction = np.array(direction).reshape((6, 1))
print direction[0:3]
dir_rot = np.dot(R_inv[0:3, 0:3], direction[0:3])
print dir_rot
#direction[0:3] = dir_rot
# Goal is the goal position, found by adding the requested direction from the user
goal = current + direction
current_angles = np.array(
[limb.joint_angles()[name] for name in limb.joint_names()])
# Get the Jacobian inverse and solve for the necessary change in joint angles
jacobian_inv = kin.jacobian_transpose()
delta = jacobian_inv * (goal - current)
commands = current_angles.flatten() + delta.flatten()
# Command the joints
command_list = commands.tolist()
joint_command = dict(zip(limb.joint_names(), command_list[0]))
limb.set_joint_positions(joint_command)
def command_ik(side, direction):
"""Use the Rethink IK service to figure out a desired joint position"""
if side == 'left':
limb = left
iksvc = left_iksvc
ns = left_ns
else:
limb = right
iksvc = right_iksvc
ns = right_ns
current_p = np.array(limb.endpoint_pose()['position'] +
limb.endpoint_pose()['orientation'])
direction = np.array(direction)
desired_p = current_p + direction
ik_command.service_request(iksvc, desired_p, side, False)
zeros = [0] * 4
inc = 0.1
bindings = {
'q': (command_ik, ['right', [inc, 0, 0] + zeros], "increase right x"),
'a': (command_ik, ['right', [-inc, 0, 0] + zeros], "decrease right x"),
'w': (command_ik, ['right', [0, inc, 0] + zeros], "increase right y"),
's': (command_ik, ['right', [0, -inc, 0] + zeros], "decrease right y"),
'e': (command_ik, ['right', [0, 0, inc] + zeros], "increase right z"),
'd': (command_ik, ['right', [0, 0, -inc] + zeros], "decrease right z"),
'u': (command_ik, ['left', [inc, 0, 0] + zeros], "increase left x"),
'j': (command_ik, ['left', [-inc, 0, 0] + zeros], "decrease left x"),
'i': (command_ik, ['left', [0, inc, 0] + zeros], "increase left y"),
'k': (command_ik, ['left', [0, -inc, 0] + zeros], "decrease left y"),
'o': (command_ik, ['left', [0, 0, inc] + zeros], "increase left z"),
'l': (command_ik, ['left', [0, 0, -inc] + zeros], "decrease left z"),
'z': (grip_right.close, [], "right: gripper close"),
'x': (grip_right.open, [], "right: gripper open"),
'c': (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 = baxter_external_devices.getch()
if c:
#catch Esc or ctrl-c
if c in ['\x1b', '\x03']:
done = True
rospy.signal_shutdown("Example finished.")
elif c in bindings:
cmd = bindings[c]
#expand binding to something like "command_jacobian(left, [0.1, 0, 0])"
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]))
self.left_arm.move_to_joint_positions(l_angles)
self.right_arm.move_to_joint_positions(r_angles)
if __name__ == '__main__':
print("Welcome.")
rospy.init_node("rethink_rsdk_joint_velocity")
#print("Getting Baxter's state... ")
rs = baxter_interface.RobotEnable()
print("Attempting to enable Baxter")
rs.enable()
task_completer = Mover('both')
task_completer.move()
print("Completing tasks. Press Esc to quit.")
done = False
while not done and not rospy.is_shutdown():
c = baxter_external_devices.getch()
if c:
#catch Esc or ctrl-c
if c in ['\x1b', '\x03']:
done = True
else:
print("Press Esc to quit.")
print("All tasks completed.")
print("Disabling Baxter... ")
rs.disable()
print("done.")
def run(self):
#print "Recording Grids Location..."
#self.grids_location_recorder()
#print "Recording Grid Roi..."
#self.grids_roi_recorder()
cur_arm = 'left'
step = 0.004
print "Input: "
while not rospy.is_shutdown():
c = baxter_external_devices.getch(1.0)
if c:
if c in ['w']:
pose = self.current_poses['left']
x = pose.pose.position.x
y = pose.pose.position.y
z = -0.0247 #pose.pose.position.z
ox = 0.0
oy = 1.0
oz = 0.0
ow = 0.0
print " "
print "Get w"
x = x + step
cur_arm = 'left'
elif c in ['z']:
pose = self.current_poses['left']
x = pose.pose.position.x
y = pose.pose.position.y
z = -0.0247 #pose.pose.position.z
ox = 0.0
oy = 1.0
oz = 0.0
ow = 0.0
print " "
print "Get z"
x = x - step
cur_arm = 'left'
elif c in ['a']:
pose = self.current_poses['left']
x = pose.pose.position.x
y = pose.pose.position.y
z = -0.0247 #pose.pose.position.z
ox = 0.0
oy = 1.0
oz = 0.0
ow = 0.0
print " "
print "Get a"
y = y + step
cur_arm = 'left'
elif c in ['s']:
pose = self.current_poses['left']
x = pose.pose.position.x
y = pose.pose.position.y
z = -0.0247 #pose.pose.position.z
ox = 0.0
oy = 1.0
oz = 0.0
ow = 0.0
print " "
print "Get s"
y = y - step
cur_arm = 'left'
if c in ['i']:
pose = self.current_poses['right']
x = pose.pose.position.x
y = pose.pose.position.y
z = -0.0247 #pose.pose.position.z
ox = 0.0
oy = 1.0
oz = 0.0
ow = 0.0
print " "
print "Get i"
x = x + step
cur_arm = 'right'
elif c in ['m']:
pose = self.current_poses['right']
x = pose.pose.position.x
y = pose.pose.position.y
z = -0.0247 #pose.pose.position.z
ox = 0.0
oy = 1.0
oz = 0.0
ow = 0.0
print " "
print "Get m"
x = x - step
cur_arm = 'right'
elif c in ['j']:
pose = self.current_poses['right']
x = pose.pose.position.x
y = pose.pose.position.y
z = -0.0247 #pose.pose.position.z
ox = 0.0
oy = 1.0
oz = 0.0
ow = 0.0
print " "
print "Get j"
y = y + step
cur_arm = 'right'
elif c in ['k']:
pose = self.current_poses['right']
x = pose.pose.position.x
y = pose.pose.position.y
z = -0.0247 #pose.pose.position.z
ox = 0.0
oy = 1.0
oz = 0.0
ow = 0.0
y = y - step
cur_arm = 'right'
print " "
print "Get k"
else:
rospy.sleep(0.1)
continue
msg_string = cur_arm + ':move_to:' + \
str(x) + \
',' + \
str(y) + \
',' + \
str(z) + \
',' + \
str(ox) + \
',' + \
str(oy) + \
',' + \
str(oz) + \
',' + \
str(ow)
self.ArmCmdPub.publish(msg_string)
rospy.sleep(0.1)