Source

puma / fk.c

Trammell Hudson 195598f 


















Trammell Hudson 89e7b1b 



Trammell Hudson 195598f 










Trammell Hudson 252f512 


Trammell Hudson 195598f 








































Trammell Hudson 252f512 


Trammell Hudson 195598f 



Trammell Hudson 252f512 
Trammell Hudson 195598f 
































Trammell Hudson 252f512 
Trammell Hudson 195598f 


/** \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;
}