puma / fk.c

/** \file
 * Forward Kinesmatics for the PUMA arms.
 */
#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <math.h>

#include "matrix.h"

/**
 * Rotations are right-hand rule!  (Counter clockwise)
 *
 * 1. Translate by d along the new z axis.
 * 2. Rotate by the new theta about the new z axis.
 * 3. Translate by a along the old x axis.
 * 4. Rotate by a about the old x axis.
 *
 * This does not handle cross coupling!
 *
 * Note that for the PUMA the straight-up home position is
 * [0, -90, 90, 0, 0, 0], not all zeros.  This is since the X
 * axis is aligned with the horizontal plane.
 */
typedef struct {
	double alpha; // rotation around old x axis
	double a; // translation along old x axis
	double d; // translation along new z axis
} dh_param_t;

static const dh_param_t dh_params[] = {
	{   0,   0,   0 }, // origin is at center of shoulder
	{ -90,   0,  70 }, // 
	{   0, 205,  60 },
	{  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
};


static m44_t
fk_dh_build(
	const dh_param_t * dh,
	const double theta
)
{
	const double ct = cos(theta);
	const double st = sin(theta);
	const double ca = cos(dh->alpha * M_PI/180);
	const double sa = sin(dh->alpha * M_PI/180);

	return (m44_t) { .v = {
		{  ct,    -st,      0,     dh->a },
		{  st*ca,  ct*ca, -sa, -sa*dh->d },
		{  st*sa,  ct*sa,  ca,  ca*dh->d },
		{  0,      0,       0,         1 },
	}};
}


static void
m44_print(
	const m44_t * const m
)
{
	int i, j;
	for (i=0 ; i < 4 ; i++)
	{
		for (j=0 ; j < 4 ; j++)
			printf(" %.3f", m->v[i][j]);
		printf("\n");
	}
}


int
fk(
	double * pos_out,
	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 = num_axes-1 ; i >= 0 ; i--)
	{
		m44_t t = fk_dh_build(&dh_params[i], theta[i]);

		//printf("%d: %f\n", i, theta[i]);
		//m44_print(&t);
		//printf("\n");

		m44_mult4v(pos_out, &t, xyz4);
		xyz4[0] = pos_out[0];
		xyz4[1] = pos_out[1];
		xyz4[2] = pos_out[2];
		xyz4[3] = pos_out[3];
	}

	return 0;
}


int main(int argc, char **argv)
{
	double theta[6];

	if (argc != 7)
	{
		fprintf(stderr, "need 6 axis angles\n");
		return -1;
	}

	for (int i = 1 ; i < 7 ; i++)
		theta[i-1] = atof(argv[i]) * M_PI / 180;

	double xyz[3] = { 0, 0, 0 };
	double pos[4];
	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.