Homepage Demos Overview Downloads Tutorials 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 
00008 using namespace std;
00009 
00010 Kinematics * kine = NULL;
00011 ROBOOP::Config * Kinematics::roconfig = NULL;
00012 Kinematics::InterestPointMap * Kinematics::ips = NULL;
00013 
00014 void
00015 Kinematics::init() {
00016   unsigned int nchains=::config->motion.kinematic_chains.size();
00017   if(nchains==0) {
00018     serr->printf("ERROR Kinematics::init(): no kinematic chains were selected\n");
00019     return;
00020   }
00021   jointMaps[BaseFrameOffset]=JointMap(0,0);
00022   chains.resize(nchains);
00023   chainMaps.resize(nchains);
00024   if(roconfig==NULL)
00025     roconfig=new ROBOOP::Config(::config->motion.makePath(::config->motion.kinematics),true);
00026   for(unsigned int i=0; i<nchains; i++) {
00027     string section=::config->motion.kinematic_chains[i];
00028     chains[i]=new ROBOOP::Robot(*roconfig,section);
00029     int dof=0;
00030     if(roconfig->select_int(section,"dof",dof)!=0) {
00031       serr->printf("ERROR Kinematics::init(): unable to find 'dof' in chain %d (%s)\n",i,section.c_str());
00032       return;
00033     }
00034     chainMaps[i].resize(dof+1);
00035     for(int l=0; l<=dof; l++)
00036       chainMaps[i][l]=-1U;
00037     for(int l=1; l<=dof; l++) {
00038       ostringstream ostr;
00039       ostr << section << "_LINK" << l;
00040       string link = ostr.str();
00041       if(roconfig->parameter_exists(link,"tekkotsu_output")) {
00042         int tkout=-1U;
00043         roconfig->select_int(link,"tekkotsu_output",tkout);
00044         if((unsigned int)tkout>=NumOutputs) {
00045           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());
00046         } else {
00047           jointMaps[tkout]=JointMap(i,l);
00048           chainMaps[i][l]=tkout;
00049         }
00050       }
00051       if(roconfig->parameter_exists(link,"tekkotsu_frame")) {
00052         int tkout=-1U;
00053         roconfig->select_int(link,"tekkotsu_frame",tkout);
00054         tkout+=NumOutputs;
00055         if((unsigned int)tkout>=NumReferenceFrames)
00056           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());
00057         else
00058           jointMaps[tkout]=JointMap(i,l);
00059       }
00060     }
00061   }
00062 
00063   if(ips==NULL) {
00064     int numIP=0;
00065     roconfig->select_int("InterestPoints","Length",numIP);
00066     ips=new InterestPointMap(numIP);
00067     pair<string,InterestPoint> ip;
00068     string chain;
00069     for(int i=1; i<=numIP; i++) {
00070       char section[100];
00071       snprintf(section,100,"InterestPoint%d",i);
00072       roconfig->select_float(section,"x",ip.second.x);
00073       roconfig->select_float(section,"y",ip.second.y);
00074       roconfig->select_float(section,"z",ip.second.z);
00075       roconfig->select_string(section,"chain",chain);
00076       roconfig->select_int(section,"link",ip.second.link);
00077       roconfig->select_string(section,"name",ip.first);
00078       for(ip.second.chain=0; ip.second.chain<(::config->motion.kinematic_chains.size()); ip.second.chain++)
00079         if(::config->motion.kinematic_chains[ip.second.chain]==chain)
00080           break;
00081       if(ip.second.chain==::config->motion.kinematic_chains.size())
00082         serr->printf("WARNING: Chain %s is not recognized for interest point %d\n",chain.c_str(),i);
00083       else
00084         ips->insert(ip);
00085     }
00086   }
00087 
00088   /*cout << "Joint Maps" << endl;
00089   for(unsigned int i=0; i<NumOutputs; i++)
00090     cout << outputNames[i] << ": " << jointMaps[i].chain << ' ' << jointMaps[i].link << endl;
00091   for(unsigned int i=NumOutputs; i<NumReferenceFrames; i++)
00092     cout << i << ": " << jointMaps[i].chain << ' ' << jointMaps[i].link << endl;
00093   cout << "Chain Maps" << endl;
00094   for(unsigned int i=0; i<chains.size(); i++)
00095     for(unsigned int j=1; j<chainMaps[i].size(); j++)
00096     cout << "chainMaps["<<i<<"]["<<j<<"] == " << chainMaps[i][j] << endl;*/
00097 }
00098 
00099 NEWMAT::ReturnMatrix
00100 Kinematics::frameToBase(unsigned int j) {
00101   unsigned int c=-1U,l=-1U;
00102   if(!lookup(j,c,l)) {
00103     NEWMAT::Matrix A(4,4);
00104     A<<ROBOOP::fourbyfourident;
00105     A.Release(); return A;
00106   }
00107   update(c,l);
00108   return chains[c]->convertFrame(l,0);
00109 }
00110 
00111 NEWMAT::ReturnMatrix
00112 Kinematics::linkToBase(unsigned int j) {
00113   unsigned int c=-1U,l=-1U;
00114   if(!lookup(j,c,l)) {
00115     NEWMAT::Matrix A(4,4);
00116     A<<ROBOOP::fourbyfourident;
00117     A.Release(); return A;
00118   }
00119   update(c,l);
00120   return chains[c]->convertLink(l,0);
00121 }
00122 
00123 NEWMAT::ReturnMatrix
00124 Kinematics::baseToFrame(unsigned int j) {
00125   unsigned int c=-1U,l=-1U;
00126   if(!lookup(j,c,l)) {
00127     NEWMAT::Matrix A(4,4);
00128     A<<ROBOOP::fourbyfourident;
00129     A.Release(); return A;
00130   }
00131   update(c,l);
00132   return chains[c]->convertFrame(0,l);
00133 }
00134 
00135 NEWMAT::ReturnMatrix
00136 Kinematics::baseToLink(unsigned int j) {
00137   unsigned int c=-1U,l=-1U;
00138   if(!lookup(j,c,l)) {
00139     NEWMAT::Matrix A(4,4);
00140     A<<ROBOOP::fourbyfourident;
00141     A.Release(); return A;
00142   }
00143   update(c,l);
00144   return chains[c]->convertLink(0,l);
00145 }
00146 
00147 NEWMAT::ReturnMatrix
00148 Kinematics::frameToFrame(unsigned int ij, unsigned int oj) {
00149   unsigned int ci=-1U,li=-1U,co=-1U,lo=-1U;
00150   if(ij==oj || !lookup(ij,ci,li) || !lookup(oj,co,lo)) {
00151     NEWMAT::Matrix A(4,4);
00152     A<<ROBOOP::fourbyfourident;
00153     A.Release(); return A;
00154   }
00155   if(ci==co) {
00156     update(ci,li>lo?li:lo);
00157     return chains[ci]->convertFrame(li,lo);
00158   } else if(li==0) {
00159     update(co,lo);
00160     return chains[co]->convertFrame(0,lo);
00161   } else if(lo==0) {
00162     update(ci,li);
00163     return chains[ci]->convertFrame(li,0);
00164   } else {
00165     update(ci,li);
00166     update(co,lo);
00167     NEWMAT::Matrix ans=chains[co]->convertFrame(0,lo)*chains[ci]->convertFrame(li,0);
00168     ans.Release(); return ans;
00169   }
00170 }
00171 
00172 NEWMAT::ReturnMatrix
00173 Kinematics::frameToLink(unsigned int ij, unsigned int oj) {
00174   unsigned int ci=-1U,li=-1U,co=-1U,lo=-1U;
00175   if(ij==oj || !lookup(ij,ci,li) || !lookup(oj,co,lo)) {
00176     NEWMAT::Matrix A(4,4);
00177     A<<ROBOOP::fourbyfourident;
00178     A.Release(); return A;
00179   }
00180   if(ci==co) {
00181     update(ci,li>lo?li:lo);
00182     return chains[ci]->convertFrameToLink(li,lo);
00183   } else if(li==0) {
00184     update(co,lo);
00185     return chains[co]->convertFrameToLink(0,lo);
00186   } else if(lo==0) {
00187     update(ci,li);
00188     return chains[ci]->convertFrameToLink(li,0);
00189   } else {
00190     update(ci,li);
00191     update(co,lo);
00192     NEWMAT::Matrix ans=chains[co]->convertLink(0,lo)*chains[ci]->convertFrame(li,0);
00193     ans.Release(); return ans;
00194   }
00195 }
00196 
00197 NEWMAT::ReturnMatrix
00198 Kinematics::linkToFrame(unsigned int ij, unsigned int oj) {
00199   unsigned int ci=-1U,li=-1U,co=-1U,lo=-1U;
00200   if(ij==oj || !lookup(ij,ci,li) || !lookup(oj,co,lo)) {
00201     NEWMAT::Matrix A(4,4);
00202     A<<ROBOOP::fourbyfourident;
00203     A.Release(); return A;
00204   }
00205   if(ci==co) {
00206     update(ci,li>lo?li:lo);
00207     return chains[ci]->convertLinkToFrame(li,lo);
00208   } else if(li==0) {
00209     update(co,lo);
00210     return chains[co]->convertLinkToFrame(0,lo);
00211   } else if(lo==0) {
00212     update(ci,li);
00213     return chains[ci]->convertLinkToFrame(li,0);
00214   } else {
00215     update(ci,li);
00216     update(co,lo);
00217     NEWMAT::Matrix ans=chains[co]->convertFrame(0,lo)*chains[ci]->convertLink(li,0);
00218     ans.Release(); return ans;
00219   }
00220 }
00221 
00222 NEWMAT::ReturnMatrix
00223 Kinematics::linkToLink(unsigned int ij, unsigned int oj) {
00224   unsigned int ci=-1U,li=-1U,co=-1U,lo=-1U;
00225   if(ij==oj || !lookup(ij,ci,li) || !lookup(oj,co,lo)) {
00226     NEWMAT::Matrix A(4,4);
00227     A<<ROBOOP::fourbyfourident;
00228     A.Release(); return A;
00229   }
00230   if(ci==co) {
00231     update(ci,li>lo?li:lo);
00232     return chains[ci]->convertLink(li,lo);
00233   } else if(li==0) {
00234     update(co,lo);
00235     return chains[co]->convertLink(0,lo);
00236   } else if(lo==0) {
00237     update(ci,li);
00238     return chains[ci]->convertLink(li,0);
00239   } else {
00240     update(ci,li);
00241     update(co,lo);
00242     NEWMAT::Matrix ans=chains[co]->convertLink(0,lo)*chains[ci]->convertLink(li,0);
00243     ans.Release(); return ans;
00244   }
00245 }
00246 
00247 void
00248 Kinematics::getInterestPoint(const std::string& name, unsigned int& j, NEWMAT::Matrix& ip) {
00249   unsigned int c=-1U,l=-1U;
00250   getInterestPoint(name,c,l,ip);
00251   j=chainMaps[c][l];
00252 }
00253 
00254 void
00255 Kinematics::getInterestPoint(const std::string& name, unsigned int& c, unsigned int& l, NEWMAT::Matrix& ip) {
00256   InterestPointMap::iterator it=ips->find(name);
00257   ip=NEWMAT::ColumnVector(4);
00258   if(it==ips->end()) {
00259     serr->printf("ERROR: '%s' is not a recognized interest point\n",name.c_str());
00260     ip=0;
00261     c=l=-1U;
00262   }
00263   c=(*it).second.chain;
00264   l=(*it).second.link;
00265   ip=pack((*it).second.x,(*it).second.y,(*it).second.z);
00266   //cout << ci << ' ' << li << ' ' << co << ' ' << lo << endl;
00267 }
00268 
00269 NEWMAT::ReturnMatrix
00270 Kinematics::getFrameInterestPoint(unsigned int frame, const std::string& name) {
00271   NEWMAT::ColumnVector ans(4);
00272   ans=0;
00273   unsigned int co=-1U,lo=-1U;
00274   if(!lookup(frame,co,lo)) {
00275     ans.Release(); return ans;
00276   }
00277   for(unsigned int pos=0,len=0; pos<name.size(); pos+=len+1) {
00278     len=name.find(',',pos);
00279     if(len==string::npos)
00280       len=name.size();
00281     len-=pos;
00282     unsigned int ci=-1U,li=-1U;
00283     NEWMAT::ColumnVector ip;
00284     getInterestPoint(name.substr(pos,len),ci,li,ip);
00285     if(ci==-1U) {
00286       ip.Release(); return ip;
00287     }
00288     if(ci==co) {
00289       update(ci,li>lo?li:lo);
00290       ans+=chains[ci]->convertLinkToFrame(li,lo)*ip;
00291     } else if(li==0) {
00292       update(co,lo);
00293       ans+=chains[co]->convertLinkToFrame(0,lo)*ip;
00294     } else if(lo==0) {
00295       update(ci,li);
00296       ans+=chains[ci]->convertLinkToFrame(li,0)*ip;
00297     } else {
00298       update(ci,li);
00299       update(co,lo);
00300       ans+=chains[co]->convertFrame(0,lo)*chains[ci]->convertLink(li,0)*ip;
00301     }
00302   }
00303   ans/=ans(4); //not strictly necessary, but may save some people headaches
00304   ans.Release(); return ans;
00305 }
00306 
00307 LegOrder_t
00308 Kinematics::findUnusedLeg(const NEWMAT::ColumnVector& down) {
00309   float height[NumLegs]; //not actually the real height, but proportional to it, which is all we need
00310   for(unsigned int i=0; i<NumLegs; i++) {
00311     height[i]=NEWMAT::DotProduct(frameToBase(PawFrameOffset+i).SubMatrix(1,3,4,4),down.SubMatrix(1,3,1,1));
00312     //cout << "height["<<i<<"]=="<<height[i]<<endl;
00313   }
00314   
00315   //Find the highest foot
00316   unsigned int highleg=0;
00317   for(unsigned int i=1; i<NumLegs; i++)
00318     if(height[highleg]>height[i])
00319       highleg=i;
00320   //cout << "High: " << highleg << endl;
00321   return static_cast<LegOrder_t>(highleg);
00322 }
00323 
00324 NEWMAT::ReturnMatrix
00325 Kinematics::calculateGroundPlane() {
00326   return calculateGroundPlane(pack(state->sensors[BAccelOffset],state->sensors[LAccelOffset],state->sensors[DAccelOffset]));
00327 }
00328 
00329 NEWMAT::ReturnMatrix
00330 Kinematics::calculateGroundPlane(const NEWMAT::ColumnVector& down) {
00331   //Find the unused foot
00332   unsigned int highleg=findUnusedLeg(down);
00333   
00334   //Fit a plane to the remaining 3 feet
00335   NEWMAT::Matrix legs(3,3);
00336   for(unsigned int i=0; i<highleg; i++)
00337     legs.Column(i+1)=frameToBase(PawFrameOffset+i).SubMatrix(1,3,4,4);
00338   for(unsigned int i=highleg+1; i<NumLegs; i++)
00339     legs.Column(i)=frameToBase(PawFrameOffset+i).SubMatrix(1,3,4,4);
00340   //cout << legs;
00341   NEWMAT::ColumnVector ones(3); ones=1;
00342   NEWMAT::ColumnVector p(4);
00343   p.SubMatrix(1,3,1,1) = legs.t().i()*ones;
00344   p(4)=1;
00345   p.Release(); return p;
00346 }
00347 
00348 NEWMAT::ReturnMatrix
00349 Kinematics::projectToPlane(unsigned int j, const NEWMAT::ColumnVector& r_j,
00350                            unsigned int b, const NEWMAT::ColumnVector& p_b,
00351                            unsigned int f)
00352 {
00353   /*! Mathematcal implementation:
00354    *  
00355    *  Need to convert @a p_b to @a p_j
00356    *  
00357    *  Once we have the transformation Tb_j from b to j, we need:\n
00358    *    T2=inv(Tb_j)'; T2(3,1:end-1)=-T2(3,1:end-1);\n
00359    *  but since we know a few things about the structure of T,
00360    *  we don't have to explicitly calculate that inverse. */
00361   NEWMAT::Matrix T2=linkToLink(b,j);
00362   //cout << "Transform b->j:\n"<<T2;
00363   T2.SubMatrix(4,4,1,3)=T2.SubMatrix(1,3,4,4).t()*T2.SubMatrix(1,3,1,3);
00364   T2.SubMatrix(1,3,4,4)=0;
00365   //cout << "Transform plane b->j:\n"<<T2;
00366   NEWMAT::ColumnVector p_j=T2*p_b;
00367   //cout << "p_j:\n"<<p_j.t();
00368 
00369   
00370   /*! After we obtain @a p_j, we can find the point of intersection of
00371    *  @a r_j and @a p_j using:
00372    *  @f[ \frac{p_d}{p_{xyz} \cdot r}r @f]
00373    *  Where @f$p_{xyz}@f$ is the first three elemnts of @a p_j, and
00374    *  @f$p_d@f$ is the fourth (hopefully last) element of p_j.
00375    *
00376    *  Of course, if @f$p_{xyz} \cdot r@f$ is 0, then r and p are parallel
00377    *  (since @a p_j is the normal of the plane, so a line perpendicular to
00378    *  the normal is parallel to the plane), so we set the resulting
00379    *  homogenous coordinates accordingly to represent an interesection at
00380    *  infinity. */
00381 
00382   float denom=0;
00383   for(unsigned int i=1; i<=3; i++)
00384     denom+=r_j(i)*p_j(i);
00385   NEWMAT::ColumnVector intersect=r_j;
00386   if(denom==0)
00387     intersect(4)=0;
00388   else {
00389     float s=p_j(4)/denom;
00390     for(unsigned int i=1; i<=3; i++)
00391       intersect(i)*=s;
00392     intersect(4)=1;
00393   }
00394   //cout << "Intersect_j:\n"<<intersect.t();
00395   NEWMAT::Matrix ans=linkToLink(j,f)*intersect;
00396   ans.Release(); return ans;
00397 }
00398 
00399 void
00400 Kinematics::update(unsigned int c, unsigned int l) {
00401   for(unsigned int j=1; j<=l; j++) {
00402     unsigned int tkout=chainMaps[c][j];
00403     if(tkout<NumOutputs)
00404       chains[c]->set_q(state->outputs[tkout],j);
00405   }
00406 }
00407 
00408 
00409 /*! @file
00410  * @brief 
00411  * @author ejt (Creator)
00412  *
00413  * $Author: dst $
00414  * $Name: tekkotsu-2_2_1 $
00415  * $Revision: 1.19 $
00416  * $State: Exp $
00417  * $Date: 2004/10/28 23:06:10 $
00418  */
00419 

Tekkotsu v2.2.1
Generated Tue Nov 23 16:36:38 2004 by Doxygen 1.3.9.1