Real-time quintic Hermite interpolation for robot trajectory execution

The Panda Robot and the Franka Controller

At the lowest level the robot arm servos are controlled via a proprietary interface by the “Franka Controller” (FC). The “Franka Control Interface” (https://frankaemika.github.io/docs/libfranka.html) (FCI) is a software addon for the FC which enables real-time trajectory feeding, synchronized with reading the state of the robot; including joint torques and estimated external forces.

When the FCI is installed on the FC, the freely available C++ library “libfranka”(https://github.com/frankaemika/libfranka) can be used to operate the robot arm at a cycle time of 1 ms. The mechanism is to hand a callback function to libfranka when starting the control loop. This callback is then invoked every 1 ms with the status of the robot arm, expecting to be returned within less than one cycle. This cycle time is dubbed the micro cycle time.

In Figure 1A FC is modelled as a process controlling the Panda arm, running on the Franka Controller PC, i.e., it is born with its own node. The FCI is exposed over an Ethernet UDP socket, on an IP address configured in the FC. Thus the trajectory feeding is performed from a different node; e.g., a user-supplied PC. libfranka provides various methods of control, ranging from operation (Cartesian) space control of the tool flange to joint torque control. This paper does not go into detail with libfranka and the presented work only uses the joint position control mode.

Motion service and application

For the purpose of obeying the micro cycle real-time requirement the “Motion Service” (MS) has been developed and implemented. As illustrated in Fig. 1A, MS links with libfranka for setting up the callback communicating with the FC over FCI, and starting the control loop.

The MS is running in its own process, on its own node, separate from the FC. The “Motion Application” (MA) process may run on the same computational node as MS. For connection responsiveness, it is better to have the MS and the MA run on the same node. However, for dedicating more computing power to the MA, leaving more computational resources for sensor analysis and control algorithms, it could be running on a dedicated node. The presented architecture, where the MS is exposing its interface via an Ethernet UDP socket, is flexible in this regard.

Figure 1A illustrates how an optional force-torque sensor may be added. Such a force-torque sensor would typically be mechanically mounted between the wrist of the robot arm and the robot tool for measuring the wrench, W, on the tool. A force-torque sensor is not used in the presented setup and experiments. Instead the estimated wrench, $\hat{W}$ from the robot dynamics is used. This estimate is obtained as a feature from libfranka and provided by the MS to the MA.

Software mechanisms

For understanding the general workings of the MS, this section gives a brief overview of the central mechanistic design.

Figure 1B shows a sequence diagram illustrating the central, operative synchronization and responsibility of the three threads of the MS code. In addition the network-remote entities FC and MA are modelled as threads with remote message interaction to the MS threads.

In addition to this central, operative interaction the connection thread in the MS has a complex logic for maintaining control of the connection from an MA. This will not be treated here. Even though the connection management, which enables the FC-MS interaction to be kept alive over repeated MA-sessions, the main focus of this paper is to describe the interpolation in the MS.

Also not illustrated in Fig. 1B is the setup and initialization procedures of the MS, which establishes the real-time loop with the FC and prepares for receiving connections from an MA. Like the connection maintenance, this is also important, but the details are not in the focus of this paper.

A micro cycle is initiated by the callback from the FC to the Responder thread in the MS, conveying the robot arm status. The Responder thread is only responsible for conveying this trigger and status data to the Updater thread, and for returning the prepared next micro setpoint to the FC obtained from the Updater thread in response. Upon receiving the new micro state and returning the next micro setpoint, Updater thread is triggering itself to calculate the micro setpoint for the next micro cycle.

When the Updater thread is triggered to prepare the micro setpoint for the next micro cycle, it first updates its micro and macro cycle accounting, which determines whether a new macro cycle is to be started. The current macro cycle is valid for the next micro cycle if the current spline domain is valid for the time of the next micro cycle. In that case, the current spline is used to prepare the next micro setpoint.

If a new macro cycle is to be started, the Updater triggers the Connection thread, and the next macro setpoint is retrieved from it. If a valid new macro pose is retrieved, a new spline based on this is set up, and used for micro setpoint calculation. If not, due to late response or disconnect from the MA, a braking spline is generated. The severity of this braking spline depends on whether the missing macro setpoint was due to lateness or a broken connection. In the former case, only light braking will be generated, since it is expected that the MA will resume its responsiveness. In the latter case full deceleration braking is generated since the robot arm then must come to a full stop as fast as possible.

The Connection thread is, during operation in an MA-session, responsible for the interaction with the MA. It is triggered by the Updater thread when a new macro cycle is started, which it propagates on to the MA. After triggering the MA, it expects to receive a new macro setpoint some time later, before the end of the newly started macro cycle. When a macro setpoint is received from the MA, a flag is set to tell that a new macro setpoint is received. This flag is checked by the Updater thread, and is used to determine, if the MA was within its deadline. The Updater clears the flag, preparing it for receiving the next macro setpoint from the MA.

Timing-wise, when the macro cycle is between macro setpoint i and i + 1, a received macro setpoint is stored for use as the i + 3’th macro setpoint; i.e., one extra macro setpoint in the stream is retained. This delay is introduced to be able to correctly estimation the acceleration to target when splining toward i + 2, calculated using the central acceleration estimator over the macro setpoints i + 1, i + 2, and i + 3. The same principle is used for the velocity at macro setpoint i + 2, which results in the average velocity on the segments i + 1 → i + 2 and i + 2 → i + 3.

The calculus of motion quantities at the macro trajectory points is based on the stream of joint positions received from the MA, which are assumed to arrive at regular times separated by the macro cycle time, δt_mac. This stream of macro joint position vectors, {p_i}_i, is the fundamental input data from the MA to the MS. Over these, the central discrete estimators of velocities and accelerations may be concisely expressed as (1)${\displaystyle {\mathbf{p}}_{i+2}^{\left(1\right)}=0.5\left[\frac{{\mathbf{p}}_{i+2}-{\mathbf{p}}_{i+1}}{\delta t_{\text{mac}}}+\frac{{\mathbf{p}}_{i+3}-{\mathbf{p}}_{i+2}}{\delta t_{\text{mac}}}\right]}$ (2)${\displaystyle {\mathbf{p}}_{i+2}^{\left(2\right)}=\frac{{\mathbf{p}}_{i+3}-2{\mathbf{p}}_{i+2}+{\mathbf{p}}_{i+1}}{\delta t_{\text{mac}}^2}}$

These estimated first and second derivatives of the macro joint trajectory are used for calculating the splines, which are then sampled to generate the micro trajectory positions.

Hermite splines

Hermite spline and Hermite interpolation are named in honour of the 19th century French mathematician Charles Hermite. General treatment on multi-node Hermite splines of arbitrary order may be found in several publications and textbooks. E.g., Spitzbart (1960) focused on a general formulation for arbitrary order, while Krogh (1970) focused on efficient computation of interpolation and numerical differentiation with continuous derivative. However, course notes by Finn (2004) introduces an elegant formulation for the basis polynomials for cubic and quintic Hermite interpolation. Both have been implemented in the MS. The formalism can be concisely written, with the same enumeration of the basis functions, as (3)${\displaystyle {\tilde{p}}_n\left(u\right)=\sum _{i=0}^{\left(n-1\right)∕2}{p}_0^{\left(i\right)}{H}_i^n\left(u\right)+p_1^{\left(i\right)}{H}_{n-i}^n\left(u\right)\text{for}u\in \left[0;1\right]}$

In Eq. (3) we assume an underlying trajectory p(u) of which we know $p_{\tau }^{\left(i\right)}=p^{\left(i\right)}\left(\tau \right)$ for i ∈ {0..n − 1} and τ ∈ {0, 1}, where p⁽ⁱ⁾ is the i’th derivative of p. The basis functions for Hermite interpolation to order n are denoted ${\left\{H_i^n\right\}}_{i\in \left\{0..n\right\}}$. For cubic (n = 3) and quintic (n = 5) Hermite interpolation the basis functions can be found listed in Finn (2004). For completenes the coefficients are listed in matrix notation in Eqs. (4) and (5).

(4)${\displaystyle {\mathbf{H}}^3=\left[\begin{array}{cccc}\hfill 1\hfill & \hfill 0\hfill & \hfill -3\hfill & \hfill 2\hfill \\ \hfill 0\hfill & \hfill 1\hfill & \hfill -2\hfill & \hfill 1\hfill \\ \hfill 0\hfill & \hfill 0\hfill & \hfill -1\hfill & \hfill 1\hfill \\ \hfill 0\hfill & \hfill 0\hfill & \hfill 3\hfill & \hfill -2\hfill \end{array}\right]}$ (5)${\displaystyle {\mathbf{H}}^5=\left[\begin{array}{cccccc}\hfill 1\hfill & \hfill 0\hfill & \hfill 0\hfill & \hfill -10\hfill & \hfill 15\hfill & \hfill -6\hfill \\ \hfill 0\hfill & \hfill 1\hfill & \hfill 0\hfill & \hfill -6\hfill & \hfill 8\hfill & \hfill -3\hfill \\ \hfill 0\hfill & \hfill 0\hfill & \hfill \frac{1}{2}\hfill & \hfill -\frac{3}{2}\hfill & \hfill \frac{3}{2}\hfill & \hfill -\frac{1}{2}\hfill \\ \hfill 0\hfill & \hfill 0\hfill & \hfill 0\hfill & \hfill \frac{1}{2}\hfill & \hfill -1\hfill & \hfill \frac{1}{2}\hfill \\ \hfill 0\hfill & \hfill 0\hfill & \hfill 0\hfill & \hfill -4\hfill & \hfill 7\hfill & \hfill -3\hfill \\ \hfill 0\hfill & \hfill 0\hfill & \hfill 0\hfill & \hfill 10\hfill & \hfill -15\hfill & \hfill 6\hfill \\ \hfill \hfill \end{array}\right]}$

In an online or real-time system a change of variable is necessary for Eq. (3) to be applicable. When the system at run-time establishes a new interpolation segment it takes the form [t₀; t₁] with δt_mac = t₁ − t₀. Introducing the substitution on the interval $u\left(t\right)=\frac{t-t_0}{\delta t_{\text{mac}}}$, and letting $p_{\tau }^{\left(i\right)}=p^{\left(i\right)}\left(\tau \right)$ for i ∈ {0..n − 1} and τ ∈ {t₀, t₁}, the directly applicable version of Eq. (3) is (6)${\displaystyle {\tilde{p}}_n\left(t\right)=\sum _{i=0}^{\left(n-1\right)∕2}{\left(\delta t_{\text{mac}}\right)}^i\left(p_{t_0}^{\left(i\right)}{H}_i^n\left(u\left(t\right)\right)+p_{t_1}^{\left(i\right)}{H}_{n-i}^n\left(u\left(t\right)\right)\right)\text{for}t\in \left[t_0;t_1\right]}$

Equation (6) has been implemented in the MS code base for both cubic and quintic Hermite splines, and can be used based on joint velocities and accelerations from the macroscopic trajectory which is fed from the MA.

It is noteworthy that a piece-wise cubic Hermite spline is a C¹-smooth function and a piece-wise quintic Hermite spline is a C²-smooth function. With respect to generating motion trajectories, the great difference between these is the jerk; the third derivative of the position trajectory. Many motion controllers require being fed a limited-jerk trajectory, which is fulfilled by a C²-trajectory, whereas a C¹-trajectory exhibits infinite jerk.

Cosine joint motion

A simple cosine motion is generated and the motion of a single joint is observed. This will first and foremost give insight into the tracking delay in the system. The presented results defines the tracking delay from the MC perspective as the time passed from receiving a macro setpoint from the MA until a status packet from the FC shows that the joint position have been achieved.

Figure 2B show a selected time range for the position trajectory of the moving joint. The time range have been selected to easily inspect the delays from the MA over the “Franka MS” (FMS), and to the report back from the FC. The observed delay between the MA and the FMS is approximately 20 ms with the delay from the FMS to the reported FC position is another approximate 20 ms. In total, for the trajectory execution we can estimate a 40 ms delay.

It is evident that there is an inherent tracking delay of approximately 20 ms in the FC. This is affected by various filters and compliance parameters which are set at the initialization of the control loop through the FCI. Thus a lower inherent tracking delay may possible with more tight control parameters. Particularly the experiments presented here have been using a value of 10.0 Hz for the parameter cutoff_frequency in libfranka. This parameter controls, according to the documentation of libfranka, a first order low-pass filter. The reason for choosing a low cutoff frequency is observable as a “blackout” of two to three micro cycles near t = 3.185s in Fig. 2. Such blackouts occurs generally every second, but not regularly. The reason for these blackouts have not been clarified, but they probably arise from running the system on a full desktop PC; i.e., there have been no stripping of services or other running processes on the PC to increase the real-time responsiveness.

Execution of a simple cosine motion in one joint also allows the observation of the difference between using cubic and quintic Hermite splines. To this end, classes have been implemented in the FMS code base for both of these. Figure 3 shows accelerations obtained by discrete derivation of the MA and FMS trajectories for the observed joint. Figure 3A shows the acceleration in a trajectory in a session where the FMS runs with cubic Hermite interpolation and Fig. 3B shows the same type of cosine trajectory from a session using quintic Hermite interpolation.

The cubic Hermite interpolation acceleration curve in Fig. 3A is clearly discontinuous, which is consistent with its C¹-property. Correspondingly the quintic Hermite interpolation acceleration curve in Fig. 3B is clearly continuous, which is consistent with its C²-property.

Simple cutting

A simple application for a cutting experiment has been set up. This is an application where the robot tool is a knife and the task is to cut through a presented object. By simple cutting is meant moving the tool according to a pre-calculated trajectory; in contrast to sensor-based adaptive cutting where sensor input is used in the motion generation loop.

The application in this experiment does not exploit the ability to make real-time generation of, or corrections to, the commanded trajectory. The purpose of the experiment is to do a simple demonstration of the robustness of the MS implementation and to get an indication of the reliability and stability of the wrist wrench estimation obtained from libfranka.

The minimalistic setup used in the experiment is illustrated in Fig. 4. Figure 4A shows a photo of the knife held in the robot gripper and a test object; in this case a stick of EPS strapped to a bracket.

Figure 4B illustrates the geometric setup of the knife. The knife shaft is held clamped in the gripper, and thus defines the commanded knife directions for cutting ${\hat{c}}_c$, which is perpendicular to the edge, and shearing ${\hat{s}}_c$, which is parallel to the edge. The actual cutting and shearing directions are defined by the knife edge at the point of interaction. These are illustrated as ${\hat{c}}_a$ and ${\hat{s}}_a$, respectively. Interpretation of the data for the recorded forces in a cutting experiment is deeply dependent of the recognition of the relation between these commanded and actual directions.

A cutting experiment is executed by moving the knife in a cyclic series of strokes, forward strokes in the positive ${\hat{s}}_c$ direction and backward strokes in the negative ${\hat{s}}_c$ direction, while progressing steadily in the positive ${\hat{c}}_c$ direction. The motion of the knife happens thus in a plane spanned by the ${\hat{c}}_c$ and ${\hat{s}}_c$. The commanded position trajectory is shown in Fig. 5A. The origin of the positions is where the knife is held at the start of the cutting process.

The corresponding forces on the knife along the ${\hat{c}}_c$ and ${\hat{s}}_c$ directions, based on the estimated wrench obtained from libfranka in the FMS, is seen in Fig. 5B. In an ideal experiment one would expect a constant, negative cutting force in the ${\hat{c}}_c$ direction and a square wave shearing force, symmetric around zero, in the ${\hat{s}}_c$ direction. Both of these would be smoothly modulated in size by the initially increasing, and later decreasing, interaction of the knife with the object as it passes through it.

There are a couple of deviations from this ideal, qualitative behaviour. Most noteworthy is the non-smoothness of the direct cutting force. Next is the asymmetry around zero of the shear force. Both of these are easily understood by the illustration of the actual cutting and shearing directions observed in Fig. 4B.