# Emulating the movement of a differential drive robot

I want to emulate the movements of an Khepera-like roboter (so a differntial drive robot) using Unity. It has two engines, the engine speed is set over the network by RPC magic which ends up calling the function move(). That function calculates the desired acceleration and saves it in a member variable.

FixedUpdate() then tries to accelerate up/down to the desired speed and by the desired rotation on each frame. To not mess with the general physic engine I am not setting those values directly but am using AddRelativeForce() and AddRelativeTorque(). However I have two problems:

• on a change of the desired velocity (the setpoint basically) the movement becomes very jerky. That is expected because the code doesn't try to ensure a smooth transation, as a linear interpolation would do. I'm not sure how I could do that while continuing to use AddRelative{Torque|Force}().
• The bigger problem is that the movement is wrong. The basic kinds of movement seem to work[1]. However when trying to driving in a cirlce (i.e. a steady Move(vL = 100, vR = 75), the green line) the robot moves completely irrational and I'm not sure why that is the case.

My code is as follows, some parts ommited for clarity:

private Vector3 acceleration;
private float phi;
public const float R = 0.5f;
public const float L = 1f;

// FixedUpdate  is called once per frame, before the physics engine starts
void FixedUpdate()
{
/* ... */

/* As AccelerateTo() multiplies by the framerate (to get a smooth movement independent
* of it) small accelerations such as (0.00558295, -0.00144335, 0.00295163) will lead
* to an de-acceleartion of (-0.3, 0.1, -0.1) so that the robot stands still. However
* that often overshoots.
* So if the robot should not accelerate and the current acceleration is very small,
* don't use AccelerateTo() but just modify it directly (though this is a kinda ugly
* hack, an PID controller would lead to more realistic behaviour)
*/
if (acceleration.Equals(new Vector3()) && rb.velocity.sqrMagnitude < Constants.DBL_EPSILON) {
rb.velocity = new Vector3();
} else {
rb.AddRelativeForce(acceleration, ForceMode.VelocityChange);
platePhys.addForce(acceleration);
}
acceleration = acceleration * (1 - Time.deltaTime);

/* rotate according to angles */
rb.AddRelativeTorque(new Vector3(0, (phi * 180 / Mathf.PI) * 5, 0), ForceMode.VelocityChange); // Force, Impulse
phi = phi * (1 - Time.deltaTime);
}

public void Move(float vR, float vL)
{
/* ... */

/**
* R = wheel radius
* L = distance between wheels
* phi = rotation angle (which way the robot is facing)
*     = R/L (v_r - v_l)
* dx = R/2 (v_r + v_l) cos(phi)
* dy = R/2 (v_r + v_l) sin(phi)
* https://www.coursera.org/learn/mobile-robot/lecture/GnbnD/differential-drive-robots
*/

phi = R / L * (vR - vL);
float dX = R / 2 * (vR + vL) * Mathf.Cos(phi);
float dY = R / 2 * (vR + vL) * Mathf.Sin(phi);

/* ... */

/* move forward */
acceleration = new Vector3(dY, 0, dX);
print(acceleration);

}


[1] Sorry for the german legend, translation follows: "Im Uhrzeigersinn" = clockwise, "Gegen ..." = CCW, "Vorwaerts" = forward, "Ruckwaerts" = backwards

• The article you linked doesn't really say anything about the movement system of that robot. Could you find a better way to describe the movement mechanics you want to implement? – Philipp May 26 '18 at 11:57
• @Philipp yes that's a good idea, I've added another link. There's small details sprinkled all over the code, I just forgot to put them all in one place for the purpose of this question – user45891 May 27 '18 at 8:47