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

Tekkotsu v2.2.2
Generated Tue Jan 4 15:43:14 2005 by Doxygen 1.4.0