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
00089
00090
00091
00092
00093
00094
00095
00096
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
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);
00304 ans.Release(); return ans;
00305 }
00306
00307 LegOrder_t
00308 Kinematics::findUnusedLeg(const NEWMAT::ColumnVector& down) {
00309 float height[NumLegs];
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
00313 }
00314
00315
00316 unsigned int highleg=0;
00317 for(unsigned int i=1; i<NumLegs; i++)
00318 if(height[highleg]>height[i])
00319 highleg=i;
00320
00321 return static_cast<LegOrder_t>(highleg);
00322 }
00323
00324 NEWMAT::ReturnMatrix
00325 Kinematics::calculateGroundPlane(const NEWMAT::ColumnVector& down) {
00326
00327 unsigned int highleg=findUnusedLeg(down);
00328
00329
00330 NEWMAT::Matrix legs(3,3);
00331 for(unsigned int i=0; i<highleg; i++)
00332 legs.Column(i+1)=frameToBase(PawFrameOffset+i).SubMatrix(1,3,4,4);
00333 for(unsigned int i=highleg+1; i<NumLegs; i++)
00334 legs.Column(i)=frameToBase(PawFrameOffset+i).SubMatrix(1,3,4,4);
00335
00336 NEWMAT::ColumnVector ones(3); ones=1;
00337 NEWMAT::ColumnVector p(4);
00338 p.SubMatrix(1,3,1,1) = legs.t().i()*ones;
00339 p(4)=1;
00340 p.Release(); return p;
00341 }
00342
00343 NEWMAT::ReturnMatrix
00344 Kinematics::projectToPlane(unsigned int j, const NEWMAT::ColumnVector& r_j,
00345 unsigned int b, const NEWMAT::ColumnVector& p_b,
00346 unsigned int f)
00347 {
00348
00349
00350
00351
00352
00353
00354
00355
00356 NEWMAT::Matrix T2=linkToLink(b,j);
00357
00358 T2.SubMatrix(4,4,1,3)=T2.SubMatrix(1,3,4,4).t()*T2.SubMatrix(1,3,1,3);
00359 T2.SubMatrix(1,3,4,4)=0;
00360
00361 NEWMAT::ColumnVector p_j=T2*p_b;
00362
00363
00364
00365
00366
00367
00368
00369
00370
00371
00372
00373
00374
00375
00376
00377 float denom=0;
00378 for(unsigned int i=1; i<=3; i++)
00379 denom+=r_j(i)*p_j(i);
00380 NEWMAT::ColumnVector intersect=r_j;
00381 if(denom==0)
00382 intersect(4)=0;
00383 else {
00384 float s=p_j(4)/denom;
00385 for(unsigned int i=1; i<=3; i++)
00386 intersect(i)*=s;
00387 intersect(4)=1;
00388 }
00389
00390 NEWMAT::Matrix ans=linkToLink(j,f)*intersect;
00391 ans.Release(); return ans;
00392 }
00393
00394 void
00395 Kinematics::update(unsigned int c, unsigned int l) {
00396 for(unsigned int j=1; j<=l; j++) {
00397 unsigned int tkout=chainMaps[c][j];
00398 if(tkout<NumOutputs)
00399 chains[c]->set_q(state->outputs[tkout],j);
00400 }
00401 }
00402
00403
00404
00405
00406
00407
00408
00409
00410
00411
00412
00413
00414