Trammell Hudson avatar Trammell Hudson committed 61085f2

Move joint configuration into joint_cfg_t

Comments (0)

Files changed (3)

 #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];
 #pragma once
 
+/** Combine the DH parameters for FK/IK and limits/scaling for drive */
+typedef struct {
+	double d;
+	double a;
+	double scale; // rads to counter ticks
+	double angle; // offset from "0" in the DH map
+	double coupled; // amount this axis is cross coupled to the previous
+	int min;
+	int max;
+} joint_cfg_t;
+
 extern int
 ik_first(
+	const joint_cfg_t * const joints,
 	double * const theta, // output commands
 	const double * const xyz, // desired position (in mm)
 	const int right, // right arm = +1, left arm = -1
 
 extern int
 ik_wrist(
-	double * const theta, // input/output commands
+	const joint_cfg_t * const joints,
+	double * const theta, // input(0-2)/output(3-5) 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)
 #endif
 
 
+static joint_cfg_t joints[] = {
+	[1] = {
+		.scale = (-11200 - +12000),
+		.min = -16000,
+		.max = +16000,
+	},
+
+	[2] = {
+		.scale = (+18000 - -18000),
+		.min = -24000,
+		.max = +24000,
+		.angle = -90,
+		.d = 130, // mm from center of rotation to center of arm
+		.a = 205, // mm along first arm
+	},
+
+	[3] = {
+		.scale = (-10500 - +11200),
+		.min = -18500,
+		.max = +18500,
+		.angle = +90,
+		.a = 0, // mm between center of rotation of elbow?
+	},
+
+	[4] = {
+		.scale = (+8000 - -8000),
+		.min = -12000,
+		.max = +20000,
+		.d = 225, // mm along second arm to center of wrist
+	},
+
+	[5] = { 
+		.scale = (-8000 - +8000),
+		.min = -10000,
+		.max = +10000,
+		.coupled = 9000.0 / -40000.0,
+	},
+
+	[6] = {
+		.scale = (-6500 - +6500),
+		.min = -14000,
+		.max = +14000,
+		.coupled = 1200.0 / -10000.0,
+	},
+};
+
+
 
 /** Return the current time in usec */
 unsigned long
 	double n[] = { 0, 1, 0 };
 
 
-	if (!ik_first(theta, xyz, 1, 1))
+	if (!ik_first(joints, 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))
+	if (!ik_wrist(joints, theta, xyz, a, s, n, 1))
 	{
 		printf("unreachable wrist\n");
 		return 0;
 	}
 
-	// Axis limits and offset in the home position
-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, +18000, -90, -24000, 24000 },
-		{ +11200, -10500, +90, -18500, 18500 },
-		{ -8000, +8000, 90, -12000, 20000 },
-		{ +8000, -8000, 0, -10000, 10000 },
-		{ -6500, +6500, 0, -14000, 14000 },
-	};
-
 	double commands[6];
 
 	for (int i = 0 ; i < 6 ; i++)
 	{
-		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;
+		const joint_cfg_t * const lim = &joints[i+1];
+		double command = theta[i] - lim->angle * M_PI/180;
+		command = command * lim->scale / M_PI;
 
 		// handle cross coupling
-		if (i == 4)
-		{
-			command += commands[3] * (9000.0/-40000);
-		} else
-		if (i == 5)
-		{
-			command += commands[4] * (1200.0/-10000);
-		}
+		if (i > 0)
+			command += commands[i-1] * lim->coupled;
 
 		if(0)
 		printf("%d: %f => %.0f\n", i, theta[i] * 180 /M_PI, command);
 		commands[i] = command;
 	}
 
-	// need to update limits for cross coupling of wrist
-
 	for (int i = 0 ; i < 6 ; i++)
 	{
 		if (0)
 	double z
 )
 {
-#if 0
-	const double phi_1 = atan2(y,x);
-	const double r = sqrt(x*x + y*y);
-
-	const int cmd_1 = phi_1 * robot->servos[1].scale;
-
-	printf("0: %.2f deg %d\n",
-		phi_1 * 180 / M_PI,
-		cmd_1
-	);
-
-	robot_move(robot, 1, cmd_1, 1);
-	robot_ik_test(robot, r, 0);
-#endif
 	robot_ik_test(robot, x, y, z);
 	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.