Author Topic: Inverse Kinematics  (Read 3043 times)

0 Members and 1 Guest are viewing this topic.

Offline scrvtchTopic starter

  • Jr. Member
  • **
  • Posts: 11
  • Helpful? 0
Inverse Kinematics
« on: April 02, 2008, 07:29:35 PM »
I've been designing a simple 4DOF robot arm similar to this (http://www.lynxmotion.com/Category.aspx?CategoryID=2) except without the rotating base. The length of the two segments is 17cm. By supplying an (x,y) coordinate, the following equations will determine the angle of the base servo and the "elbow" servo so that the end of the second segment is y cm above the base of the arm and x cm away. (They're based off of simple trigonometry and the law of cosines.) It works in both the first and fourth quadrant so it can reach above and below its base (to an extent) and I will add some limits so it doesn't "overreach."

Code: [Select]
c^2 = x^2 + y^2
c = sqrt(c^2)

Angle2 = arccos(1 - (c^2/578))
Angle1 = arcsin(y/c) + 90 - (Angle2/2)

Now for my questions:

I am planning on controlling the arm with Admin's Axon microcontroller (when it becomes available), but am fairly new with programming. Are the divide, square, squaroot, arccos, and arcsin functions usable in C with the Axon microcontroller? If they are, how would I set up these equations in a program and how fast would the calculations take?

Also, I did some googling to try to find methods for squaring and taking the square root of numbers and found the following website: http://www.dattalo.com/technical/theory/sqrt.html. The author talks about methods for finding the square root of binary numbers and gives the following functions further down in the paper:

Square Root:
Code: [Select]
unsigned char sqrt(unsigned int N)
{
  unsigned int x,j;

  for(j= 1<<7; j<>0; j>>=1)
  {
    x = x + j;
    if( x*x > N)
      x = x - j;
  }

  return(x);
}

Square:
Code: [Select]
s1 = 1<<14
s2 = 0
do
{
  if((s1 + s2) <= N)
  {
    N = N - s2 -s1
    s2 = s2 | (s1<<1)
  }
  s1 = s1 >> 1
  N  = N  << 1
} while(s1 > 2^6)

return(s2>>8)

Would these work? Again, I'm still new with programming, so how could I implement these in my program to use with my equations?

Thank you for your help!

EDIT: I found Admin's sin/cos/tan lookup table code (http://www.societyofrobots.com/robotforum/index.php?topic=1308.msg8622#msg8622) Could I use something similar for my purposes? Also, I read that the trig functions are in avr-libc. How would I use these in my program or would it be better to go with a lookup table? (Sorry for my lack of programming know-how...)


« Last Edit: April 02, 2008, 07:57:27 PM by scrvtch »

Offline Maltaeron

  • Full Member
  • ***
  • Posts: 65
  • Helpful? 0
Re: Inverse Kinematics
« Reply #1 on: April 03, 2008, 12:07:39 AM »
Use the look up tables if you need the speed, or use avrlib's functions if you can afford the time.

For the tables, you could just copy Admin's, as they are fairly accurate -- two decimal places for every fifth degree --, or if you need more accuracy, you/someone could write new ones with more degree entries or "tangent line approximation", making a line between two points you know and finding your value from that line.

With avr-libc, just add an include for the math,

Code: [Select]
include <math.h>
and you can make the calls, but be warned, as this uses doubles for high precision, but slow speed.


I would first try out the math library, and if that is too slow, try to get some tables together -- Admin's trig tables should be plenty, and a square root table shouldn't be too hard...

Good Luck,

 


Get Your Ad Here