Trammell Hudson avatar Trammell Hudson committed 073b4d2

ik reachability seems to work

Comments (0)

Files changed (1)

  * 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 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);
 
 		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;
 		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;
 		theta[2] = atan2(sin_t3, cos_t3);
 	}
 
-	return 0;
+	return 1;
 }
 
 
-#if 0
-int main(void)
+int main(int argc, char **argv)
 {
-	const double xyz[3] = { 250, 0, 0 };
+	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];
 
-	ik_first(theta, xyz, 1, 1); // right, above
+	// 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
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.