%% 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 + deltat56t7 = 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;  endifendfor]]></programlisting></para><para>This example shows a <emphasis>nominal</emphasis> motion, i.e., one inwhich 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 increasein complexity:<itemizedlist><listitem><para>It is not straightforward to know in advance whether one mustaccelerate or decelerate in both parts of the motion (i.e., timeintervals t0-t3, and t4-t7). Therefore, theformulas 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 onwhether 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 arecalculated 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 theacceleration reaches its maximum value. This can be found from fillingin t=t1 in <xref linkend="fig-jerk-integrations-pos">, which leads toa <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 fromthe 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;longenough&rdquo; to indeed reach the maximum velocity. If that is notthe 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"> haszero-acceleration phases t1-t2, t3-t4 and t5-t6, because it wants toreach maximum velocity. If the resulting motion is too long, even whenthese 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 jerkis kept, such that the slope of the acceleration changes remains thesame; 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 maximumacceleration.  </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 givenvia-points. Of course, there exists a large variety of possibleinterpolating 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 theorigins of the input via-points, and its orientation is determined bythe linear interpolation of the quaternion vectors corresponding tothe via-points' orientations.</para><para>This motion interpolation is one of the many with alinear translational motion and some form of angular interpolationwith a fixed axis in space. This orientation interpolation can oftengenerate 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 asequence 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 notnecessarily 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 notsymmetric 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> methodsis a scalar smaller than 1, which reduces the velocity or accelerationconstraint. The <function>Reset&hellip;</function> methods reset the constraintsto their nominal values.   </para>   <para>All these methods can be called at any instant during an ongoingmotion.   </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 asinput argument. The output can be position, velocity and/oracceleration, depending on the order of the interpolator.   </para>  </listitem> </varlistentry></variablelist>It is also useful to have a <emphasis>simulation mode</emphasis> forthe interpolator, i.e., one method call would produce a vector ofsetpoints, calculated at given time intervals, and covering thecomplete motion. This simulation method call does not need a statefulobject.</para></section></section> </article>