Source code for triangula.chassis

from math import cos, sin, degrees, radians, pi
from time import time

from euclid import Vector2, Point2
from numpy import array as np_array
from numpy.linalg import solve as np_solve

__author__ = 'tom'

def test():
    chassis = HoloChassis(wheels=[
        HoloChassis.OmniWheel(position=Point2(1, 0), angle=0, radius=60),
        HoloChassis.OmniWheel(position=Point2(-1, 0), angle=0, radius=60)]
    print chassis.get_wheel_speeds(Motion(translation=Vector2(0, 0), rotation=0.5))
    print chassis.get_wheel_speeds(Motion(translation=Vector2(0, 0), rotation=0.5), origin=Point2(1, 0))

def rotate_point(point, angle, origin=None):
    Rotate a Point2 around another Point2

    :param euclid.Point2 point:
        The point to rotate
    :param float angle:
        Angle in radians, clockwise rotation
    :param euclid.Point2 origin:
        Origin of the rotation, defaults to (0,0) if not specified
        A new :class:`euclid.Point2` containing the rotated input point
    if origin is None:
        origin = Point2(0, 0)
    s = sin(-angle)
    c = cos(-angle)
    return Point2(c * (point.x - origin.x) - s * (point.y - origin.y) + origin.x,
                  s * (point.x - origin.x) + c * (point.y - origin.y) + origin.y)

def rotate_vector(vector, angle, origin=None):
    Rotate a :class:`euclid.Vector2` around a :class:`euclid.Point2`

    :param euclid.Vector2 vector:
        The vector to rotate
    :param float angle:
        Angle in radians, clockwise rotation
    :param euclid.Point2 origin:
        Origin of the rotation, defaults to (0,0) if not specified
        A new :class:`euclid.Point2` containing the rotated input point
    if origin is None:
        origin = Point2(0, 0)
    s = sin(-angle)
    c = cos(-angle)
    return Vector2(c * (vector.x - origin.x) - s * (vector.y - origin.y) + origin.x,
                   s * (vector.x - origin.x) + c * (vector.y - origin.y) + origin.y)

def smallest_difference(a, b, max_value=2 * pi):
    Given two floats, a and b, and a maximum possible value for both a and b, calculate the smallest delta from a to b.
    For example, if a=1.0, b=2.5 and max_value=2.6, this should return -1.1, as subtracting 1.1 from a would result in
    -0.1, which will then be transformed to 2.5 after taking its modulus with 2.6. If max_value was 10, it would return
    +1.5, as this is the lower magnitude delta needed to go from 1.0 to 2.5. This function is used when calculating the
    shortest delta between two pose orientations, for this reason the max_value defaults to 2*pi for use when working
    in radians.

    If either a or b are less than zero or greater than the maximum value they will be treated as a % max_value or b %
    max_value respectively for the purposes of this calculation.

    :param float a:
        First value (see above)
    :param b:
        Second value (see above)
    :param max_value:
        Modulus, defaults to 2*pi if not specified
        A value d such that (a + d) % max_value == b, and abs(d) is minimal (as there would be an infinite number of
        possible d that satisfy this relationship).
    mod_a = a % max_value
    mod_b = b % max_value
    if abs(mod_a - mod_b) <= max_value / 2:
        return mod_b - mod_a
    elif mod_a >= mod_b:
        return mod_b + (max_value - mod_a)
        return -(mod_a + (max_value - mod_b))

def get_regular_triangular_chassis(wheel_distance, wheel_radius, max_rotations_per_second):
    Build a HoloChassis object with three wheels, each identical in size and maximum speed. Each wheel is positioned
    at the corner of a regular triangle, and with direction perpendicular to the normal vector at that corner.

    :param wheel_distance:
        Distance in millimetres between the contact points of each pair of wheels (i.e. the length of each edge of the
        regular triangle)
    :param wheel_radius:
        Wheel radius in millimetres
    :param max_rotations_per_second:
        Maximum wheel speed in revolutions per second
        An appropriately configured HoloChassis
    point = Point2(0, cos(radians(30)) * wheel_distance / 2.0)
    vector = Vector2(-2 * pi * wheel_radius, 0)

    # Pink
    wheel_a = HoloChassis.OmniWheel(
    # Yellow
    wheel_b = HoloChassis.OmniWheel(
        position=rotate_point(point, pi * 2 / 3),
        vector=rotate_vector(vector, pi * 2 / 3),
    # Green
    wheel_c = HoloChassis.OmniWheel(
        position=rotate_point(point, pi * 4 / 3),
        vector=rotate_vector(vector, pi * 4 / 3),

    return HoloChassis(wheels=[wheel_a, wheel_b, wheel_c])

[docs]class WheelSpeeds: """ A simple container to hold desired wheel speeds, and to indicate whether any speeds were scaled back due to impossibly high values. """
[docs] def __init__(self, speeds, scaling): """ Create a new wheel speeds container :param speeds: A sequence of float values, one per wheel, in revolutions per second :param float scaling: If a requested translation or rotation was too fast for the chassis to perform, it will return an instance of this class with the scaling set to a value greater than 1.0. This indicates that it was unable to provide the requested trajectory but has instead provided the highest magnitude one possible. This parameter then contains the proportion of the requested trajectory that was possible to provide. For example, if the motion requested was a translation of 10mm/s in the X axis and a rotation of 10 radians per second, but on calculation this resulted in excessive wheel speeds which weren't possible, it might be scaled back to 6mm/s on X and 6 radians per second - the motion is proportionately the same just slower, and in this case the scaling value would be 0.6. """ self.speeds = speeds self.scaling = scaling
def __str__(self): return 'WheelSpeeds[ speeds={}, scaling={} ]'.format(self.speeds, self.scaling)
[docs]class Motion: """ A container to hold the translation and rotation vector representing the robot's motion. This is always expressed in the robot's coordinate frame, so a translation component of 0,1 always means the robot is heading forwards, irrespective of the current orientation of the robot (i.e. if the robot was turned 90 degrees in world space this 0,1 motion would be a movement along the X axis in world space, but the Y axis in robot space). The rotation component of the motion is expressed in radians per second, positive values corresponding to clockwise rotation when viewed from the direction relative to the plane such that X is positive to the right and Y positive upwards. """
[docs] def __init__(self, translation=None, rotation=0): """ Constructor :param euclid.Vector2 translation: Vector2 representing the translation component in robot coordinate space of the motion. Defaults to Vector2(0,0) :param float rotation: Rotation in radians per second. Defaults to 0. """ if translation is not None: self.translation = translation else: self.translation = Vector2(0, 0) self.rotation = rotation
def __str__(self): return 'Motion[ x={}, y={}, theta={} (deg={}) ]'.format(self.translation.x, self.translation.y, self.rotation, degrees(self.rotation))
[docs]class DeadReckoning: """ Encapsulates the logic required to track the robot's position in world space using wheel encoders and chassis kinematics. To update the state of this object you need to call the update_from_counts function - this will compute the difference in counts for each wheel, and from this derive the rotational speed for each wheel since the last measurement. The :class:`triangula.chassis.HoloChassis` is then used to convert these speeds into an arc, with the assumption that wheel speeds were constant during the time interval. This arc is used to update the :class:`triangula.chassis.Pose` representing the current best estimate of the robot's position. Because this is in effect integrating over sensor readings, any errors, particularly in the chassis geometry or dimensions, or in the number of counts per revolution (for example if the gearing isn't quite what you think it is or there's enough slop in the gearbox that readings can drift) will accumulate over time. To mitigate this, if you have precise instantaneous information such as a compass reading every few seconds, these readings can be used to explicitly set the position, orientation, or both of the :class:`triangula.chassis.Pose` tracked by this class. As there's an implicit assumption that wheel speeds are constant between encoder readings, this class will yield more accurate results when updated frequently. The exact optimal update frequency will depend on the encoder resolutions, chassis geometry etc. Some manual tuning may be required. """
[docs] def __init__(self, chassis, counts_per_revolution=64 * 19, max_count_value=1 << 15): """ Constructor :param triangula.chassis.HoloChassis chassis: The :class:`triangula.chassis.HoloChassis` to be used to define kinematics for this DeadReckoning :param float counts_per_revolution: The number of counts registered by the wheel encoders per revolution of the wheel. Defaults to 64*19 to be the 64 count encoder fitted to a 19:1 reduction gearbox. :param int max_count_value: The largest value read from the encoders, this is used to determine when we've wrapped around the zero point, defaults to 1<<16 to reflect that count values are held in the microcontroller module as a uint16_t """ self.chassis = chassis self.counts_per_revolution = counts_per_revolution self.max_count_value = max_count_value self.last_encoder_values = None self.last_reading_time = None self.pose = None
[docs] def reset(self): """ Clear the state of this :class:`triangula.chassis.DeadReckoning` """ self.last_encoder_values = None self.last_reading_time = None self.pose = None
[docs] def set_position(self, position): """ Explicitly set the position of the robot in world coordinates. Overrides the current value tracked by this instance. Use this when you have better information and want to update the state accordingly. :param euclid.Point2 position: The new position to set, as a :class:`euclid.Point2`, coordinates are in mm """ self.pose.position = position return self.pose
[docs] def set_orientation(self, orientation): """ Explicitly set the orientation of the robot in world coordinates. Use this to explicitly update the orientation, for example when you have a sufficiently accurate compass fix that it can be used to eliminate any accumulated errors built up by the dead reckoning algorithm. :param float orientation: The new orientation to set, in radians from the positive Y axis, clockwise rotations being positive. This value will be normalised to the range 0-2PI :return: The current (updated) value of the :class:`triangula.chassis.Pose` """ self.pose.orientation = orientation % (2 * pi) return self.pose
[docs] def update_from_counts(self, counts): """ Update the pose from a new set of encoder values :param counts: A list of encoder counts, one per wheel :return: The updated :class:`triangula.chassis.Pose` object (this is also modified in the internal state of the DeadReckoning) """ reading_time = time() if self.last_encoder_values is None: self.last_encoder_values = counts self.last_reading_time = reading_time self.pose = Pose(Point2(0, 0), 0) else: time_delta = reading_time - self.last_reading_time wheel_speeds = [smallest_difference(current_reading, last_reading, self.max_count_value) / ( self.counts_per_revolution * time_delta) for last_reading, current_reading in zip(counts, self.last_encoder_values)] motion = self.chassis.calculate_motion(speeds=wheel_speeds) self.pose = self.pose.calculate_pose_change(motion, time_delta) self.last_encoder_values = counts self.last_reading_time = reading_time return self.pose
[docs]class Pose: """ A container to hold the position as a Point2 along with orientation in radians, where 0 corresponds to the positive Y axis (0,1). Orientation is expressed in radians, with positive values indicating a rotation from the positive Y axis in the clockwise direction, i.e. a rotation of 0 is North, pi/2 East, pi South and 3pi/2 West. """
[docs] def __init__(self, position=None, orientation=0): """ Constructor :param euclid.Point2 position: A Point2 containing the position of the centre of the robot. Defaults to Point2(0,0) :param float orientation: Orientation in radians, 0 being the positive Y axis, positive values correspond to clockwise rotations, i.e. pi/4 is East. This value will be normalised to be between 0 and 2 * pi. Defaults to 0 """ if position is not None: self.position = position else: self.position = Point2(0, 0) self.orientation = orientation % (2 * pi)
[docs] def distance_to_pose(self, to_pose): """ Return the distance to the other pose position :param triangula.chassis.Pose to_pose: The target pose """ return abs(self.position - to_pose.position)
[docs] def is_close_to(self, to_pose, max_distance=0.001, max_orientation_difference=radians(1)): """ Check whether we're close to the specified pose, defining closeness as both distance on the plane and difference in orientation. :param to_pose: The target pose :param max_distance: Maximum distance within which we'll count as being close, defaults to 0.001 :param max_orientation_difference: Maximum number of radians we can be off the target pose's orientation to count as close, defaults to 1 degree (calculated with ``radians(1)``) :return: True if this pose is regarded as close to the other, False otherwise """ if self.distance_to_pose(to_pose) > max_distance: return False elif abs(smallest_difference(self.orientation, to_pose.orientation)) > max_orientation_difference: return False else: return True
[docs] def translate(self, vector): """ Create a new pose, with the same orientation as this one and the specified translation applied to its position. :param euclid.Vector2 vector: Vector by which the position of this pose should be translated when creating the new Pose :return: Returns the new Pose """ return Pose(position=self.position + vector, orientation=self.orientation)
[docs] def pose_to_pose_vector(self, to_pose): """ Calculates the Vector2, in robot coordinate space (remember that Pose objects use world coordinates!) that represents the translation required to move from this Pose to the specified target Pose. :param triangula.chassis.Pose to_pose: A target :class:`triangula.chassis.Pose`, the resultant vector in robot space will translate the robot to the position contained in this pose. Note that this does not take any account of the orientation component of the to_pose, only the starting one. :return: A :class:`euclid.Vector2` containing the translation part, in robot space, of the motion required to move from this Pose to the target. """ return rotate_vector( vector=Vector2(to_pose.position.x - self.position.x, to_pose.position.y - self.position.y), angle=-self.orientation)
[docs] def pose_to_pose_motion(self, to_pose, time_seconds): """ Calculates a Motion which should be applied to the current Pose to move the robot towards the target, such that it should hit the target at no less than time_seconds into the future. This function must be called on any Pose update, i.e. from a dead reckoning module, as it doesn't do any course planning (it would, for example, be possible to calculate a single constant motion to move in an arc to the target Pose, but this would be rather inefficient, better to incrementally home in on the target by repeatedly calling this function). To move as fast as possible to the target, set the time to something implausibly small, then use the chassis functions to limit the resultant motion to the range possible for the chassis. This would require some kind of motion limit to avoid skidding and messing up the Pose calculation logic. :param to_pose: A target :class:`triangula.chassis.Pose` :param time_seconds: A the minimum number of seconds to transition to the target pose. :return: A :class:`triangula.chassis.Motion` containing the motion required to attain the target pose in the specified time. This is highly likely to be impossible, in which case using the chassis functions to determine the wheel power and extract the scaling factor will give the actual time (ignoring acceleration limits) to transition to the target. """ translation = self.pose_to_pose_vector(to_pose=to_pose) rotation = smallest_difference(self.orientation, to_pose.orientation) return Motion(translation=translation / time_seconds, rotation=rotation / time_seconds)
[docs] def calculate_pose_change(self, motion, time_delta): """ Given this as the starting Pose, a Motion and a time in seconds, calculate the resultant Pose at the end of the time interval. This makes use of the fact that if you travel in a consistent direction while turning at a constant rate you will describe an arc. By calculating the centre point of this arc we can simply rotate the starting pose around this centre point. This is considerably simpler than integrating over the motion 3-vector. A special case is used to avoid division by zero errors when there is no rotation component to the motion. :param triangula.chassis.Motion motion: The motion of the robot, assumed to be constant for the duration of the time interval. The motion is expressed in the robot's coordinate frame, so a translation of (0,1) is always a forward motion, irrespective of the current orientation. :param float time_delta: The time in seconds during which the specified motion should be applied. :return: A :class:`triangula.chassis.Pose` which represents resultant pose after applying the supplied motion for the given time. """ # Total delta in orientation angle over the time interval orientation_delta = motion.rotation * time_delta # Scaled translation vector rotated into world coordinate space (motion uses robot space) translation_vector_world = rotate_vector(motion.translation, self.orientation) * time_delta ':type : euclid.Vector2' if orientation_delta == 0: # No orientation, trivially add the rotated, scaled, translation vector to the current pose return self.translate(translation_vector_world) else: centre_of_rotation = self.position + translation_vector_world.cross() / orientation_delta ':type : euclid.Point2' final_position = rotate_point(self.position, angle=orientation_delta, origin=centre_of_rotation) return Pose(position=final_position, orientation=self.orientation + orientation_delta)
def __str__(self): return 'Pose[x={}, y={}, orientation={} (deg={})]'.format(self.position.x, self.position.y, self.orientation, degrees(self.orientation))
[docs]class HoloChassis: """ An assembly of wheels at various positions and angles, which can be driven independently to create a holonomic drive system. A holonomic system is one where number of degrees of freedom in the system is equal to the number of directly controllable degrees of freedom, so for a chassis intended to move in two dimensions the degrees of freedom are two axes of translation and one of rotation. For a full holonomic system we therefore need at least three wheels defined. """
[docs] def __init__(self, wheels): """ Create a new chassis, specifying a set of wheels. :param wheels: A sequence of :class:`triangula.chassis.HoloChassis.OmniWheel` objects defining the wheels for this chassis. """ self.wheels = wheels self._matrix_coefficients = np_array([[wheel.co_x, wheel.co_y, wheel.co_theta] for wheel in self.wheels])
[docs] def calculate_motion(self, speeds): """ Invert the motion to speed calculation to obtain the actual linear and angular velocity of the chassis given a vector of wheel speeds. See :param speeds: An array of wheel speeds, expressed as floats with units of radians per second, positive being towards the wheel vector. :return: A :class:`triangula.chassis.Motion` object containing the calculated translation and rotation in the robot's coordinate space. """ motion_array = np_solve(self._matrix_coefficients, np_array(speeds)) return Motion(Vector2(x=float(motion_array[0]), y=float(motion_array[1])), rotation=float(motion_array[2]))
[docs] def get_max_translation_speed(self): """ Calculate the maximum translation speed, assuming all directions are equivalent and that there is no rotation component to the motion. :return: Maximum speed in millimetres per second as a float """ unrealistic_speed = 10000.0 scaling = self.get_wheel_speeds(Motion(translation=Vector2(0, unrealistic_speed), rotation=0)).scaling return unrealistic_speed * scaling
[docs] def get_max_rotation_speed(self): """ Calculate the maximum rotation speed around the origin in radians per second, assuming no translation motion at the same time. :return: Maximum radians per second as a float """ unrealistic_speed = 2 * pi * 100 scaling = self.get_wheel_speeds(Motion(translation=Vector2(0, 0), rotation=unrealistic_speed)).scaling return unrealistic_speed * scaling
[docs] def get_wheel_speeds(self, motion, origin=Point2(x=0, y=0)): """ Calculate speeds to drive each wheel in the chassis at to attain the specified rotation / translation 3-vector. :param triangula.chassis.Motion motion: Desired motion of the robot chassis :param euclid.Point2 origin: Optional, can define the centre of rotation to be something other than 0,0. Units are in millimetres. Defaults to rotating around x=0, y=0. :return: A :class:`triangula.chassis.WheelSpeeds` containing both the target wheel speeds and the scaling, if any, which was required to bring those speeds into the allowed range for all wheels. This prevents unexpected motion in cases where only a single wheel is being asked to turn too fast, in such cases all wheel speeds will be scaled back such that the highest is within the bounds allowed for that particular wheel. This can accommodate wheels with different top speeds. """ def velocity_at(point): """ Compute the velocity as a Vector2 at the specified point given the enclosing translation and rotation values Method: Normalise the vector from the origin to the point, then take the cross of itself to produce a unit vector with direction that of a rotation around the origin. Scale this by the distance from the origin and by the rotation in radians per second, then simply add the translation vector. :param euclid.Point2 point: Point at which to calculate velocity :return: A :class:`euclid.Vector2` representing the velocity at the specified point in mm/s """ d = point - origin return d.cross() * motion.rotation + motion.translation wheel_speeds = list(wheel.speed(velocity_at(wheel.position)) for wheel in self.wheels) scale = 1.0 for speed, wheel in zip(wheel_speeds, self.wheels): if wheel.max_speed is not None and abs(speed) > wheel.max_speed: wheel_scale = wheel.max_speed / abs(speed) scale = min(scale, wheel_scale) return WheelSpeeds(speeds=list(speed * scale for speed in wheel_speeds), scaling=scale)
[docs] class OmniWheel: """ Defines a single omni-wheel within a chassis assembly. Omni-wheels are wheels formed from rollers, where the motion of the roller is perpendicular to the motion of the primary wheel. This is distinct from a mechanum wheel where the rollers are at an angle (normally around 40-30 degrees) to the primary wheel. Omni-wheels must be positioned on the chassis with non-parallel unit vectors, mechanum wheels can in some cases be positioned with all unit vectors parallel. A wheel has a location relative to the chassis centre and a vector describing the direction of motion of the wheel when driven with a positive angular velocity. The location is specified in millimetres, and the magnitude of the wheel vector should be equal to the number of millimetres travelled in a single revolution. This allows for different sized wheels to be handled within the same chassis. """
[docs] def __init__(self, position, max_speed=0, angle=None, radius=None, vector=None): """ Create a new omni-wheel object, specifying the position and either a direction vector directly or the angle in degrees clockwise from the position Y axis along with the radius of the wheel. :param euclid.Point2 position: The wheel's contact point with the surface, specified relative to the centre of the chassis. Units are millimetres. :param float max_speed: The maximum number of revolutions per second allowed for this wheel. When calculating the wheel speeds required for a given trajectory this value is used to scale back all motion if any wheel would have to move at an impossible speed. If not specified this defaults to None, indicating that no speed limit should be placed on this wheel. :param angle: The angle, specified in radians from the positive Y axis where positive values are clockwise from this axis when viewed from above, of the direction of travel of the wheel when driven with a positive speed. If this value is specified then radius must also be specified and dx,dy left as None. :param radius: The radius in millimetres of the wheel, measuring from the centre to the contact point with the surface, this may be hard to determine for some wheels based on their geometry, particularly for wheels with cylindrical rollers, as the radius will vary. For these cases it may be worth directly measuring the circumference of the entire assembly and calculating radius rather than measuring directly. This is used to determine the magnitude of the direction vector. If this is not None then the angle must also be specified, and dx,dy left as None. :param euclid.Vector2 vector: 2 dimensional vector defining the translation of the wheel's contact point after a full revolution of the wheel. """ self.position = position self.max_speed = max_speed if angle is None and radius is None and vector is not None: # Specify wheel based on direct vector """ self.vector = vector elif angle is not None and radius is not None and vector is None: # Specify based on angle from positive Y axis and radius """ circumference = 2 * pi * radius self.vector = Vector2(sin(angle) * circumference, cos(angle) * circumference) else: raise ValueError('Must specify exactly one of angle and radius or translation vector') self.vector_magnitude_squared = self.vector.magnitude_squared() self.co_x = self.vector.x / self.vector_magnitude_squared self.co_y = self.vector.y / self.vector_magnitude_squared self.co_theta = (self.vector.x * self.position.y - self.vector.y * self.position.x) / self.vector_magnitude_squared
[docs] def speed(self, velocity): """ Given a velocity at a wheel contact point, calculate the speed in revolutions per second at which the wheel should be driven. Method: we want to find the projection of the velocity onto the vector representing the drive of this wheel. We store the vector representing a single revolution of travel as self.vector, so the projection onto this would be / abs(self.vector)). However, we want revolutions per second, so we must then divide again by abs(self.vector), leading to / abs(self.vector))/abs(self.vector). Because the definition of the dot product is the sum of x1*x2, y1*y2, ... any scalar applied to each x, y ... of a single vector can be moved outside the dot product, so we can simplify as / abs(self.vector)^2. As the magnitude of the vector is taken by sqrt(x^2+y^2) we can simply express this as (x^2+y^2), held in the convenient function magnitude_squared(). So our final simplified form is / self.vector.magnitude_squared(). For efficiency, and because self.vector doesn't change, we can pre-compute this. :param euclid.Vector2 velocity: The velocity at the wheel's contact point with the surface, expressed in mm/s :return: Target wheel speed in rotations per second to hit the desired vector at the contact point. """ return / self.vector_magnitude_squared