Robot

Dependencies:   mbed QEI

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers Robot.cpp Source File

Robot.cpp

00001 //
00002 // Created by m007 on 26/06/2020.
00003 //
00004 
00005 #include "Ro/Robot.h"
00006 Robot::Robot(float _ToolMatrix[][4],float _BaseMatrix[][4],const float DH_Table[], const float lenght[],float dyn_mat [][10],const float MotorParam[],const float ControlGain[],int RobotDOF,const Link::Links_Type link [], const float MotorConstraints[],PinName PinSensorA[], PinName PinSensorB[], const float SensorProperties[], PinName PinDriver[]):ToolMatrix(4,4),BaseMatrix(4,4),_Jacobian(6,RobotDOF){
00007     _RobotDOF = RobotDOF;
00008     ToolMatrix = _ToolMatrix;
00009     BaseMatrix = _BaseMatrix;
00010 
00011     _Linker = new Link[_RobotDOF];
00012     _Motor = new Motor[_RobotDOF];
00013 
00014     for(int i=0;i<_RobotDOF; i++){
00015 
00016         _Motor[i]._Control= new Control(ControlGain[i*3], ControlGain[i*3+1], ControlGain[i*3+2],
00017                                               MotorParam[i*5],MotorParam[i*5+1] ,20, MotorParam[i*5+2], MotorParam[i*5+3],MotorParam[i*5+4]);
00018         _Motor[i]._Sensor= new Sensor(PinSensorA[i], PinSensorB[i],SensorProperties[(i*4)], SensorProperties[(i*4)+1],SensorProperties[(i*4)+2], SensorProperties[(i*4)+3]);
00019         _Motor[i]._Driver= new Driver(PinDriver[i]);
00020 
00021         _Motor[i].Constrains_v = MotorConstraints[(i*3)];
00022         _Motor[i].Constrains_a = MotorConstraints[(i*3)+1];
00023         _Motor[i].Constrains_j = MotorConstraints[(i*3)+2];
00024 
00025          _Linker[i].Type = link[i];
00026          _Linker[i].Lenght = lenght[i];
00027          _Linker[i].theta = DH_Table[((i*4)+0)];
00028          _Linker[i].alpha = DH_Table[((i*4)+1)];
00029          _Linker[i].d = DH_Table[((i*4)+2)];
00030          _Linker[i].a = DH_Table[((i*4)+3)];
00031 
00032          _Linker[i].Inertia_tensor(0,0) = dyn_mat[i][1]+dyn_mat[i][2];
00033          _Linker[i].Inertia_tensor(0,1) = dyn_mat[i][3];
00034          _Linker[i].Inertia_tensor(0,2) = dyn_mat[i][4];
00035 
00036          _Linker[i].Inertia_tensor(1,0) = dyn_mat[i][3];
00037          _Linker[i].Inertia_tensor(1,1) = dyn_mat[i][0]+dyn_mat[i][2];
00038          _Linker[i].Inertia_tensor(1,2) = dyn_mat[i][5];
00039 
00040          _Linker[i].Inertia_tensor(2,0) = dyn_mat[i][4];
00041          _Linker[i].Inertia_tensor(2,1) = dyn_mat[i][5];
00042          _Linker[i].Inertia_tensor(2,2) = dyn_mat[i][1]+dyn_mat[i][0];
00043 
00044          _Linker[i].Cent_Gravity(0)= dyn_mat[i][6];
00045          _Linker[i].Cent_Gravity(1) = dyn_mat[i][7];
00046          _Linker[i].Cent_Gravity(2) = dyn_mat[i][8];
00047 
00048          _Linker[i].Mass = dyn_mat[i][9];
00049          for(int x=0; x<4; x++) {
00050             for (int y = 0; y < 4; y++) {
00051                  _Linker[i].A0(x,y)=0;
00052             }
00053         }
00054     }
00055 }
00056 Robot::Robot(Matrix<float>& _ToolMatrix,Matrix<float>& _BaseMatrix,const float DH_Table[], const float lenght[],float dyn_mat [][10],const float MotorParam[],const float ControlGain[],int RobotDOF,const Link::Links_Type link [], const float MotorConstraints[],PinName PinSensorA[], PinName PinSensorB[],const float SensorProperties[], PinName PinDriver[]):ToolMatrix(4,4),BaseMatrix(4,4),_Jacobian(6,RobotDOF){
00057     _RobotDOF = RobotDOF;
00058     ToolMatrix = _ToolMatrix;
00059     BaseMatrix = _BaseMatrix;
00060 
00061     _Linker = new Link[_RobotDOF];
00062     _Motor = new Motor[_RobotDOF];
00063 
00064     for(int i=0;i<_RobotDOF; i++){
00065         _Motor[i]._Control= new Control(ControlGain[i*3], ControlGain[i*3+1], ControlGain[i*3+2],
00066                                                 MotorParam[i*5],MotorParam[i*5+1] ,20, MotorParam[i*5+2], MotorParam[i*5+3],MotorParam[i*5+4]);
00067         _Motor[i]._Sensor= new Sensor(PinSensorA[i], PinSensorB[i],SensorProperties[(i*4)], SensorProperties[(i*4)+1],SensorProperties[(i*4)+2], SensorProperties[(i*4)+3]);
00068         _Motor[i]._Driver= new Driver(PinDriver[i]);
00069 
00070         _Motor[i].Constrains_v = MotorConstraints[(i*3)];
00071         _Motor[i].Constrains_a = MotorConstraints[(i*3)+1];
00072         _Motor[i].Constrains_j = MotorConstraints[(i*3)+2];
00073 
00074         _Linker[i].Type = link[i];
00075         _Linker[i].Lenght = lenght[i];
00076         _Linker[i].theta = DH_Table[((i*4)+0)];
00077         _Linker[i].alpha = DH_Table[((i*4)+1)];
00078         _Linker[i].d = DH_Table[((i*4)+2)];
00079         _Linker[i].a = DH_Table[((i*4)+3)];
00080 
00081         _Linker[i].Inertia_tensor(0,0) = dyn_mat[i][1]+dyn_mat[i][2];
00082         _Linker[i].Inertia_tensor(0,1) = dyn_mat[i][3];
00083         _Linker[i].Inertia_tensor(0,2) = dyn_mat[i][4];
00084 
00085         _Linker[i].Inertia_tensor(1,0) = dyn_mat[i][3];
00086         _Linker[i].Inertia_tensor(1,1) = dyn_mat[i][0]+dyn_mat[i][2];
00087         _Linker[i].Inertia_tensor(1,2) = dyn_mat[i][5];
00088 
00089         _Linker[i].Inertia_tensor(2,0) = dyn_mat[i][4];
00090         _Linker[i].Inertia_tensor(2,1) = dyn_mat[i][5];
00091         _Linker[i].Inertia_tensor(2,2) = dyn_mat[i][1]+dyn_mat[i][0];
00092 
00093         _Linker[i].Cent_Gravity(0)= dyn_mat[i][6];
00094         _Linker[i].Cent_Gravity(1) = dyn_mat[i][7];
00095         _Linker[i].Cent_Gravity(2) = dyn_mat[i][8];
00096 
00097         _Linker[i].Mass = dyn_mat[i][9];
00098 
00099         for(int x=0; x<4; x++) {
00100             for (int y = 0; y < 4; y++) {
00101                 _Linker[i].A0(x,y)=0;
00102             }
00103         }
00104     }
00105 }
00106 Robot::~Robot(){
00107     for(int i=0;i<_RobotDOF; i++){
00108         delete[] _Motor[i]._Control;
00109         delete[] _Motor[i]._Sensor;
00110         delete[] _Motor[i]._Driver;
00111     }
00112     delete[] _Linker;
00113     delete[] _Motor;
00114 }
00115 Vector<float> Robot::GetTorque(){
00116     Vector<float> result(_RobotDOF);
00117     for(int i=0; i<_RobotDOF; i++){
00118         result(i) = _Linker[i].Tau;
00119     }
00120     return result;
00121 }
00122 Matrix <float> Robot::GetRotation(int num){
00123     Matrix<float> result(3,3);
00124     result = _Linker[num].A0;
00125     return result;
00126 }
00127 Matrix <float> Robot::GetRotation(Matrix<float> mat){
00128     Matrix<float> result(3,3);
00129     result = mat;
00130     return result;
00131 }
00132 Vector <float>  Robot::GetPosition(Matrix<float> mat){
00133     Vector<float> result(3);
00134     for(int i=0; i<3;i++) {
00135         result(i) = mat(i,3);
00136     }
00137     return result;
00138 }
00139 
00140 Link Robot::GetLink(int num){
00141     return _Linker[num];
00142 }
00143 Matrix<float> Robot::GetJacobian(){
00144     return _Jacobian;
00145 }
00146 
00147 float Robot::ConstraintsVelocityMotor(int num){
00148     return _Motor[num].Constrains_v;   
00149 }
00150 float Robot::ConstraintsAccelerationMotor(int num){
00151     return _Motor[num].Constrains_a;  
00152 }
00153 float Robot::ConstraintsJerkMotor(int num){
00154     return _Motor[num].Constrains_j;
00155 }
00156 
00157 Vector<float> Robot::GetKP(){
00158     Vector<float> result(_RobotDOF);
00159     for(int i=0; i<_RobotDOF; i++){
00160         result(i) = _Motor[i]._Control->GetKP();
00161     }
00162     return result;
00163 }
00164 Vector<float> Robot::GetKV(){
00165     Vector<float> result(_RobotDOF);
00166     for(int i=0; i<_RobotDOF; i++){
00167         result(i) = _Motor[i]._Control->GetKV();
00168     }
00169     return result;   
00170 }
00171 Vector<float> Robot::GetKI(){
00172     Vector<float> result(_RobotDOF);
00173     for(int i=0; i<_RobotDOF; i++){
00174         result(i) = _Motor[i]._Control->GetKI();
00175     }
00176     return result;
00177 }
00178 
00179 
00180 Vector<float> Robot::GetEndPosition(){
00181     Vector<float> result(3);
00182     Matrix<float> A(4,4);
00183     A = _Linker[0].A0;
00184     for(int i=1; i<_RobotDOF; i++){
00185         A = A*_Linker[i] .A0;
00186     }
00187     result(0) = A(0,3);
00188     result(1) = A(1,3);
00189     result(2) = A(2,3);
00190     return result;
00191 }
00192 int Robot::GetDOF(){
00193     return _RobotDOF;
00194 }
00195 float Robot::ReadSensor(int num){
00196     return _Motor[num]._Sensor->readSensor();
00197 }
00198 float Robot::ReadVelocity(float prevValue,float sampling,int num){
00199     return _Motor[num]._Sensor->estimatevelocity (prevValue,sampling);
00200 }
00201 Vector<float> Robot::ReturnSaturationControl(Vector<float> value){
00202     Vector<float> result(_RobotDOF);
00203     for(int i=0; i<_RobotDOF; i++){
00204         result(i) = _Motor[i]._Control->SaturationControl(value(i));
00205     }
00206     return result;
00207 }
00208 Vector<float> Robot::ReturnComputeP(Vector<float> qref, Vector<float> qi){
00209     Vector<float> result(_RobotDOF);
00210     for(int i=0; i<_RobotDOF; i++){
00211         result(i) = _Motor[i]._Control->ComputeP(qref(i), qi(i));
00212     }
00213     return result;
00214 }
00215 Vector<float> Robot::ReturnComputePD_FirstVersion(Vector<float> qref, Vector<float> qi, Vector<float> qdotref, Vector<float> qdoti){
00216     Vector<float> result(_RobotDOF);
00217     for(int i=0; i<_RobotDOF; i++){
00218         result(i) = _Motor[i]._Control->ComputePD_FirstVersion(qref(i), qi(i), qdotref(i), qdoti(i));
00219     }
00220     return result;
00221 }
00222 Vector<float> Robot::ReturnComputePD_SecondVersion(Vector<float> qref, Vector<float> qi, Vector<float> qdoti){
00223     Vector<float> result(_RobotDOF);
00224     for(int i=0; i<_RobotDOF; i++){
00225         result(i) = _Motor[i]._Control->ComputePD_SecondVersion(qref(i), qi(i), qdoti(i));
00226     }
00227     return result;
00228 }
00229 Vector<float> Robot::ReturnComputePID(Vector<float> qref, Vector<float> qi, Vector<float> qdotref, Vector<float> qdoti){
00230     Vector<float> result(_RobotDOF);
00231     for(int i=0; i<_RobotDOF; i++){
00232         result(i) = _Motor[i]._Control->ComputePID(qref(i), qi(i), qdotref(i), qdoti(i));
00233     }
00234     return result;
00235 }
00236 Vector<float> Robot::ReturnComputePDG(Vector<float> qref, Vector<float> qi, Vector<float> qdotref, Vector<float> qdoti, Vector<float> Tau){
00237     Vector<float> result(_RobotDOF);
00238     for(int i=0; i<_RobotDOF; i++){
00239         result(i) = _Motor[i]._Control->ComputePDG(qref(i), qi(i), qdotref(i), qdoti(i), Tau(i));
00240     }
00241     return result;
00242 }
00243 int Robot::SendData(Vector<float> data){
00244     int isCorrect;
00245     for(int i=0; i<_RobotDOF; i++){
00246         isCorrect = _Motor[i]._Driver->sendmessage(data(i));
00247         if(isCorrect == 0){
00248             return 0;
00249         }
00250     }
00251     return 1;
00252 }
00253 void Robot::ForwardKinematics(const float thetaOffset[]){
00254     float STheta,CTheta,SAlpha,CAlpha,theta,d;
00255     for (int i=0; i<_RobotDOF;i++) {
00256         if ( _Linker[i].Type == Link::REVOLUTION) {
00257             theta = thetaOffset[i];
00258             d =  _Linker[i].d;
00259         }else{
00260             d =  thetaOffset[i];
00261             theta =  _Linker[i].theta;
00262         }
00263         STheta = sinf(theta);
00264         CTheta = cosf(theta);
00265         SAlpha = sinf(  _Linker[i].alpha);
00266         CAlpha = cosf(  _Linker[i].alpha);
00267         if( (CAlpha>-0.000001F)&&(CAlpha<0.000001F)){
00268             CAlpha = 0;
00269         }
00270         if( (SAlpha>-0.000001F)&&(SAlpha<0.000001F)){
00271             SAlpha = 0;
00272         }
00273         if( (STheta>-0.000001F)&&(STheta<0.000001F)){
00274             STheta = 0;
00275         }
00276         if( (CTheta>-0.000001F)&&(CTheta<0.000001F)){
00277             CTheta = 0;
00278         }
00279         for (int x = 0; x < 4; x++) {
00280             for (int y = 0; y < 4; y++) {
00281                 if (x == 0 && y == 0) {
00282                      _Linker[i] .A0 (x,y)  = CTheta;
00283                 } else if (x == 0 && y == 1) {
00284                      _Linker[i] .A0  (x,y)  = -1 * CAlpha * STheta;
00285                 } else if (x == 0 && y == 2) {
00286                      _Linker[i] .A0  (x,y)  = SAlpha * STheta;
00287                 } else if (x == 0 && y == 3) {
00288                      _Linker[i] .A0  (x,y)  =  _Linker[i] .a * CTheta;
00289                 } else if (x == 1 && y == 0) {
00290                      _Linker[i] .A0  (x,y)  = STheta;
00291                 } else if (x == 1 && y == 1) {
00292                      _Linker[i] .A0  (x,y)  = CAlpha * CTheta;
00293                 } else if (x == 1 && y == 2) {
00294                      _Linker[i] .A0  (x,y)  = -1 * SAlpha * CTheta;
00295                 } else if (x == 1 && y == 3) {
00296                      _Linker[i] .A0  (x,y)  =  _Linker[i] .a * STheta;
00297                 } else if (x == 2 && y == 1) {
00298                      _Linker[i] .A0  (x,y)  = SAlpha;
00299                 } else if (x == 2 && y == 2) {
00300                      _Linker[i] .A0  (x,y)  = CAlpha;
00301                 } else if (x == 2 && y == 3) {
00302                      _Linker[i] .A0  (x,y)  = d;
00303                 } else if (x == 3 && y == 3) {
00304                      _Linker[i] .A0  (x,y)  = 1;
00305                 }
00306                 else{
00307                      _Linker[i] .A0  (x,y) =0;
00308                 }
00309                 if( ( _Linker[i] .A0  (x,y) >-0.000001F)&&( _Linker[i] .A0  (x,y) )<0.000001F){
00310                      _Linker[i] .A0  (x,y) =0;
00311                 }
00312 
00313             }
00314         }
00315     }
00316 }
00317 
00318  Vector<float> Robot::InverseKinematics(End_Effector* EFF_q0, int conf){
00319     /*USER SPECIFICATIONS*/
00320     Vector<float> result (_RobotDOF);
00321     float Xf = EFF_q0->x;
00322     float Yf = EFF_q0->y;
00323     float fi = EFF_q0->pitch;
00324     float theta = EFF_q0->roll;
00325 
00326     float L1 = _Linker[0].ReturnLenght();
00327     float L2 = _Linker[1].ReturnLenght();
00328     float L3 = _Linker[2].ReturnLenght();
00329     float L4 = _Linker[3].ReturnLenght();
00330 
00331     //Wrist points
00332     float Cfi = cosf(fi);
00333     float Sfi = sinf(fi);
00334     if ((Cfi > -0.000001F) && (Cfi < 0.000001F)) {
00335         Cfi = 0;
00336     }
00337     if ((Sfi > -0.000001F) && (Sfi < 0.000001F)) {
00338         Sfi = 0;
00339     }
00340     float Xm = Xf - L4 * Cfi;
00341     float Ym = Yf - L4 * Sfi;
00342 
00343     //Calc articular coordinates
00344     //q4
00345     result(3) = theta;
00346 
00347     //q2
00348     float C2 = ((powf(Xm, 2.0F)) + (powf(Ym, 2.0F)) - (powf(L1, 2.0F)) - (powf(L2, 2.0F))) / (2 * L1 * L2);
00349     float S2 = sqrtf(1 - powf(C2, 2.0F));
00350     float q21 = atan2f(S2, C2);
00351     float q22 = atan2f(-S2, C2);
00352     result (1) = q21;
00353 
00354     //q1
00355     float alpha = atan2f((L2 * sinf(result(1))), (L1 + L2 * cosf(result(1))));
00356     float beta_1 = atan2f(Ym, Xm);
00357     float beta_2 = atan2f(Ym, -Xm);
00358 
00359     float q1;
00360     if (conf == 0) {
00361         //Elbow up
00362         q1 = beta_1 - alpha;
00363     } else {
00364         //Elbow down
00365         q1 = beta_2 - alpha;
00366     }
00367     result (0)= q1;
00368     //q3
00369     result (2) = fi-result(0)-result(1);
00370     return result;
00371 }
00372 void Robot::Jacobian(float q[]){
00373     ForwardKinematics(q);
00374     Matrix<float> A0(4,4);
00375     A0 = BaseMatrix;
00376     for(int i=0; i<_RobotDOF; i++){
00377         A0 = A0 *_Linker[i].A0;
00378     }
00379     struct Vector<float> p0_dof;
00380     p0_dof(0) = A0(0,3);
00381     p0_dof(1) = A0(1,3);
00382     p0_dof(2) = A0(2,3);
00383 
00384     Vector<float> p[_RobotDOF];
00385     Vector<float>  pA0;
00386     pA0(0) = BaseMatrix(0,3);
00387     pA0(1) = BaseMatrix(1,3);
00388     pA0(2) = BaseMatrix(2,3);
00389 
00390     p[0] = p0_dof - pA0;
00391 
00392     Vector<float> z[_RobotDOF];
00393     Vector<float> z0;
00394     z0(0) = BaseMatrix(0,2);
00395     z0(1) = BaseMatrix(1,2);
00396     z0(2) = BaseMatrix(2,2);
00397 
00398     z[0]=z0;
00399 
00400 
00401     Matrix<float> A01;
00402     A01 = BaseMatrix * _Linker[0].A0;
00403     Vector<float> Jv(3);
00404     Vector<float> Jw(3);
00405     
00406     Matrix<float> J(6,_RobotDOF);
00407     if(_Linker[0].Type == Link::REVOLUTION){
00408         Jv = z[0].crossProduct(p[0]);
00409         Jw = z[0];
00410     }else{
00411         Jv = z[0];
00412         Jw = zeros(3);
00413     }
00414         J(1,0) = Jv(0);
00415         J(2,0) = Jv(1);
00416         J(3,0) = Jv(2);
00417         J(4,0) = Jw(0);
00418         J(5,0) = Jw(1);
00419         J(6,0) = Jw(2);
00420         
00421     for(int i=1;i<_RobotDOF;i++){
00422         pA0(0) = A01(0,3);
00423         pA0(1) = A01(1,3);
00424         pA0(2) = A01(2,3);
00425 
00426         z0(0) = A01(0,2);
00427         z0(1) = A01(1,2);
00428         z0(2) = A01(2,2);
00429 
00430         p[i] = p0_dof - pA0;
00431         z[i] = z0;
00432 
00433         if(_Linker[i].Type == Link::REVOLUTION){
00434             Jv = z[i].crossProduct(p[i]);
00435             Jw = z[i];
00436         }else{
00437             Jv = z[i];
00438             Jw = zeros(3);
00439         }
00440         
00441         A01 = A01 * _Linker[i].A0;
00442         J(0,i) = Jv(0);
00443         J(1,i) = Jv(1);
00444         J(2,i) = Jv(2);
00445         J(3,i) = Jw(0);
00446         J(4,i) = Jw(1);
00447         J(5,i) = Jw(2);
00448     }
00449     _Jacobian = J;
00450 }
00451 
00452 void Robot::EulerNewton(const float q[],const float qd[],const float qdd[]){
00453     Matrix<float> Rtot(3,3);
00454 
00455     //Get inverse rotation
00456     ForwardKinematics(q);
00457     Matrix<float> R3_post_inv = GetRotation(0).Inverse();
00458     Rtot = GetRotation(0);
00459 
00460     //Vector join Si-1 and Si
00461     Vector<float> R_pi(3);
00462 
00463     R_pi(0) = _Linker[0].a;
00464     if (( R_pi(0) > -0.000001F) && (R_pi(0)) < 0.000001F) {
00465         R_pi(0) = 0;
00466     }
00467 
00468     R_pi(1) = _Linker[0].d*sinf(_Linker[0].alpha);
00469     if (( R_pi(1) > -0.000001F) && (R_pi(1)) < 0.000001F) {
00470         R_pi(1) = 0;
00471     }
00472 
00473     R_pi(2) = _Linker[0].d*cosf(_Linker[0].alpha);
00474     if (( R_pi(2) > -0.000001F) && (R_pi(2)) < 0.000001F) {
00475         R_pi(2) = 0;
00476     }
00477 
00478     //Center of gravity
00479     Vector<float> s;
00480     if(_Linker[0].Mass !=0 ) {
00481         s(0) = _Linker[0].Cent_Gravity(0) / _Linker[0].Mass;
00482         s(1) = _Linker[0].Cent_Gravity(1) / _Linker[0].Mass;
00483         s(2) = _Linker[0].Cent_Gravity(2) / _Linker[0].Mass;
00484     }else{
00485         s = zeros(3);
00486     }
00487 
00488     //
00489     float Z0[3] = {0,0,1};
00490     Vector<float> z0;
00491     z0 = Z0;
00492 
00493     float g0[3] = {0,9.81F,0};
00494     Vector<float> g;
00495     g = g0;
00496 
00497     Vector<float> w,wd,vd,a,z03qd,z03qdd;
00498 
00499     if (_Linker[0].Type == Link::REVOLUTION){
00500         z03qd = z0 * qd[0];
00501         z03qdd = z0 * qdd[0];
00502        w  =  R3_post_inv * z03qd;
00503 
00504        wd  =  R3_post_inv * z03qdd;
00505 
00506         vd = (wd.crossProduct(R_pi)) + w.crossProduct(w.crossProduct(R_pi)) + (R3_post_inv * g);
00507     }else{
00508         z03qdd = (z0 * qdd[0]) + g;
00509         w = zeros(3);
00510         wd = zeros(3);
00511         vd = R3_post_inv * z03qdd;
00512     }
00513     a = (wd.crossProduct(s)) + (w.crossProduct(w.crossProduct(s))) + (vd);
00514 
00515     _Linker[0].w =  w;
00516     _Linker[0].wd = wd;
00517     _Linker[0].vd = vd;
00518     _Linker[0].acc = a;
00519 
00520     Vector<float> aux;
00521 
00522     for (int i=1; i<_RobotDOF; i++){
00523         //Get inverse rotation
00524         Rtot = Rtot * GetRotation(i);
00525         R3_post_inv = GetRotation(i).Inverse();
00526 
00527         //Vector join Si-1 and Si
00528         R_pi(0) = _Linker[i].a;
00529         R_pi(1) = _Linker[i].d*sinf(_Linker[i].alpha);
00530         R_pi(2) = _Linker[i].d*cosf(_Linker[i].alpha);
00531 
00532         //Center of gravity
00533         s(0) = _Linker[i].Cent_Gravity(0)/ _Linker[i].Mass;
00534         s(1) = _Linker[i].Cent_Gravity(1)/ _Linker[i].Mass;
00535         s(2) = _Linker[i].Cent_Gravity(2)/ _Linker[i].Mass;
00536 
00537         z03qd = z0 * qd[i];
00538         z03qdd = z0 * qdd[i];
00539 
00540         if (_Linker[i].Type == Link::REVOLUTION){
00541 
00542             aux = _Linker[i-1].w + z03qd;
00543             w  = R3_post_inv * aux;
00544             aux = _Linker[i-1].wd + z03qdd;
00545             wd = R3_post_inv * aux + (_Linker[i-1].w.crossProduct(z03qd));
00546             vd = (wd.crossProduct(R_pi)) + (w.crossProduct(w.crossProduct(R_pi))) + (R3_post_inv * _Linker[i-1].vd);
00547 
00548         }else{
00549 
00550             w = R3_post_inv * _Linker[i-1].w;
00551             wd = R3_post_inv * _Linker[i-1].wd;
00552             aux = z03qdd + _Linker[i-1].vd;
00553             vd = R3_post_inv * aux + (w.crossProduct(R_pi)) + ((w * 2.0F).crossProduct(R3_post_inv*z0*qd[i]))
00554                  + (w.crossProduct(w.crossProduct(R_pi)));
00555         }
00556 
00557         a = (wd.crossProduct(s)) + (w.crossProduct(w.crossProduct(s))) + vd;
00558         _Linker[i].w = w;
00559         _Linker[i].wd = wd;
00560         _Linker[i].vd = vd;
00561         _Linker[i].acc = a;
00562 
00563     }
00564 
00565     //Vector join Si-1 and Si
00566     R_pi(0) = _Linker[_RobotDOF-1].a;
00567     R_pi(1) = _Linker[_RobotDOF-1].d*sinf(_Linker[_RobotDOF-1].alpha);
00568     R_pi(2) = _Linker[_RobotDOF-1].d*cosf(_Linker[_RobotDOF-1].alpha);
00569 
00570     //Center of gravity
00571     s(0) = _Linker[_RobotDOF-1].Cent_Gravity(0)/ _Linker[_RobotDOF-1].Mass;
00572     s(1) = _Linker[_RobotDOF-1].Cent_Gravity(1)/ _Linker[_RobotDOF-1].Mass;
00573     s(2) = _Linker[_RobotDOF-1].Cent_Gravity(2)/ _Linker[_RobotDOF-1].Mass;
00574 
00575 
00576 
00577     // Fuerza ejercida sobre i en el cdm
00578     Vector<float> f_cdm;
00579     Vector<float> M_cdm;
00580     Vector<float> f;
00581     Vector<float> M;
00582 
00583     f_cdm = _Linker[_RobotDOF-1].acc * _Linker[_RobotDOF-1].Mass;
00584     M_cdm = (_Linker[_RobotDOF-1].Inertia_tensor * _Linker[_RobotDOF-1].wd)  +
00585             (_Linker[_RobotDOF-1].w.crossProduct(_Linker[_RobotDOF-1].Inertia_tensor * _Linker[_RobotDOF-1].w));
00586 
00587 
00588     f = f_cdm * -1;
00589 
00590     M = (R_pi + s).crossProduct(f_cdm) + M_cdm;
00591 
00592     float Tau;
00593     if (_Linker[_RobotDOF-1].Type == Link::REVOLUTION){
00594         Tau = (R3_post_inv.Transpose() * M) * z0;
00595     }else{
00596         Tau = (R3_post_inv.Transpose() * f) * z0;
00597     }
00598     _Linker[_RobotDOF-1].Tau = Tau;
00599     _Linker[_RobotDOF-1].f = f;
00600     _Linker[_RobotDOF-1].M = M;
00601 
00602     Matrix<float> R3_post;
00603     Matrix<float> R3_inv;
00604     Matrix<float> Rtot_inv;
00605     Matrix<float> R3_inv_ant;
00606     Vector<float> R_pi_post;
00607     Vector<float> A;
00608 
00609     for (int i=_RobotDOF-2; i>=0; i--){
00610 
00611         R3_post = GetRotation(i+1);
00612         printf("Hello");
00613         R3_inv = GetRotation(i+1).Inverse();
00614         Rtot = Rtot * R3_inv;
00615         Rtot_inv = Rtot.Inverse();
00616 
00617         R3_inv_ant = GetRotation(i).Inverse();
00618 
00619         //Vector join Si-1 and Si
00620         R_pi(0) = _Linker[i].a;
00621         R_pi(1) = _Linker[i].d*sinf(_Linker[i].alpha);
00622         R_pi(2) = _Linker[i].d*cosf(_Linker[i].alpha);
00623         R_pi_post = R3_inv * R_pi;
00624 
00625         //Center of gravity
00626         s(0) = _Linker[i].Cent_Gravity(0)/ _Linker[i].Mass;
00627         s(1) = _Linker[i].Cent_Gravity(1)/ _Linker[i].Mass;
00628         s(2) = _Linker[i].Cent_Gravity(2)/ _Linker[i].Mass;
00629 
00630 
00631         f_cdm = _Linker[i].acc * _Linker[i].Mass;
00632         M_cdm = (_Linker[i].Inertia_tensor * _Linker[i].wd)  +
00633                 (_Linker[i].w.crossProduct(_Linker[i].Inertia_tensor * _Linker[i].w));
00634 
00635         f = (R3_post * _Linker[i+1].f) + f_cdm;
00636         A   =   (_Linker[i+1].M + R_pi_post.crossProduct(_Linker[i+1].f));
00637         M = (R3_post * A) + ((R_pi + s).crossProduct(f_cdm) + M_cdm);
00638 
00639         if (_Linker[i].Type == Link::REVOLUTION){
00640             Tau = (R3_inv_ant.Transpose() * M) * z0;
00641         }else {
00642             Tau = (R3_inv_ant.Transpose() * f) * z0;
00643         }
00644 
00645         _Linker[i].Tau=Tau;
00646         _Linker[i].f = f;
00647         _Linker[i].M = M;
00648 
00649     }
00650 
00651 }
00652 
00653 void Robot::GravityTorque(float q[]){
00654     float grav[3] = {0, 9.81F, 0};
00655     Vector<float> g;
00656     g = grav;
00657     ForwardKinematics(q);
00658     Matrix<float> A0(4,4);
00659     A0 = _Linker[0].A0;
00660 
00661     Vector<float> pcg(4);
00662 
00663     Vector<float> position[_RobotDOF];
00664     position[0] = zeros(3);
00665     position[1] = GetPosition(A0);
00666 
00667     Vector<float> z0 [_RobotDOF];
00668     z0[0](0) = 0;
00669     z0[0](1) = 0;
00670     z0[0](2) = 1;
00671 
00672     Vector<float> m(4);
00673     for (int i = 0; i <_RobotDOF; i++) {
00674         if (i > 0) {
00675             z0[i](0) = A0(0,2);
00676             z0[i](1) = A0(1,2);
00677             z0[i](2) = A0(2,2);
00678 
00679             A0 = A0 * _Linker[i].A0;
00680             if (i<_RobotDOF-1) {
00681                 position[i + 1] = GetPosition(A0);
00682             }
00683         }
00684         m(0) = _Linker[i].Cent_Gravity(0) / _Linker[i].Mass;
00685         m(1) = _Linker[i].Cent_Gravity(1) / _Linker[i].Mass;
00686         m(2) = _Linker[i].Cent_Gravity(2) / _Linker[i].Mass;
00687         m(3) = 1;
00688 
00689 
00690         pcg = A0 * m;
00691         _Motor[i].pcg(0) = pcg(0);
00692         _Motor[i].pcg(1) = pcg(1);
00693         _Motor[i].pcg(2) = pcg(2);
00694     }
00695     Vector<float> M;
00696     Vector<float> mg;
00697     float Tau;
00698     for (int i =0; i < _RobotDOF; i++) {
00699         mg = g * _Linker[i].Mass;
00700         M = mg.crossProduct((_Motor[i].pcg - position[i]));
00701         for (int j = 0; j < _RobotDOF; j++) {
00702             if(j>i) {
00703                 mg = g * _Linker[j].Mass;
00704                 M = M + (mg.crossProduct(_Motor[j].pcg)-position[i]);
00705             }
00706         }
00707         Tau = M * z0[i];
00708         _Linker[i].Tau = Tau;
00709         M = zeros(3);
00710     }
00711 
00712 }