# #StackBounty: #unity #movement Emulating the movement of a differential drive robot

### Bounty: 100

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()
{
/* ... */

/* move. During the tests accelerating maximally lead to the following
* maximal acceleartion:
* sqrt(11.2^2+0.7^2+0.7^2) = 11.24366488294630737835
*
* So let's say 15 is the maximal value that can be set via the controller.
* Currently there's weird shit happening where after the robot rotated it moves
* extremly fast, so cap the maximal applied acceleration.*/
const float MAX_ACCELEARTION = 15;

/* 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 {
AccelerateTo(rb, acceleration, MAX_ACCELEARTION);
}
acceleration = acceleration * (1 - Time.deltaTime);

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

/**
*  https://gamedev.stackexchange.com/a/113203
*/
private void AccelerateTo(Rigidbody body, Vector3 targetVelocity, float maxAccel)
{
Vector3 deltaV = targetVelocity - body.velocity;
Vector3 accel = deltaV / Time.deltaTime;

if (accel.sqrMagnitude > maxAccel * maxAccel)
{
accel = accel.normalized * maxAccel;
}

print(body.velocity + " ==> " + deltaV + " ==> " + accel);
}

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

/**
* 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

Get this bounty!!!

This site uses Akismet to reduce spam. Learn how your comment data is processed.