Implementation of a Pick-and-Place Task with a 6-Axis Robot using Numerical Methods
Related Post:
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: \(J^+ = J^T (J J^T)^{-1}\). However, this formula has a fatal flaw. When the robot arm fully extends or aligns its joints (a state called 'singularity'), the determinant of (\(JJ^T\)) approaches zero. Dividing by near-zero causes the inverse matrix to explode to infinity, making the robot violently glitch and crash the simulation. So, we apply the Damped Least Squares (DLS) method.
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 massiveqposarray.
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.
