Source

puma / ik.c

Full commit
/** \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;
}


#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];

	// 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;
}
#endif