Commits

Trammell Hudson committed 195598f

forward kinematics

  • Participants
  • Parent commits c1ddf3d

Comments (0)

Files changed (3)

 servo: servo.o
 	$(CC) -o $@ $^ $(LDFLAGS)
 
+fk: fk.o
+	$(CC) -o $@ $^ $(LDFLAGS)
+
 servo.o: servo.c
 
 clean:
+/** \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!
+ */
+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, 205 },
+	{ -90,   0,  00 },
+	{  90,   0,   0 },
+};
+
+
+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 * theta, // six axes angles
+	const double * xyz // position in final joint space
+)
+{
+	double xyz4[4] = { xyz[0], xyz[1], xyz[2], 1 };
+
+	for (int i = 6-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, theta, xyz);
+
+	printf("%f %f %f %f\n", pos[0], pos[1], pos[2], pos[3]);
+	return 0;
+}
+/** \file
+ * 4x4 matrix math
+ */
+
+#pragma once
+
+typedef struct
+{
+	double v[4][4];
+} m44_t;
+
+
+static inline void
+m44_mult(
+	m44_t * c,
+	const m44_t * a,
+	const m44_t * b
+)
+{
+	int i, j, k;
+
+	for (i = 0 ; i < 4 ; i++)
+	{
+		for (j = 0 ; j < 4 ; j++)
+		{
+			double s = 0;
+			for (k = 0 ; k < 4 ; k++)
+				s += a->v[k][j] * b->v[i][k];
+			c->v[i][j] = s;
+		}
+	}
+}
+
+
+static inline void
+m44_mult4v(
+	double * const c,
+	const m44_t * const a,
+	const double * const b
+)
+{
+	int i, j;
+
+	for (i = 0 ; i < 4 ; i++)
+	{
+		double v = 0;
+
+		for (j = 0 ; j < 4 ; j++)
+			v += a->v[i][j] * b[j];
+
+		c[i] = v;
+	}
+}
+
+
+static inline m44_t
+m44_eye(void)
+{
+	return (m44_t) { .v = {
+		{ 1, 0, 0, 0},
+		{ 0, 1, 0, 0},
+		{ 0, 0, 1, 0},
+		{ 0, 0, 0, 1},
+	}};
+}
+
+
+static inline m44_t
+m44_zero(void)
+{
+	return (m44_t) { .v = {
+		{ 0, 0, 0, 0},
+		{ 0, 0, 0, 0},
+		{ 0, 0, 0, 0},
+		{ 0, 0, 0, 0},
+	}};
+}