Commits

Trammell Hudson committed 29b11e2

IK on the wrist is looking better

  • Participants
  • Parent commits 1885e59

Comments (0)

Files changed (2)

 	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;
+	const double C6 = (-S1*C4-C1*C23*S4)*sx + (C1*C4-S1*C23*S4)*sy + S4*S23*sz;
 
 	theta[5] = atan2(S6,C6);
 
 
 	// Adjust xyz by the desired wrist orientation
 	// to determine the position of the spherical joint
-	double a[] = { 0, 1, 0 };
-	double s[] = { 1, 0, 0 };
+	double a[] = { -1, 0, 0 };
+	double s[] = { 0, 1, 0 };
 	double n[] = { 0, 0, 1 };
 
 
 		{ +12000, -11200, 0,  -16000, 16000 },
 		{ -18000, +18000, -90, -24000, 24000 },
 		{ +11200, -10500, +90, -18500, 18500 },
-		{ -8000, +8000, 0, -12000, 12000 },
+		{ -8000, +8000, 90, -12000, 20000 },
 		{ +8000, -8000, 0, -10000, 10000 },
-		{ +6500, -6500, 0, -14000, 14000 },
+		{ -6500, +6500, 0, -14000, 14000 },
 	};
 
 	double commands[6];
 			command += commands[4] * (1200.0/-10000);
 		}
 
-		printf("%d: %f => %.0f\n", i, theta[i], command);
+		if(1)
+		printf("%d: %f => %.0f\n", i, theta[i] * 180 /M_PI, command);
 
 		if (command < lim->min || command > lim->max)
 		{