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
00091
00092
00093
00094
00095
00096
00097
00098
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
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);
00306 ans.Release(); return ans;
00307 }
00308
00309 LegOrder_t
00310 Kinematics::findUnusedLeg(const NEWMAT::ColumnVector& down) {
00311 float height[NumLegs];
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
00315 }
00316
00317
00318 unsigned int highleg=0;
00319 for(unsigned int i=1; i<NumLegs; i++)
00320 if(height[highleg]>height[i])
00321 highleg=i;
00322
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
00334 unsigned int highleg=findUnusedLeg(down);
00335
00336
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
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
00356
00357
00358
00359
00360
00361
00362
00363 NEWMAT::Matrix T2=linkToLink(b,j);
00364
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
00368 NEWMAT::ColumnVector p_j=T2*p_b;
00369
00370
00371
00372
00373
00374
00375
00376
00377
00378
00379
00380
00381
00382
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
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
00412
00413
00414
00415
00416
00417
00418
00419
00420
00421