Analytical Kinematics Implementation

2-DOF ROBOT ARM

Analytical Inverse Kinematics driven VEX arm with servo calibration. Built to test workspace theories.

VEX V5 ROBOTICS KINEMATICS PYTHON
2-DOF
Planar Control
Analytical
IK Solver
Real-time
Trajectory Calc
v5
VEX Platform

// GALLERY

Back view of robot arm
Top view of robot arm
Side view of robot arm

// QUICK_FACTS

DEGREES OF FREEDOM
2 (planar)
LINK LENGTHS
L₁ = L₂ = 1 (normalized)
ACTUATION
Two VEX servos
CONTROLLER
VEX (RobotC)
WORKSPACE
≤ 2 link lengths from origin
HARDWARE
VEX servos, gear + chain drive

// GOAL

Create a robot arm with 2 degrees of freedom that can reliably reach any coordinate within two popsicle-stick lengths of the base.

// APPROACH

A horizontal two-link arm uses gear and chain to drive the elbow and a standard servo on the shoulder. I modeled the links with equal length (L₁ = L₂ = 1) so the analytical IK routine stays compact, then ported that solver into RobotC.

  • Originally planned upper servo at second stick, switched to chain and gear for stability
  • Changed from numerical to analytical approach after seeing RobotC runtime

// KINEMATICS

// FORWARD_KINEMATICS

q1 = angle 1,  q2 = angle 2
x  = cos(q1) + cos(q1 + q2)
y  = sin(q1) + sin(q1 + q2)

// INVERSE_KINEMATICS

r  = x² + y²
q1 = acos(r/2 - 1)

// SOURCE_CODE

// IK Controller for 2-DOF Arm
#pragma config (Motor, port2, shoulder)
#pragma config (Motor, port3, elbow)

const float PI = 3.14159265;
const float gorp = 180.0/PI;

int toCmd(float deg) {
  if(deg < 0) deg = 0;
  if(deg > 120) deg = 120;
  return (int)(-127 + (deg * (254.0/120.0)));
}

float elbo(float x, float y) {
  float r = pow(x,2) + pow(y,2);
  float q1 = acos(r/2 - 1);
  return q1;
}

float sholder(float x, float y) {
  float b = elbo(x, y);
  float q2 = atan2(y, x) - b/2;
  return q2;
}

task main() {
  float k = 1, l = 1;
  motor[shoulder] = toCmd(sholder(k,l)*gorp);
  motor[elbow] = toCmd(elbo(k,l)*gorp);
  while(true) wait1Msec(20);
}