mbs_trajectories.c File Reference

#include "mbs_trajectories.h"
#include "mbs_message.h"

Functions
int	mbs_get_ramp_time (double *t1, double *t2, double *t3, double delta_q, double v12, double a01, double a23, double t0)
	Compute the time related to a ramp trajectory. More...
int	mbs_ramp_3_order (double *q, double *qd, double *qdd, double tsim, double t0, double t1, double qmax)
	Compute a 3-order ramp trajectory. More...
int	mbs_ramp_5_order (double *q, double *qd, double *qdd, double tsim, double t0, double t1, double qmax)
	Compute a 5-order ramp trajectory. More...
int	mbs_ramp_2_order_3_piece_from_acc_vel (double *q, double *qd, double *qdd, double tsim, double v12, double a01, double a23, double t0, double qmax)
	Compute a 3-piece 2-order ramp trajectory. More...
int	mbs_ramp_2_order_3_piece (double *q, double *qd, double *qdd, double tsim, double t0, double t1, double t2, double t3, double qmax)
	Compute a 3-piece 2-order ramp trajectory. More...
int	mbs_ramp_3_order_3_piece (double *q, double *qd, double *qdd, double tsim, double t0, double t1, double t2, double t3, double qmax)
	Compute a 3-piece 3-order ramp trajectory. More...
int	mbs_ramp_4_order_3_piece (double *q, double *qd, double *qdd, double tsim, double t0, double t1, double t2, double t3, double qmax)
	Compute a 3-piece 4-order ramp trajectory. More...
void	sweep (double tsim, double t0, double f0, double t1, double f1, double zmax, double *z, double *zd, double *zdd)
	Compute a frequency sweep on a sine signal. More...
void	mbs_sine_signal (double tsim, double ampl, double phi, double f, double off, double *q, double *qd, double *qdd)
	Compute a sine trajectory. More...
void	mbs_cosine_signal (double tsim, double ampl, double phi, double f, double off, double *q, double *qd, double *qdd)
	Compute a cosine trajectory. More...
void	mbs_sigmoid (double tsim, double lambda, double t0, double *q, double *qd, double *qdd)
	Compute a sigmoid signal. More...
void	mbs_add_signal (double q1, double qd1, double qdd1, double q2, double qd2, double qdd2, double *q3, double *qd3, double *qdd3)
	Compute the sum of 2 signals. More...
void	mbs_sub_signal (double q1, double qd1, double qdd1, double q2, double qd2, double qdd2, double *q3, double *qd3, double *qdd3)
	Compute the substraction of 2 signals. More...
void	mbs_mult_signal (double q1, double qd1, double qdd1, double q2, double qd2, double qdd2, double *q3, double *qd3, double *qdd3)
	Compute the multiplication of 2 signals. More...
int	mbs_div_signal (double q1, double qd1, double qdd1, double q2, double qd2, double qdd2, double *q3, double *qd3, double *qdd3)
	Compute the division of 2 signals. More...

Function Documentation

mbs_add_signal()

void mbs_add_signal	(	double	q1,
		double	qd1,
		double	qdd1,
		double	q2,
		double	qd2,
		double	qdd2,
		double *	q3,
		double *	qd3,
		double *	qdd3
	)

Compute the sum of 2 signals.

Compute the position, velocity and acceleration of the sum of 2 signals.

Parameters

[in]	q1	position of the first signal.
[in]	qd1	velocity of the first signal.
[in]	qdd1	acceleration of the first signal.
[in]	q2	position of the second signal.
[in]	qd2	velocity of the fsecond signal.
[in]	qdd2	acceleration of the second signal.
[out]	q3	position of the sum.
[out]	qd3	velocity of the sum.
[out]	qdd3	acceleration of the sum.

mbs_cosine_signal()

void mbs_cosine_signal	(	double	tsim,
		double	ampl,
		double	phi,
		double	f,
		double	off,
		double *	q,
		double *	qd,
		double *	qdd
	)

Compute a cosine trajectory.

Compute the position, velocity and acceleration of a cosine trajectory q = ampl * cos(2 * pi * f * t + phi) + off

Parameters

[in]	tsim	time of the simulation.
[in]	ampl	amplitude of the cosine function.
[in]	phi	phase angle of the cosine function.
[in]	f	frequency of the cosine function.
[in]	off	offset of the cosine function.
[out]	q	position.
[out]	qd	velocity.
[out]	qdd	acceleration.

mbs_div_signal()

int mbs_div_signal	(	double	q1,
		double	qd1,
		double	qdd1,
		double	q2,
		double	qd2,
		double	qdd2,
		double *	q3,
		double *	qd3,
		double *	qdd3
	)

Compute the division of 2 signals.

Compute the position, velocity and acceleration of the division of 2 signals.

Parameters

[in]	q1	position of the first signal.
[in]	qd1	velocity of the first signal.
[in]	qdd1	acceleration of the first signal.
[in]	q2	position of the second signal.
[in]	qd2	velocity of the fsecond signal.
[in]	qdd2	acceleration of the second signal.
[out]	q3	position of the division q1/q2.
[out]	qd3	velocity of the division.
[out]	qdd3	acceleration of the division.

Returns

Error status, <0 in case of failure.

mbs_get_ramp_time()

int mbs_get_ramp_time	(	double *	t1,
		double *	t2,
		double *	t3,
		double	delta_q,
		double	v12,
		double	a01,
		double	a23,
		double	t0
	)

Compute the time related to a ramp trajectory.

Compute the time instant for a ramp when position, constant speed and acceleration/deceleration are given

Parameters

[out]	t1	stop time of the acceleration.
[out]	t2	stop time of the constant speed.
[out]	t3	stop time.
[in]	t0	start time.
[in]	delta_q	difference between start and end position.
[in]	v12	constant speed (>0).
[in]	a01	acceleration (>0).
[in]	a23	deceleration (<0).

Returns

Error status, <0 in case of failure.

mbs_mult_signal()

void mbs_mult_signal	(	double	q1,
		double	qd1,
		double	qdd1,
		double	q2,
		double	qd2,
		double	qdd2,
		double *	q3,
		double *	qd3,
		double *	qdd3
	)

Compute the multiplication of 2 signals.

Compute the position, velocity and acceleration of the multiplication of 2 signals.

Parameters

[in]	q1	position of the first signal.
[in]	qd1	velocity of the first signal.
[in]	qdd1	acceleration of the first signal.
[in]	q2	position of the second signal.
[in]	qd2	velocity of the fsecond signal.
[in]	qdd2	acceleration of the second signal.
[out]	q3	position of the multiplication.
[out]	qd3	velocity of the multiplication.
[out]	qdd3	acceleration of the multiplication.

mbs_ramp_2_order_3_piece()

int mbs_ramp_2_order_3_piece	(	double *	q,
		double *	qd,
		double *	qdd,
		double	tsim,
		double	t0,
		double	t1,
		double	t2,
		double	t3,
		double	qmax
	)

Compute a 3-piece 2-order ramp trajectory.

Compute the position, velocity and acceleration of a 3-piece 2-order ramp trajectory. Position at start time and velocity at start and stop time are equal to zero. Position at end time is equal to qmax. Postion and velocity are continuous at intermediate times.

Parameters

[in]	t0	start time.
[in]	t1	first intermediate time.
[in]	t2	second intermediate time.
[in]	t3	stop time.
[in]	tsim	simulation time.
[in]	qmax	amplitude of the ramp.
[out]	q	position.
[out]	qd	velocity.
[out]	qdd	acceleration.

Returns

Error status, <0 in case of failure.

mbs_ramp_2_order_3_piece_from_acc_vel()

int mbs_ramp_2_order_3_piece_from_acc_vel	(	double *	q,
		double *	qd,
		double *	qdd,
		double	tsim,
		double	v12,
		double	a01,
		double	a23,
		double	t0,
		double	qmax
	)

Compute a 3-piece 2-order ramp trajectory.

Compute the position, velocity and acceleration of a 3-piece 2-order ramp trajectory from the constant acceleration in the first and third piece and the constant velocity in the second piece. Position at start time and velocity at start and stop time are equal to zero. Position at end time is equal to qmax. Postion and velocity are continuous at intermediate time.

Parameters

[in]	v12	velocity in the second piece (>0).
[in]	a01	acceleration in the first piece (>0).
[in]	a23	acceleration in the third piece (<=0).
[in]	tsim	simulation time.
[in]	qmax	amplitude of the ramp
[out]	q	position.
[out]	qd	velocity.
[out]	qdd	acceleration.

Returns

Error status, <0 in case of failure.

mbs_ramp_3_order()

int mbs_ramp_3_order	(	double *	q,
		double *	qd,
		double *	qdd,
		double	tsim,
		double	t0,
		double	t1,
		double	qmax
	)

Compute a 3-order ramp trajectory.

Compute the position, velocity and acceleration of a 3-order ramp trajectory. Position at start time and velocity at start and stop time are equal to zero. Position at end time is equal to qmax.

Parameters

[in]	t0	start time.
[in]	t1	stop time.
[in]	tsim	simulation time.
[in]	qmax	amplitude of the ramp.
[out]	q	position.
[out]	qd	velocity.
[out]	qdd	acceleration.

Returns

Error status, <0 in case of failure.

mbs_ramp_3_order_3_piece()

int mbs_ramp_3_order_3_piece	(	double *	q,
		double *	qd,
		double *	qdd,
		double	tsim,
		double	t0,
		double	t1,
		double	t2,
		double	t3,
		double	qmax
	)

Compute a 3-piece 3-order ramp trajectory.

Compute the position, velocity and acceleration of a 3-piece 3-order ramp trajectory. Position at start time, velocity and acceleration at start and stop time are equal to zero. Position at end time is equal to qmax. Postion and velocity and acceleration are continuous at intermediate times. Position is cubic in the intermediate time interval.

Parameters

[in]	t0	start time.
[in]	t1	first intermediate time.
[in]	t2	second intermediate time.
[in]	t3	stop time.
[in]	tsim	simulation time.
[in]	qmax	amplitude of the ramp.
[out]	q	position.
[out]	qd	velocity.
[out]	qdd	acceleration.

Returns

Error status, <0 in case of failure.

mbs_ramp_4_order_3_piece()

int mbs_ramp_4_order_3_piece	(	double *	q,
		double *	qd,
		double *	qdd,
		double	tsim,
		double	t0,
		double	t1,
		double	t2,
		double	t3,
		double	qmax
	)

Compute a 3-piece 4-order ramp trajectory.

Compute the position, velocity and acceleration of a 3-piece 4-order ramp trajectory. Position at start time and velocity at start and stop time are equal to zero. Position at end time is equal to qmax. Postion and velocity are continuous at intermediate time. Postion and velocity and acceleration are continuous at intermediate times. Position is linear in the intermediate time interval.

Parameters

[in]	t0	start time.
[in]	t1	first intermediate time.
[in]	t2	second intermediate time.
[in]	t3	stop time.
[in]	tsim	simulation time.
[in]	qmax	amplitude of the ramp.
[out]	q	position.
[out]	qd	velocity.
[out]	qdd	acceleration.

Returns

Error status, <0 in case of failure.

mbs_ramp_5_order()

int mbs_ramp_5_order	(	double *	q,
		double *	qd,
		double *	qdd,
		double	tsim,
		double	t0,
		double	t1,
		double	qmax
	)

Compute a 5-order ramp trajectory.

Compute the position, velocity and acceleration of a 3-order ramp trajectory. Position at start time and velocity at start and stop time are equal to zero. Position at end time is equal to qmax.

Parameters

[in]	t0	start time.
[in]	t1	stop time.
[in]	tsim	simulation time.
[in]	qmax	amplitude of the ramp.
[out]	q	position.
[out]	qd	velocity.
[out]	qdd	acceleration.

Returns

Error status, <0 in case of failure.

mbs_sigmoid()

void mbs_sigmoid	(	double	tsim,
		double	lambda,
		double	t0,
		double *	q,
		double *	qd,
		double *	qdd
	)

Compute a sigmoid signal.

Compute the position, velocity and acceleration of a sigmoid signal. See https://fr.wikipedia.org/wiki/Sigmoïde_(mathématiques)

Parameters

[in]	tsim	time of the simulation.
[in]	t0	inflection point of the sigmoid.
[in]	lambda	linked to the curvature. It is equal to four times the derivative at the inflection point.
[out]	q	position.
[out]	qd	velocity.
[out]	qdd	acceleration.

mbs_sine_signal()

void mbs_sine_signal	(	double	tsim,
		double	ampl,
		double	phi,
		double	f,
		double	off,
		double *	q,
		double *	qd,
		double *	qdd
	)

Compute a sine trajectory.

Compute the position, velocity and acceleration of a sine trajectory. q = ampl * sin(2 * pi * f * t + phi) + off

Parameters

[in]	tsim	time of the simulation.
[in]	ampl	amplitude of the sine function.
[in]	phi	phase angle of the sine function.
[in]	f	frequency of the sine function.
[in]	off	position offset of the sine function.
[out]	q	position.
[out]	qd	velocity.
[out]	qdd	acceleration.

mbs_sub_signal()

void mbs_sub_signal	(	double	q1,
		double	qd1,
		double	qdd1,
		double	q2,
		double	qd2,
		double	qdd2,
		double *	q3,
		double *	qd3,
		double *	qdd3
	)

Compute the substraction of 2 signals.

Compute the position, velocity and acceleration of the substraction of 2 signals.

Parameters

[in]	q1	position of the first signal.
[in]	qd1	velocity of the first signal.
[in]	qdd1	acceleration of the first signal.
[in]	q2	position of the second signal.
[in]	qd2	velocity of the fsecond signal.
[in]	qdd2	acceleration of the second signal.
[out]	q3	position of the substraction q1-q2.
[out]	qd3	velocity of the substraction.
[out]	qdd3	acceleration of the substraction.

sweep()

void sweep	(	double	tsim,
		double	t0,
		double	f0,
		double	t1,
		double	f1,
		double	zmax,
		double *	z,
		double *	zd,
		double *	zdd
	)

Compute a frequency sweep on a sine signal.

Compute the position, velocity and acceleration of a sine signal with a frequency sweep. Frequency increases linearly with time.

Parameters

[in]	tsim	time of the simulation.
[in]	t0	start time.
[in]	t1	stop time.
[in]	f0	start frequency.
[in]	f1	stop frequency.
[in]	zmax	amplitude of oscillation.
[out]	z	position.
[out]	zd	velocity.
[out]	zdd	acceleration.