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

Tekkotsu v2.4.1
Generated Tue Aug 16 16:32:47 2005 by Doxygen 1.4.4