franka_interface

# franka_interface A lightweight interface for Franka robots built on top of libfranka and ros. Features: - Joint impedance control - Real-time state access - joint position - joint velocity - joint torque - TCP pose - TCP velocity - Simple Python APIs - C++ backend with pybind11 bindings - Low latency and minimal dependencies ## Current package This repository currently provides a ROS 1 `ros_control` controller package: - package: `franka_interface` - controller plugin: `franka_interface/JointImpedanceController` - official-hold config: `config/joint_impedance_official_hold.yaml` - compliant-hold config: `config/joint_impedance_compliant_hold.yaml` - very-soft hold config: `config/joint_impedance_very_soft_hold.yaml` - damping-mode config: `config/joint_impedance_damping_mode.yaml` - high-damping-mode config: `config/joint_impedance_high_damping_mode.yaml` - full launch file: `launch/joint_impedance_controller.launch` - full relaxed arm + gripper launch file: `launch/full_system_relaxed.launch` - load-only launch file: `launch/load_joint_impedance_controller.launch` - follow launch file: `launch/joint_impedance_follow.launch` - relaxed collision launch file: `launch/franka_control_relaxed_collision.launch` - official-style launch file: `launch/joint_impedance_controller_official_style.launch` - upstream reference files: `reference/franka_example_controllers/` The controller implements joint-level impedance: ```text tau_cmd = K * (q_d - q) + D * (dq_d - dq_filtered) + coriolis_factor * coriolis ``` For `libfranka`/`franka_ros`, joint torque commands are specified without gravity and friction. This controller follows the official Franka joint impedance example and does not explicitly add gravity or friction terms to the commanded torque. Coriolis compensation is enabled by default. ## Build inside the Ubuntu 20.04 Docker/franka_ros environment The recommended layout is to place this repository inside the container's catkin workspace `src` directory: ```bash cd ~/catkin_ws/src ln -s /path/in/container/franka_interface franka_interface cd ~/catkin_ws catkin_make source devel/setup.bash ``` If the repository is mounted from the Ubuntu 22.04 host, start the container with the repository bind-mounted, then build inside Ubuntu 20.04: ```bash docker run --rm -it --net=host --privileged \ -v /home/sqz/projects/franka_interface:/root/catkin_ws/src/franka_interface \ <your-franka-ros-image> bash ``` Inside the container: ```bash source /opt/ros/noetic/setup.bash cd /root/catkin_ws catkin_make source devel/setup.bash ``` ## Run Start with `config/joint_impedance_official_hold.yaml`, which uses the same joint gains as the official Franka joint impedance example but holds the current joint pose instead of generating the Cartesian circular trajectory. After that is stable, switch to `config/joint_impedance_compliant_hold.yaml` for hand-guided compliance tests. - `arm_id`: usually `panda` - `joint_names`: must match the robot namespace - `k_gains`, `d_gains`: use the official-hold profile first, then tune the compliant profile slowly Launch: ```bash roslaunch franka_interface joint_impedance_controller.launch robot_ip:=<robot-ip> ``` Command a 7-DoF joint target: ```bash rostopic pub -1 /joint_impedance_controller/command std_msgs/Float64MultiArray \ "data: [0.0, -0.785398, 0.0, -2.35619, 0.0, 1.5708, 0.785398]" ``` You can also send 14 values: first 7 are `q_d`, last 7 are `dq_d`. Send a small relative joint step: ```bash rosrun franka_interface send_joint_step.py _joint_index:=1 _step_deg:=-5.0 ``` Run the joint-space linear interpolator separately: ```bash roslaunch franka_interface joint_linear_interpolator.launch ``` Send a target with default duration: ```bash rostopic pub -1 /joint_linear_interpolator/target std_msgs/Float64MultiArray \ "data: [0.0, -0.785398, 0.0, -2.35619, 0.0, 1.5708, 0.785398]" ``` The full relaxed launch also starts this interpolator by default. Override its publish rate with: ```bash roslaunch franka_interface full_system_relaxed.launch \ robot_ip:=172.16.0.2 interpolator_rate:=500.0 ``` Send a target with explicit duration as the 8th value. Duration is clamped to `[1.0, 30.0]` seconds by default: ```bash rostopic pub -1 /joint_linear_interpolator/target std_msgs/Float64MultiArray \ "data: [0.0, -0.785398, 0.0, -2.35619, 0.0, 1.5708, 0.785398, 3.0]" ``` For a trajectory that is already sampled at a fixed rate, use the waypoint player. It accepts a `trajectory_msgs/JointTrajectory`; each point's `positions` field is one seven-joint waypoint. `source_rate` describes the planning sample rate, while `rate` is the actual command publication rate. When they differ, the player linearly interpolates positions between adjacent waypoints. Velocities are not required: The default `full_system_relaxed.launch` and `start_full_system.sh` start this player automatically. Its default source rate is 40 Hz and its command output rate is 200 Hz. To start the full system with 40 Hz planning and 200 Hz command output: ```bash roslaunch franka_interface full_system_relaxed.launch \ robot_ip:=172.16.0.2 \ waypoint_source_rate:=40.0 waypoint_rate:=200.0 ``` For standalone testing, use `joint_waypoint_player.launch` and pass `source_rate:=40.0 rate:=200.0` there instead. Publish the trajectory on `/joint_waypoint_player/trajectory`. The player publishes `[q_d, dq_d]`, with `dq_d` set to seven zeros. Do not run an active waypoint player and an active target/delta command at the same time because both publish to the same impedance-controller command topic. For a small test trajectory, `send_joint_step_trajectory.py` reads the current controller target and generates a trajectory from that target: ```bash rosrun franka_interface send_joint_step_trajectory.py \ _joint_index:=1 _step_deg:=5.0 _duration:=2.0 _source_rate:=40.0 ``` Debug data is published at `publish_rate` on: ```text /joint_impedance_controller/debug_tau ``` The message type is `std_msgs/Float64MultiArray` with this layout: ```text [0:7] tau_commanded [7:14] tau_measured [14:21] tau_J_d [21:28] coriolis [28:35] q [35:42] q_d [42:49] dq_filtered ``` Stop and unload the controller cleanly: ```bash rosrun franka_interface stop_joint_impedance_controller.sh ``` Run the external follow target node after `franka_control` is running: ```bash roslaunch franka_interface joint_impedance_follow.launch arm_id:=panda ``` Start Franka control with the default robot IP: ```bash rosrun franka_interface start_franka_control.sh ``` Start Franka control with relaxed collision thresholds: ```bash roslaunch franka_interface franka_control_relaxed_collision.launch \ robot_ip:=172.16.0.2 load_gripper:=false ``` Or start Franka control and the compliant joint impedance controller together: ```bash roslaunch franka_interface joint_impedance_controller_relaxed_collision.launch \ robot_ip:=172.16.0.2 load_gripper:=false ``` The relaxed thresholds live in `config/franka_control_relaxed_collision.yaml`. They reduce reflex sensitivity for compliance/hand-guiding tests; they do not change libfranka hard torque, velocity, or acceleration limits. Start the full relaxed arm + Franka Hand stack: ```bash roslaunch franka_interface full_system_relaxed.launch robot_ip:=172.16.0.2 ``` This launch starts relaxed `franka_control`, the compliant joint impedance controller, official `franka_gripper`, and the `gripper_interface` wrapper. Do not also start `franka_gripper.launch` separately when using it. Convenience script: ```bash rosrun franka_interface start_full_system.sh ``` Recover Franka reflex/error state: ```bash rosrun franka_interface recover_franka_errors.sh ``` ## Safety notes - Test with low stiffness first. - Keep the user stop available. - Keep the first controller version aligned with the official `PD + coriolis` torque command before adding experimental feedforward terms. ## Manual ROS startup The ROS stack is started manually from the Ubuntu host. Install the command: ```bash sudo install -m 0755 host/start-franka /usr/local/bin/start-franka sudo install -m 0755 host/my_command /usr/local/bin/my_command ``` With FCI enabled and the robot stationary, run: ```bash start-franka ``` Print the command guide at any time: ```bash my_command ``` The command ensures that `franka_fci_dev` is running, sources ROS and the catkin workspace inside the container, and executes the existing `start_full_system.sh`. It attaches to the launch process in the terminal; press `Ctrl-C` to stop the complete ROS stack. After stopping, run `start-franka` again for a fresh launch. This is deliberately a foreground command. If the SSH session closes, the attached ROS stack may also terminate. Use `tmux` or `screen` only when the stack must survive an SSH disconnect. The old host-side FCI supervisor files are no longer part of the startup path. They are not required for the MCP service or the manual ROS workflow. ## MCP service The repository contains a two-process MCP integration: - `host/franka_mcp_server.py` is a host-side, always-on MCP Streamable HTTP server. It does not import ROS and remains available when FCI is disabled. - `scripts/franka_mcp_ros_gateway.py` is launched with the manually started ROS stack and listens only on `127.0.0.1:8765`. It reads the Franka state and submits complete trajectories to the local ROS nodes. The network-facing endpoint is intended to be served by the host at `https://172.16.0.1:8443/mcp`. The MCP service exposes: - `get_status` - `get_robot_state` - `get_full_robot_state` - `move_to_joint_target` - `track_joint_trajectory` - `open_gripper` - `close_gripper` - `move_gripper` (disabled by default) Motion commands are rejected while FCI or the robot is not ready. They are not queued for later execution. A waypoint request is sent as one complete list; the local waypoint player uses its timestamps to interpolate at the configured 200 Hz output rate. The impedance controller remains the local 1 kHz torque loop. `open_gripper` and `move_gripper` call the existing configured gripper commands without changing their parameters. `close_gripper` uses the existing default close force when called without arguments; an explicit `force` uses the existing `/gripper_interface/close_force` command. `move_gripper` takes an absolute opening width in meters. The `move_gripper` implementation is retained for compatibility but is not registered by default. Set `FRANKA_MCP_ENABLE_MOVE_GRIPPER=true` in the MCP service environment and restart the service to expose it again. The official MCP Python SDK currently requires Python 3.10 or newer, so install it in a host-side virtual environment rather than in the ROS Noetic Python 3.8 environment: ```bash python3 --version sudo install -d -m 0755 /opt/franka-mcp sudo python3 -m venv /opt/franka-mcp/venv sudo /opt/franka-mcp/venv/bin/pip install --upgrade pip sudo /opt/franka-mcp/venv/bin/pip install -r /home/sqz/projects/franka_interface/host/mcp-requirements.txt ``` Create a local certificate containing the robot host IP, configure a random token in `/etc/default/franka-mcp`, then install and start the service: ```bash sudo install -d -m 0750 -o root -g sqz /etc/franka-mcp sudo openssl req -x509 -newkey rsa:3072 -nodes -days 825 \ -keyout /etc/franka-mcp/server.key \ -out /etc/franka-mcp/server.crt \ -subj "/CN=172.16.0.1" \ -addext "subjectAltName=IP:172.16.0.1" sudo chown root:sqz /etc/franka-mcp/server.key /etc/franka-mcp/server.crt sudo chmod 0640 /etc/franka-mcp/server.key sudo chmod 0644 /etc/franka-mcp/server.crt sudo install -m 0644 systemd/franka-mcp.env.example /etc/default/franka-mcp # Edit FRANKA_MCP_TOKEN before starting the service. sudo install -m 0644 systemd/franka-mcp.service /etc/systemd/system/franka-mcp.service sudo systemctl daemon-reload sudo systemctl enable --now franka-mcp.service ``` The ROS workspace still needs to be rebuilt inside the container so the local gateway is installed: ```bash cd /root/catkin_ws source /opt/ros/noetic/setup.bash catkin_make source devel/setup.bash ``` The MCP server and the ROS stack have independent lifecycles. When the manual ROS stack is stopped, `get_status` remains available and reports `fci_connected: false`; state and motion calls report that the robot is unavailable. MCP readiness is derived directly from the local ROS gateway, not from a supervisor status file. On Mix, Codex can connect directly over the wired `172.16.0.0/24` network. The current Codex configuration format for a Streamable HTTP server is shown in `host/mix_codex_config.toml.example`; merge that table into Mix's `~/.codex/config.toml`, export the same `FRANKA_MCP_TOKEN`, and restart Codex. Because the service uses a private certificate, install `/etc/franka-mcp/server.crt` into Mix's trusted CA store before connecting.