Shopping Cart

Posts
«  
  »

Robotic Arm Inverse Kinematics on Arduino

Lynxmotion AL5D robotic arm

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.

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154

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

Related posts:

  1. Controlling robotic arm with Arduino and USB mouse
  2. Arduino USB Host Mini – first prototype.
  3. Arduino USB Host Mini – initial revision
  4. Digital camera control from Arduino-hosted webpage
  5. Vigorius stirring redefined. Part 1 – mechanics.
  6. Vigorius stirring redefined. Part 2 – electronics
  7. PTPDevinfo in 16K
May 12th, 2010 | Tags: , , , , , , , | Category: Arduino, MCU, programming | 44 comments

44 comments to Robotic Arm Inverse Kinematics on Arduino

  • karan
    July 11, 2010 at 9:03 am · Reply

    HI can you tell me if i want to use this for my custom arm which may have less no of servos, and different measurement of arm segment.

    Thanks for advance

  • bld
    July 15, 2010 at 12:18 pm · Reply

    Love that arm, but do you have any video’s of it running? Would love to see it use the wrist and all at the same time.

  • Gabriel
    July 22, 2010 at 4:24 pm · Reply

    Very cool!

    I think this is the only DIY robot arm using inverse kinematics I’ve seen on the web.

    If I ever get around to finishing my own, I might be able to use some of this code.

    Thanks!

  • karan
    August 4, 2010 at 11:37 am · Reply

    thanks for quick replay, can u please explain your half shield and which value of that big cap you are using. I am confused of your wrist part. in code what is wrist angle? is it up down motion ? it seems from code that the change in length due to wrist movement, am i right ?

    Does arm take shortest way from one point to another ? does code take care of case like this

    I move from park to point A so if i give co-ordinates of point B can it go there or it always has to be from park point ?

    How you decide the bounds for arm, is it just by checking the values of different extreme position? I want to say that how can i find it for my arm? was it given by lynxmotion? Please elaborate.

    Sorry for lots of question but i think it will help me and a lot of people.

    • oleg
      August 7, 2010 at 11:27 am · Reply

      You ask too many questions in a single post – it’s hard to find time to write an answer to all of them at once, so I won’t :-)

      There is a link to excel spreadsheet with formulas hosted on lynxmotion.com; many things would become clear after studying it. The cap value is not crtitical – just use one which seems big enough.

      Bounds are not decided. It’s a matter of simple geometrical calculations; I will implement it eventually. One bound is a sphere with length of the arm as radius and base joint as center.

  • Kannan
    August 23, 2010 at 7:57 am · Reply

    As the other post rightly mentioned, this is the first time someone tamed the beast IK and caged inside Arduino.

    Thanks for sharing.

  • RAMU
    August 30, 2010 at 3:18 am · Reply

    HI I am thiking of making an automated drilling machine. how can i implement it.

  • Edney
    September 27, 2010 at 6:31 pm · Reply

    hi
    you could send me the code of the completed project??
    got some functions that are not implemented in this topic
    eg
    servos.setposition (BAS_SERVO, ftl (bas_servopulse));
    that ftl (bas_servopulse) does what??

  • Nemanja
    February 14, 2011 at 8:05 am · Reply

    Hi,
    i have an old lynxmotion arm i think its 5 :S but i dont have the servo shield. How can i modify your code to do invers kinematics with my arm?

    • oleg
      February 14, 2011 at 8:09 am · Reply

      The code doesn’t depend on particular servo shield, they all accept either angle or pulse width. You still need to drive servos somehow.

      • Nemanja
        February 14, 2011 at 10:05 am · Reply

        Ok, i understand thet, but i have a problem understanding the code. I attached my servos to PWM 2,3,4,5, i am not thet good in programing to change your code to drive servos from those locations. Where in the code are named locations of servos?
        Thx for fast reply

  • gregory
    May 11, 2011 at 8:30 am · Reply

    Hi,i notice you are using six servos from the #define statements. Is it possible for me to use one controller to control 5 encoders( with will control H-bridges)simultaneously. I understand that i will need to use 5 interrupts, from your experience is there such a micro-controller out that will be capable of doing this, or close to this?

    Sorry for being out of the topic but your assistance will be much appreciated.

  • gregory
    May 11, 2011 at 8:32 am · Reply

    (which will control 5 H-bridges)*

  • Warlox
    June 2, 2011 at 9:56 pm · Reply

    Hey
    I am really new to arduino so i have a kind of a stupid question.What does the function void set_arm( float x, float y, float z, float grip_angle_d ) do? I mean if we give values of x , y, z and grip angle to the function, What will be the output of the function?
    I am asking it because the funtions void zero_x(), void circle and void line are independent of the function void set_arm. So from where will the end result generate? (which will turn the motor in the specific angle). Hope you understand my question

    • oleg
      June 2, 2011 at 11:05 pm · Reply

      functions generating motion coordinates calculate those coordinates and then call set_arm(), see, for example, circle() line 151. set_arm() output is pulses for servos, it does all inverse kinematics before that, i.e, calculates joint angles form coordinates.

  • Omer
    June 3, 2011 at 7:53 am · Reply

    Hey oleg
    I have checked the link of the excel sheet you shared but it is for 2D arm. It only takes the values of x and y. Can you please share the algo or perhaps the equations that you used to find out the IK of the arm. Here is another excel sheet from lynxmotion and they have mentioned the equations they used. http://www.lynxmotion.com/images/html/proj058.htm

    Thnx

  • Omer
    June 3, 2011 at 8:29 am · Reply

    Its quite confusing But i really liked your code. One question that i had in mind is where do we the values of x, y and z in your code?

    set_arm(12.3,45.3,23.4,45)

    is this the right method of inserting the x,y,z coordinates in the function so that the arm moves to the required x,y,z coordinates? if it is the right method then what is the need for void circle, void line and void zero_x ?

    • oleg
      June 3, 2011 at 8:39 am · Reply

      set_arm() takes x,y,and z coordinates. circle() generates coordinates so arm moves in a circle. You can also enter coordinates directly, if you’d like.

  • Omer
    June 3, 2011 at 9:08 am · Reply

    There are no pins defined in this code. I mean which four pins should be used to connect arduino to the servo shield.
    Thank you so much for giving us your time. Really appreciate it.

  • Michael
    June 26, 2011 at 11:08 am · Reply

    Hi,

    When I compare http://www.micromegacorp.com/downloads/documentation/AN044-Robotic%20Arm.pdf with your work, I can see the connections – and for the most part I understand how most of the figures are derived.

    However the PDF appeared to rush and skip detailed explanations of how the shoulder and wrist angles are determined (my maths isnt that strong).

    Do you have time to break it down for me?

    Regards
    Michael

    • oleg
      June 26, 2011 at 11:43 am · Reply

      Joints are all the same. A joint and two “bones” plus distance between far ends of two bones form a triangle. An arm is treated as a set of triangles in x,z plane, rotating base adds y plane.

  • Jonatas
    July 23, 2011 at 9:32 am · Reply

    I’m using 6 servos directly to the Arduino port (2-8). To control them servo.h use the library, providing for each servant, an angle of 0-180 °.
    What changes should I do in the code since I’m using this library and not servoshield.h?

  • Lucas
    July 28, 2011 at 8:52 am · Reply

    Did you notice an error in the Application Note 44 article? I believe their calculations for r’ and z’ mix up the use of cosine and sine. I assume you corrected this in your software, which I’ll be reviewing shortly. I have all of the same components and will be testing your code in a few days. Your explanation is very clear and helpful. Thanks for putting this together.

    One last question, why did you not use the SSC-32 servo controller that comes with the LynxMotion arm? It seems to me this would be a lot easier than making your own servo controller.

    • oleg
      July 28, 2011 at 9:38 am · Reply

      Did you notice an error in the Application Note 44 article?

      I don’t remember. There are several ways to calculate angles and I was looking at many code samples to find one which can be done in the least amount of machine cycles. My calculations are based on formulas from one of the spreadsheets posted on Lynxmotion site.

      One last question, why did you not use the SSC-32 servo controller that comes with the LynxMotion arm? It seems to me this would be a lot easier than making your own servo controller.

      I think you can buy Lynxmotion kit without SSC-32. Servo controllers are simple circuits – I had mine lying around and just used it.

      • Lucas
        July 30, 2011 at 2:18 pm · Reply

        With a few modifications, the code you posted works fine with the SSC-32 servo controller. I’ll need to go through the calculations in a little more detail to see how you implemented them. The SSC-32 doesn’t come with a nice servo library like the one you used, but it isn’t that hard to write out the commands I need. It’s a blast to see your arm finally doing what you command it to do! Now if I could just have it make breakfast for me in the morning!

  • Chris
    November 30, 2011 at 9:17 am · Reply

    Hi,

    I am trying to use your code in my project. I really like it, you have commented everything and it’s readable. I changed the length of the “bones” and the servo limits, what I don’t understand is this part in your code:

    /* 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 );

    Can you explain more, please ? I don’t get what are these values.
    Thanks a lot,
    Chris.

    • oleg
      November 30, 2011 at 10:21 pm · Reply

      That’s transformations from joint angles to servo pulse lengths. 1500 is 1.5ms which corresponds to middle position of a servo. 11.1, 6.6, etc., are constants, like “degrees per millisecond” ratios. The rest are actual angles of various joints and direction of rotation.

  • vipin sharma
    April 21, 2012 at 11:28 pm · Reply

    sir,
    how can i build this and where i can purchase the projects matter ??

Leave a Reply

  

  

  

You can use these HTML tags

<a href="" title=""> <abbr title=""> <acronym title=""> <b> <blockquote cite=""> <cite> <code> <del datetime=""> <em> <i> <q cite=""> <strike> <strong> <pre lang="" line="" escaped="" highlight="">