triangula.chassis: Kinematics¶

Basics¶

Before we can do anything clever we have to define two concepts:

Pose

The position of the robot. This consists of the location on a 2-dimensional plane of the robot’s centre point, and an orientation. The coordinates here are in ‘world space’, that is to say they’re relative to some fixed point in the world in which the robot is moving. The triangula.chassis.Pose class represents a pose.

Motion

The robot’s speed both in translation (a 2-dimensional vector representing movement across the plane) and rotation (around a centre point). These are expressed in ‘robot space’, so all motions are relative to the robot’s chassis. This means that a motion with a translation component of [0,1] is always ‘forwards’ as far as the robot is concerned. The triangula.chassis.Motion class represents a motion.

Because this library is dealing with small scale robots, I use millimetres as my distance dimension. This isn’t actually codified anywhere, so as long as you’re entirely consistent you could use metres, yards, furlongs or whatever, it only matters that you always use the same unit whenever either distance or speed is required.

Angular units, rotation speeds and orientations, are always in radians. This is extremely important, if you find radians hard to grapple with you can use the functions ‘math.radians’ and ‘math.degrees’ to convert degrees to radians and radians to degrees respectively.

Time units are always seconds, although again you could use something different as long as you’re entirely consistent. This applies to time and, consequently, speeds and velocities.

class triangula.chassis.Pose(position=None, orientation=0)[source]¶

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.

__init__(position=None, orientation=0)[source]¶

Constructor

Parameters:	position (euclid.Point2) – A Point2 containing the position of the centre of the robot. Defaults to Point2(0,0) orientation (float) – 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

calculate_pose_change(motion, time_delta)[source]¶

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.

Parameters:	motion (triangula.chassis.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. time_delta (float) – The time in seconds during which the specified motion should be applied.
Returns:	A triangula.chassis.Pose which represents resultant pose after applying the supplied motion for the given time.

distance_to_pose(to_pose)[source]¶

Return the distance to the other pose position

Parameters:	to_pose (triangula.chassis.Pose) – The target pose

is_close_to(to_pose, max_distance=0.001, max_orientation_difference=0.017453292519943295)[source]¶

Check whether we’re close to the specified pose, defining closeness as both distance on the plane and difference in orientation.

Parameters:	to_pose – The target pose max_distance – Maximum distance within which we’ll count as being close, defaults to 0.001 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))
Returns:	True if this pose is regarded as close to the other, False otherwise

pose_to_pose_motion(to_pose, time_seconds)[source]¶

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.

Parameters:	to_pose – A target triangula.chassis.Pose time_seconds – A the minimum number of seconds to transition to the target pose.
Returns:	A 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.

pose_to_pose_vector(to_pose)[source]¶

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.

Parameters:	to_pose (triangula.chassis.Pose) – A target 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.
Returns:	A euclid.Vector2 containing the translation part, in robot space, of the motion required to move from this Pose to the target.

translate(vector)[source]¶

Create a new pose, with the same orientation as this one and the specified translation applied to its position.

Parameters:	vector (euclid.Vector2) – Vector by which the position of this pose should be translated when creating the new Pose
Returns:	Returns the new Pose

class triangula.chassis.Motion(translation=None, rotation=0)[source]¶

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.

__init__(translation=None, rotation=0)[source]¶

Constructor

Parameters:	translation (euclid.Vector2) – Vector2 representing the translation component in robot coordinate space of the motion. Defaults to Vector2(0,0) rotation (float) – Rotation in radians per second. Defaults to 0.

Defining the Chassis¶

In order to perform any kinds of calculations we need to know about the geometry of the chassis. In Triangula’s case her chassis is a triangle with omni-wheels at each corner, perpendicular to the vertex normal vector, but the code in this module is capable of handling any arbitrary combination of omni-wheel position, size, orientation etc.

The triangula.chassis.HoloChassis class specifies a chassis defined as an arrangement of various size omni-wheels. Wheels can be added to the chassis object with arbitrary orientation and position, and wheels of multiple sizes can be specified. This class is then responsible for converting an arbitrary triangula.chassis.Motion to target speeds for each wheel expressed as revolutions per second. It can also perform the inverse mapping, taking a set of wheel speeds and producing the inferred Motion.

class triangula.chassis.HoloChassis(wheels)[source]¶

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.

class OmniWheel(position, max_speed=0, angle=None, radius=None, vector=None)[source]¶

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.

__init__(position, max_speed=0, angle=None, radius=None, vector=None)[source]¶

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.

Parameters:

position (euclid.Point2) – The wheel’s contact point with the surface, specified relative to the centre of the chassis. Units are millimetres. max_speed (float) – 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. 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. 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. vector (euclid.Vector2) – 2 dimensional vector defining the translation of the wheel’s contact point after a full revolution of the wheel.

speed(velocity)[source]¶

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 velocity.dot(self.vector / abs(self.vector)). However, we want revolutions per second, so we must then divide again by abs(self.vector), leading to velocity.dot(self.vector / 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 velocity.dot(self.vector) / 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 velocity.dot(self.vector) / self.vector.magnitude_squared(). For efficiency, and because self.vector doesn’t change, we can pre-compute this.

Parameters:	velocity (euclid.Vector2) – The velocity at the wheel’s contact point with the surface, expressed in mm/s
Returns:	Target wheel speed in rotations per second to hit the desired vector at the contact point.

HoloChassis.__init__(wheels)[source]¶

Create a new chassis, specifying a set of wheels.

Parameters:	wheels – A sequence of triangula.chassis.HoloChassis.OmniWheel objects defining the wheels for this chassis.

HoloChassis.calculate_motion(speeds)[source]¶

Invert the motion to speed calculation to obtain the actual linear and angular velocity of the chassis given a vector of wheel speeds. See http://docs.scipy.org/doc/numpy-1.10.1/reference/generated/numpy.linalg.solve.html

Parameters:	speeds – An array of wheel speeds, expressed as floats with units of radians per second, positive being towards the wheel vector.
Returns:	A triangula.chassis.Motion object containing the calculated translation and rotation in the robot’s coordinate space.

HoloChassis.get_max_rotation_speed()[source]¶

Calculate the maximum rotation speed around the origin in radians per second, assuming no translation motion at the same time.

Returns:	Maximum radians per second as a float

HoloChassis.get_max_translation_speed()[source]¶

Calculate the maximum translation speed, assuming all directions are equivalent and that there is no rotation component to the motion.

Returns:	Maximum speed in millimetres per second as a float

HoloChassis.get_wheel_speeds(motion, origin=Point2(0.00, 0.00))[source]¶

Calculate speeds to drive each wheel in the chassis at to attain the specified rotation / translation 3-vector.

Parameters:

motion (triangula.chassis.Motion) – Desired motion of the robot chassis origin (euclid.Point2) – 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.

Returns:

A 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.

When calculating the target wheel speeds for a given motion, it’s possible that we simply can’t perform the desired motion. This typically happens because a requested wheel speed is impossibly high for one or more of the motors. If this happens the entire motion will be scaled back such that the fastest wheel is moving at full speed. The triangula.chassis.WheelSpeeds class wraps up the speeds of each wheel in revolutions per second, and also any scaling that has been applied to bring the motion into the range that’s possible to actually perform.

class triangula.chassis.WheelSpeeds(speeds, scaling)[source]¶

A simple container to hold desired wheel speeds, and to indicate whether any speeds were scaled back due to impossibly high values.

__init__(speeds, scaling)[source]¶

Create a new wheel speeds container

Parameters:

speeds – A sequence of float values, one per wheel, in revolutions per second scaling (float) – 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.

Location Awareness¶

Triangula has hall effect encoders on all her wheels. This means we can track, with reasonable precision, the exact movement of each wheel (although bear in mind we can’t track whether those wheels are actually driving across the ground rather than slipping). In a case where we have perfect traction we can therefore use these encoder values, or, more accurately, changes in the encoder values, to compute the triangula.chassis.Motion we’re currently performing.

When we have a motion, and a time during which that motion applied, we can calculate the change in pose. For example, if we know we’re moving forwards at 10mm/s and that 1s has passed we can trivially move our current pose 10mm forwards. For cases where we’re also rotating (given Triangula’s design this is pretty much all the time) the logic is more complicated, the triangula.chassis.Pose class contains a function calculate_pose_change which will handle these cases, taking a motion and time delta and returning a new pose representing the pose after the motion has been applied.

This logic is all wrapped up in the triangula.chassis.DeadReckoning class:

class triangula.chassis.DeadReckoning(chassis, counts_per_revolution=1216, max_count_value=32768)[source]¶

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 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 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 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.

__init__(chassis, counts_per_revolution=1216, max_count_value=32768)[source]¶

Constructor

Parameters:

chassis (triangula.chassis.HoloChassis) – The triangula.chassis.HoloChassis to be used to define kinematics for this DeadReckoning counts_per_revolution (float) – 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. max_count_value (int) – 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

reset()[source]¶

Clear the state of this triangula.chassis.DeadReckoning

set_orientation(orientation)[source]¶

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.

Parameters:	orientation (float) – 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
Returns:	The current (updated) value of the triangula.chassis.Pose

set_position(position)[source]¶

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.

Parameters:	position (euclid.Point2) – The new position to set, as a euclid.Point2, coordinates are in mm

update_from_counts(counts)[source]¶

Update the pose from a new set of encoder values

Parameters:	counts – A list of encoder counts, one per wheel
Returns:	The updated triangula.chassis.Pose object (this is also modified in the internal state of the DeadReckoning)

This class tracks the current best guess for the robot’s pose, and can be updated with encoder values (in which case it works out the derived motion and applies it over the time since the last update), or with explicit values for the pose (this is useful when we want to reset the current pose, or when we have information from some absolute sensor such as a compass).

Bear in mind that any dead-reckoning algorithm will inevitably accumulate errors over time. The degree to which this happens is down to a number of properties of the chassis, the wheels, the encoders, the ground over which the robot is moving, and many others. With good quality hardware, a rigid chassis, and, crucially, accurate dimensions, the accuracy appears to be pretty good. Any errors in dimensions, or any loss of traction, will very rapidly introduce errors in the estimated pose. It’s good enough for three-point-turn challenges, probably not good enough to navigate to somewhere hundreds of metres away, but the exact level of accuracy will depend on how you’ve built your robot!