Trj_Interval (STRUCT) ¶ TYPE Trj_Interval : STRUCT Represents the information about an “interval of uncertainty”. That is, an interval dTau2_pos … dTau2_neg where a root of v_e(dTau2) is located. Properties: w_ini = w(0) w_pos = w(dTau1 + dTau2_pos + dTau3_pos) w_neg = w(dTau1 + dTau2_neg + dTau3_neg) w_off = w(dTau1 + dTau2_off) w_pos.dv >= 0 w_neg.dv <= 0 dTau2_pos <= dTau2_neg dTau2_off <= dTau2_pos InOut: Name Type Comment dTau1 LREAL The initial duration of maximum jerk. Equals the sum of the duration of the first two phases of tr_off. dTau2_off LREAL The offset (<= dTau2_pos) at which the state w_off is known. Must equal Trj_Duration(tr_off) - dTau1. dTau2_pos LREAL The lower bound of the interval. dTau2_neg LREAL The upper bound of the interval. w_ini DynState The initial state w_off DynState The state at time dTau1 + dTau2_offset. w_pos DynState The state at the end if dTau2_pos is used. w_neg DynState The state at the end if dTau2_neg is used. tr_off Trajectory The trajectory until dTau2_off. tr_rst Trajectory The trajectory from dTau2_off to dTau2_pos bRootFound BOOL Whether a root (trajectory with end velocity zero) has been found. dsActualTrajectoryEnd LREAL The actual end position of the trajectory if a root has been found. Might differ from w_pos.ds (up to eps.dEps_s) if the interval is a target trajectory, since w_pos.ds is set to the end of the segment. eps Trj_Epsilons The epsilon values used to compute the interval.

Trj_FailureInfo (STRUCT) ¶ TYPE Trj_FailureInfo : STRUCT Information about failures of the CP-Planner ( Trj_CP_Single ) InOut: Name Type Comment cause Trj_FailureCause The cause of the failure. error SM3_Error.SMC_ERROR The underlying error of the failure.

Trj_FindMinTau2 (FUN) ¶ FUNCTION Trj_FindMinTau2 : BOOL Determines the minimum time needed to reduce the acceleration of the initial state to zero, after a phase dTau1 of maximum jerk. The solved system has the following form: w_k+1 = w_k * A + b * j_k where the state w_k consists of path position, velocity and acceleration, and the jerk j_k is the input. The jerk j_k is defined by / j_hgh if dT < dTau1 j_k = | j_low otherwise where j_low and j_hgh are the minimum and maximum admissible jerk in state k. On success, the returned trajectory always has 4 phases. The first two with maximum jerk, the third and the fourth with minimum jerk. The termination criteria for the trajectory are: dT > dTau and a <= 0, or error in computation of jerk bounds (i.e. path exceeded, working space exceeded, …) InOut: Scope Name Type Inout errCtx SMRE.ErrorContext jerkBounds Trj_JerkBounds tr Trajectory timer SMRB.TimerHires impedingConstraint Constraint_Info Input wI DynState t0 LREAL dTs LREAL dTau1 LREAL dEps_v LREAL Return Trj_FindMinTau2 BOOL Output da_max LREAL

Trj_CP_Single (FB) ¶ FUNCTION_BLOCK FINAL Trj_CP_Single Computation of continuous path trajectories. This is the main function block for our CP (continuous path) algorithm. It will compute a trajectory up to a given distance on the path, assuming the path and its first two derivatives w.r.t. the arc length has no discontinuities. The main interface for this FB is Trj_JerkBounds, which is passed as a parameter to the Next method. For each state, consisting of the position on the path, the path velocity and the path acceleration, Trj_JerkBounds computes the maximum and minimum admissible path jerk. The resulting trajectory is stored in a queue of type Trj_SampleQueue. This queue is task-safe: a different task can read and remove values from this queue concurrently. Methods: CommitNewTrajectory NewSafeTrajectory NewTargetTrajectory Structure: CommitNewTrajectory (Method) NewSafeTrajectory (Method) NewTargetTrajectory (Method)

Trj_CP_Single.CommitNewTrajectory (METH) ¶ METHOD FINAL CommitNewTrajectory : BOOL InOut: Scope Name Type Inout trs Trj_State result Trj_PlanningResult errCtx SMRE.ErrorContext Inout Const iv_new Trj_Interval Input dTs LREAL Return CommitNewTrajectory BOOL

Trj_CP_Target.Compute (METH) ¶ METHOD FINAL Compute : BOOL InOut: Scope Name Type Inout errCtx SMRE.ErrorContext iv Trj_Interval jerkBounds Trj_JerkBounds timer SMRB.TimerHires result Trj_PlanningResult Inout Const tiv Trj_TargetInterval trsInit Trj_State Return Compute BOOL

Trj_Cycler (FB) ¶ FUNCTION_BLOCK FINAL Trj_Cycler InOut: Scope Name Type Comment Inout Const tr Trajectory Input bExecute BOOL bAbort BOOL dTs LREAL wI DynState eps Trj_Epsilons Output wCur DynState The resulting state after applying dJerkCur for time dTsCur to the last state dJerkCur LREAL The jerk applied for the current time interval dTCur LREAL The interval length for which dJerkCur is applied phaseType Trj_PhaseType bPlanningCycleComplete BOOL TRUE iff an entire cycle has passed bDone BOOL bError BOOL ErrorID SM3_Error.SMC_ERROR dbgData Trj_DebugData scheduleInfo Trj_Sample_ScheduleInfo

Trj_CP_Single.NewSafeTrajectory (METH) ¶ METHOD FINAL NewSafeTrajectory : BOOL Computes the next sample. InOut: Scope Name Type Comment Inout errCtx SMRE.ErrorContext iv_new Trj_Interval jerkBounds Trj_JerkBounds FB to compute maximum and minimum admissible jerk timer SMRB.TimerHires The time budget for the computation, already enabled result Trj_PlanningResult tiv Trj_TargetInterval plTime PlannerTime Input dTau1_I LREAL Inout Const trsInit Trj_State cfg Trj_PlanningConfig Return NewSafeTrajectory BOOL Output trjEnd LREAL trjDur LREAL

Trj_CP_Single.NewTargetTrajectory (METH) ¶ METHOD FINAL NewTargetTrajectory : BOOL Computes the next sample. InOut: Scope Name Type Comment Inout errCtx SMRE.ErrorContext iv_new Trj_Interval jerkBounds Trj_JerkBounds FB to compute maximum and minimum admissible jerk timer SMRB.TimerHires The time budget for the computation, already enabled result Trj_PlanningResult Inout Const tiv Trj_TargetInterval trsInit Trj_State cfg Trj_PlanningConfig Return NewTargetTrajectory BOOL

Trj_Epsilons (ALIAS) ¶ TYPE Trj_Epsilons : SM3_TrajectoryGeneration.Dyn_Epsilons