def __init__(self, local=True):
"""
Instantiates an interface for darias.
"""
# if not local
self._enabled = True
self._root = "/franka_ros_interface"
# if local
# self._root = "/panda_simulator"
# rospy.init_node("franka_robot_gym")
rospy.init_node("robopy_interface")
self.arms = Arms(self)
self._arm_interface = ArmInterface(True)
self._gripper_interface = GripperInterface()
self._robot_status = RobotEnable()
self._ctrl_manager = FrankaControllerManagerInterface(
ns=self._root, sim=False) # TODO: (sim=false for real robot))
print(self._ctrl_manager)
self._movegroup_interface = None # TODO: consider removing it
self._joint_command_publisher = rospy.Publisher(
self._root + '/motion_controller/arm/joint_commands',
# 'joint_commands',
JointCommand,
tcp_nodelay=True,
queue_size=1)
while not (self.arms.ready):
pass
self.groups = {}
self._building_groups()
# TODO: temporary
self._joint_names = self.groups["arm_gripper"].refs
feedback_cb=_feedback_cb)
result = stop_action_client.wait_for_result(rospy.Duration(15.))
parser = argparse.ArgumentParser(
"Stop current gripper action; Open or close gripper.")
gripper_group = parser.add_mutually_exclusive_group(required=False)
gripper_group.add_argument("-o",
"--open",
dest="open",
action='store_true',
default=False,
help="open gripper")
gripper_group.add_argument("-c",
"--close",
dest="close",
action='store_true',
default=False,
help="close gripper")
args = parser.parse_args(rospy.myargv()[1:])
if args.open or args.close:
gi = GripperInterface()
if args.open:
gi.open()
else:
gi.close()
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]))