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

This post explores how to solve a maze using Proximal Policy Optimization (PPO) within a custom MuJoCo environment I built previously. We'll start by understanding PPO. The core mechanism of PPO is 'clipping,' which keeps policy updates within a safe range and prevents extreme changes.

The logic is quite straightforward and is based on the Right-Hand Rule. If the car drifts too far from the right wall or fails to stay parallel, it loses a reward. Furthermore, if it gets too close to the wall, it is also penalized. Conversely, maintaining the correct distance and alignment yields a positive reward. To implement this, we use LiDAR sensors to continuously measure the distance to the wall. The code below demonstrates this logic:

// ... rest of the code ...
forward, right_forward, right, right_back = obs

reward += ((action[0] + action[1]) / 2) * 1.5
reward += 0.5

if right < 1.5:
    parallel_error = abs(right_back - right_forward)
    dist_error = abs(right - self.target_dist)
    steering_penalty = abs(action[0] - action[1])
    reward -= parallel_error * 3
    reward -= dist_error * 2
    reward -= steering_penalty * 0.5
else:
    if action[0] > action[1]:
        reward += (action[0] - action[1]) * 3.0

if forward < 1.5:
    reward -= (1.5 - forward) * 10
    if action[1] > action[0]:
        reward += (action[1] - action[0]) * 3.0

if right_forward < 1.5 and right_back < 1.5:
    if action[1] > action[0]:
        reward += (action[1] - action[0]) * 3.0

if min(obs) < 0.3:
    reward -= 200.0
    terminated = True

info = {}
return obs, float(reward), terminated, truncated, info

Through this project, I realized that reward design is the key to reinforcement learning. Simply increasing penalties or rewards is not a perfect solution. In complex environments, excessive penalties often lead to the agent giving up (inaction), while excessive rewards cause reward hacking. Although this project isn't completely flawless, it successfully solved the core problem to a meaningful extent.

https://youtu.be/o87S0XbEy3A

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.

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.

• Implementation of a Pick-and-Place Task with a 6-Axis Robot

This article explains how to use numerical methods, which are a common approach to robot movement. First, we will explore Jacobian-based methods, which primarily consist of the Jacobian Transpose, the Pseudo-inverse, and Damped Least Squares (DLS). Following this overview, we will derive the Jacobian matrix and apply the Damped Least Squares (DLS) method to control the movement of a 6-DOF robot arm.

Step 1: State Backup and Target Orientation Setup

Eigen::VectorXd solve_ik(const Eigen::Vector3d &target_pos, int site_id) {
  // ... rest of the code ...
  Eigen::VectorXd original_qpos(m->nq);
  for (int i = 0; i < m->nq; ++i)
    original_qpos[i] = d->qpos[i];

  // ... rest of the code ...
  for (int i = 0; i < m->nq; ++i)
    d->qpos[i] = original_qpos[i];
  mj_forward(m, d);

  return target_qpos;
}

We must save initial qpos before calculation. If we don't, the robot arm will move instantly because the MuJoCo engine updates the robot's physical state during the calculation loop. By backing up the origin state, we can restore it after the calculation and ensure a smooth physical movement to the target.

Step 2: Calculating the 6-DOF Error Vector

  // ... rest of the code ...
  Eigen::Vector3d pos_error = target_pos - current_pos;

  // ... rest of the code ...
  Eigen::Vector3d ori_error = get_orientation_error(R_target, R_current);

  // ... rest of the code ...

In this step, we calculate the difference between the target pose and the current pose of the end-effector. In robotics, this difference is mathematically referred to as the 'error'.

Step 3: Extracting the Jacobian Matrix

void get_full_jacobian(Eigen::MatrixXd &J, int site_id) {
  std::vector<mjtNum> jacp(3 * m->nv);
  std::vector<mjtNum> jacr(3 * m->nv);

  mj_jacSite(m, d, jacp.data(), jacr.data(), site_id);

  for (int i = 0; i < 3; ++i) {
    for (int j = 0; j < m->nv; ++j) {
      J(i, j) = jacp[i * m->nv + j];
      J(i + 3, j) = jacr[i * m->nv + j];
    }
  }
}

Our ultimate goal is to find out how much the motors need to rotate (\(dq\)) to cover the spatial error (\(dx\)). Mathematically, the relationship is defined by the Jacobian matrix as \(dx = J \cdot dq\). Therefore, to find \(dq\), we need to calculate the inverse of the Jacobian (\(dq = J^{-1} \cdot dx\)). The first thing we must do is extract this \(J\) matrix.

Instead of providing a single combined matrix, the mj_jacSite function calculates the Jacobian Position (jacp) and the Jacobian Rotation (jacr) separately as 1D arrays. Since we are using the Eigen library for complex matrix operations in the next steps, we need to. convert these raw 1D C-style arrays into a proper 2D Eigen matrix.

Step 4: Computing the Inverse Matrix using DLS

    // ... rest of the code ...
    Eigen::MatrixXd lambda_matrix =
        Eigen::MatrixXd::Identity(6, 6) * (damping * damping);
    Eigen::MatrixXd J_inv =
        J.transpose() * (J * J.transpose() + lambda_matrix).inverse();
    // ... rest of the code ...

To find the joint changes (\(dq\)), we traditionally use the Moore-Penrose Pseudo-inverse:

Step 5: Virtual Integration and Forward Kinematics

    // ... rest of the code ...
    mj_integratePos(m, d->qpos, dq.data(), step_size);
    mj_forward(m, d);
    // ... rest of the code ...

Once we obtain the required joint changes (dq), we must apply them to our robot. However, applying the full change all at once can lead to inaccuracies. To minimize the error as much as possible, we need a process of iterative refinement—a method fundamentally identical to how Artificial Intelligence (AI) optimizes models.

Just like Gradient Descent in AI, we take small, iterative steps instead of one massive leap. We use the mj_integratePos function to update the virtual joint positions (d->qpos) by adding a fraction of dq.

After taking a small step, we call mj_forward to update the forward kinematics. This allows us to check the new position of the end-effector in the virtual space. We repeat this process—stepping and checking—until the error is minimized below our target tolerance.

Step 6: Finalizing the Solution and Restoring Physics

      // ... rest of the code ...
      Eigen::VectorXd target_qpos = solve_ik(current_target_pos, attachment);

      for (int i = 0; i < m->nu; ++i) {
        if (i == 6) continue;
        int joint_id = m->actuator_trnid[i * 2];
        int qpos_adr = m->jnt_qposadr[joint_id];
        d->ctrl[i] = target_qpos[qpos_adr];
      }
      // ... rest of the code ...

Finally, we have our solved joint positions (target_qpos). Now we must send these target angles to the actual motors to make the robot move. However, we cannot simply copy the array over. In MuJoCo, the control array (ctrl) and the position array (qpos) have different sizes and structures.

This requires a specific mapping process. Here is how we track down the correct index.

• m->nu : The total number of actuators (motors). We loop through each one.

• if (i == 6) continue; : We skip the 6th actuator, which controls the gripper. The gripper is operated separately by our State Machine (open/close commands), not by the IK solver.

• actuator_trnid : This array tells us which physical joint is driven by the current motor.

• jnt_qposadr : Once we know the joint ID, this array gives us the exact starting address of that joint's data inside the massive qpos array.

By tracing this path, we extract the exact target angle for each specific joint from our target_qpos and feed it into the d->ctrl array. The physics engine then takes over, applying the necessary forces to smoothly drive the robotic arm to the target position. You can see the final result of this entire process in the video below and find the complete simulation source on my GitHub.

This paper explains the ROS 2 framework, which provides essential components for robotics development environments. It also introduces real-world case studies to demonstrate how ROS 2 can be utilized and what makes it so useful.

Motivation

It is almost impossible to discuss modern robotics development without mentioning ROS 2. As an open-source platform, it has a dominant market share compared to other robot operating systems. This is why I wanted to learn deeply about ROS 2. I firmly believe that properly mastering one foundational robot OS will help me easily adapt to other development environments, and even build a strong foundation if I ever design a new robot OS in the future.

Key Takeaways

This paper offers a comprehensive breakdown of ROS 2. First, it introduces the background of ROS 2 and highlights the key differences and improvements over ROS 1. Then, it dives into specific topics: scope, design principles, design requirements, communication patterns, middleware architecture, software quality, performance, reliability, and security. Notably, the paper provides benchmarking test data in the performance section, offering a clear visual representation of its capabilities. Finally, it presents real-world use cases, showing ROS 2 as an enabler, an equalizer, and an accelerator in the robotics industry.

Personal Insights

From a current perspective, ROS 2 is undoubtedly an excellent framework. However, if I had to point out one downside, it would be its practical reliance on Ubuntu. It is not that you cannot develop on Windows or macOS, but the limitations are quite clear. This practical limitation feels a bit disappointing.

Reference

• Robot Operating System 2: Design, Architecture, and Uses In The Wild

• Exploring Robotic Dynamics with MuJoCo: 2-DOF Modeling

• Exploring Robotic Dynamics with MuJoCo: Calculating 3-DOF coordinates

In this post, I will demonstrate a simple pick-and-place operation using a UR5e robotic arm. The sequence follows a standard workflow: starting from the home position, moving to the red box, grasping it, transferring it to the target location, and finally releasing it.

Since the UR5e features the updated offset wrist design (unlike older models), I performed some calibration for accuracy. However, please note that there is still a slight margin of error in the final movements.

This image below

The image above shows the robot's home position. First, we calculate \(\theta_1\) using the base and target coordinates. During this step, a calibration is applied based on Link 5, which serves as the virtual wrist center. Then, \(\theta_2\) and \(\theta_3\) are derived using the Law of Cosines, with a slight compensation applied to the height value.

After moving to the calculated position and grasping the box, the next coordinates are re-calculated using the same method for the transfer and release. Throughout the process, the state of each step is recorded, and subsequent commands are executed based on the displacement and the gripping force.

Below is an animation demonstrating the full pick-and-place workflow.

Post-Implementation Review

Since the UR5e doesn't have the traditional wrist design, I had to spend quite a bit of time studying and applying various calibrations. While the results weren't perfectly precise, it was a deeply rewarding experience because I learned so much by tackling the challenges head-on.

You can find the complete simulation source code here.

• Exploring Robotic Dynamics with MuJoCo: 2-DOF Modeling

In this session, to handle a 6-DOF robot, we will first learn about calculating 3-DOF coordinates. This practice will help us understand how to move the robot. We will fix 3 axes and manipulate the other 3, because starting with full 6-DOF control might be difficult to understand.

This image below shows robot's initial position. We will keep axes 4, 5, and 6 fixed at 0 degrees, and use axes 1, 2, and 3 to move the robot arm toward the target, the red ball.

First, looking at the robotic arm and the target point from a top-down view, we will calculate how much the arm needs to rotate to face the target. Second, just as we did with the 2-axis robot, we will calculate the angles for axes 2 and 3 using the law of cosines. Below is the source code for the calculation process based on the UR5e.

JointAngles InverseKinematics::solve_ik_3dof(double x, double y, double z) {
  // First link length
  const double L1 = 0.425;
  // Second link length
  const double L2 = 0.392;
  joint_angles.theta1 = std::atan2(y, x);

  // The horizontal distance from the base link to the target.
  double r = std::sqrt(x*x + y*y);
  // The vertical distance from the base link to the target, adjusting for the base link's height.
  double r_v = z - 0.163;
  // The distance from the base link to the target.
  double d = std::sqrt(r*r + r_v*r_v);
  double cos_ratio = (d*d - L1*L1 - L2*L2) / (2*L1*L2);

  if (cos_ratio > 1.0) cos_ratio = 1.0;
  if (cos_ratio < -1.0) cos_ratio = -1.0;

  joint_angles.theta3 = std::acos(cos_ratio);

  // The angle between L1 and the line from the base link to the target.
  double inner_angle_1 = std::acos((L2*L2 - d*d - L1*L1) / (2*L1*d));
  // The angle of elevation to the target.
  double inner_angle_2 = std::atan2(z-0.163, r);
  joint_angles.theta2 = inner_angle_1 + inner_angle_2;
  return joint_angles;
}

Next, here is the simulation result.

Summary

Before diving into the full robotic arm control, I ran into a simple simulation using only three degrees of freedom (3-DOF). You can find the complete simulation source code here.

Before we get into complex robot manipulation, it’s essential to understand the fundamentals of how to get a robot to a desired location. We will focus on a straightforward simulation: moving the robotic arm from its current state to a specific target coordinate.

If we know the target coordinates and the robot’s current position, we can calculate exactly how the arm needs to move to reach that point. This method is called Inverse Kinematics (IK).

Conversely, if we already know the specific angles for each joint and want to calculate where the robot’s end-effector will end up, that process is called Forward Kinematics (FK).

In this first scenario, we will explore how to use Inverse Kinematics to calculate the required joint angles within a 2D XY coordinate system. Fortunately, in this planar setup, we can determine the angle for each joint simply by applying the Law of Cosines.

First, let’s establish our known variables: the robot’s base position, the link lengths, and the target coordinates.

For this simulation, we will define the robot’s base at the origin (0,0) and set the length of each link (joint) to 20cm. The target destination is set to x = 10cm and y = 30cm.

1. Calculate the distance to the target (\(r\))

$$r^2 = x^2+y^2$$

$$ r^2=10^2+30^2=100+900=1000$$

2. Solve for Joint 2 (\(\theta_2\)) using the Law of Cosines

$$r^2 = L_1^2 + L_2^2 - 2L_1L_2\cos(180^\circ - \theta_2)$$

$$1000 = 20^2 + 20^2 - 2(20)(20)\cos(\pi - \theta_2)$$

$$1000 = 400 + 400 - 800\cos(\pi - \theta_2)$$

$$200 = -800\cos(\pi - \theta_2)$$

$$\cos(\pi - \theta_2)=-cos(\theta_2) = -0.25$$

$$\theta_2 = \arccos(0.25) \approx \mathbf{1.318 \text{ rad}}$$

3. Solve for Joint 1 (\(\theta_1\))

$$\phi = \arctan(\frac{30}{10}) \approx 1.249 \text{ rad}$$

Although we could calculate this angle using arccos(x/r), the cosine function cannot differentiate between “up” (+y) and “down” (-y). It would return the same positive angle for both (10,30) and (10,-30). By using arctan, we preserve the sign information, allowing the robot to navigate the full \(360^\circ\) range.

$$\alpha = \frac{\theta_2}{2} \approx 0.659 \text{ rad}$$

$$\theta_1 = \phi - \alpha$$

$$ \theta_1 = 1.249 - 0.659 \approx \mathbf{0.590 \text{ rad}}$$

The figure above visualizes the calculation process to aid your understanding.

The video above shows a simple simulation of the scenario using MuJoCo. In this demo, I simply hardcoded the calculated values. You can press the Spacebar to start the simulation.

Summary

In this post, we explored the basics of Inverse Kinematics and calculation methods using a simple 2-DOF robot, followed by a simulation in MuJoCo.

The full code is available here.

Modeling Normal Stress (Axial Loading)

Normal stress occurs when a force acts perpendicular to a material's cross-section, causing it to stretch or compress.

$$\sigma = E\epsilon$$

• \(\sigma\) (Normal Stress): The intensity of internal force per unit area.

• \(E\) (Young’s Modulus): A fundamental material property representing its resistance to elastic deformation.

• \(\epsilon\) (Normal Strain): The ratio of deformation relative to the original length.

Modeling Shear Stress (Torsional Loading)

While normal stress involves stretching, shear stress involves sliding or twisting forces acting parallel to the surface.

$$\tau = G\gamma$$

• \(\tau\) (Shear Stress): The force per unit area acting parallel to the surface.

• \(G\) (Shear Modulus): The material's resistance to shape change, also known as rigidity.

• \(\gamma\) (Shear Strain): A measure of the angular deformation (how much the object has been tilted or twisted).

Poisson's Ratio (\(\nu\))

Next is Poisson's ratio(\(\nu\)). It serves as a fundamental measure of volumetric change and is a key parameter in determining a material's stiffness. The following formulas define the strain ratios under tensile loading, where the material is elongated along its axis:

Sectional Strain (\(\epsilon_{area}\))

This represents the ratio of the change in cross-sectional area to the original area.

ϵarea=ΔAA≈−2νϵ (where ϵ>0 denotes the axial tensile strain)

Volumetric Strain (\(\epsilon_v\))

This is the ratio of the change in volume to the original volume. A Poisson's ratio of 0.5 implies that the material is incompressible, resulting in zero volumetric strain.

$$\epsilon_v = \frac{\Delta V}{V} = \epsilon (1 - 2\nu)$$

Shear Modulus (\(G\))

The Shear Modulus relates the shear stress to the shear strain.

$$G = \frac{E}{2(1+\nu)}$$

Bulk Modulus (\(K\))

The Bulk Modulus measures a substance's resistance to uniform compression.

$$K = \frac{E}{3(1-2\nu)}$$

The graph below illustrates the variation of G and K with respect to Poisson’s ratio using MATLAB.

Summary

In this post, we explored Hooke’s Law and Poisson’s ratio, specifically focusing on how \(G\) and \(K\) each relate to Poisson’s ratio.

The full code is available here.

Loads are primarily classified into static loads and dynamic loads. First, static loads are forces applied slowly that remain constant over time.

• Normal Load: A force acting perpendicular to a surface, such as a book resting on a table.

• Shear Load: A force causing layers of a material to slide past one another, like the action of scissors.

In contrast, dynamic loads are forces that vary with time.

• Repeated Load: A load applied repeatedly in a single direction, typically varying in magnitude.

• Alternating Load: A load that cycles between opposite directions.

• Impact Load: A sudden, high-force load applied over a very short period.

Now, let’s look at Stress and Strain. Stress is defined as the force acting per unit area. Essentially, it represents the intensity of the internal force distributed within the material. Strain describes the deformation resulting from that stress. It is defined as the ratio of the change in dimension to the original dimension.

The graph below shows a stress-strain diagram generated from a MATLAB simulation of a mild steel tensile test. Note that the data has been slightly modified for educational purposes to make it easier to understand.

• Elastic Region: The initial phase up to the Upper Yield Point. This point is the peak stress before the transition to plastic deformation.

• Yield Drop & Plateau Region: After the stress drops to the Lower Yield Point, the Yield Plateau Region begins. Here, strain increases significantly with constant or fluctuating stress.

• Strain Hardening Region: Stress begins to rise again until it reaches the peak, known as the Ultimate Tensile Strength (UTS).

• Necking Region & Fracture: Beyond the UTS, the material enters the Necking Region, where it deforms under less force until final fracture occurs.

Summary

In conclusion, materials deform because the internal stress caused by external loads exceeds the material's limits. This is why plotting the stress-strain curve is so important—it allows us to make more informed decisions when selecting materials for our structural designs.

The full code is available here.