URDF for Darwin Mini robot

robotis-darwin-mini-humanoid-robot

URDF + Darwin Mini

URDF (Unified Robot Description Format) is an XML-based DSL to describe robots, commonly used in ROS (Robot Operating System).  In this post, I describe how I wrote the URDF description of the ROBOTIS Mini humanoid robot (previously known as Darwin Mini) by ROBOTIS Co. Ltd.

The great thing about ROS is the availability of plenty of Tutorials to get started, and this is true for URDF.  I will therefore not go through all the details, but rather try to give an overview of the approach I took, as it could be applied to other robots as well.

ROBOTIS has kindly made available the 3D model (in STL format) for Darwin-Mini, which comes handy when it comes to describe the robot.

Visualising the model

ros3djs-darwin-miniIt is very important when building the URDF description, to constantly visualise it, to make sure we are on the right track.  The most common approach to visualise an robot in ROS is through RViz.  However, being on Mac OS X (where it is a pain to build ROS graphical tools like RViz), I chose another way: render the 3D model in my browser, using the very cool ros3djs ROS package, based on three.js.

As you can see, I have a little Jetty server (actually a Clojure ring server!) running on port 3000, which serves a static HTML page displaying the 3D view.  The browser connects to a WebSocket interface (see rosbridge_suite package) to obtain coordinate frames of all 3D objects in real-time (see TF package).

Building our model

OK, now that we have our infrastructure in place to visualise a model, we need to create it.

I actually built it progressively, one piece after another.  An URDF description is in fact a tree, were the root is the base link (which is normally attached to the world), and other links attached to the base link and to each other to form branches.  For example, the body structure has multiple branches: the head, 2 arms and 2 legs.  Pretty straightforward.

Right hand of Darwin Mini robotIn order to build the tree, I started with just one link, displayed it in the Web Viewer, tweaked its orientation, then attaching another link to it.  And so on.

I started by building the hand.  As you can see, all pieces are tight to each other, so the links are attached to each other through what is called a "fixed" joint (as opposed to moving joints).

Defining joints

So, for example, here is what we have:

    <joint name="body_torso_joint" type="fixed">
      <origin xyz="0 0 0" rpy="0 0 0" />
      <parent link="body_skeleton_link"/>
      <child link="body_torso_link"/>
    </joint>
    <link name="body_torso_link">
      <visual>
        <origin xyz="0 0 0" rpy="0 0 0" />
        <geometry>
          <mesh filename="package://darwin_description/meshes/DMF-B01(W).stl" scale="0.001 0.001 0.001" />
        </geometry>
        <material name="White" />
      </visual>
    </link>

Typically, a part (body_torso_link) is attached to the parent part (body_skeleton_link) through a "fixed" joint.  The <visual> tag refers to the STL mesh of the part being described, and <material> applies the color.

xl320-angle-rangeLet us look at moving joints now.  In URDF, there are multiple kinds of moving joint: floating, revolute or prismatic.  For Darwin Mini, we only have revolute joints.

When defining the "revolute" joints, we need to take into account the acceptable angle range of the XL-320 servos, and the fact that the “normal” (rest) position of our robot is at 150°.

Also, there are 2 situations: (1) the body of the servo is fixed and the rotating axis is connected to the child link; (2) the rotating axis is attached to the parent link and the body of the servo is rotating.  Abstracting away the geometry of the servos, here is what we get for case (1):

    <joint name="neck_servo_joint" type="fixed">
      <origin xyz="0 0 0" rpy="0 0 0" />
      <parent link="body_link"/>
      <child link="neck_servo_link"/>
    </joint>
    <link name="neck_servo_link">
      <visual><!-- ... --></visual>
    </link>
    <joint name="neck_joint" type="revolute">
      <axis xyz="0 0 1" />
      <limit lower="0" upper="${300*M_PI/180}" effort="${XL320_MAX_TORQUE}" velocity="${XL320_MAX_VELOCITY}" />
      <origin xyz="0 0 0" rpy="0 0 ${-150*M_PI/180}" />
      <parent link="neck_servo_link"/>
      <child link="neck_link"/>
    </joint>
    <link name="neck_link" />

Notice the body of the servo (neck_servo_link) is fixed to the parent link through neck_servo_joint, and we define a “dummy” link (neck_link) attached to the revolute joint (neck_joint).  Child links will then be attached to this dummy link.

Parametrise and modularise the robot model

When starting to build up our model, the code quickly becomes very long and verbose.  A common way to address this complexity in URDF descriptions is to make use of xacro (XML macro system) to define a model in a modular way.

We can define each basic part as a xacro block, with a few parameters.  Each block has a ${name} (which will be used as prefix for the joint and link names) and a ${parent} to which the child link will be attached.  Moreover, to describe symmetric parts, a nice trick I found (in PR2 description) is to pass a ${side} (“l” or “r” for “left” / “right”) and a ${reflect} parameter which takes a value of “1” for left side and “-1” for right side.  This ${reflect} comes handy for example when rotating the part in opposite directions for left and right sides.

Here is an example:

  <xacro:macro name="darwin_spu5" params="name parent xyz rpy">
    <joint name="${name}_joint" type="fixed">
      <origin xyz="${xyz}" rpy="${rpy}" />
      <parent link="${parent}"/>
      <child link="${name}_link"/>
    </joint>
    <link name="${name}_link">
      <visual><!-- ... --></visual>
    </link>
  </xacro:macro>

Putting it all together

Here are the xacro files I end up with:

materials.urdf.xacro # materials (colors) used in basic blocks
common.urdf.xacro    # basic blocks (1 per 3D part)
servo.urdf.xacro     # servo motor & revolute joints
hand.urdf.xacro      # hands
arm.urdf.xacro       # arms to which hands are attached
leg.urdf.xacro       # legs and foots
body.urdf.xacro      # body to which arms & legs are attached
base.urdf.xacro      # base where the body is attached
darwin.urdf.xacro    # main file --includes all the files above

Next steps

So far, I explained how I built the URDF description for Darwin Mini.  The next step will be to “gazebo-ise” the model (make it an SDF) so that it can be simulated in Gazebo, and eventually used to control the real Darwin Mini robot.

Three awesome MOOCs to build intelligent robots

Are you interested in robotics and AI? There are so many great learning resources online. Let me share the last 3 MOOCs (massive open online courses) I took recently and enjoyed a lot.

1. MITx 6.832x Underactuated Robotics by Prof. Russ Tedrake

In this excellent MITx online course, Prof. Tedrake introduces a novel view on robotics and control.  Instead of focusing on controllability and feedback linearisation, he goes beyond the limits by embracing the fact that systems in movement are often not completely controllable, and still can be guided to move as desired.

Having forgotten most of my mechanical engineering classes, I found the quizzes and assignments quite involved, but also very rewarding when reaching the solution.  Russ introduces Drake, a complete robotics framework written in Matlab and giving access to all concepts developed in the course.

Starting by (and constantly going back to) basic concepts such as the inverted pendulum or the double integrator, Prof. Tedrake takes his students through an exciting journey, giving the mathematical and numerical tools to solve real-world problems.

In this course, you will learn about walking, swimming, flying, juggling robots, and much more!

Thank you so much to Russ and his team for providing this awesome content.

2. Robotic Vision by Prof. Peter Corke

This course graciously offered by Prof. Corke at QUT (Queensland University of Technology, Brisbane) is following Introduction to Robotics, in which we built a robotic arm as part of the final project.  In this course, Peter talks about the fascinating field of robotic vision, that is, how can a robot learn about the outside world, just by analysing images taken by one or more cameras.

Vision is a very fundamental sense that we, human beings take for granted, but which is very powerful, and at the same time, challenging to equip a robot with, considering the overwhelming quantity of data and the many transformations required for a robot (computer) to make sense of it.

Prof. Corke explains very gently all the theory behind vision, the process of image formation, the relationship between the 3D and 2D worlds, equips the students with powerful image analysis tools (through the Robotic Vision Matlab toolbox, implementing all algorithms explained in the course), and even introducing more advanced topics such as image servoing (i.e. how a robot can orient and control itself through vision).

The assignments are relatively easy, but still give good foundations to get started and build vision-enabled robots.

3. Introduction to Natural Language Processing by Prof. Dragomir R. Radev

This MIT course offered on Coursera is less about robotics, but nonetheless touching the very hot topics of natural language processing, at the era of Siri, Cortana and other language capable machines.

You will learn all methods of natural language processing, including parsing, stemming, part-of-speech tagging, word sense disambiguation, information extraction, clustering, etc. all the way to question-and-answer systems, sentiment analysis, text summarisation, and machine translation.

Prof. Radev obviously is passionated by languages and seems to know so many of them.  He pronounces French and Japanese equally well (!) and draws all kinds of very interesting links between languages.  He introduces many algorithms to make a computer capable of processing human language for a variety of purposes.

Quizzes help you keep in mind the important points from the lectures, and the 3 programming assignments are relatively involved.  It certainly helps to know a bit of Python and be willing to spend time on really understanding the algorithms you are required to implement.

This lecture is a very good investment of your time if you are interested in NLP, and Drago is a pleasure to hear and learn from.

Happy Learning!

 

rosclj – ROS node in Clojure

Playing with robots, and learning ROS (robot operating system), I was happy to find out about rosclj, Clojure bindings for ROS.  I would be able to control robots from the REPL; how nice!

However, I quickly discovered that the only thing remaining from rosclj is its documentation: the code (originally in a SVN repo of Berkeley) has disappeared.  But never mind.  Clojure can do Java interoperop, right?  So I gave it a shot, and it turned out pretty well, so I thought I’d publish my findings because there is not much up-to-date information out there.

I started by familiarising myself with rosjava and create a pub-sub sample project following the nice tutorial here.  But don’t worry, you don’t really need to fiddle out with catkin to get Clojure talk to a ROS node.  This is actually simpler.

 

Your first ROS node in Clojure

Here are the steps:

1. Create a lein project (I called mine roscljtest), and add rosjava repository and dependencies to your project.clj.  Here is mine:

(defproject roscljtest "0.1.0-SNAPSHOT"
  :description "FIXME: write description"
  :url "http://example.com/FIXME"
  :license {:name "Eclipse Public License"
            :url "http://www.eclipse.org/legal/epl-v10.html"}
  :dependencies [[org.clojure/clojure "1.7.0"]
                 [org.ros.rosjava_core/rosjava "0.2.1"]
                 [org.ros.rosjava_messages/geometry_msgs "1.11.7"]]
  :repositories [["rosjava" "https://raw.githubusercontent.com/rosjava/rosjava_mvn_repo/master"]]
  :aot :all)

2. Write a ROS node in Clojure.  I simply translated the Listener from the rosjava tutorial to clojure:

(ns roscljtest.core
  (:import [org.apache.commons.logging Log]
           [org.ros.message MessageListener]
           [org.ros.namespace GraphName]
           [org.ros.node AbstractNodeMain ConnectedNode NodeMain]
           [org.ros.node.topic Subscriber]))

(gen-class
  :name "roscljtest.core.CljListener"
  :extends org.ros.node.AbstractNodeMain
  :prefix "clist-")

(defn clist-getDefaultNodeName [this]
  (GraphName/of "rosjava/listener"))

(defn clist-onStart [this connectedNode]
  (let [log (.getLog connectedNode)]
    (doto (.newSubscriber connectedNode "chatter" std_msgs.String/_TYPE)
      (.addMessageListener
        (proxy [MessageListener] []
          (onNewMessage [msg]
            (.info log (str "I heard: \"" (.getData msg) "\""))))))))

3. Build the Clojure code, and create a uberjar, which will be useful to avoid messing up with the classpath:

> lein uberjar

If everything goes well, the following jar file should be generated, and (notice the :aot :all in our project.clj) contain all .class files: roscljtest-0.1.0-SNAPSHOT-standalone.jar

4. Start ROS core and the Talker from the Java tutorial (don’t start the Listener, since we will start the one we just wrote in Clojure)

> roscore &
> cd src/rosjava_catkin_package_a/my_pub_sub_tutorial
> cd build/install/my_pub_sub_tutorial/bin
> ./my_pub_sub_tutorial com.github.rosjava.rosjava_catkin_package_a.my_pub_sub_tutorial.Talker &

5. Finally, start the Clojure node, as follows:

> java -cp target/roscljtest-0.1.0-SNAPSHOT-standalone.jar org.ros.RosRun roscljtest.core.CljListener

Once the connection is established, you should see the following output flow, at a rate of about 1 message per second:

Oct 17, 2015 8:28:18 PM org.ros.internal.node.RosoutLogger info
INFO: I heard: "Hello world! 26"
Oct 17, 2015 8:28:19 PM org.ros.internal.node.RosoutLogger info
INFO: I heard: "Hello world! 27"

Next Steps

OK, this is great.  And the thing I like the most, is that we don’t even need our project to be “catkinized”: we can just use Leiningen as usual (provided we specify the correct repo in our project.clj).

I was actually even able to run the CljListener on a separate machine (provided I set ROS_MASTER_URI and ROS_IP correctly on both machines): I did not even have to install ROS on the machine running the CljListener!

Next step will be to make this a little more idiomatic Clojure, because extending Java classes and overriding methods is no fun.  And I want to be able to run commands from the REPL, which I haven’t tried yet.

But all in all, this looks very promising.

PyCon JP 2015

I had the pleasure to attend PyCon JP 2015, held in Heisei Plaza in Tokyo on October 9-10, featuring the “Possibilities of Python”.  Although I am not really into Python (busy with Clojure & robots!), I thought is would be a good way to meet programming enthusiasts in Tokyo.  And it was.

My first impression about the Python community is its diversity.  Beyond gender diversity, I felt Pythonistas (seems to be how to call someone who writes Python) were coming from so many different areas: I met people in the devops world (of course), but also researchers, roboticists, data scientists, linguists, game developers, startup owners, photographers and hackers of all sorts.  Great community!

Now let me pick up a few talks/sessions which resonated with me.

My first (and favourite) talk is by Hideyuki Takei (@HideyukiTakei) on how to make a 4-leg robot learn to move on its own using reinforcement learning in Python.  His idea is simple, but really great: let the robot “try out” different possible actuator settings, and based on a reward (basically, the total travelled distance), learn the “good” patterns (i.e. the ones that will make the robot move forward).  I thought this was wonderful.  He is using Gazebo robot simulator for learning (to avoid making his robot actually move so many times), and showed off his robot walking.  It was impressive to see a real motion pattern emerge after only 400 iterations!

My next pick is actually a poster session by Ugo-Nama (@movingsloth) who presented his attempt to create a toad robot, particularly focusing on modelling of the perception of the world by the toad.  He is using (again) Q-learning and CNN (convolutional neural networks) to analyse the visual input of the toad and deduce whether the toad sees a “prey” or a “predator”.  This information can then be transformed them into actionable goals, such as “jump”, “run”, etc.  His work is based on the research of J.P. Ewert who studied neurophysiological foundations of behavior.

Make a Toad

I also liked the talk of Tomoko Uchida (@moco_beta) on natural language processing, and in particular Morphological Analysis using Python.  This was appealing to me, because the problem of lexical parsing turns out to be more difficult in Japanese than in Western languages for example, where the separation between words is clear.  She explained how she built janome, a morphological analysis library written in Python.  I tried it out, and yes, it parses the following sentence correctly!

>>> from janome.tokenizer import Tokenizer
>>> t = Tokenizer()
>>> for token in t.tokenize(u'すもももももももものうち'):
...     print(token)
...
すもも 名詞,一般,*,*,*,*,すもも,スモモ,スモモ
も 助詞,係助詞,*,*,*,*,も,モ,モ
もも 名詞,一般,*,*,*,*,もも,モモ,モモ
も 助詞,係助詞,*,*,*,*,も,モ,モ
もも 名詞,一般,*,*,*,*,もも,モモ,モモ
の 助詞,連体化,*,*,*,*,の,ノ,ノ
うち 名詞,非自立,副詞可能,*,*,*,うち,ウチ,ウチ
>>> ^D

I also attended many other talks / poster sessions, including “How to participate to programming contests and become a strong programmer” by Chica Matsueda, “Building a Scalable Python gRPC Service using Kubernetes” by Ian Lewis, “Finding related phrases on large amount of tweets in real time using Tornado/Elasticsearch” by Satoru Kadowaki, “Python and the Semantic Web: Building a Linked Data Fragment Server with Asynco and Redis” by Jeremy Nelson, “Let it crash: what Python can learn from Erlang” by Benoit Chesneau, a panel session on diversity, “How to develop an FPGA system easily using PyCoRAM” by Shinya Takamaeda, “Sekai no Kao” (the Face of the World) by Hideki Tanaka et al., and “Rise of the Static Generator” by Justin Mayer.

Lots of great content, but especially great people.  Many thanks to the organisers for such a great conference.  I’m hoping to attend PyCon 2016… and maybe submit a talk?

Raspberry Pi robot with vision and speech on ROS

After having played with Lego Mindstorms NXT, this is my first “real” robot project.

I used the Raspberry Pi at the heart of my KANO to build a little robot.  It is based on the “Raspberry Pi camera robot” of Dawn Robotics (2WD Dagu Magician chassis), to which I attached the speaker shipped with KANO.

My long-term goal is to build a robot platform like the NAOqi OS running on NAO or Pepper.  These wonderful robots of Aldebaran robotics inspire me.

But let’s start small.

Installing ROS on the Pi and OS X

Building the Raspberry Pi camera robot was relatively simple, based on the video and blog post guidance from Dawn Robotics.

I then installed Raspbian on my Pi, very simple.  Next, I started installing ROS (Robot Operating System) -the “Indigo” distro- on the Pi, following these instructions.  This was a little trickier, but came out pretty well after all.

The next step was to install ROS on my Mac to control my robot remotely.  This is where the plan started to break: as mentioned on the ROS wiki, OS X support is experimental.  After a lot of wizzling and fiddling, I gave up, and decided to go for plan B: install Virtual Box on my Mac, with Ubuntu.  That was so much easier, because ROS installs mostly out-of-the-box on Ubuntu through apt-get.

After some network issues to allow my Virtual Box VM communicating with the Pi, I finally could run ROS remotely.  ROS core on the VM and a bunch of nodes on the Pi.

Programming the robot

Here is what my robot does:

  • It just sets a fixed speed on the wheel motors (about 10cm/sec)
  • It uses computer vision to analyse its own trajectory.  It will detect if it drifts to the left or to the right.
  • If a drift is detected, the robot will speak out “Left” or “Right” (by playing a wav file).

Here are the nodes I pulled together:

  • rosberrypi_cam: great little module by Dan Lazewatsky based on RaspiCam_Cv (I had first tried raspicam_node, but I needed raw image for processing, rather than compressed and wanted to choose the encoding) – I added input parameters to set the image width and height, as well as choosing the encoding.  It is available on Github.
  • ros_dagu_ctrl: a little node to control the Dagu chassis, and send commands to the motors as well as to the camera pan/tilt servos.
  • ros_sound: a little node to play out a wav file.
  • ros_trajectory_analyzer: the node running the main program: it commands the robot motors, uses OpenCV to analyse the image produced by the camera, and issues voice commands to the ros_sound node, to say “Left” or “Right”.

I will give more details on each node in another post.

The result

Here is what it looks like:

Note: if you listen carefully, you should hear the robot say “Left” and “Right” as it moves.

The image is analysed on the Mac using OpenCV.  Green lines show the direction of the points tracked from one frame to the next, thereby allowing to estimate the direction of the robot (blue line).  More on this later.

Conclusion so far

The Raspberry Pi is an exciting platform for robotic projects.

ROS seems very promising, as it offers a solid architecture for creating complex robotic applications.

(best? ide-for-clojure)

Hello, Clojureverse!

Since I started learning Clojure, I tried different development environments, including Eclipse and LightTable, but today, I am back to the simplest-thing-that-could-possibly-work: VIM.

Eclipse with CCWI started with Eclipse with counterclockwise. It has syntax highlighting and a REPL and it was OK when I worked on my first Clojure project: clogo, a LOGO turtle interpreter. (I’ll surely come back on this one). It certainly does the job, but I would not really recommend Eclipse for Clojure… too heavyweight.

After installing Leinigen and using VIM for a short period of time, I stumbled onto LightTable, which is clean and nice, with a minimalistic design. It seemed promising as it includes a REPL with real-time evaluation of the Clojure code you type. But I found it not very practical, because unlike an autocompletion feature, I like my REPL to wait until I’m done typing. So I ended up copying and pasting code from LT to my REPL, which I thought was probably not the best way. (The other thing that I didn’t like about LT is that the screen could only be split vertically; maybe it’s improved today).

In quest of a more integrated environment, I went back to VIM and tried the NailGun server for REPL integration. It is OK, and although the execution of Clojure code in a separate window seemed a good idea, it looks like NG is sometimes getting lost when I inadvertently close it’s buffer. That’s really a pain.

MacVim and vim-fireplaceAt this point, I found vim-fireplace (earlier called foreplay.vim) which is simple and really nice. It will find your REPL and automatically connect to it, and it can not only execute a block of code in a single keystroke, but also run your tests, or let you see the documentation for any function.

So to summarise: my environment today is Leiningen, and MacVim with vim-fireplace (+ vim-clojure-static and rainbow_parentheses.vim plugins). I am using clojure.test for testing, and cloverage to measure my test coverage. And I use criterium library together with VisualVM when I have to look at performance.

Voilà. Enough talking. Let’s start coding…