KinematicChain.cpp

00001 // -*- mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*-
00002 /* 
00003  * Copyright (C) 2008 Micha Hersch, EPFL
00004  * RobotCub Consortium, European Commission FP6 Project IST-004370
00005  * email:   micha.hersch@robotcub.org
00006  * website: www.robotcub.org
00007  * Permission is granted to copy, distribute, and/or modify this program
00008  * under the terms of the GNU General Public License, version 2 or any
00009  * later version published by the Free Software Foundation.
00010  *
00011  * A copy of the license can be found at
00012  * http://www.robotcub.org/icub/license/gpl.txt
00013  *
00014  * This program is distributed in the hope that it will be useful, but
00015  * WITHOUT ANY WARRANTY; without even the implied warranty of
00016  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
00017  * Public License for more details
00018  */
00019 #include "KinematicChain.h"
00020 
00021 
00022 KinematicChain::KinematicChain(int n){
00023   int i;
00024   nb_joints = n;
00025 #ifdef BLOCK_KC
00026   joints = joints_block;
00027   reverse = reverse_block;
00028 #endif
00029   tol = 3; //inverse kin tolerance
00030   modality = UNSPECIFIED_MODALITY;
00031 #ifdef WITH_LAST_LINK
00032   last_link = new Translation();
00033   own_last_link=1;
00034 #endif
00035   for(i=0;i<n;i++){
00036     joints[i] = new RigidTransfo();
00037     reverse[i] = 1;
00038   }
00039 
00040   for(i=n;i<MAX_LINKS;i++){
00041     joints[i] = NULL;
00042     reverse[i] = 0;
00043   }
00044 }
00045 
00046 
00047 KinematicChain::~KinematicChain(){
00048     if(own_last_link){
00049         delete last_link;
00050     }
00051 
00052 }
00053 
00054 
00055 void KinematicChain::FreeChain(){
00056   cout<<"freeing kinematic chain"<<endl;
00057   for(int i=0;i<MAX_LINKS;i++){
00058     if(joints[i]) delete joints[i];
00059   }
00060 #ifdef WITH_LAST_LINK 
00061   delete last_link;
00062 #endif
00063 }
00064 
00065 
00066 // int KinematicChain::AddJoint(RigidTransfo *t,int inv){
00067 //   reverse[nb_joints]=inv;
00068 //   joints[nb_joints++]=t;
00069 //   return nb_joints;
00070 // }
00071 
00072 int KinematicChain::SetAllLinks(CVector3_t *links){
00073   int ret =1;
00074   for(int i=0;i<=nb_joints;i++){
00075     ret*= SetLink(i,links[i]);
00076   } 
00077   return ret;
00078 }
00079 
00083 int KinematicChain::SetLink(int i, CVector3_t link){
00084   if(i==nb_joints){
00085 #ifdef WITH_LAST_LINK
00086     last_link->SetTranslation(link);
00087 #endif
00088   }
00089   else{
00090     if(i>nb_joints){
00091       return 0;
00092     }
00093     else{
00094       joints[i]->SetTranslation(link);
00095     }
00096   }
00097   return 1;
00098 }
00099 
00100 
00101 // int KinematicChain::GetLink(int i, CVector3_t link){
00102 //  if(i>nb_joints){
00103 //     return 0;
00104 //   }
00105 //  if(i==nb_joints){
00106 //    return last_link->GetTranslation(link);
00107 //  }
00108 //  else{ 
00109 //    return joints[i]->GetTranslation(link);
00110 //  }
00111 // }
00112 
00113 int KinematicChain::SetRotationAxis(int i,CVector3_t axis){
00114   if(i>=nb_joints){
00115     return 0;
00116   }
00117   return joints[i]->SetRotationAxis(axis);
00118 }
00119 
00120 int KinematicChain::SetAllRotationAxes(CVector3_t *axes){
00121   int ret =1;
00122   for(int i=0;i<nb_joints;i++){
00123     ret*= SetRotationAxis(i,axes[i]);
00124   } 
00125   return ret;
00126 }
00127 
00128 void KinematicChain::ScaleRate(float factor){
00129  for(int i=0;i<nb_joints;i++){
00130    joints[i]->ScaleRate(factor);
00131  }
00132 }
00133 
00134 
00135 int KinematicChain::GetRotationAxis(int i,CVector3_t axis){
00136   if(i>=nb_joints){
00137     return 0;
00138   }
00139   return joints[i]->GetRotationAxis(axis);
00140 }
00141 
00142 float KinematicChain::GetAngle(int i){
00143   if(i>=0 && i< nb_joints){
00144     return joints[i]->GetRotationAngle();
00145   }
00146   else{
00147     cout<<"no such joint: "<<i<<endl;
00148     return 0.0;
00149   }
00150 }
00151 
00152 void KinematicChain::RandomAxes(){
00153   for(int i=0;i<nb_joints;i++){
00154     joints[i]->RandomAxis();
00155   }
00156 }
00157 
00158 int KinematicChain::Copy(const KinematicChain& kc){
00159   int i,n,ret;
00160   
00161   if(nb_joints>kc.GetNbJoints()){
00162     n = kc.GetNbJoints();
00163     ret =1;
00164   }
00165   else{
00166     if(nb_joints<kc.GetNbJoints()){
00167       n = nb_joints;
00168       ret =-1;
00169     }
00170     else{
00171       n = nb_joints;
00172       ret =0;
00173     }
00174   }
00175   for(i=0;i<n;i++){
00176     joints[i]->Copy(*(kc.GetTransfo(i)));
00177   }
00178 #ifdef WITH_LAST_LINK
00179   last_link->SetTranslation(kc.GetLastLink()->GetTranslation());
00180 #endif
00181 
00182   return ret;
00183 }
00184 
00185 
00186 
00187 void KinematicChain::ForwardKinematics(CVector3_t pos, CVector3_t local_pos)const{
00188     CVector3_t v1,v2;
00189   int i;
00190 #ifdef WITH_LAST_LINK
00191   if(!local_pos){
00192       v_clear(v2); 
00193       last_link->Transform(v2,v1);
00194   }
00195   else{
00196       last_link->Transform(local_pos,v1);
00197   }
00198 #else
00199   if(!local_pos){
00200       v_clear(v1);
00201   }
00202   else{
00203      v_copy(local_pos,v1);
00204   }
00205 #endif 
00206  for(i=nb_joints-1;i>=0;i--){
00207     Transform(i,v1,v2);
00208     v_copy(v2,v1);
00209   }
00210   v_copy(v1,pos);
00211 }
00212 
00213 
00214 void KinematicChain::Transform(int i,CVector3_t in, float angle,CVector3_t out){
00215   joints[i]->SetAngle(angle);
00216   Transform(i,in,out);
00217 }
00218 
00219 
00220 void KinematicChain::SetAngles(float *angles){
00221   for(int i=0;i<nb_joints;i++){
00222       joints[i]->SetAngle(angles[i]);
00223   }
00224 }
00225 
00226 
00227 #ifdef DEPRECATED
00228 void KinematicChain::InverseKinematics(CVector3_t pos){
00229   float tol2,d1,d2;
00230   int it=0;
00231   tol2= tol*tol;
00232   d2= 1000;
00233   do{
00234     //  d1=InverseKinematicsStep(pos);
00235     d1=RedundantInverseKinematicsStep(pos);
00236     if(it%10==0){
00237       if(abs(d1-d2)<0.001 && d1>tol){
00238 //      for(int i=0;i<nb_joints;i++){
00239 //        joints[i]->AddToNorm(RND(0.2)-0.1);
00240 //      }
00241 //      SetRandomAngles();
00242 //      d2=1000;
00243       }
00244       d2=d1;
00245     }
00246     cout<<d1<<endl;
00247   }while(d1>tol && it++<200);
00248 }
00249 #endif
00250 
00255 float KinematicChain::InverseKinPosCCD(CVector3_t pos){
00256   CVector3_t stack[MAX_LINKS];
00257   CVector3_t tar,newrod,rod,diff;
00258   float dist=-1.0f;
00259   for(int j=0;j<50;j++){
00260     InverseKinematicsStack(pos,stack);
00261 #ifdef WITH_LAST_LINK
00262     last_link->GetTranslation(rod);
00263 #else
00264     v_clear(rod);//modify for reaching with specific points
00265 #endif
00266     for(int i=nb_joints-1;i>=0;i--){
00267       if(IsInverted(i)==-1){
00268         v_copy(stack[i],tar);
00269         Translate(i,rod,newrod); //getting to joint
00270         joints[i]->AimAt(tar,newrod);
00271         joints[i]->InvertRotationAngle();
00272         Rotate(i,newrod,rod);   // updating "rod"
00273         v_sub(tar,rod,diff);
00274       }
00275       else{
00276         Rotate(i,stack[i+1],tar); //rotating back
00277         joints[i]->AimAt(tar,rod);
00278         Rotate(i,rod,newrod);
00279         Translate(i,newrod,rod);
00280         v_sub(tar,newrod,diff);
00281       }
00282       dist=v_length(diff);
00283       if(dist<=tol){return dist;}
00284       //     cout<<v_length(diff)<<endl;;
00285     }
00286   }
00287   return dist;
00288 }
00289 
00290 
00291 
00292 
00293 void KinematicChain::InverseKinRotCCD(CQuat_t q){
00294   int i,j;
00295   CQuat_t q1,q2,q3,q4;
00296   
00297   q_inv(q,q1);
00298   for(i=0;i<nb_joints;i++){ //should be nb_joints-1 if not using last_link
00299     GetQuaternion(i,q2);
00300     q_multiply(q2,q1,q3);
00301     q_copy(q3,q1);
00302   } 
00303   for(j=0;j<20;j++){
00304     for(i=nb_joints-1;i>=0;i--){//should be nb_joints-2 if not using last_link
00305       GetInverseQuaternion(i,q2);
00306       q_multiply(q2,q1,q3); //q1*inv(Ri)
00307       joints[i]->MinimizeRotation(q3);
00308       if(IsInverted(i)==-1){
00309         joints[i]->InvertRotationAngle();
00310       }
00311       GetQuaternion(i,q2);
00312       q_multiply(q3,q2,q1);
00313       q_multiply(q2,q3,q4);
00314       cout<<"rot "<<v_length(q4)<<endl;
00315       //      if(v_length(diff)<tol){return;}
00316     }
00317     q_multiply(q,q1,q2);
00318     q_inv(q,q3);
00319     q_multiply(q2,q3,q1);
00320   }
00321 }
00322 
00323 float KinematicChain::InverseKinematicsCCD(CVector3_t pos, CQuat_t rot){
00324   int i,j;
00325   float rdist=0,tdist=0,k=0.001; //rotation vs translation weight
00326   CVector3_t stack[MAX_LINKS];
00327   CVector3_t tar,newrod,rod,diff;
00328   CQuat_t q1,q2,q3,q4;
00329 
00330   q_inv(rot,q1);
00331   for(i=0;i<nb_joints;i++){
00332     GetQuaternion(i,q2);
00333     q_multiply(q2,q1,q3);
00334     q_copy(q3,q1);
00335   } 
00336   for(j=0;j<50;j++){
00337 #ifdef WITH_LAST_LINK
00338     last_link->GetTranslation(rod);
00339 #else
00340     v_clear(rod);
00341 #endif
00342     InverseKinematicsStack(pos,stack);
00343     for(i=nb_joints-1;i>=0;i--){
00344       GetInverseQuaternion(i,q2);
00345       q_multiply(q2,q1,q3); //q1*inv(Ri)
00346       if(IsInverted(i)==-1){
00347         v_copy(stack[i],tar);
00348         Translate(i,rod,newrod); //getting to joint
00349         joints[i]->MinimizePositionAndRotation(tar,newrod,q3,k);
00350         joints[i]->InvertRotationAngle();
00351         Rotate(i,newrod,rod);   // updating "rod"
00352         v_sub(tar,rod,diff);
00353       }
00354       else{
00355         Rotate(i,stack[i+1],tar); //rotating back
00356         joints[i]->MinimizePositionAndRotation(tar,rod,q3,k);
00357         Rotate(i,rod,newrod);
00358         Translate(i,newrod,rod);
00359         v_sub(tar,newrod,diff);
00360       }
00361       GetQuaternion(i,q2);
00362       q_multiply(q3,q2,q1);
00363       q_multiply(q2,q3,q4);
00364       rdist = v_length(q4);//rotation distance, only the first 3 components 
00365       tdist = v_length(diff);//translation distance
00366       //     cout<<"rot "<<rdist<<" pos "<<tdist<<" prod: "<<(1-k)*rdist+k*tdist<<endl;
00367       if(tdist<tol && rdist<0.05){return rdist/0.05+tdist;}
00368     }
00369     q_multiply(rot,q1,q2);
00370     q_inv(rot,q3);
00371     q_multiply(q2,q3,q1);
00372   }
00373   return rdist/0.05 + tdist;
00374 }
00375 
00376 
00377 
00378 
00379 
00383 void KinematicChain::RotationStack(CQuat_t *q1){
00384   CQuat_t q;
00385   q_clear(q1[0]);//
00386   for(int i=0;i<nb_joints;i++){
00387     joints[i]->GetQuaternion(q);
00388     if(reverse[i]==-1){
00389       v_scale(q,-1,q); //inverting only the first 3 comp
00390     }
00391     q_multiply(q,q1[i],q1[i+1]);
00392   }
00393 }
00394 
00395 
00400 void KinematicChain::ForwardKinematicsStack(CVector3_t pos,CVector3_t *v1){
00401   v_copy(pos,v1[nb_joints]);
00402   for(int i=nb_joints;i>0;i--){
00403     Transform(i-1,v1[i],v1[i-1]);
00404  }
00405 }
00410 void KinematicChain::InverseKinematicsStack(CVector3_t pos, CVector3_t *ikstack){
00411   v_copy(pos,ikstack[0]);
00412   for(int i=0;i<nb_joints;i++){
00413     InverseTransform(i,ikstack[i],ikstack[i+1]);
00414   }
00415 }
00416 
00417 void KinematicChain::GlobalTransfo(RigidTransfo& rt){
00418   CVector3_t transl;
00419   GlobalTranslation(transl);
00420   GlobalRotation(rt);
00421   rt.SetTranslation(transl);
00422 }
00423 
00424 void KinematicChain::GlobalRotation(Rotation& r){
00425   int i;
00426   r.Identity();
00427   for(i=0;i<nb_joints;i++){
00428     if(reverse[i] == -1){
00429       joints[i]->Invert();
00430       r = r*((Rotation&)(*(joints[i])));
00431       joints[i]->Invert();   
00432     }
00433     else{
00434       r = r*((Rotation&)(*(joints[i])));
00435     }
00436   }
00437 }
00438 
00443 float KinematicChain::CollisionDetection(float& l1, float& l2){
00444   float distance,d=0.0f;
00445   int last = nb_joints-1;
00446   RigidTransfo rt;
00447   Rotation rot;
00448   CVector3_t v,v1,v2,p1,p2;
00449   v_clear(v);
00450   for(int i=1;i<last;i++){
00451     d += v_squ_length(joints[i]->GetTranslation());
00452   }
00453   if(d<epsilon){
00454     return -1;
00455   }
00456   GlobalTransfo(rt);
00457   GlobalRotation(rot);
00458   v_clear(p1);
00459   rt.Transform(p1,p2);
00460   if(IsInverted(last)==1){
00461     v_scale(GetTransfo(last)->GetTranslation(),-1,v);
00462   }
00463   else{
00464     GetTransfo(last)->GetTranslation(v);
00465   }
00466   rot.Transform(v,v2);
00467   //  cout<<"transfo "<<v<<" "<<v2<<" "<<v_length(v)<<" "<<v_length(v2)<<endl;
00468   
00469   if(IsInverted(0)==-1){
00470     v_scale(GetTransfo(0)->GetTranslation(),-1,v1);
00471   }
00472   else{
00473     GetTransfo(0)->GetTranslation(v1);
00474   }
00475 //   cout<<"input"<<endl<<p1<<" "<<v1<<endl;
00476 //   cout<<p2<<" "<<v2<<endl;
00477 //   cout<<*this<<endl;
00478   if(segments_nearest_points(p1,v1,p2,v2,&l1,&l2,&distance)){
00479     return distance;  
00480   }
00481   else{
00482     return -1;
00483   }
00484 }
00485 
00486 
00487 
00488 #ifdef DEPRECATED
00489 
00492 float KinematicChain::RedundantInverseKinematicsStep(CVector3_t target){
00493   int i,j;
00494   CVector3_t kinstack[MAX_LINKS];
00495   CVector3_t ikstack[MAX_LINKS];
00496   CVector3_t v1[MAX_LINKS];
00497   CVector3_t v2[MAX_LINKS];
00498   CVector3_t diff[MAX_LINKS];
00499   float delta_norm[MAX_LINKS];
00500   float sum=0;
00501   CVector3_t pos,tmp;
00502   CQuat_t q,q1,q2;
00503   Rotation rot;
00504  
00505   v_clear(pos);
00506   ForwardKinematicsStack(pos,kinstack);
00507   InverseKinematicsStack(target,ikstack);
00508   for(i=0;i<nb_joints;i++){
00509       joints[i]->InverseNormDerivative(ikstack[i],v1[i]);
00510       joints[i]->NormDerivative(kinstack[i+1],v2[i]);  
00511       v_sub(kinstack[i],ikstack[i],diff[i]);
00512       delta_norm[i]=0;
00513   }
00514   v_sub(kinstack[nb_joints],ikstack[nb_joints],diff[nb_joints]);
00515 
00516   for(j=0;j<=nb_joints;j++){
00517 
00518   // i in inverse side
00519     q_clear(q2);
00520     delta_norm[j-1] += v_dot(diff[j],v1[j-1]);//i = j-1
00521     for(i=j-2;i>=0;i--){
00522       joints[i+1]->GetQuaternion(q1);
00523       q_multiply(q2,q1,q);
00524       q_copy(q,q2);
00525  //      ComposedRotation(i+1,j-1,q);     
00526       v_scale(q,-1,q);
00527       rot.SetQuaternion(q);
00528       rot.Transform(v1[i],tmp);
00529       delta_norm[i] += v_dot(diff[j],tmp);
00530       //      cout<<i<<" + "<<v_dot(diff[j],tmp)<<endl;
00531     }
00532 
00533     // i in forward side
00534     if(j<nb_joints){
00535       delta_norm[j] -= v_dot(diff[j],v2[j]); //i=j
00536       cout<<j<<" - "<<v_dot(diff[j],v2[j])<<endl;
00537       q_clear(q2);
00538       for(i=j+1;i<nb_joints;i++){
00539         joints[i-1]->GetQuaternion(q1);
00540         q_multiply(q1,q2,q);
00541         q_copy(q,q2);
00542         // ComposedRotation(j,i-1,q);
00543         rot.SetQuaternion(q);
00544         rot.Transform(v2[i],tmp);
00545         delta_norm[i] -= v_dot(diff[j],tmp);
00546         cout<<i<<" - "<<v_dot(diff[j],tmp)<<endl;
00547       }
00548     }
00549   }
00550 
00551   for(j=0;j<nb_joints;j++){
00552     sum+=delta_norm[j]*delta_norm[j];
00553   }
00554   sum = 1/(sqrt(sum)*100);
00555   for(j=0;j<nb_joints;j++){
00556     joints[j]->AddToNorm(delta_norm[j]*sum);
00557   }
00558   cout<<*this<<endl;
00559   return v_length(diff[0]);
00560 }
00561 
00562 
00563 float KinematicChain::InverseKinematicsStep(CVector3_t pos){
00564   
00565   CVector3_t v1[MAX_LINKS];
00566   CVector3_t v,delta,v2;
00567   CQuat_t q;
00568   CQuat_t q1[MAX_LINKS];
00569   Rotation rot;
00570   int i;
00571   float dangles[MAX_LINKS];
00572   float sum,ret_val;
00573   v_clear(q1[0]);
00574   q_set(q1[0],0,q1[0]);
00575   for(i=0;i<nb_joints;i++){
00576     joints[i]->GetQuaternion(q);
00577     if(reverse[i]==-1){
00578       v_scale(q,-1,q); //inverting only the first 3 comp
00579     }
00580  q_multiply(q,q1[i],q1[i+1]);
00581   }
00582 
00583     // forward kinematics
00584 #ifdef WITH_LAST_LINK   
00585   v_clear(v1[nb_joints]);
00586   last_link->Transform(v1[nb_joints],v1[nb_joints-1]);
00587 #else
00588   v_clear(v1[nb_joints-1]);
00589 #endif
00590   for(i=nb_joints-1;i>0;i--){
00591     Transform(i,v1[i],v1[i-1]);
00592  }
00593     Transform(0,v1[0],v);
00594   v_sub(pos,v,v); //(y-R(0)
00595   ret_val = v_length(v);
00596   //   cout<<v_length(v)<<" :";
00597 
00598   //  cout<<"diff ";coutvec(v);
00599   sum=0;
00600   for(i=0;i<nb_joints;i++){
00601 
00602     rot.SetQuaternion(q1[i]);
00603 #ifdef OLD_KIN
00604     rot.Derivative(m1);
00605      v_transform_normal2(v,m1,delta);
00606 #endif
00607     //updating angle
00608      if(joints[i]->IsAdaptive()){
00609        joints[i]->NormDerivative(v1[i],v2);
00610        if(reverse[i]==-1){
00611          joints[i]->InverseNormDerivative(v1[i],v2);
00612        }
00613        else{
00614          joints[i]->NormDerivative(v1[i],v2);
00615        }
00616      }
00617      else{
00618        v_clear(v2);
00619      }
00620 #ifndef OLD_KIN
00621     rot.Transform(v2,delta);
00622 #endif
00623 //    coutvec(v);
00624 //     coutvec(delta);
00625     //   cout<<"dot "<<v_dot(delta,v);
00626     dangles[i] = v_dot(delta,v);
00627    
00628     sum+=dangles[i]*dangles[i];
00629     // cout
00630     //    cout<<" "<<sum; 
00631   }
00632   if(sum<epsilon){return ret_val;}
00633   sum = 1/(sqrt(sum)*500);
00634   for(i=0;i<nb_joints;i++){
00635     //  cout<<";"<<dangles[i];
00636     joints[i]->AddToNorm(dangles[i]*sum); //to be checked
00637   }
00638   return ret_val;
00639 }
00640 
00641 #endif
00642 
00643 //jac is assumed to be a homogeneous matrix 
00644 // the last joint is assumed to be irrelevant for the position
00645 void  KinematicChain::GetJacobian(float *jac){
00646   CQuat_t q[MAX_LINKS];
00647   CVector3_t v1[MAX_LINKS];
00648   CVector3_t v2,der; 
00649   float f;
00650   Rotation rot;
00651 #ifdef WITH_LAST_LINK
00652   last_link->GetTranslation(v2);
00653 #else
00654   v_clear(v2);//modify for reaching with specific point
00655 #endif
00656   RotationStack(q);
00657   ForwardKinematicsStack(v2,v1);
00658 #ifdef WITH_LAST_LINK
00659   for(int i=0;i<nb_joints;i++)//{ below
00660 #else
00661   for(int i=0;i<nb_joints-1;i++)//{ below ;  assuming the last rotation is irrelevant for the position
00662 #endif    
00663     { 
00664    rot.SetQuaternion(q[i]);
00665     if(reverse[i]==-1){
00666       joints[i]->InverseNormDerivative(v1[i+1],v2);
00667     }
00668     else{
00669       joints[i]->NormDerivative(v1[i+1],v2);
00670     }
00671     rot.Transform(v2,der);//d/dsin(theta/2);
00672     f=0.5*joints[i]->GetAlpha();
00673     v_scale(der,f,jac+4*i);
00674     jac[4*i+3]=0;
00675   }
00676 }
00677 
00678 //position and rotation jacobian
00679 void  KinematicChain::GetFullJacobian(float *jac){
00680   CQuat_t q[MAX_LINKS];
00681   CVector3_t v1[MAX_LINKS];
00682   CVector3_t v2,der; 
00683   CQuat_t q1,q2,q3,q4;
00684   float f;
00685   Rotation rot;
00686 
00687 #ifdef WITH_LAST_LINK
00688   last_link->GetTranslation(v2);
00689 #else
00690   v_clear(v2);
00691 #endif
00692   q_clear(q1);
00693   RotationStack(q);
00694   ForwardKinematicsStack(v2,v1);
00695   for(int i=nb_joints-1;i>=0;i--){
00696     rot.SetQuaternion(q[i]);
00697     joints[i]->NormDerivative(v1[i],v2);
00698     if(reverse[i]==-1){
00699       joints[i]->InverseNormDerivative(v1[i+1],v2);
00700       joints[i]->InverseQuaternionNormDerivative(q3);
00701     }
00702     else{
00703       joints[i]->NormDerivative(v1[i+1],v2);
00704       joints[i]->QuaternionNormDerivative(q3);
00705     }
00706     rot.Transform(v2,der);//d/dsin(theta/2);
00707     f=0.5*joints[i]->GetAlpha();
00708     v_scale(der,f,jac+6*i);
00709     q_multiply(q1,q3,q4);
00710     q_multiply(q4,q[i],q3);//q3 = q[i]* q2'* q1 where '*': quat. mult
00711     v_scale(q3,f,jac+6*i+3);//we take only the 3 first components
00712     GetQuaternion(i,q2);  
00713     q_multiply(q1,q2,q3); //for the next iteration
00714     q_copy(q3,q1);
00715  }
00716 }
00717 
00718 void KinematicChain::InitKinematicStack(const CVector3_t local_pos, CVector3_t out)const{
00719 #ifdef WITH_LAST_LINK
00720   CVector3_t v;
00721   if(local_pos){   
00722     last_link->GetTranslation(v);
00723     v_add(v,local_pos,out);
00724   }
00725   else{
00726     last_link->GetTranslation(out);
00727   }
00728 #else
00729   if(local_pos){
00730     v_copy(local_pos,out);
00731   }
00732   else{
00733     v_clear(out);
00734   }
00735 #endif
00736 }
00737 
00738 #define OPTIMISED_ROTATION_UPDATE
00739 #ifdef OPTIMISED_ROTATION_UPDATE
00740 //only rotations are considered for now
00741 float KinematicChain::UpdateWithFullJacobian(float *angle_pos,
00742                                              float *angle_disp, RigidTransfo *rt){
00743 
00744      CVector3_t delta_a[MAX_LINKS];
00745      CVector3_t v,vd,qd,qdiff,qds;
00746      CMatrix4_t m1,m2;
00747      CMatrix4_t rot_acc[MAX_LINKS];
00748      //  Rotation rot;
00749      float jac[MAX_LINKS*6];
00750      CQuat_t q1,q2,q3,q4,q7,q8;
00751      int i,j;
00752      float ret_val;//return value
00753      float sum_rot;
00754      SetAngles(angle_pos);
00755      GetFullJacobian(jac);
00756      v_clear(vd);//accumulator for v_dot
00757      v_clear(qd);//accumulator for q_dot
00758      for(i=0;i<nb_joints;i++){
00759         v_scale(jac+i*6+3,angle_disp[i],v);
00760         v_add(v,qd,qd);
00761      }
00762      if(v_length(qd)<epsilon){return 0;}//no movement
00763      rt->GetRotationParam(qds);//q_dot_star
00764      v_sub(qd,qds,qdiff);  
00765           ret_val = v_length(qdiff)/(v_length(qds));
00766      sum_rot=0;
00767     
00768 
00769      q_clear(q7); 
00770      q_clear(q8);
00771     
00772      for(j=0;j<nb_joints;j++){ //q8 is initialized with the gloabl rotation
00773          GetQuaternion(j,q2);
00774          q_multiply(q2,q8,q3);
00775          q_copy(q3,q8);
00776      }
00777 
00778      for(j=0;j<nb_joints;j++){
00779          m_clear(rot_acc[j]);
00780          if(j>0){
00781              GetQuaternion(j-1,q2);
00782              q_multiply(q7,q2,q3);
00783              q_copy(q3,q7);
00784          }
00785 
00786          GetInverseQuaternion(j,q2);
00787          q_multiply(q8,q2,q3);
00788          q_copy(q3,q8);
00789 
00790          q_clear(q1);         //initialization for i<j 
00791          q_copy(q7,q4);       //initialization for i<j 
00792          for(i=0;i<nb_joints;i++){ //k
00793              if(i<j){
00794                  QuaternionAxisDerivative(j,m1);
00795                  if(i>0){
00796                      GetQuaternion(i-1,q2);
00797                      q_multiply(q2,q1,q3);
00798                      q_copy(q3,q1);
00799                  }
00800                  GetInverseQuaternion(i,q2);
00801                  q_multiply(q4,q2,q3);
00802                  q_copy(q3,q4);
00803                
00804                  QuaternionAngleDerivative(i,q2);
00805                  q_multiply(q4,q2,q3);
00806                  q_multiply(q3,q1,q2); //before the matrix
00807                  q_copy(q8,q3);//after the matrix
00808              }
00809              if(i==j){   // i==j
00810                  QuaternionAxisAngleDerivative(j,m1);
00811                  q_copy(q8,q3);        // after the matrix
00812                  q_copy(q7,q2);        //before the matrix         
00813                  q_clear(q1);   //initialization for i>j 
00814                  q_copy(q8,q4); //initialization for i>j 
00815              }
00816              //i>j
00817              if(i>j){
00818                  QuaternionAxisDerivative(j,m1);
00819 
00820                  if(i>j+2){
00821                      GetQuaternion(i-1,q2);
00822                      q_multiply(q2,q1,q3);
00823                      q_copy(q3,q1);
00824                  }
00825                  
00826                  GetInverseQuaternion(i,q2);
00827                  q_multiply(q4,q2,q3);
00828                  q_copy(q3,q4);
00829 
00830                  QuaternionAngleDerivative(i,q3);
00831                  q_multiply(q4,q3,q2);
00832                  q_multiply(q2,q1,q3); //after the matrix
00833                  q_copy(q7,q2);// befire the matrix
00834              }
00835               
00836    //compute matrices            
00837              q_multiply(m1,q2,q4);
00838              q_copy(q4,m1);
00839              q_multiply(m1+4,q2,q4);
00840              q_copy(q4,m1+4);
00841              q_multiply(m1+8,q2,q4);
00842              q_copy(q4,m1+8);// last column of m1 is zero
00843                   
00844              q_multiply(q3,m1,m2);
00845              q_multiply(q3,m1+4,m2+4);
00846              q_multiply(q3,m1+8,m2+8);// last column of m1 is zero
00847              m_rescale(angle_disp[i],m2,m1);
00848              //accumulating sum_{i} d/daj(d/dthi(q)*thi
00849              // a index is horizontal, q index is vertical
00850              m_add(m1,rot_acc[j],rot_acc[j]);
00851          }
00852          v_transform_normal2(qdiff,rot_acc[j],delta_a[j]);
00853          v_scale(delta_a[j],-1,delta_a[j]); //gradient descent
00854          //cout<<"delta a "<<delta_a[j]<<endl;
00855          sum_rot += v_squ_length(delta_a[j]);
00856      }
00857      sum_rot = 0.5/sqrtf(sum_rot);
00858      for(j=0;j<nb_joints;j++){
00859          v_scale(delta_a[j],sum_rot,delta_a[j]);
00860          joints[j]->AddToAxis(delta_a[j]);
00861      }
00862      return ret_val;
00863 }
00864 
00865 #else
00866 
00867 //only rotations are considered for now
00868 float KinematicChain::UpdateWithFullJacobian(float *angle_pos,
00869                                              float *angle_disp, RigidTransfo *rt){
00870 
00871      CVector3_t delta_a[MAX_LINKS];
00872      CVector3_t v,vd,qd,qdiff,qds;
00873      CMatrix4_t m1,m2;
00874      CMatrix4_t rot_acc[MAX_LINKS];
00875      //  Rotation rot;
00876      float jac[MAX_LINKS*6];
00877      CQuat_t q1,q2,q3;
00878      int i,j,k;
00879      float ret_val;//return value
00880      float sum_rot;
00881      SetAngles(angle_pos);
00882      GetFullJacobian(jac);
00883      v_clear(vd);//accumulator for v_dot
00884      v_clear(qd);//accumulator for q_dot
00885      for(i=0;i<nb_joints;i++){
00886     //     v_scale(jac+i*6,angle_disp[i],v);
00887 //         v_add(v,vd,vd);
00888         v_scale(jac+i*6+3,angle_disp[i],v);
00889         v_add(v,qd,qd);
00890      }
00891      if(v_length(qd)<epsilon){return 0;}//no movement
00892      //   q_complete(qd,q1);
00893     //  cout<<"angle "<<angle_disp[0]<<endl;
00894 //      cout<<"having  ";coutvec(q1);
00895 //     rt->GetQuaternion(q2);  cout<<"instead ";coutvec(q2);    
00896      //    rt->GetInverseQuaternion(q2);
00897      //    q_multiply(q1,q2,q4);
00898      //    coutvec4(q4);
00899      //     ret_val = 2*acos(q4[3]); // q4 contains q*^{-1}q
00900    
00901      //     ret_val = 2*acos(q4[3]); 
00902      // ret_val = v_length(q4);
00903      rt->GetRotationParam(qds);//q_dot_star
00904      v_sub(qd,qds,qdiff);  
00905           ret_val = v_length(qdiff)/(v_length(qds));
00906      // ret_val = v_dot(qd,qds)/(v_length(qd)*v_length(qds));
00907 
00908      
00909      sum_rot=0;
00910      for(j=0;j<nb_joints;j++){
00911          m_clear(rot_acc[j]);
00912          for(i=0;i<nb_joints;i++){
00913              //        rt->GetInverseQuaternion(q1);  // first rotation in chain is q*^{-1}
00914              q_clear(q1);
00915              for(k=0;k<nb_joints;k++){
00916                  // k==j
00917                  if(k==j){
00918                      if(k==i){
00919                          QuaternionAxisAngleDerivative(j,m1);
00920                          //        cout<<m1<<endl;
00921                      }
00922                      else{
00923                          QuaternionAxisDerivative(j,m1);
00924                      }
00925                      q_multiply(m1,q1,q3);
00926                      q_copy(q3,m1);
00927                      q_multiply(m1+4,q1,q3);
00928                      q_copy(q3,m1+4);
00929                      q_multiply(m1+8,q1,q3);
00930                      q_copy(q3,m1+8);// last column of m1 is zero
00931                      q_clear(q1);
00932                  }
00933                  else{
00934                      //k==i
00935                      if(k==i){
00936                          QuaternionAngleDerivative(i,q2);
00937                          q_multiply(q2,q1,q3);
00938                          q_copy(q3,q1);
00939                      }
00940                      else{ //k!=i && k!=j
00941                          GetQuaternion(k,q2);
00942                          q_multiply(q2,q1,q3);
00943                          q_copy(q3,q1);
00944                      }
00945                  }
00946              }
00947              q_multiply(q1,m1,m2);
00948              q_multiply(q1,m1+4,m2+4);
00949              q_multiply(q1,m1+8,m2+8);// last column of m1 is zero
00950              m_rescale(angle_disp[i],m2,m1);
00951              //accumulating sum_{i} d/daj(d/dthi(q)*thi
00952              // a index is horizontal, q index is vertical
00953              m_add(m1,rot_acc[j],rot_acc[j]);
00954          }
00955          v_transform_normal2(qdiff,rot_acc[j],delta_a[j]);
00956          v_scale(delta_a[j],-1,delta_a[j]); //gradient descent
00957          //cout<<"delta a "<<delta_a[j]<<endl;
00958          sum_rot += v_squ_length(delta_a[j]);
00959      }
00960      sum_rot = 0.5/sqrtf(sum_rot);
00961      for(j=0;j<nb_joints;j++){
00962          v_scale(delta_a[j],sum_rot,delta_a[j]);
00963          joints[j]->AddToAxis(delta_a[j]);
00964      }
00965      return ret_val;
00966 }
00967 
00968 #endif
00969 
00970 // #define COSINE_MIN
00971 // #define WITH_CORRECTION
00972 float KinematicChain::UpdateWithJacobian(float *angle_pos, float *angle_disp, CVector3_t visual_disp, 
00973                                          float threshold, CVector3_t local_pos){
00974   CVector3_t v1[MAX_LINKS],vv[MAX_LINKS], delta_rot[MAX_LINKS], delta_tr[MAX_LINKS];
00975   CQuat_t q1[MAX_LINKS];
00976   CMatrix3_t jstack[MAX_LINKS];
00977   CVector3_t v,v2,dpos;
00978   CQuat_t q2,q3,q4,q5,q6;
00979   int i,k;
00980   Rotation rot;
00981   CMatrix3_t m0,m1,m2,m3,m4,m6,m7;
00982   float f,sum_rot=0,sum_tr=0,ret_val;
00983   SetAngles(angle_pos);
00984   InitKinematicStack(local_pos,v2);
00985   ForwardKinematicsStack(v2,v1);
00986   RotationStack(q1);
00987   q_clear(q6);
00988 
00989   // computing jacobian
00990   v_clear(dpos);
00991   for(i=0;i<nb_joints;i++){
00992     rot.SetQuaternion(q1[i]);
00993     joints[i]->AngleDerivative(v1[i+1],v2);
00994     //   cout<<v1[i+1]<<endl;
00995     rot.Transform(v2,v);
00996     v_scale(v,angle_disp[i],v2);
00997     v_add(dpos,v2,dpos);
00998 }
00999 
01000 #ifdef COSINE_MIN
01001   float norm_prop = v_length(dpos);
01002   float norm_vis  = v_length(visual_disp);
01003   if(norm_vis> epsilon && norm_prop >epsilon){
01004     ret_val = v_dot(dpos,visual_disp)/(norm_prop*norm_vis);
01005   }
01006   else{
01007     cout<<norm_prop<<" - "<<norm_vis<<endl;
01008     return -2.0;
01009   }
01010 
01011 #ifdef WITH_CORRECTION
01012   // computing correction term for minimizing the angle (and not the dot product)
01013   CMatrix3_t m_corr;
01014   v_mult(dpos,dpos,m2);
01015   f= 1.f/(norm_prop*norm_prop);
01016   m_rescale(f,m2,m1);
01017   m_identity(m2);
01018   m_sub(m2,m1,m2);
01019   f=1.f/norm_prop;
01020   m_rescale(f,m2,m_corr);
01021 #else
01022   m_identity(m_corr);
01023 #endif
01024 
01025 
01026 #else
01027   CVector3_t diff;
01028    v_sub(visual_disp,dpos,diff);
01029    ret_val = v_length(diff);
01030   if(isnan(ret_val)){return -2.0;}
01031 #endif
01032 
01033 
01034   // pre-computing products (could be put in the loop below)
01035   for(k=0;k<nb_joints;k++){
01036     joints[k]->Jacobian(jstack[k]);
01037     v_transform_normal2(v1[k+1],jstack[k],vv[k]);
01038   //   cout<<k<<" vv "<<vv[k]<<"  "<< v1[k+1]<<endl;
01039 //     cout<<jstack[k]<<endl;
01040   }
01041   for(i=0;i<nb_joints;i++){
01042     m_clear(m4);//accumulator for dT/dli
01043     m_clear(m7);//accumulator for dT/dai
01044     q_clear(q2);
01045     q_clear(q3);
01046     joints[i]->AxisDerivative(v1[i+1],m0);
01047 
01048     for(k=0;k<i;k++){
01049       q_multiply(q2,q3,q4); //q2 = joints[k-1]
01050       q_copy(q4,q3);//outer rotations      
01051       joints[k]->GetQuaternion(q2);
01052       q_inv(q2,q5);// joints[k]
01053       q_multiply(q6,q5,q4);
01054       q_copy(q4,q6);//inner rotations
01055       rot.SetQuaternion(q6);
01056       rot.RotToMatrix(m6);//inner rotations
01057       rot.SetQuaternion(q3);
01058       rot.RotToMatrix(m3);//outer rotations 
01059 
01060       // translation update accumulator      
01061       m_identity(m2);
01062       joints[k]->NormDerivative(m6+0,m2+0);
01063       joints[k]->NormDerivative(m6+4,m2+4);
01064       joints[k]->NormDerivative(m6+8,m2+8);
01065  
01066       m3_multiply(m3,m2,m1);
01067       f =angle_disp[k]*0.5*joints[k]->GetAlpha();//converting to d/dtheta 
01068       m_rescale(f,m1,m2);
01069       m_add(m4,m2,m4);
01070 
01071       //rotation update accumulator
01072       m3_multiply(m6,m0,m2);
01073       m3_multiply(jstack[k],m2,m1);
01074       m3_multiply(m3,m1,m2);
01075       m_add(m7,m2,m7);
01076      }
01077 
01078                        
01079     // k=i
01080     q_multiply(q2,q3,q4); //q2 = joints[i-1]
01081     q_copy(q4,q3);      
01082     rot.SetQuaternion(q3);
01083     rot.RotToMatrix(m3);//outer rotation
01084 
01085     joints[i]->AngleAxisDerivative(v1[i+1],m2);
01086     m_rescale(angle_disp[i],m2,m1);
01087     m_add(m7,m1,m7);
01088 
01089     q_clear(q6);
01090     joints[i]->GetQuaternion(q2);
01091     for(k=i+1;k<nb_joints;k++){
01092       q_multiply(q2,q6,q4); //q2 = joint[k-1]
01093       q_copy(q4,q6);
01094       joints[k]->GetQuaternion(q2);
01095       rot.SetQuaternion(q6);
01096       rot.Transform(vv[k],v2);
01097       joints[i]->AxisDerivative(v2,m1);
01098       m3_multiply(m3,m1,m2);
01099       m_rescale(angle_disp[k],m2,m1);
01100       m_add(m7,m1,m7);
01101       if(isnan(m1[0])){
01102           cout<<m1<<endl;
01103           cout<<m3<<endl;
01104           cout<<vv[k]<<endl;
01105           cout<<i<<" "<<k<<endl;
01106           return -2.0;
01107       }
01108     }
01109 #ifdef COSINE_MIN
01110     m3_multiply(m_corr,m7,m1);
01111     v_transform_normal2(visual_disp,m1,delta_rot[i]);
01112     m3_multiply(m_corr,m4,m1);
01113     v_transform_normal2(visual_disp,m1,delta_tr[i]);
01114 #else
01115      v_transform_normal2(diff,m7,delta_rot[i]);
01116      v_transform_normal2(diff,m4,delta_tr[i]);
01117      //     cout<<delta_rot[i]<<endl;
01118 #endif
01119     sum_rot += v_squ_length(delta_rot[i]);  
01120     sum_tr += v_squ_length(delta_tr[i]);   
01121   }
01122 
01123 
01124 
01125   if(sum_rot>epsilon){//not necessary (already done in AddToAxis);
01126     sum_rot = 1/sqrt(sum_rot);
01127   }
01128   if(sum_tr>epsilon){
01129     sum_tr = 30/sqrt(sum_tr);//was 50
01130     //   sum_tr =0;
01131   }
01132   for(i=0;i<nb_joints;i++){
01133     v_scale(delta_tr[i],sum_tr,delta_tr[i]);
01134     v_scale(delta_rot[i],sum_rot,delta_rot[i]);// not necessary (already done in AddToAxis);
01135     
01136     joints[i]->Translation::Add(delta_tr[i]); 
01137     joints[i]->Rotation::AddToAxis(delta_rot[i]);
01138   }
01139   if(v_squ_length(v1[0])>300){
01140     //    ScaleChain(300);
01141   }
01142   return ret_val;
01143 }
01144 
01145 #ifndef OLD_UPDATE
01146 float KinematicChain::Update(float *angle, CVector3_t vision, float threshold, CVector3_t local_pos){
01147   CVector3_t v1[MAX_LINKS], delta[MAX_LINKS], delta_tr[MAX_LINKS];
01148   CVector3_t v,v2;
01149   //  CQuat_t q; unused
01150   CQuat_t q1[MAX_LINKS];
01151   CMatrix3_t m2,m3;
01152   Rotation rot;
01153   int i;
01154   float sum=0.0f,sum_tr=0.0f,ret_val;
01155   
01156   SetAngles(angle);
01157   InitKinematicStack(local_pos,v2);
01158   ForwardKinematicsStack(v2,v1);
01159   v_sub(vision,v1[0],v); //(y-R(0))
01160   ret_val = v_length(v);//return value
01161   if(ret_val > threshold){
01162       return -ret_val;
01163   }
01164   // stop learning if ok
01165   //  if (ret_val<1){return ret_val;}
01166 
01167 
01168   // computing the composed of all rotations
01169   RotationStack(q1);
01170   
01171   // starting updating process
01172   for(i=0;i<nb_joints;i++){
01173     //    for(i=0;i<2;i++){
01174     if(!joints[i]->IsAdaptive()){
01175       v_clear(delta[i]);
01176       v_clear(delta_tr[i]);
01177     }
01178     else{
01179       if(reverse[i]==-1){
01180         rot.SetQuaternion(q1[i+1]);//including Ri in rotation
01181       }
01182       else{
01183         rot.SetQuaternion(q1[i]);
01184       }
01185        
01186       //         cout<<"rot "<<i<<" "<<rot<<endl;
01187       /*------------- updating translation -----------------*/
01188       m_identity(m2);
01189       // computing rotation matrix (should be equivalent to rot.RotToMatrix(m3))
01190       rot.Transform((m2+0),(m3+0));
01191       rot.Transform((m2+4),(m3+4));
01192       rot.Transform((m2+8),(m3+8));
01193       v_transform_normal2(v,m3,delta_tr[i]);
01194 
01195       //joints[i]->Translation::Add(delta_tr[i]);
01196       sum_tr+= v_squ_length(delta_tr[i]);
01197 
01198       /*-------------   updating rotation -----------------*/
01199       if(reverse[i]==-1){
01200         joints[i]->Translation::InverseTransform(v1[i+1],v2);//including transl. in transfo
01201         joints[i]->InverseAxisDerivative(v2,m2);
01202         rot.SetQuaternion(q1[i]); //exluding Ri from rotation
01203       }
01204       else{
01205         joints[i]->AxisDerivative(v1[i+1],m2);
01206       }
01207       rot.Transform((m2+0),(m3+0));
01208       rot.Transform((m2+4),(m3+4));
01209       rot.Transform((m2+8),(m3+8));
01210 
01211       v_transform_normal2(v,m3,delta[i]);
01212       //     cout<<"addedl "<<delta[i]<<endl;
01213       //      cout<<"mat"<<endl<<m2<<endl;
01214       //    joints[i]->Rotation::AddToAxis(delta[i]);
01215 
01216       sum+=v_squ_length(delta[i]);
01217 
01218       if(reverse[i] == -1){
01219         //       v_scale(delta[i],-1,delta[i]);
01220         v_scale(delta_tr[i],-1,delta_tr[i]);
01221       }
01222 
01223     }
01224   }
01225 #ifdef WITH_LAST_LINK
01226   // updating last link
01227   rot.SetQuaternion(q1[nb_joints]);
01228   //     rot.Transform((CVector3_t)(m2+0), (CVector3_t)(m3+0));
01229   m_identity(m2);
01230   rot.Transform((m2+0),(m3+0));
01231   rot.Transform((m2+4),(m3+4));
01232   rot.Transform((m2+8),(m3+8));
01233   v_transform_normal2(v,m3,delta_tr[nb_joints]);
01234   //  last_link->Add(delta_tr[nb_joints]);
01235   sum_tr+=v_squ_length(delta_tr[nb_joints]);
01236 #endif
01237   if(sum>epsilon){
01238     sum = 1/sqrt(sum);
01239     //       sum=0;
01240   }
01241   if(sum_tr>epsilon){
01242     sum_tr = 30/sqrt(sum_tr);//was 50
01243   }
01244   for(i=0;i<nb_joints;i++){
01245     v_scale(delta_tr[i],sum_tr,delta_tr[i]);
01246     v_scale(delta[i],sum,delta[i]);
01247     joints[i]->Translation::Add(delta_tr[i]);
01248     joints[i]->Rotation::AddToAxis(delta[i]);
01249     //      cout<<"added "<<delta[i]<<endl;     
01250   }
01251 #ifdef WITH_LAST_LINK
01252   v_scale(delta_tr[nb_joints],sum_tr,delta_tr[nb_joints]);
01253   last_link->Add(delta_tr[nb_joints]);
01254 #endif
01255   return ret_val;
01256 }
01257 
01258 #else
01259 float KinematicChain::Update(float *angle, CVector3_t vision, float threshold, CVector3_t local_pos){
01260   CVector3_t v1[MAX_LINKS], delta[MAX_LINKS], delta_tr[MAX_LINKS];
01261   CVector3_t v,v2;
01262   CQuat_t q;
01263   CQuat_t q1[MAX_LINKS];
01264   CMatrix3_t m2,m3;
01265   Rotation rot;
01266   int i,last;
01267   float sum=0.0f,sum_tr=0.0f,ret_val;
01268   
01269 
01270   // computing the composed of all rotations (should be replaced by RotationStack and adapted)
01271   v_clear(q1[0]);
01272   q_set(q1[0],0,q1[0]);
01273   for(i=0;i<nb_joints;i++){
01274     joints[i]->SetAngle(angle[i]);
01275     joints[i]->GetQuaternion(q);
01276     if(reverse[i]==-1){
01277       v_scale(q,-1,q); //inverting only the first 3 comp
01278     }
01279     q_multiply(q,q1[i],q1[i+1]);//q1[i] contains all rotations up to i-1
01280   }
01281 
01282   // forward kinematics (should be replaced by forward kinematics stack and adapted)
01283 #ifdef WITH_LAST_LINK
01284   last = nb_joints;
01285 #else
01286   last =nb_joints-1;
01287 #endif 
01288   if(local_pos==NULL){ // zero by default
01289     v_clear(v1[last]);
01290   }
01291   else{
01292     v_copy(local_pos,v1[last]);
01293   }
01294 #ifdef LAST_LINK
01295   last_link->Transform(v1[nb_joints],v1[nb_joints-1]);
01296 #endif
01297   for(i=nb_joints-1;i>0;i--){
01298 
01299     Transform(i,v1[i],angle[i],v1[i-1]);//v1[i] contains all transfo down to T(i+1)angles are supposed to be already set
01300     //    cout<<"v1 "<<i-1<<" "<<v1[i-1]<<endl;
01301   }
01302   Transform(0,v1[0],v);
01303   // cout<<*(joints[0])<<endl;
01304   //   cout<<"v "<<v<<endl;
01305 
01306   //difference between real and current transfo
01307   v_sub(vision,v,v); //(y-R(0)
01308   ret_val = v_length(v);//return value
01309   // stop learning if ok
01310   //  if (ret_val<1){return ret_val;}
01311   for(i=0;i<nb_joints;i++){
01312     //    for(i=0;i<2;i++){
01313     if(!joints[i]->IsAdaptive()){
01314       v_clear(delta[i]);
01315       v_clear(delta_tr[i]);
01316     }
01317     else{
01318       if(reverse[i]==-1){
01319         rot.SetQuaternion(q1[i+1]);//including Ri in rotation
01320       }
01321       else{
01322         rot.SetQuaternion(q1[i]);
01323       }
01324        
01325       //         cout<<"rot "<<i<<" "<<rot<<endl;
01326       /*------------- updating translation -----------------*/
01327       m_identity(m2);
01328       // computing rotation matrix (should be equivalent to rot.RotToMatrix(m3))
01329       rot.Transform((m2+0),(m3+0));
01330       rot.Transform((m2+4),(m3+4));
01331       rot.Transform((m2+8),(m3+8));
01332       v_transform_normal2(v,m3,delta_tr[i]);
01333 
01334       //joints[i]->Translation::Add(delta_tr[i]);
01335       sum_tr+= v_squ_length(delta_tr[i]);
01336 
01337       /*-------------   updating rotation -----------------*/
01338       if(reverse[i]==-1){
01339         joints[i]->Translation::InverseTransform(v1[i],v2);//including transl. in transfo
01340         joints[i]->InverseAxisDerivative(v2,m2);
01341         rot.SetQuaternion(q1[i]); //exluding Ri from rotation
01342       }
01343       else{
01344         joints[i]->AxisDerivative(v1[i],m2);
01345       }
01346       rot.Transform((m2+0),(m3+0));
01347       rot.Transform((m2+4),(m3+4));
01348       rot.Transform((m2+8),(m3+8));
01349 
01350       v_transform_normal2(v,m3,delta[i]);
01351       //     cout<<"addedl "<<delta[i]<<endl;
01352       //      cout<<"mat"<<endl<<m2<<endl;
01353       //    joints[i]->Rotation::AddToAxis(delta[i]);
01354 
01355       sum+=v_squ_length(delta[i]);
01356 
01357       if(reverse[i] == -1){
01358         //       v_scale(delta[i],-1,delta[i]);
01359         v_scale(delta_tr[i],-1,delta_tr[i]);
01360       }
01361 
01362     }
01363   }
01364 #ifdef WITH_LAST_LINK
01365   // updating last link
01366   rot.SetQuaternion(q1[nb_joints]);
01367   //     rot.Transform((CVector3_t)(m2+0), (CVector3_t)(m3+0));
01368   m_identity(m2);
01369   rot.Transform((m2+0),(m3+0));
01370   rot.Transform((m2+4),(m3+4));
01371   rot.Transform((m2+8),(m3+8));
01372   v_transform_normal2(v,m3,delta_tr[nb_joints]);
01373   //  last_link->Add(delta_tr[nb_joints]);
01374   sum_tr+=v_squ_length(delta_tr[nb_joints]);
01375 #endif
01376   if(sum>epsilon){
01377     sum = 1/sqrt(sum);
01378     //       sum=0;
01379   }
01380   if(sum_tr>epsilon){
01381     sum_tr = 30/sqrt(sum_tr);//was 50
01382   }
01383   for(i=0;i<nb_joints;i++){
01384     v_scale(delta_tr[i],sum_tr,delta_tr[i]);
01385     v_scale(delta[i],sum,delta[i]);
01386     joints[i]->Translation::Add(delta_tr[i]);
01387     joints[i]->Rotation::AddToAxis(delta[i]);
01388     //      cout<<"added "<<delta[i]<<endl;     
01389   }
01390 #ifdef WITH_LAST_LINK
01391   v_scale(delta_tr[nb_joints],sum_tr,delta_tr[nb_joints]);
01392   last_link->Add(delta_tr[nb_joints]);
01393 #endif
01394   return ret_val;
01395 }
01396 
01397 #endif
01398 
01409 float KinematicChain::UpdateTouch(float *angle, float k0, float k1){
01410 
01411   CVector3_t kinstack[MAX_LINKS], delta[MAX_LINKS], delta_tr[MAX_LINKS];
01412   CVector3_t v,v2,touch_pos0,touch_pos1;
01413   //  CQuat_t q;
01414   CQuat_t q1[MAX_LINKS];
01415   CMatrix3_t m2,m3;
01416   //  RigidTransfo **joints_bk=NULL;
01417   //   int * reverse_bk;
01418   Rotation rot;
01419   int i;
01420   float sum=0.0f,sum_tr=0.0f,ret_val;
01421   int shifted =0;
01422   joints[nb_joints-1]->GetTranslation(touch_pos1);
01423   v_scale(touch_pos1,k1,touch_pos1);
01424   joints[0]->GetTranslation(touch_pos0);
01425   v_scale(touch_pos0,k0,touch_pos0);
01426 
01427   SetAngles(angle);
01428   nb_joints--; //discarding the last rotation
01429   
01430    if(IsInverted(0)==-1){
01431      // joints_bk = joints;   
01432      //   joints = &(joints[1]); //discarding the first rotation (shifting)
01433      // reverse_bk = reverse;
01434      //  reverse = &(reverse[1]);  
01435      joints++;
01436      reverse++;
01437      nb_joints--;
01438      shifted=1;
01439   }
01440  
01441 
01442 
01443   // forward kinematics
01444   ForwardKinematicsStack(touch_pos1,kinstack);
01445  
01446   //difference between real and current transfo
01447   v_sub(touch_pos0,kinstack[0],v); //(y-R(0)
01448  
01449   ret_val = v_length(v);//return value
01450   // stop learning if ok
01451   if (ret_val<10){
01452   // computing the composed of all rotations
01453   RotationStack(q1);
01454   for(i=0;i<nb_joints;i++){
01455     //    for(i=0;i<2;i++){
01456     if(!joints[i]->IsAdaptive()){
01457       v_clear(delta[i]);
01458       v_clear(delta_tr[i]);
01459     }
01460     else{
01461       if(reverse[i]==-1){
01462         rot.SetQuaternion(q1[i+1]);//including Ri in rotation
01463       }
01464       else{
01465         rot.SetQuaternion(q1[i]);
01466       }
01467        
01468       /*------------- updating translation -----------------*/
01469   
01470       // computing rotation matrix (should be equivalent to rot.InverseTransform(v,delta_tr))
01471       rot.RotToMatrix(m3);
01472       v_transform_normal2(v,m3,delta_tr[i]);
01473     
01474       if(reverse[i] == -1){ // to be checked
01475         v_scale(delta_tr[i],-1,delta_tr[i]);
01476       }    
01477       sum_tr+= v_squ_length(delta_tr[i]);
01478 
01479       /*-------------   updating rotation -----------------*/
01480 
01481       if(reverse[i]==-1){
01482         joints[i]->Translation::InverseTransform(kinstack[i+1],v2);//including transl. in transfo
01483         joints[i]->InverseAxisDerivative(v2,m2);
01484         rot.SetQuaternion(q1[i]); //exluding Ri from rotation
01485       }
01486       else{
01487         joints[i]->AxisDerivative(kinstack[i+1],m2);
01488       }
01489 
01490       rot.Transform((m2+0),(m3+0));
01491       rot.Transform((m2+4),(m3+4));
01492       rot.Transform((m2+8),(m3+8));
01493       v_transform_normal2(v,m3,delta[i]);
01494 
01495       sum+=v_squ_length(delta[i]);
01496     }
01497   }
01498   if(sum>epsilon){
01499     sum = 1/sqrt(sum);
01500   }
01501   if(sum_tr>epsilon){
01502     sum_tr = 30/sqrt(sum_tr);//was 50
01503   }
01504   for(i=0;i<nb_joints;i++){
01505     v_scale(delta_tr[i],sum_tr,delta_tr[i]);
01506     v_scale(delta[i],sum,delta[i]);
01507     joints[i]->Translation::Add(delta_tr[i]);
01508     joints[i]->Rotation::AddToAxis(delta[i]);
01509   }
01510   }
01511   nb_joints++;
01512   if(shifted){ //shifting back
01513     // joints = joints_bk;
01514     //    reverse = reverse_bk;
01515     joints--;
01516     reverse--;
01517     nb_joints++;
01518   }
01519   return ret_val;
01520 }
01521 
01522 float KinematicChain::ScaleChain(float length){
01523   float f,len = 0;
01524   CVector3_t v;
01525   int i;
01526   for(i=0;i<nb_joints;i++){
01527     joints[i]->GetTranslation(v);
01528     len+= v_squ_length(v);
01529   }
01530   len = sqrtf(len);
01531   f = length/len;
01532   for(i=0;i<nb_joints;i++){
01533     joints[i]->Translation::Scale(f);
01534   }
01535   return len;
01536 }
01537 
01538 int KinematicChain::GetLink(int i, CVector3_t link){
01539 #ifdef WITH_LAST_LINK
01540   if(i==nb_joints){
01541     last_link->GetTranslation(link);
01542     return 1;
01543   }
01544 #endif
01545   if(i>=0 && i<nb_joints){
01546     joints[i]->GetTranslation(link);
01547     return 1;
01548   }
01549   return 0;
01550 }
01551 
01552 
01553 void KinematicChain::LinkRef(float *angle,int n,CVector3_t in,CVector3_t out){
01554   // to implement
01555 }
01556 
01557 
01558 void KinematicChain::PrintAxes(ostream& out){
01559   CVector3_t v;
01560   //    cout<<"printing"<<endl;
01561   // out<<endl;
01562   for(int i=0;i<nb_joints;i++){
01563     joints[i]->GetRotationAxis(v);
01564     out<<v<<" ";
01565   }
01566   out<<endl;
01567 }
01568 
01569 float *KinematicChain::Serialize(float *data, int offset){
01570   int j=0;
01571   int n = GetNbJoints();
01572   data[j++] = (float)(n-offset);//number of DOFS sent
01573   for(int i=offset;i<n;i++){
01574     v_copy(joints[i]->GetRotationAxis(),data+j);
01575     j+=3;
01576     v_copy(joints[i]->GetTranslation(),data+j); 
01577     j+=3;
01578     data[j++] = (float) reverse[i];
01579   }
01580 #ifdef WITH_LAST_LINK
01581   v_copy(last_link->GetTranslation(),data+j); 
01582 #endif
01583   return data;
01584 }
01585 
01590 int KinematicChain::Deserialize(const float *data, int offset){
01591   int j =0;
01592   int n = (int) data[j++];
01593   int last = n+offset;
01594   if(last>nb_joints){
01595     cout<<"warning not enough joints in Deserialize"<<endl;
01596     last = nb_joints;
01597   }
01598   for(int i=offset;i<last;i++){
01599     joints[i]->SetRotationAxis(data+j);
01600     j+=3;
01601     joints[i]->SetTranslation(data+j);
01602     j+=3;
01603     reverse[i] = sign(data[j]);
01604     j++;
01605   }
01606 #ifdef WITH_LAST_LINK
01607   last_link->SetTranslation(data+j);
01608 #endif
01609   return last-offset;
01610 }
01611 
01612 
01613 int KinematicChain::Deserialize(const float *data, int from, int to){
01614     int k,i=0;
01615     int n = (int) data[i];
01616     if(to>n-1){
01617         cout<<"warning: not enough data in Deserialize"<<endl;
01618         to = n-1;
01619     }    
01620     if(to-from+1>nb_joints){
01621         cout<<"warning: not enough joints in Deserialize"<<endl;
01622         to = nb_joints-1;
01623     }
01624     for(int j=from;j<=to;j++){
01625         k=j*7+1;
01626         joints[i]->SetRotationAxis(data+k);
01627         k+=3;
01628         joints[i]->SetTranslation(data+k);
01629         k+=3;
01630         reverse[i] = sign(data[k]);
01631         i++;
01632     }
01633 #ifdef WITH_LAST_LINK
01634     k=to*7+2;
01635     last_link->SetTranslation(data+k);
01636 #endif
01637   return to-from+1;
01638 }
01639 
01640 
01641 
01642 ostream& operator<<(ostream& out, const KinematicChain& kc)
01643 {
01644   int n = kc.GetNbJoints();
01645   for(int i=0;i<n;i++){
01646     out<<*(kc.GetTransfo(i));
01647 #ifdef KC_INVERTED
01648     out<<" "<<kc.IsInverted(i);
01649 #endif
01650     out<<" - ";
01651   }
01652 #ifdef WITH_LAST_LINK
01653   out<<*(kc.GetLastLink());
01654 #endif
01655   out<<endl;
01656   return out;
01657 }
01658 
01659 istream& operator>>(istream& in, KinematicChain& kc)
01660 {
01661   int n,m,i;
01662   RigidTransfo *j;
01663   CVector3_t v,w;
01664   char c1,c2;
01665   float angle;
01666   
01667   SkipComments(in);
01668   m=kc.GetNbJoints();
01669   in>>n; //reading the number of joints
01670   SkipComments(in);
01671   for(i=0;i<m;i++){ 
01672       //   in>>*(kc.GetTransfo(i));
01673       in>>angle>>c1>>v[0]>>v[1]>>v[2]>>c2>>w[0]>>w[1]>>w[2];
01674       kc.GetTransfo(i)->SetRotationAxis(v);
01675       kc.GetTransfo(i)->SetTranslation(w);
01676       kc.GetTransfo(i)->SetAngle(angle);
01677       SkipComments(in);
01678   }
01679   for(i=m;i<n;i++){ 
01680     j = new RigidTransfo();
01681     //    in>>(*j);
01682       in>>angle>>c1>>v[0]>>v[1]>>v[2]>>c2>>w[0]>>w[1]>>w[2];
01683       j->SetRotationAxis(v);
01684       j->SetTranslation(w);
01685       j->SetAngle(angle);
01686     kc.AddJoint(j,1);
01687     SkipComments(in);
01688   }
01689 #ifdef WITH_LAST_LINK
01690     in>>v[0]>>v[1]>>v[2];
01691     if(!in.fail() ){
01692       kc.SetLastLink(v);
01693     }
01694 #endif
01695   for(i=m;i>n;i--){ 
01696     kc.DeleteJoint();
01697   }
01698   return in;
01699 }
01700 
01701 
01702 
 All Data Structures Functions Variables

Generated on Wed Sep 22 16:51:26 2010 for Body_Schema_Learning by  doxygen 1.6.1