Implementing Autonomous Target Navigation in MuJoCo via the Right-Hand Rule

In this post, we'll explore simple autonomous navigation using the right-hand rule before diving into more complex AI. Think of the right-hand rule as a person finding their way through a dark room by keeping their hand on the right wall. To simulate this, we'll use a simple car model equipped with a LiDAR sensor in MuJoCo.

First, we'll build a maze and a simple car model. The maze includes a destination point and several walls to navigate. The image below shows the simulation environment we'll be using.

The logic is quite straightforward. If the car drifts too far from the right wall, it moves back toward it. Conversely, if it gets too close to the wall, it steers away to maintain a safe distance. To implement this, we use the LiDAR sensor to constantly measure the distance to the wall. The code below demonstrates the logic:

void controller(const std::vector<double>& lidar_data, mjData *d) {
  double front_dist = lidar_data[0];
  double right_dist = lidar_data[270];
  double right_corner_dist = lidar_data[310];
  double speed = 1.0;
  if (front_dist < 0.8) {
    d->ctrl[0] = -speed;
    d->ctrl[1] = speed;
  } else if (right_dist > 0.5 || right_corner_dist > 0.5) {
    d->ctrl[0] = speed;
    d->ctrl[1] = speed * 0.05;
  } else if (right_dist < 0.5 || right_corner_dist < 0.5) {
    d->ctrl[0] = speed * 0.05;
    d->ctrl[1] = speed;
  } else {
    d->ctrl[0] = speed;
    d->ctrl[1] = speed;
  }
}

The code below shows how to retrieve the LiDAR sensor data:

const int NUM_RAYS = 360;
const double MAX_RANGE = 10.0;

std::vector<double> get_lidar_data(const mjModel* m, mjData* d, int car_body_id) {
  mjtNum* pos = d->xpos + 3 * car_body_id;
  mjtNum* mat = d->xmat + 9 * car_body_id;

  std::vector<double> lidar_data(NUM_RAYS, MAX_RANGE);

  for (int i = 0; i < NUM_RAYS; i++) {
    double angle = (i * 2.0 * M_PI) / NUM_RAYS;

    mjtNum local_vec[3] = {cos(angle), sin(angle), 0.0};
    mjtNum global_vec[3] = {0, 0, 0};

    mju_mulMatVec(global_vec, mat, local_vec, 3, 3);
    int geomid = -1;
    mjtNum normal[3];
    mjtNum dist = mj_ray(m, d, pos, global_vec, NULL, 1, car_body_id, &geomid, normal);

    if (dist > 0 && dist < MAX_RANGE) {
      lidar_data[i] = dist;
    }
  }
  return lidar_data;
}

The core of this implementation is the mj_ray function. It is a built-in MuJoCo utility used to measure distances within the simulation. If you'd like to learn more about how it works, you can check the official reference documentation.

https://www.youtube.com/watch?v=mFOSd44_NfA

Finally, you can watch the final result of the simulation in the video above. The complete source code for this project is available on my GitHub repository.