<!DOCTYPE article PUBLIC "-//OASIS//DTD DocBook V4.1//EN"	"docbook/dtd/4.1/docbook.dtd" [<!ENTITY orocos "<acronym>Orocos</acronym>">]><article><articleinfo>  <title>   Interpolation API  </title>  <author>    <firstname>Herman</firstname>    <surname>Bruyninckx</surname>    <affiliation>      <address>        Herman.Bruyninckx(at)mech.kuleuven.ac.be      </address>    </affiliation>  </author> <copyright>  <year>2003&ndash;2004</year>  <holder>Herman Bruyninckx &mdash;Permission is granted to copy, distribute and/or modify this documentunder the terms of the GNU General Public License(<ulink url="http://www.fsf.org/copyleft/gpl.html">http://www.fsf.org/copyleft/gpl.html</ulink>), where the <emphasis>source code</emphasis> of the document is the <ulink url="interpolation-api.xml">XML file</ulink>.  </holder> </copyright> <abstract> <para> <emphasis role="strong">Abstract</emphasis> </para> <para>This document describes the <emphasis>algorithms</emphasis> and the<ulink url="deep-shallow-api.html">application programming interface</ulink>(&ldquo;API&rdquo;) for <emphasis role="strong">motion interpolation</emphasis>.A motion interpolator gets its input from the<ulink url="motion-api.html">motion generator</ulink>, in the form ofa <emphasis>start point</emphasis>&ldquo;<emphasis>A</emphasis>&rdquo; and a <emphasis>finalpoint</emphasis> &ldquo;<emphasis>B</emphasis>&rdquo; which are&ldquo;far apart&rdquo;, and itcalculates a sequence of intermediate &ldquo;closely separated&rdquo;setpoints with a granularity that matches the timing of the motioncontroller. </para><!-- link with ViaPoint from motion-api! --> </abstract> <revhistory>  <revision>   <revnumber>0.01</revnumber>   <date>April 14, 2004</date>   <authorinitials>HB</authorinitials>  </revision> </revhistory></articleinfo><section id="motion-interpolation-class"><title>Interpolation class</title><para>This Section describes some families of <link linkend="motion-interpolation">motion interpolators</link>, andtheir generic <link linkend="motion-interpolation-api">API</link>. As explained before, an interpolator takes as inputs a <ulink url="motion-api.html#VIA-POINT">via-point</ulink>, or a<ulink url="motion-api.html#PATH">path</ulink>, and generates as outputa list of <emphasis role="strong">setpoints</emphasis>, at a muchhigher granularity than the input via-points.</para><para>Motion interpolators are used in the following typical applications:<itemizedlist><listitem><para><emphasis role="strong">On-line setpoint generator.</emphasis>Most often, such an interpolator works at the samefrequency as the controller, or at an integer multiple of it.</para></listitem><listitem><para><emphasis role="strong">Off-line motion simulation.</emphasis>In this application mode, the <emphasis>time resolution</emphasis> ofthe generated interpolation points is often less important than the<emphasis>spatial resolution</emphasis>, especially if the simulatoris interested in finding out the exact geometry traced by the motion.</para></listitem></itemizedlist>The various types of interpolators described above require differentAPIs and implementations.</para><section id="motion-interpolation"><title>Motion interpolators</title><para>The variety of algorithms is caused by the variety of devices andmotion details:<itemizedlist><listitem><para>The via-points and/or setpoints contain only position information, oralso higher derivatives.</para></listitem><listitem><para>The setpoints are specified as 1D trajectories (e.g., for eachactuated joint separately), or as higher-dimensional trajectories(e.g., for a vector of actuated joints that have to move in asynchronized way; or for a Cartesian interpolation, where there ismost often a geometric coupling between the various degrees of freedom).</para></listitem><listitem><para>The setpoints have constant or non-constant spacing, in time and/orin position.</para></listitem><listitem><para>The setpoints are to be generated in a periodic way.</para></listitem></itemizedlist>The following sub-sections describe some of the most popular motioninterpolators.<section id="trapezoidal-velocity"><title> <parameter>TrapezoidalVelocity</parameter>, <parameter>SingleSCurve</parameter></title><para>This is a 1D &ldquo;point-to-point&rdquo; interpolator. It generates amotion from the current state (given position, zero velocity, zeroacceleration) to a desired position, which must be reached with zerovelocity and acceleration. Both velocity and acceleration have maximumconstraints, that should not be exceeded during the motion.</para><para>The motion profile has different phases, generated by accelerationpulses (<xref linkend="fig-trap-vel">):<orderedlist numeration="lowerroman"><listitem><para>Maximum acceleration pulse, i.e., linearly growing velocity, acceleratingat the specified maximum. This phase takes until the time instant t1in <xref linkend="fig-trap-vel">.</para></listitem><listitem><para>Constant velocity, i.e., zero acceleration. This phase runs betweentime instants t1 and t2 in <xref linkend="fig-trap-vel">.</para></listitem>                                                                                <listitem><para>Maximum deceleration pulse, between time instant t2 till the end ofthe motion at time instant t3.</para></listitem>                                                                                </orderedlist>The name of the interpolator comes from the facts that: (i) theresulting <emphasis>position</emphasis> profile looks like an&ldquo;S&rdquo;; (ii) the <emphasis>velocity</emphasis>profile consists of a trapezoid; and (iii) the acceleration profileconsists of square-wave pulses.  The motion time is determined by thespecified acceleration and velocity constraints.</para><para>The name <emphasis>SingleSCurve</emphasis> is less constraining thanthe name <emphasis>TrapezoidalVelocity</emphasis>, because the idea ofusing pulses to generate a motion is more general than the case towhich it is applied here; for example,<xref linkend="trapezoidal-acceleration-to-velocity"> applies jerkpulses to interpolate between two desired velocities.</para><para><figure id="fig-trap-vel" float="1" pgwide="0"><title>Trapezoidal velocity interpolation to reach a given position.(The position is plotted at one-tenth of the scale.)</title><mediaobject><imageobject> <imagedata align="center" fileref="../pictures/trapvel.png" format="PNG"></imageobject><imageobject> <imagedata align="center" fileref="../pictures/trapvel.eps" format="EPS"></imageobject></mediaobject></figure></para><para>The mathematics of this motion are as depicted in<xref linkend="fig-acceleration-integrations">. These followstraightforwardly from the integration of the acceleration pulses.Note that the initial velocity v<subscript>0</subscript> and accelerationa<subscript>0</subscript> are zero in the currently discussedinterpolator.</para><para><figure id="fig-acceleration-integrations" float="1" pgwide="0"><title>Formulas for the integration of acceleration pulses, with a magnitudeof a<subscript>m</subscript> and a sign &sigma;.</title><mediaobject><imageobject> <imagedata align="center"            fileref="../pictures/acceleration-integrations.png"            format="PNG"></imageobject><imageobject> <imagedata align="center"            fileref="../pictures/acceleration-integrations.eps"            format="EPS"></imageobject><textobject> <phrase><![CDATA[\thispagestyle{empty}\begin{displaymath}\begin{aligned}a(t) = \phantom{\frac{1}{2}}&a_m\ \sigma      \left\{  I_0(t-t_0) - I_0(t-t_1) - I_0(t-t_2) + I_0(t-t3)      \right\}\\\\v(t) = \phantom{\frac{1}{2}}&a_m\ \sigma      \left\{  I_0(t-t_0) - I_0(t-t_1) - I_0(t-t_2) + I_0(t-t3)      \right\}\\  &+ v_0\\\\p(t) = \frac{1}{2} &a_m \ \sigma      \left\{  I_0(t-t_0) - I_0(t-t_1) - I_0(t-t_2) + I_0(t-t3)      \right\}\\  &+ v_0 (t-t_0) + p_0\\\\p_0 = p(t&=t_0), \quadv_0 = v(t=t_0), \quada_0 = a(t=t_0)\\\\I_y(x) &=  \begin{cases}  x^y, \quad x > 0 \\  0, \quad x \le 0  \end{cases}\end{aligned}\end{displaymath}]]> </phrase></textobject></mediaobject></figure></para><para>Often, the distance between start and end position is too small, inorder for the motion to reach its maximum velocity. The interpolatorthen skips the zero acceleration phase (i.e., t2 = t1), and reducesthe acceleration time (i.e., t1 gets smaller, and the maximallyreached velocity gets smaller, see the right-hand sideof <xref linkend="fig-acceleration-integrations">).Alternatively, the maximum acceleration could be reduced; which meansthat the motion will take longer, but will require less energy, besafer, and introduce less vibrations and noise.</para><para><xref linkend="trapvel-code"> shows the <ulink url="http://www.octave.org">Octave</ulink> code with whichthe plots in <xref linkend="fig-trap-vel"> were generated. Only theinitial position conditions differ between both plots; for the&ldquo;short&rdquo; motion at the right-hand side of<xref linkend="fig-acceleration-integrations">, the maximumacceleration was maintained, but the acceleration/deceleration timesare reduced.The presented code is not a direct implementation of the equations in<xref linkend="fig-acceleration-integrations">, but somewhat optimizedfor execution speed. (Later Sections give more direct, flexibleimplementations.)</para><para><example id="trapvel-code"><title>Octave code for trapezoidal velocity interpolation.</title><programlisting><![CDATA[p0 = 20;   p3 = 30;vm = 3;    am = 1;t1 = vm / am;sigma = sign( p3 - p0 );deltax1 = sigma * am *  t1 * t1 / 2.0;deltaT = ( p3 - p0 - 2.0 * deltax1 ) / ( sigma * vm );if ( deltaT > 1.0 )    duration = 2 * t1 + deltaT;    t2 = duration - t1;else    t1 = sqrt( ( p3 - p0 ) / sigma / am );    duration = t1 * 2.0;    t2 = t1;endiftime=[0:0.01:duration];[nr, nc] = size(time);p = zeros(nr,nc); v = zeros(nr,nc); a = zeros(nr,nc);                                                                                a3 = sigma * am / 2.0;a2 = 0;a1 = p0;                                                                                b3 = 0;b2 = a2 + 2 * a3 * t1 - 2.0 * b3 * t1;b1 = a1 + t1 * ( a2 + a3 * t1 ) - t1 * ( b2 + t1 * b3 );                                                                                c3 = -sigma * am / 2.0;c2 = b2 + 2 * b3 * t2 - 2.0 * c3 * t2;c1 = b1 + t2 * ( b2 + b3 * t2 ) - t2 * ( c2 + t2 * c3 );                                                                                for t=1:nc  if ( time(t) < 0 )    p(t) = p0;    v(t) = 0;    a(t) = 0;  elseif ( time(t) < t1 )    p(t) = a1 + time(t) * ( a2 + a3 * time(t));    v(t) = a2 + 2 * a3 * time(t);    a(t) = 2 * a3;  elseif ( time(t) < t2 )    p(t) = b1 + time(t) * ( b2 + b3 * time(t));    v(t) = b2 + 2 * b3 * time(t);    a(t) = 2 * b3;  elseif ( time(t) <= duration )    p(t) = c1 + time(t) * ( c2 + c3 * time(t) );    v(t) = c2 + 2 * c3 * time(t);    a(t) = 2 * c3;  else    p(t) = p3;    v(t) = 0;    a(t) = 0;  endifendfor]]></programlisting></example></para><para>This <emphasis>SingleSCurve</emphasis> interpolator is quite popular,because it offers a good trade-off between: <itemizedlist><listitem><para><emphasis>Computational complexity</emphasis>: the algorithm can becalculated analytically, and in short constant time.</para></listitem><listitem><para><emphasis>Time-optimality</emphasis>: the motion uses the specifiedmaximum velocity and/or acceleration as long as possible.</para></listitem><listitem><para><emphasis>Flexibility</emphasis>: the &ldquo;pulse integration&rdquo;mathematics of <xref linkend="fig-acceleration-integrations">are the basis for many interpolators, as illustrated in the followingSections. </para></listitem><listitem><para><emphasis>Smoothness</emphasis>: the acceleration of the motion iscontinuous.</para></listitem></itemizedlist></para></section><section id="trapezoidal-acceleration-to-velocity"><title><parameter>TrapezoidalAccelerationToVelocity</parameter></title>