Source

puma / fk.c

Diff from to
 	{   0,   0,   0 }, // origin is at center of shoulder
 	{ -90,   0,  70 }, // 
 	{   0, 205,  60 },
-	{  90,   0, 205 },
-	{ -90,   0,  00 },
-	{  90,   0,   0 },
+	{  90,   0, 225 }, // to the center of the wrist joint
+	{ -90,   0,   0 }, // no translation
+	{  90,   0,  60 }, // from the center of the wrist to the tool platform
 };
 
 
 int
 fk(
 	double * pos_out,
-	const double * theta, // six axes angles
-	const double * xyz // position in final joint space
+	const double * xyz, // position in final joint space
+	const double * theta, // axes angles
+	const int num_axes
 )
 {
 	double xyz4[4] = { xyz[0], xyz[1], xyz[2], 1 };
 
-	for (int i = 6-1 ; i >= 0 ; i--)
+	for (int i = num_axes-1 ; i >= 0 ; i--)
 	{
 		m44_t t = fk_dh_build(&dh_params[i], theta[i]);
 
 
 	double xyz[3] = { 0, 0, 0 };
 	double pos[4];
-	fk(pos, theta, xyz);
+	fk(pos, xyz, theta, 6);
 
 	printf("%f %f %f %f\n", pos[0], pos[1], pos[2], pos[3]);
 	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.