Added the math and kinematics modules for the darwin robot.

include/darwin_kinematics.h

+#ifndef _DARWIN_KINEMATICS_H
+#define _DARWIN_KINEMATICS_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include "stm32f1xx.h"
+
+#define DARWIN_PELVIS_LENGTH               29.0 //mm
+#define DARWIN_LEG_SIDE_OFFSET             37.0 //mm
+#define DARWIN_THIGH_LENGTH                93.0 //mm
+#define DARWIN_CALF_LENGTH                 93.0 //mm
+#define DARWIN_ANKLE_LENGTH                33.5 //mm
+#define DARWIN_LEG_LENGTH                  (DARWIN_THIGH_LENGTH + DARWIN_CALF_LENGTH + DARWIN_ANKLE_LENGTH) //mm (THIGH_LENGTH + CALF_LENGTH + ANKLE_LENGTH)
+
+// public functions
+uint8_t darwin_leg_ik(float *out, float x, float y, float z, float a, float b, float c);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif

include/darwin_math.h

+#ifndef _DARWIN_MATH_H
+#define _DARWIN_MATH_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include "stm32f1xx.h"
+
+#define   PI    3.14159
+
+typedef struct{
+  float x;
+  float y;
+  float z;
+}TPoint3D;
+
+// point public functions
+void point3d_init(TPoint3D *point);
+void point3d_load(TPoint3D *point, float x, float y, float z);
+void point3d_copy(TPoint3D *point_dst, TPoint3D *point_src);
+
+typedef struct {
+  float x;
+  float y;
+  float z;
+}TVector3D;
+
+// vector public functions
+void vector3d_init(TVector3D *vector);
+void vector3d_load(TVector3D *vector, float x, float y, float z);
+void vector3d_copy(TVector3D *vector_dst, TVector3D *vector_src);
+float vector3d_length(TVector3D *vector);
+
+enum{
+  m00=0,
+  m01,
+  m02,
+  m03,
+  m10,
+  m11,
+  m12,
+  m13,
+  m20,
+  m21,
+  m22,
+  m23,
+  m30,
+  m31,
+  m32,
+  m33,
+};
+
+typedef struct{
+  float coef[16];
+}TMatrix3D;
+
+// matrix public functions
+void matrix3d_init(TMatrix3D *matrix);
+void matrix3d_copy(TMatrix3D *matrix_dst, TMatrix3D *matrix_src);
+void matrix3d_identity(TMatrix3D *matrix);
+uint8_t matrix3d_inverse(TMatrix3D *org, TMatrix3D *inv);
+void matrix3d_set_transform(TMatrix3D *matrix, TPoint3D *point, TVector3D *vector);
+void matrix3d_mult(TMatrix3D *dst,TMatrix3D *a,TMatrix3D *b);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif

src/darwin_kinematics.c

+#include "darwin_kinematics.h"
+#include "darwin_math.h"
+#include <math.h>
+
+uint8_t darwin_leg_ik(float *out, float x, float y, float z, float a, float b, float c)
+{
+  TMatrix3D Tad, Tda, Tcd, Tdc, Tac;
+  TVector3D vec,orientation;
+  TPoint3D position;
+  float _Rac, _Acos, _Atan, _k, _l, _m, _n, _s, _c, _theta;
+
+  vector3d_load(&orientation,a * 180.0 / PI, b * 180.0 / PI, c * 180.0 / PI);
+  point3d_load(&position,x, y, z - DARWIN_LEG_LENGTH);
+  matrix3d_set_transform(&Tad,&position,&orientation);
+
+  vec.x = x + Tad.coef[2] * DARWIN_ANKLE_LENGTH;
+  vec.y = y + Tad.coef[6] * DARWIN_ANKLE_LENGTH;
+  vec.z = (z - DARWIN_LEG_LENGTH) + Tad.coef[10] * DARWIN_ANKLE_LENGTH;
+
+  // Get Knee
+  _Rac = vector3d_length(&vec);
+  _Acos = acos((_Rac * _Rac - DARWIN_THIGH_LENGTH * DARWIN_THIGH_LENGTH - DARWIN_CALF_LENGTH * DARWIN_CALF_LENGTH) / (2 * DARWIN_THIGH_LENGTH * DARWIN_CALF_LENGTH));
+  if(isnan(_Acos) == 1)
+    return 0x00;;
+  *(out + 3) = _Acos;
+
+  // Get Ankle Roll
+  Tda = Tad;
+  if(matrix3d_inverse(&Tda,&Tda) == 0x00)
+    return 0x00;
+  _k = sqrt(Tda.coef[7] * Tda.coef[7] + Tda.coef[11] * Tda.coef[11]);
+  _l = sqrt(Tda.coef[7] * Tda.coef[7] + (Tda.coef[11] - DARWIN_ANKLE_LENGTH) * (Tda.coef[11] - DARWIN_ANKLE_LENGTH));
+  _m = (_k * _k - _l * _l - DARWIN_ANKLE_LENGTH * DARWIN_ANKLE_LENGTH) / (2 * _l * DARWIN_ANKLE_LENGTH);
+  if(_m > 1.0)
+    _m = 1.0;
+  else if(_m < -1.0)
+    _m = -1.0;
+  _Acos = acos(_m);
+  if(isnan(_Acos) == 1)
+    return 0x00;
+  if(Tda.coef[7] < 0.0)
+    *(out + 5) = -_Acos;
+  else
+    *(out + 5) = _Acos;
+  // Get Hip Yaw
+  vector3d_load(&orientation,*(out + 5) * 180.0 / PI, 0, 0);
+  point3d_load(&position,0, 0, -DARWIN_ANKLE_LENGTH);
+  matrix3d_set_transform(&Tcd,&position,&orientation);
+  Tdc = Tcd;
+  if(matrix3d_inverse(&Tdc,&Tdc) == 0x00)
+    return 0x00;
+  matrix3d_mult(&Tac,&Tad,&Tdc);
+  _Atan = atan2(-Tac.coef[1] , Tac.coef[5]);
+  if(isinf(_Atan) == 1)
+    return 0x00;
+  *(out) = _Atan;
+
+  // Get Hip Roll
+  _Atan = atan2(Tac.coef[9], -Tac.coef[1] * sin(*(out)) + Tac.coef[5] * cos(*(out)));
+  if(isinf(_Atan) == 1)
+    return 0x00;
+  *(out + 1) = _Atan;
+
+  // Get Hip Pitch and Ankle Pitch
+  _Atan = atan2(Tac.coef[2] * cos(*(out)) + Tac.coef[6] * sin(*(out)), Tac.coef[0] * cos(*(out)) + Tac.coef[4] * sin(*(out)));
+  if(isinf(_Atan) == 1)
+    return 0x00;
+  _theta = _Atan;
+  _k = sin(*(out + 3)) * DARWIN_CALF_LENGTH;
+  _l = -DARWIN_THIGH_LENGTH - cos(*(out + 3)) * DARWIN_CALF_LENGTH;
+  _m = cos(*(out)) * vec.x + sin(*(out)) * vec.y;
+  _n = cos(*(out + 1)) * vec.z + sin(*(out)) * sin(*(out + 1)) * vec.x - cos(*(out)) * sin(*(out + 1)) * vec.y;
+  _s = (_k * _n + _l * _m) / (_k * _k + _l * _l);
+  _c = (_n - _k * _s) / _l;
+  _Atan = atan2(_s, _c);
+  if(isinf(_Atan) == 1)
+    return 0x00;
+  *(out + 2) = _Atan;
+  *(out + 4) = _theta - *(out + 3) - *(out + 2);
+
+  return 0x01;
+}

src/darwin_math.c

+#include "darwin_math.h"
+#include <math.h>
+
+// point functions
+void point3d_init(TPoint3D *point)
+{
+  point->x=0.0;
+  point->y=0.0;
+  point->z=0.0;
+}
+
+void point3d_load(TPoint3D *point, float x, float y,float z)
+{
+  point->x=x;
+  point->y=y;
+  point->z=z;  
+}
+
+void point3d_copy(TPoint3D *point_dst, TPoint3D *point_src)
+{
+  point_dst->x=point_src->x;
+  point_dst->y=point_src->y;
+  point_dst->z=point_src->z;
+}
+
+// vector functions
+void vector3d_init(TVector3D *vector)
+{
+  vector->x=0.0;
+  vector->y=0.0;
+  vector->z=0.0;
+}
+
+void vector3d_load(TVector3D *vector, float x, float y, float z)
+{
+  vector->x=x;
+  vector->y=y;
+  vector->z=z;
+}
+
+void vector3d_copy(TVector3D *vector_dst, TVector3D *vector_src)
+{
+  vector_dst->x=vector_src->x;
+  vector_dst->y=vector_src->y;
+  vector_dst->z=vector_src->z;
+}
+
+float vector3d_length(TVector3D *vector)
+{
+  return sqrt(vector->x*vector->x + vector->y*vector->y + vector->z*vector->z); 
+}
+
+// matrix functions
+void matrix3d_init(TMatrix3D *matrix)
+{
+   matrix3d_identity(matrix);
+}
+
+void matrix3d_copy(TMatrix3D *matrix_dst, TMatrix3D *matrix_src)
+{
+  matrix_dst->coef[m00]=matrix_src->coef[m00];
+  matrix_dst->coef[m01]=matrix_src->coef[m01];
+  matrix_dst->coef[m02]=matrix_src->coef[m02];
+  matrix_dst->coef[m03]=matrix_src->coef[m03];
+  matrix_dst->coef[m10]=matrix_src->coef[m10];
+  matrix_dst->coef[m11]=matrix_src->coef[m11];
+  matrix_dst->coef[m12]=matrix_src->coef[m12];
+  matrix_dst->coef[m13]=matrix_src->coef[m13];
+  matrix_dst->coef[m20]=matrix_src->coef[m20];
+  matrix_dst->coef[m21]=matrix_src->coef[m21];
+  matrix_dst->coef[m22]=matrix_src->coef[m22];
+  matrix_dst->coef[m23]=matrix_src->coef[m23];
+  matrix_dst->coef[m30]=matrix_src->coef[m30];
+  matrix_dst->coef[m31]=matrix_src->coef[m31];
+  matrix_dst->coef[m32]=matrix_src->coef[m32];
+  matrix_dst->coef[m33]=matrix_src->coef[m33];
+}
+
+void matrix3d_zero(TMatrix3D *matrix)
+{
+  matrix->coef[m00]=0.0;
+  matrix->coef[m01]=0.0;
+  matrix->coef[m02]=0.0;
+  matrix->coef[m03]=0.0;
+  matrix->coef[m10]=0.0;
+  matrix->coef[m11]=0.0;
+  matrix->coef[m12]=0.0;
+  matrix->coef[m13]=0.0;
+  matrix->coef[m20]=0.0;
+  matrix->coef[m21]=0.0;
+  matrix->coef[m22]=0.0;
+  matrix->coef[m23]=0.0;
+  matrix->coef[m30]=0.0;
+  matrix->coef[m31]=0.0;
+  matrix->coef[m32]=0.0;
+  matrix->coef[m33]=0.0;
+}
+
+void matrix3d_identity(TMatrix3D *matrix)
+{
+  matrix->coef[m00]=1.0;
+  matrix->coef[m01]=0.0;
+  matrix->coef[m02]=0.0;
+  matrix->coef[m03]=0.0;
+  matrix->coef[m10]=0.0;
+  matrix->coef[m11]=1.0;
+  matrix->coef[m12]=0.0;
+  matrix->coef[m13]=0.0;
+  matrix->coef[m20]=0.0;
+  matrix->coef[m21]=0.0;
+  matrix->coef[m22]=1.0;
+  matrix->coef[m23]=0.0;
+  matrix->coef[m30]=0.0;
+  matrix->coef[m31]=0.0;
+  matrix->coef[m32]=0.0;
+  matrix->coef[m33]=1.0;
+}
+
+uint8_t matrix3d_inverse(TMatrix3D *org, TMatrix3D *inv)
+{
+  TMatrix3D src, tmp;
+  float det;
+  uint8_t i;
+
+  /* transpose matrix */
+  for (i = 0; i < 4; i++)
+  {
+    src.coef[i] = org->coef[i*4];
+    src.coef[i + 4] = org->coef[i*4 + 1];
+    src.coef[i + 8] = org->coef[i*4 + 2];
+    src.coef[i + 12] = org->coef[i*4 + 3];
+  }
+
+  /* calculate pairs for first 8 elements (cofactors) */
+  tmp.coef[0] = src.coef[10] * src.coef[15];
+  tmp.coef[1] = src.coef[11] * src.coef[14];
+  tmp.coef[2] = src.coef[9] * src.coef[15];
+  tmp.coef[3] = src.coef[11] * src.coef[13];
+  tmp.coef[4] = src.coef[9] * src.coef[14];
+  tmp.coef[5] = src.coef[10] * src.coef[13];
+  tmp.coef[6] = src.coef[8] * src.coef[15];
+  tmp.coef[7] = src.coef[11] * src.coef[12];
+  tmp.coef[8] = src.coef[8] * src.coef[14];
+  tmp.coef[9] = src.coef[10] * src.coef[12];
+  tmp.coef[10] = src.coef[8] * src.coef[13];
+  tmp.coef[11] = src.coef[9] * src.coef[12];
+  /* calculate first 8 elements (cofactors) */
+  inv->coef[0] = (tmp.coef[0]*src.coef[5] + tmp.coef[3]*src.coef[6] + tmp.coef[4]*src.coef[7]) - (tmp.coef[1]*src.coef[5] + tmp.coef[2]*src.coef[6] + tmp.coef[5]*src.coef[7]);
+  inv->coef[1] = (tmp.coef[1]*src.coef[4] + tmp.coef[6]*src.coef[6] + tmp.coef[9]*src.coef[7]) - (tmp.coef[0]*src.coef[4] + tmp.coef[7]*src.coef[6] + tmp.coef[8]*src.coef[7]);
+  inv->coef[2] = (tmp.coef[2]*src.coef[4] + tmp.coef[7]*src.coef[5] + tmp.coef[10]*src.coef[7]) - (tmp.coef[3]*src.coef[4] + tmp.coef[6]*src.coef[5] + tmp.coef[11]*src.coef[7]);
+  inv->coef[3] = (tmp.coef[5]*src.coef[4] + tmp.coef[8]*src.coef[5] + tmp.coef[11]*src.coef[6]) - (tmp.coef[4]*src.coef[4] + tmp.coef[9]*src.coef[5] + tmp.coef[10]*src.coef[6]);
+  inv->coef[4] = (tmp.coef[1]*src.coef[1] + tmp.coef[2]*src.coef[2] + tmp.coef[5]*src.coef[3]) - (tmp.coef[0]*src.coef[1] + tmp.coef[3]*src.coef[2] + tmp.coef[4]*src.coef[3]);
+  inv->coef[5] = (tmp.coef[0]*src.coef[0] + tmp.coef[7]*src.coef[2] + tmp.coef[8]*src.coef[3]) - (tmp.coef[1]*src.coef[0] + tmp.coef[6]*src.coef[2] + tmp.coef[9]*src.coef[3]);
+  inv->coef[6] = (tmp.coef[3]*src.coef[0] + tmp.coef[6]*src.coef[1] + tmp.coef[11]*src.coef[3]) - (tmp.coef[2]*src.coef[0] + tmp.coef[7]*src.coef[1] + tmp.coef[10]*src.coef[3]);
+  inv->coef[7] = (tmp.coef[4]*src.coef[0] + tmp.coef[9]*src.coef[1] + tmp.coef[10]*src.coef[2]) - (tmp.coef[5]*src.coef[0] + tmp.coef[8]*src.coef[1] + tmp.coef[11]*src.coef[2]);
+  /* calculate pairs for second 8 elements (cofactors) */
+  tmp.coef[0] = src.coef[2]*src.coef[7];
+  tmp.coef[1] = src.coef[3]*src.coef[6];
+  tmp.coef[2] = src.coef[1]*src.coef[7];
+  tmp.coef[3] = src.coef[3]*src.coef[5];
+  tmp.coef[4] = src.coef[1]*src.coef[6];
+  tmp.coef[5] = src.coef[2]*src.coef[5];
+  //Streaming SIMD Extensions - Inverse of 4x4 Matrix 8
+  tmp.coef[6] = src.coef[0]*src.coef[7];
+  tmp.coef[7] = src.coef[3]*src.coef[4];
+  tmp.coef[8] = src.coef[0]*src.coef[6];
+  tmp.coef[9] = src.coef[2]*src.coef[4];
+  tmp.coef[10] = src.coef[0]*src.coef[5];
+  tmp.coef[11] = src.coef[1]*src.coef[4];
+  /* calculate second 8 elements (cofactors) */
+  inv->coef[8] = (tmp.coef[0]*src.coef[13] + tmp.coef[3]*src.coef[14] + tmp.coef[4]*src.coef[15]) - (tmp.coef[1]*src.coef[13] + tmp.coef[2]*src.coef[14] + tmp.coef[5]*src.coef[15]);
+  inv->coef[9] = (tmp.coef[1]*src.coef[12] + tmp.coef[6]*src.coef[14] + tmp.coef[9]*src.coef[15]) - (tmp.coef[0]*src.coef[12] + tmp.coef[7]*src.coef[14] + tmp.coef[8]*src.coef[15]);
+  inv->coef[10] = (tmp.coef[2]*src.coef[12] + tmp.coef[7]*src.coef[13] + tmp.coef[10]*src.coef[15]) - (tmp.coef[3]*src.coef[12] + tmp.coef[6]*src.coef[13] + tmp.coef[11]*src.coef[15]);
+  inv->coef[11] = (tmp.coef[5]*src.coef[12] + tmp.coef[8]*src.coef[13] + tmp.coef[11]*src.coef[14]) - (tmp.coef[4]*src.coef[12] + tmp.coef[9]*src.coef[13] + tmp.coef[10]*src.coef[14]);
+  inv->coef[12] = (tmp.coef[2]*src.coef[10] + tmp.coef[5]*src.coef[11] + tmp.coef[1]*src.coef[9]) - (tmp.coef[4]*src.coef[11] + tmp.coef[0]*src.coef[9] + tmp.coef[3]*src.coef[10]);
+  inv->coef[13] = (tmp.coef[8]*src.coef[11] + tmp.coef[0]*src.coef[8] + tmp.coef[7]*src.coef[10]) - (tmp.coef[6]*src.coef[10] + tmp.coef[9]*src.coef[11] + tmp.coef[1]*src.coef[8]);
+  inv->coef[14] = (tmp.coef[6]*src.coef[9] + tmp.coef[11]*src.coef[11] + tmp.coef[3]*src.coef[8]) - (tmp.coef[10]*src.coef[11] + tmp.coef[2]*src.coef[8] + tmp.coef[7]*src.coef[9]);
+  inv->coef[15] = (tmp.coef[10]*src.coef[10] + tmp.coef[4]*src.coef[8] + tmp.coef[9]*src.coef[9]) - (tmp.coef[8]*src.coef[9] + tmp.coef[11]*src.coef[10] + tmp.coef[5]*src.coef[8]);
+  /* calculate determinant */
+  det = src.coef[0]*inv->coef[0] + src.coef[1]*inv->coef[1] + src.coef[2]*inv->coef[2] + src.coef[3]*inv->coef[3];
+  /* calculate matrix inverse */
+  if (det == 0)
+  {
+    det = 0;
+    return 0x00;
+  }
+  else
+  {
+    det = 1 / det;
+  }
+
+  for (i = 0; i < 16; i++)
+    inv->coef[i]*=det;
+
+  return 0x01;
+}
+
+void matrix3d_set_transform(TMatrix3D *matrix, TPoint3D *point, TVector3D *vector)
+{
+  float Cx = cos(vector->x * 3.141592 / 180.0);
+  float Cy = cos(vector->y * 3.141592 / 180.0);
+  float Cz = cos(vector->z * 3.141592 / 180.0);
+  float Sx = sin(vector->x * 3.141592 / 180.0);
+  float Sy = sin(vector->y * 3.141592 / 180.0);
+  float Sz = sin(vector->z * 3.141592 / 180.0);
+
+  matrix3d_identity(matrix);
+  matrix->coef[0] = Cz * Cy;
+  matrix->coef[1] = Cz * Sy * Sx - Sz * Cx;
+  matrix->coef[2] = Cz * Sy * Cx + Sz * Sx;
+  matrix->coef[3] = point->x;
+  matrix->coef[4] = Sz * Cy;
+  matrix->coef[5] = Sz * Sy * Sx + Cz * Cx;
+  matrix->coef[6] = Sz * Sy * Cx - Cz * Sx;
+  matrix->coef[7] = point->y;
+  matrix->coef[8] = -Sy;
+  matrix->coef[9] = Cy * Sx;
+  matrix->coef[10] = Cy * Cx;
+  matrix->coef[11] = point->z;
+}
+
+void matrix3d_mult(TMatrix3D *dst,TMatrix3D *a,TMatrix3D *b)
+{
+  uint8_t i,j,c;
+
+  matrix3d_zero(dst);
+  for(j = 0; j < 4; j++)
+  {
+    for(i = 0; i < 4; i++)
+    {
+      for(c = 0; c < 4; c++)
+      {
+         dst->coef[j*4+i] += a->coef[j*4+c] * b->coef[c*4+i];
+      }
+    }
+  }
+}