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