<html lang="en" xml:lang="en" xmlns="http://www.w3.org/1999/xhtml"><head><meta content="text/html; charset=UTF-8" http-equiv="content-type" /><meta name="description" /><meta content="clojure arduino robotics" name="keywords" /><meta content="Nurullah Akkaya" name="author" /><link href="/images/favicon.ico" rel="icon" type="image/x-icon" /><link href="/images/favicon.ico" rel="shortcut icon" type="image/x-icon" /><link href="/default.css" rel="stylesheet" type="text/css" /><link href="/rss-feed" rel="alternate" title="An explorer&apos;s log" type="application/rss+xml" /><link href="http://nakkaya.com/2011/07/28/holonomic-control/" rel="canonical" /><script src="http://cdn.mathjax.org/mathjax/latest/MathJax.js?config=TeX-AMS-MML_HTMLorMML" type="text/javascript"></script><title>Holonomic Control</title></head><body><div id="wrap"><div id="header"><h1><a href="/">nakkaya<span class="fade-small">dot</span><span class="fade">com</span></a></h1><div class="pages"><a class="page" href="/">Home</a> | <a class="page" href="/projects.html">Projects</a> | <a class="page" href="/archives.html">Archives</a> | <a class="page" href="/tags/">Tags</a> | <a class="page" href="/contact.html" rel="author">About</a><form action="http://www.google.com/search" id="searchform" method="get"><div><input class="box" id="s" name="q" type="text" /><input name="sitesearch" type="hidden" value="nakkaya.com" /></div></form></div></div><div id="content"><div id="post"><h2 class="page-title">Holonomic Control</h2><p>
Following snippets shows how to control a robot with omnidirectional
wheels. Holonomic wheels are wheels with 2 degrees of freedom. They are
also known as omni-directional drive wheels, omni-wheels or omniwheels
which ever floats your boat.
</p>

<p>
Omniwheels allows movement in any direction, at any angle, without
rotating beforehand. For an omniwheel robot to translate at a certain
angle, each motor needs to go at a certain speed in relation to the
other motors. Speed doesn't matter, just the ratios and to rotate at
some particular speed, we must add or subtract equally from the motor
speed of each motor.
</p>

<p>
These ratios can be calculated using the following formula assuming <i>n
&ge; 3</i>,
</p>

\begin{equation} 
  \begin{bmatrix} v_1&amp;v_2&amp;v_3&amp;v_4 \end{bmatrix}^T = 
  \begin{bmatrix}
    -sin\theta_1&amp;cos\theta_1&amp;1\\
    -sin\theta_2&amp;cos\theta_2&amp;1\\
    .. &amp; .. &amp; .. \\
    -sin\theta_n&amp;cos\theta_n&amp;1\\
  \end{bmatrix}
  \times
  \begin{bmatrix} v_x&amp;v_y&amp;R_W \end{bmatrix}^T
\end{equation}

<p>
The idea is really simple, in a nutshell each motor pushes the robot
in two directions <i>X</i> and <i>Y</i> (their <i>sin</i> and <i>cos</i>
components). Basically we are looking for the combination of motor
speeds v<sub>1</sub>, v<sub>2</sub>, v<sub>3</sub>, v<sub>4</sub>, that when combined gives us v<sub>x</sub>, v<sub>y</sub>.
</p>

<p>
All the angles of the motor axis are measured relative to the <i>x</i>
direction in the coordinate system of the robot. Each rotating the
robot in counter counterclockwise direction.
</p>

<div class="org-src-container">

<pre class="src src-clojure">(<span style="color: #ff5f00; font-weight: bold;">def</span> <span style="font-weight: bold; font-style: italic;">*wheel-angles*</span> [60 135 225 300])
</pre>
</div>

<p>
The intended direction of travel of the robot is calculated by the
control system in terms of V<sub>x</sub>, V<sub>y</sub> and R<sub>w</sub>.
</p>

<div class="org-src-container">

<pre class="src src-clojure">(<span style="color: #ff5f00; font-weight: bold;">defn</span> <span style="color: #d7af00; font-weight: bold;">velocity-coupling-matrix</span> [&amp; n]
  (reduce (<span style="color: #ff5f00; font-weight: bold;">fn</span>[h v]
            (conj h [(- (<span style="font-weight: bold; text-decoration: underline;">Math</span>/sin (<span style="font-weight: bold; text-decoration: underline;">Math</span>/toRadians v)))
                     (<span style="font-weight: bold; text-decoration: underline;">Math</span>/cos (<span style="font-weight: bold; text-decoration: underline;">Math</span>/toRadians v)) 1])) [] n))

(<span style="color: #ff5f00; font-weight: bold;">defn</span> <span style="color: #d7af00; font-weight: bold;">motor-ratios</span> [vcm x y r]
  (flatten (matrix-seq (matrix-multiply (matrix vcm) (matrix [[x][y][r]])))))
</pre>
</div>

<p>
Multiplying velocity coupling matrix with our intended direction of
travel results in a sequence of motor speed ratios.
</p>

<p>
At this point you have couple of options for movement,
</p>

<div class="org-src-container">

<pre class="src src-clojure">(<span style="color: #ff5f00; font-weight: bold;">let</span> [vcm (apply velocity-coupling-matrix <span style="font-weight: bold; font-style: italic;">*wheel-angles*</span>)
      base-rps 300]
  (<span style="color: #ff5f00; font-weight: bold;">defn</span> <span style="color: #d7af00; font-weight: bold;">motor-speeds</span> [{x <span style="font-weight: bold; text-decoration: underline;">:x</span> y <span style="font-weight: bold; text-decoration: underline;">:y</span>} r]
    (map #(* base-rps <span style="font-weight: bold; font-style: italic;">%</span>) (motor-ratios vcm x y r))))
</pre>
</div>

<p>
Calculate a normalized vector to your target and multiply that with a
base motor speed.
</p>

<div class="org-src-container">

<pre class="src src-clojure">(<span style="color: #ff5f00; font-weight: bold;">def</span> <span style="font-weight: bold; font-style: italic;">*max-velocity*</span> 5)
(<span style="color: #ff5f00; font-weight: bold;">def</span> <span style="font-weight: bold; font-style: italic;">*slowing-dist*</span> (* 5 <span style="font-weight: bold; font-style: italic;">*robot-width*</span>))

(<span style="color: #ff5f00; font-weight: bold;">defn</span> <span style="color: #d7af00; font-weight: bold;">arrive</span> [self target velocity]
  (<span style="color: #ff5f00; font-weight: bold;">let</span> [to-target (- target self)
        dist (magnitude to-target)
        ramped-speed (* <span style="font-weight: bold; font-style: italic;">*max-velocity*</span> (/ dist <span style="font-weight: bold; font-style: italic;">*slowing-dist*</span>))
        clipped-speed (min ramped-speed <span style="font-weight: bold; font-style: italic;">*max-velocity*</span>)
        desired-velocity (* to-target (/ clipped-speed dist))]
    (- desired-velocity velocity)))
</pre>
</div>

<p>
Or calculate proper velocity that will get you to your target. Above
is the arrive behavior from <a href="http://www.red3d.com/cwr/steer/">Steering Behaviors For Autonomous
Characters</a>, you can then convert that velocity to motor speeds using the
following formula, assuming your velocity is in \(\frac{meter}{sec}\) you can
convert it to \(\frac{radian}{sec}\) using,
</p>

\begin{equation}
  W = \frac{2 \times v}{d}
\end{equation}

<p>
where,
</p>

<ul class="org-ul">
<li>w -&gt; angular speed (\(\frac{radian}{sec}\))
</li>
<li>v -&gt; linear speed (\(\frac{meter}{sec}\))
</li>
<li>d -&gt; diameter (meter)
</li>
</ul>

<div class="org-src-container">

<pre class="src src-clojure">(<span style="color: #ff5f00; font-weight: bold;">def</span> <span style="font-weight: bold; font-style: italic;">*wheel-diameter*</span> 0.054)

(<span style="color: #ff5f00; font-weight: bold;">let</span> [vcm (apply velocity-coupling-matrix <span style="font-weight: bold; font-style: italic;">*wheel-angles*</span>)
      angular-velocity #(/ (* 2 <span style="font-weight: bold; font-style: italic;">%</span>) <span style="font-weight: bold; font-style: italic;">*wheel-diameter*</span>)]
  (<span style="color: #ff5f00; font-weight: bold;">defn</span> <span style="color: #d7af00; font-weight: bold;">motor-speeds</span> [{x <span style="font-weight: bold; text-decoration: underline;">:x</span> y <span style="font-weight: bold; text-decoration: underline;">:y</span>} r]
    (motor-ratios vcm
                  (angular-velocity x)
                  (angular-velocity y)
                  (angular-velocity r))))
</pre>
</div>

<p>
One thing that will bite you if you are not careful is that as the
robot's orientation changes, orientation of the robot's <i>x</i> axis also
changes, so the controller needs to account for that.
</p>

<p>
Matrix operations (from commons-math),
</p>

<div class="org-src-container">

<pre class="src src-clojure">(<span style="color: #ff5f00; font-weight: bold;">defn</span> <span style="color: #d7af00; font-weight: bold;">matrix</span> [seq]
  (Array2DRowRealMatrix. (into-array (map double-array seq))))

(<span style="color: #ff5f00; font-weight: bold;">defn</span> <span style="color: #d7af00; font-weight: bold;">matrix-multiply</span> [a b]
  (.multiply a b))

(<span style="color: #ff5f00; font-weight: bold;">defn</span> <span style="color: #d7af00; font-weight: bold;">matrix-seq</span> [m]
  (map seq (.getData m)))
</pre>
</div>
<div class="post-tags">Tags: <a href="/tags/#clojure">clojure </a><a href="/tags/#arduino">arduino </a><a href="/tags/#robotics">robotics </a></div></div><div id="related"><h3 class="random-posts">Random Posts</h3><ul class="posts"><li><span>10 Oct 2011</span><a href="/2011/10/10/cheaplist-in-clojure/">Cheaplist in Clojure</a></li><li><span>27 Jul 2010</span><a href="/2010/07/27/line-segment-circle-collision-detection/">Line Segment/Circle - Collision Detection</a></li><li><span>10 Jul 2012</span><a href="/2012/07/10/robocup-2012/">Robocup 2012</a></li><li><span>01 Jun 2010</span><a href="/2010/06/01/path-finding-using-astar-in-clojure/">Path Finding Using A-Star in Clojure</a></li><li><span>01 Dec 2009</span><a href="/2009/12/01/adding-inferior-lisp-support-for-clojure-mode/">Adding Inferior Lisp Support for clojure-mode</a></li></ul></div><div id="disqus"><div id="disqus_thread"></div><script type="text/javascript" src="//disqus.com/forums/nakkaya/embed.js"></script><noscript><a href="//disqus.com/forums/nakkaya/?url=ref">View the discussion thread.</a></noscript><a href="//disqus.com" class="dsq-brlink">blog comments powered by <span class="logo-disqus">Disqus</span></a></div></div><div id="footer"><a href="/rss-feed"> RSS Feed</a><p>&copy; 2018<a href="http://nakkaya.com"> Nurullah Akkaya</a></p></div></div><script type="text/javascript">
//<![CDATA[
(function() {
	     var links = document.getElementsByTagName('a');
	     var query = '?';
	     for(var i = 0; i < links.length; i++) {
		     if(links[i].href.indexOf('#disqus_thread') >= 0) {
								       query += 'url' + i + '=' + encodeURIComponent(links[i].href) + '&';
								       }
		     }
	     document.write('<script charset="utf-8" type="text/javascript" src="//disqus.com/forums/nakkaya/get_num_replies.js' + query + '"></' + 'script>');
	     })();
//]]>
</script></body></html>