The post Path Finding – Convex Hull Part 1 appeared first on Sams Blog.

]]>The gaming industry has developed so many algorithms that are very applicable to the world of robotics.

The developers of early games had to conquer many kinds of issues to bring their virtual bad guys to life. One of these issues was path finding, because the baddies have to run around obstacles to attack the player. Sometimes these obstacles are quite complicated; the obstacles might be U shaped, or form long winding paths. Game developers had to come up with sophisticated algorithms to get their bad guys through virtual obstacle courses. Robust path finding is essential for autonomous navigation in real world robotics.

This blog series will explain some algorithms used in path finding.

Let’s pretend an army tank needs to travel to a safe spot. Let’s call the safe spot LZ. :shrug:

The tank needs to get to LZ but there are some obstacles (the green polygons).

- Remove concave points by creating a
*convex hull*around the obstacles - Create a
*visibility graph*for every point - Run a searching algorithm over the visibility graph to find the shortest way to the LZ

The first step is to create a convex hull around each of the obstacles. The convex hull is the polygon with all of the concave points removed. Imagine wrapping the polygons in gift wrap and the gift wrap is hugging the outer points but hiding the inner points – that’s what the convex hull looks like. The convex hull is found using gift wrapping algorithms.

This step is a good way to trim down the number of points that must be search through in the later steps. It’s best to just remove concave points altogether because they will never be on the shortest path between two points. You could probably tell that intuitively.

Here is a Javascript implementation of the gift wrapping algorithm

// this function if a point if on the left side of the line defined by a and b function isLeft(a, b, point) { return ((b[0] - a[0]) * (point[1] - a[1]) - (b[1] - a[1]) * (point[0] - a[0])) > 0; } function buildConvexPolys() { convexPolys = []; for (let p in polygons) { console.log("polygon: " + p); var poly = polygons[p]; var convexPoly = []; // find left-most point (lowest x value) // because this is an extremity it is guaranteed to be on the hull var pointOnHull = poly[0]; for (let r in poly) { if (pointOnHull[0] < poly[r][0]) { pointOnHull = poly[r]; } } var endPoint; var i = 0; do { convexPoly[i] = pointOnHull; // assign the end point, to be updated as we loop over the polygon points endPoint = poly[0]; for (let j = 0; j < poly.length; j++) { // if end point is pointOnHull // OR convexPoly[i] is on the left of the line from endPoint to poly[j] if ((endPoint === pointOnHull) || (isLeft(convexPoly[i], endPoint, poly[j]) === true)) { endPoint = poly[j]; } } pointOnHull = endPoint; i += 1; } while (endPoint != convexPoly[0]) convexPolys.push(convexPoly); } }

The gift wrapping algorithm is simple and robust but is not the fastest. Consider using this alternative approach if the polygon does not self-intersect and all the points are in order.

- Iterate over each point
- if the angle formed between this point and the next MINUS the angle between this point and the last is GREATER THAN 180 degrees (PI), remove this point

The Wikipedia has a list of Convex hull algorithms – LINK

The post Path Finding – Convex Hull Part 1 appeared first on Sams Blog.

]]>The post Mapness: Google Maps for Gtk+3.0 and Vala appeared first on Sams Blog.

]]>https://github.com/supercamel/mapness

A while ago, I was using osm-gps-map inside Gtkmm/C++ applications. That worked fine. After discovering Vala, I found osm-gps-map a bit clunky to use because the generated .vapi files were never quite right and needed to be hand tweaked. Adding new features to osm-gps-map is tedious too.

Imagine getting chauffeured to work every day in a luxury car. It’s easy to get used to. Then one day your chauffeur quits and takes the car with him. All you’ve got is a push bike. And you need to get to places. That is exactly how it feels to transition from coding in Vala to coding in C. Pure frustration, not that there is anything wrong with bikes!

That is why I decided to start mapness. Vala, IMO, is the best language for the job. It generates clean bindings for C, Python and other languages, and it’s going to be easier to maintain and develop further in the future. Please give it a try and if you find a bug or want to contribute some code, just get on github!

The post Mapness: Google Maps for Gtk+3.0 and Vala appeared first on Sams Blog.

]]>The post Essential GEdit Tweaks appeared first on Sams Blog.

]]>- Get into the dconf editor (go the activities, type dconf editor). The settings for gedit are under org->gnome->gedit->preferences->editor. To get a minimap (like sublime, or the minimap plugin for atom.io) set display-overview-map to true. I also set use-default-font to false and changed the editor-font to monospace 10 to fit more text on the screen. There’s plenty of a goodness here – like turning on line numbering etc.
- In dconf editor, go to org->gnome->gedit->preferences->ui and set bottom-panel-visible to true. Also set side-panel-visible to true while you’re there. Now go to gedit->plugins and add ‘terminal’ to the list. Restart gedit and you should see a list of documents on the left and a terminal below. If you click on the documents button (top left) it’ll let you change it to a file browser kind of like atom.io.
- Split panes! This is a secret apparently. If you press Ctrl+Alt+N it will split the view into two panes so you can have two or more files open side by side.

The minimap, file browser and split panes are absolute must-haves for me so it’s very pleasing to see GEdit has this built in even – if it is hidden at first.

Gedit is actually viable competition for editors such as atom.io and sublime. It probably isn’t as hackable as atom.io, but it’s **much** faster and provides all the features I value.

The post Essential GEdit Tweaks appeared first on Sams Blog.

]]>The post Using Osm-Gps-Map Widget With Vala appeared first on Sams Blog.

]]>- Download and extract the latest source from github
- Make sure you’ve got libgirepository-dev with sudo apt-get install libgirepository-dev. If you’re on Arch try pacman -S gobject-introspection
- Run ./autogen.sh
- Run ./configure –prefix /usr
- Run make & sudo make install
- cd into the src folder
- Run ‘vapigen –pkg gtk+-3.0 –library OsmGpsMap OsmGpsMap-1.0.gir’. This produces the OsmGpsMap.vapi VAPI file needed to use OsmGpsMap with Vala.
- Go to /usr/lib/pkgconfig and rename osmgpsmap-1.0.pc to OsmGpsMap-1.0.pc

You’re now all set to write Vala code using OsmGpsMap widgets!

Here’s a demo project.

The post Using Osm-Gps-Map Widget With Vala appeared first on Sams Blog.

]]>The post Modelling Acceleration – Calculating Orientation pt4 appeared first on Sams Blog.

]]>The accuracy of an orientation sensing system can generally be improved if the acceleration dynamics of the system can be estimated. Accelerometers are commonly used in orientation sensing systems because they can sense gravity (the *down* vector). They also measure acceleration due to changes in motion – of course. Modelling acceleration dynamics means using additional sensors (such as a GPS) to calculate acceleration, then subtract that from the accelerometers to find a cleaner *down* vector.

Let’s say we’ve got a drag racer. All it does is accelerate and decelerate in a straight line down a drag strip. It takes off at 5m/s/s.

Vector<3> a(5, 0, 9.8); //5m/s/s on the x axis, 9.8m/s/s vertical is due to gravity Vector<3> euler; euler.y() = atan2(-a.x(), sqrt(a.y()*a.y() + a.z()*a.z())); euler.toDegrees(); cout << "Pitch is: " << euler.y() << endl;

Output: Pitch is -27.0309

Simply accelerating at 5m/s/s will make the orientation sensing system begin to tilt down by almost 30 degrees! Ideally, this acceleration won’t last very long and this effect would be masked to an extent by turning down the filter gain. But still, it’s easy to fix this.

float dt = 0.1; Vector<3> v1(10, 0, 0); Vector<3> v2(10.5, 0, 0); auto linear_accel = (v2-v1)/dt; Vector<3> a(5, 0, 9.8); //a is the accelerometer measurement a = a - linear_accel; //subtract linear acceleration from accelerometer measurement float pitch = atan2(-a.x(), sqrt(a.y()*a.y() + a.z()*a.z())); cout << "Pitch is: " << radians_to_degrees(pitch) << endl;

Output – “Pitch is: 0”

Centrifugal force compensation is particularly good for fixed wing aircraft that often need to fly in a circular pattern for prolonged periods of time. Without centrifugal force compensation, an aircraft orientation sensing system could have 10 degrees or more of error.

Centrifugal force is often referred to as a fictitious force, because it doesn’t exist in the inertial frame of reference. Infact, by tracking the velocity of an aircraft using the world as the inertial frame, it’s quite easy to calculate total acceleration by differentiating velocity vectors.

world_acceleration = (world_velocity - last_world_velocity)/dt;

The GPS is a sensor that measures velocity in the world frame of reference and that could be used to calculate total acceleration. The problem is, the accelerometers are in the aircraft ( body ) frame of reference. To subtract world-referenced total acceleration from body-referenced accelerometers, the total acceleration must be rotated using the orientation estimate. But to get an accurate orientation estimate, body-referenced total acceleration needs to calculated. It’s a circular dependency and it won’t work. Therefore total acceleration needs to be calculated in the body frame.

If the aircraft is well above stall speed, it’s probably fairly reasonable to assume that the magnitude of velocity is along the longitudinal axis of the aircraft. Ground speed from a GPS combined with vertical speed from a barometric altimeter can be used to find the magnitude of velocity.

Vector<3> body_velocity(ground_speed, vertical_speed, 0); body_velocity.x() = body_velocity.magnitude(); body_velocity.y() = 0;

The cross product of angular velocity and body_velocity is centrifugal force.

auto cf = av.cross(body_velocity);

Centrifugal force can be subtracted from the acceleration vector.

body_acceleration = body_acceleration - cf;

Subtracting centrifugal force and linear acceleration from accelerometer readings is essential for accurately measuring the orientation of fixed wing aircraft.

Centrifugal Force Compensation Test on Github

The post Modelling Acceleration – Calculating Orientation pt4 appeared first on Sams Blog.

]]>The post Quaternion Formulas appeared first on Sams Blog.

]]>Where the axis is v and the angle is theta

Where theta is the angle and v is the axis vector

The first step is to convert angular velocity into an axis angle. w is angular velocity and t is the time period (sample rate of gyroscopes)

Using theta as the angle and v as the axis vector, it’s possible to translate this axis angle representation into a quaternion using the above formula.

To do this, translate the quaternion into an axis-angle. Again, v represents the axis, theta is the angle and t is the

Euler angles are interesting because the order in which the angles are applied is important. The same angles applied in different orders won’t give you the same result. The most common order of application is heading, pitch then roll.

Euler angles are essentially three axis angles. Heading is a rotation around the z axis, pitch is an angle around the y axis and roll is around the x axis. It’s easy to convert these three axis angles into three quaternions, then multiply them together to create a single quaternion representation of the original euler angles.

There’s an elegant solution to this problem.

To extract a heading euler angle from a quaternion

Pitch euler angle from quaternion

Roll euler angle from quaternion

To multiply quaternion q by quaternion r to produce quaternion s.

Quaternions can be used to translate vectors from one frame of reference to another.

q(x,y,z) is a vector constructed from the x,y,z components of the quaternion q. v is the vector to rotate and rv is the rotated vector.

The post Quaternion Formulas appeared first on Sams Blog.

]]>The post Integrating Gyroscopes – Calculating Orientation pt2 appeared first on Sams Blog.

]]>If you are not familiar with integration, read this first.

Gyroscopes measure angular velocity. Angular velocity is the rate of change of angular displacement.

You know that distance = speed * time.

Well, angular displacement = angular velocity * time

Gyroscopes can’t measure orientation with reference to the world, they can only work out angular displacement from a starting point. Additionally, because they can only do this by integrating measurements, every little error is also integrated and soon large errors will accumulate. This is called gyro drift. That is why gyroscopes alone cannot be used to measure orientation.

Calculate angular displacement and add the displacement to a quaternion. The quaternion accumulates all of the angular displacements from the gyros. I’ve always called this quaternion ‘q’, and q represents the estimated orientation.

etk::Quaternion has a function called fromAngularVelocity(). It takes angular velocity in radians per second, as well as time ( dt ) in seconds where time is the gyroscope sample rate in seconds. If you’re reading the gyros 200 times per second, dt would be 1/200=0.005s.

etk::Quarternion qw; etk::Vector<3> w = gyros.read(); float dt = 0.005; qw.fromAngularVelocity(w, dt); qw.normalize(); //ensure qw is a unit quaternion //qw represents the angular displacement //that has occured since the last gyro reading

Multiplying two quaternions will ‘add’ the orientation of one to the other. In this case we want to apply angular displacement ‘qw’ to orientation ‘q’.

q = q * qw; //apply angular displacement 'qw' to orientation 'q' q.normalize(); //make sure q remains a unit quaternion

The fromAngularVelocity function uses sin and cos to calculate angular displacement. If the angular displacement is small, a faster approximation can be used.

Quaternion qw(1, w.x()*dt/2.0, w.y()*dt/2.0, w.z()*dt/2.0); qw.normalize();

This works sufficiently well in most cases and is substantially faster than the fromAngularVelocity function.

Accelerometers and magnetometers are noisy sensors. Gyroscopes are fairly noise-free, but they only measure angular velocity. Every small error is integrated and q will quickly become inaccurate. These errors can be due to mechanical vibration, temperature effecting gyro bias, lack of floating point precision and gyros measuring the rotation of the earth (15 deg per hour). The accumulation of errors is called gyro drift.

Accelerometers and magnetometers can be used to create a quaternion (accel_q) that represents orientation referenced to Earth, and this can be used to correct q for gyro drift.

Spherical Linear Interpolation (SLERP) is a function that is used to calculate a point in between two quaternions. Use SLERP to find a point somewhere between q and accel_q, and then move q to that point.

//get acceleration quaternion Quaternion accel_q = get_accel_mag_rotation(state, dt); //use SLERP to move q closer to accel_q q = q.slerp(accel_q, ORIENTATION_FILTER_GAIN);

ORIENTATION_FILTER_GAIN is a value between 0 and 1 which determines how much slerp should move q. If it’s 0, q won’t change. If it’s 1, q will become equal to accel_q every time. The ideal value will depend on how quickly q drifts and how noisy accel_q is. A reasonable starting point is 0.05.

The post Integrating Gyroscopes – Calculating Orientation pt2 appeared first on Sams Blog.

]]>The post Quaternions And How to Really Use Them appeared first on Sams Blog.

]]>If you type ‘quaternion’ into google, it’ll tell you something like this

*a complex number of the form w + xi + yj + zk, where w, x, y, z are real numbers and i, j, k are imaginary units that satisfy certain conditions.*

That’s a pretty frightening description, but with any luck, you’re a persistent type of person because once you’ve masterd quaternions (at least, unit quaternions used to represent orientation) you’ll probably find they are actually nowhere near as difficult as you may have been led to believe. For a start, you can ignore the imaginary components. They’re just a figment of your imagination, anyway.

From here on, we’ll use a representation with only four components. These are w, x, y and z.

These four components make quaternions similar to four-dimensional vectors in a lot of ways. For a start, normalising a quaternion is done in the same way.

[math]\left | q \right | = \sqrt{w^2 + x^2 + y^2 + z^2}[/math]

[math]\hat{q} = \frac{q}{\left | q \right | }[/math]

In pseudo-code

magnitude = sqrt(w*w + x*x + y*y + z*z) unit_q = q / magnitude

The quaternion library in ETK has magnitude and normalise functions

real_t magnitude = q.magnitude(); q.normalize();

One of the most import quaternion operation is multiplication. It’s used to add the amount of rotation stored in one quaternion to another. For example, quaternion A represents straight, level and east facing ( 90 degrees). Quaternion B is straight, level and facing north east ( 45 degrees ). Multiplying A and B will give a quaternion that has a heading of 135 degrees – because multiplying A by B adds the rotation of B to A. Make sense?

Here’s the proof

#include <iostream> #include <etk/etk.h> using namespace std; int main() { etk::Quaternion A, B; //heading is 90, zero pitch or roll etk::Vector<3> a(90, 0, 0); a.toRadians(); //heading is 45 etk::Vector<3> b(45, 0, 0); b.toRadians(); //make quaternions A and B from euler angle representations A.fromEuler(a); B.fromEuler(b); //multiply A by B etk::Quaternion C = A*B; //convert C to an euler angle, because it's easier for mere mortals to comprehend etk::Vector<3> euler = C.toEuler(); euler.toDegrees(); cout << euler.x() << " " << euler.y() << " " << euler.z() << endl; }

The output from this program is 135 0 0.

One very important thing to note is that quaternion multiplication is non-communicative. That means

[math]{A*B} \neq {B*A}[/math]

For unit quaternions, the conjugate is the same as the inverse. This is great because the conjugate is a lot easier to calculate than the inverse. To find the conjugate, simply invert the x, y and z components.

[math]\bar{q} = {(w, -x, -y, -z)}[/math]

etk::Quaternion has a conjugate function

etk::Quaternion conj = q.conjugate();

The conjugate is great because it allows you to unrotate or substract a rotation from another quaternion. Multiplying quaternions adds rotation but multiplying by the conjugate subtracts it.

Quaternions can be used to rotate vectors. This is really useful for converting vectors to a different frame of reference. Example:

A pilot experiences acceleration, but a large component of this is due to gravity. We know that gravity always measures 9.8m/s/s upwards in the world frame of reference and we know the orientation of the aircraft. We can use the aircrafts orientation to rotate a gravity vector into the aircraft frame of reference, then simply subtract it from total acceleration felt by the pilot.

etk::Quaternion q; etk::Vector<3> euler(285, 2.5, 1.54); euler.toRadians(); q.fromEuler(euler); //this is the orientation quaternion //total acceleration is force felt by accelerometers in the aircraft etk::Vector<3> total_acceleration(1.5, 0.2, 10.5); //gravity makes things feel like they are accelerating up at 9.8m/s/s on the vertical (z) axis etk::Vector<3> gravity(0.0, 0.0, 9.8); //rotate gravity so it's in the pilots frame of reference auto gravity_pilot_frame = q.rotateVector(gravity); //now we can subtract gravity from total acceleration etk::Vector<3> acceleration = total_acceleration-gravity_pilot_frame;

Converting angular velocity to a quaternion is easily done with ETK. Simply use the function fromAngularVelocity().

If angular velocity is regular velocity, then a quaternion represents position. It simply doesn’t make sense to convert velocity into a position unless you have a time delta. Velocity equates to a change in distance over a period of time. fromAngularVelocity() requires angular velocity in radians per second, and the sample rate of the gyroscopes in seconds. It will calculate a quaternion that represents that change in angular position.

You can also convert the quaternion back into an angular rate using toAngularVelocity().

etk::Quaternions can be converted to and from NED orientation matrices, axis angles and euler angles.

The post Quaternions And How to Really Use Them appeared first on Sams Blog.

]]>The post IMU Maths – How To Calculate Orientation appeared first on Sams Blog.

]]>I made a maths library for Arduino and it has been used in quite a few cool projects ( such as this and this ). I merged that library into ETK a while ago, and this series of posts will be using ETK as the reference implementation of the maths functions I’m going to write about.

If you want to try writing some code yourself based on these blog posts, you need to download and use ETK. It also comes with heaps of stuff that you may find useful such as PID controllers and a navigation library.

or if you’re using Git, add it as a submodule

*git submodule add https://github.com/supercamel/EmbeddedToolKit*

Here’s a script that updates submodules to the latest version

git pull origin master --recurse-submodules git submodule update --rebase --init --remote --recursive

If you’re unfamiliar with vectors, matrices or quaternions, take some time to go through these pages before diving head first into this topic.

https://www.mathsisfun.com/algebra/vectors.html

http://www.camelsoftware.com/2016/02/20/quaternions-and-how-to-really-use-them/

Nearly all IMUs use a 3 axis MARG array. This stands for Magnetometer, Accelerometer and Rate Gyroscope. In the past these sensors have been big and expensive, but MEMs technology made it possible to shrink the sensors down so they fit inside a single chip! Some sensors such as the BNO055 have a processor that works out orientation for you (this is great for basic applications but has some serious limitations that will be discussed later).

Magnetometers and accelerometers produce 3D vectors. A vector is great for indicating a direction. Think about gravity. You can feel gravity, so you know which way is up, but gravity alone can’t help you find your orientation. It doesn’t indicated which way you are facing (where’s north??). That’s where the magnetometers come in. By defining a second vector, we’re able to find orientation. One vector is good for direction, but two are required for orientation.

We can combine acceleration and magnetic vectors to produce a rotation matrix. A rotation matrix is a 3×3 matrix that contains 3 perpendicular vectors. These are north, east and down vectors.

Notice that the matrix contains 3 vectors, when it’s just been mentioned that only two are required for orientation? Matrices contain redundant information and occasionally the components of the matrix lose their orthogonal properties, which is one of the down sides to using a matrix.

Anyway, here’s how they’re made

#include <etk/etk.h> using namespace etk; Vector<3> down = accelerometers.read(); Vector<3> east = down.cross(magnetometers.read()); Vector<3> north = east.cross(down); down.normalize(); east.normalize(); north.normalize(); Matrix<3,3> rotationMatrix; rotationMatrix.vector_to_col(north, 0); rotationMatrix.vector_to_col(east, 1); rotationMatrix.vector_to_col(down, 2);

Let’s go through this. The down vector is acceleration. That’s easy enough.

The cross product of down and the magnetometer vector creates the east vector.

The cross product of down and east creates the north vector. This ensure that north is also perpendicular. It corrects for magnetic dip, too.

Each vector is normalized then packed into the rows of a matrix. The Quaterion::fromMatrix function converts the matrix to a quaternion.

etk::Quaternion q_accel; q_accel.fromMatrix(rotationMatrix);

The matrix method is interesting because it ‘automatically’ corrects for magnetic dip. An alternative method, however, is to use euler angles to construct q_accel.

Vector<3> euler; euler.x() = atan2f(mag.y(),mag.x()); euler.y() = atan2(-accel.x(), sqrt(accel.y()*accel.y() + accel.z()*accel.z())); euler.z() = atan2(accel.y(), accel.z()); q_accel.fromEuler(euler);

Notice the z axis of the magnetometers isn’t used? This method doesn’t account for magnetic dip.

In practice, accelerometers and magnetometers alone don’t work well for measuring orientation because they pick up lots of signal noise. We only want to measure gravity with accelerometers and every vibration or motion will disrupt that. Likewise, magnetometers pick up stray magnetic fields from motors or electronics. This is where gryoscopes come in handy . . .

The post IMU Maths – How To Calculate Orientation appeared first on Sams Blog.

]]>The post Ninjaskit Keypad Library appeared first on Sams Blog.

]]>Matrix keypads are fairly simple to use. They’ve got rows and columns, and each button connects a particular row to a given column.

To check if a switch is pressed – let’s say SW1 – you could put a digital high on Col1 and make Col2 – Col4 low. If Row1 is high then SW1 is pressed, otherwise SW1 is not pressed.

Reading the entire matrix is a matter of iteratively putting a high on each individual column and checking the state of each row.

Of course, this will only tell you the state of the keypad – if buttons are up or down. Generally speaking, you’ll need your program to perform some action when a button is pressed. The state of the keypad isn’t important; what is important is that something occurs when a button changes state. This paradigm is known as event-driven programming. The keypad library tracks the state of the keypad matrix and generates events when buttons are pressed or released.

Check out the example

#include "ninjaskit/ninjaskit.h" #include "keypad/keypad.h" int main() { clock_setup(); Serial1.begin(57600); //create a list of row pins gpio_pin rows[] = { {PA, 0}, {PA, 1}, {PA, 2}, {PA, 3} }; //create a list of column pins gpio_pin cols[] = { {PA, 4}, {PA, 5}, {PA, 6}, {PA, 7 }}; //create a keypad with four rows and four columns - 16 buttons total Keypad<4, 4> keypad(rows, cols); while(true) { etk::sleep_ms(10); //read the state of the keypad and generate events if necessary keypad.read_keypad(); //while there are events queued up while(keypad.get_n_events()) { //get the next event auto event = keypad.get_event(); //if it's a button press if(event.type == BUTTON_PRESS_EVENT) Serial1.print("button ", event.button, " has been pressed\r\n"); //if it's a button release if(event.type == BUTTON_RELEASE_EVENT) Serial1.print("button ", event.button, " has been released\r\n"); } } }

The post Ninjaskit Keypad Library appeared first on Sams Blog.

]]>