smove
Version:
Sinusoidal movement library
359 lines (295 loc) • 9.25 kB
JavaScript
const assert = require('assert');
// Unpack Math
const { PI, sqrt, abs, asin, cos, sin } = Math
/**
* Class for producing smooth sinusoidal movements between two points.
*
* @param {Object} o - input parameters.
* @param {number} o.xf - end position (m).
* @param {number} o.a - maximum acceleration (m/s^2).
* @param {number} o.x0 - starting position (m).
* @param {number} o.v0 - starting velocity (m/s).
* @param {number} o.v_min - lower velocity limit (m/s).
* @param {number} o.v_max - upper velocity limit (m/s).
*/
class Smove {
constructor({ xf, a, x0=0, v0=0, v_min=null, v_max=null }) {
assert(xf !== undefined);
assert(a !== undefined);
let s = [ Smove.calculate(x0, xf, v0, a) ];
if(v_min !== null && v_max !== null)
assert(v_min <= v_max);
if(v_min !== null) {
assert(v_min >= 0);
s = Smove.limitMinVelocity(s, v_min);
}
if(v_max !== null) {
assert(v_max > 0);
s = Smove.limitMaxVelocity(s, abs(v_max));
}
this.sequence = s;
}
/**
* Get the total execution time.
* @returns {number} time (s).
*/
get dt() {
const s = this.sequence[this.sequence.length - 1];
return s.t0 + s.dt;
}
/**
* Get the starting velocity.
* @returns {number} velocity (m/s).
*/
get v0() {
return this.sequence[0].v0;
}
/**
* Get the starting position.
* @returns {number} position (m).
*/
get x0() {
return this.sequence[0].x0;
}
/**
* Get the final position.
* @returns {number} position (m).
*/
get xf() {
return this.sequence[this.sequence.length - 1].xf;
}
/**
* Get the Nyquist rate.
* @returns {number} frequency (Hz).
*/
get fs() {
// Find the highest frequency component
let f = 0;
for(let i = 0; i < this.sequence.length; ++i) {
if(this.sequence[i].f === undefined)
continue;
if(this.sequence[i].f > f)
f = this.sequence[i].f;
}
// Double it
return 2 * f;
}
/**
* Sample the sequence at a frequency of 'f' Hz and return an array of
* velocity values. If no frequency is given the Nyquist rate will be
* used.
*
* @param {number} [f] - sampling frequency (Hz).
* @returns {Array<Object>} sampled data.
*/
sample(f=this.fs) {
const period = 1 / f;
let data = []
let t = 0;
while(t <= this.dt) {
const v = this.getVelocity(t);
const x = this.getPosition(t);
data.push({ t, v, x });
t += period;
}
return data;
}
/**
* Returns the velocity at time 't'.
*
* @param {number} t - point of time referenced from the start of the smove.
* @returns {number} velocity (m/s).
*/
getVelocity(t) {
assert(t >= 0);
for(let i = 0; i < this.sequence.length; ++i) {
const s = this.sequence[i];
if(t > (s.t0 + s.dt))
continue;
if(s.A === undefined)
return s.v0; // Constant velocity
const { A, f, phi, t0 } = s;
return -A * f * sin((f * (t - t0)) + phi);
}
return 0;
}
/**
* Returns the change in position at time 't'.
*
* @param {number} t - point of time referenced from the start of the smove.
* @returns {number} position (m).
*/
getPosition(t) {
assert(t >= 0);
for(let i = 0; i < this.sequence.length; ++i) {
const s = this.sequence[i];
if(t > (s.t0 + s.dt))
continue;
if(s.A === undefined)
return (s.v0 * (t - s.t0)) + s.x0;
const { A, f, phi, m, t0 } = s;
return A * cos((f * (t - t0)) + phi) - m + s.x0;
}
return this.xf;
}
/**
* Calculate a sinusoidal movement between two points.
*
* Original algorithm by Joseph Sullivan.
*
* @param {number} x0 - start position (m).
* @param {number} xf - end position (m).
* @param {number} v0 - start velocity (m/s)
* @param {number} a - acceleration (m/s^2)
* @returns {Object}
* @private
*/
static calculate(x0, xf, v0, a) {
// Delta X
const dx = xf - x0;
if(dx == 0)
return {};
// Amplitude
let A = -a * dx**2 / (a * 2 * abs(dx) - v0**2);
if(dx < 0)
A *= -1;
const f = sqrt(abs(a / A)); // Frequency
const phi = asin(-v0 / (A * f)); // Phase angle
const m = A * cos(phi); // Offset
const dt = (PI - phi) / f; // Delta time
const t0 = 0; // Start time
// Final position
xf = A * cos((f * dt) + phi) - m + x0;
if(isNaN(xf))
throw RangeError("Failed to calculate end-point");
return { x0, xf, v0, a, A, f, phi, m, t0, dt };
}
/**
* Adjust a smove for maximum velocity constraints.
*
* @param {Object} s - smove to adjust.
* @param {number} v_max - maximum velocity.
* @returns {Array<Object>}
* @private
*/
static limitMaxVelocity(s, v_max) {
if(Array.isArray(s)) {
let sequence = [];
for(let i = 0; i < s.length; ++i) {
const result = Smove.limitMaxVelocity(s[i], v_max);
sequence = sequence.concat(result);
}
return sequence;
}
if(s.A === undefined)
return [ s ];
// Unpack initial values
const { x0, xf, A, f, phi, m } = s;
const v_peak = abs(A * f);
if(v_peak <= v_max)
return [ s ];
// Deep copy s1
const s1 = JSON.parse(JSON.stringify(s));
// Change end-point to when v_max occurs.
s1.dt = (asin(v_max / abs(A * f)) - phi) / f
const dx = A * cos((f * s1.dt) + phi) - m;
s1.xf = dx + x0;
// Deep copy s2
const s2 = JSON.parse(JSON.stringify(s1));
// Change end-point to when v=0 occurs.
s2.x0 = s1.xf;
s2.xf = dx + s2.x0;
if(xf > x0)
s2.phi = PI - asin(-v_max / (A * f));
else
s2.phi = PI - asin(v_max / (A * f));
s2.m = A * cos(s2.phi);
// Calculate delay
const dt = (abs(xf - x0) - abs(2 * dx)) / v_max;
let delay_xf = 0;
if(xf > x0)
delay_xf = s1.xf + (v_max * dt);
else
delay_xf = s1.xf - (v_max * dt);
const delay = {
x0: s1.xf,
xf: delay_xf,
v0: (xf > x0) ? v_max : -v_max,
t0: s1.t0 + s1.dt,
dt: dt,
}
// Shift phase 2 to be after delay
s2.x0 = delay.xf;
s2.xf = delay.xf + dx;
s2.t0 = delay.t0 + delay.dt;
return [ s1, delay, s2 ];
}
/**
* Adjust a smove for minimum velocity constraints.
*
* @param {Object} s - smove to adjust.
* @param {number} v_min - minimum velocity.
* @returns {Array<Object>}
* @private
*/
static limitMinVelocity(s, v_min) {
if(Array.isArray(s)) {
let sequence = [];
for(let i = 0; i < s.length; ++i) {
const result = Smove.limitMinVelocity(s[i], v_min);
sequence = sequence.concat(result);
}
return sequence;
}
// Unpack initial values
const { x0, xf, a, A, f, phi, m, t0 } = s;
// Check peak velocity
const v_peak = abs(A * f);
if(v_peak <= v_min) {
return [{
x0: x0,
xf: xf,
v0: (xf > x0) ? v_min : -v_min,
t0: t0,
dt: abs(x0 - xf) / v_min,
}];
}
// Find the time when v_min occurs.
const t_min = (asin(v_min / abs(A * f)) - phi) / f;
if(Number.isNaN(t_min) || t_min <= s.t0 || t_min > s.dt)
return [ s ]
// Calculate the position change during t_min
const dx = A * cos((f * t_min) + phi) - m + x0;
// Re-calculate smove
if(xf > x0)
s = Smove.calculate(dx, xf-dx, v_min, a);
else
s = Smove.calculate(dx, xf-dx, -v_min, a);
const dt = abs(dx / v_min);
s.t0 = dt;
s.dt -= t_min;
// Calculate delays
const delay1 = {
x0: x0,
xf: dx,
v0: (xf > x0) ? v_min : -v_min,
t0: 0,
dt: dt,
}
const delay2 = {
x0: s.xf,
xf: s.xf + dx,
v0: (xf > x0) ? v_min : -v_min,
t0: s.t0 + s.dt,
dt: dt,
}
// Shift the graph to begin after delay
if(xf > x0)
s.phi = asin(-v_min / (A * f));
else
s.phi = asin(v_min / (A * f));
s.m = A * cos(s.phi);
return [ delay1, s, delay2 ];
}
}
module.exports=exports=Smove;