# Commits

committed 6287cb0

Implemented direct ik algorithm

• Participants
• Parent commits 252f512

# File Makefile

` fk: fk.o`
` 	\$(CC) -o \$@ \$^ \$(LDFLAGS)`
` `
`+ik: ik.o`
`+	\$(CC) -o \$@ \$^ \$(LDFLAGS)`
`+`
` servo.o: servo.c`
` `
` clean:`

# File ik.c

`+/** \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 <math.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?`
`+`
`+	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);`
`+		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);`
`+		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 0;`
`+}`
`+`
`+`
`+#if 0`
`+int main(void)`
`+{`
`+	const double xyz[3] = { 250, 0, 0 };`
`+	double theta[6];`
`+`
`+	ik_first(theta, xyz, 1, 1); // right, above`
`+	for(int i = 0 ; i < 3 ; i++)`
`+		printf("%f\n", theta[i] * 180 / M_PI);`
`+`
`+	return 0;`
`+}`
`+#endif`