/* SPFAmodel.cpp */ #include "../include/spfa/SPFAmod.h" #define X 0 // first 3 columns in a transformation matrix #define Y 1 #define Z 2 #define T 3 // translation column in a transformation matrix class JointMatrix JointMatrix::q; // define the static class member JointMatrix::JointMatrix( const double R, const double Theta) { double lambda[6]; int i; for( i = 0; i < 6; i+=2 ) { lambda[i ] = 2*M_PI/3.0 * i/2 - Theta/2.0; lambda[i+1] = 2*M_PI/3.0 * i/2 + Theta/2.0; } for( i = 0; i < 6; i++ ) { m[i][X] = R * cos(lambda[i]); m[i][Y] = R * sin(lambda[i]); m[i][Z] = 0.0; } } // JointMatrix( double, double) JointMatrix::JointMatrix( const double *matrixPtr) { for( int i = 0; i < 6; i++ ) { m[i][X] = *matrixPtr++; m[i][Y] = *matrixPtr++; m[i][Z] = *matrixPtr++; } } // JointMatrix(double*) //------------------------------------------------------------- JointMatrix& operator *(const RotationMatrix& Rot, const JointMatrix& p) { for( int i = 0; i < 6; ++i ){ JointMatrix::q.m[i][X] = p.m[i][X]*Rot.m[X][X] + p.m[i][Y]*Rot.m[X][Y] + p.m[i][Z]*Rot.m[X][Z] + Rot.m[X][T]; JointMatrix::q.m[i][Y] = p.m[i][X]*Rot.m[Y][X] + p.m[i][Y]*Rot.m[Y][Y] + p.m[i][Z]*Rot.m[Y][Z] + Rot.m[Y][T]; JointMatrix::q.m[i][Z] = p.m[i][X]*Rot.m[Z][X] + p.m[i][Y]*Rot.m[Z][Y] + p.m[i][Z]*Rot.m[Z][Z] + Rot.m[Z][T]; } return JointMatrix::q; } // operator * JointMatrix& operator -(const JointMatrix& a, const JointMatrix& b) { // if the result of the previous operation is already stored in "q" if( &a == &JointMatrix::q) { for( int i = 0; i < 6; ++i ){ JointMatrix::q.m[i][X] -= b.m[i][X]; JointMatrix::q.m[i][Y] -= b.m[i][Y]; JointMatrix::q.m[i][Z] -= b.m[i][Z]; } } else { // store the result in "q" for( int i = 0; i < 6; ++i ){ JointMatrix::q.m[i][X] = a.m[i][X] - b.m[i][X]; JointMatrix::q.m[i][Y] = a.m[i][Y] - b.m[i][Y]; JointMatrix::q.m[i][Z] = a.m[i][Z] - b.m[i][Z]; } } return JointMatrix::q; } //------------------------------------------------------------- // SPFA_Model constructors //------------------------------------------------------------- SPFA_Model::SPFA_Model( double Rb, double Rp // base & platform radii , double ThetaBase, double ThetaPlatform // angles between joints , double Height ) // initial platform height : base(Rb, ThetaBase), platform(Rp, ThetaPlatform) { int i; // base = JointMatrix(Rb, ThetaBase); // platform = JointMatrix(Rp, ThetaPlatform); // the lengths of the links are equal to the distances between the joints for( i = 0; i < 6; i++ ) { links[i] = sqrt( sqr(platform.m[i][X] - base.m[i][X]) + sqr(platform.m[i][Y] - base.m[i][Y]) + sqr(platform.m[i][Z] - base.m[i][Z] + Height) ); } for( i = 0; i < 6; i++ ) heights[i] = 0.0; // initial actuator positions } // SPFA_Model(double,double,double,double,double) SPFA_Model::SPFA_Model (const double *modelPtr) {// array of model values int i; base = JointMatrix(modelPtr); // copy to base (char*)modelPtr += sizeof(JointMatrix); // advance the pointer platform = JointMatrix(modelPtr); // copy to platform (char*)modelPtr += sizeof(JointMatrix); // advance the pointer for( i = 0; i < 6; i++ ) links [i] = *modelPtr++; for( i = 0; i < 6; i++ ) heights[i] = *modelPtr++; } // SPFA_Model(double*) void SPFA_Model::SPFA_Inverse( const RotationMatrix& Rot, double h[6]) { //q = Rot * platform - base; // the result is stored in JointMatrix::q for( int i=0; i < 6 ; ++i ) h[i] = JointMatrix::q.m[i][Z] - sqrt ( sqr(links[i]) - sqr(JointMatrix::q.m[i][X]) - sqr(JointMatrix::q.m[i][Y]) ); } //SPFA_Model::SPFA_Inverse(RotationMatrix&) //------------------------------------------------------------- void SPFA_Model::SPFA_Inverse( double const x[6], double h[6]) { class RotationMatrix Rot(x); SPFA_Inverse( Rot, h ); } //SPFA_Model::SPFA_Inverse(RotationMatrix&) //------------------------------------------------------------- void SPFA_Model::SPFA_Inverse ( const Trajectory inTrajectory, Trajectory outActuators) { int i; for(i = 0; i < inTrajectory.numcoords; ++i ){ SPFA_Inverse( &inTrajectory.coord[i][1], &outActuators.coord[i][1] ); outActuators.coord[i][0] = inTrajectory.coord[i][0]; // copy time } // update the number of coordinates stored in outActuators // outActuators.numcoords = i; } // SPFA_Inverse //------------------------------------------------------------- Trajectory& SPFA_Model::SPFA_Inverse( const Trajectory& inTrajectory) { Trajectory *pTr = new Trajectory( inTrajectory.getNumCoords() ); // using pointer because the object would be destructed at the end of the function for(int i = 0; i < inTrajectory.numcoords; ++i ){ SPFA_Inverse( &inTrajectory.coord[i][1] , &(pTr->coord[i][1])); pTr->coord[i][0] = inTrajectory.coord[i][0]; // copy time } return *pTr; } //------------------------------------------------------------- class RotationMatrix& RotationMatrix::RollPitchYaw(const double x[6]) { double c1 = cos(x[YAW]), s1 = sin(x[YAW]); double c2 = cos(x[PITCH]), s2 = sin(x[PITCH]); double c3 = cos(x[ROLL]), s3 = sin(x[ROLL]); m[X][X] = c2*c3; m[X][Y] = s1*s2*c3 - c1*s3; m[X][Z] = c1*s2*c3 + s1*s3; m[Y][X] = c2*s3; m[Y][Y] = s1*s2*s3 + c1*c3; m[Y][Z] = c1*s2*s3 - s1*c3; m[Z][X] = - s2; m[Z][Y] = s1*c2; m[Z][Z] = c1*c2; m[X][T] = x[X]; m[Y][T] = x[Y]; m[Z][T] = x[Z]; return *this; } // RotationMatrix& RollPitchYaw(const double x[6]) //------------------------------------------------------------- RotationMatrix::RotationMatrix(const double x[6]) { m[X][T] = x[0]; m[Y][T] = x[0]; m[Z][T] = x[0]; m[T][T] = 1.0; m[T][X] = m[T][Y] = m[T][Z] = 0.0; double c1 = cos(x[YAW]), s1 = sin(x[YAW]); double c2 = cos(x[PITCH]), s2 = sin(x[PITCH]); double c3 = cos(x[ROLL]), s3 = sin(x[ROLL]); m[X][X] = c2*c3; m[X][Y] = s1*s2*c3 - c1*s3; m[X][Z] = c1*s2*c3 + s1*s3; m[Y][X] = c2*s3; m[Y][Y] = s1*s2*s3 + c1*c3; m[Y][Z] = c1*s2*s3 - s1*c3; m[Z][X] = - s2; m[Z][Y] = s1*c2; m[Z][Z] = c1*c2; m[X][T] = x[X]; m[Y][T] = x[Y]; m[Z][T] = x[Z]; }