Robot
Embed:
(wiki syntax)
Show/hide line numbers
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 }
Generated on Wed Aug 3 2022 01:29:06 by
1.7.2