#include <TrajGen.h>

Static Public Member Functions
static Triple< double, double, double >	getStoredParameters (const Plan &p)

static void	addParams (Plan &p, double bankAngle, double gsAccel, double vsAccel)

static bool	turnCanBeInscribed (const Plan &lpc, int i, const Position &BOT, const Position &EOT)

static Plan	markVsChanges (const Plan &lpc)

static Plan	generateTurnTCPs (const Plan &kpc, double bankAngle)

static Plan	generateTurnTCPs (const Plan &lpc, double default_bank_angle, bool continueGen)

static void	generateTurnTcpAt (Plan &traj, int ixNp2, double default_bankAngle, bool continueGen)

static void	generateTurnTcp (Plan &traj, int ixNp2, double R, bool bankUsed, bool continueGen)

static double	neededTurnLegDistance (const Plan &p, int ix, double radius)

static double	neededLegLengthBankAngle (const Plan &p, int ix, double bankAngle)

static std::pair< int, double >	movePointsWithinTurn (Plan &traj, int ixBOT, int ixNp2, int ixEOT, double gsIn)

static Plan	generateTurnTCPsRadius (const Plan &lpc)

static Plan	generateTurnTCPsRadius (const Plan &lpc, bool continueGen)

static void	generateGsTCPsAt (Plan &traj, int i, double gsAccel, bool repairGs)

static bool	adjustGsInsideAccel (Plan &traj, int ixBGS, double aBGS, double distToEGS)

static int	generateGsToZero (Plan &traj, int i, double a, double targetGs)

static void	adjustGsInsideAccelToEGS (Plan &traj, int ixBGS, double aBGS, int ixEGS)

static bool	attemptRepair2 (Plan &kpc, int ixBGS, int ixEGS, double maxGsAccel)

static TcpData	makeBGS (const TcpData &tcp1, double a, bool isAltPreserve)

static TcpData	makeEGS ()

static Plan	generateGsTCPs (const Plan &fp, double maxGsAccel, bool repairGs)

static void	generateGsTCP (Plan &traj, int i, double maxGsAccel, bool repairGs)

static Plan	generateVsTCPs (const Plan &kpc, double maxVsAccel, bool continueGen, bool repairVs_zeroGs)

static Plan	generateVsTCPs (const Plan &kpc, double maxVsAccel)

static int	generateVsTCP (Plan &traj, int i, double vsAccel, bool continueGen, bool repairVs_zeroGs)

static bool	fixAlts (Plan &traj, int bindex, int eindex, double vs1, double accel)

static double	calcRadius (const Plan &p, int ix, double bank)

static Plan	linearRepairShortTurnLegs (const Plan &fp, double bank, bool addMiddle)

static Plan	makeMarkedVsConstant (const Plan &kpc)

static Plan	repairPlan (const Plan &lpc, bool repairTurn, bool repairVs, double bankAngle, double vsAccel)

static Plan	makeKinematicPlan (const Plan &fp, double bankAngle, double gsAccel, double vsAccel, bool repairTurn, bool repairVs, bool repairGs)

static Plan	makeKinematicPlan (const Plan &fp, double bankAngle, double gsAccel, double vsAccel)

static Plan	makeVsKinematicPlan (const Plan &p, double vsAccel)

static Plan	genDirect2 (const Plan &lpc, const Position &so, const Velocity &vo, double to, double bankAngle)

static Plan	genDirectToLinear (const Plan &fp, const Position &so, const Velocity &vo, double to, double bankAngle, double timeBeforeTurn)

static Plan	buildDirectTo (const NavPoint &s, const Velocity &v, const Plan &base, double bankAngle)

static Plan	makeLinearPlan (const Plan &fp)

static void	setVerbose (bool b)

static void	printError (const std::string &s)

Static Public Attributes
static const double	MIN_MARK_LEG_TIME = Units::from("s",60)

static const double	minorVfactor = 0.01

static const double	maxVs = Units::from("fpm",10000)

static const double	maxAlt = Units::from("ft",60000)

static const std::string	turnFail = "<TrajGen.turnGenError>"

static const std::string	gsFail = "<TrajGen.gsGenError>"

static const std::string	vsFail = "<TrajGen.vsGenError>"

Static Private Member Functions
static double	directToContinueTime (const Plan &lpc, const Position &so, const Velocity &vo, double to, double bankAngle)

Static Private Attributes
static bool	verbose = false

Detailed Description

Trajectory generation functionality. This class translates Linear plans into Kinematic plans. The translation back to a Linear plan from a Kinematic plan is accomplished using reversion methods defined in PlanUtil (See PlanUtil.revertAllTCPS).

Note: It is very easy to create a linear plan that is infeasible, that is, it contains a segment where there is insufficient time or distance to achieve the acceleration implicit in it. If TrajGen.makeKinematicPlan fails to generate a Kinematic plan, an error message is added to the plan. There are "repair" methods available that modify the linear plan that often turn it into a feasible plan (See PlanUtil.repairShortVsLegs, PlanUtil.fixBadTurns)

Member Function Documentation

◆ addParams()

void larcfm::TrajGen::addParams	(	Plan &	p,
		double	bankAngle,
		double	gsAccel,
		double	vsAccel
	)

static

Store accel magnitude values used to generate this kinematic plan in a plan's note field. Negative values will be ignored.

Parameters

p	plan to modify
bankAngle	bank angle in radians, negative for none
gsAccel	gs accel in m/s, negative for none
vsAccel	vs accel in m/s, negative for none

Store accel values in a plan's note field

Parameters

p	plan to modify
bankAngle	bank angle in radians, negative for none
gsAccel	gs accel in m/s, negative for none
vsAccel	vs accel in m/s, negative for none

◆ adjustGsInsideAccel()

bool larcfm::TrajGen::adjustGsInsideAccel	(	Plan &	traj,
		int	ixBGS,
		double	aBGS,
		double	distToEGS
	)

static

Adjust ground speed of all points that will fall within distance to new EGS (distToEGS) This is necessary because points between a newly added BGS – EGS region will have time based on the previous non-accelerated section.

Parameters

traj	the plan file of interest
ixBGS	index of BGS (that has just been added, but EGS has not yet been added!)
aBGS	signed acceleration value
distToEGS	distance from BGS to new EGS point

Note: It is necessary to adjust by distance and not time. The times will change, so the EGS should added after this function is called.

remove all points that will fall within distance to new EGS (distToEGS)

Parameters

traj	the plan file of interest
ixBGS	index of BGS
aBGS	acceleration value
distToEGS	distance from BGS to new EGS point

◆ adjustGsInsideAccelToEGS()

void larcfm::TrajGen::adjustGsInsideAccelToEGS	(	Plan &	traj,
		int	ixBGS,
		double	gsAccel,
		int	ixEGS
	)

static

Adjust ground speed of all points that will fall within distance to new EGS (distToEGS)

Parameters

traj	the plan file of interest
ixBGS	index of BGS
aBGS	signed acceleration value
distToEGS	distance from BGS to new EGS point

Note: this function used where EGS has already been added.

◆ buildDirectTo()

Plan larcfm::TrajGen::buildDirectTo	(	const NavPoint &	s,
		const Velocity &	v,
		const Plan &	base,
		double	bankAngle
	)

static

Construct a (hopefully) feasible linear plan that describes a path from s to the goal point. If this fails, it reurns a simple direct plan.

◆ calcRadius()

double larcfm::TrajGen::calcRadius	(	const Plan &	p,
		int	ix,
		double	bank
	)

static

calculate radius at vertex ix, If a radius has been set use it, otherwise use bank angle and GS to calculate value

Parameters

p	Plan
ix	index of vertex point
bank	bank angle

Returns

◆ directToContinueTime()

double larcfm::TrajGen::directToContinueTime	(	const Plan &	lpc,
		const Position &	so,
		const Velocity &	vo,
		double	to,
		double	bankAngle
	)

staticprivate

Attempts to compute (heuristic) the amount of time to continue current velocity before starting direct to turn. (in 2-D)

Parameters

lpc	Plan trying to connect to at point zero.
so	current position
vo	current velocity
to	current time
bankAngle	max bank angle

Returns

◆ fixAlts()

bool larcfm::TrajGen::fixAlts	(	Plan &	traj,
		int	bindex,
		int	eindex,
		double	vs1,
		double	accel
	)

static

This method makes all of the vertical speed out of bindex to be vs1. It also smoothes the velocity profile according to the "accel" parameter.

Note: this is essential to making the BVS-EVS pair consistent

Parameters

traj	trajectory of interest
bindex	start of smoothing region
eindex	end of smoothing region
vs1	velocity to use for smoothing
accel	acceleration in this BGS EGS region

Returns: true, if successful

◆ genDirect2()

Plan larcfm::TrajGen::genDirect2	(	const Plan &	pc,
		const Position &	so,
		const Velocity &	vo,
		double	to,
		double	bankAngle
	)

static

*** UNUSED ***

Parameters

lpc	a linear plan that needs to be connected to, preferable at point 0
so	current position
vo	current velocity
to	current time
bankAngle	turn bank angle

Returns: new plan with (so,vo,to) added as a first point and a leadIn point added

Parameters

lpc	a linear plan that needs to be connected to, preferable at point 0
so	current position
vo	current velocity
to	current time
bankAngle	turn bank angle

Returns: new plan with (so,vo,to) added as a first point and a leadIn point added

◆ genDirectToLinear()

Plan larcfm::TrajGen::genDirectToLinear	(	const Plan &	fp,
		const Position &	so,
		const Velocity &	vo,
		double	to,
		double	bankAngle,
		double	timeBeforeTurn
	)

static

Constructs a new linear plan that connects current state to existing linear plan lpc (in 2-D).

Parameters

fp	a linear plan that needs to be connected to at point 0
so	current position
vo	current velocity
to	current time
bankAngle	turn bank angle
timeBeforeTurn	delay time before continuing turn. (can call directToContinueTime for an estimate)

Returns: new linear plan with (so,vo,to) added as a first point and a leadIn point added If we cannot connect to point zero of lpc this returns a plan with its error status set. The resulting plan may be infeasible from a kinematic view.

Parameters

lpc	a linear plan that needs to be connected to, preferable at point 0
so	current position
vo	current velocity
to	current time
bankAngle	turn bank angle

Returns: new plan with (so,vo,to) added as a first point and a leadIn point added

◆ generateGsTCPs()

Plan larcfm::TrajGen::generateGsTCPs	(	const Plan &	fp,
		double	maxGsAccel,
		bool	repairGs
	)

static

Generates ground speed TCPs (in forward direction)

Parameters

fp	plan to be processed
maxGsAccel	ground speed acceleration (non-negative)
repairGs	if segment is too short, repair by skipping over it (i.e. achieve max possible accel)

Returns: plan with BGS-EGS pairs added

◆ generateGsTCPsAt()

void larcfm::TrajGen::generateGsTCPsAt	(	Plan &	traj,
		int	i,
		double	gsAccel,
		bool	repairGs
	)

static

Create a BGS-EGS pair starting at index "i". The speed into and out of EGS will be "targetGs" Note: targetGS is often chosen to be gsOut(i+1) so that there is continuity at "i+1" after this method is called at "i"

Parameters

traj	trajectory
i	index where BGS is created
gsAccel	unsigned acceleration value
targetGs	target ground speed (usually gsOut(i+1) – calculated earlier)
timeOffset	create BGS this much time after time(i), it can be zero

Note: This method will skip over points until the targetGs is obtained.

◆ generateGsToZero()

int larcfm::TrajGen::generateGsToZero	(	Plan &	traj,
		int	i,
		double	gsAccel,
		double	targetGs
	)

static

generate an BGS-EGS pair with the EGS at point i

Parameters

traj	trajectory
i	point with GSOut = 0
a	absolute acceleration
targetGs	should be zero are almost zero

Returns: index where BGS is located

◆ generateTurnTcp()

void larcfm::TrajGen::generateTurnTcp	(	Plan &	traj,
		int	ixNp2,
		double	R,
		bool	bankUsed,
		bool	continueGen
	)

static

Generate turn at vertex "ixNP2" with radius R

Parameters

traj	trajectory under construction
ixNp2	index of np2 in plan
R	unsigned radius
bankUsed	true if default bank angle was used to compute R
continueGen	continue generation, even with problems

◆ generateTurnTcpAt()

void larcfm::TrajGen::generateTurnTcpAt	(	Plan &	traj,
		int	ixNp2,
		double	default_bankAngle,
		bool	continueGen
	)

static

Generate turn at vertex "ixNP2". If there is a stored radius use that value, otherwise compute radius from default_bankAngle and speed

Parameters

traj	traj
ixNp2	turn vertex – should not be in a turn already
dafault_bankAngle	default banks angle used to compute radius
continueGen	if true continue turn generation in the presence of errors

◆ generateTurnTCPs()

Plan larcfm::TrajGen::generateTurnTCPs	(	const Plan &	lpc,
		double	default_bank_angle,
		bool	continueGen
	)

static

Kinematic generator that adds turn TCPs. This defers ground speed changes until after the turn. It assumes legs are long enough to support the turns. bank angle must be nonnegative!!!!!

Parameters

lpc	linear plan
default_bank_angle	the default bank angle, if a radius is not present in the plan.
continueGen	if true, do not terminate generation if an error message is produced

Returns: a turn plan with BOTs and EOTs

◆ generateTurnTCPsRadius() [1/2]

Plan larcfm::TrajGen::generateTurnTCPsRadius ( const Plan & lpc )

static

Kinematic generator that adds turn TCPs. This defers ground speed changes until after the turn. It assumes legs are long enough to support the turns.

Parameters

lpc	linear plan

Returns: a plan with BOTs and EOTs

◆ generateTurnTCPsRadius() [2/2]

Plan larcfm::TrajGen::generateTurnTCPsRadius	(	const Plan &	lpc,
		bool	continueGen
	)

static

Kinematic generator that adds turn TCPs. This defers ground speed changes until after the turn. It assumes legs are long enough to support the turns.

Parameters

lpc	linear plan
continueGen	continue generation, even with problems

Returns: a plan with BOTs and EOTs

◆ generateVsTCP()

int larcfm::TrajGen::generateVsTCP	(	Plan &	traj,
		int	i,
		double	vsAccel,
		bool	continueGen,
		bool	repairVs_zeroGs
	)

static

Generate a BVS EVS pair at index i if deltaVs is large enough

Parameters

traj	trajectory
i	index
vsAccel	vertical acceleration
continueGen	if true continue generation even if errors are encountered, results in a partial kinematic plan

Returns: the index of the EVS if successful, otherwise just i

t1 = previous point's time (previous EVS) t2 = np2.time() = end time of the linear segment where the vertical speed change occurred nextVsChangeTm = time of the next vertical change (i.e. where the next TCP will appear) vs1 vertical speed into point at time t2 vs2 vertical speed out of point at time t2

                        vs1                              vs2
        t1  -------------------------------- t2 ------------------------------ nextVsChangeTm
                                       ^     "i"   ^
                                       |           |
                                     tbegin       tend

◆ generateVsTCPs()

Plan larcfm::TrajGen::generateVsTCPs	(	const Plan &	kpc,
		double	maxVsAccel,
		bool	continueGen,
		bool	repairVs_zeroGs
	)

static

Generate Vertical acceleration TCPs It assumes that all horizontal passes have been completed.

Parameters

kpc	kinematic plan with final horizontal path
maxVsAccel	vertical speed acceleration (positive)
continueGen	true means continue generation even with problems

Returns: plan

Generate Vertical acceleration TCPs It assumes that all horizontal passes have been completed.

Parameters

kpc	kinematic plan with final horizontal path
maxVsAccel	vertical speed acceleration (positive)

Returns

◆ linearRepairShortTurnLegs()

Plan larcfm::TrajGen::linearRepairShortTurnLegs	(	const Plan &	fp,
		double	bank,
		bool	addMiddle
	)

static

Repair function for a linear plan This can eliminate short legs. This function look for two sequential turns with a very short leg segment between them. It collapses the two turns into one or no turns

Parameters

fp	linear plan to be repaired
bank	bank angle
addMiddle	if true, it adds a middle point when it deletes two points (one turn left)

Returns: repaired Plan

Note: if addMiddle is false, point info and named waypoint may be lost

◆ makeBGS()

TcpData larcfm::TrajGen::makeBGS	(	const TcpData &	tcp1,
		double	a,
		bool	isAltPreserve
	)

static

Create a BGS point from a previous point (that is using its TCP info in tcp1)

Parameters

tcp1
a
timeOffset
isAltPreserve

Returns

◆ makeKinematicPlan() [1/2]

Plan larcfm::TrajGen::makeKinematicPlan	(	const Plan &	fp,
		double	bankAngle,
		double	gsAccel,
		double	vsAccel
	)

static

The resulting PlanCore will be "clean" in that it will have all original points, with no history of deleted points. Also all TCPs should reference points in a feasible plan. If the trajectory is modified, it will have added, modified, or deleted points. If the conversion fails, the resulting plan will have one or more error messages (and may have a point labeled as "TCP_generation_failure_point").

Parameters

fp	input plan (is linearized if not already so)
bankAngle	maximum allowed (and default) bank angle for turns
gsAccel	maximum allowed (and default) ground speed acceleration (m/s^2)
vsAccel	maximum allowed (and default) vertical speed acceleration (m/s^2)

Returns: plan

◆ makeKinematicPlan() [2/2]

Plan larcfm::TrajGen::makeKinematicPlan	(	const Plan &	fp,
		double	bankAngleIn,
		double	gsAccelIn,
		double	vsAccelIn,
		bool	repairTurn,
		bool	repairGs,
		bool	repairVs
	)

static

TODO (helps me find this method)

The resulting Plan will be "clean" in that it will have all original points, with no history of deleted points. Also all TCPs should reference points in a feasible plan. If the trajectory is modified, it will have added, modified, or deleted points. If the conversion fails, the resulting plan will have one or more error messages (and may have a point labeled as "TCP_generation_failure_point"). Note: This method seeks to preserve ground speeds of legs. The time at a waypoint is assumed to be not important compared to the original ground speed. Note. If a turn vertex NavPoint has a non-zero "radius", then that value is used rather than "bankAngle"

Parameters

fp	input plan (is linearized if not already so)
bankAngle	maximum allowed (and default) bank angle for turns
gsAccel	maximum allowed (and default) ground speed acceleration (m/s^2)
vsAccel	maximum allowed (and default) vertical speed acceleration (m/s^2)
repairTurn	attempt to repair infeasible turns as a preprocessing step
repairGs	attempt to repair infeasible ground speed accelerations
repairVs	attempt to repair infeasible vertical speed accelerations as a preprocessing step

Returns: the resulting kinematic plan

TODO

The resulting PlanCore will be "clean" in that it will have all original points, with no history of deleted points. Also all TCPs should reference points in a feasible plan. If the trajectory is modified, it will have added, modified, or deleted points. If the conversion fails, the resulting plan will have one or more error messages (and may have a point labeled as "TCP_generation_failure_point").

Parameters

fp	input plan (is linearized if not already so)
bankAngle	maximum allowed (and default) bank angle for turns
gsAccel	maximum allowed (and default) ground speed acceleration (m/s^2)
vsAccel	maximum allowed (and default) vertical speed acceleration (m/s^2)
minVsChangeRecognized	minimum vs change that will register as "non-constant" (m/s)
repairTurn	attempt to repair infeasible turns as a preprocessing step
repairGs	attempt to repair infeasible gs accelerations as a preprocessing step
repairVs	attempt to repair infeasible vs accelerations as a preprocessing step
constantGS	if true, produces a constant ground speed kinematic plan with gs being average of linear plan

◆ makeLinearPlan()

Plan larcfm::TrajGen::makeLinearPlan ( const Plan & fp )

static

Returns a PlanCore version of the given plan. If it was a kinematic plan, this will attempt to regress the TCPs to their original source points (if the proper metadata is available). If this is already a PlanCore, return a copy.

Parameters

fp plan

Returns: plan

Returns a PlanCore version of the given plan. If it was a kinematic plan, this will attempt to regress the TCPs to their original source points (if the proper metadata is available). If this is already a PlanCore, return a copy.

◆ makeMarkedVsConstant()

Plan larcfm::TrajGen::makeMarkedVsConstant ( const Plan & kpc )

static

Penultimate Kinematic generation pass. Repairs altitudes so that only VSC points have vertical speed changes. Averages vertical speeds between alt preserved points by modifying altitudes on points between them.

Parameters

kpc	kinematic plan

Returns: plan

◆ makeVsKinematicPlan()

Plan larcfm::TrajGen::makeVsKinematicPlan	(	const Plan &	p,
		double	vsAccel
	)

static

This assumes a plan that has altitude preserve points marked. Generate the vertical profile (only) for VsTCPs in this plan.

Parameters

p	input plan (need not be linear)
vsAccel	vertical acceleration

Returns: plan with vertical TCPs generated

◆ markVsChanges()

Plan larcfm::TrajGen::markVsChanges ( const Plan & lpc )

static

This takes a Plan lpc and returns a plan that has all points with vertical changes as "AltPreserve" points, with all other points being "Original" points. Beginning and end points are always marked.

Parameters

lpc	source plan
vsAccel	vertical acceleration

Returns: kinematic plan with marked points

This takes a plancore lpc and returns a kinematic plan core that has all points with vertical changes as "AltPreserve" points, with all other points being "Original" points. Beginning and end points are always marked.

Parameters

lpc	source plan

Returns: kinematic plan with marked points

◆ movePointsWithinTurn()

std::pair< int, double > larcfm::TrajGen::movePointsWithinTurn	(	Plan &	traj,
		int	ixBOT,
		int	ixNp2,
		int	ixEOT,
		double	gsIn
	)

static

Re-aligns points that are within a newly generated turn at j

◆ neededTurnLegDistance()

double larcfm::TrajGen::neededTurnLegDistance	(	const Plan &	p,
		int	ix,
		double	radius
	)

static

Calculates the minimum leg distance that can handle a specified radius

Parameters

p	the Plan (i.e. trajectory)
ix	vertex index
radius	radius of turn

Returns: minimum leg distance, return 0 if no turn would be generated here

Parameters

p	the Plan (i.e. trajectory)
ix	vertex index
radius	radius of turn

Returns: minimum leg distance, return 0 if no turn would be generated here

◆ repairPlan()

Plan larcfm::TrajGen::repairPlan	(	const Plan &	lpc,
		bool	repairTurn,
		bool	repairVs,
		double	bankAngle,
		double	vsAccel
	)

static

Method to repair an infeasible linear plan where there are segments that are too short to achieve specified acceleration.

Parameters

lpc	linear plan
repairTurn	true, to attempt repair of infeasible turns
repairVs	true, to attempt repair of infeasible vertical speed segments
bankAngle	bank angle
vsAccel	vertical speed acceleration

Returns: repaired plan

◆ turnCanBeInscribed()

bool larcfm::TrajGen::turnCanBeInscribed	(	const Plan &	lpc,
		int	i,
		const Position &	BOT,
		const Position &	EOT
	)

static

Returns true if turn at i can be inscribed in the available leg space. Note that it will check for collinear points using prevTrackChange and nextTrackChange

Parameters

lpc	source plan
i	vertex to be tested
BOT	Beginning of turn
EOT	End of turn

Returns: true iff the BOT - EOT pair can be placed within vertex

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

Modules/ACCoRD/inc/TrajGen.h
Modules/ACCoRD/src/TrajGen.cpp

Static Public Member Functions

Static Public Attributes

Static Private Member Functions

Static Private Attributes

Detailed Description

Member Function Documentation

◆ addParams()

◆ adjustGsInsideAccel()

◆ adjustGsInsideAccelToEGS()

◆ buildDirectTo()

◆ calcRadius()

◆ directToContinueTime()

◆ fixAlts()

◆ genDirect2()

◆ genDirectToLinear()

◆ generateGsTCPs()

◆ generateGsTCPsAt()

◆ generateGsToZero()

◆ generateTurnTcp()

◆ generateTurnTcpAt()

◆ generateTurnTCPs()

◆ generateTurnTCPsRadius() [1/2]

◆ generateTurnTCPsRadius() [2/2]

◆ generateVsTCP()

◆ generateVsTCPs()

◆ linearRepairShortTurnLegs()

◆ makeBGS()

◆ makeKinematicPlan() [1/2]

◆ makeKinematicPlan() [2/2]

◆ makeLinearPlan()

◆ makeMarkedVsConstant()

◆ makeVsKinematicPlan()

◆ markVsChanges()

◆ movePointsWithinTurn()

◆ neededTurnLegDistance()

◆ repairPlan()

◆ turnCanBeInscribed()