puma / ik.c

Trammell Hudson 6287cb0 








Trammell Hudson 073b4d2 
Trammell Hudson 6287cb0 
Trammell Hudson 073b4d2 
Trammell Hudson 6287cb0 
Trammell Hudson 61085f2 

Trammell Hudson 6287cb0 

Trammell Hudson 61085f2 
Trammell Hudson 6287cb0 









Trammell Hudson 61085f2 



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 61085f2 
Trammell Hudson 1885e59 







Trammell Hudson 61085f2 



Trammell Hudson d7316cb 
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(
	const joint_cfg_t * const joints,
	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 a2 = joints[2].a;
	const double d2 = joints[2].d;
	const double a3 = joints[3].a;
	const double d4 = joints[4].d;

	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(
	const joint_cfg_t * const joints,
	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
)
{
	(void) joints;
	(void) xyz;
	(void) wrist;

	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
Tip: Filter by directory path e.g. /media app.js to search for public/media/app.js.
Tip: Use camelCasing e.g. ProjME to search for ProjectModifiedEvent.java.
Tip: Filter by extension type e.g. /repo .js to search for all .js files in the /repo directory.
Tip: Separate your search with spaces e.g. /ssh pom.xml to search for src/ssh/pom.xml.
Tip: Use ↑ and ↓ arrow keys to navigate and return to view the file.
Tip: You can also navigate files with Ctrl+j (next) and Ctrl+k (previous) and view the file with Ctrl+o.
Tip: You can also navigate files with Alt+j (next) and Alt+k (previous) and view the file with Alt+o.