Sunday, April 16, 2017

Probabilistic Robotics - Odometry - Velocity based Model

Autonomous navigation heavily relies on probabilities. The hard part isn't getting to build ROS based robots. But actually understand what are the underlying parameters and algorithms. One such book that is literally a bible to the approach of these algorithms is the book by S. Thrun, W. Burgard, and D. Fox - Probabilistic Robotics.
I first thought of directly publishing the odometry of Clerkbot using the ROS nodes. Not knowing the underlying models and states isn't very good for anyone interested in robotics. Here's my take on the Velocity based model based on the book suggested which is also the model which the base_local_planner works. This is because inherently navigation planning uses velocity commands to plan for obstacle avoidance. The odometer readings are only helpful after the command of control has been given.

There are two models into the probabilistic kinematics:

  1. Odometric Model
  2. Velocity Model
I would like to overemphasise one thing. That is in probabilistic robotics, as the name suggests, nothing is certain. You are constantly estimating states based on given inputs and due to noises present,  the uncertainty is shown with the help of probability distributions. So nothing is certain when you are dealing with noises present in almost all the states. The odomteric model can be realised with just one probability distribution,

This is when you want to estimate the state the pose of the robot at current time t=t, given the the control commands given at time t=t from the previous pose state at time t=t-1.
You can cross-verify here what I wanted to tell you in the previous para. That given a pose state of the robot and a control command given, you cannot be certain of the position of robot at time t=t+1 because of the noise in motors or odometers or whatever actuator you choose. You cannot be certain about your position, but you can devise a normal distribution of the position of the robot given you have the noise information.

Note: Odometry model is more accurate since you are getting values of the revolutions from encoders present. But for motion planning, obstacle avoidance velocity model wins the race.

Velocity model can be broken down to the following points:
  1. The translational and rotational velocities given at an instant is v,w (read as mu and omega). Now, the radius r covered is, v/w.
  2. Given the initial point of (x,y, theta), the final pose can be estimated by using the equation assuming error less state system.(Figures 2 and 3).
  3. Final equations given the initial pose and after assuming constant angular and translational                                   velocity and delta time t. (Figure 4)

fig 2 : Rotational and translational motion
fig 3: Center of circle equations

fig 4: The final pose estimate(error less system)

Now this is where the concept of probabilistic kinematics and noise kicks in. Consider there are noises present in the rotational and translational velocity with a noise which has zero mean and a variance b.  Hence now the final velocities are, containing real world noises.

fig 5: Noise into the states


Sunday, April 9, 2017

ROS Autonomous Navigation Bot - [Clerkbot] - Odometry #1

Odometry is an important aspect in autonomous navigation, but alas not an uber-essential one. There is Hector Slam algorithm that does not require odometry, but this bot does use it.

Odometry is important since you need to tell the ROS main channel as to where you are with respect to the environment and how much you have moved from the initial point. There's another addition, you can get the angle with respect to the initial point too.

Again, the map's origin is different, the odometry's origin is somewhere else. So are the origin of sensors(Lidar) and the geometric axis of rotation of your robot. So you're continuously sending the state of your robot's position and it is being transformed from local to global axes of the map. Same is the case of other transformations.

Basically you have nodes already available that takes care of your encoder counts. But,  I haven't used any of these. Primarily because I wanted to know the intricacies of the working of ROS nodes. There is a tutorial on odometry too but then it is too subjective and not to the point. This being a non- holonomic robot didn't need much of the working from the mechanical aspect.

Here's the odometery tests in accordance with the sanity tests as described here.

Friday, April 7, 2017

Key points for Embedded Programming

There are some intricacies involved when you look at the core assembly level in embedded systems. People who I have seen not take a formal Embedded Course or any formal C course in general, end up coding the less efficient way. This is a generalisation but I have too at some point of time while  coding controllers,  have been coding the wrong way. The thing is developing logic is one aspect of the story and making the most of the data types, qualifiers and specifiers is also important. This is primarily because while coding embedded systems, we generally have constraints on the flash memory or RAM or the pins available. So the better you code, the better will be the optimisation and the better the controller will perform.

Take a look at this code snippet from my code here.

1) Use of static and const

This is an interrupt handler for the counting of encoder states and look at the static variable. The static and const is what I want to focus here. Some key points:
  • What's actually happening here is that, the variable count needs to compute the counts, leave the function, go to main and come back while keeping track of the counts.
  • static is used here since I want to essentially keep track of the counts, but then if I use a local variable, it will get destroyed the moment it leaves the function.
  • Having a static specifier, helps here as it tells the compiler to statically allocate a fixed size for the variable. This helps in retaining the value of variable when scope of the function gets destroyed and refrains the compiler from making copies if the variable.
  • const is a qualifier and it tells the compiler not to make any changes to the variable.
  • a global static variable tends to restrict the variable to be under scope of that file only. Meaning no other file can share the same variable.
2) Use of volatile

This is another grey area in embedded. Not many know of it and not many use this, atleast the amateur embedded programmers. volatile is again a qualifier and not a storage specifier like static. Basically the compiler keeps on making optimisation so that the code runs faster and efficiently. When it comes to the compiler, it basically converts the high level language to a machine language and in this is where it tries to optimise the code. 
So by putting a volatile qualifier you are basically telling the compiler not to optimise it. Here's a good example

3) Use of pointers

Again a topic largely neglected by intermediate and amateur embedded programmers. Basic logic is needed to get the code running but running into optimisations and increasing the controller efficiency is also needed. I'm not going to divulge into details, but basically pointers point to addresses directly. This is important because most of the time you are writing some driver and you need the address the memory location directly.
Also, the fact that it does not let the program make copies if the variable at run time.

4) Use of debuggers

When you are making large projects, you can't do a trial and error everytime to get the bugs in the code. You ought to have a good debugger to see what happens to your code line by line.
This also ensures if you have peripherals attached to your controller, you can actually check if the data incoming is correct or not. I use a gdb server setup to load and debug using the st-link on STM32.

A short video of which you can see here:

Sunday, March 5, 2017

ROS Autonomous Navigation Bot - [Clerkbot] - Initial Tests

Finally it took me three months to fully come up with this robot and just a fun fact, it took me a month to just tune the ocean of parameters.  Here I say two months to build  the robot, although a good 3 months to 'learn' the ROS framework which includes a Gazebo simulation of a UAV and UGV.  Now that it is over,  we are all geared up to the challenge of UAV Autonomous navigation.

This whole robot setup is part of a year long research project on UAV and UGV platforms under Dr. D.K. Kothari, HOD, EC Department, Nirma University. The UGV setup is planned to be a precursor to the UAV setup as we wanted to get on 'hands' with ROS framework. Getting our hands directly on a UAV can be a daunting task, and UGV was commissioned to fulfil that exact need.  The UGV is working perfectly. But if you want to know as to why exactly it is named so, you'll need to wait.

This is going to be a tutorial cum documentation posts for the Clerkbot. In the coming videos and posts I would be dealing with all the details of the robot.  I would also soon establish an open source platform for the same setup so that others can benefit from the same.

Here's the Initial tests video of the UGV:

Thursday, January 5, 2017

STM32 Cortex M3 Series - [STM32F103C8T6] - #2 - Encoder Interface

Before I begin kindly keep my GitHub code as reference throughout the tutorial. I'm currently working on encoder interface to generate odometry data for my Autonomous UGV and UAV project. This can be done in various ways by encoders of various types. The one I'm having is this 300RPM motor with a quadrature encoder circuitry embedded to it's shaft. Here's the motor I'm talking about from Robokits:

Figure 1. The Quadrature Encoder Motor

The encoder circuitry has +5, and GND as logic inputs and two channel outputs A and B giving timely square outputs.
(NOTE: Apparently I spent around two full days for the lack of a datasheet on the website trying to figure out why the outputs aren't coming only to realise later that the pins need to be pulled high with a 10k resistor. )
What seems so easy to do on paper is to simply get the interrupts from both the channel for a higher resolution and provide conditional loops in the interrupt routines to get the values and send the values back to main loop. 
WRONG. Never do this. This is bad coding. The prominent reasons for this are:

  1. The encoder readings change very fast. There is a possibility that it just might skip some values give an error reading(channel_A_state=channel_B_state=0 or 1) and count can be a wrong one or an illegal one.
  2. Checking for a high or low value in an Interrupt Service Routine is a bad choice too. The ISR's should be as small as possible. This is because, the core has been interrupted and you need to get back to look into other tasks too.

What I would like you to point towards is this stackexchange post answer which points out to the use of using decisions in getting values of the interrupt values:
You should have ZERO ifs. No decisions. Store your AB state, i.e., 00 or 01, then append your next state, i.e 0001 means AB went from 00 to 01,thus B changed from 0 to 1. Make this a +1. If starting from 00, and you change to 10, then call this a -1. Build a 16 element array of all possible transitions holding the number that needs to be added to your count if it occurs, noting that some are illegal and need to be handled.
The counting is essentially on the works of the graycode.  So we are going to create a graycode and check it against an array of legal and illegal states. Here's the algorithm:

  1. Create an array of the possible states in the graycode with something like this:

    int8_t states[] = {0,-1,1,0,1,0,0,-1,-1,0,0,1,0,1,-1,0}
  2. Also create a graycode containing the (final_state= prev_AB_state+current_AB_state). Left shift the previous AB state by 2 and append the current AB state to the final state.(If this is confusing to you, see the full code on my git.)
  3. Here's how a graycode looks:

So if for e.g ,
  • The final_state is 0001, it means the prev_AB_state=00 and it has changed to 01. This would be given a legal state of -1, or anti-clockwise rotation
  • The final_state is 0010, it means the prev_AB_state=00 and it has changed to 10. This would be given a legal state of 1, or clockwise rotation.
  • Now if both channels A and B are changing then we term it as an illegal state as both cannot change the state together.
I would soon post a video on the same and my next post would be on setting up OpenOCD debugging and Linux Eclipse CDT setup.


Wednesday, December 14, 2016

The Low Cost Automatic 'Wall-Painter' Robot- A Sneak Peak!

After nearly 3 months of working and designing the 'Wall-Painter' Idea(I proposed my Idea to the Idea Labs Committee, Nirma University about 3 months ago) along with team members Dishant Kavathia and Ammar Gandhi. The objective of the Wall-Painter is simple - A dedicated app will provide templates of paint designs as an input to the Wall-Painter, and it will oblige to do the required. I had and been doing projects since the last two years, but there was always a small amount of lack of professional 'furnishing' and 'designing.' Hence, the goal this time was to give ample time in designing and refining the project at hand.

The Idea required a manifestation of the Idea in my mind to a proper 3D Model first. Dishant Kavathia, a fellow teammate, has been super-amazing in making the model come right on SolidWorks and also rectify the underlying mechanical difficulties. Ammar and I would be looking at the electronic installation and the algorithms to drive the tasks. Also, I can't indulge in any more details as this is under development.

Here I am attaching a few snapshots of the proposed plan as designed by us (Model made by Dishant Kavathia)

 Figure 1. The skeleton model

Figure 2. The basic X-Y frame

STM32 Cortex M3 Series - [STM32F103C8T6] - #1 - LED Blinking

So finally after three months, here I am with one of the first posts on the ARM STM32 Cortex M3 series after working for about 3-4 months from now.  The 8-bit controllers seem somewhat ugly in comparison to the powerful 32-bit ARM architecture, boasting a hell lot of peripherals- DMA's, OTG, Ethernet and hundreds of GPIO's(Obviously, I've still not touched them and in my learning process). This becomes a different ball game as the application for which you might be needing this for, won't require this much of computational prowess. Starting with the very 'Hello World' of every hardware project which is the LED blinking:

The IDE I've been using is the CooCox IDE, a very decent IDE along with the arm toolchain for arm programming with the good amount of online community for sample codes. There are two ways that we can program using this IDE:
  1. Using the existing well versed and documented libraries of the stmf103xx  series, which I've used currently and this blog has been helpful to me regarding the programming of the series.
  2. Secondly, one could use the low, register level programming using just the stm basic library and well documented in the YouTube tutorials by Patrick Hood Daniel.
The final goal is to light up an LED so we'll need to setup the GPIO's or the port for the LED blinking. Before we begin, it would be easier if you would have the reference manual (and not the datasheet) in the background.

  1. The Cortex M3 core is connected to different peripherals via different buses. These buses are prominent parts of arm architecture known as the AHB, or the Advanced High-performance Bus.  The AHB is then connected to different APBs,  which is the Advanced Peripheral Bus(APB). From the datasheet information given below:

    figure 1. Architecture diagram, AHB and APB2 buses

    We can see that the GPIO's are driven by the clock from the APB2 bus, clocking at a maximum of 72Mhz. So our, first task will be to enable the clock source for these GPIO's. 
  2. Next,  as we are going to use the GPIO's. We need to specify the parameters for the port or pin to work - input or output, type of input or output etc. as with the case with every microcontroller. 
    The following are the types of Input and output types available on the GPIO's as given in the reference manual(Pg.158, section 9.1)
Figure 2. Input and output pin configurations

Figure 3. Basic input and output block Structure of the GPIO's 

Here's a brief on the types of these pins:
  • Input floating : In these pins the singals are in a 'Floating' state or tri-state. Meaning the signals are of no use unless they are pulled up or down by a high source or the output is in some definite state.
  • Input pull-up : In these type of pins, the input state when nothing is connected to it is High since a resistor is 'pulled-up' internally.
  • Input pull-down : Same as above put pin is pulled down with a high value resistor when nothing is connected and it is internal in the structure.
  • Output Open-drain : The output transistor is open drain and a resistor with a voltage level needs to be tied up to the drain .
  • Output push pull : The output is open drain is only one direction meaning, when pin has to go high it has to rely on the resistor and the pin by itself cannot source. Therefor it has two transistors both to sink and to source the current. Here is a good text from this site:
    "The push-pull output actually uses two transistors. Each will be on to drive the output to the appropriate level: the top transistor will be on when the output has to be driven high and the bottom transistor will turn on when the output has to go low.
    The advantage of the push-pull output is the higher speed, because the line is driven both ways. With the pullup the line can only rise as fast as the RC time constant allows. The R is the pullup, the C is the parasitic capacitance, including the pin capacitance and the board capacitance.
    The push-pull can typically source more current. With the open-drain the current is limited by the R and R cannot be made very small, because the lower transistor has to sink that current when the output is low; that means higher power consumption. "
I would continue with the coding, in the next part as this would become a very long post.
Thanks for watching and Cheers!