Differential Steering

The very first challenge in building a robot that can self-navigate is localization. Given a map of the environment and a stream of motion related sensory input, how can a robot produce a reliable estimate of its position and orientation? Our very first step in tackling this question was to try and understand how our robot, Magabot, actually moves.

Magabot uses what is often called a differential steering system, where two wheels are rigidly fixed on each side of the robot’s structure. Movement trajectories are generated by rotating each wheel at different speeds.

Magabot comes equipped with rotary encoders on the wheels, which allow us to measure by how much each wheel has turned between two intervals of time in encoder clicks (k), a discrete unit measure of wheel revolution. If we know the wheel radius (R) and the number of encoder clicks per full turn (T), we can compute how far on the ground each wheel has traveled, assuming no slippage.

In order to use this information to derive an estimate of position and orientation, we have to define an inertial frame of reference for the robot. For convenience, we will choose the center point between the two wheels, which we will simply call C. At any given moment in time, the localization state of the robot can be given, in 2D map coordinates, by a vector of three variables, namely (x, y, θ).

When both wheels are turning at the same speed, the robot will travel forward or backward in the direction it is currently facing, which is given by θ. Let us call the distance on the ground covered by the left and right wheels S_L and S_R, respectively. In this case, the center of the robot will also move by the same amount as S_L and S_R, and its displacement in map coordinates can be computed from basic trigonometry.

Let us now imagine that we hold the left wheel in place while the right wheel travels some amount S_R. Since the wheels are rigidly attached, the trajectory of the right wheel will be circular with the pivot or center about the left wheel, as illustrated below. The center of the robot will also follow a circular trajectory, and again because of the rigid nature of the structure, it follows that the length of the motion will be exactly half of the distance covered by the right wheel. It is also possible to compute the angle of the circular trajectory if we know the distance between the left and the right wheels (d) as this is exactly the radius of the circle.

At this point we can compute the displacement of the robot through a useful approximation of the trajectory. Essentially, for sufficiently small distances, the trajectory of the circular motion approximates a straight line, and we can approximate the displacement using the exact same equations we obtained for the straight movement case. At the beginning of each update step, we compute the distance traveled (S_C) and assume a straight line motion through the current heading direction (θ). We then update θ, and iterate. In practice, if the robot does not move exceedingly fast, the sampling rate of the robot’s controller can be high enough that the approximation remains valid.

Finally, let us consider the case where the two wheels are moving at different speeds. Assuming the speed of each wheel remains constant throughout the trajectory, the motion of the robot will also be circular, with the exact location of the pivot dependent on the ratio between the two wheels. The schematic of this last trajectory is presented below.

As you may notice, the full radius of the circular motion is not known in this case, as it depends on an extra free parameter (d’). However, this parameter is related to the angle of the circle through the equations for the motion of the right and left wheel, which allows us to actually eliminate it. This effectively reduces the computation of the angle to a normalized difference between the speeds of the two wheels. As for the length of the motion of the robot center (S_C), we observe that again it is constrained by rigidity, and that it  will correspond to the length of the arc intermediate to the motion of the left and right wheels. With the angle and distance of motion in hand, we can again use the same straight line approximation as in the previous case and compute the generalized displacement for any S_L and S_R.

Having derived this motion model, it is now possible for the robot to compute predictions of expected location given its encoder sensory input. Unfortunately, it is not possible to accurately estimate the location of the robot using just this method. The fact that we have to continuously integrate the motion means it is particularly susceptible to even small errors, as they will gradually accumulate into the estimation. Unavoidable sensory and motor noise as well as some of the assumptions of the model which are not always verified (such as non-slippage) will be enough to ruin the estimation over time. In later meetings, we will look at how we can integrate information from other sensors to correct this estimate.

Reactivation of the GIR Club

The Games, Interaction and Robotics club has finally been reactivated last Thursday, March 8th. This year the club will be kickstarted with some exciting robotic projects:

 

1. Build a self-driving car: following Sebastian Thrun’s new online course with the same name, we will make our own autonomous vehicle to navigate around the CCU, interact with people and maybe perform some useful chores such as getting us some coffee. We will use the Magabot mobile platform as a hardware base on which to develop the navigation and behavior algorithms.
2. Design an autonomous articulated creature that learns to control its own body and builds a model of its sensorimotor world: using the Bioloid modular robotic kit we will experiment and create an artificial animal that can explore and understand the environment on its own.
3. Assemble our very own quadcopter: following the several open-hardware (e.g. Aeroquad) and open-software (e.g. Arducopter) standards that have been developed to support the creation of aerial vehicles, we will take our autonomous agent models to the skies.

The Magabot and Bioloid kits have already arrived and should give us plenty of material to get started. As last year, we will keep track of developments in this blog.

OpenCV .NET Bindings

It’s been a while since our last post and there’s so much catching up to do. We’ve been busy with a lot of exciting projects and hopefully we’ll get the chance to go over it in the next couple of weeks.

For now, we have a more practical resource to share with you. The last couple of projects in the GIR club have been moving consistently to the use of Computer Vision for natural interaction. We’ve been using the well-known OpenCV as a reference library for fast implementations of many of the core vision and image processing algorithms. Since we are currently taking up C# as our main programming language, we ended up starting the development of a .NET binding which can be used to access the routines from a managed environment. It is designed in such a way as to be compatible with the Mono framework interop binding system so it should be relatively cross-platform.

The bindings are going to be open-source under the MIT license, and we’re currently hosting them in a Bitbucket Mercurial repository, so to access them you’ll need a Mercurial version control client application. They are definitely not complete at the moment, but they will grow on a need basis, so expect useful functionality to come up first, and also feel free to suggest additions or even submit your own patches. The package also comes with a couple of the original OpenCV samples translated to C# code so you can have a feel of how to use them.

Get them while they’re hot: https://bitbucket.org/horizongir/opencv.net

Zebrafish Tracking System

During the experimental project weeks which took place last month we had the chance to go back to the development of vision-based behavior tracking systems, this time working with the famous Danio rerio, aka Zebrafish.

Our goal was to set up an interactive environment for the fish, where different visual stimuli could be presented from an LCD screen placed underneath the tank. The tank itself was supported on top of a Styrofoam box, with a thermostat for controlling the water temperature, and a divisor to ensure the fish would circulate only in the obstacle-free area.

The visual cues displayed on the screen were controlled in real-time depending on the position of the zebrafish acquired using a video camera positioned above the tank. In order to reduce the interference of the LCD video output with the tracking system, an infrared pass filter was placed in front of the camera lens to block the visible light emitted by the screen. Of course, this setup will also block the regular illumination needed for image capture, so we had to assemble infrared illumination sources on the side of the tank to ensure the fish was adequately visible at all possible tank positions.

The system was fully developed in C#, using OpenTK for computer graphics displayed on the LCD monitor and the .NET interface of the AVT FirePackage for image acquisition. The tracking algorithm was developed on top of the OpenCV computer vision library, using language bindings to the necessary functions and data structures. The basic outline of the image processing steps is included below.

Raw

Grayscale

Threshold

Crop

Erode/Dilate

Find Contours

Since there was a fair contrast between the fish and the swimming area we again relied on cropping and thresholding the image for maximizing our signal-to-noise. However, the dynamics of the aquatic environment and the presence of illumination artifacts demanded connected component analysis. Fortunately, OpenCV has an excellent algorithm for extracting not only the connected components, but even the contour shape information of each component. After the application of the morphological filters, we could pick up the fish by simply going for the biggest component on the screen.

After we had a solid grip on the fish’s location, it became very easy to try out different position-dependent environments such as dynamic brightness control or moving dot patterns. It was even possible to design a virtual companion for our subjects by having a sprite with some simple physics follow the fish around using steering behaviors.

As always, feel free to comment and contact us if you have any questions or new ideas for interactive setups.

Gadgets

Last week in the GIR club we had a lot of new stuff to play around with! First we got the opportunity to try out one of the wireless EEG devices from Emotiv to control virtual objects with our brain waves and engage in some biofeedback experiments. There’s some training required to translate the signals into actions and like most EEGs there is the possibility of noise coming in from facial muscle activity, but still the feeling of control was very impressive.

The device includes an API, the Research Edition SDK, which allows us to access the readings in real-time so it will hopefully be possible to design some cool psychophysics experiments.

The other news is that we now have our own Farrusco robotics kit! Below you can see a shot of our fully assembled and working robot, kindly provided by Guibot. The support structures for the bumpers, the board and the rotation servo were assembled using a CNC machine.

We also got an extra chassis with gear-box DC motors for building a proper differential drive we can use on our own moving robotic platforms.

The last part of the session comprised an Arduino introductory workshop, where we went through how to program the micro-controller, including how to read and write from digital ports, use the ADC-enabled analog input ports and how to use PWM to generate analog output.

Tiago and Bruno from INDP 2010 already put this technology to good use and assembled the world’s first robotic car for snails, which will be a major step to making the dreams of Turbo come true. We’re expecting to provide a special report on its development soon, but here’s a snapshot preview of the contraption.

We’re going to continue our study and development of Braitenberg vehicles this week, but if you have any interactive concept or idea you would like to turn into reality, feel free to talk to us about it. We’re always looking for cool projects!

Vehicles

The last club session started with a discussion on what is Artificial Life, its aims and methods. The topic is of course somewhat controversial since it necessarily requires a suitable working definition of Life at a functional level, and particularly one which does not require a specific biochemical substrate. After much argumentation, we settled upon some key principles which allowed us to build a provisional definition of life for working through this field:

• Life is a process.
• Life entails a struggle against the second law of thermodynamics.
• Metabolism, homeostasis and reproduction are key aspects in the fight against entropy.
• Life generates increasing complexity out of more basic building blocks.

During the discussion we also went through some of the milestones of Artificial Life, such as Conway’s Game of Life or Karl Sims’ Evolved Virtual Creatures. We decided to begin our explorations by trying to reproduce one of the classical proposals for the emergence of complex behavior: Braitenberg Vehicles.

According to Braitenberg, complexity in behavior begins at the very outset of simple transduction of sensory stimulation to motor output, in an organism exposed to a noisy environment. The very presence of noise will introduce variability and unpredictability in the behavior of the system, which will already be perceived as complex to human analysis. We tried to reproduce these conditions using our previously developed 2D physics simulation.

The environment was modeled as a plane viewed from the top. We assume that gravity keeps the vehicle on the plane (no jumping), with movement happening exclusively from forces applied to the vehicle on the X-Y axes. This means that with respect to the physics simulation there is actually no gravitic pull. There is a frictional component, however, resulting from contact between the vehicle and the ground, which was modeled as a dampening of the linear velocity of the vehicle over time. Also we assumed an asymmetrical friction model that introduces torque components in the trajectory resulting from irregularities in the terrain.

The vehicle sensory apparatus was modeled using the physics engine collision detection system. Sources of stimulation were also assumed to decay with distance and to arise from point emitters located somewhere in the world (e.g. attenuated light, sound or temperature). This allowed us to model them using circular shapes which are registered for collision detection but which will not elicit any mechanical collision response. Instead, they translate into stimulation strength which is inversely proportional to distance whenever a collision is detected between them. This stimulation will then be transduced to motor force applied to the vehicle body. By experimenting with different vehicle configurations we managed to reproduce Braitenberg vehicles #1 and #2.

Vehicle #1

Vehicle #2a

Vehicle #2b

We were also curious to see the principles applied to physical robots, and we took the opportunity that the experimental week was underway to try and fabricate some of these vehicles. Here are some shots of our first prototype:

Looks nice, but this prototype is actually still not capable of movement, since the DC motors we plugged in are lacking a proper gear box and they fail to generate enough traction to push the robot forward.

This week we will continue our exploration of both the software and hardware Braitenberg vehicles to see how far we can go. Feel free to join us and see you next time.

Voice Paint

On the first week of the experimental course we had to come up with mini-projects written in LabVIEW that would demonstrate data-acquisition and information processing in real-time. One of the developed projects was Voice Paint, a data visualization experiment where we convert sound into paintings. Here is the result:

The first information processing step in Voice Paint consists of analyzing audio signals recorded from a microphone in search of the dominant tone and extracting its amplitude and frequency.

Next we want to define the velocity vector which will move the canvas cursor, such that changes in frequency modulate orientation and changes in amplitude control the speed. We did this by defining the velocity vector in polar coordinates, where the frequency signal is the angle and the amplitude is the radius. From the polar coordinates we then extract the cartesian components of the velocity vector which we then add to the cursor position.

The drawing is accomplished by using an intensity graph. This indicator takes a two-dimensional array of values and plots them in color by using a defined intensity scale. On each frame we basically took the current cursor position and updated the array at that position with the value from the amplitude. This means that amplitude not only controls speed but also stroke color intensity. Here’s another example:

You can find the Voice Paint VI here. Feel free to play around with it in LabVIEW if you have the chance.

Demolisher

The last GIR session was dedicated to playing around with the Farseer Physics Engine and working out some interesting physics-based game mechanics. One of the most commonly used themes in such games is to demolish stuff by just throwing projectiles at stacks of boxes. We tried to reproduce this mechanics by working around the idea of a slingshot, a concept which has also been used in the past with varying degrees of success (cf. Angry Birds). Here’s our result:

The basic game mechanics were relatively simple to implement as soon as we had the framework in place. Building up on top of the results from previous sessions, we used our simple 2D render engine to visualize our physics world, taking care to define a metric coordinate system. The prototype was then assembled by defining three simple elements: a floor, the blocks and a projectile. The basic layout is depicted below.

The floor is simply a static physical object which basically prevents the other elements from simply falling off the edge of the screen due to gravity, which would be uninteresting in this case. Static objects work for the engine as bodies with zero velocity and infinite mass.

The blocks are individually defined as rectangular bodies with some mass and they just tumble around under the effect of gravity and collisions. They are simply arranged into stacks at the game creation phase so they can be targeted by the projectile later on.

The really interesting element which pretty much defines the whole mechanics is the projectile. We broke down its logic into three separate stages that the projectile goes through during the game operation.

The projectile state map

States are represented by the colored circles and transitions by the directed arrows. The events which cause the transitions are also indicated. During each state the projectile has different physical properties and behaviors. During the Ready and Set states, for example, the projectile is set up as a static physical object which can be dragged around by the mouse cursor, up to a maximum distance determined by the launching radius. When the Go state is finally reached however, a launching impulse is applied to the projectile body and dynamics simulation is turned on as it flies through the air.

The magnitude and direction of the impulse are determined by the position, relative to the center of the launching radius, of the projectile when the player releases the mouse button. During the transition to the Go state, the impulse vector is computed as the difference between the center and the release position (the launching vector), scaled by some arbitrary factor.

And that’s it! You can find the source code for the game prototype here. Only a simple level structure is loaded by the code, but it would now be possible to create more elaborate puzzles and rules by playing with just these simple elements, maybe by adding different types of blocks also with different states. Feel free to play around with it and ask us about it if you have any questions.

Next week we’re planning on discussing the design of autonomous agents on top of our 2D physics simulation and we will move on to create some simple behavior rules. Hope to see you there.

Rigid Body Dynamics – Introduction

This week we finally got the chance to begin the study and experimentation with the simulation of rigid body dynamics. Rigid bodies are idealized, infinitely hard, non-deformable solid objects. Their dynamics can be sufficiently approximated by classical Newtonian mechanics and can provide a rich playground for interaction.

In the last session we went over the requirements for putting together a realistic simulation, including: the formulation of a Newtonian physics system as an ordinary differential equation; the importance of having a good numerical integrator for solving the ODE; strategies for computing collision detection and collision response between rigid bodies; and finally how to specify different forms of interesting physical systems as constraints on the motion of rigid bodies (e.g. to simulate joints, hinges and motors).

Our discussion is still ongoing, but already the very first steps found their way into a cool psychophysics experiment run by Tiago Marques, Bruno Miranda and Ivo Marcelo to assess the hypothesis of how an internal naive model of gravity can impact our position estimations of falling bodies.

Tune in on our next club meeting for more interesting results and experiments. In the meantime, a good non-technical introduction to rigid body simulations by Russell Smith can be found in the Open Dynamics Engine website. We’re currently working with 2D simulations using the Farseer Physics Engine, which provides a unified numerical integrator, collision detection and constraint solver solution for game physics.

Drosophila Tracking System

This week the interaction club did convene, but with a sudden turn to Computer Vision, as the course work presented an interesting challenge: tracking a fly’s position and orientation over time on a petri dish.

The data was a feed of video images recorded from a camera looking down from a fixed position onto the dish in which the fly was walking freely. In order to achieve a minimal control of illumination, a sheet of paper was folded into a cylinder around the dish. Additionally, the paper was used to present different visual stimuli to the fly. Below you can see an example of a raw video frame.

From looking at the video it is clear the fly is in reasonable contrast with the background, making it possible to apply a simple thresholding operation to dramatically improve our signal-to-noise ratio. Additionally, since we wanted to measure the position of the fly in the metric system, we decided to crop the video to the edges of the dish. Since we knew the diameter of the dish to be 60 mm, the operation was enough to define a metric coordinate system from pixel displacement. Below you can see the effects of the cropping and thresholding operations:

As you can see, the vertical stripes are dark enough to interfere with the isolation of the fly in the dish. This problem was solved by multiplying the thresholded binary image by a circular mask covering only the base of the dish, where no visual stimuli are present.

Now that we had our fly perfectly isolated as a patch of white pixels, the remaining problem was to extract position and orientation values from the image. One way of approaching the problem is to look at the white pixels as a distribution of points in two-dimensional space. In this case, a reasonable estimate of the fly’s position is the center of mass of the distribution, which is given simply by the expected values for the x and y components.

As for the orientation, we can take advantage of the fact that a fly is to a good approximation an elliptical object on camera, with a definite longitudinal axis running from the head to the tail. Again by looking at the patch as a distribution of points, this longitudinal axis coincides with the line of greatest dispersion in the data.

This is exactly the problem addressed by principal component analysis, or PCA, which is a mathematical procedure to identify a set of orthonormal basis vectors which can be used to account for a large proportion of the variability in data. In two dimensions, the first component of PCA simply is the line of greatest dispersion, and we can use it to compute the angle the fly makes with the horizontal axis.

The only problem is that a line doesn’t have a direction, so we cannot in principle distinguish the head from the tail just from doing PCA. However, if we define the direction of the head in the first frame, we can always use the direction of the previous frame to decide between both possibilities, by assuming the fly does not rotate more than 180º between two frames.

With position and orientation extraction in place, it was possible to compute both the linear and angular velocities of the fly, since we also knew about the actual frame rate of video recording.

Overall the system reflects a relatively general set of principles for object recognition and tracking in video sequences which can be reused in different contexts. Of course, there are still a number of problems to be addressed if the system is to support more complex situations, such as tracking of multiple flies and more variable illumination conditions.

You can find the source code of the simple tracker here, including instructions on how to use it. Thank you very much to all who participated in the discussion and hope to see you all in the next meeting.