Translate degrees into 3-motor movement

Question

I have a robot with three motors (red), each with an omni-directional (blue) (see example of omniwheel here). If I want the robot to move forward, I can activate both motor 1 and 3, and the wheel at 2 will spin freely, perpendicular to the direction its motor can spin.

How could I write a function that takes degrees as an input and outputs 3 values ranging from 0-1 (no motor speed, full motor speed), so the robot faces the same direction while moving towards the true bearing specified in degrees?

E.g. 45 degrees is inputted and the robot moves North-East, relative to North in the diagram, while maintaining a constant rotation.

Thanks.

Isn't it just that the speed of the outer edge of a wheel due to rotation (along the motor axis) should be equal to the magnitude of the component of the travel velocity in the direction perpendicular to the motor axis? Cool setup though, I learned that omni wheels exist. — Jay Calamari, Jun 14 '18 at 04:23

MBo · Answer 1 · 2018-06-14T05:21:07.937

Some thoughts:

Motor rotation velocities V1..V3 should be in ranges -1..1, where -1 is full clockwise rotation, 1 is full CCW rotatation.

Motors create moment of rotation. To exclude rotation of robot, sum of moments should be zero. For equal legs

  V1 + V2 + V3 = 0

When moments are compensated, every motor causes force along OX or OY axis corresponding to projection of it's velocity onto axis. To provide moving in direction Fi with speed S:

- V1 * Sqrt(3)/2 + V2  - V3 * Sqrt(3)/2 = S * Cos (Fi)   //OX axis
 -V1 / 2 + V3 / 2  = S * Sin (Fi)                         //OY axis

Checking for moving up

   Fi  = Pi/2
   V1, V2, V3 = -1, 0, 1
   V1 + V2 + V3 = 0  //moment is OK
   Sqrt(3)/2 - Sqrt(3)/2 = 0  //zero speed or OX
   1/2 + 1/2 = S  //S speed for OY

In general: solve system of three linear equations, get V1, V2, V3

  V1 + V2 + V3 = 0
- V1 * Sqrt(3)/2 + V2  - V3 * Sqrt(3)/2 = S * Cos (Fi)  
 -V1 / 2 + V3 / 2  = S * Sin (Fi)

Solving for 45 degrees gives

   > solve({v1+v2+v3=0,-0.87*v1+v2-0.87*v3=0.71,-0.5*v1+0.5*v3=0.71},{v1,v2,v3});
   {v1 = -.90, v2 = .38, v3 = .52}

Some transformations to get closed-form solution:

  V2 = -V1 - V3
- V1 * Sqrt(3)/2 -V1 - V3  - V3 * Sqrt(3)/2 = S * Cos (Fi)  
  V1 + V3 =  - 2 * S * Cos (Fi) / (2 + Sqrt(3))
  V3 - V1 =  2 * S * Sin(Fi)
  and finally
  V3 = S * (Sin(Fi) - Cos (Fi) / (2 + Sqrt(3)))
  V1 = - S * (Sin(Fi) + Cos (Fi) / (2 + Sqrt(3))) 
  V2 = -V1 - V3

If after calculations some velocity has absolute value V that exceeds 1.0, divide all velocities by this value to ensure they are in range

(of course, real-life dynamic is more complex)

score 1 · Accepted Answer · answered Jun 14 '18 at 05:08

Interesting question! Would this work:

import math
def three_motors(degree):
    theta1, theta2, theta3 = 150.0, 270.0, 30.0
    motor1 = math.sin((degree-theta1)*(math.pi/180.0))
    motor2 = math.sin((degree-theta2)*(math.pi/180.0))
    motor3 = math.sin((degree-theta3)*(math.pi/180.0))
    return motor1, motor2, motor3

three_motors(0)
Out[40]: (-0.49999999999999994, 1.0, -0.49999999999999994)

Sign convention is if your eye was at the center and you were looking out at the spinning wheels, then a positive number corresponds to a counter-clockwise rotation and a negative number corresponds to a clockwise rotation. You could divide by the norm if that's required.

Thank you! This makes the problem simple for me. – Caspar B Jun 15 '18 at 06:53 — Caspar B, Jun 15 '18 at 06:53

score 1 · Answer 3 · answered Jun 14 '18 at 05:10

I'm not sure if you're asking for the actual code-writing or the math to write the code, but I can help with the math.

I'm starting from assuming the function has an input desired bearing Phi, and outputs v1, v2, and v3 for the motors.

v1, v2, and v3 and all going to be elements on the range [-1,1], where -1 will rotate the robot clockwise and an output of +1 will rotate counterclockwise.

The bearing input Phi determines which direction the robot will move. You can convert that into x_dot and y_dot. Theta dot represents angular rotation (and it will be 0).

x_dot = -sin(phi)

y_dot = cos(phi)

theta_dot = 0

x_dot = v1_x + v2_x + v3_x

y_dot = v1_y + v2_y + v3_y

theta_dot = v1 + v2 + v3 = 0

(For the equations below, I structured them so that straight up is zero degrees, and it increases going counterclockwise so that you can more easily generalize to other wheel placements)

v1 = v1_x + v1_y = - v1*sin(135)i + v1*cos(135)j

v2 = v2_x + v2_y = - v2*sin(270)i + v2*cos(270)j

v3 = v3_x + v3_y = - v3*sin(45)i + v3*cos(45)j

Now you will want to setup a system of equations.

x_dot = -sin(phi) = v1_x + v2_x + v3_x

y_dot = cos(phi) = v1_y + v2_y + v3_y

theta_dot = 0 = v1 + v2 + v3

This is a system of 3 equations with 3 unknowns, your unknowns being v1, v2, and v3. The matrix form of the equation will look like A*v = x, and the solution will be v = A^-1 * x.

This will give you values for your v vector (v1, v2, v3). Before outputting those values, I'd normalize them (just in case they aren't) by dividing by max(abs(v1,v2,v3)).

If you want to further edit your code to allow for the robot to change the direction its facing while also moving, simply make Theta_dot non-zero (positive is counterclockwise, negative clockwise). In this case, you'd also have to take in to account the change in reference frame as you rotate.

Just a quick question - why are you defining the x and y values by sin and cos respectively? Isn’t cos related to x and y related to sin? — Caspar B, Jun 15 '18 at 06:34

Translate degrees into 3-motor movement

3 Answers3