00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026 #include "kinematicChain.h"
00027
00028 #include "matvec3D.h"
00029 #include "body.h"
00030 #include "robot.h"
00031 #include "dof.h"
00032 #include "joint.h"
00033 #include "world.h"
00034 #include "matvecIO.h"
00035 #include "dynJoint.h"
00036 #include "humanHand.h"
00037 #include "math/matrix.h"
00038 #include "tinyxml.h"
00039
00040 #ifdef MKL
00041 #include "mkl_wrappers.h"
00042 #else
00043 #include "lapack_wrappers.h"
00044 #endif
00045
00046
00047 #include "maxdet.h"
00048
00049
00050 #include "debug.h"
00051
00052 KinematicChain::KinematicChain(Robot *r,int chainNumber, int jointNum) : owner(r),chainNum(chainNumber),
00053 numJoints(0),numLinks(0),lastJoint(NULL), IVRoot(NULL),numChildren(0), firstJointNum(jointNum)
00054 {
00055 }
00056
00062 KinematicChain::~KinematicChain()
00063 {
00064 int i;
00065 IVTran->unref();
00066
00067 delete [] lastJoint;
00068
00069 for (i=0;i<numJoints;i++)
00070 if (jointVec[i]) delete jointVec[i];
00071
00072 for (i=0;i<numLinks;i++)
00073 if (linkVec[i]) owner->getWorld()->destroyElement(linkVec[i]);
00074
00075
00076
00077
00078 for (i=numChildren-1;i>=0;i--)
00079 owner->detachRobot(children[i]);
00080 }
00081
00091 int
00092 KinematicChain::createDynamicJoints(const std::vector<int> &dynJointTypes)
00093 {
00094 Link* prevLink = owner->getBase();
00095 for (int l=0; l<numLinks; l++){
00096 transf dynJointTran = transf::IDENTITY;
00097 if(l==0) dynJointTran = tran;
00098
00099 if (dynJointTypes[l] == DynJoint::BALL) {
00100 linkVec[l]->setDynJoint(new BallDynJoint(
00101 jointVec[lastJoint[l]-2],
00102 jointVec[lastJoint[l]-1],jointVec[lastJoint[l]],
00103 prevLink,linkVec[l],
00104 dynJointTran, jointVec[lastJoint[l]]->getTran().inverse()));
00105 jointVec[lastJoint[l]-2]->dynJoint = linkVec[l]->getDynJoint();
00106 jointVec[lastJoint[l]-1]->dynJoint = linkVec[l]->getDynJoint();
00107 jointVec[lastJoint[l]-0]->dynJoint = linkVec[l]->getDynJoint();
00108 } else if (dynJointTypes[l] == DynJoint::UNIVERSAL) {
00109 linkVec[l]->setDynJoint(new UniversalDynJoint(
00110 jointVec[lastJoint[l]-1], jointVec[lastJoint[l]],
00111 prevLink, linkVec[l],
00112 dynJointTran, jointVec[lastJoint[l]]->getTran().inverse()));
00113 jointVec[lastJoint[l]-1]->dynJoint = linkVec[l]->getDynJoint();
00114 jointVec[lastJoint[l]-0]->dynJoint = linkVec[l]->getDynJoint();
00115 } else if (dynJointTypes[l] == DynJoint::REVOLUTE) {
00116 linkVec[l]->setDynJoint(new RevoluteDynJoint(jointVec[lastJoint[l]],
00117 prevLink,linkVec[l],dynJointTran));
00118 jointVec[lastJoint[l]]->dynJoint = linkVec[l]->getDynJoint();
00119 } else if (dynJointTypes[l] == DynJoint::PRISMATIC) {
00120 linkVec[l]->setDynJoint(new RevoluteDynJoint(jointVec[lastJoint[l]],
00121 prevLink,linkVec[l],dynJointTran));
00122 jointVec[lastJoint[l]]->dynJoint = linkVec[l]->getDynJoint();
00123 } else if (dynJointTypes[l] == DynJoint::FIXED) {
00124 DBGA("FIXED dynamic joints not yet fully supported");
00125 return FAILURE;
00126
00127 } else {
00128 DBGA("Unknown joint type requested");
00129 return FAILURE;
00130 }
00131 prevLink = linkVec[l];
00132 }
00133 return SUCCESS;
00134 }
00135
00145 int
00146 KinematicChain::initChainFromXml(const TiXmlElement* root,QString &linkDir)
00147 {
00148 numJoints = countXmlElements(root,"joint");
00149 if (numJoints < 1) {
00150 DBGA("number of joints < 1");
00151 return FAILURE;
00152 }
00153
00154 numLinks = countXmlElements(root,"link");
00155 if (numLinks < 1) {
00156 DBGA("Number of links < 1");
00157 return FAILURE;
00158 }
00159
00160 jointVec.resize(numJoints, NULL);
00161 linkVec.resize(numLinks, NULL);
00162
00163 lastJoint = new int[numLinks];
00164
00165 IVRoot = new SoSeparator;
00166 IVTran = new SoTransform;
00167 IVTran->ref();
00168
00169
00170 const TiXmlElement* element = findXmlElement(root,"transform");
00171 if(element){
00172 if(!getTransform(element,tran)){
00173 QTWARNING("Fail to perform transformation");
00174 return FAILURE;
00175 }
00176 }
00177 tran.toSoTransform(IVTran);
00178
00179 DBGA(" Creating joints");
00180 numDOF = 0;
00181 std::list<const TiXmlElement*> elementList = findAllXmlElements(root, "joint");
00182 std::list<const TiXmlElement*>::iterator p;
00183 int j;
00184 for(p = elementList.begin(), j=0; p!=elementList.end(); p++,j++){
00185 DBGA(" Joint " << j);
00186 QString type = (*p)->Attribute("type");
00187 if(type.isNull()){
00188 QTWARNING("No Joint Type Specified");
00189 return FAILURE;
00190 }
00191 if(type == "Revolute"){
00192 jointVec[j] = new RevoluteJoint(this);
00193 } else if(type == "Prismatic"){
00194 jointVec[j] = new PrismaticJoint (this);
00195 } else {
00196 DBGA("Unknown joint type requested");
00197 return FAILURE;
00198 }
00199 if (jointVec[j]->initJointFromXml(*p, firstJointNum+j) == FAILURE) {
00200 DBGA("Failed to initialize joint");
00201 return FAILURE;
00202 }
00203 }
00204
00205 DBGA(" Creating links");
00206 std::vector<int> dynJointTypes;
00207 int lastJointNum = -1;
00208 elementList = findAllXmlElements(root, "link");
00209 int l;
00210 for (l=0, p=elementList.begin(); p!=elementList.end(); p++,l++){
00211 DBGA(" Link " << l);
00212 QString jointType=(*p)->Attribute("dynamicJointType");
00213 if(jointType.isNull()){
00214 QTWARNING("No Dynamic Joint Type Specified");
00215 return FAILURE;
00216 }
00217 jointType = jointType.stripWhiteSpace();
00218 if (jointType == "Revolute") {
00219 dynJointTypes.push_back(DynJoint::REVOLUTE);
00220 lastJointNum += 1;
00221 } else if (jointType == "Ball") {
00222 dynJointTypes.push_back(DynJoint::BALL);
00223 lastJointNum += 3;
00224 } else if (jointType == "Universal") {
00225 dynJointTypes.push_back(DynJoint::UNIVERSAL);
00226 lastJointNum += 2;
00227 } else if (jointType == "Prismatic") {
00228 dynJointTypes.push_back(DynJoint::PRISMATIC);
00229 lastJointNum += 1;
00230 } else if (jointType == "Fixed") {
00231 dynJointTypes.push_back(DynJoint::FIXED);
00232 } else {
00233 DBGA("Unknown joint type requested");
00234 return FAILURE;
00235 }
00236
00237 QString linkFilename = (*p)->GetText();
00238 linkFilename = linkFilename.stripWhiteSpace();
00239 QString linkName = QString(owner->name()) + QString("_chain%1_link%2").arg(chainNum).arg(l);
00240 linkVec[l] = new Link(owner,chainNum,l,owner->getWorld(),linkName.latin1());
00241 if (linkVec[l]->load(linkDir + linkFilename)==FAILURE) {
00242 delete linkVec[l]; linkVec[l] = NULL;
00243 DBGA("Failed to load file for link " << l);
00244 return FAILURE;
00245 }
00246
00247
00248
00249
00250
00251
00252 QString collisionRule=(*p)->Attribute("collisionRule");
00253
00254 if(!collisionRule.isNull()){
00255 collisionRule = collisionRule.stripWhiteSpace();
00256 if (collisionRule == "Off"){
00257 linkVec[l]->addToIvc(true);
00258 linkVec[l]->myWorld->toggleCollisions(false, linkVec[l],NULL);
00259 DBGA("Collisions off for link " << l);
00260 }else if (collisionRule == "OverlappingPair"){
00261
00262
00263
00264
00265
00266 linkVec[l]->addToIvc();
00267 QString targetChain=(*p)->Attribute("targetChain");
00268 targetChain = targetChain.stripWhiteSpace();
00269 if (targetChain == "base")
00270 linkVec[l]->myWorld->toggleCollisions(false, linkVec[l],owner->getBase());
00271 else{
00272
00273 QString targetLink = (*p)->Attribute("targetLink");
00274 if (!targetLink.isNull()){
00275 bool ok = TRUE;
00276 int linkNum = targetLink.toInt(&ok);
00277 if (!ok){
00278 DBGA("targetLink not a valid input");
00279 return FAILURE;
00280 }
00281
00282 if(targetChain.isNull())
00283 linkVec[l]->myWorld->toggleCollisions(false, linkVec[l],linkVec[linkNum]);
00284 else{
00285 int chainNum = targetChain.toInt(&ok);
00286 if (!ok){
00287 DBGA("targetChain not a valid input");
00288 return FAILURE;
00289 }
00290 linkVec[l]->myWorld->toggleCollisions(false, linkVec[l],owner->getChain(chainNum)->linkVec[linkNum]);
00291 }
00292 }
00293 }
00294 }
00295 else{
00296 DBGA("Unknown Collision Rule");
00297 return FAILURE;
00298 }
00299 }else{
00300 linkVec[l]->addToIvc();
00301 }
00302
00303
00304 lastJoint[l] = lastJointNum;
00305 if (lastJoint[l] >= numJoints) {
00306 DBGA("Wrong last joint value: " << lastJoint[l]);
00307 return FAILURE;
00308 }
00309
00310 IVRoot->addChild(linkVec[l]->getIVRoot());
00311 }
00312
00313 DBGA(" Creating dynamic joints");
00314 if (createDynamicJoints(dynJointTypes) == FAILURE) {
00315 DBGA("Failed to create dynamic joints");
00316 return FAILURE;
00317 }
00318
00319 jointsMoved = true;
00320 updateLinkPoses();
00321 owner->getWorld()->tendonChange();
00322 owner->getIVRoot()->addChild(IVRoot);
00323
00324 return SUCCESS;
00325 }
00326
00327
00332 void
00333 KinematicChain::cloneFrom(const KinematicChain *original)
00334 {
00335 IVRoot = new SoSeparator;
00336 IVTran = new SoTransform;
00337 IVTran->ref();
00338 tran = original->getTran();
00339 tran.toSoTransform(IVTran);
00340
00341 numJoints = original->getNumJoints();
00342 numLinks = original->getNumLinks();
00343 jointVec.resize(numJoints,NULL);
00344 linkVec.resize(numLinks,NULL);
00345 lastJoint = new int[numLinks];
00346
00347 numDOF = 0;
00348 for (int j=0; j<numJoints; j++){
00349 if (original->getJoint(j)->getType() == REVOLUTE) {
00350 jointVec[j] = new RevoluteJoint(this);
00351 }else if (original->getJoint(j)->getType() == PRISMATIC) {
00352 jointVec[j] = new PrismaticJoint(this);
00353 }
00354 jointVec[j]->cloneFrom( original->getJoint(j) );
00355 }
00356
00357 std::vector<int> dynJointTypes;
00358 for (int l=0; l<numLinks; l++){
00359 lastJoint[l] = original->getLastJoint(l);
00360 QString linkName = QString(owner->name())+QString("_chain%1_link%2").arg(chainNum).arg(l);
00361 linkVec[l] = new Link(owner,chainNum,l,owner->getWorld(),linkName);
00362 linkVec[l]->cloneFrom( original->getLink(l) );
00363
00364 IVRoot->addChild(linkVec[l]->getIVRoot());
00365 dynJointTypes.push_back( original->getLink(l)->getDynJoint()->getType() );
00366 }
00367 createDynamicJoints(dynJointTypes);
00368
00369 jointsMoved = true;
00370 owner->getIVRoot()->addChild(IVRoot);
00371 }
00372
00378 void
00379 KinematicChain::attachRobot(Robot *r,const transf &offsetTr)
00380 {
00381 children.push_back(r);
00382 childOffsetTran.push_back(offsetTr);
00383 numChildren++;
00384
00385 if (r->getMountPiece())
00386 r->getMountPiece()->
00387 setDynJoint(new FixedDynJoint(linkVec[numLinks-1],r->getMountPiece(), offsetTr));
00388 else
00389 r->getBase()->
00390 setDynJoint(new FixedDynJoint(linkVec[numLinks-1],r->getBase(), offsetTr));
00391 }
00392
00397 void
00398 KinematicChain::detachRobot(Robot *r)
00399 {
00400 int i,j;
00401 std::vector<transf>::iterator tp;
00402 std::vector<Robot *>::iterator rp;
00403
00404 for (i=0,rp=children.begin();rp!=children.end();i++,rp++)
00405 if (*rp==r) {children.erase(rp); break;}
00406
00407 for (j=0,tp=childOffsetTran.begin();tp!=childOffsetTran.end();j++,tp++)
00408 if (j==i) {childOffsetTran.erase(tp); break;}
00409
00410 numChildren--;
00411 }
00412
00413 std::list<Joint*>
00414 KinematicChain::getJoints()
00415 {
00416 std::list<Joint*> joints;
00417 for (int j=0; j<numJoints; j++) {
00418 joints.push_back(jointVec[j]);
00419 }
00420 return joints;
00421 }
00426 void KinematicChain::filterCollisionReport(CollisionReport &colReport)
00427 {
00428
00429 CollisionReport::iterator it = colReport.begin();
00430 bool keep;
00431 while ( it != colReport.end() ) {
00432
00433 if ( (*it).first->getOwner() != owner ) {
00434 if ( (*it).second->getOwner() != owner ) {
00435 keep = false;
00436 } else {
00437 if ( ((Link*)(*it).second)->getChainNum() == chainNum )
00438 keep = true;
00439 else
00440 keep = false;
00441 }
00442 } else if ( (*it).second->getOwner() != owner ) {
00443 if ( ((Link*)(*it).first)->getChainNum() == chainNum )
00444 keep = true;
00445 else
00446 keep = false;
00447 } else {
00448 keep = false;
00449 }
00450 if (!keep) {
00451 it = colReport.erase(it);
00452 } else {
00453 it++;
00454 }
00455 }
00456 }
00457
00464 void
00465 KinematicChain::getJointValues(double *jointVals) const
00466 {
00467 for (int j=0; j<numJoints; j++) {
00468 jointVals[firstJointNum + j] = jointVec[j]->getVal();
00469 }
00470 }
00471
00477 void
00478 KinematicChain::setJointValues(const double *jointVals)
00479 {
00480 for (int j=0; j<numJoints; j++) {
00481 jointVec[j]->setVal( jointVals[firstJointNum + j] );
00482 }
00483 }
00484
00491 void
00492 KinematicChain::updateLinkPoses()
00493 {
00494 std::vector<transf> newLinkTranVec;
00495 newLinkTranVec.resize(numLinks, transf::IDENTITY);
00496 fwdKinematics(NULL, newLinkTranVec);
00497
00498 for (int l=0;l<numLinks;l++) {
00499 linkVec[l]->setTran(newLinkTranVec[l]);
00500 }
00501
00502 for (int j=0;j<numChildren;j++) {
00503 children[j]->simpleSetTran(childOffsetTran[j]*newLinkTranVec[numLinks-1]);
00504 }
00505
00506 if ( owner->inherits("HumanHand") ){
00507 ((HumanHand*)owner)->updateTendonGeometry();
00508 owner->getWorld()->tendonChange();
00509 }
00510 }
00511
00521 void
00522 KinematicChain::fwdKinematics(const double *jointVals, std::vector<transf> &newLinkTranVec) const
00523 {
00524 transf total = tran * owner->getTran();
00525 int l=0;
00526 for (int j=0;j<numJoints;j++) {
00527 if (!jointVals) {
00528 total = jointVec[j]->getTran( jointVec[j]->getVal() ) * total;
00529 } else {
00530 total = jointVec[j]->getTran( jointVals[firstJointNum + j] ) * total;
00531 }
00532 if (l<numLinks && lastJoint[l]==j) {
00533 newLinkTranVec[l] = total;
00534 l++;
00535 }
00536 }
00537 }
00538
00557 void
00558 KinematicChain::getJointLocations(const double *jointVals, std::vector<transf> &jointTranVec) const
00559 {
00560 transf total = tran * owner->getTran();
00561 for (int j=0;j<numJoints;j++) {
00562 jointTranVec[j] = total;
00563 if (!jointVals) {
00564 total = jointVec[j]->getTran( jointVec[j]->getVal() ) * total;
00565 } else {
00566 total = jointVec[j]->getTran( jointVals[firstJointNum + j] ) * total;
00567 }
00568 }
00569 }
00570
00575 void
00576 KinematicChain::infinitesimalMotion(const double *jointVals, std::vector<transf> &newLinkTranVec) const
00577 {
00578
00579
00580 Matrix J(actuatedJacobian(linkJacobian(false)));
00581
00582 Matrix theta(numJoints, 1);
00583
00584
00585 double inf = 0.1;
00586
00587 double eps = 1.0e-6;
00588 for(int j=0; j<numJoints; j++) {
00589 int sign;
00590 if ( jointVals[firstJointNum + j] + eps < jointVec[j]->getVal() ) {
00591 sign = -1;
00592 } else if ( jointVals[firstJointNum + j] > jointVec[j]->getVal() + eps ) {
00593 sign = 1;
00594 } else {
00595 sign = 0;
00596 }
00597 theta.elem(j,0) = sign * inf;
00598 }
00599
00600 Matrix dm(6*numLinks, 1);
00601 matrixMultiply(J, theta, dm);
00602
00603 for (int l=0; l<numLinks; l++) {
00604 transf tr = rotXYZ( dm.elem(6*l+3, 0), dm.elem(6*l+4, 0), dm.elem(6*l+5, 0) ) *
00605 translate_transf( vec3( dm.elem(6*l+0, 0), dm.elem(6*l+1, 0), dm.elem(6*l+2, 0) ) );
00606 newLinkTranVec[l] = tr;
00607 }
00608 }
00609
00610 void
00611 KinematicChain::getDynamicJoints(std::vector<DynJoint*> *dj) const
00612 {
00613 DynJoint *lastDynJoint = NULL;
00614 for(int j=0; j<numJoints; j++) {
00615 if (jointVec[j]->dynJoint == lastDynJoint) continue;
00616 lastDynJoint = jointVec[j]->dynJoint;
00617 dj->push_back(lastDynJoint);
00618 }
00619 }
00620
00626 Matrix
00627 KinematicChain::linkJacobian(bool worldCoords) const
00628 {
00629 Matrix J(Matrix::ZEROES<Matrix>(numLinks * 6, numLinks * 6));
00630 Matrix indJ(6,6);
00631 for (int l=0; l<numLinks; l++) {
00632 for (int dl=0; dl<=l; dl++) {
00633 DynJoint *dynJoint = linkVec[dl]->getDynJoint();
00634 dynJoint->jacobian(linkVec[l]->getTran(), &indJ, worldCoords);
00635 J.copySubMatrix(6*l, 6*dl, indJ);
00636 }
00637 }
00638 return J;
00639 }
00640
00646 Matrix
00647 KinematicChain::activeLinkJacobian(bool worldCoords)
00648 {
00649 Matrix J(linkJacobian(worldCoords));
00650 if (!J.rows() || !J.cols()) return Matrix(0,0);
00651 int activeLinks = 0;
00652 for (int l=0; l<numLinks; l++) {
00653 if (linkVec[l]->getNumContacts()) {
00654 activeLinks++;
00655 }
00656 }
00657 if (!activeLinks) {
00658 DBGA("Active link Jac requested, but no active links!");
00659 return Matrix::ZEROES<Matrix>(0,0);
00660 }
00661 int activeRows = 6*activeLinks;
00662 Matrix activeJ(activeRows, J.cols());
00663 int linkCount = 0;
00664
00665 for (int l=0; l<numLinks; l++) {
00666 if (!linkVec[l]->getNumContacts()) continue;
00667 activeJ.copySubBlock(6*linkCount, 0, 6, J.cols(), J, 6*l, 0);
00668 linkCount++;
00669 }
00670 return activeJ;
00671 }
00672
00678 Matrix
00679 KinematicChain::actuatedJacobian(const Matrix &fullColumnJ) const
00680 {
00681 std::vector<DynJoint*> dynJoints;
00682 getDynamicJoints(&dynJoints);
00683 int activeRows = fullColumnJ.rows();
00684 if (!activeRows) return Matrix(0,0);
00685
00686 int numConstrained = 0;
00687 int numActuated = 0;
00688 for(int dj=0; dj<(int)dynJoints.size(); dj++) {
00689 numConstrained += dynJoints[dj]->getNumConstraints();
00690 numActuated += 6 - dynJoints[dj]->getNumConstraints();
00691 }
00692 assert(numActuated + numConstrained == 6*numLinks);
00693 assert(fullColumnJ.cols() == 6*numLinks);
00694 Matrix blockJ(activeRows, numActuated);
00695
00696 int actIndex = 0;
00697 for(int dj=0; dj<(int)dynJoints.size(); dj++) {
00698 char constraints[6];
00699 dynJoints[dj]->getConstraints(constraints);
00700 for (int c=0; c<6; c++) {
00701 if (!constraints[c]) {
00702
00703 blockJ.copySubBlock(0, actIndex, activeRows, 1, fullColumnJ, 0, 6*dj+c);
00704 actIndex++;
00705 }
00706 }
00707 }
00708 assert(actIndex == numActuated);
00709 return blockJ;
00710 }
00711
00716 Matrix
00717 KinematicChain::jointTorquesVector(Matrix fullRobotTorques)
00718 {
00719 assert(fullRobotTorques.cols() == 1);
00720 Matrix tau(numJoints,1);
00721 for (int j=0; j<numJoints; j++) {
00722 tau.elem(j,0) = fullRobotTorques.elem(jointVec[j]->getNum(), 0);
00723 }
00724 return tau;
00725 }
00726
00732 int
00733 KinematicChain::getNumContacts(Body *body)
00734 {
00735 int numContacts = 0;
00736 for (int l=0; l<numLinks; l++) {
00737 numContacts += linkVec[l]->getNumContacts(body);
00738 }
00739 return numContacts;
00740 }
00741
00747 std::list<Contact*>
00748 KinematicChain::getContacts(Body *body)
00749 {
00750 std::list<Contact*> contacts;
00751 for (int l=0; l<numLinks; l++) {
00752 std::list<Contact*> linkContacts = linkVec[l]->getContacts(body);
00753 contacts.insert(contacts.end(), linkContacts.begin(), linkContacts.end());
00754 }
00755 return contacts;
00756 }
