Tekkotsu Homepage
Demos
Overview
Downloads
Dev. Resources
Reference
Credits

Kinematics.cc

Go to the documentation of this file.
00001 #include "Kinematics.h"
00002 #include "Shared/Config.h"
00003 #include "roboop/robot.h"
00004 #include "roboop/config.h"
00005 #include "Shared/WorldState.h"
00006 #include <sstream>
00007 #include <float.h>
00008 #include <iostream>
00009 
00010 using namespace std;
00011 
00012 Kinematics * kine = NULL;
00013 ROBOOP::Config * Kinematics::roconfig = NULL;
00014 Kinematics::InterestPointMap * Kinematics::ips = NULL;
00015 std::vector< std::vector<unsigned int> > Kinematics::chainMaps;
00016 Kinematics::JointMap Kinematics::jointMaps[NumReferenceFrames];
00017 bool Kinematics::staticsInited=false;
00018 #ifndef THREADSAFE_KINEMATICS
00019 std::vector<ROBOOP::Robot*> Kinematics::chains;
00020 #endif
00021 
00022 void
00023 Kinematics::init() {
00024 #ifdef THREADSAFE_KINEMATICS
00025   unsigned int nchains=::config->motion.kinematic_chains.size();
00026   chains.resize(nchains);
00027   for(unsigned int i=0; i<nchains; i++) {
00028     chains[i]=newChain(i);
00029     //cout << "Created " << chains[i] << " for " << this << endl;
00030   }
00031 #endif
00032   checkStatics();
00033 }
00034 
00035 void Kinematics::initStatics() {
00036   if(!staticsInited) {
00037     unsigned int nchains=::config->motion.kinematic_chains.size();
00038     if(nchains==0) {
00039       serr->printf("ERROR Kinematics::init(): no kinematic chains were selected\n");
00040       return;
00041     }
00042     jointMaps[BaseFrameOffset]=JointMap(0,0);
00043     
00044 #ifndef THREADSAFE_KINEMATICS
00045     chains.resize(nchains);
00046     for(unsigned int i=0; i<nchains; i++) {
00047       chains[i]=newChain(i);
00048       //cout << "Created " << chains[i] << " for " << this << endl;
00049     }
00050 #endif
00051     
00052     if(ips==NULL) {
00053       int numIP=0;
00054       roconfig->select_int("InterestPoints","Length",numIP);
00055       ips=new InterestPointMap(numIP);
00056       pair<string,InterestPoint> ip;
00057       string chain;
00058       for(int i=1; i<=numIP; i++) {
00059         char section[100];
00060         snprintf(section,100,"InterestPoint%d",i);
00061         roconfig->select_float(section,"x",ip.second.x);
00062         roconfig->select_float(section,"y",ip.second.y);
00063         roconfig->select_float(section,"z",ip.second.z);
00064         roconfig->select_string(section,"chain",chain);
00065         roconfig->select_int(section,"link",ip.second.link);
00066         roconfig->select_string(section,"name",ip.first);
00067         ip.first.erase(ip.first.find_last_not_of('~')+1); // strip old-style '~' padding at end of output names
00068         for(ip.second.chain=0; ip.second.chain<(::config->motion.kinematic_chains.size()); ip.second.chain++)
00069           if(::config->motion.kinematic_chains[ip.second.chain]==chain)
00070             break;
00071         if(ip.second.chain==::config->motion.kinematic_chains.size())
00072           serr->printf("WARNING: Chain %s is not recognized for interest point %d\n",chain.c_str(),i);
00073         else
00074           ips->insert(ip);
00075       }
00076     }
00077     staticsInited=true;
00078 #ifndef THREADSAFE_KINEMATICS
00079     delete roconfig; // static chains are initialized, don't need to keep this around...
00080 #endif
00081   }
00082   /*cout << "Joint Maps" << endl;
00083   for(unsigned int i=0; i<NumOutputs; i++)
00084     cout << outputNames[i] << ": " << jointMaps[i].chain << ' ' << jointMaps[i].link << endl;
00085   for(unsigned int i=NumOutputs; i<NumReferenceFrames; i++)
00086     cout << i << ": " << jointMaps[i].chain << ' ' << jointMaps[i].link << endl;
00087   cout << "Chain Maps" << endl;
00088   for(unsigned int i=0; i<chains.size(); i++)
00089     for(unsigned int j=1; j<chainMaps[i].size(); j++)
00090     cout << "chainMaps["<<i<<"]["<<j<<"] == " << chainMaps[i][j] << endl;*/
00091 }
00092 
00093 ROBOOP::Robot* Kinematics::newChain(unsigned int chainIdx) {
00094   if(roconfig==NULL) {
00095     unsigned int nchains=::config->motion.kinematic_chains.size();
00096     chainMaps.resize(nchains);
00097     roconfig=new ROBOOP::Config(::config->motion.makePath(::config->motion.kinematics),true);
00098     for(unsigned int i=0; i<nchains; i++) {
00099       string section=::config->motion.kinematic_chains[i];
00100       int dof=0;
00101       if(roconfig->select_int(section,"dof",dof)!=0) {
00102         serr->printf("ERROR Kinematics::init(): unable to find 'dof' in chain %d (%s)\n",i,section.c_str());
00103         chainMaps[i].resize(0);
00104         continue;
00105       }
00106       chainMaps[i].resize(dof+1);
00107       for(int l=0; l<=dof; l++)
00108         chainMaps[i][l]=-1U;
00109       for(int l=1; l<=dof; l++) {
00110         ostringstream ostr;
00111         ostr << section << "_LINK" << l;
00112         string link = ostr.str();
00113         if(roconfig->parameter_exists(link,"tekkotsu_output")) {
00114           int tkout=-1U;
00115           roconfig->select_int(link,"tekkotsu_output",tkout);
00116           if((unsigned int)tkout>=NumOutputs) {
00117             serr->printf("WARNING Kinematics::init(): invalid tekkotsu_output %d on chain %d (%s), link %d (%s)\n",tkout,i,section.c_str(),l,link.c_str());
00118           } else {
00119             jointMaps[tkout]=JointMap(i,l);
00120             chainMaps[i][l]=tkout;
00121           }
00122         }
00123         if(roconfig->parameter_exists(link,"tekkotsu_frame")) {
00124           int tkout=-1U;
00125           roconfig->select_int(link,"tekkotsu_frame",tkout);
00126           tkout+=NumOutputs;
00127           if((unsigned int)tkout>=NumReferenceFrames)
00128             serr->printf("WARNING Kinematics::init(): invalid tekkotsu_frame %d on chain %d (%s), link %d (%s)\n",tkout,i,section.c_str(),l,link.c_str());
00129           else {
00130             jointMaps[tkout]=JointMap(i,l);
00131             chainMaps[i][l]=tkout;
00132           }
00133         }
00134       }
00135     }
00136   }
00137   if(chainMaps[chainIdx].size()==0)
00138     return NULL;
00139   return new ROBOOP::Robot(*roconfig,::config->motion.kinematic_chains[chainIdx]);
00140 }
00141 
00142 Kinematics::~Kinematics() {
00143 #ifdef THREADSAFE_KINEMATICS
00144   for(unsigned int i=0; i<chains.size(); i++) {
00145     if(chains[i]==NULL)
00146       cerr << "Warning: Kinematics chains appear to already be deleted! (multiple free?)" << endl;
00147     delete chains[i];
00148     chains[i]=NULL;
00149   }
00150 #endif
00151 }
00152 
00153 NEWMAT::ReturnMatrix
00154 Kinematics::jointToBase(unsigned int j) {
00155 #ifndef THREADSAFE_KINEMATICS
00156   checkStatics();
00157 #endif
00158   unsigned int c=-1U,l=-1U;
00159   if(!lookup(j,c,l)) {
00160     NEWMAT::Matrix A(4,4);
00161     A<<ROBOOP::fourbyfourident;
00162     A.Release(); return A;
00163   }
00164   update(c,l);
00165   return chains[c]->convertFrame(l,0);
00166 }
00167 
00168 NEWMAT::ReturnMatrix
00169 Kinematics::linkToBase(unsigned int j) {
00170 #ifndef THREADSAFE_KINEMATICS
00171   checkStatics();
00172 #endif
00173   unsigned int c=-1U,l=-1U;
00174   if(!lookup(j,c,l)) {
00175     NEWMAT::Matrix A(4,4);
00176     A<<ROBOOP::fourbyfourident;
00177     A.Release(); return A;
00178   }
00179   update(c,l);
00180   return chains[c]->convertLink(l,0);
00181 }
00182 
00183 NEWMAT::ReturnMatrix
00184 Kinematics::baseToJoint(unsigned int j) {
00185 #ifndef THREADSAFE_KINEMATICS
00186   checkStatics();
00187 #endif
00188   unsigned int c=-1U,l=-1U;
00189   if(!lookup(j,c,l)) {
00190     NEWMAT::Matrix A(4,4);
00191     A<<ROBOOP::fourbyfourident;
00192     A.Release(); return A;
00193   }
00194   update(c,l);
00195   return chains[c]->convertFrame(0,l);
00196 }
00197 
00198 NEWMAT::ReturnMatrix
00199 Kinematics::baseToLink(unsigned int j) {
00200 #ifndef THREADSAFE_KINEMATICS
00201   checkStatics();
00202 #endif
00203   unsigned int c=-1U,l=-1U;
00204   if(!lookup(j,c,l)) {
00205     NEWMAT::Matrix A(4,4);
00206     A<<ROBOOP::fourbyfourident;
00207     A.Release(); return A;
00208   }
00209   update(c,l);
00210   return chains[c]->convertLink(0,l);
00211 }
00212 
00213 NEWMAT::ReturnMatrix
00214 Kinematics::jointToJoint(unsigned int ij, unsigned int oj) {
00215 #ifndef THREADSAFE_KINEMATICS
00216   checkStatics();
00217 #endif
00218   unsigned int ci=-1U,li=-1U,co=-1U,lo=-1U;
00219   if(ij==oj || !lookup(ij,ci,li) || !lookup(oj,co,lo)) {
00220     NEWMAT::Matrix A(4,4);
00221     A<<ROBOOP::fourbyfourident;
00222     A.Release(); return A;
00223   }
00224   if(ci==co) {
00225     update(ci,li>lo?li:lo);
00226     return chains[ci]->convertFrame(li,lo);
00227   } else if(li==0) {
00228     update(co,lo);
00229     return chains[co]->convertFrame(0,lo);
00230   } else if(lo==0) {
00231     update(ci,li);
00232     return chains[ci]->convertFrame(li,0);
00233   } else {
00234     update(ci,li);
00235     update(co,lo);
00236     NEWMAT::Matrix ans=chains[co]->convertFrame(0,lo)*chains[ci]->convertFrame(li,0);
00237     ans.Release(); return ans;
00238   }
00239 }
00240 
00241 NEWMAT::ReturnMatrix
00242 Kinematics::jointToLink(unsigned int ij, unsigned int oj) {
00243 #ifndef THREADSAFE_KINEMATICS
00244   checkStatics();
00245 #endif
00246   unsigned int ci=-1U,li=-1U,co=-1U,lo=-1U;
00247   if(!lookup(ij,ci,li) || !lookup(oj,co,lo)) {
00248     NEWMAT::Matrix A(4,4);
00249     A<<ROBOOP::fourbyfourident;
00250     A.Release(); return A;
00251   }
00252   if(ci==co) {
00253     update(ci,li>lo?li:lo);
00254     return chains[ci]->convertFrameToLink(li,lo);
00255   } else if(li==0) {
00256     update(co,lo);
00257     return chains[co]->convertFrameToLink(0,lo);
00258   } else if(lo==0) {
00259     update(ci,li);
00260     return chains[ci]->convertFrameToLink(li,0);
00261   } else {
00262     update(ci,li);
00263     update(co,lo);
00264     NEWMAT::Matrix ans=chains[co]->convertLink(0,lo)*chains[ci]->convertFrame(li,0);
00265     ans.Release(); return ans;
00266   }
00267 }
00268 
00269 NEWMAT::ReturnMatrix
00270 Kinematics::linkToJoint(unsigned int ij, unsigned int oj) {
00271 #ifndef THREADSAFE_KINEMATICS
00272   checkStatics();
00273 #endif
00274   unsigned int ci=-1U,li=-1U,co=-1U,lo=-1U;
00275   if(!lookup(ij,ci,li) || !lookup(oj,co,lo)) {
00276     NEWMAT::Matrix A(4,4);
00277     A<<ROBOOP::fourbyfourident;
00278     A.Release(); return A;
00279   }
00280   if(ci==co) {
00281     update(ci,li>lo?li:lo);
00282     return chains[ci]->convertLinkToFrame(li,lo);
00283   } else if(li==0) {
00284     update(co,lo);
00285     return chains[co]->convertLinkToFrame(0,lo);
00286   } else if(lo==0) {
00287     update(ci,li);
00288     return chains[ci]->convertLinkToFrame(li,0);
00289   } else {
00290     update(ci,li);
00291     update(co,lo);
00292     NEWMAT::Matrix ans=chains[co]->convertFrame(0,lo)*chains[ci]->convertLink(li,0);
00293     ans.Release(); return ans;
00294   }
00295 }
00296 
00297 NEWMAT::ReturnMatrix
00298 Kinematics::linkToLink(unsigned int ij, unsigned int oj) {
00299 #ifndef THREADSAFE_KINEMATICS
00300   checkStatics();
00301 #endif
00302   unsigned int ci=-1U,li=-1U,co=-1U,lo=-1U;
00303   if(ij==oj || !lookup(ij,ci,li) || !lookup(oj,co,lo)) {
00304     NEWMAT::Matrix A(4,4);
00305     A<<ROBOOP::fourbyfourident;
00306     A.Release(); return A;
00307   }
00308   if(ci==co) {
00309     update(ci,li>lo?li:lo);
00310     return chains[ci]->convertLink(li,lo);
00311   } else if(li==0) {
00312     update(co,lo);
00313     return chains[co]->convertLink(0,lo);
00314   } else if(lo==0) {
00315     update(ci,li);
00316     return chains[ci]->convertLink(li,0);
00317   } else {
00318     update(ci,li);
00319     update(co,lo);
00320     NEWMAT::Matrix ans=chains[co]->convertLink(0,lo)*chains[ci]->convertLink(li,0);
00321     ans.Release(); return ans;
00322   }
00323 }
00324 
00325 void
00326 Kinematics::getInterestPoint(const std::string& name, unsigned int& j, NEWMAT::Matrix& ip) {
00327 #ifndef THREADSAFE_KINEMATICS
00328   checkStatics();
00329 #endif
00330   unsigned int c=-1U,l=-1U;
00331   getInterestPoint(name,c,l,ip);
00332   j=chainMaps[c][l];
00333 }
00334 
00335 void
00336 Kinematics::getInterestPoint(const std::string& name, unsigned int& c, unsigned int& l, NEWMAT::Matrix& ip) {
00337 #ifndef THREADSAFE_KINEMATICS
00338   checkStatics();
00339 #endif
00340   InterestPointMap::iterator it=ips->find(name);
00341   ip=NEWMAT::ColumnVector(4);
00342   if(it==ips->end()) {
00343     serr->printf("ERROR: '%s' is not a recognized interest point\n",name.c_str());
00344     ip=0;
00345     c=l=-1U;
00346   }
00347   c=(*it).second.chain;
00348   l=(*it).second.link;
00349   ip=pack((*it).second.x,(*it).second.y,(*it).second.z);
00350   //cout << ci << ' ' << li << ' ' << co << ' ' << lo << endl;
00351 }
00352 
00353 NEWMAT::ReturnMatrix
00354 Kinematics::getJointInterestPoint(unsigned int joint, const std::string& name) {
00355 #ifndef THREADSAFE_KINEMATICS
00356   checkStatics();
00357 #endif
00358   NEWMAT::ColumnVector ans(4);
00359   ans=0;
00360   unsigned int co=-1U,lo=-1U;
00361   if(!lookup(joint,co,lo)) {
00362     ans.Release(); return ans;
00363   }
00364   for(std::string::size_type pos=0,len=0; pos<name.size(); pos+=len+1) {
00365     len=name.find(',',pos);
00366     if(len==string::npos)
00367       len=name.size();
00368     len-=pos;
00369     unsigned int ci=-1U,li=-1U;
00370     NEWMAT::ColumnVector ip;
00371     getInterestPoint(name.substr(pos,len),ci,li,ip);
00372     if(ci==-1U) {
00373       ip.Release(); return ip;
00374     }
00375     if(ci==co) {
00376       update(ci,li>lo?li:lo);
00377       ans+=chains[ci]->convertLinkToFrame(li,lo)*ip;
00378     } else if(li==0) {
00379       update(co,lo);
00380       ans+=chains[co]->convertLinkToFrame(0,lo)*ip;
00381     } else if(lo==0) {
00382       update(ci,li);
00383       ans+=chains[ci]->convertLinkToFrame(li,0)*ip;
00384     } else {
00385       update(ci,li);
00386       update(co,lo);
00387       ans+=chains[co]->convertFrame(0,lo)*chains[ci]->convertLink(li,0)*ip;
00388     }
00389   }
00390   ans/=ans(4); //not strictly necessary, but may save some people headaches
00391   ans.Release(); return ans;
00392 }
00393 
00394 #ifdef TGT_HAS_LEGS
00395 void
00396 Kinematics::calcLegHeights(const NEWMAT::ColumnVector& down, float heights[NumLegs]) {
00397 #ifndef THREADSAFE_KINEMATICS
00398   checkStatics();
00399 #endif
00400   //initialize to the height of the ball of the foot
00401   for(unsigned int i=0; i<NumLegs; i++) {
00402     NEWMAT::ColumnVector ip_b=jointToBase(PawFrameOffset+i).SubMatrix(1,3,4,4);
00403     float h=-ip_b(1)*down(1) - ip_b(2)*down(2) - ip_b(3)*down(3);
00404     h-=BallOfFootRadius; //add the ball's radius
00405     heights[i]=h;
00406   }
00407   //see if any interest points are lower
00408   for(InterestPointMap::const_iterator it=ips->begin(); it!=ips->end(); ++it) {
00409     unsigned int c=(*it).second.chain;
00410     LegOrder_t leg;
00411     if(config->motion.kinematic_chains[c]=="LFr")
00412       leg=LFrLegOrder;
00413     else if(config->motion.kinematic_chains[c]=="RFr")
00414       leg=RFrLegOrder;
00415     else if(config->motion.kinematic_chains[c]=="LBk")
00416       leg=LBkLegOrder;
00417     else if(config->motion.kinematic_chains[c]=="RBk")
00418       leg=RBkLegOrder;
00419     else
00420       continue;
00421     unsigned int l=(*it).second.link;
00422     NEWMAT::Matrix ip=pack((*it).second.x,(*it).second.y,(*it).second.z);
00423     update(c,l);
00424     NEWMAT::ColumnVector ip_b=chains[c]->convertLinkToFrame(l,0)*ip;
00425     float h=-ip_b(1)*down(1) - ip_b(2)*down(2) - ip_b(3)*down(3);
00426     if(h<heights[leg])
00427       heights[leg]=h;
00428   }
00429 }
00430 
00431 LegOrder_t
00432 Kinematics::findUnusedLeg(const NEWMAT::ColumnVector& down) {
00433 #ifndef THREADSAFE_KINEMATICS
00434   checkStatics();
00435 #endif
00436   float heights[NumLegs];
00437   calcLegHeights(down,heights);
00438   //Find the highest foot
00439   unsigned int highleg=0;
00440   for(unsigned int i=1; i<NumLegs; i++)
00441     if(heights[i]>heights[highleg])
00442       highleg=i;
00443   //cout << "High: " << highleg << endl;
00444   return static_cast<LegOrder_t>(highleg);
00445 }
00446 
00447 NEWMAT::ReturnMatrix
00448 Kinematics::calculateGroundPlane() {
00449   NEWMAT::ColumnVector down=pack(state->sensors[BAccelOffset],-state->sensors[LAccelOffset],state->sensors[DAccelOffset]);
00450   if(down.SumSquare()<1.01) //1 because homogenous coord is 1
00451     down=pack(0,0,-1); //default to a down vector if sensors don't give a significant indication of gravity
00452   return calculateGroundPlane(down);
00453 }
00454 
00455 // TODO comment out name tracking (and this following #include) once we're sure it's working
00456 #include "PostureEngine.h"
00457 NEWMAT::ReturnMatrix
00458 Kinematics::calculateGroundPlane(const NEWMAT::ColumnVector& down) {
00459 #ifndef THREADSAFE_KINEMATICS
00460   checkStatics();
00461 #endif
00462   NEWMAT::Matrix lowip(3,3); //3 points define a plane
00463   float heights[3];
00464   unsigned int legs[3];
00465   std::string names[3];
00466   //initialize to max float
00467   for(unsigned int i=0; i<3; i++) {
00468     heights[i]=FLT_MAX;
00469     legs[i]=-1U;
00470   }
00471   //Check the balls of the foot
00472   for(unsigned int i=0; i<NumLegs; i++) {
00473     NEWMAT::ColumnVector ip_b=jointToBase(PawFrameOffset+i).SubMatrix(1,3,4,4);
00474     float h=-ip_b(1)*down(1) - ip_b(2)*down(2) - ip_b(3)*down(3);
00475     h-=BallOfFootRadius; //add the ball's radius
00476     if(h<heights[0]) {
00477       if(h<heights[1]) {
00478         heights[0]=heights[1];
00479         lowip.SubMatrix(1,3,1,1)=lowip.SubMatrix(1,3,2,2);
00480         legs[0]=legs[1];
00481         names[0]=names[1];
00482         if(h<heights[2]) {
00483           heights[1]=heights[2];
00484           lowip.SubMatrix(1,3,2,2)=lowip.SubMatrix(1,3,3,3);
00485           legs[1]=legs[2];
00486           names[1]=names[2];
00487           
00488           heights[2]=h;
00489           lowip.SubMatrix(1,3,3,3)=ip_b;
00490           legs[2]=i;
00491           names[2]="paw"; names[2]+=(char)('0'+i);
00492         } else {
00493           heights[1]=h;
00494           lowip.SubMatrix(1,3,2,2)=ip_b;
00495           legs[1]=i;
00496           names[1]="paw"; names[1]+=(char)('0'+i);
00497         }
00498       } else {
00499         heights[0]=h;
00500         lowip.SubMatrix(1,3,1,1)=ip_b;
00501         legs[0]=i;
00502         names[0]="paw"; names[0]+=(char)('0'+i);
00503       }
00504     }
00505   }
00506   //cout << "Ground plane initial: " << names[0] <<" ("<<heights[0]<<") " << names[1] << " ("<<heights[1]<<") " << names[2] << " ("<<heights[2]<<")"<< endl;
00507   
00508   //now check interest points
00509   for(InterestPointMap::const_iterator it=ips->begin(); it!=ips->end(); ++it) {
00510     if((*it).first.substr(0,3)=="Toe")
00511       continue;
00512     unsigned int c=(*it).second.chain;
00513     unsigned int l=(*it).second.link;
00514     NEWMAT::Matrix ip=pack((*it).second.x,(*it).second.y,(*it).second.z);
00515     update(c,l);
00516     NEWMAT::ColumnVector ip_b=chains[c]->convertLinkToFrame(l,0)*ip;
00517     float h=-ip_b(1)*down(1) - ip_b(2)*down(2) - ip_b(3)*down(3);
00518     if(h<heights[0]) {
00519       unsigned int leg;
00520       if(config->motion.kinematic_chains[c]=="LFr")
00521         leg=LFrLegOrder;
00522       else if(config->motion.kinematic_chains[c]=="RFr")
00523         leg=RFrLegOrder;
00524       else if(config->motion.kinematic_chains[c]=="LBk")
00525         leg=LBkLegOrder;
00526       else if(config->motion.kinematic_chains[c]=="RBk")
00527         leg=RBkLegOrder;
00528       else
00529         leg=-1U;
00530       
00531       if(h<heights[1]) {
00532         if(h<heights[2]) {
00533           if(leg==-1U || legs[1]!=leg && legs[2]!=leg) {
00534             heights[0]=heights[1];
00535             lowip.SubMatrix(1,3,1,1)=lowip.SubMatrix(1,3,2,2);
00536             legs[0]=legs[1];
00537             names[0]=names[1];
00538           }
00539           if(leg==-1U || legs[2]!=leg) {
00540             heights[1]=heights[2];
00541             lowip.SubMatrix(1,3,2,2)=lowip.SubMatrix(1,3,3,3);
00542             if(legs[2]!=leg)
00543               legs[1]=legs[2];
00544             names[1]=names[2];
00545           }
00546           
00547           heights[2]=h;
00548           lowip.SubMatrix(1,3,3,3)=ip_b.SubMatrix(1,3,1,1);
00549           legs[2]=leg;
00550           names[2]=(*it).first;
00551         } else {
00552           if(leg!=-1U && legs[2]==leg)
00553             continue;
00554           if(leg==-1U || legs[1]!=leg) {
00555             heights[0]=heights[1];
00556             lowip.SubMatrix(1,3,1,1)=lowip.SubMatrix(1,3,2,2);
00557             legs[0]=legs[1];
00558             names[0]=names[1];
00559           }
00560           heights[1]=h;
00561           lowip.SubMatrix(1,3,2,2)=ip_b.SubMatrix(1,3,1,1);
00562           legs[1]=leg;
00563           names[1]=(*it).first;
00564         }
00565       } else {
00566         if(leg!=-1U && (legs[1]==leg || legs[2]==leg))
00567           continue;
00568         heights[0]=h;
00569         lowip.SubMatrix(1,3,1,1)=ip_b.SubMatrix(1,3,1,1);
00570         legs[0]=leg;
00571         names[0]=(*it).first;
00572       }
00573     }
00574   }
00575   
00576   //Fit a plane to the remaining 3 feet
00577   NEWMAT::ColumnVector ones(3); ones=1;
00578   NEWMAT::ColumnVector p(4);
00579   try {
00580     p.SubMatrix(1,3,1,1) = lowip.t().i()*ones;
00581   } catch(...) {
00582     std::cout << "Exception during ground plane processing, saving current posture..." << std::flush;
00583     if(PostureEngine * tpose=dynamic_cast<PostureEngine*>(this)) {
00584       tpose->setSaveFormat(true,state);
00585       tpose->saveFile("/error.pos");
00586     } else {
00587       PostureEngine pose;
00588       pose.takeSnapshot();
00589       pose.setWeights(1);
00590       pose.setSaveFormat(true,state);
00591       pose.saveFile("/error.pos");
00592     }
00593     std::cout << "Wrote current sensor info \"error.pos\" on memstick" << std::endl;
00594     cout << "Ground plane was using " << names[0] <<" ("<<heights[0]<<") " << names[1] << " ("<<heights[1]<<") " << names[2] << " ("<<heights[2]<<")"<< endl;
00595     cout << lowip;
00596     throw;
00597   }
00598   p(4)=1;
00599   p.Release(); return p;
00600 }
00601 
00602 #else // NO LEGS -- constant ground plane
00603 
00604 NEWMAT::ReturnMatrix
00605 Kinematics::calculateGroundPlane() {
00606   return calculateGroundPlane(pack(0,0,-1));
00607 }
00608 
00609 NEWMAT::ReturnMatrix
00610 Kinematics::calculateGroundPlane(const NEWMAT::ColumnVector& /*down*/) {
00611   return pack(0,0,1,0); // flat x-y plane through the origin
00612 }
00613 
00614 #endif
00615 
00616 NEWMAT::ReturnMatrix
00617 Kinematics::projectToPlane(unsigned int j, const NEWMAT::ColumnVector& r_j,
00618                            unsigned int b, const NEWMAT::ColumnVector& p_b,
00619                            unsigned int f)
00620 {
00621   /*! Mathematical implementation:
00622    *  
00623    *  Need to convert @a p_b to @a p_j
00624    *  
00625    *  Once we have the transformation Tb_j from b to j, we need:\n
00626    *    T2=inv(Tb_j)'; T2(3,1:end-1)=-T2(3,1:end-1);\n
00627    *  but since we know a few things about the structure of T,
00628    *  we don't have to explicitly calculate that inverse. */
00629   NEWMAT::Matrix T2=linkToLink(b,j);
00630   //cout << "Transform b->j:\n"<<T2;
00631   T2.SubMatrix(4,4,1,3)=T2.SubMatrix(1,3,4,4).t()*T2.SubMatrix(1,3,1,3);
00632   T2.SubMatrix(1,3,4,4)=0;
00633   //cout << "Transform plane b->j:\n"<<T2;
00634   NEWMAT::ColumnVector p_j=T2*p_b;
00635   //cout << "p_j:\n"<<p_j.t();
00636 
00637   
00638   /*! After we obtain @a p_j, we can find the point of intersection of
00639    *  @a r_j and @a p_j using:
00640    *  @f[ \frac{p_d}{p_{xyz} \cdot r}r @f]
00641    *  Where @f$p_{xyz}@f$ is the first three elemnts of @a p_j, and
00642    *  @f$p_d@f$ is the fourth (hopefully last) element of p_j.
00643    *
00644    *  Of course, if @f$p_{xyz} \cdot r@f$ is 0, then r and p are parallel
00645    *  (since @a p_j is the normal of the plane, so a line perpendicular to
00646    *  the normal is parallel to the plane), so we set the resulting
00647    *  homogeneous coordinates accordingly to represent an interesection at
00648    *  infinity. */
00649 
00650   float denom=0;
00651   for(unsigned int i=1; i<=3; i++)
00652     denom+=r_j(i)*p_j(i);
00653   NEWMAT::ColumnVector intersect=r_j;
00654   if(denom==0)
00655     intersect(4)=0;
00656   else {
00657     float s=p_j(4)/denom;
00658     for(unsigned int i=1; i<=3; i++)
00659       intersect(i)*=s;
00660     intersect(4)=1;
00661   }
00662   //cout << "Intersect_j:\n"<<intersect.t();
00663   NEWMAT::Matrix ans=linkToLink(j,f)*intersect;
00664   ans.Release(); return ans;
00665 }
00666 
00667 void
00668 Kinematics::update(unsigned int c, unsigned int l) {
00669   for(unsigned int j=1; j<=l; j++) {
00670     unsigned int tkout=chainMaps[c][j];
00671     if(tkout<NumOutputs)
00672       chains[c]->set_q(state->outputs[tkout],j);
00673   }
00674 }
00675 
00676 
00677 /*! @file
00678  * @brief 
00679  * @author ejt (Creator)
00680  *
00681  * $Author: ejt $
00682  * $Name: tekkotsu-4_0 $
00683  * $Revision: 1.37 $
00684  * $State: Exp $
00685  * $Date: 2007/09/26 23:12:05 $
00686  */
00687 

Tekkotsu v4.0
Generated Thu Nov 22 00:54:53 2007 by Doxygen 1.5.4