DiffGrid4_ND (STRUCT) ¶ TYPE DiffGrid4_ND EXTENDS DiffGrid4 : STRUCT DiffGrid with numerator/divisor representation InOut: Name Type Inherited from aI ARRAY [0..4] OF LREAL DiffGrid4 aII ARRAY [0..4] OF LREAL DiffGrid4 aIII ARRAY [0..4] OF LREAL DiffGrid4 divisorI LREAL divisorII LREAL divisorIII LREAL
DynState_IsConstant (FUN) ¶ FUNCTION DynState_IsConstant : BOOL InOut: Scope Name Type Inout Const w DynState Input j LREAL Return DynState_IsConstant BOOL
DynState_Serialize (FUN) ¶ FUNCTION DynState_Serialize : BOOL InOut: Scope Name Type Return DynState_Serialize BOOL Input stream SM0.IOutStream Inout Const w DynState
Trj_SetStartTime (FUN) ¶ FUNCTION Trj_SetStartTime Sets the start time of the trajectory InOut: Scope Name Type Inout tr Trajectory Input t0 LREAL
Trj_tEnd (FUN) ¶ FUNCTION Trj_tEnd : LREAL InOut: Scope Name Type Inout Const tr Trajectory Return Trj_tEnd LREAL
ClosestPowerOf2 (FUN) ¶ FUNCTION ClosestPowerOf2 : LREAL InOut: Scope Name Type Return ClosestPowerOf2 LREAL Input x LREAL
DiffGrid2 (STRUCT) ¶ TYPE DiffGrid2 : STRUCT InOut: Name Type aI ARRAY [0..2] OF LREAL aII ARRAY [0..2] OF LREAL
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_ImproveIV_Root (FB) ¶ FUNCTION_BLOCK FINAL Trj_ImproveIV_Root Improves an interval of uncertainty around the root of v_e until the root is found with the desired accuracy. InOut: Scope Name Type Comment Inout errCtx SMRE.ErrorContext jerkBounds Trj_JerkBounds jerkCacheTau2 Trj_JerkCache timer SMRB.TimerHires The timer, already enabled impedingConstraint Constraint_Info Inout Const iv_in Trj_Interval eps Trj_Epsilons Input dTs LREAL Output error BOOL iv_out Trj_Interval nIter DINT bAcceptable BOOL Whether the found solution is a root
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.
