def map_keyboard():
"""
Map keyboard keys to robot joint motion. Keybindings can be
found when running the script.
"""
limb = PandaArm()
has_gripper = limb.get_gripper() is not None
# joints = limb.joint_names()
def set_ee_target(action, value):
pos, ori = limb.ee_pose()
if action == 'position':
pos += value
status, j_des = limb.inverse_kinematics(pos, ori)
# print j_des
if status:
limb.exec_position_cmd(j_des)
def set_g(action):
if has_gripper:
if action == "close":
limb.get_gripper().close()
elif action == "open":
limb.get_gripper().open()
elif action == "calibrate":
limb.get_gripper().calibrate()
def reset_robot(args):
limb.untuck()
bindings = {
'5': (set_ee_target, ['position', np.asarray([pos_increment, 0, 0])], "x increase"),
'2': (set_ee_target, ['position', np.asarray([-pos_increment, 0, 0])], "x decrease"),
'1': (set_ee_target, ['position', np.asarray([0, pos_increment, 0])], "y increase"),
'3': (set_ee_target, ['position', np.asarray([0, -pos_increment, 0])], "y decrease"),
'7': (set_ee_target, ['position', np.asarray([0, 0, pos_increment])], "z increase"),
'4': (set_ee_target, ['position', np.asarray([0, 0, -pos_increment])], "z decrease"),
'r': (reset_robot, [None], "reset to neutral pose")
}
if has_gripper:
bindings.update({
'8': (set_g, "close", "close gripper"),
'9': (set_g, "open", "open gripper"),
'i': (set_g, "calibrate", "calibrate gripper")
})
done = False
print("Controlling joints. Press ? for help, Esc to quit.")
while not done and not rospy.is_shutdown():
c = 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]
if c == '8' or c == 'i' or c == '9':
cmd[0](cmd[1])
print("command: %s" % (cmd[2],))
else:
#expand binding to something like "set_j(right, 'j0', 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():
"""
Map keyboard keys to robot joint motion. Keybindings can be
found when running the script.
"""
limb = ArmInterface()
gripper = GripperInterface(ns=limb.get_robot_params().get_base_namespace())
has_gripper = gripper.exists
joints = limb.joint_names()
def set_j(limb, joint_name, delta):
joint_command = limb.joint_angles()
joint_command[joint_name] += delta
limb.set_joint_positions(joint_command)
# limb.set_joint_positions_velocities([joint_command[j] for j in joints], [0.00001]*7) # impedance control when using this example might fail because commands are sent too quickly for the robot to respond
def set_g(action):
if has_gripper:
if action == "close":
gripper.close()
elif action == "open":
gripper.open()
elif action == "calibrate":
gripper.calibrate()
bindings = {
'1': (set_j, [limb, joints[0], 0.01], joints[0] + " increase"),
'q': (set_j, [limb, joints[0], -0.01], joints[0] + " decrease"),
'2': (set_j, [limb, joints[1], 0.01], joints[1] + " increase"),
'w': (set_j, [limb, joints[1], -0.01], joints[1] + " decrease"),
'3': (set_j, [limb, joints[2], 0.01], joints[2] + " increase"),
'e': (set_j, [limb, joints[2], -0.01], joints[2] + " decrease"),
'4': (set_j, [limb, joints[3], 0.01], joints[3] + " increase"),
'r': (set_j, [limb, joints[3], -0.01], joints[3] + " decrease"),
'5': (set_j, [limb, joints[4], 0.01], joints[4] + " increase"),
't': (set_j, [limb, joints[4], -0.01], joints[4] + " decrease"),
'6': (set_j, [limb, joints[5], 0.01], joints[5] + " increase"),
'y': (set_j, [limb, joints[5], -0.01], joints[5] + " decrease"),
'7': (set_j, [limb, joints[6], 0.01], joints[6] + " increase"),
'u': (set_j, [limb, joints[6], -0.01], joints[6] + " decrease")
}
if has_gripper:
bindings.update({
'8': (set_g, "close", "close gripper"),
'i': (set_g, "open", "open gripper"),
'9': (set_g, "calibrate", "calibrate gripper")
})
done = False
rospy.logwarn(
"Controlling joints. Press ? for help, Esc to quit.\n\nWARNING: The motion will be slightly jerky!!\n"
)
while not done and not rospy.is_shutdown():
c = 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]
if c == '8' or c == 'i' or c == '9':
cmd[0](cmd[1])
print("command: %s" % (cmd[2], ))
else:
#expand binding to something like "set_j(right, 'j0', 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(viewitems(bindings),
key=lambda x: x[1][2]):
print(" %s: %s" % (key, val[2]))