Holonomic Drive Mathmatics Discussion

Have we all heard of holomonic drives here? If not let me expound… A holomonic drive uses things called “omniwheels” which are sort of like normal wheels, but with rollers mounted along the wheel which allow it to move perpendicular to the plane of the wheel. If you mount four of these with one pair perpendicular to the other. This allows motion in any direction from any starting position.

Now comes the interesting part, how on earth do you control this darn thing?! Sure you can move in any direction, but this would confuse any mortal driver. So I had the “brilliant” idea of using a gyro to give the robot absolute direction, that is when you push the stick away and it goes away, and vice versa, but in uncomplicating the drivers’ job I have complicated my own. Now not only must the joystick inputs be scaled and realigned, but now they must be modified by the gyro’s output, too. Oi. So here are some rudimentary equations…

true y input = (cos(angle from origin) * original y input) + (sin(angle from origin + 90) * original x input)

true x input = (cos(angle from origin + 90) * original x input) + (sin(angle from origin) * original y input)

You can see my dillema, especially after you add the z axis, though it should be a good deal simpler… PLEASE ADVISE! :ahh:

I developed a simple kiwi drive (wheels mounted at 45 degree angles) code for a prototype; let’s see if I remember it correctly. I used code very similar to the default 1 joystick code for translational (no spinning) movement. You can put your angle adjusted p1_x and p1_y axis into the translational code. Obviously, the motors have to be linked diagonally.

…| |

LF - left front motor
LR - left rear motor
RF - right front motor
RR - right rear motor

RF = LR = Limit_Mix(2000 + p1_y + p1_x - 127);
LF = RR = Limit_Mix(2000 + p1_y - p1_x + 127);

Now for spin:
Still it is pretty simple and much like the default code. You need a spin joystick (p2_x). Now left and right sides are pretty much linked.

RF = Limit_Mix(2000 + RF + p2_x - 127);
RR = Limit_Mix(2000 + RR + p2_x - 127);
LF = Limit_Mix(2000 + LF - p2_x + 127);
LR = Limit_Mix(2000 + LR - p2_x + 127);

As I remember, this works pretty well. Now just keep track of your gyro. Watch out for drift over time, as the error compounds. I would put some filter caps in the gyro to reduce the error. Also make sure your motors are all running in the same direction. GOOD LUCK!

Another thought I had when thinking about controlling this type of drive system using a gyro to note rotation would be a button on the OI to “Reset” the gyro to zero or something if it got out of whack.

You drive the robot so it faces the correct rotation for zero, for example, and the hit the “Reset” button and it knows that is supposed to be zero again.

Also, I was thinking that if you just used a pot for rotation and a joystick for movement, and instead of using gyros for absolute movement, you still have a front, you could do really cool thinks like effortlessly drive in a perfect circle. You would just turn the pot to rotate a little, and then drive left or right. As the robot drives left or right, it will rotate and cause you to drive in a circle. Thought that might be kind’ve cool. We would also have a foot switch or something to “Disable Rotation” so you could quickly stop the rotation and drive normally.

Good luck.

You can see my dillema, especially after you add the z axis, though it should be a good deal simpler… PLEASE ADVISE!

Why do you need to be able to control the robot in the z axis? If x and y are on the ground then that would the z axis pointing up in the air.

A pretty good decription is available in the article titled “Closed Loop Motion Control for Mobile Robots” by Rich LeGrand. You can buy it for $1.50 here:


Includes all of the kinematics equations you need for a four-wheel holonomic robot.

I believe John meant x and y to be movement in the horizontal plane and z to be rotation of orientation. Unless he wanted the robot to fly, which, knowing John, might be his intention…

Im not the programmer for our team, but we have had holonomic robots the last 2 years. We have found the best way is to have one joystick sort of the strafe joystick. push the stick forward and it makes the bot drive to the front, and pretty much it goes where ever you move the stick. Then another stick the controls the rotation. We have been devolping a system using a gyro to help correct for slippage. SPAM had a system last year that they could push the stick away from the driver and tbe bot would go away from you.

wow. I wanted to know how teams did this as I saw a few at GLR with 4 omni wheels that rotated…We used them on the front of the robot and 2 drive wheels in the back and it works out well…except omni wheels crack and break after rough use (competition)…so to anyone who uses them, watch them closely for cracking and sperating…and good luck :smiley:

That was what we called “steadySPAM”, here’s how it worked:

Given the joystick X and Y,
1) find the joystick angle of rotation, theta
2) find the joystick magnitude

Given the robot angular orientation psi, subtract it from theta.

From this new theta’ and the magnitude,
find a new joystick X and Y

Use the new joystick X and Y to determine the inputs
for the motors.

…given joyx, joyy and robot angular orientation wrt the field, theta.

  1. First, I’d flip the joystick x-axis, since it hurts my head to have values
    increasing to the left, then, move the origin to 0,0:

joyy1 = joyy-127
joyx1 = (255-joyxx)-127

Next, get the joystick angle of rotation:
note: …must trap divide by zero
if joyy = 127 and joyx = 127, then
joytheta = 0 // neutral
else if joyy = 127, then
if joyx <= 127, then
joytheta = 90 // right
joytheta = -90 // left
joytheta = arctan(joyx1/joyy1)

and the magnitude:
joyspeed = sqrt(joyy1joyy1 + joyx1joyx1)

You can get the robot psi by using a yaw rate sensor and integrating. We defined theta and psi similar to, but not quite like a compass. Straight up field is 0deg. Rotating to the right the angle changes from 0 to 180 deg. Rotating to the left the angle changes from 0 to -180deg.

Subtract psi from theta,
joytheta’ = joytheta - psi

The new joystick x and y are:
joyx’ = 255 - (joyspeedsin(joytheta’)+127)
joyy’ = joyspeed

(Use these to calculate the motor outputs. You can use the default code mixing equation, but we included an additional 45deg rotation on top of that.)

However, using robotaddicts equations, you can do something similar and avoid an arctan and a square root:

…given joyx, joyy and robot angular orientation wrt the field, theta.

  1. First, adjust the joystick axes:

joyy1 = joyy-127
joyx1 = (255-joyxx)-127

  1. Next rotate the joystick axes into the orientation of the robot.

joyy2 = joyy1cos(theta) + joyx1sin(theta)
joyx2 = joyx1cos(theta) - joyy1sin(theta)

Now, get back to a typical joystick set of axes:

joyy’ = joyy2+127
joyx’ = 255- (joyx2+127)

This took care of translation. For rotation, we had a separate stick and applied it similar to “The Lucas”


One quick comment that I would have is to also make a back-up drive system, which drives whatso-ever direction from the point of view of the robot. Your drivers I hope played enough video games to get this figured out (easily).

Just in case…
(and because your drivers actually might like this better, and because your absolute positioning system is going to drift, and it will be a hassle for drivers to figure out when to reset. etc.)

By using the driver’s reference frame instead of the robots (move the joystick forward and the robot drives away from you, no matter which direction its facing) allows you to translate AND rotate at the same time. The other holonomic implementations don’t allow you to have this “frisbee” motion. All you need is a pot or a set of buttons to adjust the robot’s perceived heading if it drifts. It has the added bonus of making it easier to use the gyro as feedback to cancel unwanted rotation.

This drive system is so weird to program. We tried it with a normal robot this year, but didn’t have enough time to finish. If you write your own code for finding the magnitude and direction of the joystick, avoid using arctan and square root functions at all costs (or make your own versions of them). We wrote our own square root function, but found out a little too late that an arctan2() call chews up 12% of your code space.

We decided at the outset to use integer math, so I don’t have any data on using floating point. Your experience, however, is insightful. We wrote table lookups for sine/cosine and arctan, and, like you, wrote an integer square root. We had no timing problems with the main loop.


Here’s the equations for a three wheel holonomic base. Since you’re using 4 wheels that are aligned on primary axises, you should be able to simplify it somewhat.

I tried simulating a 3 wheel holonomic base, and it was virtually (no pun intended) uncontrollable without some sort of PID. I guess 4 wheels shouldn’t be quite as bad though.

I tried simulating a 3 wheel holonomic base, and it was virtually (no pun intended) uncontrollable without some sort of PID. I guess 4 wheels shouldn’t be quite as bad though.

I think Wildstang built a 3 wheel holonomic back in 2001 or 2002. They didn’t use it in competition, they just distributed a really cool video (as Raul and those guys tend to do). If they got that thing to work on one of those old PBASIC Stamps, so it shouldn’t require to much processing. Of course they wrote StangPS on a Stamp, so it might require a lot of processing.[/quote]

Sorry to post and run like that, I’ve been a bit buzy. If you take a look above at the equations I posted your robot would go very screwy. This is turning out to be much harder than it sounded… and it sounded pretty tough to begin with! >sigh< I guess I’ll just have to beat my head on it some more… at least a robot POV holonomic would be easy to program…

Oh well. Time to get the team members with more years of math than me and bend their heads around it. Untill then, however, thanks for your help, and these equations will make the third concussion of my life… :smiley:

You’re thinking of 857’s Kiwi drive in 2002. http://stuweb.ee.mtu.edu/~alkrajew/FIRST/kiwi.mpg. The control was extremly simple.

:slight_smile: :slight_smile: :slight_smile:
Hooray! I have made a major breakthrough. Using my old ideas the robot could think it had an input greater than 255. What I have done is this:

cos(gyro_angle) * P1_y - sin(gyro_angle) * p1_x
cos(gyro_angle) + sin(gyro_angle)

What the bottom part does is makes it so that if both inputs are say, 255, then it boils down to (1*255)/1! This makes the scaled inputs never equal more or less than the maximum or minimum, respectively. I have left out the truncate function and the 0-centering of the inputs (and the extra +127 at the end) to simplify things, but you get the idea. No more of this :confused: here!

The conversion of these new, spiffy inputs to drive commands was a bit perplexing, too. Because the wheels look like this,

(imagine that they’re at 90 degree angles to each other)


the computations are a bit tricky. I boiled it down to two major situations and two minor situations in one major. The major situations are whether or not the inputs are more along the drive axis of that set of wheels, or more not-along it. A deviation of 45 degrees or less means that the drive is more along the axis, but a deviation of the inputs by more that 45 degrees is not along the drive axis. The minor situations are whether the wheel set should be forwards or backwards when the drive is more-aligned with the axis. A topographical-style look of an inputs-to-outputs graph makes it out like so:

non-aligned w/axis:
\ \ \
\ \ \
\ \ \

aligned w/axis:

If you get what I mean. In the aligned w/axis case the motors drive at the greatest (or least, depending on direction) input. In the not-aligned w/axis case it becomes a simple “what’s-this-angle?” and “what’s this-cos?” problem.

Thank you for listening, I hope you enjoyed this. If you have any ideas on how to improve this process, just post it here and I’ll get back to you. Maybe. I hope… :wink:

This is based on information in the article I posted in a previous thread.

If you have four omni wheels at right-angles to one another, the following psuedo code will give you basic control of the robot. If you fix Phi, the robot’s heading, at 0 then the movement will be relative to the robot. If you use a gyro and integrate the heading then the movement will be relative to the driver. Assume the wheels are labeled NORTH, EAST, SOUTH, WEST in the clockwise direction looking down on the robot. The X axis is perpendicular to the wheel labeled EAST and the Y axis is perpendicular to the wheel labeled NORTH. That means the Y axis is parallel to the axel for the NORTH/SOUTH wheels and the X axis is parallel to the axel for the EAST/WEST wheels. This is only a simplified version of the algorithm to get you started, you will have to work at it to get to to run really smoothly. Using feedback from encoders on the wheels or a gyro is almost a necessity.

A very simple way to apply the gyro feedback to remove unwanted rotation would be to compare the current gyro value to the commanded rotational velocity and add the difference to r. Not having tried this I don’t know how well it will work. You can divide the error by 2 or 3 to damp is slightly without adding the differential and integral terms.

sint16 g = (sint16)((gyro >> 2) - 127);
sint16 error = r - g;
r += error;

(Ex. r = 0, g = 10. This means you want to go straight but are rotating slightly clockwise. error = (0 - 10) = -10; r += error = 0 - 10 = -10; You will be increasing the counter-clockwise rotation because you want to go straight but are rotating slightly in the clockwise direction. This is a proportional-only feedback loop with a gain of 1)


  1. Heading is in integer degrees in the range [0,359]
  2. You have a math library that can calculate sine and cosine (I can give you one if you need it, using fixed-point decimals with 10 bits of precision).
  3. You have a type library that defines sint16 as a signed int and sint32 as a signed long.


Vx - X axis component of the desired velocity, range [0,254]
Vy - Y axis component of the desired velocity, range [0,254]
Vr - Desired rotational velocity
Phi - Current heading


PWM_North - PWM value applied to the wheel labeled NORTH
PWM_South - PWM value applied to the wheel labeled SOUTH
PWM_East - PWM value applied to the wheel labeled EAST
PWM_West - PWM value applied to the wheel labeled WEST


// convert inputs to signed integers
sint32 x = (sint32)pc_p1_x;
sint32 y = (sint32)pc_p1_y;
sint16 r = (sint16)pc_p2_x;
sint16 phi = robot.heading; // set phi to 0 to use robot’s reference frame

// declare local working variables
sint32 sinPhi = sind(phi);
sint32 cosPhi = cosd(phi);
sint32 x2;
sint32 y2;
sint16 vNorth = 127;
sint16 vSouth = 127;
sint16 vEast = 127;
sint16 vWest = 127;

// transform velocity inputs to range -127,127]
x -= 127;
y -= 127;
r -= 127;

// apply rotational transform to x and y velocity
x2 = (cosPhi * x) + (sinPhi * y);
y2 = -(sinPhi * x) + (cosPhi * y);

// drop the decimal from the new x and y components
x2 = ftoi(x2);
y2 = ftoi(y2);

// calculate wheel velocities
vNorth = x2 - r;
vSouth = -x2 - r;
vEast = -y2 - r ;
vWest = y2 - r;

// adjust values to match valid PWM range
pwm01 = AdjustPwm(vNorth);
pwm02 = AdjustPwm(vSouth);
pwm03 = AdjustPwm(vEast);
pwm04 = AdjustPwm(vWest);


// simple helper function to keep a signed PWM value inside
// the valid range

uchar AdjustPwm(sint32 val)
if (val > 127)
return 254;
else if (val < -127)
return 0;
return (uchar)(val + 127);

Sorry seanwitte. All that went right over my head.

Crud. The :confused: 's are back… DARN TANGENTS!!! Sin and cos are so easy to work with, so why am I getting numbers over 20,000?! Here are some various permutations of equations I have been trying… assuming that this is for inputs of robot_y > 127 and robot_x < 127.



You get the idea. I am soooo very lost. Maybe I should do some converting to sin’s and cos’s that I seem to do OK with… Luckily though, once I solve one non-aligned quadrant all the others are easy. I think I may be having some problems with tan not liking the parameters I have, but how would I know? Tan is the crazy one here, not me… right?! I may also have the problem of being the only one on my team who wants to keep going 'till at least the end of the school year, but that’s for another thread… Anyhoo.

Could someone out there tell me if I got the tail anywhere near the donkey with these equations? :o