6 DoF Stewart Platform with Inverse kinematics

Hey I'm a little new here I have been lurking and reading others posts but its about time I share my own project. I have been building a scale model of a platform and trying to integrate it with SimTools.


The platform is a relatively simple design with six RC servos used to drive the legs. I have a strong background in math and robotics so I "threw together" the complete inverse kinematics model if the system. I can go into detail late but for now here is what it looks like.


I’m having some troubles with incorporating Simtools with my 6 DoF platform. I do know it works as I have done some basic trajectory mapping and the platform behaves as expected.

Now I think that I have 2 ways to proceed. 1) I can bring my IK solution into the axis assignment so Simtools outputs the joint configuration directly. Or… 2) I put the IK solution into the Arduino and just have Simtools output a desired platform configuration. (X,Y,Z, Roll, Pitch, Yaw).

I was struggling with coding the axis assignment, so I decided to do the latter and move the computation of joint angles to the Arduino. I wouldn’t mind some guidance on this if anyone has input on how to get Simtools to output after IK is applied. For method 2 it seemed fairly straight forward to set up the axis assignment using the default 2D with each of the output axes corresponding to one of the DoF defined in by the software.



With this method I just need to parse the Serial data put out by simtools. Now I was struggling here too, I am using a start of line char “ “ (space) to indicate where in the incoming stream of bits the Arduino should start reading. This led me to the following interface settings:



But I am getting juttery behavior when I use the test output. Now I wanted to verify that I am parsing the bits the way I should be so I built a little Serial Sniffer that takes the serial data and displays to an LCD. Which seems to get the right bit and a steady signal, but still its seems to move randomly. I’m not sure what is going on so I thought I would post and see what advice I can get from the forum.


Arduino Code:
//Needed for I2c Coms
#include <Wire.h>
//Used to controll the PWM outputs for the 6 motors
#include <Adafruit_PWMServoDriver.h>
#define MIN_PULSE_WIDTH 500
#define MAX_PULSE_WIDTH 2500
#define FREQUENCY 333
Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver();
//Data Parsing Parameters
const unsigned int MAX_MESSAGE_LENGTH = 10;
const unsigned int nAxis = 6;
bool ready2ReadMessage = 0;
char startOfMessage = ' ';
// Workspace parameters
const int R_MAX = 35;
const int Z_RANGE = 25;
//Create struct so Inverse Kinematics can pass back 6 Vars
struct jointConfig{
float q1;
float q2;
float q3;
float q4;
float q5;
float q6;
};
void setup() {
Serial.println("Serial Begin");
pwm.begin();
pwm.setPWMFreq(FREQUENCY);
Serial.begin(9600);
jointConfig Q = invKin(0,0,0,0,0,0);
moveJoints(Q.q1, Q.q2, Q.q3, Q.q4, Q.q5, Q.q6);
}
void loop() {
// The following section of code reads bit sequentially from the serial port until either the max character length
// is reached or the end of line delimiter is recieved.
while (Serial.available()>0) {
//create place to store message
static char message[MAX_MESSAGE_LENGTH];
static unsigned int message_pos = 0;
//read byte from message
char inByte = Serial.read();
//Check if it is the start of message
if (inByte == startOfMessage){
ready2ReadMessage = 1;
}

//message coming in so check the byte is not Terminating Character
if (ready2ReadMessage && (message_pos<MAX_MESSAGE_LENGTH+1)){
message[message_pos-1] = inByte;
message_pos++;
} else { //Full message recieved...
// Convert inbound Chars to doubles
double Xdouble = message[0];
double Ydouble = message[1];
double Zdouble = message[2];
double rolldouble = message[3];
double pitchdouble = message[4];
double yawdouble = message[5];
// Then change that 0-255 value to real word data (mm/rad)
double X = -R_MAX+((Xdouble/256)*(2*R_MAX));
double Y = -R_MAX+(Ydouble/256)*(R_MAX-(-R_MAX));
double Z = -Z_RANGE/2+(Zdouble/256)*(Z_RANGE);
double roll = -HALF_PI+(rolldouble/256)*PI;
double pitch = -HALF_PI+(pitchdouble/256)*PI;
double yaw = -HALF_PI+(yawdouble/256)*PI;
jointConfig Q = invKin(X,Y,Z,roll,pitch,yaw);
moveJoints(Q.q1, Q.q2, Q.q3, Q.q4, Q.q5, Q.q6);
message_pos = 0;
ready2ReadMessage = 0;
delay(100);
}
}
}
//*************************** INVERSE KINEMATICS FUNCTION*************************************//
// This function takes the desired position of x,y, and z in mm and anguar position of the platform in Rad.
// Then returns the joint configuration that cooresponds to that platform configuration.
struct jointConfig invKin(float x, float y, float z, float roll, float pitch, float yaw){
struct jointConfig Qret;
//Geometric Parameters of the system
float a1 = 40;
float a2 = 24;
float d2 = 85;
float a4 = 150;
float xp = 85;
float yp = 10;
float h = sqrt(sq(a4) - sq(yp - a1 - a2));
//Speeds up Comuptation so they only have to be calculated once
float sp = sin(pitch);
float sr = sin(roll);
float sy = sin(yaw);
float cp = cos(pitch);
float cr = cos(roll);
float cy = cos(yaw);
Qret.q1 = - atan2(a4*pow((1 - sq(d2 - x - xp*(cp*cy - sp*sr*sy) + yp*(cp*sy + cy*sp*sr))/sq(a4)), 0.5 )*pow((sq(sq(a4)*(sq(d2 - x - xp*(cp*cy - sp*sr*sy) + yp*(cp*sy + cy*sp*sr))/sq(a4) - 1) + sq(h - z - xp*(cy*sp + cp*sr*sy) + yp*(sp*sy - cp*cy*sr)) + sq(a1 + y - yp*cr*cy - xp*cr*sy) - sq(a2))/(4*sq(a2)*sq(a4)*(sq(d2 - x - xp*(cp*cy - sp*sr*sy) + yp*(cp*sy + cy*sp*sr))/sq(a4) - 1)) + 1), 0.5), a2 + (sq(a4)*(sq(d2 - x - xp*(cp*cy - sp*sr*sy) + yp*(cp*sy + cy*sp*sr))/sq(a4) - 1) + sq(h - z - xp*(cy*sp + cp*sr*sy) + yp*(sp*sy - cp*cy*sr)) + sq(a1 + y - yp*cr*cy - xp*cr*sy) - sq(a2))/(2*a2)) + atan2(h - z - xp*(cy*sp + cp*sr*sy) + yp*(sp*sy - cp*cy*sr), yp*cr*cy - y - a1 + xp*cr*sy),acos((sq(a4)*(sq(d2 - x - xp*(cp*cy - sp*sr*sy) + yp*(cp*sy + cy*sp*sr))/sq(a4) - 1) + sq(h - z - xp*(cy*sp + cp*sr*sy) + yp*(sp*sy - cp*cy*sr)) + sq(a1 + y - yp*cr*cy - xp*cr*sy) - sq(a2))/(2*a2*a4*pow((1 - sq(d2 - x - xp*(cp*cy - sp*sr*sy) + yp*(cp*sy + cy*sp*sr))/sq(a4)),0.5))),-asin((d2 - x - xp*(cp*cy - sp*sr*sy) + yp*(cp*sy + cy*sp*sr))/a4);
Qret.q2 = atan2(z - h + xp*(cy*sp + cp*sr*sy) + yp*(sp*sy - cp*cy*sr), y - a1 + yp*cr*cy - xp*cr*sy) - atan2(-a4*sqrt(1 - sq(x - d2 + xp*(cp*cy - sp*sr*sy) + yp*(cp*sy + cy*sp*sr))/sq(a4))*sqrt(sq(sq(a1 - y - yp*cr*cy + xp*cr*sy) + sq(a4)*(sq(x - d2 + xp*(cp*cy - sp*sr*sy) + yp*(cp*sy + cy*sp*sr))/sq(a4) - 1) + sq(z - h + xp*(cy*sp + cp*sr*sy) + yp*(sp*sy - cp*cy*sr))- sq(a2))/(4*sq(a2)*sq(a4)*(sq(x - d2 + xp*(cp*cy - sp*sr*sy) + yp*(cp*sy + cy*sp*sr))/sq(a4) - 1)) + 1), a2 + (sq(a1 - y - yp*cr*cy + xp*cr*sy) + sq(a4)*(sq(x - d2 + xp*(cp*cy - sp*sr*sy) + yp*(cp*sy + cy*sp*sr))/sq(a4) - 1) + sq(z - h + xp*(cy*sp + cp*sr*sy) + yp*(sp*sy - cp*cy*sr)) - sq(a2))/(2*a2)),-acos((sq(a1 - y - yp*cr*cy + xp*cr*sy) + sq(a4)*(sq(x - d2 + xp*(cp*cy - sp*sr*sy) + yp*(cp*sy + cy*sp*sr))/sq(a4) - 1) + sq(z - h + xp*(cy*sp + cp*sr*sy) + yp*(sp*sy - cp*cy*sr)) - sq(a2))/(2*a2*a4*sqrt(1 - sq(x - d2 + xp*(cp*cy - sp*sr*sy) + yp*(cp*sy + cy*sp*sr))/sq(a4)))),asin((x - d2 + xp*(cp*cy - sp*sr*sy) + yp*(cp*sy + cy*sp*sr))/a4);
Qret.q3 = - atan2(a4*sqrt(sq(sq(h - z + (xp/2 - (sqrt(3)*yp)/2)*(cy*sp + cp*sr*sy) - (yp/2 + (sqrt(3)*xp)/2)*(sp*sy - cp*cy*sr)) + sq(y/2 - a1 + (sqrt(3)*(x + (yp/2 + (sqrt(3)*xp)/2)*(cp*sy + cy*sp*sr) - (xp/2 - (sqrt(3)*yp)/2)*(cp*cy - sp*sr*sy)))/2 + (cr*cy*(yp/2 + (sqrt(3)*xp)/2))/2 + (cr*sy*(xp/2 - (sqrt(3)*yp)/2))/2) - sq(a2) + sq(a4)*(sq(d2 + x/2 + ((yp/2 + (sqrt(3)*xp)/2)*(cp*sy + cy*sp*sr))/2 - ((xp/2 - (sqrt(3)*yp)/2)*(cp*cy - sp*sr*sy))/2 - (sqrt(3)*(y + cr*cy*(yp/2 + (sqrt(3)*xp)/2) + cr*sy*(xp/2 - (sqrt(3)*yp)/2)))/2)/sq(a4) - 1))/(4*sq(a2)*sq(a4)*(sq(d2 + x/2 + ((yp/2 + (sqrt(3)*xp)/2)*(cp*sy + cy*sp*sr))/2 - ((xp/2 - (sqrt(3)*yp)/2)*(cp*cy - sp*sr*sy))/2 - (sqrt(3)*(y + cr*cy*(yp/2 + (sqrt(3)*xp)/2) + cr*sy*(xp/2 - (sqrt(3)*yp)/2)))/2)/sq(a4) - 1)) + 1)*sqrt(1 - sq(d2 + x/2 + ((yp/2 + (sqrt(3)*xp)/2)*(cp*sy + cy*sp*sr))/2 - ((xp/2 - (sqrt(3)*yp)/2)*(cp*cy - sp*sr*sy))/2 - (sqrt(3)*(y + cr*cy*(yp/2 + (sqrt(3)*xp)/2) + cr*sy*(xp/2 - (sqrt(3)*yp)/2)))/2)/sq(a4)), a2 + (sq(h - z + (xp/2 - (sqrt(3)*yp)/2)*(cy*sp + cp*sr*sy) - (yp/2 + (sqrt(3)*xp)/2)*(sp*sy - cp*cy*sr)) + sq(y/2 - a1 + (sqrt(3)*(x + (yp/2 + (sqrt(3)*xp)/2)*(cp*sy + cy*sp*sr) - (xp/2 - (sqrt(3)*yp)/2)*(cp*cy - sp*sr*sy)))/2 + (cr*cy*(yp/2 + (sqrt(3)*xp)/2))/2 + (cr*sy*(xp/2 - (sqrt(3)*yp)/2))/2) - sq(a2) + sq(a4)*(sq(d2 + x/2 + ((yp/2 + (sqrt(3)*xp)/2)*(cp*sy + cy*sp*sr))/2 - ((xp/2 - (sqrt(3)*yp)/2)*(cp*cy - sp*sr*sy))/2 - (sqrt(3)*(y + cr*cy*(yp/2 + (sqrt(3)*xp)/2) + cr*sy*(xp/2 - (sqrt(3)*yp)/2)))/2)/sq(a4) - 1))/(2*a2)) + atan2(h - z + (xp/2 - (sqrt(3)*yp)/2)*(cy*sp + cp*sr*sy) - (yp/2 + (sqrt(3)*xp)/2)*(sp*sy - cp*cy*sr), y/2 - a1 + (sqrt(3)*(x + (yp/2 + (sqrt(3)*xp)/2)*(cp*sy + cy*sp*sr) - (xp/2 - (sqrt(3)*yp)/2)*(cp*cy - sp*sr*sy)))/2 + (cr*cy*(yp/2 + (sqrt(3)*xp)/2))/2 + (cr*sy*(xp/2 - (sqrt(3)*yp)/2))/2), acos((sq(h - z + (xp/2 - (sqrt(3)*yp)/2)*(cy*sp + cp*sr*sy) - (yp/2 + (sqrt(3)*xp)/2)*(sp*sy - cp*cy*sr)) + sq(y/2 - a1 + (sqrt(3)*(x + (yp/2 + (sqrt(3)*xp)/2)*(cp*sy + cy*sp*sr) - (xp/2 - (sqrt(3)*yp)/2)*(cp*cy - sp*sr*sy)))/2 + (cr*cy*(yp/2 + (sqrt(3)*xp)/2))/2 + (cr*sy*(xp/2 - (sqrt(3)*yp)/2))/2) - sq(a2) + sq(a4)*(sq(d2 + x/2 + ((yp/2 + (sqrt(3)*xp)/2)*(cp*sy + cy*sp*sr))/2 - ((xp/2 - (sqrt(3)*yp)/2)*(cp*cy - sp*sr*sy))/2 - (sqrt(3)*(y + cr*cy*(yp/2 + (sqrt(3)*xp)/2) + cr*sy*(xp/2 - (sqrt(3)*yp)/2)))/2)/sq(a4) - 1))/(2*a2*a4*sqrt(1 - sq(d2 + x/2 + ((yp/2 + (sqrt(3)*xp)/2)*(cp*sy + cy*sp*sr))/2 - ((xp/2 - (sqrt(3)*yp)/2)*(cp*cy - sp*sr*sy))/2 - (sqrt(3)*(y + cr*cy*(yp/2 + (sqrt(3)*xp)/2) + cr*sy*(xp/2 - (sqrt(3)*yp)/2)))/2)/sq(a4)))), -asin((d2 + x/2 + ((yp/2 + (sqrt(3)*xp)/2)*(cp*sy + cy*sp*sr))/2 - ((xp/2 - (sqrt(3)*yp)/2)*(cp*cy - sp*sr*sy))/2 - (sqrt(3)*(y + cr*cy*(yp/2 + (sqrt(3)*xp)/2) + cr*sy*(xp/2 - (sqrt(3)*yp)/2)))/2)/a4);
Qret.q4 = atan2(z - h - (xp/2 + (sqrt(3)*yp)/2)*(cy*sp + cp*sr*sy) - (yp/2 - (sqrt(3)*xp)/2)*(sp*sy - cp*cy*sr), (sqrt(3)*((yp/2 - (sqrt(3)*xp)/2)*(cp*sy + cy*sp*sr) - x + (xp/2 + (sqrt(3)*yp)/2)*(cp*cy - sp*sr*sy)))/2 - y/2 - a1 + (cr*cy*(yp/2 - (sqrt(3)*xp)/2))/2 - (cr*sy*(xp/2 + (sqrt(3)*yp)/2))/2) - atan2(-a4*sqrt(1 - sq(((yp/2 - (sqrt(3)*xp)/2)*(cp*sy + cy*sp*sr))/2 - x/2 - d2 + ((xp/2 + (sqrt(3)*yp)/2)*(cp*cy - sp*sr*sy))/2 + (sqrt(3)*(y - cr*cy*(yp/2 - (sqrt(3)*xp)/2) + cr*sy*(xp/2 + (sqrt(3)*yp)/2)))/2)/sq(a4))*sqrt (sq(sq(a4)*(sq(((yp/2 - (sqrt(3)*xp)/2)*(cp*sy + cy*sp*sr))/2 - x/2 - d2 + ((xp/2 + (sqrt(3)*yp)/2)*(cp*cy - sp*sr*sy))/2 + (sqrt(3)*(y - cr*cy*(yp/2 - (sqrt(3)*xp)/2) + cr*sy*(xp/2 + (sqrt(3)*yp)/2)))/2)/sq(a4) - 1) + sq(h - z + (xp/2 + (sqrt(3)*yp)/2)*(cy*sp + cp*sr*sy) + (yp/2 - (sqrt(3)*xp)/2)*(sp*sy - cp*cy*sr)) + sq(a1 + y/2 - (sqrt(3)*((yp/2 - (sqrt(3)*xp)/2)*(cp*sy + cy*sp*sr) - x + (xp/2 + (sqrt(3)*yp)/2)*(cp*cy - sp*sr*sy)))/2 - (cr*cy*(yp/2 - (sqrt(3)*xp)/2))/2 + (cr*sy*(xp/2 + (sqrt(3)*yp)/2))/2) - sq(a2))/(4*sq(a2)*sq(a4)*(sq(((yp/2 - (sqrt(3)*xp)/2)*(cp*sy + cy*sp*sr))/2 - x/2 - d2 + ((xp/2 + (sqrt(3)*yp)/2)*(cp*cy - sp*sr*sy))/2 + (sqrt(3)*(y - cr*cy*(yp/2 - (sqrt(3)*xp)/2) + cr*sy*(xp/2 + (sqrt(3)*yp)/2)))/2)/sq(a4) - 1)) + 1), a2 + (sq(a4)*(sq(((yp/2 - (sqrt(3)*xp)/2)*(cp*sy + cy*sp*sr))/2 - x/2 - d2 + ((xp/2 + (sqrt(3)*yp)/2)*(cp*cy - sp*sr*sy))/2 + (sqrt(3)*(y - cr*cy*(yp/2 - (sqrt(3)*xp)/2) + cr*sy*(xp/2 + (sqrt(3)*yp)/2)))/2)/sq(a4) - 1) + sq(h - z + (xp/2 + (sqrt(3)*yp)/2)*(cy*sp + cp*sr*sy) + (yp/2 - (sqrt(3)*xp)/2)*(sp*sy - cp*cy*sr)) + sq(a1 + y/2 - (sqrt(3)*((yp/2 - (sqrt(3)*xp)/2)*(cp*sy + cy*sp*sr) - x + (xp/2 + (sqrt(3)*yp)/2)*(cp*cy - sp*sr*sy)))/2 - (cr*cy*(yp/2 - (sqrt(3)*xp)/2))/2 + (cr*sy*(xp/2 + (sqrt(3)*yp)/2))/2) - sq(a2))/(2*a2)), -acos((sq(a4)*(sq(((yp/2 - (sqrt(3)*xp)/2)*(cp*sy + cy*sp*sr))/2 - x/2 - d2 + ((xp/2 + (sqrt(3)*yp)/2)*(cp*cy - sp*sr*sy))/2 + (sqrt(3)*(y - cr*cy*(yp/2 - (sqrt(3)*xp)/2) + cr*sy*(xp/2 + (sqrt(3)*yp)/2)))/2)/sq(a4) - 1) + sq(h - z + (xp/2 + (sqrt(3)*yp)/2)*(cy*sp + cp*sr*sy) + (yp/2 - (sqrt(3)*xp)/2)*(sp*sy - cp*cy*sr)) + sq(a1 + y/2 - (sqrt(3)*((yp/2 - (sqrt(3)*xp)/2)*(cp*sy + cy*sp*sr) - x + (xp/2 + (sqrt(3)*yp)/2)*(cp*cy - sp*sr*sy)))/2 - (cr*cy*(yp/2 - (sqrt(3)*xp)/2))/2 + (cr*sy*(xp/2 + (sqrt(3)*yp)/2))/2) - sq(a2))/(2*a2*a4*sqrt(1 - sq(((yp/2 - (sqrt(3)*xp)/2)*(cp*sy + cy*sp*sr))/2 - x/2 - d2 + ((xp/2 + (sqrt(3)*yp)/2)*(cp*cy - sp*sr*sy))/2 + (sqrt(3)*(y - cr*cy*(yp/2 - (sqrt(3)*xp)/2) + cr*sy*(xp/2 + (sqrt(3)*yp)/2)))/2)/sq(a4)))), asin((((yp/2 - (sqrt(3)*xp)/2)*(cp*sy + cy*sp*sr))/2 - x/2 - d2 + ((xp/2 + (sqrt(3)*yp)/2)*(cp*cy - sp*sr*sy))/2 + (sqrt(3)*(y - cr*cy*(yp/2 - (sqrt(3)*xp)/2) + cr*sy*(xp/2 + (sqrt(3)*yp)/2)))/2)/a4);
Qret.q5 = - atan2(a4*sqrt(sq(sq(h - z + (xp/2 + (sqrt(3)*yp)/2)*(cy*sp + cp*sr*sy) - (yp/2 - (sqrt(3)*xp)/2)*(sp*sy - cp*cy*sr)) + sq(y/2 - a1 - (sqrt(3)*(x + (yp/2 - (sqrt(3)*xp)/2)*(cp*sy + cy*sp*sr) - (xp/2 + (sqrt(3)*yp)/2)*(cp*cy - sp*sr*sy)))/2 + (cr*cy*(yp/2 - (sqrt(3)*xp)/2))/2 + (cr*sy*(xp/2 + (sqrt(3)*yp)/2))/2) - sq(a2) + sq(a4)*(sq(d2 + x/2 + ((yp/2 - (sqrt(3)*xp)/2)*(cp*sy + cy*sp*sr))/2 - ((xp/2 + (sqrt(3)*yp)/2)*(cp*cy - sp*sr*sy))/2 + (sqrt(3)*(y + cr*cy*(yp/2 - (sqrt(3)*xp)/2) + cr*sy*(xp/2 + (sqrt(3)*yp)/2)))/2)/sq(a4) - 1))/(4*sq(a2)*sq(a4)*(sq(d2 + x/2 + ((yp/2 - (sqrt(3)*xp)/2)*(cp*sy + cy*sp*sr))/2 - ((xp/2 + (sqrt(3)*yp)/2)*(cp*cy - sp*sr*sy))/2 + (sqrt(3)*(y + cr*cy*(yp/2 - (sqrt(3)*xp)/2) + cr*sy*(xp/2 + (sqrt(3)*yp)/2)))/2)/sq(a4) - 1)) + 1)*sqrt(1 - sq(d2 + x/2 + ((yp/2 - (sqrt(3)*xp)/2)*(cp*sy + cy*sp*sr))/2 - ((xp/2 + (sqrt(3)*yp)/2)*(cp*cy - sp*sr*sy))/2 + (sqrt(3)*(y + cr*cy*(yp/2 - (sqrt(3)*xp)/2) + cr*sy*(xp/2 + (sqrt(3)*yp)/2)))/2)/sq(a4)), a2 + (sq(h - z + (xp/2 + (sqrt(3)*yp)/2)*(cy*sp + cp*sr*sy) - (yp/2 - (sqrt(3)*xp)/2)*(sp*sy - cp*cy*sr)) + sq(y/2 - a1 - (sqrt(3)*(x + (yp/2 - (sqrt(3)*xp)/2)*(cp*sy + cy*sp*sr) - (xp/2 + (sqrt(3)*yp)/2)*(cp*cy - sp*sr*sy)))/2 + (cr*cy*(yp/2 - (sqrt(3)*xp)/2))/2 + (cr*sy*(xp/2 + (sqrt(3)*yp)/2))/2) - sq(a2) + sq(a4)*(sq(d2 + x/2 + ((yp/2 - (sqrt(3)*xp)/2)*(cp*sy + cy*sp*sr))/2 - ((xp/2 + (sqrt(3)*yp)/2)*(cp*cy - sp*sr*sy))/2 + (sqrt(3)*(y + cr*cy*(yp/2 - (sqrt(3)*xp)/2) + cr*sy*(xp/2 + (sqrt(3)*yp)/2)))/2)/sq(a4) - 1))/(2*a2)) + atan2(h - z + (xp/2 + (sqrt(3)*yp)/2)*(cy*sp + cp*sr*sy) - (yp/2 - (sqrt(3)*xp)/2)*(sp*sy - cp*cy*sr), y/2 - a1 - (sqrt(3)*(x + (yp/2 - (sqrt(3)*xp)/2)*(cp*sy + cy*sp*sr) - (xp/2 + (sqrt(3)*yp)/2)*(cp*cy - sp*sr*sy)))/2 + (cr*cy*(yp/2 - (sqrt(3)*xp)/2))/2 + (cr*sy*(xp/2 + (sqrt(3)*yp)/2))/2), acos((sq(h - z + (xp/2 + (sqrt(3)*yp)/2)*(cy*sp + cp*sr*sy) - (yp/2 - (sqrt(3)*xp)/2)*(sp*sy - cp*cy*sr)) + sq(y/2 - a1 - (sqrt(3)*(x + (yp/2 - (sqrt(3)*xp)/2)*(cp*sy + cy*sp*sr) - (xp/2 + (sqrt(3)*yp)/2)*(cp*cy - sp*sr*sy)))/2 + (cr*cy*(yp/2 - (sqrt(3)*xp)/2))/2 + (cr*sy*(xp/2 + (sqrt(3)*yp)/2))/2) - sq(a2) + sq(a4)*(sq(d2 + x/2 + ((yp/2 - (sqrt(3)*xp)/2)*(cp*sy + cy*sp*sr))/2 - ((xp/2 + (sqrt(3)*yp)/2)*(cp*cy - sp*sr*sy))/2 + (sqrt(3)*(y + cr*cy*(yp/2 - (sqrt(3)*xp)/2) + cr*sy*(xp/2 + (sqrt(3)*yp)/2)))/2)/sq(a4) - 1))/(2*a2*a4*sqrt(1 - sq(d2 + x/2 + ((yp/2 - (sqrt(3)*xp)/2)*(cp*sy + cy*sp*sr))/2 - ((xp/2 + (sqrt(3)*yp)/2)*(cp*cy - sp*sr*sy))/2 + (sqrt(3)*(y + cr*cy*(yp/2 - (sqrt(3)*xp)/2) + cr*sy*(xp/2 + (sqrt(3)*yp)/2)))/2)/sq(a4)))), -asin((d2 + x/2 + ((yp/2 - (sqrt(3)*xp)/2)*(cp*sy + cy*sp*sr))/2 - ((xp/2 + (sqrt(3)*yp)/2)*(cp*cy - sp*sr*sy))/2 + (sqrt(3)*(y + cr*cy*(yp/2 - (sqrt(3)*xp)/2) + cr*sy*(xp/2 + (sqrt(3)*yp)/2)))/2)/a4);
Qret.q6 = atan2(z - h - (xp/2 - (sqrt(3)*yp)/2)*(cy*sp + cp*sr*sy) - (yp/2 + (sqrt(3)*xp)/2)*(sp*sy - cp*cy*sr), (cr*cy*(yp/2 + (sqrt(3)*xp)/2))/2 - y/2 - (sqrt(3)*((yp/2 + (sqrt(3)*xp)/2)*(cp*sy + cy*sp*sr) - x + (xp/2 - (sqrt(3)*yp)/2)*(cp*cy - sp*sr*sy)))/2 - a1 - (cr*sy*(xp/2 - (sqrt(3)*yp)/2))/2) - atan2(-a4*sqrt(sq(sq(h - z + (xp/2 - (sqrt(3)*yp)/2)*(cy*sp + cp*sr*sy) + (yp/2 + (sqrt(3)*xp)/2)*(sp*sy - cp*cy*sr)) + sq(a1 + y/2 + (sqrt(3)*((yp/2 + (sqrt(3)*xp)/2)*(cp*sy + cy*sp*sr) - x + (xp/2 - (sqrt(3)*yp)/2)*(cp*cy - sp*sr*sy)))/2 - (cr*cy*(yp/2 + (sqrt(3)*xp)/2))/2 + (cr*sy*(xp/2 - (sqrt(3)*yp)/2))/2) - sq(a2) + sq(a4)*(sq(d2 + x/2 - ((yp/2 + (sqrt(3)*xp)/2)*(cp*sy + cy*sp*sr))/2 - ((xp/2 - (sqrt(3)*yp)/2)*(cp*cy - sp*sr*sy))/2 + (sqrt(3)*(y - cr*cy*(yp/2 + (sqrt(3)*xp)/2) + cr*sy*(xp/2 - (sqrt(3)*yp)/2)))/2)/sq(a4) - 1))/(4*sq(a2)*sq(a4)*(sq(d2 + x/2 - ((yp/2 + (sqrt(3)*xp)/2)*(cp*sy + cy*sp*sr))/2 - ((xp/2 - (sqrt(3)*yp)/2)*(cp*cy - sp*sr*sy))/2 + (sqrt(3)*(y - cr*cy*(yp/2 + (sqrt(3)*xp)/2) + cr*sy*(xp/2 - (sqrt(3)*yp)/2)))/2)/sq(a4) - 1)) + 1)*sqrt(1 - sq(d2 + x/2 - ((yp/2 + (sqrt(3)*xp)/2)*(cp*sy + cy*sp*sr))/2 - ((xp/2 - (sqrt(3)*yp)/2)*(cp*cy - sp*sr*sy))/2 + (sqrt(3)*(y - cr*cy*(yp/2 + (sqrt(3)*xp)/2) + cr*sy*(xp/2 - (sqrt(3)*yp)/2)))/2)/sq(a4)), a2 + (sq(h - z + (xp/2 - (sqrt(3)*yp)/2)*(cy*sp + cp*sr*sy) + (yp/2 + (sqrt(3)*xp)/2)*(sp*sy - cp*cy*sr)) + sq(a1 + y/2 + (sqrt(3)*((yp/2 + (sqrt(3)*xp)/2)*(cp*sy + cy*sp*sr) - x + (xp/2 - (sqrt(3)*yp)/2)*(cp*cy - sp*sr*sy)))/2 - (cr*cy*(yp/2 + (sqrt(3)*xp)/2))/2 + (cr*sy*(xp/2 - (sqrt(3)*yp)/2))/2) - sq(a2) + sq(a4)*(sq(d2 + x/2 - ((yp/2 + (sqrt(3)*xp)/2)*(cp*sy + cy*sp*sr))/2 - ((xp/2 - (sqrt(3)*yp)/2)*(cp*cy - sp*sr*sy))/2 + (sqrt(3)*(y - cr*cy*(yp/2 + (sqrt(3)*xp)/2) + cr*sy*(xp/2 - (sqrt(3)*yp)/2)))/2)/sq(a4) - 1))/(2*a2)), -acos((sq(h - z + (xp/2 - (sqrt(3)*yp)/2)*(cy*sp + cp*sr*sy) + (yp/2 + (sqrt(3)*xp)/2)*(sp*sy - cp*cy*sr)) + sq(a1 + y/2 + (sqrt(3)*((yp/2 + (sqrt(3)*xp)/2)*(cp*sy + cy*sp*sr) - x + (xp/2 - (sqrt(3)*yp)/2)*(cp*cy - sp*sr*sy)))/2 - (cr*cy*(yp/2 + (sqrt(3)*xp)/2))/2 + (cr*sy*(xp/2 - (sqrt(3)*yp)/2))/2) - sq(a2) + sq(a4)*(sq(d2 + x/2 - ((yp/2 + (sqrt(3)*xp)/2)*(cp*sy + cy*sp*sr))/2 - ((xp/2 - (sqrt(3)*yp)/2)*(cp*cy - sp*sr*sy))/2 + (sqrt(3)*(y - cr*cy*(yp/2 + (sqrt(3)*xp)/2) + cr*sy*(xp/2 - (sqrt(3)*yp)/2)))/2)/sq(a4) - 1))/(2*a2*a4*sqrt(1 - sq(d2 + x/2 - ((yp/2 + (sqrt(3)*xp)/2)*(cp*sy + cy*sp*sr))/2 - ((xp/2 - (sqrt(3)*yp)/2)*(cp*cy - sp*sr*sy))/2 + (sqrt(3)*(y - cr*cy*(yp/2 + (sqrt(3)*xp)/2) + cr*sy*(xp/2 - (sqrt(3)*yp)/2)))/2)/sq(a4)))), -asin((d2 + x/2 - ((yp/2 + (sqrt(3)*xp)/2)*(cp*sy + cy*sp*sr))/2 - ((xp/2 - (sqrt(3)*yp)/2)*(cp*cy - sp*sr*sy))/2 + (sqrt(3)*(y - cr*cy*(yp/2 + (sqrt(3)*xp)/2) + cr*sy*(xp/2 - (sqrt(3)*yp)/2)))/2)/a4);
return Qret;
}
//*************************** Drive Motor Function*************************************//
void moveJoints(float q1, float q2, float q3, float q4, float q5, float q6)
{
float MIN_ANGLE = -HALF_PI;
float MAX_ANGLE = HALF_PI;

// Convert to PWM
//q1
float pulse_wide1 = MIN_PULSE_WIDTH + ((q1-MIN_ANGLE)/(MAX_ANGLE-MIN_ANGLE))*(MAX_PULSE_WIDTH-MIN_PULSE_WIDTH);
int pulse_width1 = int(pulse_wide1/1000000*FREQUENCY*4096);
//q2
float pulse_wide2 = MIN_PULSE_WIDTH + ((q2-MIN_ANGLE)/(MAX_ANGLE-MIN_ANGLE))*(MAX_PULSE_WIDTH-MIN_PULSE_WIDTH);
int pulse_width2 = int(pulse_wide2/1000000*FREQUENCY*4096);
//q3
float pulse_wide3 = MIN_PULSE_WIDTH + ((q3-MIN_ANGLE)/(MAX_ANGLE-MIN_ANGLE))*(MAX_PULSE_WIDTH-MIN_PULSE_WIDTH);
int pulse_width3 = int(pulse_wide3/1000000*FREQUENCY*4096);
//q4
float pulse_wide4 = MIN_PULSE_WIDTH + ((q4-MIN_ANGLE)/(MAX_ANGLE-MIN_ANGLE))*(MAX_PULSE_WIDTH-MIN_PULSE_WIDTH);
int pulse_width4 = int(pulse_wide4/1000000*FREQUENCY*4096);
//q5
float pulse_wide5 = MIN_PULSE_WIDTH + ((q5-MIN_ANGLE)/(MAX_ANGLE-MIN_ANGLE))*(MAX_PULSE_WIDTH-MIN_PULSE_WIDTH);
int pulse_width5 = int(pulse_wide5/1000000*FREQUENCY*4096);
//q6
float pulse_wide6 = MIN_PULSE_WIDTH + ((q6-MIN_ANGLE)/(MAX_ANGLE-MIN_ANGLE))*(MAX_PULSE_WIDTH-MIN_PULSE_WIDTH);
int pulse_width6 = int(pulse_wide6/1000000*FREQUENCY*4096);

//Control Motor
pwm.setPWM(0, 0, pulse_width1);
pwm.setPWM(1, 0, pulse_width2);
pwm.setPWM(2, 0, pulse_width3);
pwm.setPWM(3, 0, pulse_width4);
pwm.setPWM(4, 0, pulse_width5);
pwm.setPWM(5, 0, pulse_width6);
}

Could it be that maybe in the very front of your code that you spell control with 2 L's? I mean I'm no Arduino expert so I really don't know I just an observation

It is a comment "//" so nothing written there will effect the code.

I did figure some stuff out. There was a problem in that version of the code with how the with how the platform config is parsed from the char output of Simtools. with straight double conversion the values go 1 -> 128 then -128 -> -1 for the second half of the ASCII table, so I added a little loop to take care of that. Also because the Arduino is doing the heavy lifting in terms of calculating the IK solution a send frequency below 15ms is just too fast for the platform to handle.

can you tell us more about the 'inverse kinematics' ?

Sure it is a bit complicated so I will give the cliff notes:

I started by defining a series of linked coordinate frames using something called the Devanit-Hartenberg (DH) Method for each leg:



Using this method you get a series of transformation matrices that relate each of the successive frames. I found that representing the O0->Oi,4 frame to be convenient as it show that because I used to rotation axes that are in line any movement in the normal direction is completely taken up by the 3rd joint angle.


No the other two joint angles q1 and q2 are coupled in a non linear equation so I used a projection of the leg to solve for the other two angles. I used the plane that is orthogonal to the rotation axis of the first two joints (Normal to x_i,1 and y_i,1)


Then used the cosine law to solve for the other two angles keeping in mind that every other leg either has an "elbow up or elbow down" solution:



next problem is to get the coords of the O0->Oi,4 in the local i,1 frame. I used a bunch of linear algebra operations and simplifications but it reduces to:

finally separate for just qi,1.

Its a bit of a bear to do by hand so I just put everything into matlab and coppied the output into the arduino program.

I cant put the whole solution on the forum just yet as I this is the work for me to get honours in my mechanical engineering degree.

what is the advantage of it ?

So when using Simtools in it base form only gets you a linear approximation of the motion of the platform. So even if you tune it very well, when you get to distances that are farther away from the origin (center of motion) the movement of the platform will likely break down. On the other hand when you uses an inverse kinematics solution you solve for what the joint angles should be given a platform position. This means that the position of the platform will be accurate even at the limits of auction angles. At this scale I get about 70mm of movement in the horizontal and 35mm in the vertical which is pretty good .

When studying robotics you'll find Inverse Kinematics is one of the first topics covered. The challenges here is that its in 3D so the math gets a bit hairy and the solution doesn't just pop out when you describe the joint variables.

It looks to me the system works when there are more than one arm/linkages per actuator

Hi,
Your interface output dont have identifier. They are just a straight numbers. Maybe the arduino program get confused which number belong to what axis.

Your setting is <Axis1a><Axis2a>.........<Axis6a>.

What i write in my program is [R<Axis1a>][P<Axis2a>][H<Axis3a>][V<Axis4a>]

R for roll, P for pitch and so on.

Edit: I use codes from SMC3 as example
Edit: Just noticed you use space to mark start of message so my reply above is not relevant.
I used to write inverse kinematics for robotic arm and hexapod spider robot leg more than 10 years ago. But now my brain cant no longer process IK

So q values are the legs length?
You enter with yaw, roll, pitch.... To get the legs length.
Or are you getting yaw, roll... From the legs length?