Robotic Arm Inverse Kinematics on Arduino

By Oleg Mazurov

Lynxmotion AL5D robotic arm

I’m proud owner of Lynxmotion AL5D robotic arm. The parts kit is of very high quality, and as a result, the arm is very strong and versatile. I wanted my arm to be portable and independent of big computers and all currently available controllers lack flexibility that I needed, therefore I started building my own controller around Arduino platform. This article shows first preliminary result of this work – inverse kinematics code which would be used to position the arm.

In robotics, inverse kinematics is a method to position a tip of some linked stricture in 3D space by calculating joint angles from tip X, Y, and Z coordinates. Much information about the subject exists on the web, for example, this introductory article explains the subject using simple trigonometry.

To move the arm, six servos need to be controlled ( five for the arm without wrist rotate ). Given that large amount of processing time would be spent calculating servo angles, I decided not to drive servos directly from Arduino pins and made simple servo shield using Renbotics schematic and library code. I built only half of the circuit using single 4017 counter – this gives me seven servo control channels, which is plenty.

In addition to the article linked above, I’d like to mention two other resources, which helped me tremendously during code development. First is Micromega Application Note 44, which shows inverse kinematics equations for similar arm. They also have nice video of working arm. It should be noted that gripper of AL5D arm has much simpler geometry, therefore second order polynomial calculations are not necessary. The second one is this Lynxmotion project page with Excel spreadsheet. Many formulas from the spreadsheet were used in my code; I also used the spreadsheet during debugging after modifying arm dimensions.

Below is first working draft of inverse kinematics code. It can be used as-is or transformed into a library. As presented, it shall be used with caution – no boundary check is performed so it is quite easy to inadvertently send the arm flying into your forehead or the control board. The code uses single-precision floating point math, which seems to be adequate for the task.

#include <servoShield.h>
 
/* Servo control for AL5D arm */
 
/* Arm dimensions( mm ) */
#define BASE_HGT 67.31      //base hight 2.65"
#define HUMERUS 146.05      //shoulder-to-elbow "bone" 5.75"
#define ULNA 187.325        //elbow-to-wrist "bone" 7.375"
#define GRIPPER 100.00          //gripper (incl.heavy duty wrist rotate mechanism) length 3.94"
 
#define ftl(x) ((x)>=0?(long)((x)+0.5):(long)((x)-0.5))  //float to long conversion
 
/* Servo names/numbers */
/* Base servo HS-485HB */
#define BAS_SERVO 0
/* Shoulder Servo HS-5745-MG */
#define SHL_SERVO 1
/* Elbow Servo HS-5745-MG */
#define ELB_SERVO 2
/* Wrist servo HS-645MG */
#define WRI_SERVO 3
/* Wrist rotate servo HS-485HB */
#define WRO_SERVO 4
/* Gripper servo HS-422 */
#define GRI_SERVO 5
 
/* pre-calculations */
float hum_sq = HUMERUS*HUMERUS;
float uln_sq = ULNA*ULNA;
 
ServoShield servos;                       //ServoShield object
 
void setup()
{
  servos.setbounds( BAS_SERVO, 900, 2100 );
  servos.setbounds( SHL_SERVO, 1000, 2100 );
  servos.setbounds( ELB_SERVO, 900, 2100 );
  servos.setbounds( WRI_SERVO, 600, 2400 );
  servos.setbounds( WRO_SERVO, 600, 2400 );
  servos.setbounds( GRI_SERVO, 600, 2400 );
  /**/
  servos.start();                         //Start the servo shield
  servo_park();
  Serial.begin( 115200 );
  Serial.println("Start");
  delay( 500 );
}
 
void loop()
{
 
  //zero_x();
  //line();
  circle();
 }
 
/* arm positioning routine utilizing inverse kinematics */
/* z is height, y is distance from base center out, x is side to side. y,z can only be positive */
//void set_arm( uint16_t x, uint16_t y, uint16_t z, uint16_t grip_angle )
void set_arm( float x, float y, float z, float grip_angle_d )
{
  float grip_angle_r = radians( grip_angle_d );    //grip angle in radians for use in calculations
  /* Base angle and radial distance from x,y coordinates */
  float bas_angle_r = atan2( x, y );
  float rdist = sqrt(( x * x ) + ( y * y ));
  /* rdist is y coordinate for the arm */
  y = rdist;
  /* Grip offsets calculated based on grip angle */
  float grip_off_z = ( sin( grip_angle_r )) * GRIPPER;
  float grip_off_y = ( cos( grip_angle_r )) * GRIPPER;
  /* Wrist position */
  float wrist_z = ( z - grip_off_z ) - BASE_HGT;
  float wrist_y = y - grip_off_y;
  /* Shoulder to wrist distance ( AKA sw ) */
  float s_w = ( wrist_z * wrist_z ) + ( wrist_y * wrist_y );
  float s_w_sqrt = sqrt( s_w );
  /* s_w angle to ground */
  //float a1 = atan2( wrist_y, wrist_z );
  float a1 = atan2( wrist_z, wrist_y );
  /* s_w angle to humerus */
  float a2 = acos((( hum_sq - uln_sq ) + s_w ) / ( 2 * HUMERUS * s_w_sqrt ));
  /* shoulder angle */
  float shl_angle_r = a1 + a2;
  float shl_angle_d = degrees( shl_angle_r );
  /* elbow angle */
  float elb_angle_r = acos(( hum_sq + uln_sq - s_w ) / ( 2 * HUMERUS * ULNA ));
  float elb_angle_d = degrees( elb_angle_r );
  float elb_angle_dn = -( 180.0 - elb_angle_d );
  /* wrist angle */
  float wri_angle_d = ( grip_angle_d - elb_angle_dn ) - shl_angle_d;
 
  /* Servo pulses */
  float bas_servopulse = 1500.0 - (( degrees( bas_angle_r )) * 11.11 );
  float shl_servopulse = 1500.0 + (( shl_angle_d - 90.0 ) * 6.6 );
  float elb_servopulse = 1500.0 -  (( elb_angle_d - 90.0 ) * 6.6 );
  float wri_servopulse = 1500 + ( wri_angle_d  * 11.1 );
 
  /* Set servos */
  servos.setposition( BAS_SERVO, ftl( bas_servopulse ));
  servos.setposition( WRI_SERVO, ftl( wri_servopulse ));
  servos.setposition( SHL_SERVO, ftl( shl_servopulse ));
  servos.setposition( ELB_SERVO, ftl( elb_servopulse ));
 
}
 
/* move servos to parking position */
void servo_park()
{
  servos.setposition( BAS_SERVO, 1715 );
  servos.setposition( SHL_SERVO, 2100 );
  servos.setposition( ELB_SERVO, 2100 );
  servos.setposition( WRI_SERVO, 1800 );
  servos.setposition( WRO_SERVO, 600 );
  servos.setposition( GRI_SERVO, 900 );
  return;
}
 
void zero_x()
{
  for( double yaxis = 150.0; yaxis < 356.0; yaxis += 1 ) {
    set_arm( 0, yaxis, 127.0, 0 );
    delay( 10 );
  }
  for( double yaxis = 356.0; yaxis > 150.0; yaxis -= 1 ) {
    set_arm( 0, yaxis, 127.0, 0 );
    delay( 10 );
  }
}
 
/* moves arm in a straight line */
void line()
{
    for( double xaxis = -100.0; xaxis < 100.0; xaxis += 0.5 ) {
      set_arm( xaxis, 250, 100, 0 );
      delay( 10 );
    }
    for( float xaxis = 100.0; xaxis > -100.0; xaxis -= 0.5 ) {
      set_arm( xaxis, 250, 100, 0 );
      delay( 10 );
    }
}
 
void circle()
{
  #define RADIUS 80.0
  //float angle = 0;
  float zaxis,yaxis;
  for( float angle = 0.0; angle < 360.0; angle += 1.0 ) {
      yaxis = RADIUS * sin( radians( angle )) + 200;
      zaxis = RADIUS * cos( radians( angle )) + 200;
      set_arm( 0, yaxis, zaxis, 0 );
      delay( 1 );
  }
}

Here is a short description of the code. The coordinate space follows one in Micromega AN44. set_arm() takes 3 coordinates and grip angle and sets servo angles for base, elbow, shoulder and wrist. Arm dimensions are defined in millimeters at the beginning of the sketch. Three short functions are written to demonstrate the functionality. zero_x() moves the arm in a straight line along the y-axis, line() moves the arm side to side and circle() moves the arm in a circle in y-z plane. In all 3 functions, wrist angle is set to 0 degrees, it can be changed to some other value to make motion more interesting.

Resources mentioned before the code listing are very helpful in understanding the calculations and problem solving approach. If they are not enough, please don’t hesitate to ask questions. I will be posting my progress as more code is developed. Stay tuned.

Oleg.

Circuits@Home

Posts

Bookmarks

Robotic Arm Inverse Kinematics on Arduino

99 comments to Robotic Arm Inverse Kinematics on Arduino

Recent Posts