Source

puma / ik.c

Diff from to
 }
 
 
+int
+ik_wrist(
+	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
+)
+{
+	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)*sz + (C1*C4-S1*C23*S4)*sy + S4*S23*sz;
+
+	theta[5] = atan2(S6,C6);
+
+	return 1;
+}
+
+
 #if 0
 int main(int argc, char **argv)
 {
 	};
 	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))
 	{
 		return -1;
 	}
 
-	for(int i = 0 ; i < 3 ; i++)
+	// 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;