Link.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2008, AIST, the University of Tokyo and General Robotix Inc.
00003  * All rights reserved. This program is made available under the terms of the
00004  * Eclipse Public License v1.0 which accompanies this distribution, and is
00005  * available at http://www.eclipse.org/legal/epl-v10.html
00006  * Contributors:
00007  * National Institute of Advanced Industrial Science and Technology (AIST)
00008  */
00009 
00043 #include "Link.h"
00044 #include <stack>
00045 
00046 using namespace std;
00047 using namespace hrp;
00048 
00049 
00050 Link::Link()
00051 {
00052     body = 0;
00053     index = -1;
00054     jointId = -1;
00055     jointType = FIXED_JOINT;
00056     parent = 0;
00057     sibling = 0;
00058     child = 0;
00059     
00060     isHighGainMode = false;
00061     isCrawler = false;
00062 
00063     defaultJointValue = 0.0;
00064 }
00065 
00066 
00067 Link::Link(const Link& org)
00068     : name(org.name)
00069 {
00070     body = 0;
00071     index = -1; // should be set by a Body object
00072     jointId = org.jointId;
00073     jointType = org.jointType;
00074 
00075     p = org.p;
00076     R = org.R;
00077     v = org.v;
00078     w = org.w;
00079     dv = org.dv;
00080     dw = org.dw;
00081 
00082     q = org.q;
00083     dq = org.dq;
00084     ddq = org.ddq;
00085     u = org.u;
00086 
00087     a = org.a;
00088     d = org.d;
00089     b = org.b;
00090     Rs = org.Rs;
00091     m = org.m;
00092     I = org.I;
00093     c = org.c;
00094 
00095     fext = org.fext;
00096     tauext = org.tauext;
00097 
00098     Jm2 = org.Jm2;
00099 
00100     ulimit = org.ulimit;
00101     llimit = org.llimit;
00102     uvlimit = org.uvlimit;
00103     lvlimit = org.lvlimit;
00104 
00105     defaultJointValue = org.defaultJointValue;
00106     torqueConst = org.torqueConst;
00107     encoderPulse = org.encoderPulse;
00108     Ir = org.Ir;
00109     gearRatio = org.gearRatio;
00110     gearEfficiency = org.gearEfficiency;
00111     rotorResistor = org.rotorResistor;
00112 
00113     isHighGainMode = org.isHighGainMode;
00114     isCrawler = org.isCrawler;
00115 
00116     if(org.coldetModel){
00117         coldetModel = new ColdetModel(*org.coldetModel);
00118     }
00119 
00120     parent = child = sibling = 0;
00121 
00122     if(org.child){
00123         stack<Link*> children;
00124         for(Link* orgChild = org.child; orgChild; orgChild = orgChild->sibling){
00125             children.push(orgChild);
00126         }
00127         while(!children.empty()){
00128             addChild(new Link(*children.top()));
00129             children.pop();
00130         }
00131     }
00132 
00133 }
00134 
00135 
00136 Link::~Link()
00137 {
00138     Link* link = child;
00139     while(link){
00140         Link* linkToDelete = link;
00141         link = link->sibling;
00142         delete linkToDelete;
00143     }
00144 }
00145 
00146 
00147 void Link::addChild(Link* link)
00148 {
00149     if(link->parent){
00150         link->parent->detachChild(link);
00151     }
00152 
00153     link->sibling = child;
00154     link->parent = this;
00155     child = link;
00156 
00157     link->setBodyIter(body);
00158 }
00159 
00160 
00166 bool Link::detachChild(Link* childToRemove)
00167 {
00168     bool removed = false;
00169 
00170     Link* link = child;
00171     Link* prevSibling = 0;
00172     while(link){
00173         if(link == childToRemove){
00174             removed = true;
00175             if(prevSibling){
00176                 prevSibling->sibling = link->sibling;
00177             } else {
00178                 child = link->sibling;
00179             }
00180             break;
00181         }
00182         prevSibling = link;
00183         link = link->sibling;
00184     }
00185 
00186     if(removed){
00187         childToRemove->parent = 0;
00188         childToRemove->sibling = 0;
00189         childToRemove->setBodyIter(0);
00190     }
00191 
00192     return removed;
00193 }
00194 
00195 
00196 void Link::setBodyIter(Body* body)
00197 {
00198     this->body = body;
00199     for(Link* link = child; link; link = link->sibling){
00200         link->setBodyIter(body);
00201     }
00202 }
00203 
00204 
00205 std::ostream& operator<<(std::ostream &out, Link& link)
00206 {
00207     link.putInformation(out);
00208     return out;
00209 }
00210 
00211 void Link::putInformation(std::ostream& out)
00212 {
00213     out << "Link " << name << " Link Index = " << index << ", Joint ID = " << jointId << "\n";
00214 
00215     out << "Joint Type: ";
00216 
00217     switch(jointType) {
00218     case FREE_JOINT:
00219         out << "Free Joint\n";
00220         break;
00221     case FIXED_JOINT:
00222         out << "Fixed Joint\n";
00223         break;
00224     case ROTATIONAL_JOINT:
00225         out << "Rotational Joint\n";
00226         out << "Axis = " << a << "\n";
00227         break;
00228     case SLIDE_JOINT:
00229         out << "Slide Joint\n";
00230         out << "Axis = " << d << "\n";
00231         break;
00232     }
00233 
00234     out << "parent = " << (parent ? parent->name : "null") << "\n";
00235 
00236     out << "child = ";
00237     if(child){
00238         Link* link = child;
00239         while(true){
00240             out << link->name;
00241             link = link->sibling;
00242             if(!link){
00243                 break;
00244             }
00245             out << ", ";
00246         }
00247     } else {
00248         out << "null";
00249     }
00250     out << "\n";
00251 
00252     out << "b = "  << b << "\n";
00253     out << "Rs = " << Rs << "\n";
00254     out << "c = "  << c << "\n";
00255     out << "m = "  << m << "\n";
00256     out << "Ir = " << Ir << "\n";
00257     out << "I = "  << I << "\n";
00258     out << "torqueConst = " << torqueConst << "\n";
00259     out << "encoderPulse = " << encoderPulse << "\n";
00260     out << "gearRatio = " << gearRatio << "\n";
00261     out << "gearEfficiency = " << gearEfficiency << "\n";
00262     out << "Jm2 = " << Jm2 << "\n";
00263     out << "ulimit = " << ulimit << "\n";
00264     out << "llimit = " << llimit << "\n";
00265     out << "uvlimit = " << uvlimit << "\n";
00266     out << "lvlimit = " << lvlimit << "\n";
00267 
00268     if(false){
00269         out << "R = " << R << "\n";
00270         out << "p = " << p << ", wc = " << wc << "\n";
00271         out << "v = " << v << ", vo = " << vo << ", dvo = " << dvo << "\n";
00272         out << "w = " << w << ", dw = " << dw << "\n";
00273 
00274         out << "u = " << u << ", q = " << q << ", dq = " << dq << ", ddq = " << ddq << "\n";
00275 
00276         out << "fext = " << fext << ", tauext = " << tauext << "\n";
00277 
00278         out << "sw = " << sw << ", sv = " << sv << "\n";
00279         out << "Ivv = " << Ivv << "\n";
00280         out << "Iwv = " << Iwv << "\n";
00281         out << "Iww = " << Iww << "\n";
00282         out << "cv = " << cv << ", cw = " << cw << "\n";
00283         out << "pf = " << pf << ", ptau = " << ptau << "\n";
00284         out << "hhv = " << hhv << ", hhw = " << hhw << "\n";
00285         out << "uu = " << uu << ", dd = " << dd << "\n";
00286 
00287         out << std::endl;
00288     }
00289 }
00290 
00291 void Link::calcSubMassCM()
00292 {
00293     subm = m;
00294     submwc = m*wc;
00295     if (child){ 
00296         child->calcSubMassCM();
00297         subm += child->subm;
00298         submwc += child->submwc;
00299         Link *l = child->sibling;
00300         while (l){
00301             l->calcSubMassCM();
00302             subm += l->subm;
00303             submwc += l->submwc;
00304             l = l->sibling;
00305         }
00306     }
00307     /*
00308     std::cout << "calcSubMassCM() : " << name << ", subm = " << subm 
00309               << ", subCM = " << vector3(submwc/subm) << std::endl;
00310     */
00311 }
00312 
00313 void Link::calcSubMassInertia(Matrix33& subIw)
00314 {
00315       subIw = R*I*R.transpose();
00316       if (subm!=0.0) subIw +=  m*hat(wc - submwc/subm).transpose()*hat(wc - submwc/subm);
00317       if (child){
00318         Matrix33 childsubIw;
00319         child->calcSubMassInertia(childsubIw);
00320         subIw += childsubIw;
00321         if (child->subm!=0.0) subIw += child->subm*hat(child->submwc/child->subm - submwc/subm).transpose()*hat(child->submwc/child->subm - submwc/subm);
00322         Link *l = child->sibling;
00323         while (l){
00324           Matrix33 lsubIw;
00325           l->calcSubMassInertia(lsubIw);
00326           subIw += lsubIw;
00327           if (l->subm!=0.0) subIw += l->subm*hat(l->submwc/l->subm - submwc/subm).transpose()*hat(l->submwc/l->subm - submwc/subm);
00328           l = l->sibling;
00329         }
00330       }
00331 }


openhrp3
Author(s): AIST, General Robotix Inc., Nakamura Lab of Dept. of Mechano Informatics at University of Tokyo
autogenerated on Sun Apr 2 2017 03:43:55