Solving symbolic third order polynomial

Hey need help finding the a0, ... a3 coefficients of a third order polynomial with constraints.

In the picture below (blue) are the constraints. This is a polynomial for trajectory, where on time t0 the function equals the starting position and on time t1 the end position. Also the derivative of the formula resembles the speed of the trajectory.

I tried it in Matlab but the answers I get don't match the (picture red) solution.

Could you help me?

syms q(t) a0 a1 a2 a3 q0 q1 dq0 dq1 t0 t1;

% formules
q(t) = a0 + a1*t + a2*t^2 + a3*t^3

q(t) = 

dq = diff(q,t)

dq(t) = 

% solving
eqn = [q(t0)==q0, q(t1)==q1, dq(t0) == dq0, dq(t1) == dq1];
S = solve(eqn, [a0 a1 a2 a3]);

S.a0

ans = 

If I solve the eqution with self chosen parameters and then it works, but I the symbolic answer.

% solving
t0 = 0;
t1 = 2;
q0 = 0;
q1 = 150;
dq0 = 0;
dq1 = 0;
eqn = [q(t0)==q0, q(t1)==q1, dq(t0) == dq0, dq(t1) == dq1];
S = solve(eqn, [a0 a1 a2 a3]);

q(t) = subs(q, [a0 a1 a2 a3], [S.a0, S.a1, S.a2, S.a3])

fplot(q, [0 2])

Answer 1

[Torsten]

syms t t0 t1 q0 q1 q0dot q1dot a0 a1 a2 a3
f(t) = a0 + a1*(t-t0) + a2*(t-t0)^2 + a3*(t-t0)^3;
df = diff(f,t);
S = solve([f(t0)==q0,df(t0) == q0dot,f(t1)==q1,df(t1)==q1dot],[a0 a1 a2 a3])

S = struct with fields:
    a0: q0
    a1: q0dot
    a2: -(3*q0 - 3*q1 - 2*q0dot*t0 + 2*q0dot*t1 - q1dot*t0 + q1dot*t1)/(t0 - t1)^2
    a3: -(2*q0 - 2*q1 - q0dot*t0 + q0dot*t1 - q1dot*t0 + q1dot*t1)/((t0 - t1)*(t0^2 - 2*t0*t1 + t1^2))

S.a0

ans = 

S.a1

ans = 

q0dot

S.a2

ans = 

S.a3

ans = 

[Axel Degrande]

Why is it that you replaced t to t-t0 in the function f?

[Torsten]

The coefficients a0 = q0 and a1 = q0dot in your screenshot clearly indicate that q is Taylor-expanded around t = t0, not t = 0. And this is usual practise because if the conditions are prescribed at t0 and t1, the form of the coefficients is much simpler.

The last contribution to a2 (-2*q0dot/(t1-t0)^2) is wrong in any case: it must have unit [q]/s^2, but it has unit [q]/s^3 (s is time unit, e.g. seconds).

Answer 2

[Paul]

Hi Axel,

What is the source for the equations in red?

Here is Matlab's solution:

syms q(t) a0 a1 a2 a3 q0 q1 dq0 dq1 t0 t1;
% formules
q(t) = a0 + a1*t + a2*t^2 + a3*t^3;
dq = diff(q,t);

% solving
eqn = [q(t0)==q0, q(t1)==q1, dq(t0) == dq0, dq(t1) == dq1];
S = solve(eqn, [a0 a1 a2 a3]);
simplify(S.a0,100)

ans = 

simplify(S.a1,100)

ans = 

simplify(S.a2,100)

ans = 

simplify(S.a3,100)

ans = 

It looks like a3 is the same as the equation in red, but the others aren't (I tried various ways to simplify them further without success).

Verify the solution

qq(t) = simplify(subs(q,S));
dqq(t) = diff(qq(t));
simplify([qq(t0) qq(t1) dqq(t0) dqq(t1)])

ans = 

Now implement the red equations (double check this as I had to keep scrolling back and forth)

a0 = q0;
a1 = dq1;
a2 = 3/(t1-t0)^2*(q1-q0) - 1/(t1-t0)*dq1 - 2/(t1-t0)^2*dq0

a2 = 

a3 = -2/(t1-t0)^3*(q1-q0) + 1/(t1-t0)^2*(dq1 + dq0)

a3 = 

qqq(t) = a1 + a1*t + a2*t^2 + a3*t^3;
simplify([qqq(t0); qqq(t1)])

ans = 

Doesn't satisfy the first two blue constraints (or at least doesn't appear that way, I tried some simplifcations w/o success).

[Torsten]

@Paul

The form of the polynomial is not chosen correctly. See my answer for the "correct" q(t).

[Sam Chak]

Hi @Paul

If I'm not mistaken, the problem is related to path planning, a topic often covered in courses like Robot Dynamics & Control and Vehicle Dynamics & Control (including spacecraft, aircraft, automobiles, and watercraft).

[Axel Degrande]

Hi @Paul,

The source is from my Masters class Prof. But I wanted to check if what he said was true so I tried to calculate it in Matlab. So do you think that the (red) answers are wrong?

[Paul]

The red answers are definitely wrong for the form of the cubic polynomial posed in your code. They are probably, i.e., I didn't verify myself, correct for the form of cubic polynmial posed by Torsten.

Answer 3

[Sam Chak]

Hi @Axel Degrande

The trajectory position is chosen to be a cubic polynomial. From this cubic polynomial, the velocity can be obtained:

Since there are four unknowns, namely the initial position q0, initial velocity dq0, final position q1, and final velocity dq1, we can construct four equations with four constraints as follows:

These equations can then be expressed in a compact matrix form, as a single matrix equation of the form :

This system of four linear equations can be solved using the Inverse Matrix Method. See also: linsolve().

syms t1 t0 q(t)

t0   = sym('0');    % initial time
t1   = sym('2');    % final   time

q0   = sym('0');    % initial position
dq0  = sym('0');    % initial velocity
q1   = sym('150');  % final   position
dq1  = sym('0');    % final   velocity

A    = [1 t0 t0^2   t0^3;
        0  1 2*t0 3*t0^2;
        1 t1 t1^2   t1^3;
        0  1 2*t1 3*t1^2]

A = 

B    = [q0; dq0; q1; dq1]

B = 

% x  = linsolve(A, B)
x    = A\B

x = 

% Cubic Polynomial Trajectory
q(t) = x(1) +   x(2)*t + x(3)*t^2 +   x(4)*t^3

q(t) = 

dq(t)=          x(2)   + 2*x(3)*t + 3*x(4)*t^2

dq(t) = 

% Plot the trajectory
t0   = double(t0);  % convert the symbolic value of t0 to double precision
t1   = double(t1);  % convert the symbolic value of t1 to double precision
q0   = double(q0);  % convert the symbolic value of q0 to double precision
q1   = double(q1);  % convert the symbolic value of q1 to double precision
fplot( q, [t0 t1], 'LineWidth', 3, 'Color', '#528AFA'), hold on    % Sky palette
fplot(dq, [t0 t1], 'LineWidth', 3, 'Color', '#ffb7c5'), hold off   % Sakura palette

% Labels
grid on
axis([-0.5 2.5 -50 200])
xlabel('Time (seconds)')
ylabel('Amount')
title('Cubic Trajectory, q(t) = '+string(q))
legend({'$q(t)$', '$\dot{q}(t)$'}, 'Interpreter', 'LaTeX', 'AutoUpdate', 'off', 'Location', 'South', 'FontSize', 12)
xline([t0 t1], '--', {sprintf('t0: %.0f s', t0) sprintf('t1: %.0f s', t1)}, 'color', '#7F7F7F');
yline([q0 q1], '--', {sprintf('q0: %.0f m', q0) sprintf('q1: %.0f m', q1)}, 'color', '#7F7F7F', 'LabelVerticalAlignment', 'bottom');

[Sam Chak]

As demonstrated by @Paul, there is a slight discrepancy in the coefficient '' when comparing the solution computed by MATLAB to that supplied by @Axel Degrande (OP). @Torsten's modified polynomial formula also works, especially when it is relative to the start point (, ).

syms t1 t0 q0 q1 dq0 dq1

A  = [1 t0 t0^2   t0^3;
      0  1 2*t0 3*t0^2;
      1 t1 t1^2   t1^3;
      0  1 2*t1 3*t1^2];
B  = [q0; dq0; q1; dq1];
x  = A\B;
x  = simplify(x, 200)   % Solution for {a0; a1; a2; a3}

x = 

% Verifying solution
a0 = subs(x(1), t0, 0)

a0 = 

a1 = subs(x(2), {t0, t1}, {0, 2})

a1 = 

% solution computed by MATLAB
a2 = expand(x(3));
a2 = simplify(a2, 100)

a2 = 

subs(a2, {t0, t1}, {0, 2})

ans = 

% testing solution supplied by OP
at = expand((3/(t1 - t0)^2)*(q1 - q0) - (1/(t1 - t0))*dq1 - (2/(t1 - t0)^2)*dq0);
at = simplify(at, 100)

at = 

subs(at, {t0, t1}, {0, 2})

ans = 

[Sam Chak]

Hi @Axel Degrande,

In this example, I tested using the initial velocity dq0 = 50 and compared the output of your formula for to that computed by MATLAB. MATLAB returns , but your formula gives . Can you please check the source of your formula? Sometimes, there are errors in printed publications and lecture notes.

syms t1 t0 q(t)

t0   = sym('0');    % initial time
t1   = sym('2');    % final   time

q0   = sym('0');    % initial position
dq0  = sym('50');   % initial velocity
q1   = sym('150');  % final   position
dq1  = sym('0');    % final   velocity

A    = [1 t0 t0^2   t0^3;
        0  1 2*t0 3*t0^2;
        1 t1 t1^2   t1^3;
        0  1 2*t1 3*t1^2];
B    = [q0; dq0; q1; dq1];
x    = A\B      % a1 = 50; a2 = 125/2; a3 = -25

x = 

% Cubic Polynomial Trajectory
q(t) = x(1) +   x(2)*t + x(3)*t^2 +   x(4)*t^3;
dq(t)=          x(2)   + 2*x(3)*t + 3*x(4)*t^2;

% Plot the trajectory
t0   = double(t0);  % convert the symbolic value of t0 to double precision
t1   = double(t1);  % convert the symbolic value of t1 to double precision
q0   = double(q0);  % convert the symbolic value of q0 to double precision
q1   = double(q1);  % convert the symbolic value of q1 to double precision
fplot( q, [t0 t1], 'LineWidth', 3, 'Color', '#528AFA'), hold on    % Sky palette
fplot(dq, [t0 t1], 'LineWidth', 3, 'Color', '#ffb7c5'), hold off   % Sakura palette

% Labels
grid on
axis([-0.5 2.5 -50 200])
xlabel('Time (seconds)')
ylabel('Amount')
title('Cubic Trajectory, q(t) = '+string(q))
legend({'$q(t)$', '$\dot{q}(t)$'}, 'Interpreter', 'LaTeX', 'AutoUpdate', 'off', 'Location', 'South', 'FontSize', 12)
xline([t0 t1], '--', {sprintf('t0: %.0f s', t0) sprintf('t1: %.0f s', t1)}, 'color', '#7F7F7F');
yline([q0 q1], '--', {sprintf('q0: %.0f m', q0) sprintf('q1: %.0f m', q1)}, 'color', '#7F7F7F', 'LabelVerticalAlignment', 'bottom');

% Verifying the formula supplied by OP
a2 = (3/(t1 - t0)^2)*(q1 - q0) - (1/(t1 - t0))*dq1 - (2/(t1 - t0)^2)*dq0

a2 = 

[Axel Degrande]

Hi @Sam Chak,

Thank you for your answering! Its a different approach to the problem, but I need the symbolic answers. When I use your approach with symbolic math, I get the same result as above (pic below).

So maybe my master's class professor is wrong.

syms q(t) a0 a1 a2 a3 q0 q1 dq0 dq1 t0 t1;
A    = [1 t0 t0^2   t0^3;
        0  1 2*t0 3*t0^2;
        1 t1 t1^2   t1^3;
        0  1 2*t1 3*t1^2]
B    = [q0; dq0; q1; dq1]

x  = linsolve(A, B)

[Sam Chak]

Hi @Axel Degrande

This is the symbolic approach. The linsolve() command can also return the symbolic results for the coefficients {}. Because the cubic trajectory function connects a point at a specific time to another point at time via a 3rd-degree polynomial, the mathematical function can be expressed relative to the point at a specific time , and it looks like this:

In the program, we can assign the starting point to be at the initial time :

syms f(t) t0 t1 a0 a1 a2 a3 q0 dq0 q1 dq1

f(t) = a0 + a1*(t - t0) + a2*(t - t0)^2 + a3*(t - t0)^3

f(t) = 

g(t) = diff(f)

g(t) = 

A    = [1 t0-t0 (t0-t0)^2 (t0-t0)^3;
        0     1 2*(t0-t0) 3*(t0-t0)^2;
        1 t1-t0 (t1-t0)^2 (t1-t0)^3;
        0     1 2*(t1-t0) 3*(t1-t0)^2]

A = 

B    = [q0; dq0; q1; dq1]

B = 

x    = linsolve(A, B);
a    = simplify(x, 100)     % [a0; a1; a2; a3]

a = 

Update: I think I have found the typo in the formula (see green circle). It is good that you double-check before applying the formulas blindly.