Source

puma / ik.c

Full commit
Trammell Hudson 6287cb0 








Trammell Hudson 073b4d2 
Trammell Hudson 6287cb0 
Trammell Hudson 073b4d2 
Trammell Hudson 6287cb0 

















Trammell Hudson 073b4d2 


Trammell Hudson 6287cb0 


Trammell Hudson 5b931ef 
Trammell Hudson 6287cb0 



Trammell Hudson 5b931ef 
Trammell Hudson 6287cb0 






Trammell Hudson 073b4d2 


Trammell Hudson 6287cb0 










Trammell Hudson 073b4d2 

Trammell Hudson 6287cb0 







Trammell Hudson 073b4d2 
Trammell Hudson 6287cb0 


Trammell Hudson 1885e59 





































Trammell Hudson 29b11e2 
Trammell Hudson 1885e59 






Trammell Hudson 5b931ef 
Trammell Hudson 073b4d2 
Trammell Hudson 6287cb0 
Trammell Hudson 073b4d2 










Trammell Hudson 6287cb0 

Trammell Hudson 1885e59 



Trammell Hudson 073b4d2 






Trammell Hudson 1885e59 









Trammell Hudson 6287cb0 



Trammell Hudson 5b931ef 
/** \file
 * Direct inverse kinesmatics for the PUMA.
 *
 * Based on "A geometric approach in solving the inverse kinematics of
 * PUMA robots" by Lee and Ziegler, 1983.
 *
 * http://deepblue.lib.umich.edu/bitstream/handle/2027.42/6192/bac6709.0001.001.pdf?sequence=5
 */
#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include "ik.h"

int
ik_first(
	double * const theta, // output commands
	const double * const xyz, // desired position
	const int right, // right arm = +1, left arm = -1
	const int above // elbow above arm = +1, below arm = -1
)
{
	const double px = xyz[0];
	const double py = xyz[1];
	const double pz = xyz[2];

	const double d2 = 130; // mm from center of rotation to center of arm
	const double a2 = 205; // mm along first arm
	const double d4 = 225; // mm along second arm to center of wrist
	const double a3 = 0; // mm between center of rotation of elbow?

	if (px*px + py*py < d2*d2)
		return 0;

	const double r = sqrt(px*px + py*py - d2*d2);
	const double R = sqrt(px*px + py*py + pz*pz - d2*d2);

	//printf("r=%f R=%f\n", r, R);

	// theta[0] defined in equation 26
	theta[0] = atan2(
		-right * py * r - px * d2,
		-right * px * r + py * d2
	);

	// theta[1] is equations 28 - 35.
	{
		const double sin_alpha = -pz / R;
		const double cos_alpha = -right * r / R;
		const double cos_beta = (a2*a2 + R*R - (d4*d4 + a3*a3)) / (2*a2*R);
		if (cos_beta > 1 || cos_beta < -1)
			return 0;

		const double sin_beta = sqrt(1 - cos_beta*cos_beta);
		const double sin_t2 = sin_alpha * cos_beta + right * above * cos_alpha * sin_beta;
		const double cos_t2 = cos_alpha * cos_beta - right * above * sin_alpha * sin_beta;
		theta[1] = atan2(sin_t2, cos_t2);
	}

	// theta[2] 
	{
		const double t2 = d4*d4 + a3*a3;
		const double t = sqrt(t2);
		const double cos_phi = (a2*a2 + t2 - R*R) / (2 * a2 * t);
		if (cos_phi > 1 || cos_phi < -1)
			return 0;
		const double sin_phi = right * above * sqrt(1 - cos_phi*cos_phi);
		const double sin_beta = d4 / t;
		const double cos_beta = fabs(a3) / t;
		const double sin_t3 = sin_phi*cos_beta - cos_phi*sin_beta;
		const double cos_t3 = cos_phi*cos_beta + sin_phi*sin_beta;
		theta[2] = atan2(sin_t3, cos_t3);
	}

	return 1;
}


int
ik_wrist(
	double * const theta, // input/output commands
	const double * const xyz, // desired position (in mm)
	const double * const a, // desired approach vector
	const double * const s, // desired sliding vector (for hand opening)
	const double * const n, // desired normal vector
	const int wrist // wrist up = +1, wrist down = -1
)
{
	const double M = 1;
	const double ax = a[0];
	const double ay = a[1];
	const double az = a[2];

	const double sx = s[0];
	const double sy = s[1];
	const double sz = s[2];

	const double nx = n[0];
	const double ny = n[1];
	const double nz = n[2];

	const double C1 = cos(theta[0]);
	const double S1 = sin(theta[0]);
	const double C23 = cos(theta[1] + theta[2]);
	const double S23 = sin(theta[1] + theta[2]);

	const double S4 = M * (C1*ay - S1*ax);
	const double C4 = M * (C1*C23*ax + S1*C23*ay- S23*az);

	theta[3] = atan2(S4, C4);

	const double S5 = (C1*C23*C4 - S1*S4)*ax + (S1*C23*C4 + C1*S4)*ay - C4* S23*az;
	const double C5 = C1*S23*ax + S1*S23*ay + C23*az;
	theta[4] = atan2(S5,C5);

	const double S6 = (-S1*C4 - C1*C23*S4)*nx + (C1*C4-S1*C23*S4)*ny + S4*S23*nz;
	const double C6 = (-S1*C4-C1*C23*S4)*sx + (C1*C4-S1*C23*S4)*sy + S4*S23*sz;

	theta[5] = atan2(S6,C6);

	return 1;
}


#if 0
int main(int argc, char **argv)
{
	if (argc != 4)
	{
		fprintf(stderr, "need xyz args\n");
		return -1;
	}

	const double xyz[3] = {
		atof(argv[1]),
		atof(argv[2]),
		atof(argv[3])
	};
	double theta[6];

	double a[] = { 0, 1, 0 };
	double s[] = { 1, 0, 0 };
	double n[] = { 0, 0, 1 };

	// right, above
	if (!ik_first(theta, xyz, 1, 1))
	{
		fprintf(stderr, "[%f,%f,%f] unreachable\n", xyz[0], xyz[1], xyz[2]);
		return -1;
	}

	// We now have our first three joint angles
	// compute the hand hangles
	if (!ik_wrist(theta, xyz, a, s, n, 1))
	{
		printf("unreachable wrist\n");
		return 0;
	}


	for(int i = 0 ; i < 6 ; i++)
		printf("%f\n", theta[i] * 180 / M_PI);

	return 0;
}
#endif