from time import time as time_now
from triangula.chassis import Motion
[docs]class MotionLimit:
"""
Utility class to limit rate of motion changes, similar to the :class:`triangula.dynamics.RateLimit` but specialised
to limit rate of change of :class:`triangula.chassis.Motion` instances. It computes the necessary linear and angular
acceleration required to transition from the previously registered Motion to the new one, and scales between the two
Motions such that the limits aren't exceeded. Linear interpolation is used to map between the previous and new
motions. This works around the drawback with the :class:`triangula.dynamics.RateLimit` when applied to wheel speeds
where the resultant motion will incorporate a degree of rotation even when no rotations are included in either
motion.
"""
[docs] def __init__(self, linear_acceleration_limit, angular_acceleration_limit):
"""
Create a new instance configured with the specified limits.
:param float linear_acceleration_limit:
Maximum allowed linear acceleration in mm per second per second
:param angular_acceleration_limit:
Maximum allowed radial acceleration in radians per second per second
"""
self.linear_acceleration_limit = linear_acceleration_limit
self.angular_acceleration_limit = angular_acceleration_limit
self.last_motion = None
self.last_motion_time = None
[docs] def limit_and_return(self, motion):
"""
Apply limits to the requested motion based on the current state of the MotionLimit, returning the closest Motion
which complies with the specified limits.
:param triangula.chassis.Motion motion:
The requested :class:`triangula.chassis.Motion`,
:return:
The modified motion, or the supplied one if it complied with the limits
"""
now = time_now()
if self.last_motion is None:
self.last_motion = motion
self.last_motion_time = now
return motion
# Calculate the requested linear acceleration magnitude in mm/s/s to achieve the desired motion change
time_delta = now - self.last_motion_time
motion_delta = abs(motion.translation - self.last_motion.translation)
linear_acceleration = motion_delta / time_delta
angular_acceleration = abs(motion.rotation - self.last_motion.rotation) / time_delta
scaling = 1.0
if linear_acceleration > self.linear_acceleration_limit:
scaling = self.linear_acceleration_limit / linear_acceleration
if angular_acceleration > self.angular_acceleration_limit:
scaling = min(scaling, self.angular_acceleration_limit / angular_acceleration)
scaled_translation = motion.translation * scaling + self.last_motion.translation * (1.0 - scaling)
':type : euclid.Vector2'
scaled_rotation = motion.rotation * scaling + self.last_motion.rotation * (1.0 - scaling)
scaled_motion = Motion(rotation=scaled_rotation, translation=scaled_translation)
self.last_motion = scaled_motion
self.last_motion_time = now
return scaled_motion
[docs]class RateLimit:
"""
Utility class to provide time-based rate limiting.
"""
[docs] def __init__(self, limit_function=None):
"""
Create a new rate limit object, this can be used to enforce maximum rates of change in a set of values, these
are generally expressed in rate per second, but could use any arbitrary function. For a default application the
provided function which can handle rate per second should be sufficient, but the ability to provide a custom
function allows for e.g. permitting larger rates when decreasing. This can be used in conjunction with the motor
power functions to provide a degree of traction control (we can't actually detect slipping in wheels, we just
don't have sufficient information), by limiting the maximum requested power rate change over time.
:param limit_function:
A function of old_value * old_time * new_value * new_time which will return a potentially limited new value
given a requested value, historical value and timepoints for both.
"""
self.previous_values = None
self.previous_time = None
self.limit_function = limit_function
[docs] def limit_and_return(self, values):
"""
Take a list of values, update the internal state of the RateLimit and return a modified list of values which are
restricted by the configured limit function.
:param float[] values:
Values to attempt to apply
:return:
New values to apply, modified by the configured limit function
"""
now = time_now()
if self.previous_time is None:
self.previous_time = now
self.previous_values = values
return values
updated_values = [self.limit_function(previous_value, self.previous_time, value, now) for
(previous_value, value) in
zip(self.previous_values, values)]
self.previous_values = updated_values
self.previous_time = now
return updated_values
@staticmethod
[docs] def fixed_rate_limit_function(rate_per_second):
"""
Create and return a new limit function which will lock the maximum delta applied to the specified rate per
second.
:param float rate_per_second:
Largest allowed delta per second
:return:
A function which can be used in the :class:`triangula.dynamics.RateLimit` constructor and which will enforce
a fixed maximum absolute rate of change across successive values such that no value in the supplied vector
will vary at a higher rate than provided.
"""
def limit_function(previous_value, previous_time, new_value, new_time):
time_delta = new_time - previous_time
value_delta = abs(previous_value - new_value)
requested_rate = value_delta / time_delta
if requested_rate <= rate_per_second:
return new_value
else:
return previous_value + ((new_value - previous_value) * rate_per_second / requested_rate)
return limit_function