Introduction

In robot kinematics, a joint path is a sequence of positions for one or more joints. A joint trajectory is the time function interpolating these positions.

This post examines generating joint trajectories with cubic splines.

Say we have a robotic arm with one revolute joint, and we want to rotate its joint position QQ from 00 to 9090 degrees.

Qinit=0Qfinal=π/2 \begin{aligned} Q_{init} &= 0 \\ Q_{final} &= \pi/2 \\ \end{aligned}


Figure: The joint start and goal

We don’t care how long it takes, but the joint must start from rest and and end at rest.

Vinit=0Vfinal=0 \begin{aligned} V_{init} &= 0 \\ V_{final} &= 0 \\ \end{aligned}


Initial Solution

We can satisfy these constraints by interpolating the joint position, velocity, and acceleration with a cubic spline,

Q(t)=At3+Bt2+Ct+D// PositionV(t)=3At2+2Bt+C// VelocityA(t)=6At+2B// Acceleration \begin{aligned} Q(t) &= At^3 + Bt^2 + Ct + D &&\text{// Position} \\ V(t) &= 3At^2 + 2Bt + C &&\text{// Velocity} \\ A(t) &= 6At + 2B &&\text{// Acceleration} \\ \end{aligned}

where tt is the time since the movement started.

We start by finding the coefficients AA, BB, CC, and DD.

Duration=(To be Determined)Displacement=QfinalQinitA=(2Displacement/Duration+Vinit+Vfinal)Duration2B=(3Displacement/Duration2VinitVfinal)DurationC=VinitD=Qinit \begin{aligned} Duration &= \textit{(To be Determined)} \\ Displacement &= Q_{final} - Q_{init} \\ A &= \frac{(2 \cdot -Displacement / Duration + V_{init} + V_{final})}{Duration^2} \\ B &= \frac{(3 \cdot Displacement / Duration - 2 \cdot V_{init} - V_{final})}{Duration} \\ C &= V_{init} \\ D &= Q_{init} \\ \end{aligned}
Since we don’t care how long the movement takes, let’s choose arbitrarily that the movement should last 11 second:

Duration=1 Duration = 1

then we have the coefficients

A=(2(π/2)/1+0+0)/12=πB=(3(π/2)/1200)/1=3π/2C=0D=0 \begin{aligned} A &= (2 \cdot -(\pi/2)/1 + 0 + 0)/1^2 = -\pi \\ B &= (3 \cdot (\pi/2)/1 - 2 \cdot 0 -0)/1 = 3\pi/2 \\ C &= 0 \\ D &= 0 \\ \end{aligned}

Plugging these values back into the cubic equations, we can see in the figure that the joint at t=1st = 1s has position Q=π/2radQ = \pi/2\>rad and velocity v=0rad/sv = 0\>rad/s.

Figure: Joint Position, Velocity, and Acceleration over Time


Joint Constraints

In reality, a joint may not physically be able to move in 1 second, so let’s consider some realistic constraints.

Say the joint has a maximum angular velocity of 6°/s6°/s and a maximum angular acceleration of 3°/s23°/s^2. Assume this holds true regardless of its payload or position, i.e. ignore dynamics.

Vlimit=0.104719755rad/sAlimit=0.0523599rad/s2 \begin{aligned} V_{limit} &= 0.104719755 \> rad/s \\ A_{limit} &= 0.0523599 \> rad/s^2 \end{aligned}

Clearly the solution plotted above exceeds these limits:

Vmax=3π/4,t=0.5sAmax=3π,t=0s=3π,t=1s \begin{aligned} V_{max} &= 3\pi/4, t = 0.5s \\ A_{max} &= 3\pi, t = 0s \\ &= -3\pi, t = 1s \\ \end{aligned}

We can reduce the velocity and acceleration by scaling the duration, i.e. making the movement take longer. The time-optimal solution is found analytically according to Melchiorri [1]:

double get_scale()  
{
  double v_scale = abs(Vmax) / Vlimit
  double a_scale = sqrt(abs(Amax) / Alimit)
  return max(v_scale, a_scale)
}

If a_scale is larger than v_scale, then the acceleration limit is constraining the duration. If v_scale is larger than a_scale, then the velocity limit is constraining the duration.

v_scale = abs(3π/4) / 0.104719755  = 22.5
a_scale = sqrt(abs(3π)/0.0523599)  = 13.417

In this case, the velocity limit is the dominating constraint. The time-optimal duration is 22.5 seconds.

We can verify by recalculating the polynomial coefficients with the new duration.

A=(2(π/2)/22.5)/22.52=0.00027580511B=(3(π/2)/22.5)/22.5=0.00930842267C=0D=0 \begin{aligned} A &= (2 \cdot -(\pi/2)/22.5)/22.5^2 = -0.00027580511 \\ B &= (3 \cdot (\pi/2)/22.5)/22.5 = 0.00930842267 \\ C &= 0 \\ D &= 0 \\ \end{aligned}

Figure: Scaled Joint Position, Velocity, and Acceleration over Time

We can see that the joint at t=22.5st = 22.5s has position Q=π/2radQ = \pi/2 \>rad and velocity v=0rad/sv = 0 \>rad/s. The maximum velocity is at t=11.25st = 11.25s with v=0.104719755rad/sv = 0.104719755 \>rad/s. The maximum acceleration is at t=0t = 0 and t=22.5st = 22.5s with a=0.0186rad/s2a = 0.0186 \>rad/s^2 and a=0.0186rad/s2a = -0.0186 \>rad/s^2, respectively. The joint velocity and acceleration constraints are satisfied. \blacksquare


Task Space Constraints

Let’s add another constraint. Let’s say the frame attached to the tip of the joint has maximum translational speed and angular velocity components.


Figure: Diagram of a frame at the joint tip. The frame is right-handed, i.e. Z points out of the page.

X˙max=100mm/sY˙max=100mm/sZ˙max=100mm/sRx˙max=9°/sRy˙max=9°/sRz˙max=9°/s \begin{aligned} \dot{X}_{max} &= 100mm/s \\ \dot{Y}_{max} &= 100mm/s \\ \dot{Z}_{max} &= 100mm/s \\ \dot{R_x}_{max} &= 9°/s \\ \dot{R_y}_{max} &= 9°/s \\ \dot{R_z}_{max} &= 9°/s \\ \end{aligned}

Aside: I say components because e.g. a velocity vector moving with X˙=Y˙=Z˙=100mm/s\dot{X} = \dot{Y} = \dot{Z} = 100mm/s would actually be moving at (100²+100²+100²) =173mm/s\sqrt{(100²+100²+100²)} ~= 173mm/s. One could certainly solve for a velocity vector constraint, too.

Similar to joint space constraints, we can meet task space constraints by scaling the duration of the trajectory, but we need to know the relation from joint space to task space.

The relation from joint space to task space is known as forward kinematics. Conversion from joint position to task space position is forward position, and conversion from joint velocity to task space velocity is forward velocity. This topic is widely covered elsewhere.

Let’s say our robot joint has position QQ, angular velocity Q˙\dot{Q} (also known as V(t)V(t)), and radius rr from its center of rotation to the tip frame. Then the following relations apply:

X=rcos(Q)Y=rsin(Q)Z=0Rx=0Ry=0Rz=QX˙=Q˙rsin(Q)Y˙=Q˙rcos(Q)Z˙=0Rx˙=0Ry˙=0Rz˙=Q˙ \begin{aligned} X &= r \cdot cos(Q) \\ Y &= r \cdot sin(Q) \\ Z &= 0 \\ R_x &= 0 \\ R_y &= 0 \\ R_z &= Q \\ \\ \dot{X} &= -\dot{Q} \cdot r \cdot sin(Q) \\ \dot{Y} &= \dot{Q} \cdot r \cdot cos(Q) \\ \dot{Z} &= 0 \\ \dot{Rx} &= 0 \\ \dot{Ry} &= 0 \\ \dot{Rz} &= \dot{Q} \\ \end{aligned}

For example, if r=1meterr = 1 \>meter, Q=0radQ = 0 \>rad, and Qdot=πrad/sQdot = \pi \> rad/s, then

X=1mY=0mRz=0mX˙=0m/sY˙=1m/sRz˙=πrad/s \begin{aligned} X &= 1m \\ Y &= 0m \\ R_z &= 0m \\ \dot{X} &= 0 \>m/s \\ \dot{Y} &= 1 \>m/s \\ \dot{R_z} &= \pi \> rad/s \\ \end{aligned}

For another example, if r=1meterr = 1 \>meter, Q=π/2radQ = \pi/2 \>rad, and Qdot=πrad/sQdot = \pi \> rad/s, then

X=0mY=1mRz=0mX˙=1m/sY˙=0m/sRz˙=πrad/s \begin{aligned} X &= 0m \\ Y &= 1m \\ R_z &= 0m \\ \dot{X} &= -1 \>m/s \\ \dot{Y} &= 0 \>m/s \\ \dot{R_z} &= \pi \> rad/s \\ \end{aligned}

For details, see this video lecture.

Going back to our 1-second trajectory, since the joint velocity is a parabola, which is symmetric, the maximum occurs at any of t=0t = 0, t=0.5t = 0.5, or t=1t = 1. Since Vinit=Vfinal=0V_{init} = V_{final} = 0, the maximum occurs at t=0.5t = 0.5. This results in the following task space velocities:

Q(0.5)=πt³+3π2t²=π/4radV(0.5)=3π(1/2)²+3π/2=3π4rad/sX˙max=3π4rad/s1mcos(π/4)=1.67m/s=1670mm/sY˙max=3π4rad/s1msin(π/4)=1.67m/s=1670mm/sZ˙max=0m/sRx˙=0rad/sRy˙=0rad/sRz˙=3π4rad/s \begin{aligned} Q(0.5) &= -\pi t³ + \frac{3\pi}{2}t² = \pi/4 \>rad \\ V(0.5) &= -3\pi(1/2)² +3\pi/2 = \frac{3\pi}{4} rad/s \\ \dot{X}_{max} &= \frac{3\pi}{4} rad/s \cdot 1m \cdot cos(\pi/4) = 1.67m/s = 1670 \>mm/s \\ \dot{Y}_{max} &= \frac{3\pi}{4} rad/s \cdot 1m \cdot sin(\pi/4) = 1.67m/s = 1670 \>mm/s \\ \dot{Z}_{max} &= 0 \>m/s \\ \dot{R_x} &= 0 \>rad/s \\ \dot{R_y} &= 0 \>rad/s \\ \dot{R_z} &= \frac{3\pi}{4} \>rad/s \\ \end{aligned}

Dividing by the given task space constraints yields the following ratios:

Xratio=1670/100=16.7Yratio=1670/100=16.7Zratio=0Rxratio=0Ryratio=0Rzratio=3π4rad/s9°/s=15 \begin{aligned} X_{ratio} &= 1670/100 = 16.7 \\ Y_{ratio} &= 1670/100 = 16.7 \\ Z_{ratio} &= 0 \\ R_{x_{ratio}} &= 0 \\ R_{y_{ratio}} &= 0 \\ R_{z_{ratio}} &= \frac{\frac{3\pi}{4} \>rad/s}{9°/s} = 15 \\ \end{aligned}

The maximum task space ratio is 16.716.7, which is less than the previous value of vscale=22.5v_{scale} = 22.5. The previous scaled trajectory duration of 22.5s also satisfies the given task space constraints. \blacksquare


Multiple Joints

The same approach applies to robots with more than one joint.

In this case, if we have mm joints, then we will have mm cubic splines, and ascalea_{scale} and vscalev_{scale} must be calculated for each joint.

The scale resulting from dividing the forward velocity by the task space limit is also calculated. The maximum of the joint space ratios and the task space ratio yields the optimal trajectory duration.

Example

Consider adding a second joint to the previous example to create a two-joint manipulator. This joint has the same velocity and acceleration limits.

Vlimit=0.104719755rad/sAlimit=0.0523599rad/s2 \begin{aligned} V_{limit} &= 0.104719755 \> rad/s \\ A_{limit} &= 0.0523599 \> rad/s^2 \end{aligned}


Figure: A robot with two joints.

Simulate this robot on Desmos. Use the q1q_1 and q2q_2 sliders.

The first spline is unchanged.

Qinit1=0A1=πQfinal1=π/2B1=3π/2Vinit1=0C1=0Vfinal1=0D1=0 \begin{array}{c} \begin{aligned} Q_{init_1} &= 0 & A_1 &= -\pi \\ Q_{final_1} &= \pi/2 & B_1 &= 3\pi/2 \\ V_{init_1} &= 0 & C_1 &= 0 \\ V_{final_1} &= 0 & D_1 &= 0 \\ \end{aligned} \end{array}

Here is the second spline.

Qinit2=π/2A2=2πQfinal2=π/2B2=3πVinit2=0C2=0Vfinal2=0D2=π/2 \begin{array}{c} \begin{aligned} Q_{init_2} &= \pi/2 & A_2 &=2\pi \\ Q_{final_2} &= -\pi/2 & B_2 &= -3\pi \\ V_{init_2} &= 0 & C_2 &= 0 \\ V_{final_2} &= 0 & D_2 &= \pi/2 \\ \end{aligned} \end{array}

Here is the relation of joint space to task space.

X=r1cos(Q1)+r2cos(Q1+Q2)Y=r1sin(Q1)+r2sin(Q1+Q2)Z=0Rx=0Ry=0Rz=Q1+Q2X˙=Q1˙r1sin(Q1)Q1˙Q2˙r2sin(Q1+Q2)Y˙=Q1˙r1cos(Q1)+Q1˙Q2˙r2cos(Q1+Q2)Z˙=0Rx˙=0Ry˙=0Rz˙=Q1˙+Q2˙ \begin{aligned} X &= r_1 \cdot cos(Q_1) + r_2 \cdot cos(Q_1 + Q_2) \\ Y &= r_1 \cdot sin(Q_1) + r_2 \cdot sin(Q_1 + Q_2) \\ Z &= 0 \\ R_x &= 0 \\ R_y &= 0 \\ R_z &= Q_1 + Q_2 \\ \\ \dot{X} &= -\dot{Q_1} \cdot r_1 \cdot sin(Q_1) - \dot{Q_1} \cdot \dot{Q_2} \cdot r_2 \cdot sin(Q_1 + Q_2) \\ \dot{Y} &= \dot{Q_1} \cdot r_1 \cdot cos(Q_1) + \dot{Q_1} \cdot \dot{Q_2} \cdot r_2 \cdot cos(Q_1 + Q_2)\\ \dot{Z} &= 0 \\ \dot{Rx} &= 0 \\ \dot{Ry} &= 0 \\ \dot{Rz} &= \dot{Q_1} + \dot{Q_2}\\ \end{aligned}

For details, see the derivation of position here and the derivation of velocity here.

We now find the maximum velocity, acceleration, and resulting time scale for each joint.

Note: The topic of finding polynomial minima or maxima is well-covered elsewhere and can be deferred to a good algebra library. Here, we just use Desmos.

Figure: Joint-space velocities, two-joint manipulator.

First Joint

V1max=3π/4,t=0.5sA1max=3π,t=0s=3π,t=1sV1scale=3π/40.10471975=22.5A1scale=3π0.0523599=13.417 \begin{aligned} V_{1_{max}} &= 3\pi/4, t = 0.5s \\ A_{1_{max}} &= 3\pi, t = 0s \\ &= -3\pi, t = 1s \\ V_{1_{scale}} &= \frac{|3π/4|}{0.10471975} = 22.5 \\ A_{1_{scale}} &= \sqrt{\frac{|3π|}{0.0523599}} = 13.417 \\ \end{aligned}

Second Joint

V2max=3π/2,t=0.5sA2max=6π,t=0s=6π,t=1sV2scale=3π/20.10471975=45A2scale=6π0.0523599=18.974 \begin{aligned} V_{2_{max}} &= 3\pi/2, t = 0.5s \\ A_{2_{max}} &= 6\pi, t = 0s \\ &= -6\pi, t = 1s \\ V_{2_{scale}} &= \frac{|3π/2|}{0.10471975} = 45 \\ A_{2_{scale}} &= \sqrt{\frac{|6π|}{0.0523599}} = 18.974 \\ \end{aligned}

Without considering task space velocity, the time-optimal duration is 1smax(22.5,13.417,45,18.974)=45s1s \cdot max(22.5, 13.417, 45, 18.974)=45s. Clearly, since the second joint has twice as far to rotate as the first joint, it constrains the duration of the movement.

Let’s now consider task space velocity.

Figure: Task-space velocities, two-joint manipulator.

X˙max=3.245m/s,t=0.3624sY˙max=3.245m/s,t=0.6376sRz˙max=3π/4rad/s,t=0.5sXratio=3245/100=32.45Yratio=3245/100=32.45Rzratio=3π4rad/s9°/s=15 \begin{aligned} \dot{X}_{max} &= |3.245|m/s, t=0.3624s \\ \dot{Y}_{max} &= |-3.245|m/s, t=0.6376s \\ \dot{R_z}_{max} &= -3\pi/4 rad/s, t=0.5s \\ X_{ratio} &= 3245/100 = 32.45 \\ Y_{ratio} &= 3245/100 = 32.45 \\ R_{z_{ratio}} &= \frac{\frac{3\pi}{4} \>rad/s}{9°/s} = 15 \\ \end{aligned}

Since 32.45<4532.45 < 45, the joint space constraints dominate the task space constraints. The time-optimal duration is 1smax(45,32.45,15)=45s1s \cdot max(45, 32.45, 15)=45s. \blacksquare


Longer Paths

If a path Qj\pmb{Q}_j of length nn>2n | n>2 is given for joint jj, i.e. intermediate points between QinitQ_{init} and QfinalQ_{final} are given, and if the joint begins and ends with zero velocity (Vinit=Vfinal=0V_{init} = V_{final} = 0), then by enforcing the constraints of continuity on velocity and acceleration, the intermediate point velocities can be calculated with a system of linear equations following the method described by Melchiorri [1].

Example

Consider an extension of the previous 2-joint trajectory, where each joint passes through three positions: an initial position, an intermediate position, and a final position.

Q1=[Q1initQ1intermediateQ1final]Q2=[Q2initQ2intermediateQ2final] \begin{aligned} \pmb{Q}_1 &= [Q_{1_{init}} Q_{1_{intermediate}} Q_{1_{final}}] \\ \pmb{Q}_2 &= [Q_{2_{init}} Q_{2_{intermediate}} Q_{2_{final}}] \\ \end{aligned}

Let the given intermediate positions be

Q1intermediate=π/4Q2intermediate=0 \begin{aligned} Q_{1_{intermediate}} &= \pi/4 \\ Q_{2_{intermediate}} &= 0 \end{aligned}

…along with the initial and final velocities.

V1init=V1final=V2init=V2final=0V_{1_{init}} = V_{1_{final}} = V_{2_{init}} = V_{2_{final}}= 0

We must find the intermediate velocities V1intermediateV_{1_{intermediate}} and V2intermediateV_{2_{intermediate}}. We can do this by solving the system

Av=c A\pmb{v} = \pmb{c}

where

A=[2(T1+T2)T1T32(T2+T3)T2Tn22(Tn3+Tn2)Tn3Tn12(Tn2+Tn1)] A= \begin{bmatrix} 2(T_1+T_2) & T_1 \\ T_3 &2(T_2+T_3) &T_2 \\ & & \ddots \ddots \ddots \\ & & & T_{n-2} & 2(T_{n-3}+T_{n-2}) & T_{n-3} \\ & & & & T_{n-1} & 2(T_{n-2} + T_{n-1}) \end{bmatrix}

v=[V2V3Vn2Vn1] \pmb{v} = \begin{bmatrix} V_2 \\ V_3 \\ \vdots \\ V_{n-2} \\ V_{n-1} \\ \end{bmatrix}

c=[3T1T2[T12(Q3Q2)+T22(Q2Q1)]T2V13T2T3[T22(Q4Q3)+T32(Q3Q2)]3Tn3Tn2[Tn32(Qn1Qn2)+Tn22(Qn2Qn3)]3Tn2Tn1[Tn22(QnQn1)+Tn12(Qn1Qn2)]Tn2Vn] \pmb{c} = \begin{bmatrix} \frac{3}{T_1T_2}[T_1^2(Q_3-Q_2)+T_2^2(Q_2-Q_1)] \pmb{- T_2V_1} \\ \frac{3}{T_2T_3}[T_2^2(Q_4-Q_3)+T_3^2(Q_3-Q_2)] \\ \vdots \\ \frac{3}{T_{n-3}T_{n-2}}[T_{n-3}^2(Q_{n-1}-Q_{n-2})+T_{n-2}^2(Q_{n-2}-Q_{n-3})] \\ \frac{3}{T_{n-2}T_{n-1}}[T_{n-2}^2(Q_{n}-Q_{n-1})+T_{n-1}^2(Q_{n-1}-Q_{n-2})] \pmb{- T_{n-2}V_n}\\ \end{bmatrix}

and

Ti=Durationi// The duration of segment iQi=Qiinit// The initial position of segment i Vi=Viinit// The initial velocity of segment iVn=Vnfinal // The final velocity of the last segment \begin{aligned} T_i &= Duration_i & \text{// The duration of segment $i$} \\ Q_i &= Q_{i_{init}} & \text{// The initial position of segment $i$ } \\ V_i &= V_{i_{init}} & \text{// The initial velocity of segment $i$} \\ V_n &= V_{n_{final}} & \text{ // The final velocity of the last segment} \\ \end{aligned}

Reminder: There are n1n-1 splines interpolating nn control points (also called knots), and our indexing starts at 11, not 00. Therefore, i==1i==1 refers to the first spline, and i==n1i == n-1 refers to the last spline.

Aside: Here is an example implementation of the above algorithm.

Joint 1

We solve for V1intermediateV_{1_{intermediate}}.

[2(T1+T2)][V1intermediate]=[3T1T2[T12(Q1finalQ1intermediate)+T22(Q1intermediateQ1init)]T2V1init] \begin{array}{ccc} \begin{bmatrix} 2(T_1+T_2) \end{bmatrix} \begin{bmatrix} V_{1_{intermediate}} \end{bmatrix} &= \begin{bmatrix} \frac{3}{T_1T_2}[T_1^2(Q_{1_{final}}-Q_{1_{intermediate}})+T_2^2(Q_{1_{intermediate}}-Q_{1_{init}})]-T_2 \cdot V_{1_{init}} \end{bmatrix} \end{array}

If we choose again arbitrarily that each segment should have 1 second of duration, then T1=1T_1=1 and T2=1T_2=1. Then

[2(1+1)][V1intermediate]=[311[12(π/2π/4)+12(π/40)]20]4V1intermediate=[3[π/4+π/4]]V1intermediate=3π/8 rad/s \begin{aligned} \begin{bmatrix} 2(1+1) \end{bmatrix} \begin{bmatrix} V_{1_{intermediate}} \end{bmatrix} &= \begin{bmatrix} \frac{3}{1 \cdot 1}[1^2(\pi/2-\pi/4)+1^2(\pi/4-0)]-2\cdot 0 \end{bmatrix} \\ 4 \cdot V_{1_{intermediate}}&= \begin{bmatrix} 3[\pi/4+\pi/4] \end{bmatrix} \\ V_{1_{intermediate}} &= 3\pi/8 \ rad/s \end{aligned}

We can now plot the two splines.

Duration12=1Displacement12=π/4A12=(2π/4+0+3π/8)12=π/8B12=(3π/4203π/8)1=3π/8C12=Vinit=0D12=Qinit=0 \begin{aligned} Duration_{1 \rightarrow 2} &= 1 \\ Displacement_{1 \rightarrow 2} &= \pi/4 \\ A_{1 \rightarrow 2} &= \frac{(2 \cdot - \pi/4 + 0 + 3\pi/8)}{1^2} &&= -\pi/8\\ B_{1 \rightarrow 2} &= \frac{(3 \cdot \pi/4 - 2 \cdot 0 - 3\pi/8)}{1} &&= 3\pi/8\\ C_{1 \rightarrow 2} &= V_{init} &&= 0\\ D_{1 \rightarrow 2} &= Q_{init} &&= 0\\ \end{aligned}

Duration23=1Displacement23=π/4A23=(2π/4+3π/8+0)12=π/8B23=(3π/423π/80)1=0C23=Vinit=3π/8D23=Qinit=π/4 \begin{aligned} Duration_{2 \rightarrow 3} &= 1 \\ Displacement_{2 \rightarrow 3} &= \pi/4 \\ A_{2 \rightarrow 3} &= \frac{(2 \cdot - \pi/4 + 3\pi/8 + 0)}{1^2} &&= -\pi/8\\ B_{2 \rightarrow 3} &= \frac{(3 \cdot \pi/4 - 2 \cdot 3\pi/8 - 0)}{1} &&= 0\\ C_{2 \rightarrow 3} &= V_{init} &&= 3\pi/8\\ D_{2 \rightarrow 3} &= Q_{init} &&= \pi/4\\ \end{aligned}

Figure: Two cubic splines for joint 1. Can you see where they meet? Hint: Each spline has 1s of duration.

We only see one spline, but there are actually two. The first spline is valid on the interval t=(0,1)t=(0,1), and the second spline is valid on the interval t=(1,2)t=(1,2). The two splines are identical because they have the same duration, displacement, and absolute change in velocity. Therefore, while we chose to use two splines, this movement could have been interpolated by a single spline.

Joint 2

We solve for V2intermediateV_{2_{intermediate}}.

[2(1+1)][V2intermediate]=[311[12(π/20)+12(0π/2)]20]4V2intermediate=[3[π/2π/2]]V2intermediate=3π/4 rad/s \begin{aligned} \begin{bmatrix} 2(1+1) \end{bmatrix} \begin{bmatrix} V_{2_{intermediate}} \end{bmatrix} &= \begin{bmatrix} \frac{3}{1 \cdot 1}[1^2(-\pi/2-0)+1^2(0-\pi/2)]-2\cdot 0 \end{bmatrix} \\ 4 \cdot V_{2_{intermediate}}&= \begin{bmatrix} 3[-\pi/2-\pi/2] \end{bmatrix} \\ V_{2_{intermediate}} &= -3\pi/4 \ rad/s \end{aligned}

A=(2Displacement/Duration+Vinit+Vfinal)Duration2B=(3Displacement/Duration2VinitVfinal)Duration A = \frac{(2 \cdot -Displacement / Duration + V_{init} + V_{final})}{Duration^2} \\ B = \frac{(3 \cdot Displacement / Duration - 2 \cdot V_{init} - V_{final})}{Duration} \\

Duration12=1Displacement12=π/2A12=(2(π/2)+0+(3π/4))12=π/4B12=(3(π/2)20(3π/4))1=3π/4C12=Vinit=0D12=Qinit=π/2 \begin{aligned} Duration_{1 \rightarrow 2} &= 1 \\ Displacement_{1 \rightarrow 2} &= -\pi/2 \\ A_{1 \rightarrow 2} &= \frac{(2 \cdot - (- \pi/2) + 0 + (- 3\pi/4))}{1^2} &&= \pi/4\\ B_{1 \rightarrow 2} &= \frac{(3 \cdot (- \pi/2) - 2 \cdot 0 - (-3\pi/4))}{1} &&= -3\pi/4\\ C_{1 \rightarrow 2} &= V_{init} &&= 0\\ D_{1 \rightarrow 2} &= Q_{init} &&= \pi/2\\ \end{aligned}

Duration23=1Displacement23=π/2A23=(2(π/2)+(3π/4)+0)12=π/4B23=(3(π/2)2(3π/4)0)1=0C23=Vinit=3π/4D23=Qinit=0 \begin{aligned} Duration_{2 \rightarrow 3} &= 1 \\ Displacement_{2 \rightarrow 3} &= -\pi/2 \\ A_{2 \rightarrow 3} &= \frac{(2 \cdot - (-\pi/2) + (- 3\pi/4) + 0)}{1^2} &&= \pi/4\\ B_{2 \rightarrow 3} &= \frac{(3 \cdot (- \pi/2) - 2 \cdot (-3\pi/4) - 0)}{1} &&= 0\\ C_{2 \rightarrow 3} &= V_{init} &&= -3\pi/4\\ D_{2 \rightarrow 3} &= Q_{init} &&= 0\\ \end{aligned}

Figure: Two cubic splines for joint 2. They meet at t=1st=1s.

Scaling Longer Trajectories

In order to satisfy joint-space and task-space velocity and acceleration limits, the m(n1)m(n-1) resulting cubic splines (m=m= number of joints, nn = number of control points) must each be scaled by the method described previously.

Pseudocode:

// segments: sequence of splines, lenth n - 1 
//          (n == number of interpolated points)
// task_space_limit: e.g. Xlim = 100, Ylim = 100mm/s, etc.
void scale(segments, task_space_limit): 

  for segment in segments:

    joints = segment.joints // joints: container of size m
	  
    // get_scale: See Joint Constraints section
    joint_space_ratio = joints.max(joint => joint.get_scale()) 
    task_space_ratio = joints.forward_velocity() / task_space_limit
	
	r_max = max(joint_space_ratio, task_space_ratio)
    scale(segment, r_max)

Above: For each segment ii, mm ratios are calculated, one for each joint, and the task space ratio is calculated. The maximum ratio rmaxr_{max} is selected. Then all mm splines at segment ii are scaled by rmaxr_{max}.

Pseudocode:

// Scale the given spline
void scale(segment, ratio): 

  segment.duration *= ratio // Update duration

  // Update velocity
  if (segment.is_first()) spline.Vinit /= ratio
  if (segment.is_last()) spline.Vfinal /= ratio
  if (segment.is_intermediate()) spline.Vintermediate /= ratio
   
  forward_propagate(segment.next);
  segment.compute_velocities() // e.g. find new Vintermediate
  segment.compute_coefficients() // e.g. get A B C D

// Update segment start and finish times
void forward_propagate(segment):

  while (next_segment != null):
    next_segment.forward_propagate() // e.g. Tinit = Tinit_prev + Duration
    next_segment= segment.next
    

Above: When a segment is scaled, the following changes occur

  1. the segment duration
  2. the segment initial or final velocity (or both)

Forward propagation: All segments after the scaled segment are affected: the start time of each spline becomes the finishing time of the previous. These values propagate to the last spline.

Finally, the resulting intermediate velocities and polynomial coefficients must be recomputed.


References

[1] Biagiotti, L., & Melchiorri, C. (2009). Trajectory Planning for Automatic Machines and Robots. Berlin, Heidelberg: Springer Berlin Heidelberg.


Discussion

Please feel free to start a discussion on GitHub.


Main page