Source

puma / ik.c

Trammell Hudson 6287cb0 








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

















Trammell Hudson 073b4d2 


Trammell Hudson 6287cb0 















Trammell Hudson 073b4d2 


Trammell Hudson 6287cb0 










Trammell Hudson 073b4d2 

Trammell Hudson 6287cb0 







Trammell Hudson 073b4d2 
Trammell Hudson 6287cb0 


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










Trammell Hudson 6287cb0 

Trammell Hudson 073b4d2 






Trammell Hudson 6287cb0 



/** \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 * pz * 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 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];

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

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

	return 0;
}