# How do I calculate reference trajectory for a robot following a parabolic path at a constant speed?

I have a 3 DOF robot arm and have been tasked with making it follow a path of $$y=6-x^2$$. Normally to compute the reference trajectory, I would simply create a linearly spaced vector of $$x$$ values and then compute a vector of $$y$$ values using the given equation. However, for this task, I need the reference trajectory to have a constant speed. Our textbook has given us this hint, but so far, I have been unable to do anything about it.

How to use the Leibnitz rule to solve for reference trajectory:

1. Calculate the length of an arc of the parabola at a generic point whose horizontal position is $$x(t)$$;
2. Differentiate the length of the arc of the parabola at time $$t$$ to deduce the translational velocity along the arc. This velocity is constant and equal to $$V$$. Thus, you can solve for $$\dot{x}(t)$$.
3. Once you have $$\dot{x}(t)$$, you can deduce the reference position and velocity in $$x$$ and $$y$$.

This advice confuses me, mostly because I do not understand how an arc can have a length at one specific point.

Thank you for any help you can give me with this problem.

If the norm of the velocity shall remain constant along the arc of the parabola, then it holds $$\sqrt{\dot{x}^2+\dot{y}^2}=V$$.

Plugging $$\dot{y}=-2x\cdot\dot{x}$$ into the equation above, we end up with the nonlinear differential equation:

$$\dot{x}\sqrt{1+4x^2}=V.$$

It is not possible to solve the system symbolically, but we can integrate it numerically by relying for example on the model below:

The simulation provides us with the trajectories $$x\left(t\right)$$, $$y\left(t\right)$$, $$\dot{x}\left(t\right)$$, $$\dot{y}\left(t\right)$$.

The following snippet plots the data (obtained for $$V=0.1$$ m/s):

% simulate the model
mdl = 'model';
cs = getActiveConfigSet(mdl);
simOut = sim(mdl, cs);

x = find(simOut.yout, 'x');
y = find(simOut.yout, 'y');
xdot = find(simOut.yout, 'xdot');
ydot = find(simOut.yout, 'ydot');

mdlWks = get_param(mdl, 'ModelWorkspace');
V = getVariable(mdlWks, 'V');

% plot the trajectory
figure('color', 'white');
tiledlayout(3, 2);

nexttile([3 1]);
stairs(x.Values.Data, y.Values.Data);
xlabel('x [m]');
ylabel('y [m]');
grid('minor');

nexttile;
hold('on');
stairs(x.Values.Time, x.Values.Data);
stairs(y.Values.Time, y.Values.Data);
xlabel('time [s]');
ylabel('[m]');
legend({'$$x$$' '$$y$$'}, 'Interpreter', 'latex');
grid('minor');

nexttile;
hold('on');
stairs(xdot.Values.Time, xdot.Values.Data);
stairs(ydot.Values.Time, ydot.Values.Data);
xlabel('time [s]');
ylabel('[m/s]');
legend({'$$\dot{x}$$' '$$\dot{y}$$'}, 'Interpreter', 'latex');
grid('minor');

% verify the constant speed along the arc
nexttile;
hold('on');
yline(V.Value);
vel = sqrt(xdot.Values.Data.^2 + ydot.Values.Data.^2);
stairs(xdot.Values.Time, vel);
ylim([0.9 1.1]*V.Value);
xlabel('time [s]');
ylabel('[m/s]');
legend({'$$V$$' '$$\sqrt{\dot{x}^2+\dot{y}^2}$$'}, 'Interpreter', 'latex');
grid('minor');


As shown in the bottom right graph, the reconstructed velocity along the path is constant and equates to $$V$$.

You can find the model and the snippet hosted at https://github.com/pattacini/se-robotics-24695.

The arc length can by calculated by the following formula(section: finding arc length by integration): $$s=\int_a^b \sqrt{1+ \left(\frac{dy}{dx}\right)^2} dx$$

So each segment in your trajectory has a length that can be calculated from the equation above.

Assuming the system sample interval $$\Delta t$$ is small enough, the elements of the $$x$$ and $$y$$ vectors are given by:

$$x_n = x_{n-1} + \dot{x}_{n-1}\Delta t$$ $$y_n = 6 - x_n^2$$

$$\sqrt{\left(\frac{dx}{dt}\right)^2 + \left(\frac{dy}{dt}\right)^2} = V$$

To put in terms of just $$dx/dt$$:

$$\frac{dy}{dt} = \frac{dy}{dx}\frac{dx}{dt} =-2x\frac{dx}{dt}$$

so that:

$$\frac{dx}{dt} \sqrt{1 + 4x^2} = V$$

or $$\frac{dx}{dt} = \frac{V}{\sqrt{1 + 4x^2}}$$

Substituting and going back to dot notation:

$$x_n = x_{n-1} + \frac{V\Delta t}{\sqrt{1 + 4{x_{n-1}^2}}}$$

$$y_n = 6 - x_n^2$$

We are essentially starting a uniform vector in $$t$$ not $$x$$ and going from there.