00001 #include "uncalibvs_alg.h"
00002
00003
00004 Uncalibvs_Algorithm::Uncalibvs_Algorithm(void) {
00005 base_rtw();
00006 initZeros();
00007 }
00008
00009
00010 Uncalibvs_Algorithm::~Uncalibvs_Algorithm(void){
00011 }
00012
00013
00014 void Uncalibvs_Algorithm::config_update(Config& new_cfg, uint32_t level){
00015 this->lock();
00016
00017
00018 this->config_=new_cfg;
00019
00020 this->unlock();
00021 }
00022
00023
00024 void Uncalibvs_Algorithm::uncalib_vs(const bool& traditional, const bool& random_points, const bool& quadrotor, const bool& arm, const bool& output_files, const std::string& path, const double& lambda, const double& final_z, const Eigen::Matrix4f& cTo, const double& secs, const Eigen::MatrixXf& V2, KDL::Chain& chain, Eigen::MatrixXf& QQ, const Eigen::Matrix3f& Rquad_inertial, Eigen::MatrixXf& Vel_quad){
00025 this->lock();
00026
00027 traditional_=traditional;
00028 random_points_=random_points;
00029 quadrotor_=quadrotor;
00030 arm_=arm;
00031 output_files_=output_files;
00032 path_=path;
00033 cTo_=cTo;
00034 secs_=secs;
00035 V2_=V2;
00036 chain_=chain;
00037 QQ_=QQ;
00038 lambda_=lambda;
00039 final_z_=final_z;
00040
00041 if (secs_==0) {
00042 base_rtw();
00043 initZeros();
00044 }
00045
00046
00047 T0c_x_.block(0,0,3,3) << -1, 0, 0, 0, -1, 0, 0, 0, 1;
00048 T0c_x_.col(3) << 0, 0.2, -final_z_, 1;
00049 T0c_x_.row(3) << 0, 0, 0, 1;
00050
00051 T0c_0_=cTo_.inverse();
00052
00053
00054 if(output_files_){
00055 const char * outputFilename = path_.c_str();
00056 std::ofstream myfile;
00057 myfile.open (outputFilename, std::ios::out | std::ios::app | std::ios::binary);
00058 myfile << cTo_;
00059 myfile << "\n \n";
00060 myfile.close();
00061 }
00062
00063
00064
00065
00066
00067 T0c_new_=T0c_0_;
00068 Tc0_=cTo_;
00069
00070
00071 cP_ = Tc0_ * P0_;
00072
00073
00074
00075 ccP_=cP_;
00076
00077 features_vector();
00078 S_ = s_;
00079
00080
00081 Eigen::Matrix4f Tc0_x;
00082 Tc0_x=T0c_x_.inverse();
00083 cP_x_ = Tc0_x * P0_;
00084
00085
00086 ccP_=cP_x_;
00087
00088 features_vector();
00089 S_x_ = s_;
00090
00091
00092
00093 pj_ = Tc0_ * pn_;
00094
00095
00096 image_projection(ff_,ff_);
00097
00098
00099 Eigen::MatrixXf ran(2,n_);
00100 ran.setRandom();
00101 uv_ = uv_ + noise_ * ran;
00102
00103
00104 if (traditional_){
00105 compute_jacobian_t();
00106 }
00107 else {
00108 compute_jacobian_u();
00109 }
00110
00111
00112 K_control();
00113
00114 Eigen::Matrix4f diTj;
00115 diTj=Eigen::Matrix4f::Zero();
00116
00117 diTj = T0c_new_ * V_mod_;
00118
00119
00120 diTj_int_ = diTj_int_ + diTj * secs_;
00121
00122 if (quadrotor_){
00123
00124 if (arm_){
00125 qa_kinematics();
00126 qa_control();
00127
00128
00129 QQ_.block(6,0,5,1) = QQ_.block(6,0,5,1)+Velqa_.block(6,0,5,1) * secs_;
00130 QQ=QQ_;
00131 }
00132 else{
00133 q_control();
00134 }
00135 }
00136
00139
00140
00141
00142
00143
00144
00145
00146
00147
00148
00149
00150
00151
00152
00153
00154
00155
00156
00157
00158
00159
00160
00161
00162
00163
00164
00165
00166
00167
00168
00169
00170
00171
00172
00173
00174
00175
00176
00177
00178
00179
00180
00181
00182
00183
00184
00185
00186
00187
00188
00189
00190
00191
00192
00193
00194
00197 Vel_quad=Vel_;
00198
00199 this->unlock();
00200 }
00201
00203 void Uncalibvs_Algorithm::base_rtw(){
00204
00205 if (random_points_){
00207 n_=9;
00208
00209 pn_ = Eigen::MatrixXf::Zero(4,n_);
00210 pn_=1*pn_.setRandom();
00211 pn_.row(3) = Eigen::VectorXf::Ones(n_);
00212
00213 cj_0_= Eigen::MatrixXf::Zero(3,3);
00214 cj_0_(0,0)=pn_.row(0).mean();
00215 cj_0_(1,0)=pn_.row(1).mean();
00216 cj_0_(2,0)=pn_.row(2).mean();
00217
00218
00219 Eigen::MatrixXf q(3,n_);
00220 q=pn_.block(0,0,3,n_);
00221 q.colwise()-=cj_0_.col(0);
00222
00223
00224 Eigen::Matrix3f Q = (q * q.transpose());
00225
00226
00227 Eigen::JacobiSVD<Eigen::Matrix3f> svd (Q, Eigen::ComputeFullU | Eigen::ComputeFullV);
00228
00229 Eigen::Matrix3f v = svd.matrixV ();
00230
00231 cj_0_.col(1)=cj_0_.col(0)+v.col(0);
00232
00233 cj_0_.col(2)=cj_0_.col(0)-v.col(1);
00234
00235
00236 if ( svd.singularValues()(2)<0.00001){
00237 planar_=true;
00238 }
00239 else {
00240 planar_=false;
00241 }
00242
00243 Eigen::MatrixXf dd1(3,1),dd2(3,1),dd3(3,1);
00244 if (planar_){
00245 std::cout << "\n" << \
00246 "**************************************** Planar case ***************************************" \
00247 << std::endl;
00248 P0_= Eigen::MatrixXf::Zero(4,3);
00249 P0_.block(0,0,3,3)=cj_0_;
00250 P0_.row(3) << 1, 1, 1;
00251
00252 Eigen::MatrixXf D(3,2),DD(2,2);
00253
00254 dd1=cj_0_.col(1).array()-cj_0_.col(0).array();
00255 dd2=cj_0_.col(2).array()-cj_0_.col(0).array();
00256 D.col(0)=dd1;
00257 D.col(1)=dd2;
00258 DD=D.transpose()*D;
00259
00260
00261 alfas_= Eigen::MatrixXf::Zero(3,n_);
00262
00263 alfas_.block(1,0,2,4)=(DD.inverse()*D.transpose()) * q;
00264
00265 alfas_.row(0)=Eigen::MatrixXf::Ones(1,n_)-alfas_.block(1,0,2,n_).colwise().sum();
00266 }
00267 else{
00268 std::cout << "\n" << \
00269 "**************************************** NON Planar case ***************************************" \
00270 << std::endl;
00271
00272
00273 Eigen::Matrix3f Temp;
00274 Temp=cj_0_;
00275 cj_0_.resize(3,4);
00276 cj_0_.block(0,0,3,3)=Temp;
00277
00278 cj_0_.col(3)=cj_0_.col(0)+v.col(2);
00279 P0_= Eigen::MatrixXf::Zero(4,4);
00280
00281 P0_.block(0,0,3,4)=cj_0_;
00282 P0_.row(3) << 1, 1, 1, 1;
00283
00284 dd1=cj_0_.col(1).array()-cj_0_.col(0).array();
00285 dd2=cj_0_.col(2).array()-cj_0_.col(0).array();
00286 dd3=cj_0_.col(3).array()-cj_0_.col(0).array();
00287
00288 Eigen::MatrixXf D(3,3),DD(2,2);
00289 D.col(0)=dd1;
00290 D.col(1)=dd2;
00291 D.col(2)=dd3;
00292 DD=D.transpose()*D;
00293
00294 alfas_= Eigen::MatrixXf::Zero(4,n_);
00295
00296 alfas_.block(1,0,3,n_)=(DD.inverse()*D.transpose()) * q;
00297
00298 alfas_.row(0)=Eigen::MatrixXf::Ones(1,n_)-alfas_.block(1,0,3,n_).colwise().sum();
00299 }
00300 }
00301 else {
00303 n_=9;
00304
00305
00306 pn_ = Eigen::MatrixXf::Zero(4,n_);
00307 pn_.row(0) << -0.124725, -0.431413, 0.375723, 0.658402, -0.29928, 0.314608, -0.203127, -0.0350187, -0.70468;
00308 pn_.row(1) << 0.86367, 0.477069, -0.668052, -0.339326, 0.37334, 0.717353, 0.629534, -0.56835, 0.762124;
00309 pn_.row(2) << 0.86162, 0.279958, -0.119791, -0.542064, 0.912936, -0.12088, 0.368437, 0.900505, 0.282161;
00310 pn_.row(3) << 1, 1, 1, 1, 1, 1, 1, 1, 1;
00311
00312 cj_0_= Eigen::MatrixXf::Zero(3,4);
00313
00314 cj_0_.row(0) << -0.0499455, -0.559669, 0.127189, 0.791961;
00315 cj_0_.row(1) << 0.249707, 0.962512, 0.884623, 0.547683;
00316 cj_0_.row(2) << 0.313654, 0.795411, -0.438347, 0.763547;
00317
00318
00319 P0_ = Eigen::MatrixXf::Zero(4,4);
00320
00321 P0_.row(0) << -0.0499456, -0.559669, 0.127189, 0.791961;
00322 P0_.row(1) << 0.249707, 0.962513, 0.884623, 0.547683;
00323 P0_.row(2) << 0.313654, 0.795411, -0.438347, 0.763547;
00324 P0_.row(3) << 1, 1, 1, 1;
00325
00326
00327 alfas_= Eigen::MatrixXf::Zero(4,n_);
00328 alfas_.row(0) << -0.0707542, 0.826173, 2.37142, 1.76233, 0.815866, 0.122787, 0.443096, 2.23324, 0.495841;
00329 alfas_.row(1) << 0.73974, 0.340274, -1.07997, -1.19318, 0.503927, -0.0618209, 0.375215, -0.308004, 0.683816;
00330 alfas_.row(2) << -0.0355018, 0.102124, -0.181349, 0.394987, -0.41633, 0.68826, 0.172827, -0.958066, 0.233048;
00331 alfas_.row(3) << 0.366516, -0.268571, -0.110102, 0.0358625, 0.0965375, 0.250773, 0.00886206, 0.0328262, -0.412705;
00332
00333
00334
00335
00336
00337
00338
00339
00340
00341
00342
00343
00344
00345
00346
00347
00348
00349
00350
00351
00352
00353
00354
00355
00356
00357
00358
00359
00360
00361
00362
00363 planar_=false;
00364 }
00365 }
00366
00368 void Uncalibvs_Algorithm::initZeros(){
00369
00370 if (planar_){
00371
00372 s_= Eigen::MatrixXf::Zero(6,1);
00373
00374 S_= Eigen::MatrixXf::Zero(6,1);
00375
00376 S_x_= Eigen::MatrixXf::Zero(6,1);
00377
00378 cP_= Eigen::MatrixXf::Zero(4,3);
00379
00380 cP_x_= Eigen::MatrixXf::Zero(4,3);
00381
00382 ccP_= Eigen::MatrixXf::Zero(4,3);
00383
00384 J_= Eigen::MatrixXf::Zero(6,6);
00385 diff_=Eigen::MatrixXf::Zero(6,1);
00386 }
00387 else {
00388
00389
00390
00391 s_= Eigen::MatrixXf::Zero(8,1);
00392
00393 S_= Eigen::MatrixXf::Zero(8,1);
00394
00395 S_x_= Eigen::MatrixXf::Zero(8,1);
00396
00397 cP_= Eigen::MatrixXf::Zero(4,4);
00398
00399 cP_x_= Eigen::MatrixXf::Zero(4,4);
00400
00401 ccP_= Eigen::MatrixXf::Zero(4,4);
00402
00403 J_= Eigen::MatrixXf::Zero(8,6);
00404 diff_=Eigen::MatrixXf::Zero(8,1);
00405 }
00406
00407
00408
00409 uv_= Eigen::MatrixXf::Zero(2,n_);
00410
00411 diTj_int_=Eigen::Matrix4f::Zero();
00412
00413 Vel_= Eigen::MatrixXf::Zero(6,1);
00414
00415
00416
00417 u0_=376;
00418 v0_=240;
00419 ff_=10;
00420 ff_old_=ff_;
00421 noise_=0;
00422
00423
00424
00425
00426 }
00427
00429 void Uncalibvs_Algorithm::image_projection(const float& fu, const float& fv){
00430
00431
00432 uv_.row(0) = (pj_.row(0).array()/pj_.row(2).array()) * fu + u0_;
00433 uv_.row(1) = (pj_.row(1).array()/pj_.row(2).array()) * fv + v0_;
00434 }
00435
00437 void Uncalibvs_Algorithm::features_vector(){
00438
00439
00440 if (planar_){
00441 Eigen::ArrayXXf xx(1,3),yy(1,3);
00442
00443 xx=(ccP_.row(0).array()/ccP_.row(2).array());
00444 yy=(ccP_.row(1).array()/ccP_.row(2).array());
00445
00446 s_(0,0)=xx(0,0);
00447 s_(1,0)=yy(0,0);
00448 s_(2,0)=xx(0,1);
00449 s_(3,0)=yy(0,1);
00450 s_(4,0)=xx(0,2);
00451 s_(5,0)=yy(0,2);
00452 }
00453 else {
00454 Eigen::ArrayXXf xx(1,4),yy(1,4);
00455
00456 xx=(ccP_.row(0).array()/ccP_.row(2).array());
00457 yy=(ccP_.row(1).array()/ccP_.row(2).array());
00458
00459 s_(0,0)=xx(0,0);
00460 s_(1,0)=yy(0,0);
00461 s_(2,0)=xx(0,1);
00462 s_(3,0)=yy(0,1);
00463 s_(4,0)=xx(0,2);
00464 s_(5,0)=yy(0,2);
00465 s_(6,0)=xx(0,3);
00466 s_(7,0)=yy(0,3);
00467 }
00468 }
00469
00471 void Uncalibvs_Algorithm::compute_jacobian_t(){
00472 if (planar_){
00473 Eigen::ArrayXXf xx(1,3),yy(1,3),zz(1,3);
00474
00475 xx=(cP_.row(0).array()/cP_.row(2).array());
00476 yy=(cP_.row(1).array()/cP_.row(2).array());
00477 zz=(cP_.row(2).array());
00478
00479 float xj,yj,zj;
00480 for (int i=0; i<3; ++i){
00481 xj=xx(0,i);
00482 yj=yy(0,i);
00483 zj=zz(0,i);
00484 J_.row((i*2)) << (-1.0/zj), 0.0, (xj/zj), (xj*yj), -(1.0 + (xj*xj)), yj;
00485 J_.row((i*2)+1) << 0.0, (-1.0/zj), (yj/zj), (1.0 + (yj*yj)), -(xj*yj), -xj;
00486 }
00487 }
00488 else {
00489 Eigen::ArrayXXf xx(1,4),yy(1,4),zz(1,4);
00490
00491 xx=(cP_.row(0).array()/cP_.row(2).array());
00492 yy=(cP_.row(1).array()/cP_.row(2).array());
00493 zz=(cP_.row(2).array());
00494
00495 float xj,yj,zj;
00496 for (int i=0; i<4; ++i){
00497 xj=xx(0,i);
00498 yj=yy(0,i);
00499 zj=zz(0,i);
00500 J_.row((i*2)) << (-1.0/zj), 0.0, (xj/zj), (xj*yj), -(1.0 + (xj*xj)), yj;
00501 J_.row((i*2)+1) << 0.0, (-1.0/zj), (yj/zj), (1.0 + (yj*yj)), -(xj*yj), -xj;
00502 }
00503 }
00504
00505
00506 Eigen::MatrixXf Jtemp(J_.rows(),J_.cols());
00507 Jtemp=J_;
00508 J_.col(0)=-Jtemp.col(1);
00509 J_.col(1)=-Jtemp.col(0);
00510 J_.col(2)=-Jtemp.col(2);
00511 J_.col(3)=Jtemp.col(5);
00512 J_.col(4)=Jtemp.col(4);
00513 J_.col(5)=Jtemp.col(3);
00514
00515 }
00516
00518 void Uncalibvs_Algorithm::compute_jacobian_u(){
00519 if (planar_) {
00520 Eigen::MatrixXf m;
00521 m=Eigen::MatrixXf::Zero((2*n_),9);
00522
00523
00524 for (int i=0; i<n_; ++i){
00525 for (int j=0; j<3; ++j){
00526
00527 m((2*i),(j*3))=alfas_(j,i);
00528 m((2*i),(j*3)+2)=alfas_(j,i)*(u0_-uv_(0,i));
00529
00530 m((2*i)+1,(j*3)+1)=alfas_(j,i);
00531 m((2*i)+1,(j*3)+2)=alfas_(j,i)*(v0_-uv_(1,i));
00532 }
00533 }
00534
00535
00536 Eigen::MatrixXf M(9,9),UU(9,9),VV(9,9);
00537 M = (m.transpose() * m);
00538
00539
00540 Eigen::SelfAdjointEigenSolver<Eigen::MatrixXf> eigensolver(M);
00541 Eigen::VectorXf v(9,1);
00542 v =eigensolver.eigenvectors().col(0);
00543
00544
00545 int g=0;
00546 Eigen::Vector3f d,va,vb,vc,w;
00547 Eigen::Vector2f b;
00548 Eigen::MatrixXf L(3,2);
00549 L=Eigen::MatrixXf::Zero(3,2);
00550
00551 for (int i=0; i<2; ++i){
00552 for (int j=i+1; j<3; ++j){
00553 va=v.block(i*3,0,3,1);
00554 vb=v.block(j*3,0,3,1);
00555 vc=va-vb;
00556 L(g,0)=(vc.topRows(2).transpose()*vc.topRows(2));
00557 L(g,1)= (vc(2)*vc(2));
00558 w = cj_0_.col(i)-cj_0_.col(j);
00559 d(g,0)=(w.transpose() * w);
00560 g=g+1;
00561 }
00562 }
00563
00564 Eigen::Matrix2f LL;
00565 Eigen::MatrixXf L_inv(3,2);
00566
00567 LL=(L.transpose()*L);
00568 L_inv=LL.inverse()*L.transpose();
00569 b(0)= L_inv.row(0) * d;
00570 b(1)= L_inv.row(1) * d;
00571
00572
00573
00574 float Beta=+sqrt(fabs(b(0)));
00575 ff_=+sqrt(fabs(b(1)))/Beta;
00576
00577 ff_old_=ff_;
00578
00579 Eigen::VectorXf C2(9,1);
00580
00581 C2=Beta*v;
00582
00583
00584 Eigen::Matrix3f check;
00585 check.col(0)=C2.block(0,0,3,1);
00586 check.col(1)=C2.block(3,0,3,1);
00587 check.col(2)=C2.block(6,0,3,1);
00588
00589 if (check.determinant()>0){
00590 C2=-C2;
00591 }
00592 C2(2,0)=C2(2,0)*ff_;
00593 C2(5,0)=C2(5,0)*ff_;
00594 C2(8,0)=C2(8,0)*ff_;
00595
00596
00597 float xj,yj,zj;
00598 for (int i=0; i<3; ++i){
00599 xj=C2((i*3),0);
00600 yj=C2((i*3)+1,0);
00601 zj=C2((i*3)+2,0);
00602 J_.row((i*2)) << (-1.0/zj), 0.0, (xj/zj), (xj*yj), -(1.0 + (xj*xj)), yj;
00603 J_.row((i*2)+1) << 0.0, (-1.0/zj), (yj/zj), (1.0 + (yj*yj)), -(xj*yj), -xj;
00604 }
00605 }
00606
00608 else {
00609 Eigen::MatrixXf m;
00610 m=Eigen::MatrixXf::Zero((2*n_),12);
00611
00612
00613 for (int i=0; i<n_; ++i){
00614 for (int j=0; j<4; ++j){
00615
00616 m((2*i),(j*3))=alfas_(j,i);
00617 m((2*i),(j*3)+2)=alfas_(j,i)*(u0_-uv_(0,i));
00618
00619
00620 m((2*i)+1,(j*3)+1)=alfas_(j,i);
00621 m((2*i)+1,(j*3)+2)=alfas_(j,i)*(v0_-uv_(1,i));
00622 }
00623 }
00624
00625
00626 Eigen::MatrixXf M(12,12),UU(12,12),VV(12,12);
00627 M = (m.transpose() * m);
00628
00629
00630 Eigen::SelfAdjointEigenSolver<Eigen::MatrixXf> eigensolver(M);
00631 Eigen::VectorXf v(12,1);
00632 v =eigensolver.eigenvectors().col(0);
00633
00634
00635 int g=0;
00636 Eigen::Vector3f va,vb,vc,w;
00637 Eigen::VectorXf d(6,1);
00638 Eigen::Vector2f b;
00639 Eigen::MatrixXf L;
00640 L=Eigen::MatrixXf::Zero(6,2);
00641
00642 for (int i=0; i<3; ++i){
00643 for (int j=i+1; j<4; ++j){
00644 va=v.block(i*3,0,3,1);
00645 vb=v.block(j*3,0,3,1);
00646 vc=va-vb;
00647 L(g,0)=(vc.topRows(2).transpose()*vc.topRows(2));
00648 L(g,1)= (vc(2)*vc(2));
00649 w = cj_0_.col(i)-cj_0_.col(j);
00650 d(g,0)=(w.transpose() * w);
00651 g=g+1;
00652 }
00653 }
00654
00655 Eigen::Matrix2f LL;
00656 Eigen::MatrixXf L_inv(6,2);
00657
00658 LL=(L.transpose()*L);
00659 L_inv=LL.inverse()*L.transpose();
00660 b(0)= L_inv.row(0) * d;
00661 b(1)= L_inv.row(1) * d;
00662
00663
00664 float Beta=+sqrt(fabs(b(0)));
00665 ff_=+sqrt(fabs(b(1)))/Beta;
00666
00667 ff_old_=ff_;
00668
00669 Eigen::VectorXf C2(12,1);
00670
00671 C2=Beta*v;
00672
00673
00674 Eigen::Vector3f cc1,cc2,cc3;
00675 cc1=(C2.block(3,0,3,1)-C2.block(0,0,3,1));
00676 cc2=(C2.block(6,0,3,1)-C2.block(0,0,3,1));
00677 cc3=(C2.block(9,0,3,1)-C2.block(0,0,3,1));
00678
00679 Eigen::Vector3f ccc1,ccc2,ccc3;
00680 ccc1=cc1.cross(cc2);
00681 ccc2=cc1.cross(cc3);
00682 ccc3=cc2.cross(cc3);
00683
00684 float oo1,oo2,oo3;
00685 oo1=(ccc1.transpose()*cc3);
00686 oo2=(ccc2.transpose()*cc2);
00687 oo3=(ccc3.transpose()*cc1);
00688
00689 if ((oo1 < 0) || (oo2>0) || (oo3<0)){
00690 C2=-C2;
00691 }
00692 C2(2,0)=C2(2,0)*ff_;
00693 C2(5,0)=C2(5,0)*ff_;
00694 C2(8,0)=C2(8,0)*ff_;
00695 C2(11,0)=C2(11,0)*ff_;
00696
00697
00698 float xj,yj,zj;
00699 for (int i=0; i<4; ++i){
00700 xj=C2((i*3),0);
00701 yj=C2((i*3)+1,0);
00702 zj=C2((i*3)+2,0);
00703 J_.row((i*2)) << (-1.0/zj), 0.0, (xj/zj), (xj*yj), -(1.0 + (xj*xj)), yj;
00704 J_.row((i*2)+1) << 0.0, (-1.0/zj), (yj/zj), (1.0 + (yj*yj)), -(xj*yj), -xj;
00705 }
00706 }
00707
00708
00709 Eigen::MatrixXf Jtemp(J_.rows(),J_.cols());
00710 Jtemp=J_;
00711 J_.col(0)=-Jtemp.col(1);
00712 J_.col(1)=-Jtemp.col(0);
00713 J_.col(2)=-Jtemp.col(2);
00714 J_.col(3)=Jtemp.col(5);
00715 J_.col(4)=Jtemp.col(4);
00716 J_.col(5)=Jtemp.col(3);
00717 }
00718
00720 void Uncalibvs_Algorithm::K_control(){
00721 if (planar_) {
00722
00723 diff_=S_.array()-S_x_.array();
00724
00725
00726 Eigen::MatrixXf J_pseudo(6,6);
00727 J_pseudo=Eigen::MatrixXf::Zero(6,6);
00728 J_pseudo=J_.inverse();
00729 Vel_=-lambda_*(J_pseudo*diff_);
00730 }
00731
00732 else {
00733
00734 diff_=S_.array()-S_x_.array();
00735
00736 Eigen::MatrixXf JJ(6,6);
00737
00738 JJ=J_.transpose()*J_;
00739 Eigen::MatrixXf J_pseudo(8,6);
00740 J_pseudo=Eigen::MatrixXf::Zero(8,6);
00741 J_pseudo=JJ.inverse()*J_.transpose();
00742
00743
00744 Vel_=-lambda_*(J_pseudo*diff_);
00745 }
00746
00747 Eigen::MatrixXf v_linear(3,1),v_angular(3,1);
00748 Eigen::Matrix3f skew_v_angular;
00749
00750
00751 v_linear = Vel_.block(0,0,3,1);
00752
00753
00754 v_angular = Vel_.block(3,0,3,1);
00755
00756
00757 skew_v_angular.row(0) << 0, -v_angular(2), v_angular(1);
00758 skew_v_angular.row(1) << v_angular(2), 0, -v_angular(0);
00759 skew_v_angular.row(2) << -v_angular(1), v_angular(0), 0;
00760
00761
00762 V_mod_.block(0,0,3,3)=skew_v_angular;
00763 V_mod_.block(0,3,3,1)=v_linear;
00764 V_mod_.row(3) << 0, 0, 0, 0;
00765
00766
00767 }
00768
00770 void Uncalibvs_Algorithm::q_control(){
00771
00772
00773 Eigen::MatrixXf J1(8,4),J2(8,2),V1(4,1);
00774
00775 J1.col(0)=J_.col(0);
00776 J1.col(1)=J_.col(1);
00777 J1.col(2)=J_.col(2);
00778 J1.col(3)=J_.col(5);
00779 J2.col(0)=J_.col(3);
00780 J2.col(1)=J_.col(4);
00781
00782
00783
00784
00785
00786
00787
00788
00789
00790 Eigen::MatrixXf JJ1(4,4);
00791 Eigen::MatrixXf J1_pseudo(4,8);
00792
00793 JJ1=J1.transpose()*J1;
00794 J1_pseudo=JJ1.inverse()*J1.transpose();
00795
00796
00797 V1=-J1_pseudo*lambda_*(diff_+(J2*V2_));
00798
00799 Eigen::MatrixXf Velq(6,1);
00800 Velq=Eigen::MatrixXf::Zero(6,1);
00801
00802
00803 Velq.block(0,0,3,1)=V1.block(0,0,3,1);
00804 Velq(5,0)=-V1(3,0)/2;
00805
00806 Vel_=Velq.block(0,0,6,1);
00807
00808 }
00809
00811 void Uncalibvs_Algorithm::qa_control(){
00812
00813
00814
00815 Eigen::MatrixXf J1(8,4),J2(8,2),V1(4,1),Jqa1(6,9),Jqa2(6,2),Vtotal(9,1);
00816
00817 Jqa1.block(0,0,6,3)=Jqa_.block(0,0,6,3);
00818 Jqa1.block(0,3,6,6)=Jqa_.block(0,5,6,6);
00819 Jqa2=Jqa_.block(0,3,6,2);
00820
00821 Eigen::MatrixXf eye(9,9);
00822 eye=Eigen::MatrixXf::Identity(9,9);
00823
00824 Eigen::MatrixXf res(9,1),res2(9,9);
00825 Eigen::MatrixXf b(6,1);
00826
00827 b=Vel_-Jqa2*V2_;
00828
00829 res = Jqa1.colPivHouseholderQr().solve(b);
00830
00831 res2= Jqa1.colPivHouseholderQr().solve(Jqa1);
00832
00833 v_null_space();
00834
00835 Vtotal=res+(eye-res2)*Vqa0_;
00836
00838 Velqa_=Eigen::MatrixXf::Zero(11,1);
00839
00840 Velqa_.block(0,0,3,1)=Vtotal.block(0,0,3,1);
00841
00842 Velqa_.block(5,0,6,1)=Vtotal.block(3,0,6,1);
00843
00844 Velqa_.block(6,0,1,1)=-Velqa_.block(6,0,1,1);
00845 Vel_=Velqa_.block(0,0,6,1);
00846 }
00847
00849 void Uncalibvs_Algorithm::v_null_space(){
00850 Vqa0_=Eigen::MatrixXf::Zero(9,1);
00851 float qxma,qxmi,qxm,qyma,qymi,qym,qzma,qzmi,qzm,qyama,qyami,qyam,a0ma,a0mi,a0m,a1ma,a1mi,a1m,a2ma,a2mi,a2m,a3ma,a3mi,a3m,a4ma,a4mi,a4m;
00852
00853
00854 float ndof=9;
00855
00856
00857 qxma=10;
00858 qxmi=-10;
00859 qxm=0.0;
00860
00861 qyma=10;
00862 qymi=-10;
00863 qym=0.0;
00864
00865 qzma=10;
00866 qzmi=1.0;
00867 qzm=4.0;
00868
00869 qyama=3.1416;
00870 qyami=-3.1416;
00871 qyam=0.0;
00872
00873
00874 a0ma=0.548;
00875 a0mi=-0.548;
00876 a0m=0.0;
00877
00878
00879
00880
00881 a1ma=1.57;
00882 a1mi=-1.57;
00883 a1m=0.0;
00884
00885
00886
00887
00888 a2ma=5.23;
00889 a2mi=0;
00890 a2m=2.615;
00891
00892
00893
00894
00895 a3ma=5.23;
00896 a3mi=0;
00897 a3m=2.615;
00898
00899
00900
00901
00902 a4ma=3.1416;
00903 a4mi=-3.1416;
00904 a4m=0.0;
00905
00906
00907
00908
00909
00910 Vqa0_(0,0)=(-0/ndof)*((QQ_(0,0)-qxm)/((qxma-qxmi)*(qxma-qxmi)));
00911 Vqa0_(1,0)=(-0/ndof)*((QQ_(1,0)-qym)/((qyma-qymi)*(qyma-qymi)));
00912 Vqa0_(2,0)=(-50/ndof)*((QQ_(2,0)-qzm)/((qzma-qzmi)*(qzma-qzmi)));
00913 Vqa0_(3,0)=(-30/ndof)*((QQ_(5,0)-qyam)/((qyama-qyami)*(qyama-qyami)));
00914 Vqa0_(4,0)=(-100/ndof)*((QQ_(6,0)-a0m)/((a0ma-a0mi)*(a0ma-a0mi)));
00915 Vqa0_(5,0)=(-100/ndof)*((QQ_(7,0)-a1m)/((a1ma-a1mi)*(a1ma-a1mi)));
00916 Vqa0_(6,0)=(-100/ndof)*((QQ_(8,0)-a2m)/((a2ma-a2mi)*(a2ma-a2mi)));
00917 Vqa0_(7,0)=(-100/ndof)*((QQ_(9,0)-a3m)/((a3ma-a3mi)*(a3ma-a3mi)));
00918 Vqa0_(8,0)=(-30/ndof)*((QQ_(10,0)-a4m)/((a4ma-a4mi)*(a4ma-a4mi)));
00919 }
00920
00922 void Uncalibvs_Algorithm::qa_kinematics(){
00923
00924
00925 jnt_to_pose_solver_.reset(new KDL::ChainFkSolverPos_recursive(chain_));
00926 jnt_to_jac_solver_.reset(new KDL::ChainJntToJacSolver(chain_));
00927
00928
00929 unsigned int nj = chain_.getNrOfJoints();
00930 jnt_pos_ = KDL::JntArray(nj);
00931
00932 jnt_pos_.resize(nj);
00933 jacobian_.resize(nj);
00934
00935 for(unsigned int i=0; i < nj; i++){
00936 jnt_pos_.data(i) = QQ_(6+i,0);
00937 }
00938
00939
00940 KDL::Frame kdlTarm;
00941 jnt_to_pose_solver_->JntToCart(jnt_pos_, kdlTarm);
00942
00943 double ro,pit,ya;
00944 kdlTarm.M.GetEulerZYX(ya,pit,ro);
00945
00946 Eigen::Matrix4f Tarm, Tiner;
00947 Eigen::Matrix3f Rarm;
00948
00949 Rarm = Eigen::AngleAxisf(ro, Eigen::Vector3f::UnitZ()) \
00950 * Eigen::AngleAxisf(pit, Eigen::Vector3f::UnitY()) \
00951 * Eigen::AngleAxisf(ya, Eigen::Vector3f::UnitX());
00952
00953 Tarm.block(0,0,3,3)=Rarm;
00954 Tarm(0,3) = (double)kdlTarm.p.data[0];
00955 Tarm(1,3) = (double)kdlTarm.p.data[1];
00956 Tarm(2,3) = (double)kdlTarm.p.data[2];
00957 Tarm.row(3) << 0, 0, 0, 1;
00958
00959 Tiner=Tc0_*Tarm.inverse();
00960
00961
00962 QQ_.block(0,0,3,1) = Tiner.block(0,3,3,1);
00963 Eigen::Matrix3f Riner;
00964 Riner=Tiner.block(0,0,3,3);
00965
00966 QQ_.block(3,0,3,1) = Riner.eulerAngles(0,1,2);
00967
00968
00969 jnt_to_jac_solver_->JntToJac(jnt_pos_, jacobian_);
00970
00971 Jqa_=Eigen::MatrixXf::Zero(6,11);
00972
00973 Jqa_.block(0,0,6,6)=(Eigen::MatrixXf::Identity(6,6));
00974
00975
00976
00977 for (int i=0; i<6; ++i){
00978 for (int j=0; j<5; ++j){
00979 Jqa_(i,6+j)=(float)jacobian_.data(i,j);
00980 }
00981 }
00982 }
00983