Trammell Hudson avatar Trammell Hudson committed 1885e59

Wrist IK is almost there

Comments (0)

Files changed (4)

 6 green/white
 nc white/green
 
+
+
+-------
+
+Cross coupling
+command: 4 -40000
+to fix: 5 9000
+p5 command = p5 desired + p4 * (-40000/9000)
+
+command: 4 10000
+to fix: 6 -1200
+p6 command = p6 desired + p5 * (-10000 / 1200)
 }
 
 
+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;
 extern int
 ik_first(
 	double * const theta, // output commands
-	const double * const xyz, // desired position
+	const double * const xyz, // desired position (in mm)
 	const int right, // right arm = +1, left arm = -1
 	const int above // elbow above arm = +1, below arm = -1
 );
+
+
+extern 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
+);
 	double z
 )
 {
-#if 0
-	// test moving the arm to position (x,y) using the first three axis
-	// but first just do the x position, assuming facing forwards
-	// This is solving a side-side-side triangle with the three
-	// sides the given x position and the length of joint 2 and 3.
-	// angle for joint 2 is: r_3^2 = r_2^2 + x^2 - 2 r_2 x cos (90 - Phi_2)
-	// angle for joint 3 is: x^2 = r_2^2 + 3^2 - 2 r_2 r_3 cos (180 - Phi_3)
-
-	const double r_1 = robot->servos[1].length;
-	const double r_2 = robot->servos[2].length;
-	const double r_3 = robot->servos[3].length;
-
-	// if x is > r_2 + r_3, then we can't reach that point
-	if (r_2 + r_3 < x
-	||  r_2 + x < r_3
-	||  r_3 + x < r_2)
-	{
-		printf("%.2f is unreachable\n", x);
-		return - 1;
-	}
-
-	double phi_2 = acos((r_2*r_2 + x*x - r_3*r_3) / (2 * r_2 * x));
-	double phi_3 = acos((r_2*r_2 + r_3*r_3 - x*x) / (2 * r_2 * r_3));
-
-	// Adjust for joint orientations
-	phi_2 = M_PI/2 - phi_2;
-	phi_3 = M_PI - phi_3;
-
-	// translate to a command
-	const int cmd_2 = phi_2 * robot->servos[2].scale;
-	const int cmd_3 = phi_3 * robot->servos[3].scale;
-
-	printf("angles %.2f %.2f: %d %d\n",
-		phi_2 * 180 / M_PI,
-		phi_3 * 180 / M_PI,
-		cmd_2,
-		cmd_3
-	);
-
-	robot_move(robot, 2, cmd_2, 1);
-	robot_move(robot, 3, cmd_3, 1);
-#else
 	const double xyz[] = { x, y, z };
 	double theta[6];
 
+	// 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 n[] = { 0, 0, 1 };
+
+
 	if (!ik_first(theta, xyz, 1, 1))
 	{
 		printf("unreachable\n");
 		return 0;
 	}
 
+	// 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;
+	}
+
 	// Axis limits and offset in the home position
-	const double limits[][5] = {
+typedef struct {
+	double minus_90;
+	double plus_90;
+	double offset;
+	double min;
+	double max;
+} joint_limits_t;
+
+	const joint_limits_t limits[] = {
 		{ +12000, -11200, 0,  -16000, 16000 },
-		{ -18000, +16500, -90, -24000, 24000 },
+		{ -18000, +18000, -90, -24000, 24000 },
 		{ +11200, -10500, +90, -18500, 18500 },
+		{ -8000, +8000, 0, -12000, 12000 },
+		{ +8000, -8000, 0, -10000, 10000 },
+		{ +6500, -6500, 0, -14000, 14000 },
 	};
 
-	double commands[3];
+	double commands[6];
 
-	for (int i = 0 ; i < 3 ; i++)
+	for (int i = 0 ; i < 6 ; i++)
 	{
-		const double * const lim = limits[i];
-		double command = theta[i] - lim[2] * M_PI/180;
-		command = command * (lim[1] - lim[0]) / M_PI;
+		const joint_limits_t * const lim = &limits[i];
+		double command = theta[i] - lim->offset * M_PI/180;
+		command = command * (lim->plus_90 - lim->minus_90) / M_PI;
 
-		if (command < lim[3] || command > lim[4])
+		// handle cross coupling
+		if (i == 4)
+		{
+			command += commands[3] * (9000.0/-40000);
+		} else
+		if (i == 5)
+		{
+			command += commands[4] * (1200.0/-10000);
+		}
+
+		printf("%d: %f => %.0f\n", i, theta[i], command);
+
+		if (command < lim->min || command > lim->max)
 		{
 			fprintf(stderr, "%d: %f => %d out of limits! (%f,%f,%f)\n",
 				i,
 		commands[i] = command;
 	}
 
-	for (int i = 0 ; i < 3 ; i++)
+	// need to update limits for cross coupling of wrist
+
+	for (int i = 0 ; i < 6 ; i++)
 	{
 		if (0)
 		printf("command: %d %f => %d\n",
 	// Copy the xyz into the position vector for later iteration
 	for (int i = 0 ; i < 3 ; i++)
 		last_position[i] = xyz[i];
-#endif
 
 	return 0;
 }
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.