Panda Controllers

This repository implements a variety of feedback controllers for the Franka-Emika Panda using franka_ros.

Inverse Dynamics Controller

An Inverse dynamics controller computes desired joint torques based on a desired acceleration and the discrepancy between actual and desired positions and velocities.

The controller is broken down into the two following stages, firstly a feedforward acceleration term is computed:

$$\begin{array}{r}{\ddot{\mathbf{\text{q}}}}_{ff}={\ddot{\mathbf{\text{q}}}}_{desired}+{\mathbf{\text{K}}}_p({\mathbf{\text{q}}}_{desired}-{\mathbf{\text{q}}}_{actual})+{\mathbf{\text{K}}}_d({\dot{\mathbf{\text{q}}}}_{desired}-{\dot{\mathbf{\text{q}}}}_{actual})\end{array}$$

where the feedforward acceleration and desired positions and velocities are the commands sent to the controller. The actual positions and velocities are obtained from the real robot state. The desired acceleration is used with the robot inverse dynamics formula to compute torques:

$$\begin{array}{r}\tau =M({\mathbf{\text{q}}}_{actual}){\ddot{\mathbf{\text{q}}}}_{ff}+C({\mathbf{\text{q}}}_{actual},{\dot{\mathbf{\text{q}}}}_{actual}){\dot{\mathbf{\text{q}}}}_{actual}+G({\mathbf{\text{q}}}_{actual})\end{array}$$

Usage

To use this controller, simply clone this repository in franka_ros and then make sure that the controller is loaded. For example, you can add the following text to the default_controllers.yaml file in the franka_ros/franka_control/config directory:

panda_inverse_dynamics_controller:
  type: "panda_controllers/InverseDynamicsController"
  joint_names:
        - panda_joint1
        - panda_joint2
        - panda_joint3
        - panda_joint4
        - panda_joint5
        - panda_joint6
        - panda_joint7
  p_gains:
        - 300.0
        - 300.0
        - 300.0
        - 300.0
        - 150.0
        - 100.0
        - 150.0
  d_gains:
        - 40.0
        - 40.0
        - 40.0
        - 40.0
        - 20.0
        - 20.0
        - 10.0

The gain values are specified in the YAML file. These are the values I have found to work well so far.

PD+Feedforward Controller

A PD+Feedforward controller computes torques to send to the robot in a single step by considering a feedforward torque term and the deviations between desired and actual positions and velocities.

$$\begin{array}{r}\tau ={\tau }_{desired}+{\mathbf{\text{K}}}_p({\mathbf{\text{q}}}_{desired}-{\mathbf{\text{q}}}_{actual})+{\mathbf{\text{K}}}_d({\dot{\mathbf{\text{q}}}}_{desired}-{\dot{\mathbf{\text{q}}}}_{actual})\end{array}$$

Usage

To use this controller, simply clone this repository in franka_ros and then make sure that the controller is loaded. For example, you can add the following text to the default_controllers.yaml file in the franka_ros/franka_control/config directory:

panda_torque_PD_controller:
  type: "panda_controllers/TorquePDController"
  joint_names:
        - panda_joint1
        - panda_joint2
        - panda_joint3
        - panda_joint4
        - panda_joint5
        - panda_joint6
        - panda_joint7
  p_gains:
        - 50.0
        - 50.0
        - 50.0
        - 50.0
        - 50.0
        - 30.0
        - 30.0
  d_gains:
        - 75.0
        - 75.0
        - 75.0
        - 75.0
        - 20.0
        - 12.0
        - 12.0
  alpha: 0.8
  torque_rate_limiter: false
  low_pass_filter: true

QP Tracking Controller

To be implemented still

To Do

• Implement safety features for commanded velocity and make sure commanded positions are within joint limits

Author

David Russell (scsdr@leeds.ac.uk)