#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);
    body.AddRelativeForce(accel);
}

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


Get this bounty!!!

Leave a Reply

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