🐼

Franka Emika Panda

To get Franka set up in ark:

https://github.com/Robotics-Ark/ark_franka
git clone https://github.com/Robotics-Ark/ark_franka.git
cd ark_franka
pip install -e .

🔧 Franka Node Communication Breakdown

graph TD
    A["Franka/joint_group_command<br><sub>joint_group_command_t</sub>"] --> C((Franka))
    B["Franka/cartesian_command<br><sub>task_space_command_t</sub>"] --> C
    C --> D["Franka/joint_states<br><sub>joint_state_t</sub>"]

Joint Group Control Franka/joint_group_command

Joint group commands can target the entire robot "all", just the "arm" or the "gripper". Position or velocity control is selected in the robot configuration tests/config/robots/franka.yaml.

# Position command for all joints including the gripper
joint_command = [-0.3, 0.1, 0.3, -1.4, 0.1, 1.8, 0.6, 0]

controller.joint_group_command.publish(
	pack.joint_group_command(joint_command, "all")
)
# Velocity command for the gripper: 1 opens, -1 closes
gripper_command = [1]

controller.joint_group_command.publish(
	pack.joint_group_command(gripper_command, "gripper")
)
# Position command for the arm only
arm_command = [-0.3, 0.5, 0.3, -1.0, 0.1, 1.8, 0.1]

controller.joint_group_command.publish(
	pack.joint_group_command(arm_command, "arm")
)

Task Space Control Franka/cartesian_command

Cartesian commands specify a target position and orientation expressed as an XYZ vector and a quaternion.

xyz_command = np.array([0.3, 0.4, 0.8])
quaternion_command = np.array([1, 0.0, 0.0, 0.0])  # xyz-w

controller.task_space_command.publish(
    pack.task_space_command("all", xyz_command, quaternion_command)
)

Franka States Franka/joint_states

Joint states publish the state of the Franka robot. containing the data:

struct joint_state_t {
    header_t header;
    string name[n];
    double position[n];
    double velocity[n];
    double effort[n];
}

Complete Code:

https://github.com/Robotics-Ark/ark_franka/blob/main/tests/franka_controller_interface.ipynb