Exploring Robotic Dynamics with MuJoCo: Calculating 3-DOF coordinates

Related Post:

• 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.