king spider leg: my first IK program!

[animaniac]

hi, I wrote the equations for the end position of the king spider leg, but they too complex for me to invert them, do you know if the formula that gives the ax12 positions according to the leg position has been done before, or if a resource to compute the formula is available on the internet?

[limor]

There are many hexapod IK codes around google land.
Check out Zenta's hexapod for some inspiration.

phpBB [media]

[limor]

and here for example are some code examples: http://hexapodrobot.com/forum/viewforum.php?f=25

[animaniac]

hi, I managed to compute the relations and assemble a program to move the leg. zqsd (azerty keyboard) moves the leg tip and ae moves it up and down by 10mm increments:

void moveto(int moveto_ID, double moveto_TARGET, double moveto_SPEED)
{
int TARGET = 511 + floor(moveto_TARGET*195.6+0.5);; //511 is position 0 of dynamixel for the angle computation
int SPEED = min(1023, floor(moveto_SPEED*85.693+0.5)); //1023 is the dynamixel maximum speed
dxl_write_word( moveto_ID, 32, SPEED );
dxl_write_word( moveto_ID, 30, TARGET );
}

int main()
{
int CommStatus;
double position[3] = {188.5, 36, -115}; //spider leg tip position from CM5 center
double angle[3] = {0, 0, 0}; //corresponding values of the 3 dynamixel
double x = 64.5; //internal parameter (horizontal distance between the second dynamixel axis and the leg tip)
char c = 0;

// Open device
if( dxl_initialize(DEFAULT_PORTNUM, DEFAULT_BAUDNUM) == 0 )
{
printf( "Failed to open USB2Dynamixel!\n" );
printf( "Press any key to terminate...\n" );
getch();
return 0;
}
else
printf( "Succeed to open USB2Dynamixel!\n" );

while(1)
{
printf( "Press any key to continue!(press ESC to quit)\n" );
c = getch();
//printf( "%d\n", c);
if(c == 27)
break;

switch (c)
{
case 122:
position[0] = position[0]+10;
break;
case 115:
position[0] = position[0]-10;
break;
case 113:
position[1] = position[1]+10;
break;
case 100:
position[1] = position[1]-10;
break;
case 97:
position[2] = position[2]+10;
break;
case 101:
position[2] = position[2]-10;
break;


}
// Compute angles
if (position[0]>73.5) //the first dynamixel needs a specific case if the tip goes "behind" the axis
angle[0] = atan2(position[1]-36, position[0]-73.5);
else
angle[0] = atan2(36-position[1], 73.5-position[0]);

x = ( position[0]-73.5)/ cos( angle[0])-44;
angle[2] = asin(( x*x+position[2]*position[2]-12902)/12061);
angle[1] = -atan2(position[2], x)+ atan2( -93.5*cos (angle[2]), 64.5+93.5*sin (angle[2]));
//printf( "%f %f %f %f\n", x, angle[0], angle[1], angle[2]);

moveto ( 1, angle[0], 0.5);
moveto ( 2, angle[1]-0.356, 0.5); //the offsets are the set angles between the dynamixel 0 positions and the kinematic lines
moveto ( 3, angle[2]-0.471, 0.5);
CommStatus = dxl_get_result();

if( CommStatus != COMM_RXSUCCESS )
{
PrintCommStatus(CommStatus);
break;
}

}

maybe it's obvious from this, but I'm kinda new from C++ so I have a few questions:

how do I start a new project? to do this I just copied the example and modified it. I tried to create a new project, but I missed something and it never worked.

is there an easy way to control the speed instead of the position, for example the leg tip moving left continuously if I keep q pressed?

do you know a good resource to learn C++ applied to robotics, book or website? books dedicated to robotics seem to focus mainly on building the robot, and I couldn't find C++ tutorials on robotics.

thanks! this is a great forum!