TODO: document the algorithm |T1| T2 |T3|

__| |____ __ Jerk |_|

/ \ Acceleration ___/ ___

;" / / Velocity ; -—". More...

#include <VelocitySmoothing.hpp>

Collaboration diagram for VelocitySmoothing:

Public Member Functions
	VelocitySmoothing (float initial_accel=0.f, float initial_vel=0.f, float initial_pos=0.f)

	~VelocitySmoothing ()=default

void	reset (float accel, float vel, float pos)
	Reset the state. More...

void	updateDurations (float vel_setpoint)
	Compute T1, T2, T3 depending on the current state and velocity setpoint. More...

void	updateTraj (float dt, float time_stretch=1.f)
	Generate the trajectory (acceleration, velocity and position) by integrating the current jerk. More...

float	getMaxJerk () const
	Getters and setters. More...

void	setMaxJerk (float max_jerk)

float	getMaxAccel () const

void	setMaxAccel (float max_accel)

float	getMaxVel () const

void	setMaxVel (float max_vel)

float	getCurrentJerk () const

void	setCurrentAcceleration (const float accel)

float	getCurrentAcceleration () const

void	setCurrentVelocity (const float vel)

float	getCurrentVelocity () const

void	setCurrentPosition (const float pos)

float	getCurrentPosition () const

float	getVelSp () const

float	getT1 () const

float	getT2 () const

float	getT3 () const

float	getTotalTime () const

Static Public Member Functions
static void	timeSynchronization (VelocitySmoothing *traj, int n_traj)
	Synchronize several trajectories to have the same total time. More...

Private Member Functions
void	updateDurationsMinimizeTotalTime ()
	Compute T1, T2, T3 depending on the current state and velocity setpoint. More...

void	updateDurationsGivenTotalTime (float T123)
	Compute T1, T2, T3 depending on the current state and velocity setpoint. More...

int	computeDirection ()
	Compute the direction of the jerk to be applied in order to drive the current state to the desired one. More...

float	computeVelAtZeroAcc ()
	Compute the velocity at which the trajectory will be if the maximum jerk is applied during the time required to cancel the current acceleration. More...

float	computeT1 (float a0, float v3, float j_max, float a_max)
	Compute increasing acceleration time. More...

float	computeT1 (float T123, float a0, float v3, float j_max, float a_max)
	Compute increasing acceleration time using total time constraint. More...

float	saturateT1ForAccel (float a0, float j_max, float T1, float a_max)
	Saturate T1 in order to respect the maximum acceleration constraint. More...

float	computeT2 (float T1, float T3, float a0, float v3, float j_max)
	Compute constant acceleration time. More...

float	computeT2 (float T123, float T1, float T3)
	Compute constant acceleration time using total time constraint. More...

float	computeT3 (float T1, float a0, float j_max)
	Compute decreasing acceleration time. More...

Trajectory	evaluatePoly (float j, float a0, float v0, float x0, float t, int d)
	Compute the jerk, acceleration, velocity and position of a jerk-driven polynomial trajectory at a given time t. More...

Private Attributes
float	_vel_sp {0.0f}

float	_max_jerk = 22.f

float	_max_accel = 8.f

float	_max_vel = 6.f

Trajectory	_state {}

int	_direction {0}

Trajectory	_state_init {}

float	_T1 = 0.f
	Increasing acceleration [s]. More...

float	_T2 = 0.f
	Constant acceleration [s]. More...

float	_T3 = 0.f
	Decreasing acceleration [s]. More...

float	_local_time = 0.f
	Current local time. More...

Detailed Description

TODO: document the algorithm |T1| T2 |T3|

__| |____ __ Jerk |_|

/ \ Acceleration ___/ ___

;" / / Velocity ; -—".

Definition at line 61 of file VelocitySmoothing.hpp.

Constructor & Destructor Documentation

◆ VelocitySmoothing()

VelocitySmoothing::VelocitySmoothing	(	float	initial_accel = `0.f`,
		float	initial_vel = `0.f`,
		float	initial_pos = `0.f`
	)

Definition at line 40 of file VelocitySmoothing.cpp.

References reset().

Here is the call graph for this function:

◆ ~VelocitySmoothing()

VelocitySmoothing::~VelocitySmoothing ( )

default

Member Function Documentation

◆ computeDirection()

int VelocitySmoothing::computeDirection ( )

private

Compute the direction of the jerk to be applied in order to drive the current state to the desired one.

Definition at line 179 of file VelocitySmoothing.cpp.

References _state, _vel_sp, Trajectory::a, computeVelAtZeroAcc(), and math::sign().

Referenced by updateDurations().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ computeT1() [1/2]

float VelocitySmoothing::computeT1	(	float	a0,
		float	v3,
		float	j_max,
		float	a_max
	)

inlineprivate

Compute increasing acceleration time.

Definition at line 71 of file VelocitySmoothing.cpp.

References f(), math::max(), and saturateT1ForAccel().

Referenced by updateDurationsGivenTotalTime(), and updateDurationsMinimizeTotalTime().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ computeT1() [2/2]

float VelocitySmoothing::computeT1	(	float	T123,
		float	a0,
		float	v3,
		float	j_max,
		float	a_max
	)

inlineprivate

Compute increasing acceleration time using total time constraint.

Definition at line 101 of file VelocitySmoothing.cpp.

References f(), math::max(), and saturateT1ForAccel().

Here is the call graph for this function:

◆ computeT2() [1/2]

float VelocitySmoothing::computeT2	(	float	T1,
		float	T3,
		float	a0,
		float	v3,
		float	j_max
	)

inlineprivate

Compute constant acceleration time.

Definition at line 138 of file VelocitySmoothing.cpp.

References math::abs_t(), f(), FLT_EPSILON, and math::max().

Referenced by updateDurationsGivenTotalTime(), and updateDurationsMinimizeTotalTime().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ computeT2() [2/2]

float VelocitySmoothing::computeT2	(	float	T123,
		float	T1,
		float	T3
	)

inlineprivate

Compute constant acceleration time using total time constraint.

Definition at line 151 of file VelocitySmoothing.cpp.

References f(), and math::max().

Here is the call graph for this function:

◆ computeT3()

float VelocitySmoothing::computeT3	(	float	T1,
		float	a0,
		float	j_max
	)

inlineprivate

Compute decreasing acceleration time.

Definition at line 157 of file VelocitySmoothing.cpp.

References f(), and math::max().

Referenced by updateDurationsGivenTotalTime(), and updateDurationsMinimizeTotalTime().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ computeVelAtZeroAcc()

float VelocitySmoothing::computeVelAtZeroAcc ( )

private

Compute the velocity at which the trajectory will be if the maximum jerk is applied during the time required to cancel the current acceleration.

Definition at line 197 of file VelocitySmoothing.cpp.

References _max_jerk, _state, Trajectory::a, FLT_EPSILON, math::sign(), and Trajectory::v.

Referenced by computeDirection().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ evaluatePoly()

Trajectory VelocitySmoothing::evaluatePoly	(	float	j,
		float	a0,
		float	v0,
		float	x0,
		float	t,
		int	d
	)

inlineprivate

Compute the jerk, acceleration, velocity and position of a jerk-driven polynomial trajectory at a given time t.

Parameters

j	jerk
a0	initial acceleration at t = 0
v0	initial velocity
x0	initial postion
t	current time
d	direction

Definition at line 225 of file VelocitySmoothing.cpp.

References Trajectory::a, Trajectory::j, t2, t3, Trajectory::v, and Trajectory::x.

Referenced by updateTraj().

Here is the caller graph for this function:

◆ getCurrentAcceleration()

float VelocitySmoothing::getCurrentAcceleration ( ) const

inline

Definition at line 104 of file VelocitySmoothing.hpp.

Referenced by ManualVelocitySmoothingZ::updateTrajectories().

Here is the caller graph for this function:

◆ getCurrentJerk()

float VelocitySmoothing::getCurrentJerk ( ) const

inline

Definition at line 102 of file VelocitySmoothing.hpp.

Referenced by ManualVelocitySmoothingZ::updateTrajectories().

Here is the caller graph for this function:

◆ getCurrentPosition()

float VelocitySmoothing::getCurrentPosition ( ) const

inline

Definition at line 108 of file VelocitySmoothing.hpp.

Referenced by ManualVelocitySmoothingZ::updateTrajectories().

Here is the caller graph for this function:

◆ getCurrentVelocity()

float VelocitySmoothing::getCurrentVelocity ( ) const

inline

Definition at line 106 of file VelocitySmoothing.hpp.

Referenced by ManualVelocitySmoothingZ::updateTrajectories().

Here is the caller graph for this function:

◆ getMaxAccel()

float VelocitySmoothing::getMaxAccel ( ) const

inline

Definition at line 96 of file VelocitySmoothing.hpp.

Referenced by ManualVelocitySmoothingXY::getMaxAccel().

Here is the caller graph for this function:

◆ getMaxJerk()

float VelocitySmoothing::getMaxJerk ( ) const

inline

Getters and setters.

Definition at line 93 of file VelocitySmoothing.hpp.

Referenced by ManualVelocitySmoothingXY::getMaxJerk().

Here is the caller graph for this function:

◆ getMaxVel()

float VelocitySmoothing::getMaxVel ( ) const

inline

Definition at line 99 of file VelocitySmoothing.hpp.

Referenced by ManualVelocitySmoothingXY::getMaxVel().

Here is the caller graph for this function:

◆ getT1()

float VelocitySmoothing::getT1 ( ) const

inline

Definition at line 112 of file VelocitySmoothing.hpp.

◆ getT2()

float VelocitySmoothing::getT2 ( ) const

inline

Definition at line 113 of file VelocitySmoothing.hpp.

◆ getT3()

float VelocitySmoothing::getT3 ( ) const

inline

Definition at line 114 of file VelocitySmoothing.hpp.

◆ getTotalTime()

float VelocitySmoothing::getTotalTime ( ) const

inline

Definition at line 115 of file VelocitySmoothing.hpp.

References Trajectory::j.

Referenced by main(), and timeSynchronization().

Here is the caller graph for this function:

◆ getVelSp()

float VelocitySmoothing::getVelSp ( ) const

inline

Definition at line 110 of file VelocitySmoothing.hpp.

◆ reset()

void VelocitySmoothing::reset	(	float	accel,
		float	vel,
		float	pos
	)

Reset the state.

Parameters

accel	Current acceleration
vel	Current velocity
pos	Current position

Definition at line 45 of file VelocitySmoothing.cpp.

References _state, _state_init, Trajectory::a, Trajectory::j, Trajectory::v, and Trajectory::x.

Referenced by ManualVelocitySmoothingZ::reset(), and VelocitySmoothing().

Here is the caller graph for this function:

◆ saturateT1ForAccel()

float VelocitySmoothing::saturateT1ForAccel	(	float	a0,
		float	j_max,
		float	T1,
		float	a_max
	)

inlineprivate

Saturate T1 in order to respect the maximum acceleration constraint.

Definition at line 55 of file VelocitySmoothing.cpp.

Referenced by computeT1().

Here is the caller graph for this function:

◆ setCurrentAcceleration()

void VelocitySmoothing::setCurrentAcceleration ( const float accel )

inline

Definition at line 103 of file VelocitySmoothing.hpp.

Referenced by main().

Here is the caller graph for this function:

◆ setCurrentPosition()

void VelocitySmoothing::setCurrentPosition ( const float pos )

inline

Definition at line 107 of file VelocitySmoothing.hpp.

Referenced by ManualVelocitySmoothingZ::checkPositionLock(), ManualVelocitySmoothingZ::setCurrentPosition(), and ManualVelocitySmoothingXY::setCurrentPosition().

Here is the caller graph for this function:

◆ setCurrentVelocity()

void VelocitySmoothing::setCurrentVelocity ( const float vel )

inline

Definition at line 105 of file VelocitySmoothing.hpp.

Referenced by ManualVelocitySmoothingZ::checkPositionLock(), main(), ManualVelocitySmoothingZ::setCurrentVelocity(), and ManualVelocitySmoothingXY::setCurrentVelocity().

Here is the caller graph for this function:

◆ setMaxAccel()

void VelocitySmoothing::setMaxAccel ( float max_accel )

inline

Definition at line 97 of file VelocitySmoothing.hpp.

Referenced by main(), ManualVelocitySmoothingXY::setMaxAccel(), and ManualVelocitySmoothingZ::updateTrajConstraints().

Here is the caller graph for this function:

◆ setMaxJerk()

void VelocitySmoothing::setMaxJerk ( float max_jerk )

inline

Definition at line 94 of file VelocitySmoothing.hpp.

Referenced by main(), ManualVelocitySmoothingZ::setMaxJerk(), and ManualVelocitySmoothingXY::setMaxJerk().

Here is the caller graph for this function:

◆ setMaxVel()

void VelocitySmoothing::setMaxVel ( float max_vel )

inline

Definition at line 100 of file VelocitySmoothing.hpp.

Referenced by main(), ManualVelocitySmoothingXY::setMaxVel(), and ManualVelocitySmoothingZ::updateTrajConstraints().

Here is the caller graph for this function:

◆ timeSynchronization()

void VelocitySmoothing::timeSynchronization	(	VelocitySmoothing *	traj,
		int	n_traj
	)

static

Synchronize several trajectories to have the same total time.

This is required to generate straight lines. The resulting total time is the one of the longest trajectory.

Parameters

traj	an array of VelocitySmoothing objects
n_traj	the number of trajectories to be synchronized

Definition at line 266 of file VelocitySmoothing.cpp.

References FLT_EPSILON, getTotalTime(), and updateDurationsGivenTotalTime().

Referenced by FlightTaskAutoLineSmoothVel::_generateTrajectory(), main(), ManualVelocitySmoothingXY::updateTrajDurations(), and VelocitySmoothingTest::updateTrajectories().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ updateDurations()

void VelocitySmoothing::updateDurations ( float vel_setpoint )

Compute T1, T2, T3 depending on the current state and velocity setpoint.

This should be called on every cycle and before updateTraj().

Parameters

vel_setpoint velocity setpoint input

Definition at line 163 of file VelocitySmoothing.cpp.

References _direction, _local_time, _max_vel, _state, _state_init, _T1, _T2, _T3, _vel_sp, computeDirection(), math::constrain(), and updateDurationsMinimizeTotalTime().

Referenced by main(), and ManualVelocitySmoothingZ::update().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ updateDurationsGivenTotalTime()

void VelocitySmoothing::updateDurationsGivenTotalTime ( float T123 )

private

Compute T1, T2, T3 depending on the current state and velocity setpoint.

Parameters

T123	desired total time of the trajectory

Definition at line 289 of file VelocitySmoothing.cpp.

References _direction, _max_accel, _max_jerk, _state, _T1, _T2, _T3, _vel_sp, Trajectory::a, computeT1(), computeT2(), computeT3(), and Trajectory::v.

Referenced by timeSynchronization().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ updateDurationsMinimizeTotalTime()

void VelocitySmoothing::updateDurationsMinimizeTotalTime ( )

private

Compute T1, T2, T3 depending on the current state and velocity setpoint.

Minimize the total time of the trajectory

Definition at line 210 of file VelocitySmoothing.cpp.

References _direction, _max_accel, _max_jerk, _state, _T1, _T2, _T3, _vel_sp, Trajectory::a, computeT1(), computeT2(), computeT3(), and Trajectory::v.

Referenced by updateDurations().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ updateTraj()

void VelocitySmoothing::updateTraj	(	float	dt,
		float	time_stretch = `1.f`
	)

Generate the trajectory (acceleration, velocity and position) by integrating the current jerk.

Parameters

dt	integration period
time_stretch	(optional) used to scale the integration period. This can be used to slow down or fast-forward the trajectory

Definition at line 240 of file VelocitySmoothing.cpp.

References _direction, _local_time, _max_jerk, _state, _state_init, _T1, _T2, _T3, Trajectory::a, evaluatePoly(), f(), math::min(), t1, t2, t3, Trajectory::v, and Trajectory::x.

Referenced by main(), and ManualVelocitySmoothingZ::updateTrajectories().

Here is the call graph for this function:

Here is the caller graph for this function:

Member Data Documentation

◆ _direction

int VelocitySmoothing::_direction {0}

private

Definition at line 204 of file VelocitySmoothing.hpp.

Referenced by updateDurations(), updateDurationsGivenTotalTime(), updateDurationsMinimizeTotalTime(), and updateTraj().

◆ _local_time

float VelocitySmoothing::_local_time = 0.f

private

Current local time.

Definition at line 214 of file VelocitySmoothing.hpp.

Referenced by updateDurations(), and updateTraj().

◆ _max_accel

float VelocitySmoothing::_max_accel = 8.f

private

Definition at line 199 of file VelocitySmoothing.hpp.

Referenced by updateDurationsGivenTotalTime(), and updateDurationsMinimizeTotalTime().

◆ _max_jerk

float VelocitySmoothing::_max_jerk = 22.f

private

Definition at line 198 of file VelocitySmoothing.hpp.

Referenced by computeVelAtZeroAcc(), updateDurationsGivenTotalTime(), updateDurationsMinimizeTotalTime(), and updateTraj().

◆ _max_vel

float VelocitySmoothing::_max_vel = 6.f

private

Definition at line 200 of file VelocitySmoothing.hpp.

Referenced by updateDurations().

◆ _state

Trajectory VelocitySmoothing::_state {}

private

Definition at line 203 of file VelocitySmoothing.hpp.

Referenced by computeDirection(), computeVelAtZeroAcc(), reset(), updateDurations(), updateDurationsGivenTotalTime(), updateDurationsMinimizeTotalTime(), and updateTraj().

◆ _state_init

Trajectory VelocitySmoothing::_state_init {}

private

Definition at line 207 of file VelocitySmoothing.hpp.

Referenced by reset(), updateDurations(), and updateTraj().

◆ _T1

float VelocitySmoothing::_T1 = 0.f

private

Increasing acceleration [s].

Definition at line 210 of file VelocitySmoothing.hpp.

Referenced by updateDurations(), updateDurationsGivenTotalTime(), updateDurationsMinimizeTotalTime(), and updateTraj().

◆ _T2

float VelocitySmoothing::_T2 = 0.f

private

Constant acceleration [s].

Definition at line 211 of file VelocitySmoothing.hpp.

Referenced by updateDurations(), updateDurationsGivenTotalTime(), updateDurationsMinimizeTotalTime(), and updateTraj().

◆ _T3

float VelocitySmoothing::_T3 = 0.f

private

Decreasing acceleration [s].

Definition at line 212 of file VelocitySmoothing.hpp.

Referenced by updateDurations(), updateDurationsGivenTotalTime(), updateDurationsMinimizeTotalTime(), and updateTraj().

◆ _vel_sp

float VelocitySmoothing::_vel_sp {0.0f}

private

Definition at line 195 of file VelocitySmoothing.hpp.

Referenced by computeDirection(), updateDurations(), updateDurationsGivenTotalTime(), and updateDurationsMinimizeTotalTime().

The documentation for this class was generated from the following files:

src/lib/flight_tasks/tasks/Utility/VelocitySmoothing.hpp
src/lib/flight_tasks/tasks/Utility/VelocitySmoothing.cpp

Public Member Functions

Static Public Member Functions

Private Member Functions

Private Attributes

Detailed Description

Constructor & Destructor Documentation

◆ VelocitySmoothing()

◆ ~VelocitySmoothing()

Member Function Documentation

◆ computeDirection()

◆ computeT1() [1/2]

◆ computeT1() [2/2]

◆ computeT2() [1/2]

◆ computeT2() [2/2]

◆ computeT3()

◆ computeVelAtZeroAcc()

◆ evaluatePoly()

◆ getCurrentAcceleration()

◆ getCurrentJerk()

◆ getCurrentPosition()

◆ getCurrentVelocity()

◆ getMaxAccel()

◆ getMaxJerk()

◆ getMaxVel()

◆ getT1()

◆ getT2()

◆ getT3()

◆ getTotalTime()

◆ getVelSp()

◆ reset()

◆ saturateT1ForAccel()

◆ setCurrentAcceleration()

◆ setCurrentPosition()

◆ setCurrentVelocity()

◆ setMaxAccel()

◆ setMaxJerk()

◆ setMaxVel()

◆ timeSynchronization()

◆ updateDurations()

◆ updateDurationsGivenTotalTime()

◆ updateDurationsMinimizeTotalTime()

◆ updateTraj()

Member Data Documentation

◆ _direction

◆ _local_time

◆ _max_accel

◆ _max_jerk

◆ _max_vel

◆ _state

◆ _state_init

◆ _T1

◆ _T2

◆ _T3

◆ _vel_sp