This code is for testing the torque controller. The robot should be in zeroG mode, i.e.
It should stay still when no external force is acting, but should be very compliant
(almost no resistance) when pushed. This code sends zero torque command continuously,
which means the internal gravity compensation is the only torque being sent to the
robot.
Test by pushing robot.
DO NOT RUN THIS CODE WITH CUSTOM END-EFFECTOR UNLESS YOU KNOW WHAT YOU'RE DOING!
WARNING: This code only works if the robot model is good! If you have installed custom
end-effector, the gravity compensation may not be good unless you have incorporated
the model to the FCI via Desk!!
"""
if __name__ == '__main__':
rospy.init_node("test_zeroG")
r = ArmInterface() # create arm interface instance (see https://justagist.github.io/franka_ros_interface/DOC.html#arminterface for all available methods for ArmInterface() object)
rospy.loginfo("Commanding...\n")
r.move_to_neutral() # move robot to neutral pose
rate = rospy.Rate(1000)
joint_names = r.joint_names()
while not rospy.is_shutdown():
r.set_joint_torques(dict(zip(joint_names, [0.0]*7))) # send 0 torques
rate.sleep()
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]))
initial_pose = deepcopy(r.joint_ordered_angles())
input("Hit Enter to Start")
print("commanding")
vals = deepcopy(initial_pose)
count = 0
while not rospy.is_shutdown():
elapsed_time_ += period
delta = 3.14 / 16.0 * (
1 - np.cos(3.14 / 5.0 * elapsed_time_.to_sec())) * 0.2
for j, _ in enumerate(vals):
if j == 4:
vals[j] = initial_pose[j] - delta
else:
vals[j] = initial_pose[j] + delta
if count % 500 == 0:
print(vals, delta)
print("\n ---- \n")
print(" ")
# r.set_joint_positions_velocities(vals, [0.0 for _ in range(7)]) # for impedance control
r.set_joint_positions(dict(zip(r.joint_names(),
vals))) # try this for position control
count += 1
rate.sleep()
return np.array([
[0, -p[2], p[1]],
[p[2], 0, -p[0]],
[-p[1], p[0], 0]
])
if __name__ == '__main__':
rospy.init_node("reference_governor")
# Create the robot
robot = ArmInterface()
vals_pos = robot.joint_angles()
vals_vel = robot.joint_velocities()
joint_names = robot.joint_names()
rospy.loginfo('RefGov: Moving to neutral')
robot.move_to_neutral()
# Subscriber to the camera_info topic
camera_info_msg = rospy.wait_for_message("/d435_color/camera_info", CameraInfo, timeout=None)
intrinsic = get_intrinsic_param(camera_info_msg)
# Transformations
tf_listener = tf.TransformListener()
tf_broadcaster = tf.TransformBroadcaster()
# wait 5s for the robot arm and camera to be spawned in
try:
