help with the maths of a simple robotic arm

Hi,

I am experimenting with making a very basic "robotic arm" from a couple of servos, I have a couple of ideas of what I will do with this if it turns out to work ok......

I am really struggling with the maths of how place the arm and I am hoping someone can give me some help with this. Iit is probably a very basic thing for someone with good maths skills and I would guess is probably something others have already done, but I can't find any info. basic enough to get me started.

Basically, the arm I am making is like this:

I want to be able to place the end of the arm at any position inside the yellow box,
and so for any desired location in this box I wish to calculate the two required angles for the servos

I have spent a good few hours pondering this but I am not getting anywhere

Thanks in advance for any help/advice

Probably what you are looking for.

https://www.google.com/search?num=100&hl=en&lr=&tbo=d&as_qdr=all&q=inverse+kinematics&sa=X&ei=3HS6UOX4KrGu0AH374HAAg&ved=0CC4Q7xYoAA&biw=1201&bih=641

thanks :slight_smile:

just a matter of knowing what to search for......

Take a look at this and see if it will help you.

http://www.circuitsathome.com/mcu/robotic-arm-inverse-kinematics-on-arduino

Thanks again :slight_smile:

I think I have now got it, although I have yet to test it

I have been trying to teach myself some maths recently - I now know just about enough to understand how this works
I had no hope trying to figure it out on my own though !

my first try at a sketch:

/*

        Robot arm test script 1          Dec12        alan@alanesq.com
        
        Uses 2 servos on pins 10 and 11 to move a simple 2 dimensional robotic arm
        
        the arm is positioned by giving an x,y coordinate and via the gift of inverse kinematics 
        this is translated to the required servo angles.

*/


#include <Servo.h>    // servo library


// define constants
#define humerus 135    // Humerus length (mm)
#define ulna 125       // Ulna length (mm)


// define servos
Servo servoS;  // shoulder servo
Servo servoE;  // elbow servo


// define global variables 
int Xhand;            // X position of hand
int Yhand;            // y position of hand


// pre calculations
float hum_sq = humerus * humerus;        // humurus length squared
float uln_sq = ulna * ulna;              // ulna length squared

 

// ***************************************************************************


void setup() 
{ 
  //start serial connection
  Serial.begin(9600);
  Serial.println("Robot arm demo sketch");

  servoS.attach(10);  // attaches the shoulder servo on pin 10
  servoE.attach(11);  // attaches the elbow servo on pin 11
} 
 
 
// ***************************************************************************


void loop() 
{ 

  // set servo to park position
   servoS.write(0);              // move shoulder servo 
   servoE.write(0);              // move elbow servo 

  delay(3000);


  // move around in a circle
  float h = 120.0;          // x coordinate of circle center
  float k = 120.0;          // y coordinate of circle center
  float r = 90.0;           // radius of circle
  float cstep = 6.283 / 12; // steps   (2 pi divided by steps)
  while(1) {
    for ( float theta = 0; theta < 6.283; theta = theta + cstep)  {
        float x = h + r*cos(theta);
        float y = k + r*sin(theta);
        moveArm( x,y);
        delay(1000);
    }
  }
  
  
//  // move arm along a straight line in 2cm steps
//  for ( int i=80; i < 220; i = i + 20)  {
//    moveArm ( i,100 );
//    delay(2000);
//  }


  while(1);
} 


// ***************************************************************************


//  Move the arm to specified x,y position


void moveArm ( float x, float y) {

    // calculate required servo angles using inverse kinematics 
    Serial.println("Calculate servo angles:");
    
    // Work out the length of an imaginery line from the arms shoulder to the x,y position and call it B
    // using pythagoras theorem - length of b squared = x sqared + y squared
    float B = sqrt ( ( x * x ) + ( y * y ) );
    Serial.print("  B = ");     Serial.println( B );

    // Calculate the angle of the imaginary line from the x axis and call it QA
    float QA = atan2 ( y , x ); 
    Serial.print("  QA = ");     Serial.println( QA );

    // Calculate the angle from the imaginary line to the humerus (using cosine rule) and call it QB
    float B_sq = B * B;    // B squared
    float tvala = hum_sq - uln_sq + B_sq ;
    float tvalb = 2.0 * humerus * B;
    float QB = acos ( tvala / tvalb ); 
    Serial.print("  QB = ");     Serial.println( QB );
    
    // Calculate angle of shoulder servo by ading the two calculated angles together
    float angleS = QA + QB;
    Serial.print("  angleS = ");     Serial.println( angleS );

    // Calculate angle of elbow servo
    // this is done using the cosine rule 
    tvala = hum_sq + uln_sq - B_sq ;
    tvalb = 2.0 * humerus * ulna;
    float angleE = acos ( tvala / tvalb ); 
    Serial.print("  angleE = ");     Serial.println( angleE );
    
    
    // convert angles from radians to degrees
    angleE = angleE * RAD_TO_DEG;
    angleS = angleS * RAD_TO_DEG;
    

    Serial.print("moving arm to x=");  Serial.print(x);  Serial.print(" Y=");  Serial.println(y);
    Serial.print("  angleS in degrees = ");     Serial.println( angleS );
    Serial.print("  angleE in degrees = ");     Serial.println( angleE );
    moveServos(angleS,angleE);    // move the servos
  
}


// ***************************************************************************


// Move serve adjusting inaccuracy in servo angles
// i.e. when you tell a servo to go to 90 degrees, adjust it so it actually does


void moveServos ( int s, int e ) {
  
    // adjust shoulder servo
    s = s * 0.9;
  
    // adjust elbow servo
    e = e * 0.8;
  
    // Move the servos to required position
    servoS.write(s);              // move shoulder servo 
    servoE.write(e);              // move elbow servo 
  
}


// ***************************************************************************
// end

thanks again for your help - just to let you know that I have got it working !

works surprisingly well considering I am just relying on the servos for positioning

btw - the sketch above is now the updated working version (in case it is of use/help to anyone else)

after some more experimenting I have found that the servos do not position very accurately after all, the angles vary from what they should be in a very none linear way so it is difficult to correct via the sketch. They are good enough for building as an interesting experiment but not good enough for any practical use.
I am using very cheap servos so I tried some larger digital ones but they are no better so I am now experimenting with stepper motors instead, see - Roboclock robotic arm testing - YouTube

Just wondering if there is some clever way to make the joint bearings as at the moment I am just using some small nuts and bolts but of course there is a lot of play in them.......
I suspect there will be some plastic joints/bushings of some kind made for the job I can buy on eBay but I don't know what to search for?

If you want an arm with precision, you might find a small used commercial robotic arm on ebay.

http://www.ebay.com/itm/HP-Orca-Beckman-Coulter-Robot-Robotic-Arm-/170957675257?pt=LH_DefaultDomain_0&hash=item27cddf1af9

thanks - that would be very interesting to have a play with :slight_smile:

but I don't want to spend any money on this, just seeing what I can do on the cheap (this has cost me £5 and a bit of wood so far :wink:
If I can get this working I have an idea of something I will build with it, but it is just a novelty item to keep me amused.


latest: I have got it working well enough to do something with now
you can follow how it goes here: AlanEsq.com is for sale | HugeDomains

Your project is very interesting, I am also very interested in a similar project, I have just ordered some stepper motors and would like to make a very simple arm at first hopefully progressing to something more versatile. Anyway just wanted to ask, have you made any headway with your idea on using steppers instead? The maths I am fine with but I am not a programmer and the little bit that I know, I have taken from generic code, so I am dreading the code writing.

Thanks in advance

unknowntothem:
Your project is very interesting, I am also very interested in a similar project, I have just ordered some stepper motors and would like to make a very simple arm at first hopefully progressing to something more versatile. Anyway just wanted to ask, have you made any headway with your idea on using steppers instead? The maths I am fine with but I am not a programmer and the little bit that I know, I have taken from generic code, so I am dreading the code writing.

Hi,

Yes, I have the arm working nicely - my problem at the moment is making the slider for it to move up and down (to indicate hours on my clock), this has proved to be much more difficult than I expected, but I think I have just about cracked it now

As I am using cheap stepper motors and they are just directly attached to the robot arms, there is some play in it so accuracy is not it's strong point, but it is very easy to make and gives you a lot of experience of the basics of robotic arms

MY latest stepper motor version of the sketch can be seen here - AlanEsq.com is for sale | HugeDomains
this version just waits for you to enter some coordinates via the serial interface (e.g. 100,100) an it then moves the arm to that position
it calibrates by moving to some stops (when you enter R) so you would have to modify this but to your own design but for testing just tell it to go to 100,100 then manually move the arm to this position (by measuring from the stepper motor output shaft)
As I have learned doing this project, robotics is a lot more complicated than I first expected, there is a lot of room for improvement with this sketch, but it works and will be a good starting point

I found that using the stepper motor library seemed to make things more complicated so I opted for controlling the stepper motors directly (by copying the demo which the supplier of the motors gave), so the sketch may look a little complicated at first, but all it is doing is cycling though the 8 variations of outputs to turn the motors one step at a time (i.e. 1000, 1100, 0100, 0110, 0010, 0011, 0001, 1001)

BTW - I had the opposite problem to you, I have done a lot of programming before (although not professionally), but had no maths experience. So together we should be able to do something interesting with this :slight_smile: