main.cpp

00001 #include "mbed.h"
00002 #include "ARAP180.h"
00003 #include "platform/mbed_thread.h"
00004 ARAP180 arm;
00005     
00006 
00007  
00008 int main() {
00009 arm.initArmMotors();
00010  
00011 Vector6f q = Vector6f::Zero();
00012 Vector6f q_out = Vector6f::Zero();
00013 Vector6f q_out_sat = Vector6f::Zero();
00014 
00015 Matrix4f T = Matrix4f::Zero();
00016 Matrix6f J = Matrix6f::Zero();    
00017 Matrix4f Td = Matrix4f::Identity();
00018 
00019 
00020 Matrix4f T_e_b_0 = Matrix4f::Zero();
00021 /*T_e_b_0(0,2) = 1.0;
00022 T_e_b_0(1,0) = -1.0;
00023 T_e_b_0(2,1) = -1.0;
00024 T_e_b_0(0,3) = 0.142;
00025 T_e_b_0(2,3) = -0.213;
00026 T_e_b_0(3,3) = 1.0;*/
00027 
00028 
00029 
00030 Matrix4f T_e_b = Matrix4f::Identity();
00031 Matrix4f RotoTraslation = Matrix4f::Identity();
00032 
00033 Matrix4f T_r_e = Matrix4f::Identity();
00034 
00035 T_r_e(1,3) = -0.218;
00036 
00037 double t = 0.0;
00038 
00039 Vector6f q0 = Vector6f::Zero();
00040 Vector6f q_offsetted = Vector6f::Zero();
00041 Vector6f q_act = Vector6f::Zero();
00042 //q0 = arm.getJointPos();
00043 q0(0) = 0.0;
00044 q0(1) = M_PI/4;
00045 q0(2) = M_PI/3;
00046 q0(3) = -M_PI/3 + M_PI/4;
00047 q0(4) = M_PI/2;
00048 q0(5) = 0.0;
00049 T_e_b_0 = arm.forwardKinematics(q0);
00050 
00051 
00052 q_out = q0;
00053 
00054 float traslation_x; 
00055 float traslation_y; 
00056 float traslation_z;
00057 
00058 arm.setJointPos(q0);
00059 
00060 wait_us(5000000);
00061 
00062 arm.setMaxVel(100);
00063 
00064 arm.setMaxAcc(100);
00065 
00066 while (true){
00067    
00068 
00069     traslation_x = 0.0;//0.3*sin(0.2*2*M_PI*t);//+0.15*sin(0.2*2*M_PI*t);
00070     traslation_y = 0.0;//-0.4*sin(0.2*2*M_PI*t);
00071     traslation_z = 0.25*sin(0.2*2*M_PI*t);//-0.15*sin(0.2*2*M_PI*t);//0.3+0.1*sin(0.2*2*M_PI*t);
00072 
00073     RotoTraslation.block(0,0,3,3) = utilities::rotx(0.0);//utilities::rotx((M_PI/3)*sin(0.2*2*M_PI*t));
00074    
00075     RotoTraslation(0,3) = traslation_x;
00076     RotoTraslation(1,3) = traslation_y;
00077     RotoTraslation(2,3) = traslation_z;
00078     
00079     T_e_b=T_e_b_0*T_r_e*RotoTraslation*T_r_e.inverse();
00080     
00081     //T_e_b=T_e_b_0*RotoTraslation;
00082     
00083     T_e_b.block(0,0,3,3) = utilities::matrixOrthonormalization(T_e_b.block(0,0,3,3));
00084     
00085     //T_e_b(0,3) = traslation_x;
00086     //T_e_b(1,3) = traslation_y;
00087     //T_e_b(2,3) = traslation_z;
00088     
00089     /*printf("T_e_b=\n");
00090     for(int i = 0; i<4; i++){
00091     for(int j = 0; j<4; j++){
00092         printf("%4.3f", T_e_b(i,j));
00093     }
00094     printf("\r\n\n"); 
00095     } */
00096 
00097     q_out = arm.backwardKinematics(T_e_b, q_out, 0.005, 1, 0.001, 0.001 ,150, 50);
00098 
00099     q_out_sat =  arm.jointSaturation(q_out);
00100     //q_out = Vector6f::Zero();
00101 
00102     
00103     /*printf("q=");
00104     for(int i = 0; i<6; i++){
00105         printf(" %4.1f",q_out[i]);
00106     }
00107     printf("\r\n"); */  
00108     
00109      /*   printf("q_sat=");
00110     for(int i = 0; i<6; i++){
00111         printf(" %4.1f",q_out_sat[i]);
00112     }
00113     printf("\r\n\n"); */ 
00114     
00115     //q_act = arm.getJointPos();
00116     
00117     
00118     
00119    /* printf("q_act = ");
00120     for(int i = 0; i<6; i++){
00121         printf(" %4.2f ",q_act[i]);
00122     }
00123     printf("\r\n");*/
00124     
00125     arm.setJointPos(q_out);
00126     
00127     //arm.gravityVector(q);
00128     //arm.inertiaMatrix(q);
00129     //arm.stiffnessVector(q);
00130     //arm.frictionVector(q);
00131     
00132     
00133 
00134     
00135     t = t+0.001;
00136     wait_us(1000);
00137 
00138 }
00139 
00140      
00141 }
00142