Kinmatics.cpp

00001 #include "Kinematics.h"
00002 #include <math.h>
00003 #define M_PI 3.14159265358979323846
00004 
00005 
00006 void getMassMatrix(float A[2][2], volatile float z[4], float p[10]){
00007     float I1 = p[6];
00008     float I2 = p[7];
00009     float c1 = p[2];
00010     float c2 = p[3];
00011     float l1 = p[0];
00012     float m1 = p[4];
00013     float m2 = p[5];
00014     float th2 = z[1];
00015     float t2 = c2*c2;
00016     float t3 = m2*t2;
00017     float t4 = cos(th2);
00018     float t5 = c2*l1*m2*t4;
00019     float t6 = t3+t5;
00020     A[0][0] = I1+I2+t3+c1*c1*m1+l1*l1*m2+c2*l1*m2*t4*2.0;
00021     A[0][1] = t6;
00022     A[1][0] = t6;
00023     A[1][1] = I2+t3;
00024 }
00025 
00026 void getGripperPosition(float position[2], volatile float z[4], float p[10]){
00027     float l1 = p[0];
00028     float l2 = p[1];
00029     float th1 = z[0];
00030     float th2 = z[1];
00031     float t2 = th1+th2;
00032     position[0] = l2*sin(t2)+l1*sin(th1);
00033     position[1] = -l2*cos(t2)-l1*cos(th1);
00034 }
00035 
00036 void getGripperVelocity(float velocity[2], volatile float z[4], float p[10]){
00037     float dth1 = z[2];
00038     float dth2 = z[3];
00039     float l1 = p[0];
00040     float l2 = p[1];
00041     float th1 = z[0];
00042     float th2 = z[1];
00043     float t2 = th1+th2;
00044     float t3 = cos(t2);
00045     float t4 = sin(t2);
00046     velocity[0] = dth1*(l2*t3+l1*cos(th1))+dth2*l2*t3;
00047     velocity[1] = dth1*(l2*t4+l1*sin(th1))+dth2*l2*t4;
00048 }
00049 
00050 void getGripperJacobianTranspose(float Jtrans[2][2], volatile float z[4], float p[10]){
00051     float l1 = p[0];
00052     float l2 = p[1];
00053     float th1 = z[0];
00054     float th2 = z[1];
00055     float t2 = th1+th2;
00056     float t3 = cos(t2);
00057     float t4 = l2*t3;
00058     float t5 = sin(t2);
00059     float t6 = l2*t5;
00060     Jtrans[0][0] = t4+l1*cos(th1);
00061     Jtrans[0][1] = t4;
00062     Jtrans[1][0] = t6+l1*sin(th1);
00063     Jtrans[1][1] = t6;
00064 }
00065 
00066 float getEnergy(volatile float z[4], float p[10]){
00067     float I1 = p[6];
00068     float I2 = p[7];
00069     float c1 = p[2];
00070     float c2 = p[3];
00071     float dth1 = z[2];
00072     float dth2 = z[3];
00073     float g = p[8];
00074     float l1 = p[0];
00075     float m1 = p[4];
00076     float m2 = p[5];
00077     float th1 = z[0];
00078     float th2 = z[1];
00079     float t2 = dth1*dth1;
00080     float t3 = c2*c2;
00081     float t4 = dth2*dth2;
00082     float t5 = cos(th1);
00083     float t6 = cos(th2);
00084     return I1*t2*(1.0/2.0)+I2*t2*(1.0/2.0)+I2*t4*(1.0/2.0)+c1*c1*m1*t2*(1.0/2.0)+l1*l1*m2*t2*(1.0/2.0)-g*m2*(l1*t5+c2*cos(th1+th2))+m2*t2*t3*(1.0/2.0)+m2*t3*t4*(1.0/2.0)+dth1*dth2*m2*t3-c1*g*m1*t5+c2*l1*m2*t2*t6+c2*dth1*dth2*l1*m2*t6;
00085 }
00086 
00087 void getGravity(float output[2], volatile float z[4], float p[10]){
00088     float c1 = p[2];
00089     float c2 = p[3];
00090     float g = p[8];
00091     float l1 = p[0];
00092     float m1 = p[4];
00093     float m2 = p[5];
00094     float th1 = z[0];
00095     float th2 = z[1];
00096     float t2 = sin(th1);
00097     float t3 = th1+th2;
00098     float t4 = sin(t3);
00099     output[0] = g*m2*(c2*t4+l1*t2)+c1*g*m1*t2;
00100     output[1] = c2*g*m2*t4;
00101 }
00102 
00103 void getCoriolisCentrip(float output[2], volatile float z[4], float p[10]){
00104     float c2 = p[3];
00105     float dth1 = z[2];
00106     float dth2 = z[3];
00107     float l1 = p[0];
00108     float m2 = p[5];
00109     float th2 = z[1];
00110     float t2 = sin(th2);
00111     output[0] = -c2*dth2*l1*m2*t2*(dth1*2.0+dth2);
00112     output[1] = c2*dth1*dth1*l1*m2*t2;
00113 }
00114 
00115 float boundTheta(float theta){
00116     int numTurns = fix(theta/(2*M_PI));
00117     return theta-numTurns*2*M_PI;
00118 }
00119 
00120 int fix(float val){//round toward zero
00121     return val > 0 ? floor(val) : ceil(val);
00122 }