Articles & Tutorials

Franka-Emika control interface: libfranka

Introduction

The Franka-Emika Panda is a high-performance 7DOF robot arm.  It is produced by a Munich-based startup with a design heritage traceable to Germany’s DLR institute which also found its way into KUKA’s LWR robot series.  The Panda robot’s joints are sophisticated mechatronic systems capable of position and torque control.

The robot is connected to a control box which has two network ports.  One connects to the internet and allows the robot to be programmed using a graphical programming interface.  The other (10Gbps ethernet) can connect to a user’s computer and run real-time robot control code using UDP messages exchanged every millisecond — this is the Franka Control Interface (FCI).

libfranka

libfranka is a C++ library that provides user code access to the FCI, effectively allowing the users code to be run at the robot’s servo rate of 1000Hz.  This is a unique and potentially very powerful way to control a high-performance robot arm. However the potential for bad things to happen is high, so the robot’s controller implements stringent checks on what you command and safely “trips out” to protect the robot, terminating your code and applying the robot’s brakes. The definitive reference is online.

libfranka supports two ways to think about robot control: 

  1. A motion generator that describe a path in space.  Four types of motion generator are supported and provide the robot with trajectories expressed, at each time step, in terms of:
    • joint position
    • joint velocity
    • Cartesian position of end-effector
    • Cartesian position of end-effector plus elbow velocity (since the robot is redundant)
  2. A controller which sends torque commands to the joints.  The default controller causes the joints to follows the path given by the motion generator, but an external controller can be provided that can potentially ignore the motion generator.

There are four robot controllers:

  1. joint velocity
  2. internal joint impedance
  3. internal Cartesian impedance
  4. external, or user defined

For a C++ program running on the external computer the first step is to instantiate a robot for control

franka::Robot  robot("<fci-ip>");

where the object robot encapsulates the robot state and communication channel.
The motion generator and controller are defined by a callback function that receives the robot state and returns the specific type of the interface. 

Motion generators

To move the robot we attach a motion generator to the robot object by

robot.control(my_motion_generator_callback);

which will compute the next point on the trajectory for the robot’s controller to servo to. A motion generator has a signature like

franka::JointPositions 
my_motion_generator_callback( const franka::RobotState& robot_state, franka::Duration period)
{

and this function is called every millisecond.

The return type, in this case franka::JointPositions, dictates the type of motion generator, in this case it returns joint positions so is a joint position motion generator.

The callback is passed the state of the robot as well as the time of the last computational cycle which will be 1ms unless a packet error occurs — it can be considered as \(\Delta_t\).

This duration is an object that holds time internally as uint64 number of milliseconds.  The .toSec() method converts to a double.

The return type of the function specifies how the trajectory is represented, in this case it as a sequence of JointPosition objects, ie. a vector of joint coordinates. 

Other types of motion generators are created by declaring the function’s return type as one of: franka::JointVelocities, franka::CartesianPose or franka::CartesianVelocities.

The callback must allocate and initialize an output object, for example:

   // callback function
    .
    .
  franka::JointPositions output;  // create array of joint angles
  
  for (int i=0; i<7; i++)
    output[i] = ...;   // set its values
  return output;    // return it to libfranka
}

A motion generator callback continues to be run until it declares that it is done by

return franka::MotionFinished(output);

A common coding pattern is

  // main
  double time;  // global
   .
   .
  time = 0.0; 
  robot.control( my_motion_generator_callback );
   .
   .
 }
 
// callback function
franka::JointPositions 
my_motion_generator_callback( const franka::RobotState& robot_state, franka::Duration period)
{
  if (time == 0) {
    // initialize
  }
  // do stuff
  if (time > MAX)
    return franka::MotionFinished(output);
}

In C++ this can be expressed as a lambda function, as is commonly shown in the libfranka documentation.

double time = 0.0; // local 

robot.control( [=, &time] ( const franka::RobotState& robot_state, franka::Duration period) 
                                       -> franka::JointPositions {
  // function body here
  } );

where [=, &time] is the capture clause that introduces a lambda function and must always include at least the empty square brackets.  The syntax &time specifies that the local variable time is captured by reference and made accessible inside the lambda function, even though it is out of scope. The = means all other variables referred to in the lambda function are captured and made available  by value.

The callback can throw a franka::Exception if an error occurs at the control level and the .what() method indicates what went wrong.

Motion with impedance control

The examples so far have utilized a low-level velocity controller where the joints have high stiffness as is common in position-controlled robotics.  If we want compliant motion we can select one of two types of impedance controller.

We do this by providing an option when we register a call-back function.  For the case of joint impedance control this is

robot.control(my_motion_generator_callback, 
                     franka::ControllerMode::kJointImpedance);

and for Cartesian impedance control it is

robot.control(my_motion_generator_callback, franka::ControllerMode::kCartesianImpedance);

The impedance parameters must be set before the motion is instantiated (the lines above)

robot.setJointImpedance({{3000, 3000, 3000, 3000, 3000, 3000, 3000}});
robot.setCartesianImpedance({{2000, 2000, 2000, 100, 100, 100}});

and the notation {{}} initalizes the elements of the 7-element joint impedance array (with units of N/rad) or the 6-element Cartesian impedance array respectively.

 Cartesian impedance comprises translational impedance parallel to the end-effector X, Y and Z axes (with units of N/m) , and then rotational impedance about the end-effector X, Y and Z axes (Referred to in the Franka documentation as the R, P and Y axes.)  (with units of N/rad) .

Applying force and torque limits

We can tell the Panda controller to apply some limits to the motion with eight threshold parameters:

robot.setCollisionBehavior(
  torque_lower_accel, torque_upper_accel,
  torque_lower_nominal, torque_upper_tnominal,
  force_lower_accel, force_upper_accel,
  force_lower_nominal, force_upper_nominal);

those threshold starting with torque_ are 7-vectors in units of Nm, while those starting with force_ are 6-vectors in units of N along the X, Y, Z, R, P, Y axes.  

Those thresholds with suffix _accel are applied when the robot is accelerating or decelerating while those with suffix _nominal are applied during constant-velocity motion.

These threshold are all positive and apply to the magnitude of the torque and force values:

  • green zone Values less than the lower threshold are treated normally
  • orange zone Values above the lower lower, but less than the upper, are considered as contacts and this status is reflected in the RobotState. 
  • red zone Values above the upper threshold are registered as a collision and cause the robot to stop moving.

Alternatively the same thresholds can be applied to acceleration  and constant-velocity motion:

    robot.setCollisionBehavior(
        torque_lower, torque_upper,
        force_lower, force_upper);

Custom controller

A custom, or external, controller is added by

robot.control(my_external_motion_generator_callback, my_external_controller_callback);

where the callback function returns a franka::Torques object. 

The controller runs after the motion generator callback and is passed the robot state which contains the robot sensor readings and estimated values at a 1 kHz rate:

  • Joint level signals: motor and estimated joint angles and their derivatives, joint torque and derivatives, estimated external torque, joint collision/contacts.
  • Cartesian level signals: Cartesian pose, configured end effector and load parameters, external wrench acting on the end effector,
  • Cartesian collision Interface signals: the last commanded and desired values and their derivatives, as explained in the previous subsection.

As shown in the figure above, the output of the motion controller is processed by the robot’s kinematic functions to provide the desired state of the robot expressed in terms of joint positions, joint velocities, Cartesian pose and Cartesian velocity — these have the suffix _d  in the passed robot state object.

state.q is the actual joint angles, state.q_d is always the desired value from the motion generator, with inverse kinematics having been applied if required

A  joint-space impedance controller is simply

franka::Torques 
my_controller_callback( const franka::RobotState& robot_state, franka::Duration period)
{
  std::array<double, 7> tau_d_calculated;
  for (size_t i = 0; i < 7; i++) {
    tau_d[i] =  P[i] * (state.q_d[i] - state.q[i]) - D[i] * state.dq[i] + coriolis[i];
  return tau_d;

where the returned torque is a function of joint position error and velocity.

The commanded torque has gravity compensation added to it by libfranka.

It is possible to have a controller without a motion generator, for example

robot.control(my_external_controller_callback);

for example to set the joint torques to zero (hovering or zero-gravity effect)

robot.control([&](const franka::RobotState&, franka::Duration) -> franka::Torques {
      return {{0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}};
    });
\end{Code}

Not sure what stops this kind of action?

Robot kinematic and dynamic operations

Kinematic and dynamic functions of the robot are methods of the model library — a \texttt{franka::Model} object which is is created by a method call of the robot state

  franka::Model model = robot.loadModel();

Methods include

  • The forward kinematics of all robot joints.
  • The body and zero Jacobian matrices of all robot joints.
  • Dynamic parameters: inertia matrix, Coriolis and centrifugal vector and gravity vector.

For example, to compute velocity torques is:

  std::array<double, 7> coriolis = model.coriolis(state); // Coriolis torque for current state

The model library allows computation of kinematic and dynamic parameters for any arbitrary robot state, not just the current one. 

It can also be used in a non realtime fashion, e.g. in an optimization loop.

Applying a torque

For a task like handle turning we could use something as simple as constant torque on the last joint

robot.control([&](const franka::RobotState&, franka::Duration) -> franka::Torques {
      return {{0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 10.0}};
    });

but this makes no allowance for gravity acting on the other joints which might fall down.  Ideally this strategy should be combined with the the Cartesian impedance control.

Generalized motion

A useful general motion for manipulation work has the following features:

  • Cartesian velocity command expressed in the tool or world frame
  • with force guarding (collision force detection)
  • with compliance in some tool or world Cartesian DoF
  • with applied torques in some  tool or world Cartesian DoF

 


View all posts