Source

puma / ik.c

Diff from to

File ik.c

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