I am modeling a simple differential drive robot (such as the e-Puck, Khepera, etc.) with pyBox2D. This class of robots is usually controlled by indicating speeds for the right and left wheel in rad/sec.
However, Box2D can only control a (kinematic) body through two parameters: linear velocity (in meters/sec, as a 2D vector) and angular velocity (in rad/sec). I need to convert my wheel speeds to linear + angular velocities.
Linear velocity is actually straightforward. Given a wheel radius r in meters, and current robot orientation theta in radians, the forward speed is simply the average of the two wheel speeds in meters/sec and reduced to a vector according to current orientation:
(1) forwardSpeed = ((rightSpeed * r) + (leftSpeed * r)) / 2
(2) linearVelocity = (forwardSpeed * cos(theta), forwardSpeed * sin(theta))
I cannot quite figure out the correct formula for angular velocity, though. Intuitively, it should be the difference between the two speeds modulo the distance between the wheels:
(3) angularVelocity = (rightSpeed - leftSpeed) / wheelSeparation
in the limit cases: when right = left, the robot spins in place, and when either rightSpeed = 0 or leftSpeed = 0, the robot spins (pivots) around the stationary wheel, i.e. in a circle with radius = to the separation between the wheels.
I do not get the expected behavior with formula (3), though. As a test, I set the left wheel speed to 0 and progressively increased the value of the right wheel 's speed. The expected behavior is that the robot's should spin around the left wheel with increased velocity. Instead, the robot spins in circles of increasing radius, i.e it spirals outward, which suggests that the angular velocity is insufficient.
Notice that I am using a Box2D kinematic body for the robot, so friction does not play a role in my results.
