PDA

View Full Version : Eigen math library for Teensy 3?



selectnone
09-30-2013, 01:00 AM
Has anyone looked into getting the Eigen math-library (http://eigen.tuxfamily.org/) (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 (http://forum.arduino.cc/index.php?topic=144446.msg1089371#msg1089371), 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 (http://docs.autodesk.com/3DSMAX/14/ENU/MAXScript%20Help%202012/) 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...

el_supremo
09-30-2013, 03:21 AM
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


/* ----------------------------------------------------------------------
* 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 */

selectnone
09-30-2013, 10:28 AM
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

PaulStoffregen
09-30-2013, 11:23 AM
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.

selectnone
09-30-2013, 11:36 AM
Thanks, I'll boil it down to an repro-example tonight

selectnone
10-01-2013, 01:02 AM
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.



/* 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...

selectnone
10-07-2013, 11:47 AM
Just thought I'd better mention that this command-format looked like it worked, but doesn't actually multiply correctly:


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:


int retVal = arm_mat_mult_f32(&matrixA,&matrixB,&newMatrix);

BebelRowty
11-01-2013, 09:00 AM
I saw that the Teensy's arm_math library has matrix functions.

PaulStoffregen
11-01-2013, 09:52 AM
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.