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
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
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);
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;
00080 #endif
00081 }
00082
00083
00084
00085
00086
00087
00088
00089
00090
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
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);
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
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;
00405 heights[i]=h;
00406 }
00407
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
00439 unsigned int highleg=0;
00440 for(unsigned int i=1; i<NumLegs; i++)
00441 if(heights[i]>heights[highleg])
00442 highleg=i;
00443
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)
00451 down=pack(0,0,-1);
00452 return calculateGroundPlane(down);
00453 }
00454
00455
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);
00463 float heights[3];
00464 unsigned int legs[3];
00465 std::string names[3];
00466
00467 for(unsigned int i=0; i<3; i++) {
00468 heights[i]=FLT_MAX;
00469 legs[i]=-1U;
00470 }
00471
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;
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
00507
00508
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
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& ) {
00611 return pack(0,0,1,0);
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
00622
00623
00624
00625
00626
00627
00628
00629 NEWMAT::Matrix T2=linkToLink(b,j);
00630
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
00634 NEWMAT::ColumnVector p_j=T2*p_b;
00635
00636
00637
00638
00639
00640
00641
00642
00643
00644
00645
00646
00647
00648
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
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
00678
00679
00680
00681
00682
00683
00684
00685
00686
00687