Author Archives: dxydas

More software updates

With the new PC’s development tools up and running, I’ve made a number of updates to the GUI side of things, as well as the background joint controller node. Here are some of the most important from this month.

Joint controller updates

An issue I was having with the joint controller node – a main function of which is to act as a ROS wrapper class around the Dynamixel API – was that I seemed to be getting erroneous values when executing a simple one-off read from the motors. I suspected this was related to the fact that the node is also constantly reading the motors’ feedback values as fast as possible in its main loop. Even though the Dynamixel library seems to have checks for a busy comms bus, there was likely some issue with multiple threads trying to access the bus. This was easily fixed by using ros::spinOnce() in the main loop instead of using an AsyncSpinner which starts up a separate thread.

There is still some problem where positional values for some of the motors seem to be getting constantly corrupted, but only when all the torques are enabled and the motors are struggling to achieve all their goals positions (making the typical whining noise that servos have). I’ve yet to narrow down if this is a software issue or not.

Update to read/write services

The ROS read/write services have been simplified now, and two of the most useful commands are the ones that perform a sync read and write across a number of motors. As these are ROS services they can also be called from a terminal command line. For example, to receive the current position, current speed and current torque of motors 1, 3 and 5 all at once, you can run:

rosservice call /ReceiveSyncFromAX '[1, 3, 5]' 36 3

36 is the start address for the low byte of the present position, and 3 indicates the number of control table values, including the start address, to read for each motor. This is calling the sync_read function that the USB2AX offers, via a ROS service.

In a similar way you can write goal position (100), goal speed (300) and max torque (512) to motors 1, 3 and 5 all at once by running:

rosservice call /SendSyncToAX '[1, 3, 5]' 30 '[100, 300, 512, 100, 300, 512, 100, 300, 512]'

One restriction is that the sync read and write functions can only read/write consecutive control table addresses, as explained in the USB2AX link above, as well as here in the Robotis documentation.

New motor control table value editor

On the GUI side of things, I’ve made a new “motor address editor” as an improvement on the previous “motor value editor”. The new editor allows reading/writing of all control table addresses of the AX-12. You don’t have to worry about writing low/high bytes to the two-byte values, as that is handled by the editor; simply write any valid value to the parameter of interest.

I have kept the old editor as two useful features it has is let me easily write the same value to all motors, as well as send position/speed/torque in “standard” units (rad, rad/s and torque %) instead of raw values.

Recording and executing poses

I added a new test function which moves the robot through a sequence of queued poses that are saved in the GUI. The robot pauses between each sequence based on each pose’s specified delay time.

Because the function’s execution has to pause and wait for the specified dwell time time, I added the code into a separate thread. Originally I did this by subclassing QThread, but then updated it and created a “worker object” which is moved to a thread using moveToThread(), as this seems to be the better way in Qt.

Although this is very simplistic motion editor functionality which has been done many times before by other tools, I thought it would be useful to add to my GUI, as it could prove useful in the future as a complement or even a replacement of the MoveIt! ideas if they don’t work out. This GUI is now essentially a one-stop shop for easily testing various ROS functionality and other ideas as I keep on developing along the way!

New development PC

Progress has been slow as my laptop is having overheating problems, so running ROS for any extended periods was getting difficult. For this reason I have almost finished setting up a Linux development environment on my main PC which is more than capable of running ROS and all the graphical tools. I will keep using the laptop for less intensive tasks and for development away from home.

I am using Linux Mint 17.3 Rosa 64-bit on the PC vs Linux Mint 17 Qiana 32-bit on the laptop (laptop had Wi-Fi driver issues with 64-bit version), but still sticking with ROS Indigo instead of upgrading to the more recent Jade, to avoid any surprises.

Installation of ROS was straightforward as with the laptop, and follows the Ubuntu installation instructions, as Mint is built on Ubuntu. The only edit I had to make was change the repository name to use the Ubuntu Trusty codename instead of the Mint Qiana codename, so the repository name looks like this:

deb trusty main

I use Dropbox to keep everything in sync and share the same catkin workspace between the two machines. The only minor annoyance is having to delete the build and devel folders and rebuilding my packages when switching between machines, because of the 32-bit/64-bit difference.

Fitting the foot force sensors

I finally got round to installing the force sensing resistors to the underside of the Bioloid’s feet. I am using the FSR 400 Short model, with male end connectors, which are small and flat and didn’t fit in the square female header pins which often come at the end of breadboard wires. I didn’t want to solder directly as I’ve damaged these sensors in the past, so I followed a suggestion on the Adafruit website and ended up using these PCB terminal blocks.

The force sensing element of the FSR fits nicely in the pre-existing cut-out on the underside of the Bioloid foot plate, and the pins are fed through a hole in the plate so they can be connected on the topside. However, in the standard Bioloid humanoid configuration, the foot plates are attached at an offset from the ankle angle brackets, which means there is not enough space between the footplate and the bracket to connect the pins. To fix this, I moved the footplates so that the ankle brackets sit along their centres. There is still some free space between the footplates when the Bioloid sits in its resting position, so hopefully the feet will not collide with each other after this change.

For completeness, I also made changes to the CAD model and also added FSR CAD models to the feet, although that is just a cosmetic addition to the model (on a related side-note, I also found a CAD model of the USB2AX which adds to the realism)! The robot’s URDF files and CAD models have been updated on GitHub.

Here are some pictures of the results:

Further integration with MoveIt!

The integration of the Bioloid with MoveIt! has reached an important milestone, with the real robot now being able to respond to MoveIt! planned trajectories! There’s still a lot to do to improve performance, but the basic framework is ready. In the next few paragraphs I will try to describe the MoveIt! configuration process up to now.

In a previous post I mentioned how the SRDF file was created as the fist step to configuring the robot to use MoveIt! The joint_state_publisher was already set up to publish the values of the joint states, which it read from my AX-12+ motor controller, so RViz was able to display the current state of the robot’s joints live when connected to the robot. Commands could also be sent to move the robot servo’s (such as those sent by the control GUI), but this was done using my own custom ROS services. The link to closing the loop between MoveIt! and the robot was to make MoveIt! able to command motor movement.

The joint controllers

Some background reading quickly led to the tutorial page on controller configuration. In trying to figure out how to first make my AX-12+motor controller provide the required FollowJointTrajectory action, I first looked at the wiki of the actionlib stack and used a C++ SimpleActionServer. However this would mean having to then implement the trajectory following, something which is already by the ROS joint_trajectory_controller package. So instead I looked into how I could get the robot to use this controller, which to lead to reading about ros_control and a very good tutorial from Gazebo. It took some time to understand how all these packages work together, and finally found that I had to implement the hardware_interface for my AX-12+ motor controller. The biggest challenge was figuring out how to configure the controller YAML files and actually get the controller_manager to spawn the controllers correctly.

I have used various roslaunch files and configuration files that set up and run the joint controllers, which I will upload shortly onto my GitHub page, along with updates to the C++ ROS AX-12+ motor controller.

The connections between the various ROS nodes and topics have now grown quite a bit in complexity, compared to the initial ROS interconnections, and now look something like this (captured using rqt_graph):


Useful links

I have put together a list of links, mostly from the ROS answers site, which were very helpful in getting the controllers to finally work:

Control GUI updates

Progress is going well with my Qt control GUI. The main updates are:

  • New button to cut torque on all motors.
  • New motor value editor, used for writing to the servo control table addresses. The address of choice can be selected, and a value can be set to an individual servo or to all servos at once, using the broadcast ID (254). Also included are three custom functions which write position/speed or torque using more intuitive values: radians, rad/sec or % torque respectively.
  • New position dials for directly controlling the position of each motor.
  • In order to control and receive feedback from the motors, all these GUI functions interface with the ROS publish/subscribe mechanism or ROS services, which my custom usb2ax_controller ROS package is providing in its own separate ROS node.
  • I have been playing around with Qt style sheets to customise the GUI’s main elements such as background, buttons etc.. This was fairly time consuming but overall I think it improves the overall look and feel rather than just having standard grey boxes! I have also used QDockWidget to make various parts of the GUI easy to hide/show, pop out or dock and resize. This seems much more flexible than putting all widgets on one form, or using multiple standard windows or even tabbed windows.

Now the foundations are laid, it’s time to delve deeper into MoveIt!

Robot control GUI first steps

I have switched focus to software at the moment, and am writing a basic user interface that will let me interact with the servo controller and the ROS packages, specifically the MoveIt! Move Group Interface/C++ API.

The interface is written in Qt using the Qt Creator IDE. As this is very ROS focused, I am building the GUI as a ROS package using catkin_make, which was tricky to set up inside Qt Creator, but worked in the end!

Screenshot from 2015-09-22 23-45-47

Currently the GUI can start a ROS node and receive current and goal position/velocity for each servo. It also enables the creation of a list of joint poses from the current robot’s state, and the ability to add them to a second list, which will act as a queue for moving through sequences of poses later on. The lists can be manipulated and also saved/loaded to CSV text files. A group of slider widgets shows the robot’s current joint positions, with added markers to show ‘target’ positions.

A number of buttons are yet not implemented, but the idea is to make this a quick and easy interface to a number of motion plans using the MoveIt! API, as I found it cumbersome trying to test with just a simple test C++ file. Hopefully this is the first step to a GUI with many features to come!

All source code is available on GitHub if you want to have a look.

Backpack printed

The backpack print from Shapeways has arrived!

Overall I am very pleased with the quality of the print. The walls are only 1.22 mm thick which makes the design somewhat delicate with the standard plastic material, so if I had to print it again I would thicken them. The dimensions perfectly match the screws, battery and back of robot chest, so the backpack fits in without any problems. I should have also added a few more holes around the sides, as the wiring is fairly cramped. The battery cabling is fairly awkward, maybe I should have gone with an even smaller one than originally planned!

Along with the battery, the backpack also houses the SMPS2Dynamixel servo power adaptor and a bunch of wiring. The power adaptor has a tricky shape with servo connectors at a right angle to the DC jack, but I was able to use a smaller plug with its plastic casing removed in order to fit it inside the limited space.

Hopefully the battery is close enough to the centre of mass, and not too high, so it remains to be seen if the servos are powerful enough to keep the robot standing and moving upright. Now it is back to software, to get the robot to finally do some interesting moves!

New Bioloid backpack

Adding to the endless stream of distractions from the main programming, I decided that the current mess of wiring and components on the back of the Bioloid needed tidying up properly. So, I have made a 3D model for a backpack, and I’m waiting for it to be 3D printed at Shapeways. The dimensions are similar to those of the original case which held the CM-5. The two large holes on the sides are for slotting in the new 2200 mAh battery, as it was too big to fit within the backpack.

Robot VR model updates

The robot model has got a small update to make it better resemble its real counterpart rather than the default Bioloid humanoid.

I have also had some success testing the Move Group Interface/C++ API with the robot, as well as managed to start a Qt widgets project using ROS’s CMake build system. This is in its very early stages right now, but will form the basis for further tests and advanced motions with the MoveIt! package.

Playing around with Gazebo

I thought it would be interesting to see if I could get my current Bioloid URDF file into Gazebo and try out some simulations. It turned out not to be too difficult; in order to load the model in Gazebo, all I had to add to the URDF were inertial elements for each of the links.

I set up a couple of tables and beer cans in the scene, and dropped the poor Bioloid from a height!

If you already have a URDF description of your robot, Gazebo is fairly easy to set up and has great potential for any simulation-based project, however as I am focusing more on the physical robot, I probably won’t be using it much for the time being. That, plus the fact that my laptop can barely handle the program without overheating!

Git repo created

I have just set up a GitHub account and added the ROS setup code I am currently working on. You can find it all at

As I am a Git noob, here are some useful links which helped me along the way, and which may be of us to others:

Heads up!

The Raspberry Pi has now been fitted to the Bioloid using some spare brackets and sponge padding.

A GUI for the Pi’s screen has also been made using Python, Tkinter and the rospy library. This is actually my first Python program from scratch, so it’s far from perfect but it’s simple and does the job for now!

The program visualises the sensor data from the force sensors and IMU, which the Arduino is publishing on ROS topics. The slider values are unfiltered data, the same which is used by another ROS node to fuse the IMU data and provide a good estimate of the torso’s 3D orientation. The roll/pitch/yaw widgets are used to visualise a simple transformation of the accelerometer/magnetometer data, as was performed previously here. The force sensing resistor (FSR) widget is not active, as the resistors have not been wired in yet, but eventually each FSR’s value will be visualised with a coloured square box ranging from yellow (un-pressed) to red (fully pressed).

Custom Conky on the Raspberry Pi 2

I thought I’d share my Conky configuration currently running on my Pi for anyone interested, as it’s a great tool for displaying useful system information on the desktop. It’s designed for a 480×320 resolution screen, as it is running on this touchscreen, and it takes up all the available space on the Raspbian desktop, minus the menu bar (26 pixels high).

One problem that took me quite a while to solve was how to actually get Conky to launch at startup, as none of the usual methods seemed to work. In the end I found two ways of calling a startup script. First, create a simple Bash script, like this:

sleep 10 && conky -c ~/Scripts/.conkyrc_raspberry-pi-2 -d

Save it e.g. as ~/Scripts/ Then, make the script execute at startup in one of the following two ways:

  1. Open the file /etc/xdg/lxsession/LXDE-pi/autostart as sudo, and add the following line to the end (solution found here):
  2. Alternatively, create a file ~/.config/autostart/startConky.desktop, which contains the following lines (as explained here):
    [Desktop Entry]

Here are some screenshots of Conky in action. Feel free to use my config however you want!

My Conky config:

# Conky config file

# Avoid flicker
double_buffer yes

# Window
# Own window to run simultaneous 2 or more conkys
own_window yes
own_window_type normal
own_window_hints undecorated, below, sticky, skip_taskbar, skip_pager
#own_window_transparent yes

# Borders
draw_borders no

# Shades
draw_shades yes

# Position
gap_x 0
gap_y 34
alignment top_middle

# Behaviour
update_interval 2

# Font
#font arial
use_xft yes
#xftfont arial:size=8
xftfont Bitstream Vera Sans Mono:size=8
#xftfont Terminus:size=8
xftalpha 1 # Text alpha when using Xft

# Colours
#default_color d1d3c9
#default_shade_color 000000
own_window_colour 2B2B2B

# Prevent window from moving
use_spacer none

# Minimum size of window
minimum_size 464 280

# Maximum width of window
maximum_width 464

${font Arial:size=12}${color #ddaa00}${time %I:%M %p}${font}            $alignc${color #9fb6cd}UpTime: ${color }$uptime $alignr${color #9fb6cd}Temp.: ${color ff0000}${execi 2 vcgencmd measure_temp | sed 's/[^0-9.]*//g'} °C
${color }${hr}
${color #9fb6cd}CPUs${color }
${color #9fb6cd}1: ${color }${cpugauge cpu1 30, 60}  ${cpugraph cpu1, 30, 125 ffff00 ff0000 -t} $alignr${color #9fb6cd}2: ${color }${cpugauge cpu2 30, 60}  ${cpugraph cpu2, 30, 125 ffff00 ff0000 -t}
${color #9fb6cd}3: ${color }${cpugauge cpu3 30, 60}  ${cpugraph cpu3, 30, 125 ffff00 ff0000 -t} $alignr${color #9fb6cd}4: ${color }${cpugauge cpu4 30, 60}  ${cpugraph cpu4, 30, 125 ffff00 ff0000 -t}
${color }${hr}
${color #9fb6cd}MEM
${color }${memgauge 30, 60}  $alignr${memgraph 30, 380 0000ff ffff00 -t}
${color }${hr}
${color #9fb6cd}UP $alignc${color #9fb6cd}IP: ${color }${addr wlan0} $alignr${color #9fb6cd}DOWN
${color }${upspeedgraph wlan0 30, 220 0000ff ff0000 -t} $alignr${downspeedgraph wlan0 30, 220 0000ff 00ff00 -t}

ROS interconnections

In this post I will cover the current state of my ROS setup for controlling the Bioloid’s motors and custom hardware.

The rqt_graph package provides a way of visualising the interconnections between all running ROS nodes. The following graphs show the current setup:

Here is a breakdown of the running nodes:

  • bioloid_sensors (MCU Arduino code): The MCU is running an Arduino program using the rosserial_arduino library, and publishes the IMU’s data as ROS messages over serial-to-USB. The rosserial package was useful as an easy way of sending ROS messages from the MCU to the controlling PC.
  • serial_node: This is a node included in the rosserial_python package, which reads serial data from the MCU.
  • imu_tf_broadcaster: This is a custom node which uses the IMU’s data to calculate the robot’s orientation, as detailed in this previous post.
  • dummy_odom_frame_broadcaster: This is a static_transform_publisher node from tf, which transforms from the map to the odom frame without any actual change in orientation or position. This is simply to keep in line with the ROS conventions, as explained in this previous post.
  • ax_joint_controller: This is another custom node for communicating with the AX-12 servos. This is a work-in-progress, but currently acts as a ROS wrapper around the USB2Dynamixel/USB2AX library. It publishes the servo positions (in radians) as a ROS sensor_msgs JointState message, on the ax_joint_states topic. It also runs a number of ROS services for controlling the servos from other ROS nodes. These are either higher level commands (such as for setting all motors to home position), or low level commands for directly reading from or writing to the AX-12 Control Table’s addresses.
  • joint_state_publisher: The joint_state_publisher is a tool for setting and publishing joint state values for a given URDF file. The node here reads the values from the ax_joint_states topic (by setting the source_list parameter) and publishes on the joint_states topic. I suppose this node could currently be bypassed entirely, however it does provide a nice GUI window for visualising the joint positions as sliders, and is also useful for moving the robot’s joints in RViz when ax_joint_controller is offline.
  • robot_state_publisher: The robot_state_publisher works in tandem with the joint_state_publisher and publishes the state of the robot to tf. The robot_state_publisher reads the configuration of the kinematic chain from the URDF file (set by the robot_description parameter). The set up of the URDF file has been covered in a previous post. The virtual model of the robot is then displayed in RViz, with joint positions reflecting those of the physical robot.

That is about it for the current setup. I have some work-in-progress nodes which interface with the above framework and test some of the robot’s motions, so in a following post I will discuss these and show some new videos of the physical robot in action!

Wiring up the electronics

It’s been a while since the last update! I have mostly been working on the commununication between ROS and the servos, and now have a working read-write ROS servo interface, but will post about this later.

In the meanwhile, I added a bar display made up of 10 green LEDs for information feedback, as well as 4 blue decorative LEDs. I then moved the breadboarded electronics to a more permanent prototype board. I was initially hoping to make a box-shaped board which could fit nicely inside the Bioloid’s hollow “groin”, and would have the LED bar display in the front. However the USB cable connector protruded too much, so the LED bar will have to sit one side of the board. I also managed to squeeze the IMU on the underside of the board. There is still some room above the board, so I may be able to squeeze even more of the wiring/electronics in there.

Once the board was ready, I temporarily put all the hardware parts in roughly the right locations to gauge how much space will be taken up and how long the wiring should be. I have removed the AX-S1 sensor, as I probably won’t have much use for it. I think the Raspberry will sit much better on top of the torso rather than on the back, where the old CM-5 used to be. It’s very tempting to turn the screen into a VR face! As you can see, it will be a struggle to mount the massive 5500 mAh battery on the back, so I am planning on downsizing.

Here are some work-in-progress pictures plus a video:

Pi power!


Most of the parts for the Bioloid’s power supply have now arrived. Here are some pictures of the Pi 2 being powered by the 11.1 V, 5500 mAh battery. The battery also powers the SMPS2Dynamixel adaptor for the Dynamixel servos. The step-down to 5 V for the Raspberry Pi is performed by a 3A UBEC (Universal Battery Elimination Circuit), which has a handy MicroUSB output. This seems to be the ideal way of powering the Pi, rather than e.g. via the powered hub or GPIO pins, which bypass the voltage protection, especially when considering that the Pi has to power the peripherals. The USB2AX for the servo control is powered via the Raspberry’s USB port, as will the A-Star MCU.

Raspberry Pi 2

The Pi is currently running Raspbian and now also has this great 480×320 touchscreen from Adafruit! I installed ROS Indigo from source using this guide. I have also added Conky on the desktop, a lightweight and fully customisable system monitor, as a way of directly visualising the Pi’s current status. The touchscreen feature of the screen will be of use in the future, if some simple GUI is made for the robot.

Hardware layout

The next picture shows the current hardware layout of the whole robot. As there is a lot to power, I went for the beefy 5500 mAh battery, but as it weighs 417 grams, I might have to eventually get something lighter for the robot to be able to carry, at the expense of a shorter run time.

Hardware diagram

Hardware diagram

Representing the robot in ROS

Building a virtual robot

In this post, I’ll be describing the first stage in getting a working representation of the, as of yet unnamed robot, into the ROS world. I will also be briefly exploring the MoveIt! library, as this might be a useful tool for the future.

The virtual representation of the Bioloid will be built using CAD models of all the individual servos and support brackets. The robot will be loaded up in RViz, where its links an joints will be manipulated. Inverse kinematics will hopefully be calculated by the MoveIt! package later on. But first, a definition is needed of how the joints and links are all connected, and how to move between their frames of reference. The individual CAD components will be oriented correctly onto these frames.

On a side note, I had the chance to photograph the Bioloid in its current state, and also had a Raspberry Pi 2 delivered!

After some background reading, I found out that the best way to create this representation in ROS is by using a standardised XML format file called the Unified Robot Description Format (URDF).

CAD models

In the past I had made a start on drawing a CAD model of the Robotis servo, but since then Robotis has released CAD drawings of all their robot kits online here (old link).

I tidied up the .stp files using FreeCAD, by removing some placeholder parts or sub-parts which would be of no use (e.g. screws, gears and electronics on the inside of the servos and CM-5). I then converted the files into .dae (Collada) format, and imported them into Blender to add some colour textures. For some reason, saving again as .dae in Blender shrunk the model dimensions by 100. I haven’t worried too much about the actual component size at the moment, as only their relative scales have to be correct for now. There is also a bug in RViz which replaces the ambient color of Collada materials by light grey if at least one component of the specified ambient color is 0. To fix this, I manually edited the file and replaced all 0’s with 0.1 in the “ambient” tags. Below are some of the main components, as rendered in Blender.

The Unified Robot Description Format

The URDF file itself is written in a plain markup language. As the definition of various links is written more conveniently with some basic maths, and because many bits of code will inevitably be repeated or recycled (e.g. the mirroring of the left-hand side based on the right-hand side), ROS has a macro language called xacro, which makes it much easier to create and maintain URDF files. A URDF is generated from a xacro file using:

rosrun xacro model.xacro > model.urdf

Creating the file for the Bioloid took a fair while, as I created all the translations by eye without knowing the actual distance measurements between the various links, but rather by relying on the CAD components as each one was placed in the chain. I started with the right side of the model, first with the easier arms, then with the legs.

The validity of the URDF file can be checked with teh check_urdf tool. Another great tool for visualising the final result is urdf_to_graphiz, which generates a diagram of the joint and link tree. The tree of my model is shown below. Each joint (blue circles) is positioned with respect to its parent link (black rectangles), and the following/child link is positioned has the same origin as the joint, as shown here. The xyz and rpy labels next to the arrows show the translation and rotation required to get from parent frame to child frame, or in other words, it is the representation of the child’s frame with respect to the parent’s frame. You will also notice the addition of the IMU link, as well as an additional camera link, although this is just a placeholder for a possible camera in the future, and at the moment won’t be used.

Graphviz diagram of URDF file

Graphviz diagram of URDF file

The robot in RViz

The current state of the robot is shown in the screenshots below. The robot is displayed in RViz with the help of the ROS joint_state_publisher and robot_state_publisher. It is fully articulated and the individual joints can be moved with the help of the GUI which joint_state_publisher provides. The joint states will later be published from the joint values read by the real Bioloid servos. In addition to the kinematic model, I created bounding boxes around the components for the collision boundaries, which are shown in red. This was after the fact I realised that without them, the MoveIt! plugin would use the full CAD geometry in its collision detection routines, which made it almost grind to a halt!

Integration with MoveIt!

I have only played around with MoveIt! briefly so far, but the results seem very promising. The library has a useful graphical setup assistant, which essentially enhances the URDF with a Semantic Robot Description Format (SRDF) file. The URDF only contains information about how the joints and links are arranged, as well as some other information such as joint limits, and the visual and collision data. The SRDF doesn’t replace the URDF, but exists separately and contains other information, such as further self-collision checking, auxiliary joints, groups of joints, links and kinematic chains, end effectors and poses. So far I haven’t found any need to edit the SRDF directly, as it can be generated and edited by the setup assistant.

The assistant generates a new ROS package with various templates for path planning and visualisation, which is done via an RViz plugin. So far I have managed to interact with the virtual Bioloid’s arms and legs, in a similar way shown in this PR2 robot tutorial. The aim will be to later on create some poses and walking gaits which I can try out on the real robot, but that is all for now!

Useful links



RViz scaling and ambient colour issues:


Using quaternions to create a better IMU complementary filter

A better alternative to the RPY approach

After realising in my previous post that solving the gimbal lock problem for the complementary filter requires fiddly and inelegant fixes, I decided to dive into the world of quaternions. Luckily, the theory behind quaternions doesn’t need to be fully understood, in order to use them to represent 3D rotations. Particularly useful are the conversions between quaternion and rotation matrix, and axis-angle representation.

I initially tried to make the Arduino MCU (A-Star) perform the filtering process and pass the IMU frame transform to the PC, however this presented a couple of issues, the first being that there is no standard vector and matrix library in Arduino. Second, although I could write a very simple library for vector cross and dot products etc., the MCU’s 32 KB of flash memory was already almost full with all the rosserial and IMU libraries, leaving little space for anything else.
Hence I opted for letting the MCU simply pass a number of ROS message streams over to the PC, which could then do the required transformations using the ROS tf library. The Arduino code is listed at the end of this post (bioloid_sensors.ino).

The MCU reads the MinIMU-9 v3 IMU data and publishes it as ROS messages on the following topics:

  • LSM303 accelerometer x, y, z values (“accel”)
  • LSM303 magnetometer x, y, z values (“magnet”)
  • L3G gyroscope x, y, z values (“gyro”)
  • LSM303 magnetometer heading value (“heading”), derived from the LSM303’s library, as the angular difference in the horizontal plane between the x axis and magnetic North

ROS has a very handy plotting tool for topics, called rqt_plot. Below is an example dump of all the IMU data.

IMU messages plotted in rqt_plot

IMU messages plotted in rqt_plot

Quaternion-based filter

Having spent too much time on the RPY approach already, I wanted to find a simple way to achieve a relatively stable orientation from the IMU readings. I ended up implementing the method from this AHRS maths tutorial from the Firetail UAV system. The method is explained very well in that link, so there is no need to go into the details. The only change I have made is in the calculation of the filter coefficient, based on a set time constant as was done previously. My version using a ROS subscriber on the PC side is again listed at the end of this post (imu_tf_broadcaster.cpp and .h files).


Here are two videos comparing the original RPY approach against the improved quaternion approach. The resulting IMU transform is published by tf, which is used by the robot model you see in RViz, the ROS visualisation tool. The model is generated using a URDF file; I will explain this in detail in a following post.

Although in both cases the rotation is fairly smooth, you can see the problems that the RPY filtering encounters when it nears the gimbal lock position (when the robot lies horizontally). For my purposes of orientating the robot, I think the current quaternion approach is perfectly suited. I doubt I will be needing to play around with Kalman filters and the likes, as I currently don’t need the precision that UAVs etc. may need!

So that’s it for IMUs and orientation for the time being. In my next post I will start detailing the current progress with the virtual bioloid in ROS, which was seen in the above videos.

Code (click to expand):

// Hardware:
// Pololu A-Star 32U4 Mini SV
// Pololu MinIMU-9 v3 (L3GD20H and LSM303D)
// Interlink FSR 400 Short (x6)

// Important! Define this before #include <ros.h>
#define USE_USBCON

#include <Wire.h>
#include <ros.h>
#include <std_msgs/Int16MultiArray.h>
#include <geometry_msgs/Vector3.h>
#include <std_msgs/Float32.h>
#include <AStar32U4Prime.h>
#include <LSM303.h>
#include <L3G.h>

// Set up the ros node and publishers
ros::NodeHandle nh;
std_msgs::Int16MultiArray msg_fsrs;
std_msgs::MultiArrayDimension fsrsDim;
ros::Publisher pub_fsrs("fsrs", &msg_fsrs);
geometry_msgs::Vector3 msg_accel;
ros::Publisher pub_accel("accel", &msg_accel);
geometry_msgs::Vector3 msg_magnet;
ros::Publisher pub_magnet("magnet", &msg_magnet);
std_msgs::Float32 msg_heading;
ros::Publisher pub_heading("heading", &msg_heading);
geometry_msgs::Vector3 msg_gyro;
ros::Publisher pub_gyro("gyro", &msg_gyro);
unsigned long pubTimer = 0;

const int numOfFSRs = 6;
const int FSRPins[] = {A0, A1, A2, A3, A4, A5};
int FSRValue = 0;
LSM303 compass;
L3G gyro;
const int yellowLEDPin = IO_C7;  // 13
int LEDBright = 0;
int LEDDim = 5;
unsigned long LEDTimer = 0;

void setup()
    // Array for FSRs
    msg_fsrs.layout.dim = &fsrsDim;
    msg_fsrs.layout.dim[0].label = "fsrs";
    msg_fsrs.layout.dim[0].size = numOfFSRs;
    msg_fsrs.layout.dim[0].stride = 1*numOfFSRs;
    msg_fsrs.layout.dim_length = 1;
    msg_fsrs.layout.data_offset = 0;
    msg_fsrs.data_length = numOfFSRs; = (int16_t *)malloc(sizeof(int16_t)*numOfFSRs);


    // Wait until connected
    while (!nh.connected())
    nh.loginfo("ROS startup complete");


    // Enable pullup resistors
    for (int i=0; i<numOfFSRs; ++i)
        pinMode(FSRPins[i], INPUT_PULLUP);

    if (!compass.init())
        nh.logerror("Failed to autodetect compass type!");

    // Compass calibration values
    compass.m_min = (LSM303::vector<int16_t>){-3441, -3292, -2594};
    compass.m_max = (LSM303::vector<int16_t>){+2371, +2361, +2328};

    if (!gyro.init())
        nh.logerror("Failed to autodetect gyro type!");

    pubTimer = millis();

void loop()
    if (millis() > pubTimer)
        for (int i=0; i<numOfFSRs; ++i)
            FSRValue = analogRead(FSRPins[i]);
  [i] = FSRValue;
            delay(2);  // Delay to allow ADC VRef to settle

        // Compass - accelerometer:
        // 16-bit, default range +-2 g, sensitivity 0.061 mg/digit
        // 1 g = 9.80665 m/s/s
        // e.g. value for z axis in m/s/s will be: compass.a.z * 0.061 / 1000.0 * 9.80665
        //      value for z axis in g will be: compass.a.z * 0.061 / 1000.0
        // Gravity is measured as an upward acceleration:
        // Stationary accel. shows +1 g value on axis facing directly "upwards"
        // Convert values to g
        msg_accel.x = (float)(compass.a.x)*0.061/1000.0;
        msg_accel.y = (float)(compass.a.y)*0.061/1000.0;
        msg_accel.z = (float)(compass.a.z)*0.061/1000.0;

        // Compass - magnetometer:
        // 16-bit, default range +-2 gauss, sensitivity 0.080 mgauss/digit
        msg_magnet.x = (float)(compass.m.x);
        msg_magnet.y = (float)(compass.m.y);
        msg_magnet.z = (float)(compass.m.z);
        // Heading from the LSM303D library is the angular difference in
        // the horizontal plane between the x axis and North, in degrees.
        // Convert value to rads, and change range to +-pi = ( (float)(compass.heading())*M_PI/180.0 );

        // Gyro:
        // 16-bit, default range +-245 dps (deg/sec), sensitivity 8.75 mdps/digit
        // Convert values to rads/sec
        msg_gyro.x = (float)(gyro.g.x)*0.00875*M_PI/180.0;
        msg_gyro.y = (float)(gyro.g.y)*0.00875*M_PI/180.0;
        msg_gyro.z = (float)(gyro.g.z)*0.00875*M_PI/180.0;


        pubTimer = millis() + 10;  // wait at least 10 msecs between publishing

    // Pulse the LED
    if (millis() > LEDTimer)
        LEDBright += LEDDim;
        analogWrite(yellowLEDPin, LEDBright);
        if (LEDBright == 0 || LEDBright == 255)
            LEDDim = -LEDDim;

        // 50 msec increments, 2 sec wait after each full cycle
        if (LEDBright != 0)
            LEDTimer = millis() + 50;
            LEDTimer = millis() + 2000;


#include "imu_tf_broadcaster.h"

int main(int argc, char **argv)
    ros::init(argc, argv, "imu_tf_broadcaster");
    ros::NodeHandle n;
    ros::Rate loop_rate(1000);  // Hz

    Broadcaster broadcaster;

    ros::Subscriber accelSub   = n.subscribe("accel",   1000, &Broadcaster::accelCallback,   &broadcaster);
    ros::Subscriber magnetSub  = n.subscribe("magnet",  1000, &Broadcaster::magnetCallback,  &broadcaster);
    ros::Subscriber headingSub = n.subscribe("heading", 1000, &Broadcaster::headingCallback, &broadcaster);
    ros::Subscriber gyroSub    = n.subscribe("gyro",    1000, &Broadcaster::gyroCallback,    &broadcaster);

                tf::Transform(broadcaster.getQ(), tf::Vector3(0.0, 0.0, 0.0)),
                ros::Time::now(),"odom", "imu_link"));

//        std::cout << "dt: " << broadcaster.getDt() << std::endl;
//        std::cout << "q.x: " << broadcaster.getQ().x() << std::endl;
//        std::cout << "q.y: " << broadcaster.getQ().y() << std::endl;
//        std::cout << "q.z: " << broadcaster.getQ().z() << std::endl;
//        std::cout << "q.w: " << broadcaster.getQ().w() << std::endl;
//        std::cout << "----" << std::endl;

    return 0;

Broadcaster::Broadcaster() :
    q.setRPY(0.0, 0.0, 0.0);
    tfBroadcaster = new tf::TransformBroadcaster();



void Broadcaster::accelCallback(const geometry_msgs::Vector3::ConstPtr& msg)

void Broadcaster::magnetCallback(const geometry_msgs::Vector3::ConstPtr& msg)

void Broadcaster::headingCallback(const std_msgs::Float32::ConstPtr& msg)
    heading = msg->data;

void Broadcaster::gyroCallback(const geometry_msgs::Vector3::ConstPtr& msg)
    dt = ros::Time::now().toSec() - prevt;



    filterCoeff = timeConst / (timeConst + dt);

    // Use accelerometer and magnetometer data to correct gyro drift


    prevt = ros::Time::now().toSec();

//    std::cout << "angularVel x: " << angularVel.x() << std::endl;
//    std::cout << "angularVel y: " << angularVel.y() << std::endl;
//    std::cout << "angularVel z: " << angularVel.z() << std::endl;
//    std::cout << "----" << std::endl;

void Broadcaster::updateRotation()
    // New quaternion, from axis-angle notation for gyro
    tf::Quaternion qNew(angularVel.normalized(), angularVel.length()*dt);

    // Update previous value
    q *= qNew;

void Broadcaster::correctOrientation()
    // Use acceleration data only if vector is close to 1 g
    if ( fabs(accel.length() - 1) <= 0.1 )
        // These vectors have to be perpendicular.
        // As there is no guarantee that North is perpendicular to Down,
        // set North to the cross product of East and Down.
        // Gravity is measured as an upward acceleration.
        tf::Vector3 Down(accel);  // Should be -accel, but not sure why this produces inverted axes!?
        tf::Vector3 E = Down.cross(magnet);
        tf::Vector3 N = E.cross(Down);


        // The rows of the rotation matrix represent the coordinates in the original
        // space of unit vectors along the coordinate axes of the rotated space
        tf::Matrix3x3 gyroRotMat = tf::Matrix3x3(q);

        // Correction vector
        tf::Vector3 cv = ( N.cross(gyroRotMat.getRow(0)) +
                           E.cross(gyroRotMat.getRow(1)) +
                           Down.cross(gyroRotMat.getRow(2)) ) * filterCoeff;

        angularVel += cv;


#include "ros/ros.h"
#include "geometry_msgs/Vector3.h"
#include "std_msgs/Float32.h"
#include "tf/transform_broadcaster.h"

class Broadcaster
    virtual ~Broadcaster();
    void accelCallback(const geometry_msgs::Vector3::ConstPtr& msg);
    void magnetCallback(const geometry_msgs::Vector3::ConstPtr& msg);
    void headingCallback(const std_msgs::Float32::ConstPtr& msg);
    void gyroCallback(const geometry_msgs::Vector3::ConstPtr& msg);
    //void updateTimers();
    void updateRotation();
    void correctOrientation();
    tf::TransformBroadcaster* tfBroadcaster;
    tf::Quaternion getQ() const {return q;}
    double getPrevt() const {return prevt;}
    double getDt() const {return dt;}
    float getTimeConst() const {return timeConst;}
    void setTimeConst(float value) {timeConst = value;}

    tf::Vector3 accel;
    tf::Vector3 magnet;
    float heading;
    tf::Vector3 gyro;

    tf::Vector3 angularVel;
    tf::Quaternion q;
    double prevt;
    double dt;
    float timeConst;
    float filterCoeff;