interpolation-api.sgml

机器人开源项目orocos的源代码

%% algorithm:
%% Step 1: choose direction of first and third jerk pulses:
sign1 = 1;
sign2 = -1;

j0 = sign1 * jm;
j4 = sign2 * jm;

% t1 is reached from t0 by maximum acceleration from a0 till am:
deltat1 = (am-a0)/jm;
t1 = t0 + deltat1;

% time needed between t2 and t3 (= maximum deceleration from am till
% zero):
deltat3 = am/jm;

% the velocity increase in this deltat3 equals the velocity increase
% (from zero) during the time interval 0-deltat3. 
deltav3 = 0.5*jm*deltat3*deltat3;

% the velocity reaches its maximum vm at t3, hence:
v2 = vm - deltav3;

% filling in v(t=t2)=v2 gives:
t2 = (v2+a0*t0-v0+0.5*j0*deltat1*(t1+t0))/(j0*deltat1+a0);
t3 = t2 + deltat3;

% the time interval deltat between t4 and t7 equals:
deltat56 = (vm/jm-deltat3*deltat3)/deltat3;
deltat = 2*deltat3 + deltat56;

% in that time interval, the motion reaches vm, hence 
% v(t=t4) = vm. In that period, a distance p47 is covered:
% p47 = p(t=deltat,a0=0,v0=0,p0=0):
p47 = jm/6*(deltat**3-(deltat-deltat3)**3-deltat3**3);

% during the period until t3, the motion covers a distance
% p03 = p(t=t3,a0=a0,vo=vo,p0=s0):
p03 = j0/6*( (t3-t0)**3 - (t3-t1)**3 - (t3-t2)**3) + 0.5*a0*(t3-t0)**2 + v0*(t3-t0);

% the period of constant maximum velocity vm takes from t3 till t4,
% hence:
t4 = t3 + (p7-p0-p03-p47)/vm;

% TODO: the motion is "too short" if t4 < t3. The code to cope with
% this case is not here yet!

t5 = t4 + deltat3;
t6 = t5 + deltat56
t7 = t6 + deltat3;

t = [0:0.01:t7];
[nr,nc] = size(t);
jrk = zeros(nr,nc);
acc = zeros(nr,nc);
vel = zeros(nr,nc);
pos = zeros(nr,nc);

for i = 1:nc
  time = t(i);
  if (time < t0)
    error("time instant must be positive");
  elseif (time < t1)
    jrk(i) = j0;
    acc(i) = j0 * time + a0;
    vel(i) = j0 * time**2 / 2 + a0 * time + v0;
    pos(i) = j0 * time**3 / 6 + a0 * time**2/2 + v0 * time + p0;
  elseif (time < t2)
    jrk(i) = 0;
    tt1 = time -t1;
    acc(i) = j0 * (time - tt1) + a0;
    vel(i) = j0 * (time**2 - tt1**2)/ 2 + a0 * time + v0;
    pos(i) = j0 * (time**3 - tt1**3) / 6 + a0 * time**2/2 + v0 * time + p0;
  elseif (time < t3)
    tt1 = time -t1;
    tt2 = time -t2;
    jrk(i) = -j0;
    acc(i) = j0 * (time - tt1 - tt2) + a0;
    vel(i) = j0 * (time**2 - tt1**2 - tt2**2)/ 2 + a0 * time + v0;
    pos(i) = j0 * (time**3 - tt1**3 - tt2**3)/ 6 + a0 * time**2/2 + v0 * time + p0;
  elseif (time < t4)
    tt1 = time -t1;
    tt2 = time -t2;
    tt3 = time -t3;
    jrk(i) = 0;
    acc(i) = j0 * (time - tt1 - tt2 + tt3) + a0;
    vel(i) = j0 * (time**2 - tt1**2 - tt2**2 + tt3**2)/ 2 + a0 * time + v0;
    pos(i) = j0 * (time**3 - tt1**3 - tt2**3 + tt3**3)/ 6 + a0 * time**2/2 + v0 * time + p0;
    % index at time = t4:
    t4i = i;
elseif (time < t5)
    tt1 = time -t4;
    jrk(i) = j4;
    acc(i) = acc(t4i) + j4 * tt1;
    vel(i) = vel(t4i) + j4 * tt1**2 /2;
    pos(i) = pos(t4i) + j4 * tt1**3 /6 + vel(t4i)*tt1;
elseif (time < t6)
    tt1 = time -t4;
    tt2 = time -t5;
    jrk(i) = 0;
    acc(i) = acc(t4i) + j4 * (tt1 - tt2);
    vel(i) = vel(t4i) + j4 * (tt1**2 - tt2**2)/2;
    pos(i) = pos(t4i) + j4 * (tt1**3 - tt2**3)/6 + vel(t4i)*tt1;
elseif (time < t7)
    tt1 = time -t4;
    tt2 = time -t5;
    tt3 = time -t6;
    jrk(i) = -j4;
    acc(i) = acc(t4i) + j4 * (tt1 - tt2 - tt3);
    vel(i) = vel(t4i) + j4 * (tt1**2 - tt2**2 - tt3**2)/2;
    pos(i) = pos(t4i) + j4 * (tt1**3 - tt2**3 - tt3**3)/6 + vel(t4i)*tt1;
else
    jrk(i) = 0;
    acc(i) = 0;
    vel(i) = v7;
    pos(i) = p7;
  endif
endfor
]]>
</programlisting>
</para>
<para>
This example shows a <emphasis>nominal</emphasis> motion, i.e., one in
which the maximum velocity and acceleration are reached. However,
one or both of these constraints are not reached in
&ldquo;short&rdquo; motions.
This requires extensions to the code, bringing a significant increase
in complexity:
<itemizedlist>

<listitem>
<para>
It is not straightforward to know in advance whether one must
accelerate or decelerate in both parts of the motion (i.e., time
intervals t0-t3, and t4-t7). Therefore, the
formulas in <xref linkend="fig-jerk-integrations-pos"> have 
<emphasis role="strong">two</emphasis>
&sigma;1 and &sigma;2 variables (being either +1 or -1), depending on
whether the first and third jerk pulses are positive or negative,
respectively.
</para>
</listitem>

<listitem>
<para>
The durations of the nominal t0-t3 and t4-t7 parts of the motion are
calculated as follows (see
<xref linkend="fig-trap-acc-to-position"> and
<xref linkend="fig-jerk-integrations-pos">):
  <itemizedlist>

  <listitem>
  <para>
<emphasis>t1</emphasis>: this is the time instant at which the
acceleration reaches its maximum value. This can be found from filling
in t=t1 in <xref linkend="fig-jerk-integrations-pos">, which leads to
a <emphasis>linear</emphasis> equation in t1.
</para>
<para>
Note that there are
<emphasis role="strong">two alternatives</emphasis> for t1,
depending on the sign &sigma;<subscript>1</subscript>. 
and on the initial acceleration a<subscript>0</subscript>.
  </para>
  </listitem>

  <listitem>
  <para>
<emphasis>t2</emphasis>: knowing t1, the time instant t2 is found from
the velocity equation in <xref linkend="fig-jerk-integrations-pos">:
the velocity reaches its maximum at t=t2.
  </para>
  </listitem>

  <listitem>
  <para>
<emphasis>t3</emphasis>: 
  </para>
  </listitem>

  <listitem>
  <para>
<emphasis>t4</emphasis>: 
  </para>
  </listitem>

  <listitem>
  <para>
<emphasis>t5</emphasis>: 
  </para>
  </listitem>

  <listitem>
  <para>
<emphasis>t6</emphasis>: 
  </para>
  </listitem>

  <listitem>
  <para>
<emphasis>t7</emphasis>: 
  </para>
  </listitem>

  </itemizedlist>
</para>
</listitem>
                                                                                
<listitem>
<para>
So, one can find out whether the motion is &ldquo;long
enough&rdquo; to indeed reach the maximum velocity. If that is not
the case, however, shrinking the motion is not so straightforward:
  <itemizedlist>

  <listitem>
  <para>
<emphasis>Reduce the maximum velocity.</emphasis> 
The profile of <xref linkend="fig-trap-acc-to-position"> has
zero-acceleration phases t1-t2, t3-t4 and t5-t6, because it wants to
reach maximum velocity. If the resulting motion is too long, even when
these zero-acceleration phases are reduced to zero length,
the acceleration and deceleration phases should be made shorter; i.e.,
the time intervals t0-t1, t2-t3, t4-t5 and t6-t7.
In order to keep the time-optimality of the profile, the maximum jerk
is kept, such that the slope of the acceleration changes remains the
same; one just doesn't move until the maximum acceleration is reached.
  </para>
  <para>
TODO: explain <emphasis>how</emphasis> to scale the maximum velocity.
  </para>
  </listitem>

  <listitem>
  <para>
<emphasis>Reduce the maximum acceleration.</emphasis> 
  </para>
  <para>
TODO: explain <emphasis>how</emphasis> to scale the maximum
acceleration.
  </para>
  </listitem>

  <listitem>
  <para>
<emphasis>Reduce the maximum jerk.</emphasis>
  </para>
  <para>
TODO: explain <emphasis>how</emphasis> to scale the maximum jerk.
  </para>
  </listitem>

  </itemizedlist>
</para>
</listitem>

</itemizedlist>
</para>
</section>


<section id="spline-motion">
<title><parameter>SplineMotion</parameter></title>

<para>
The setpoints belong to a spline that interpolates the given
via-points. Of course, there exists a large variety of possible
interpolating splines, not only within one given family of splines,
but also because of the large number of possible spline families.
</para>

</section>


<section id="quaternion-line">
<title>
 <parameter>QuaternionLine</parameter>, 
 <parameter>&hellip;</parameter>
</title> 

<para>
The tool's origin moves along a straight Cartesian line between the
origins of the input via-points, and its orientation is determined by
the linear interpolation of the quaternion vectors corresponding to
the via-points' orientations.
</para>
<para>
This motion interpolation is one of the many with a
linear translational motion and some form of angular interpolation
with a fixed axis in space. This orientation interpolation can often
generate very counter-intuitive motions.
</para>

</section>


<section id="reeds-shepp">
<title>
 <parameter>ReedsShepp</parameter>,
 <parameter>&hellip;</parameter>
</title>

<para>
This is a 2D interpolator. It connects two points in a plane with a
sequence of straight lines and circular arcs.
</para>

</section>


</section>


<section id="motion-interpolation-api">
<title>API</title>

<para>
This Section describes the method calls for motion interpolators. 
The API assumes a <emphasis role="strong">stateful</emphasis>
interpolator object: each of the calls changes some but not
necessarily all parameters of a <emphasis>running</emphasis> interpolator.
So, the interpolator works with absolute time.
</para>
<para>
<variablelist>

 <varlistentry>
 <term>
<anchor id="motion-set-max-velocity">
<function>SetMaxVelocity</function>,
<function>SetMaxAcceleration</function>, and
<function>SetMaxJerk</function>:
 </term>
  <listitem>
   <para>
Set the constraints on the maximum values of the motion parameters.
   </para>
  </listitem>
 </varlistentry>

 <varlistentry>
 <term>
<anchor id="motion-set-min-velocity">
<function>SetMinVelocity</function>, 
<function>SetMinAcceleration</function> and
<function>SetMinJerk</function>:
 </term>
  <listitem>
   <para>
Set the constraints on the minimum, if the constraints are not
symmetric around zero.
   </para>
  </listitem>
 </varlistentry>

 <varlistentry>
 <term>
<anchor id="motion-set-goal-position">
<function>SetGoalPosition</function>,
<function>SetGoalVelocity</function>, and
<function>SetGoalAcceleration</function>:
 </term>
  <listitem>
   <para>
Give new inputs to the interpolator.
   </para>
  </listitem>
 </varlistentry>

 <varlistentry>
 <term>
<anchor id="motion-scale-velocity">
<function>ScaleVelocity</function>,
<function>ScaleAcceleration</function>.
<function>ResetVelocity</function>,
<function>ResetAcceleration</function>.
 </term>
  <listitem>
   <para>
The input argument of the <function>Scale&hellip;</function> methods
is a scalar smaller than 1, which reduces the velocity or acceleration
constraint. 
The <function>Reset&hellip;</function> methods reset the constraints
to their nominal values.
   </para>
   <para>
All these methods can be called at any instant during an ongoing
motion.
   </para>
  </listitem>
 </varlistentry>

 <varlistentry>
 <term>
<anchor id="motion-get-new-setpoint">
<function>GetNewSetpoint</function>:
 </term>
  <listitem>
   <para>
Get a new interpolation point. This method requires a time instant as
input argument. The output can be position, velocity and/or
acceleration, depending on the order of the interpolator.
   </para>
  </listitem>
 </varlistentry>

</variablelist>
It is also useful to have a <emphasis>simulation mode</emphasis> for
the interpolator, i.e., one method call would produce a vector of
setpoints, calculated at given time intervals, and covering the
complete motion. This simulation method call does not need a stateful
object.
</para>

</section>


</section> 

</article>