Eigen math library for Teensy 3?

Status
Not open for further replies.
Has anyone looked into getting the Eigen math-library (or something with similar abilities) to work with the Teensy 3?

I'm looking for a library that'll let me do matrix calculations easily, and this seems to fit the bill.

I've seen that someone's managed to get this working on the Arduino Due, which makes me hopeful, but that's using the v1.5 IDE so the instructions given don't appear to work for Teensyduino's v1.05 :p

I'm attempting to write an IK system for the tetrapod robot I've just built, in order to get it walking, but could do with a few more math-tools...

I'm used to using Autodesk 3DS Max's Maxscript language which has all the hard stuff built in ;)

I saw that the Teensy's arm_math library has matrix functions, but my attempts to use the float-matrix multiplication ended up crashing the board or something - the USB serial stopped working, so I gave up on that...
 
I got some of the CMSIS examples to work. This is the matrix example which compiles and runs on my T3 with Arduino 1.0.5 and the latest TeensyDuino (1.16?).

Pete

Code:
/* ---------------------------------------------------------------------- 
 * Copyright (C) 2010 ARM Limited. All rights reserved.   
 *  
 * $Date:        29. November 2010  
 * $Revision: 	V1.0.3
 *  
 * Project: 	    CMSIS DSP Library  
 * Title:	    arm_matrix_example_f32.c		  
 *  
 * Description:	Example code demonstrating least square fit to data  
 *				using matrix functions  
 *				 
 * Target Processor: Cortex-M4/Cortex-M3  
 *
 *
 * Version 1.0.3 2010/11/29 
 *    Re-organized the CMSIS folders and updated documentation. 
 * 
 * Version 1.0.1 2010/10/05 KK 
 *    Production release and review comments incorporated.  
 *
 * Version 1.0.0 2010/09/20 KK
 *    Production release and review comments incorporated.
 * ------------------------------------------------------------------- */

/** 
 * @ingroup groupExamples 
 */

/**    
 * @defgroup MatrixExample Matrix Example    
 * 
 * \par Description: 
 * \par
 * Demonstrates the use of Matrix Transpose, Matrix Muliplication, and Matrix Inverse 
 * functions to apply least squares fitting to input data. Least squares fitting is 
 * the procedure for finding the best-fitting curve that minimizes the sum of the 
 * squares of the offsets (least square error) from a given set of data.
 *
 * \par Algorithm:
 * \par
 * The linear combination of parameters considered is as follows: 
 * \par 
 * <code>A * X = B</code>, where \c X is the unknown value and can be estimated 
 * from \c A & \c B.
 * \par
 * The least squares estimate \c X is given by the following equation:
 * \par
 * <code>X = Inverse(A<sup>T</sup> * A) *  A<sup>T</sup> * B</code>
 *
 * \par Block Diagram:
 * \par
 * \image html matrixExample.gif
 *
 * \par Variables Description:
 * \par
 * \li \c A_f32 input matrix in the linear combination equation
 * \li \c B_f32 output matrix in the linear combination equation
 * \li \c X_f32 unknown matrix estimated using \c A_f32 & \c B_f32 matrices
 *
 * \par CMSIS DSP Software Library Functions Used:
 * \par
 * - arm_mat_init_f32()
 * - arm_mat_trans_f32()
 * - arm_mat_mult_f32()
 * - arm_mat_inverse_f32() 
 * 
 * <b> Refer  </b> 
 * \link arm_matrix_example_f32.c \endlink
 * 
 */


/** \example arm_matrix_example_f32.c 
 */

#include "arm_math.h" 
#include "math_helper.h" 

#define SNR_THRESHOLD 	90 

/* -------------------------------------------------------------------------------- 
 * Test input data(Cycles) taken from FIR Q15 module for differant cases of blockSize  
 * and tapSize 
 * --------------------------------------------------------------------------------- */

const float32_t B_f32[4] =  
{    
  782.0, 7577.0, 470.0, 4505.0 
}; 

/* -------------------------------------------------------------------------------- 
 * Formula to fit is  C1 + C2 * numTaps + C3 * blockSize + C4 * numTaps * blockSize 
 * -------------------------------------------------------------------------------- */

const float32_t A_f32[16] =  
{ 
  /* Const, 	numTaps, 	blockSize, 	numTaps*blockSize */
  1.0, 		32.0, 		4.0, 		128.0,  
  1.0, 		32.0, 		64.0, 		2048.0, 
  1.0, 		16.0, 		4.0, 		64.0, 
  1.0, 		16.0, 		64.0, 		1024.0, 
};  


/* ---------------------------------------------------------------------- 
 * Temporary buffers  for storing intermediate values 
 * ------------------------------------------------------------------- */
/* Transpose of A Buffer */
float32_t AT_f32[16]; 
/* (Transpose of A * A) Buffer */
float32_t ATMA_f32[16]; 
/* Inverse(Transpose of A * A)  Buffer */
float32_t ATMAI_f32[16]; 
/* Test Output Buffer */
float32_t X_f32[4]; 

/* ---------------------------------------------------------------------- 
 * Reference ouput buffer C1, C2, C3 and C4 taken from MATLAB  
 * ------------------------------------------------------------------- */
const float32_t xRef_f32[4] = {
  73.0, 8.0, 21.25, 2.875}; 

float32_t snr; 


/* ---------------------------------------------------------------------- 
 * Max magnitude FFT Bin test 
 * ------------------------------------------------------------------- */

//int32_t main(void) 
void setup(void) 
{ 

  arm_matrix_instance_f32 A; 		/* Matrix A Instance */

  arm_matrix_instance_f32 AT; 		/* Matrix AT(A transpose) instance */

  arm_matrix_instance_f32 ATMA; 	/* Matrix ATMA( AT multiply with A) instance */

  arm_matrix_instance_f32 ATMAI; 	/* Matrix ATMAI(Inverse of ATMA) instance */

  arm_matrix_instance_f32 B; 		/* Matrix B instance */

  arm_matrix_instance_f32 X; 		/* Matrix X(Unknown Matrix) instance */


  uint32_t srcRows, srcColumns; 	/* Temporary variables */
  arm_status status; 

  Serial.begin(9600);
  while(!Serial);
delay(2000);

Serial.println("Start");


  /* Initialise A Matrix Instance with numRows, numCols and data array(A_f32) */
  srcRows = 4; 
  srcColumns = 4; 
  arm_mat_init_f32(&A, srcRows, srcColumns, (float32_t *)A_f32); 

  /* Initialise Matrix Instance AT with numRows, numCols and data array(AT_f32) */
  srcRows = 4; 
  srcColumns = 4; 
  arm_mat_init_f32(&AT, srcRows, srcColumns, AT_f32); 

  /* calculation of A transpose */
  status = arm_mat_trans_f32(&A, &AT); 


  /* Initialise ATMA Matrix Instance with numRows, numCols and data array(ATMA_f32) */
  srcRows = 4; 
  srcColumns = 4; 
  arm_mat_init_f32(&ATMA, srcRows, srcColumns, ATMA_f32); 

  /* calculation of AT Multiply with A */
  status = arm_mat_mult_f32(&AT, &A, &ATMA); 

  /* Initialise ATMAI Matrix Instance with numRows, numCols and data array(ATMAI_f32) */
  srcRows = 4; 
  srcColumns = 4; 
  arm_mat_init_f32(&ATMAI, srcRows, srcColumns, ATMAI_f32); 

  /* calculation of Inverse((Transpose(A) * A) */
  status = arm_mat_inverse_f32(&ATMA, &ATMAI); 

  /* calculation of (Inverse((Transpose(A) * A)) *  Transpose(A)) */
  status = arm_mat_mult_f32(&ATMAI, &AT, &ATMA); 

  /* Initialise B Matrix Instance with numRows, numCols and data array(B_f32) */
  srcRows = 4; 
  srcColumns = 1; 
  arm_mat_init_f32(&B, srcRows, srcColumns, (float32_t *)B_f32);  

  /* Initialise X Matrix Instance with numRows, numCols and data array(X_f32) */
  srcRows = 4; 
  srcColumns = 1; 
  arm_mat_init_f32(&X, srcRows, srcColumns, X_f32); 

  /* calculation ((Inverse((Transpose(A) * A)) *  Transpose(A)) * B) */
  status = arm_mat_mult_f32(&ATMA, &B, &X); 

  /* Comparison of reference with test output */
  snr = arm_snr_f32((float32_t *)xRef_f32, X_f32, 4); 

  /*------------------------------------------------------------------------------ 
   	*  					Initialise status depending on SNR calculations 
   	*------------------------------------------------------------------------------*/
  if( snr > SNR_THRESHOLD) 
  { 
    status = ARM_MATH_SUCCESS; 
  } 
  else 
  { 
    status = ARM_MATH_TEST_FAILURE; 
  } 


  if( status != ARM_MATH_SUCCESS) { 
    Serial.print("Failed"); 
  } else {
    Serial.print("Success");
  }

  while(1);                             /* main function does not return */
} 
void loop(void) {}
/** \endlink */
 
I think I recognise your code from my example-searches...

My problems were probably due to my shaky grasp of values, pointers, etc in a real programming language ;)

I'll experiment some more with arm_matrix_instance_f32, but use a separate USB-serial cable to debug with - whatever I was doing to break the code was causing the Windows COM-port driver to get confused
 
I saw that the Teensy's arm_math library has matrix functions, but my attempts to use the float-matrix multiplication ended up crashing the board or something - the USB serial stopped working, so I gave up on that...

If you want help with that, you must post the code that reproduces the problem.
 
Haha! While tidying up my fail-code to make it presentable, I managed to get it working - typical ;)

This is just a basic method for finding out where each of my robot's feet are with respect to the centre of its body, based on the rotations of its joint-servos, and its limb-measurements.

(I've stripped out all the servo/sensor-driving code for clarity)

I'm planning building an inverse-kinematics system on top of this, so I can point the limbs or head-unit wherever I want.

Code:
/* Tetrapod foot-position ARM matrix example */
/* Neal D Corbett                            */
/* 2013/10/01                                */

#include <arm_math.h>

/* Teensy 3's LED pin: */
int led = 13;
boolean ledOn = true;

const int SERVOCOUNT = 12;

/* Here are the degree values servo rotation-limits: */
/* Coxa z-angles are with respect to body's root-axis */
float servoMinRots[SERVOCOUNT] = {  -38, +50,-135,-213,   -97, -95, -95, -95,    -48, -45, -50, -53, };
float servoMaxRots[SERVOCOUNT] = { +125,+217, +40, -49,   +94,+105,+100,+100,   +150,+152,+150,+150, };

/* Current servo-rotations: */
float servoRots[SERVOCOUNT] = { +45,+135,-45,-135,  +45,+45,+45,+45,  +45,+45,+45,+45, };

/* Servos used by each limb */
int limbChains[4][3] = { {0,4,8}, {1,5,9}, {2,6,10}, {3,7,11}, };

/* Robot dimensions: (mm) */
/* [Body] => Coxa => Femur => Tibia => [Foot] */
float limbPartLengths[4] = {60.0, 39.5, 56.5};

// Offsets from body-centre to limb-roots:
float limbRootOffset[2] = {41.0,23.5};
float limbOffsetMults[4][2] = {{1,1},{1,-1},{-1,-1},{-1,1}};

void setup() 
{
  // Turn on the LED:
  pinMode(led, OUTPUT);
  digitalWrite(led, ledOn);
  
  Serial.begin(9600);

  /* Convert rotations from degrees to radians, for easier trigging: */
  for (int n = 0; n < SERVOCOUNT; n++)
  {
    servoMinRots[n] = 1000 *(DEG_TO_RAD * servoMinRots[n]);
    servoMaxRots[n] = 1000 * (DEG_TO_RAD * servoMaxRots[n]);
    servoRots[n] = 1000 * (DEG_TO_RAD * servoRots[n]);
  }
}

void loop()
{
  for (int limbNum = 0; limbNum < 4; limbNum++)
  {
    /* Create matrix for limb's end-effector, position relative to last joint: */
    float32_t endEffPData[16] = {1,0,0,0, 0,1,0,0, 0,0,1,0, 0,limbPartLengths[2],0,1};
    arm_matrix_instance_f32 endEffMat = {4, 4, endEffPData};

    /* Process joint-rotations in reverse order, back from the end-effector towards the root: */
    for (int jointNum = 2; jointNum != -1; jointNum--)
    {
      // Get joint rotation:
      int servoNum = limbChains[limbNum][jointNum];
      float rads = servoRots[servoNum] / 1000;
      
      float32_t jointPData[16];
      arm_matrix_instance_f32 jointMat = {4, 4, jointPData};
      
      if (jointNum == 0)
      {
        // Rotate limb-root around Z:
        float32_t setPData[16] = {cos(rads),sin(rads),0,0, -sin(rads),cos(rads),0,0, 0,0,1,0, 0,0,0,1};
        for (int n = 0; n < 16; n++)
        {
          jointPData[n] = setPData[n];
        }
        
        // Set up limb-root's position-offset:
        for (int n = 0; n < 2; n++)
        {
          jointPData[12 + n] = limbRootOffset[n] * limbOffsetMults[limbNum][n];
        }
      }
      else 
      {
        // Rotates joint around X, then moves it forward along Y:
        float32_t setPData[16] = {1,0,0,0, 0,cos(rads),sin(rads),0, 0,-sin(rads),cos(rads),0, 0,limbPartLengths[jointNum - 1],0,1};
        for (int n = 0; n < 16; n++)
        {
          jointPData[n] = setPData[n];
        }
      }
      
      // Transform end-effector matrix:
      int retVal = arm_mat_mult_f32(&endEffMat,&jointMat,&endEffMat);
      if (retVal == ARM_MATH_SIZE_MISMATCH)
      {
        Serial.println("MATRIX SIZE MISMATCH");
      }
    }
    
    Serial.print("Foot-position "); Serial.print(limbNum); Serial.print(" - ["); Serial.print(endEffPData[12]); Serial.print(","); Serial.print(endEffPData[13]); Serial.print(","); Serial.print(endEffPData[14]); Serial.println("]"); 
  }
  
  Serial.println("");
  delay(50);
  
  // Alternate the LED:
  ledOn = !ledOn;
  digitalWrite(led, ledOn);
}

I think my earlier problems were due to me blundering around with a complete disregard to variable-context - I had functions to generate the different rotation-matrices, but they were only returning the matrices, so I guess the pData would've been getting lost...
 
Just thought I'd better mention that this command-format looked like it worked, but doesn't actually multiply correctly:

Code:
int retVal = arm_mat_mult_f32(&matrixA,&matrixB,&matrixA);

It works properly if I don't try to shortcut and use matrixA as the destination-matrix:

Code:
int retVal = arm_mat_mult_f32(&matrixA,&matrixB,&newMatrix);
 
Last edited:
I saw that the Teensy's arm_math library has matrix functions.
 
I've recently been working with the arm math library and even digging into how it works internally.

It has really impressive optimizations for 16 bit integers. I know the code above is all 32 bit float. But if you can use int16_t, the arm math library is really amazing.
 
Status
Not open for further replies.
Back
Top