utils¶

Bases: object

Added in version 1.3.0.

PIDController object.

Proportional-integral-derivative controller which computes the reference value, used to keep a variable under control with respect to a set value, based on current value and control parameters.

Methods¶

compute(): It computes the reference value based on current error.
reset(): It resets the stored cumulative error integral, used for integrative part, and the previous error, used for derivative part.

Raises

TypeError

If Kp is not a float or an int,
if Ki is not a float or an int,
if Kd is not a float or an int,
if clamping is not a bool,
if reference_min is not a float or an int,
if reference_max is not a float or an int.

ValueError

If reference_min is lower than reference_max.

compute(error: float | int, time_step: TimeInterval) → float | int

It computes the reference value based on current error.

Parameters¶

errorfloat or int: Current error.
time_stepTime: Time interval to be used for integrative and derivative parts.

Returns¶

float or int: Reference value to be used for control.

Raises

TypeError

If error is not a float or an int,
if time_step is not an instance of Time.

Notes

The reference value is computed as:

\[u(t) = K_P e(t) + K_I \int_0^t e(\tau) d \tau + K_D \frac{d e(t)}{d t}\]

where:

\(u(t)\) is the reference value,
\(e(t)\) is the current error,
\(K_P\) is the proportional constant Kp,
\(K_I\) is the integral constant Ki,
\(K_D\) is the derivative constant Kd.

The integral part is approximated with:

\[\int_0^t e(\tau) d \tau \approx \frac{1}{2}(e_i + e_{i - 1}) dt\]

and the derivative part is approximated with:

\[\frac{d e(t)}{d t} \approx \frac{e_i - e_{i - 1}}{dt}\]

where:

\(e_i\) is the current error,
\(e_{i - 1}\) is the error at the previous time step,
\(dt\) is the time_step.

If the reference value \(u(t)\) exceeds the limits reference_min or reference_max, then it is saturated to these values. In this case, if clamping is True, then the cumulative error is not updated (anti-windup).

reset() → None: It resets the stored cumulative error integral, used for integrative part, and the previous error, used for derivative part.

class SCurveTrajectory(start_position: AngularPosition, stop_position: AngularPosition, maximum_velocity: AngularSpeed, maximum_acceleration: AngularAcceleration, maximum_deceleration: AngularAcceleration, start_velocity: AngularSpeed | None = None, stop_velocity: AngularSpeed | None = None, start_time: Time | None = None)

Bases: object

Added in version 1.3.0.

SCurveTrajectory object.

It computes the S curve trajectory from the start_position to the stop_position. The trajectory is divided into three parts:

a constant acceleration part at maximum_acceleration,
a uniform velocity part at maximum_velocity,
a constant deceleration part at maximum_deceleration.

The starting conditions, at the start_time, are the start_position and the start_velocity; while the final conditions are the stop_position and stop_velocity.

The stop_position may also be lower than (but not equal to) the start_position for backward motion.

Methods¶

compute(): It computes the angular position at the given time instant, according to the S curve trajectory from the start_position to the stop_position.

Raises

TypeError

If start_position is not an instance of AngularPosition,
if stop_position is not an instance of AngularPosition,
if maximum_velocity is not an instance of AngularSpeed,
if maximum_acceleration is not an instance of AngularAcceleration,
if maximum_deceleration is not an instance of AngularAcceleration,
if start_velocity is not an instance of AngularSpeed,
if stop_velocity is not an instance of AngularSpeed,
if start_time is not an instance of Time.

ValueError

If start_position and stop_position are equal,
if maximum_velocity is not positive,
if maximum_acceleration is not positive,
if maximum_deceleration is not positive,
if start_velocity is geater than maximum_velocity,
if stop_velocity is greater than maximum_velocity.

compute(time: Time) → AngularPosition

It computes the angular position at the given time instant, according to the S curve trajectory from the start_position to the stop_position.

Parameters¶

timeTime: Specific instant of time in which to calculate the angular position in accordance with the S curve.

Returns¶

AngularPosition: Angular position according to S curve trajectory at the given time.

Raises

TypeError: If time is not an instance of Time.

Notes

The input parameter time \(t\) is compared to the S curve trajectory parameters, to establish the motion regime at that time instant. Five cases are be possibile:

\(t \lt t_0\): The time \(t\) instant preceeds the start_time \(t_0\). The motion regime is uniform velocity at start_velocity \(\dot{\theta}_0\). The angular position \(\theta\) is computed as:

\[\theta = \dot{\theta}_0 t_l + \theta_0\]

where \(t_l = t - t_0\).
\(t_0 \le t \lt t_0 + t_A\): The time \(t\) instant is greater than the start_time \(t_0\), but lower than the duration \(t_0 + t_A\). The motion regime is uniform acceleration at maximum_acceleration \(\ddot{\theta}_{A,max}\). The angular position \(\theta\) is computed as:

\[\theta = \frac{1}{2} \ddot{\theta}_{A,max} t^2_l + \dot{\theta}_0 t_l + \theta_0\]

where \(t_l = t - t_0\).
\(t_0 + t_A \le t \lt t_0 + t_A + t_U\) The time \(t\) instant is greater than the duration \(t_0 + t_A\), but lower than the duration \(t_0 + t_A + t_U\). The motion regime is uniform velocity at maximum_velocity \(\dot{\theta}_{max}\). The angular position \(\theta\) is computed as:

\[\theta = \dot{\theta}_{max} t_l + \theta_0 + \theta_A\]

where \(t_l = t - t_0 - t_A\).
\(t_0 + t_A + t_U \le t \lt t_1\) The time \(t\) instant is greater than the duration \(t_0 + t_A + t_U\), but lower than the duration \(t_1\). The motion regime is uniform deceleration at maximum_deceleration \(\ddot{\theta}_{D,max}\). The angular position \(\theta\) is computed as:

\[\theta = - \frac{1}{2} \ddot{\theta}_{D,max} t^2_l + \dot{\theta}_{max} t_l + \theta_0 + \theta_A + \theta_U\]

where \(t_l = t - t_0 - t_A - t_U\).
\(t \ge t_1\) The time \(t\) instant is greater than the duration \(t_1\). The motion regime is uniform velocity at stop_velocity \(\dot{\theta}_1\). The angular position \(\theta\) is computed as:

\[\theta = \dot{\theta}_1 t_l + \theta_1\]

where \(t_l = t - t_0 - t_A - t_U - t_D\).

In some cases, the difference between stop_position \(\theta_1\) and start_position \(\theta_0\) is small compared to the acceleration and deceleration distances \(\theta_A\) and \(\theta_D\). In that case, there is no room for uniform velocity regime at maximum_velocity \(\dot{\theta}_{max}\), so the step 3 is skipped and the maximum_velocity is not reached.

The acceleration duration \(t_A\) is computed as:

\[t_A = \frac{\dot{\theta}_{max} - \dot{\theta}_0}{\ddot{\theta}_{A,max}}\]

The uniform duration \(t_U\) is computed as:

\[t_U = \frac{\theta_U}{\dot{\theta}_{max}}\]

The deceleration duration \(t_D\) is computed as:

\[t_D = \frac{\dot{\theta}_{max} - \dot{\theta}_1}{\ddot{\theta}_{D,max}}\]

The acceleration distance \(\theta_A\) is computed as:

\[\theta_A = \frac{1}{2} \frac{\dot{\theta}_{max}^2 - \dot{\theta}_0^2}{\ddot{\theta}_{A,max}}\]

The uniform distance \(\theta_U\) is computed as:

\[\theta_U = | \theta_1 - \theta_0 | - \theta_A - \theta_D\]

The deceleration distance \(\theta_D\) is computed as:

\[\theta_D = \frac{1}{2} \frac{\dot{\theta}_{max}^2 - \dot{\theta}_1^2}{\ddot{\theta}_{D,max}}\]

The stop time is computed as:

\[t_1 = t_0 + t_A + t_U + t_D\]

Here the list of the parameters used in the above equations:

\(\theta\) is the angular position,
\(\dot{\theta}_{max}\) is the maximum_velocity,
\(\ddot{\theta}_{A,max}\) is the maximum_acceleration,
\(\ddot{\theta}_{D,max}\) is the maximum_deceleration,
\(\theta_0\) is the start_position,
\(\theta_1\) is the stop_position,
\(\dot{\theta}_0\) is the start_velocity,
\(\dot{\theta}_1\) stop_velocity,
\(t_0\) is the start_time.