How do I model particle motion using normalized position magnitude and unit vectors?

Question

I am trying to model a particle moving in a non-linear trajectory. I am working with real-time motion equipment, so I have already accounted for simple linear translation in 3D space using S-curve velocity with the equipment restraints. My project is to make a model that takes inputs for the acceleration, rate, and desired position (Cartesian coordinates), and outputs the current Cartesian position in 1 millisecond intervals.

With an input of a desired acceleration of 25cm/s^2, desired rate of 12cm/s, and desired position of (0, 0, -40), an example output of my model using linear translation would be as follows:

...
(0.500, 0, 0, -1.078453)
(0.501, 0, 0, -1.084931)
(0.502, 0, 0, -1.097958)
...

The underlying physics to produce this relies on normalizing all of the inputs, working with magnitudes, and then multiplying the resulting position magnitude by the trajectory's unit vector. This is easy for linear translations because the unit vector is constant.

I want to model more complex translational trajectories, such as a parabolic trajectory. I started by beginning the trajectory at (0, 0, -40) with a desired ending position of (0, 0, 40) while passing through (40, 0, 0) according to the function $$x =-0.025z^2 +40.$$ Since my current linear translation model handles purely magnitudes, I thought that I would only have to adjust the path's total length and update the trajectory's unit vector every millisecond.

I got the arc length of the function from -40 <= z <= 40, and that ended up being approximately 118.315 cm. Then, my thought for the unit vector was to get the tangent unit vector for the function every millisecond. I calculated this as $$r = \langle z \cdot -0.05 , 0, 1\rangle,\quad u = r/|r|$$ where $z$ is the current value of the $z$-component of the position.

I tried adjusting my model for these calculations, and the model does not output the correct results. I suspect it has something to do with the unit vector, because the output shows that the value of x begins to decrease when the x component in the unit tangent vector is less than the z component, not when the x component is negative.

I think I am heavily misguiding myself. Where did I go wrong in my approach?

Answer 1

You are essentially tasked with writing a waypoint trajectory generator. Trajectories of this sort are defined as functions of time, rather than positions. That is, we want $x(t)$ and not $x(z)$. As such, a parabolic trajectory would follow the polynomial, \begin{align} x(t) &= \alpha_0+\alpha_1t+\alpha_2t^2 \\ \dot{x}(t) &= \alpha_1+2\alpha_2t \\ \ddot{x}(t) &= 2\alpha_2 \end{align} which we can recognize as the Newtonian kinematics equations where $\alpha_0=x_0$, $\alpha_1=v$ and $\alpha_2=a/2$.

If we ignore the velocity and acceleration constraints, then a relatively simple means of obtaining the trajectories would be to utilize a spline interpolator for each position,

points = [(0, 0, -40), (40, 0, 0), (0, 0, 40)]
times = [1, 2, 3]
x_path = spline(times, [p[0] for p in points], ms_resolution)
y_path = spline(times, [p[1] for p in points], ms_resolution)
z_path = spline(times, [p[2] for p in points], ms_resolution)
for t, x, y, z in zip(times, x_path, y_path, z_path):
    print(t, x, y, z)

where ms_resolution is the timestamps in ms between start & finish and spline is a function that generates a spline interpolation given the knots (times & positions) and interpolated over the times given by the third arguments. Note that times here is somewhat arbitrary; we could also use [0, 1] as the two endpoints ought to arrive at the same resulting curve in 3D space.

However, since you want to constrain the path using the input $v$ and $a$, you will probably might have to roll your own implementation of the spline function that can apply the constraints (e.g., by inserting additional waypoints such that the velocity/acceleration is limited--for instance, utilizing finite differences, you can compute the straight-line velocity between points and if the velocity exceeds the maximum, you can insert a new point between the two points that moves further away).
Alternatively, since you know you have a peak velocity and acceleration, you can try using trapezoidal velocity profile (i.e., has an acceleration & deceleration phase bounding a constant velocity phase), though I think that this method does not guarantee hitting all of the waypoints; see also this Robotics.SE Q&A.