Solution:

Consider a simple path i.e. from $w^0$ to $w^1$ an interpolated motion between two-knot points $w^0$ to $w^1$.

The equation for the path from $w^0$ to $w^1$ is represented by

a cubic polynomial given by $\mathrm{w}(\mathrm{t})=\mathrm{a} \mathrm{t}^3+\mathrm{bt}^2+\mathrm{c} t+\mathrm{d} ; 0 \leq \mathrm{t} \leq \mathrm{T} ; \mathrm{T}\gt0$; where $\mathrm{w}(\mathrm{t})$ is a cubic polynomial trajectory in Tool Configuration Space $\mathrm{R}^6$ and $\mathrm{a}, \mathrm{b}, \mathrm{c}, \mathrm{d}$ are the unknown polynomial coefficients which has to be determined.

Once, the equation for the path is determined, the robot moves along the specified path.

Interpolation:

There are four constraints on the four unknown coefficients of the cubic interpolating polynomial.

Consider the equation $w(t)=a t^3+b t^2+c t+d$

At the start of the path, $t=0$ in w(t), Putting $t=0$ in $w(t)$ $w(0)=d=w^0$

At the end of the path, $t=T$, Putting $t=T$ in w(t) $w(T)=a T^3+b T^2+c T+d=w^1$

If the tool starts with velocity of $v^0$ at the beginning of the trajectory i.e. $t=0$ and ends with velocity $\mathrm{v}^1$ at the end of trajectory $\mathrm{t}=\mathrm{T}$, then the two end-point velocity constraints are

Velocity $=\frac{d}{d t}\left[\mathrm{at}^3+\mathrm{b} \mathrm{t}^2+\mathrm{ct}+\mathrm{d}\right]$

Therefore, $\dot{w}(t)=3 a t^2+2 b t+c$

At the start of the path, $t=0$, Putting $t=0$ in $\dot{w}(t)$; we get $\dot{\mathrm{w}}(0)=\mathrm{c}=\mathrm{v}^0$

At the end of the path $t=T$, Putting $t=T$ in $\dot{w}(t)$; we get $\dot{w}(T)=3 a T^2+2 b T+c=v^1$

To get the values of a and b; solving by trial and error method

For finding the value of a ;

1) Add $v^1$ to $v^0$ and multiply the sum by T

2) Subtract $w^0$ from $w^1$ and multiply the result by 2

We get,

$\mathrm{a}=\frac{\mathrm{T}\left(\mathrm{v}^1+\mathrm{v}^0\right)-2\left(\mathrm{w}^1-\mathrm{w}^0\right)}{T}$

For finding the value of b;

1) Take $v^1$, add it to twice $v^0$ and multiply the result by $T^2$

2) Subtract $w^0$ from $w^1$ and multiply the result by 3

3) the Result of step 1 and the result of step 2, divide by $T^2$

4) Take the negative of the entire result

We get,

$\mathrm{b}=-\frac{\mathrm{T}\left[\left(\mathrm{v}^1+2 \mathrm{v}^0\right)-3\left(\mathrm{w}^1-\mathrm{w}^0\right)\right]}{T^2}$