Body.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 
00015 #include "Body.h"
00016 #include "Link.h"
00017 #include "JointPath.h"
00018 #include "Sensor.h"
00019 #include "Light.h"
00020 #include "BodyCustomizerInterface.h"
00021 #include <hrpCollision/ColdetModel.h>
00022 #include <map>
00023 #include <cstdlib>
00024 
00025 using namespace hrp;
00026 using namespace std;
00027 
00028 static const bool PUT_DEBUG_MESSAGE = true;
00029 
00030 static bool pluginsInDefaultDirectoriesLoaded = false;
00031 
00032 #ifndef uint
00033 typedef unsigned int uint;
00034 #endif
00035 
00036 namespace hrp {
00037         
00038     class CustomizedJointPath : public JointPath
00039     {
00040         Body* body;
00041         int ikTypeId;
00042         bool isCustomizedIkPathReversed;
00043         virtual void onJointPathUpdated();
00044     public:
00045         CustomizedJointPath(Body* body, Link* baseLink, Link* targetLink);
00046         virtual ~CustomizedJointPath();
00047         virtual bool calcInverseKinematics(const Vector3& end_p, const Matrix33& end_R);
00048         virtual bool hasAnalyticalIK();
00049     };
00050 }
00051 
00052 
00053 Body::~Body()
00054 {
00055     if(customizerHandle){
00056         customizerInterface->destroy(customizerHandle);
00057     }
00058         
00059     delete rootLink_;
00060 
00061     // delete sensors
00062     for(unsigned int sensorType =0; sensorType < numSensorTypes(); ++sensorType){
00063         int n = numSensors(sensorType);
00064         for(int i=0; i < n; ++i){
00065             Sensor* s = sensor(sensorType, i);
00066             if(s){
00067                 Sensor::destroy(s);
00068             }
00069         }
00070     }
00071 }
00072 
00073 
00074 void Body::initialize()
00075 {
00076     rootLink_ = 0;
00077 
00078     customizerHandle = 0;
00079     customizerInterface = 0;
00080     bodyHandleEntity.body = this;
00081     bodyHandle = &bodyHandleEntity;
00082 }
00083         
00084 
00085 Body::Body()
00086   : allSensors(Sensor::NUM_SENSOR_TYPES)
00087 {
00088     initialize();
00089         
00090     rootLink_ = new Link;
00091     rootLink_->body = this;
00092 
00093     defaultRootPosition.setZero();
00094     defaultRootAttitude.setIdentity();
00095 }
00096 
00097 
00098 Body::Body(const Body& org)
00099     : name_(org.name_),
00100       modelName_(org.modelName_),
00101       allSensors(Sensor::NUM_SENSOR_TYPES)
00102 {
00103     initialize();
00104         
00105     setRootLink(new Link(*org.rootLink()));
00106 
00107     defaultRootPosition = org.defaultRootPosition;
00108     defaultRootAttitude = org.defaultRootAttitude;
00109         
00110     // copy sensors
00111     std::map<Link*, int> linkToIndexMap;
00112 
00113     for(unsigned int i=0; i < org.linkTraverse_.numLinks(); ++i){
00114         Link* lnk = org.linkTraverse_[i];
00115         linkToIndexMap[lnk] = i;
00116     }
00117 
00118     int n = org.numSensorTypes();
00119     for(int sensorType = 0; sensorType < n; ++sensorType){
00120         for(unsigned int i=0; i < org.numSensors(sensorType); ++i){
00121             Sensor* orgSensor = org.sensor(sensorType, i);
00122             if (orgSensor){
00123                 int linkIndex = linkToIndexMap[orgSensor->link];
00124                 Link* newLink = linkTraverse_[linkIndex];
00125                 Sensor* cloneSensor = createSensor(newLink, sensorType, orgSensor->id, orgSensor->name);
00126                 *cloneSensor = *orgSensor;
00127             }
00128         }
00129     }
00130 
00131     // do deep copy of extraJoint
00132         for(size_t i=0; i < org.extraJoints.size(); ++i){
00133                 const ExtraJoint& orgExtraJoint = org.extraJoints[i];
00134         ExtraJoint extraJoint(orgExtraJoint);
00135         for(int j=0; j < 2; ++j){
00136                         extraJoint.link[j] = link(orgExtraJoint.link[j]->index);
00137         }
00138     }
00139 
00140     if(org.customizerInterface){
00141         installCustomizer(org.customizerInterface);
00142     }
00143 }
00144 
00145 
00146 void Body::setRootLink(Link* link)
00147 {
00148     if(rootLink_){
00149         delete rootLink_;
00150     }
00151     rootLink_ = link;
00152 
00153     updateLinkTree();
00154 }
00155 
00156 
00157 void Body::setDefaultRootPosition(const Vector3& p, const Matrix33& R)
00158 {
00159     defaultRootPosition = p;
00160     defaultRootAttitude = R;
00161 }
00162 
00163 
00164 void Body::getDefaultRootPosition(Vector3& out_p, Matrix33& out_R)
00165 {
00166     out_p = defaultRootPosition;
00167     out_R = defaultRootAttitude;
00168 }
00169 
00170 
00171 Link* Body::createEmptyJoint(int jointId)
00172 {
00173     Link* empty = new Link;
00174     empty->body = this;
00175     empty->jointId = jointId;
00176     empty->p.setZero();
00177     empty->R.setIdentity();
00178     empty->v.setZero();
00179     empty->w.setZero();
00180     empty->dv.setZero();
00181     empty->dw.setZero();
00182     empty->q = 0.0;
00183     empty->dq = 0.0;
00184     empty->ddq = 0.0;
00185     empty->u = 0.0;
00186     empty->a.setZero();
00187     empty->d.setZero();
00188     empty->b.setZero();
00189     empty->Rs.setIdentity();
00190     empty->m = 0.0;
00191     empty->I.setZero();
00192     empty->c.setZero();
00193     empty->wc.setZero();
00194     empty->vo.setZero();
00195     empty->dvo.setZero();
00196     empty->fext.setZero();
00197     empty->tauext.setZero();
00198     empty->Jm2 = 0.0;
00199     empty->ulimit = 0.0;
00200     empty->llimit = 0.0;
00201     empty->uvlimit = 0.0;
00202     empty->lvlimit = 0.0;
00203     empty->defaultJointValue = 0.0;
00204     empty->Ir = 0.0;
00205     empty->gearRatio = 1.0;
00206 
00207     return empty;
00208 }
00209 
00210 
00211 void Body::updateLinkTree()
00212 {
00213     nameToLinkMap.clear();
00214     linkTraverse_.find(rootLink());
00215 
00216     int n = linkTraverse_.numLinks();
00217     jointIdToLinkArray.clear();
00218     jointIdToLinkArray.resize(n, 0);
00219     int maxJointID = -1;
00220     
00221     for(int i=0; i < n; ++i){
00222         Link* link = linkTraverse_[i];
00223         link->body = this;
00224         link->index = i;
00225         nameToLinkMap[link->name] = link;
00226 
00227         int id = link->jointId;
00228         if(id >= 0){
00229             int size = jointIdToLinkArray.size();
00230             if(id >= size){
00231                 jointIdToLinkArray.resize(id + 1, 0);
00232             }
00233             jointIdToLinkArray[id] = link;
00234             if(id > maxJointID){
00235                 maxJointID = id;
00236             }
00237         }
00238     }
00239 
00240     jointIdToLinkArray.resize(maxJointID + 1);
00241 
00242     for(size_t i=0; i < jointIdToLinkArray.size(); ++i){
00243         if(!jointIdToLinkArray[i]){
00244             jointIdToLinkArray[i] = createEmptyJoint(i);
00245             std::cerr << "Warning: Model " << modelName_ <<
00246                 " has empty joint ID in the valid IDs." << std::endl;
00247         }
00248     }
00249 
00250     calcTotalMass();
00251 
00252     isStaticModel_ = (rootLink_->jointType == Link::FIXED_JOINT && numJoints() == 0);
00253 }
00254 
00255 
00260 Link* Body::link(const std::string& name) const
00261 {
00262     NameToLinkMap::const_iterator p = nameToLinkMap.find(name);
00263     return (p != nameToLinkMap.end()) ? p->second : 0;
00264 }
00265 
00266 
00267 void Body::initializeConfiguration()
00268 {
00269     rootLink_->p = defaultRootPosition;
00270     rootLink_->setAttitude(defaultRootAttitude);
00271 
00272     rootLink_->v.setZero();
00273     rootLink_->dv.setZero();
00274     rootLink_->w.setZero();
00275     rootLink_->dw.setZero();
00276     rootLink_->vo.setZero();
00277     rootLink_->dvo.setZero();
00278     rootLink_->constraintForces.clear();
00279     
00280     int n = linkTraverse_.numLinks();
00281     for(int i=0; i < n; ++i){
00282         Link* link = linkTraverse_[i];
00283         link->u = 0.0;
00284         link->q = 0.0;
00285         link->dq = 0.0;
00286         link->ddq = 0.0;
00287         link->constraintForces.clear();
00288     }
00289  
00290     calcForwardKinematics(true, true);
00291 
00292     clearExternalForces();
00293 }
00294  
00295 
00296 
00297 double Body::calcTotalMass()
00298 {
00299     totalMass_ = 0.0;
00300 
00301     int n = linkTraverse_.numLinks();
00302     for(int i=0; i < n; ++i){
00303         totalMass_ += linkTraverse_[i]->m;
00304     }
00305 
00306     return totalMass_;
00307 }
00308 
00309 
00310 Vector3 Body::calcCM()
00311 {
00312     totalMass_ = 0.0;
00313     
00314     Vector3 mc(Vector3::Zero());
00315 
00316     int n = linkTraverse_.numLinks();
00317     for(int i=0; i < n; i++){
00318         Link* link = linkTraverse_[i];
00319         link->wc = link->p + link->R * link->c;
00320         mc += link->m * link->wc;
00321         totalMass_ += link->m;
00322     }
00323 
00324     return mc / totalMass_;
00325 }
00326 
00327 
00338 void Body::calcMassMatrix(dmatrix& out_M)
00339 {
00340     // buffers for the unit vector method
00341     dmatrix b1;
00342     dvector ddqorg;
00343     dvector uorg;
00344     Vector3 dvoorg;
00345     Vector3 dworg;
00346     Vector3 root_w_x_v;
00347     Vector3 g(0, 0, 9.8);
00348 
00349     uint nJ = numJoints();
00350     int totaldof = nJ;
00351     if( !isStaticModel_ ) totaldof += 6;
00352 
00353     out_M.resize(totaldof,totaldof);
00354     b1.resize(totaldof, 1);
00355 
00356     // preserve and clear the joint accelerations
00357     ddqorg.resize(nJ);
00358     uorg.resize(nJ);
00359     for(uint i = 0; i < nJ; ++i){
00360         Link* ptr = joint(i);
00361         ddqorg[i] = ptr->ddq;
00362         uorg  [i] = ptr->u;
00363         ptr->ddq = 0.0;
00364     }
00365 
00366     // preserve and clear the root link acceleration
00367     dvoorg = rootLink_->dvo;
00368     dworg  = rootLink_->dw;
00369     root_w_x_v = rootLink_->w.cross(rootLink_->vo + rootLink_->w.cross(rootLink_->p));
00370     rootLink_->dvo = g - root_w_x_v;   // dv = g, dw = 0
00371     rootLink_->dw.setZero();
00372         
00373     setColumnOfMassMatrix(b1, 0);
00374 
00375     if( !isStaticModel_ ){
00376         for(int i=0; i < 3; ++i){
00377             rootLink_->dvo[i] += 1.0;
00378             setColumnOfMassMatrix(out_M, i);
00379             rootLink_->dvo[i] -= 1.0;
00380         }
00381         for(int i=0; i < 3; ++i){
00382             rootLink_->dw[i] = 1.0;
00383             Vector3 dw_x_p = rootLink_->dw.cross(rootLink_->p); //  spatial acceleration caused by ang. acc.
00384             rootLink_->dvo -= dw_x_p;
00385             setColumnOfMassMatrix(out_M, i + 3);
00386             rootLink_->dvo += dw_x_p;
00387             rootLink_->dw[i] = 0.0;
00388         }
00389     }
00390 
00391     for(uint i = 0; i < nJ; ++i){
00392         Link* ptr = joint(i);
00393         ptr->ddq = 1.0;
00394         int j = i + 6;
00395         setColumnOfMassMatrix(out_M, j);
00396         out_M(j, j) += ptr->Jm2; // motor inertia
00397         ptr->ddq = 0.0;
00398     }
00399 
00400     // subtract the constant term
00401     for(int i = 0; i < out_M.cols(); ++i){
00402         out_M.col(i) -= b1;
00403     }
00404 
00405     // recover state
00406     for(uint i = 0; i < nJ; ++i){
00407         Link* ptr = joint(i);
00408         ptr->ddq  = ddqorg[i];
00409         ptr->u    = uorg  [i];
00410     }
00411     rootLink_->dvo = dvoorg;
00412     rootLink_->dw  = dworg;
00413 }
00414 
00415 
00416 void Body::setColumnOfMassMatrix(dmatrix& out_M, int column)
00417 {
00418     Vector3 f;
00419     Vector3 tau;
00420 
00421     calcInverseDynamics(rootLink_, f, tau);
00422 
00423     if( !isStaticModel_ ){
00424         tau -= rootLink_->p.cross(f);
00425         out_M.block<6,1>(0, column) << f, tau;
00426     }
00427 
00428     int n = numJoints();
00429     for(int i = 0; i < n; ++i){
00430         Link* ptr = joint(i);
00431         out_M(i + 6, column) = ptr->u;
00432     }
00433 }
00434 
00435 
00436 /*
00437  *  see Kajita et al. Humanoid Robot Ohm-sha,  p.210
00438  */
00439 void Body::calcInverseDynamics(Link* ptr, Vector3& out_f, Vector3& out_tau)
00440 {       
00441     Link* parent = ptr->parent;
00442     if(parent){
00443         Vector3 dsv,dsw,sv,sw;
00444 
00445         if(ptr->jointType != Link::FIXED_JOINT){
00446             sw.noalias()  = parent->R * ptr->a;
00447             sv  = ptr->p.cross(sw);
00448         }else{
00449             sw.setZero();
00450             sv.setZero();
00451         }
00452         dsv = parent->w.cross(sv) + parent->vo.cross(sw);
00453         dsw = parent->w.cross(sw);
00454 
00455         ptr->dw  = parent->dw  + dsw * ptr->dq + sw * ptr->ddq;
00456         ptr->dvo = parent->dvo + dsv * ptr->dq + sv * ptr->ddq;
00457 
00458         ptr->sw = sw;
00459         ptr->sv = sv;
00460     }
00461         
00462     Vector3  c,P,L;
00463     Matrix33 I,c_hat;
00464 
00465     c = ptr->R * ptr->c + ptr->p;
00466     I.noalias() = ptr->R * ptr->I * ptr->R.transpose();
00467     c_hat = hat(c);
00468     I.noalias() += ptr->m * c_hat * c_hat.transpose();
00469     P.noalias() = ptr->m * (ptr->vo + ptr->w.cross(c));
00470     L = ptr->m * c.cross(ptr->vo) + I * ptr->w;
00471 
00472     out_f   = ptr->m * (ptr->dvo + ptr->dw.cross(c)) + ptr->w.cross(P);
00473     out_tau = ptr->m * c.cross(ptr->dvo) + I * ptr->dw + ptr->vo.cross(P) + ptr->w.cross(L);
00474 
00475     if(ptr->child){
00476         Vector3 f_c;
00477         Vector3 tau_c;
00478         calcInverseDynamics(ptr->child, f_c, tau_c);
00479         out_f   += f_c;
00480         out_tau += tau_c;
00481     }
00482 
00483     ptr->u = ptr->sv.dot(out_f) + ptr->sw.dot(out_tau);
00484 
00485     if(ptr->sibling){
00486         Vector3 f_s;
00487         Vector3 tau_s;
00488         calcInverseDynamics(ptr->sibling, f_s, tau_s);
00489         out_f   += f_s;
00490         out_tau += tau_s;
00491     }
00492 }
00493 
00494 
00499 void Body::calcTotalMomentum(Vector3& out_P, Vector3& out_L)
00500 {
00501     out_P.setZero();
00502     out_L.setZero();
00503 
00504     Vector3 dwc;        // Center of mass speed in world frame
00505     Vector3 P;          // Linear momentum of the link
00506     Vector3 L;          // Angular momentum with respect to the world frame origin 
00507     Vector3 Llocal; // Angular momentum with respect to the center of mass of the link
00508 
00509     int n = linkTraverse_.numLinks();
00510     for(int i=0; i < n; i++){
00511         Link* link = linkTraverse_[i];
00512 
00513         dwc = link->v + link->w.cross(link->R * link->c);
00514 
00515         P   = link->m * dwc;
00516 
00517         //L   = cross(link->wc, P) + link->R * link->I * trans(link->R) * link->w; 
00518         Llocal.noalias() = link->I * link->R.transpose()*link->w;
00519         L                = link->wc.cross(P) + link->R * Llocal; 
00520 
00521         out_P += P;
00522         out_L += L;
00523     }
00524 }
00525 
00526 void Body::calcTotalMomentumFromJacobian(Vector3& out_P, Vector3& out_L)
00527 {
00528     out_P.setZero();
00529     out_L.setZero();
00530 
00531     dmatrix J,H;
00532     calcCMJacobian(NULL,J);
00533     calcAngularMomentumJacobian(NULL,H);
00534 
00535     dvector dq;
00536     int n = numJoints();
00537     dq.resize(n);
00538     for(int i=0; i < n; i++){
00539       Link* link = joint(i);
00540       dq[i] = link->dq;
00541     }
00542     dvector v;
00543     v.resize(n+3+3);
00544     v << dq, rootLink_->v, rootLink_->w;
00545 
00546     out_P = totalMass_ * J * v;
00547     out_L = H * v;
00548 }
00549 
00550 void Body::calcForwardKinematics(bool calcVelocity, bool calcAcceleration)
00551 {
00552     linkTraverse_.calcForwardKinematics(calcVelocity, calcAcceleration);
00553 }
00554 
00555 
00556 Light* Body::createLight(Link* link, int lightType, const std::string& name)
00557 {
00558     Light *light =  new Light(link, lightType, name);
00559     nameToLightMap[name] = light;
00560     return light;
00561 }
00562 
00563 Sensor* Body::createSensor(Link* link, int sensorType, int id, const std::string& name)
00564 {
00565     Sensor* sensor = 0;
00566 
00567     if(sensorType < Sensor::NUM_SENSOR_TYPES && id >= 0){
00568 
00569         SensorArray& sensors = allSensors[sensorType];
00570         int n = sensors.size();
00571         if(id >= n){
00572             sensors.resize(id + 1, 0);
00573         }
00574         sensor = sensors[id];
00575         if(sensor){
00576             std::cerr << "duplicated sensor Id is specified(id = "
00577                       << id << ", name = " << name << ")" << std::endl;
00578                 
00579             nameToSensorMap.erase(sensor->name);
00580         } else {
00581             sensor = Sensor::create(sensorType);
00582         }
00583         if(sensor){
00584             sensor->id = id;
00585             sensors[id] = sensor;
00586             sensor->link = link;
00587             sensor->name = name;
00588             nameToSensorMap[name] = sensor;
00589             link->sensors.push_back(sensor);
00590         }
00591     }
00592                 
00593     return sensor;
00594 }
00595 
00596 void Body::addSensor(Sensor* sensor, int sensorType, int id ){
00597     if(sensorType < Sensor::NUM_SENSOR_TYPES && id >= 0){
00598         SensorArray& sensors = allSensors[sensorType];
00599         int n = sensors.size();
00600         if(id >= n){
00601             sensors.resize(id + 1, 0);
00602         }
00603         Sensor* sameId = sensors[id];
00604         if(sameId){
00605             std::cerr << "duplicated sensor Id is specified(id = "
00606                       << id << ", name = " << sensor->name << ")" << std::endl;
00607                 
00608             nameToSensorMap.erase(sameId->name);
00609         }
00610         sensors[id] = sensor;
00611         nameToSensorMap[sensor->name] = sensor;
00612     }
00613 }
00614 
00615 
00616 void Body::clearSensorValues()
00617 {
00618     for(unsigned int i=0; i < numSensorTypes(); ++i){
00619         for(unsigned int j=0; j < numSensors(i); ++j){
00620             if(sensor(i,j))
00621                 sensor(i, j)->clear();
00622         }
00623     }
00624 }
00625 
00626 
00627 JointPathPtr Body::getJointPath(Link* baseLink, Link* targetLink)
00628 {
00629     if(customizerInterface && customizerInterface->initializeAnalyticIk){
00630         return JointPathPtr(new CustomizedJointPath(this, baseLink, targetLink));
00631     } else {
00632         return JointPathPtr(new JointPath(baseLink, targetLink));
00633     }
00634 }
00635 
00636 
00637 void Body::setVirtualJointForcesSub()
00638 {
00639     if(customizerInterface->setVirtualJointForces){
00640         customizerInterface->setVirtualJointForces(customizerHandle);
00641     }
00642 }
00643 
00644 
00645 void Body::clearExternalForces()
00646 {
00647     int n = linkTraverse_.numLinks();
00648     for(int i=0; i < n; ++i){
00649         Link* link = linkTraverse_[i];
00650         link->fext.setZero();
00651         link->tauext.setZero();
00652     }
00653 }
00654 
00655 
00656 void Body::updateLinkColdetModelPositions()
00657 {
00658     const int n = linkTraverse_.numLinks();
00659     for(int i=0; i < n; ++i){
00660         Link* link = linkTraverse_[i];
00661         if(link->coldetModel){
00662             link->coldetModel->setPosition(link->segmentAttitude(), link->p);
00663         }
00664     }
00665 }
00666 
00667 
00668 void Body::putInformation(std::ostream &out)
00669 {
00670     out << "Body: model name = " << modelName_ << " name = " << name_ << "\n\n";
00671 
00672     int n = numLinks();
00673     for(int i=0; i < n; ++i){
00674         out << *link(i);
00675     }
00676     out << std::endl;
00677 }
00678 
00679 
00683 bool Body::installCustomizer()
00684 {
00685     if(!pluginsInDefaultDirectoriesLoaded){
00686         loadBodyCustomizers(bodyInterface());
00687         pluginsInDefaultDirectoriesLoaded = true;
00688     }
00689                 
00690     BodyCustomizerInterface* interface = findBodyCustomizer(modelName_);
00691 
00692     return interface ? installCustomizer(interface) : false;
00693 }
00694 
00695 
00696 bool Body::installCustomizer(BodyCustomizerInterface * customizerInterface)
00697 {
00698     if(this->customizerInterface){
00699         if(customizerHandle){
00700             this->customizerInterface->destroy(customizerHandle);
00701             customizerHandle = 0;
00702         }
00703         this->customizerInterface = 0;
00704     }
00705         
00706     if(customizerInterface){
00707         customizerHandle = customizerInterface->create(bodyHandle, modelName_.c_str());
00708         if(customizerHandle){
00709             this->customizerInterface = customizerInterface;
00710         }
00711     }
00712 
00713     return (customizerHandle != 0);
00714 }
00715 
00716 
00717 std::ostream& operator<< (std::ostream& out, Body& body)
00718 {
00719     body.putInformation(out);
00720     return out;
00721 }
00722 
00723 
00724 static inline Link* extractLink(BodyHandle bodyHandle, int linkIndex)
00725 {
00726     return static_cast<BodyHandleEntity*>(bodyHandle)->body->link(linkIndex);
00727 }
00728 
00729 
00730 static int getLinkIndexFromName(BodyHandle bodyHandle, const char* linkName)
00731 {
00732     Body* body = static_cast<BodyHandleEntity*>(bodyHandle)->body;
00733     Link* link = body->link(linkName);
00734     return (link ? link->index : -1);
00735 }
00736 
00737 
00738 static const char* getLinkName(BodyHandle bodyHandle, int linkIndex)
00739 {
00740     return extractLink(bodyHandle, linkIndex)->name.c_str();
00741 }
00742 
00743 
00744 static double* getJointValuePtr(BodyHandle bodyHandle, int linkIndex)
00745 {
00746     return &(extractLink(bodyHandle,linkIndex)->q);
00747 }
00748 
00749 
00750 static double* getJointVelocityPtr(BodyHandle bodyHandle, int linkIndex)
00751 {
00752     return &(extractLink(bodyHandle, linkIndex)->dq);
00753 }
00754 
00755 
00756 static double* getJointTorqueForcePtr(BodyHandle bodyHandle, int linkIndex)
00757 {
00758     return &(extractLink(bodyHandle, linkIndex)->u);
00759 }
00760 
00761 
00762 BodyInterface* Body::bodyInterface()
00763 {
00764     static BodyInterface interface = {
00765         hrp::BODY_INTERFACE_VERSION,
00766         getLinkIndexFromName,
00767         getLinkName,
00768         getJointValuePtr,
00769         getJointVelocityPtr,
00770         getJointTorqueForcePtr,
00771     };
00772 
00773     return &interface;
00774 }
00775 
00776 
00777 CustomizedJointPath::CustomizedJointPath(Body* body, Link* baseLink, Link* targetLink) :
00778     JointPath(baseLink, targetLink),
00779     body(body)
00780 {
00781     onJointPathUpdated();
00782 }
00783 
00784 
00785 CustomizedJointPath::~CustomizedJointPath()
00786 {
00787 
00788 }
00789 
00790 
00791 void CustomizedJointPath::onJointPathUpdated()
00792 {
00793     ikTypeId = body->customizerInterface->initializeAnalyticIk(
00794         body->customizerHandle, baseLink()->index, endLink()->index);
00795     if(ikTypeId){
00796         isCustomizedIkPathReversed = false;
00797     } else {
00798         // try reversed path
00799         ikTypeId = body->customizerInterface->initializeAnalyticIk(
00800             body->customizerHandle, endLink()->index, baseLink()->index);
00801         if(ikTypeId){
00802             isCustomizedIkPathReversed = true;
00803         }
00804     }
00805 }
00806 
00807 
00808 bool CustomizedJointPath::calcInverseKinematics(const Vector3& end_p, const Matrix33& end_R)
00809 {
00810     bool solved;
00811         
00812     if(ikTypeId == 0 || isBestEffortIKMode){
00813 
00814         solved = JointPath::calcInverseKinematics(end_p, end_R);
00815 
00816     } else {
00817 
00818         std::vector<double> qorg(numJoints());
00819                 
00820         for(unsigned int i=0; i < numJoints(); ++i){
00821             qorg[i] = joint(i)->q;
00822         }
00823 
00824         const Link* targetLink = endLink();
00825         const Link* baseLink_ = baseLink();
00826 
00827         Vector3 p_relative;
00828         Matrix33 R_relative;
00829         if(!isCustomizedIkPathReversed){
00830             p_relative.noalias() = baseLink_->R.transpose() * (end_p - baseLink_->p);
00831             R_relative.noalias() = baseLink_->R.transpose() * end_R;
00832         } else {
00833             p_relative.noalias() = end_R.transpose() * (baseLink_->p - end_p);
00834             R_relative.noalias() = end_R.transpose() * baseLink_->R;
00835         }
00836         solved = body->customizerInterface->
00837             calcAnalyticIk(body->customizerHandle, ikTypeId, p_relative, R_relative);
00838 
00839         if(solved){
00840 
00841             calcForwardKinematics();
00842 
00843             Vector3 dp(end_p - targetLink->p);
00844             Vector3 omega(omegaFromRot((targetLink->R*targetLink->Rs).transpose() * end_R));
00845                         
00846             double errsqr = dp.dot(dp) + omega.dot(omega);
00847                         
00848             if(errsqr < maxIKErrorSqr){
00849                 solved = true;
00850             } else {
00851                 solved = false;
00852                 for(unsigned int i=0; i < numJoints(); ++i){
00853                     joint(i)->q = qorg[i];
00854                 }
00855                 calcForwardKinematics();
00856             }
00857         }
00858     }
00859 
00860     return solved;
00861 }
00862 
00863 
00864 bool CustomizedJointPath::hasAnalyticalIK()
00865 {
00866     return (ikTypeId != 0);
00867 }
00868 
00869 void Body::calcCMJacobian(Link *base, dmatrix &J)
00870 {
00871     // prepare subm, submwc
00872     JointPathPtr jp;
00873     if (base){
00874         jp = getJointPath(rootLink(), base);
00875         Link *skip = jp->joint(0);
00876         skip->subm = rootLink()->m;
00877         skip->submwc = rootLink()->m*rootLink()->wc;
00878         Link *l = rootLink()->child;
00879         if (l){
00880             if (l != skip) {
00881                 l->calcSubMassCM();
00882                 skip->subm += l->subm;
00883                 skip->submwc += l->submwc;
00884             }
00885             l = l->sibling;
00886             while(l){
00887                 if (l != skip){
00888                     l->calcSubMassCM();
00889                     skip->subm += l->subm;
00890                     skip->submwc += l->submwc;
00891                 }
00892                 l = l->sibling;
00893             }
00894         }
00895         
00896         // assuming there is no branch between base and root
00897         for (unsigned int i=1; i<jp->numJoints(); i++){
00898             l = jp->joint(i);
00899             l->subm = l->parent->m + l->parent->subm;
00900             l->submwc = l->parent->m*l->parent->wc + l->parent->submwc;
00901         }
00902         
00903         J.resize(3, numJoints());
00904     }else{
00905         rootLink()->calcSubMassCM();
00906         J.resize(3, numJoints()+6);
00907     }
00908     
00909     // compute Jacobian
00910     std::vector<int> sgn(numJoints(), 1);
00911     if (jp) {
00912         for (unsigned int i=0; i<jp->numJoints(); i++) sgn[jp->joint(i)->jointId] = -1;
00913     }
00914     
00915     for (unsigned int i=0; i<numJoints(); i++){
00916         Link *j = joint(i);
00917         switch(j->jointType){
00918         case Link::ROTATIONAL_JOINT:
00919         {
00920             Vector3 omega(sgn[j->jointId]*j->R*j->a);
00921             Vector3 arm((j->submwc-j->subm*j->p)/totalMass_);
00922             Vector3 dp(omega.cross(arm));
00923             J.col(j->jointId) = dp;
00924             break;
00925         }
00926         case Link::SLIDE_JOINT:
00927         {
00928           Vector3 dp((j->subm/totalMass_)*sgn[j->jointId]*j->R*j->d);
00929           J.col(j->jointId) = dp;
00930           break;
00931         }
00932         default:
00933             std::cerr << "calcCMJacobian() : unsupported jointType("
00934                       << j->jointType << ")" << std::endl;
00935         }
00936     }
00937     if (!base){
00938         int c = numJoints();
00939         J(0, c  ) = 1.0; J(0, c+1) = 0.0; J(0, c+2) = 0.0;
00940         J(1, c  ) = 0.0; J(1, c+1) = 1.0; J(1, c+2) = 0.0;
00941         J(2, c  ) = 0.0; J(2, c+1) = 0.0; J(2, c+2) = 1.0;
00942 
00943         Vector3 dp(rootLink()->submwc/totalMass_ - rootLink()->p);
00944         J(0, c+3) =    0.0; J(0, c+4) =  dp(2); J(0, c+5) = -dp(1);
00945         J(1, c+3) = -dp(2); J(1, c+4) =    0.0; J(1, c+5) =  dp(0);
00946         J(2, c+3) =  dp(1); J(2, c+4) = -dp(0); J(2, c+5) =    0.0;
00947     }
00948 }
00949 
00950 void Body::calcAngularMomentumJacobian(Link *base, dmatrix &H)
00951 {
00952     // prepare subm, submwc
00953     JointPathPtr jp;
00954 
00955     dmatrix M;
00956     calcCMJacobian(base, M);
00957     M.conservativeResize(3, numJoints());
00958     M *= totalMass();
00959 
00960     if (base){
00961         jp = getJointPath(rootLink(), base);
00962         Link *skip = jp->joint(0);
00963         skip->subm = rootLink()->m;
00964         skip->submwc = rootLink()->m*rootLink()->wc;
00965         Link *l = rootLink()->child;
00966         if (l){
00967             if (l != skip) {
00968                 l->calcSubMassCM();
00969                 skip->subm += l->subm;
00970                 skip->submwc += l->submwc;
00971             }
00972             l = l->sibling;
00973             while(l){
00974                 if (l != skip){
00975                     l->calcSubMassCM();
00976                     skip->subm += l->subm;
00977                     skip->submwc += l->submwc;
00978                 }
00979                 l = l->sibling;
00980             }
00981         }
00982         
00983         // assuming there is no branch between base and root
00984         for (unsigned int i=1; i<jp->numJoints(); i++){
00985             l = jp->joint(i);
00986             l->subm = l->parent->m + l->parent->subm;
00987             l->submwc = l->parent->m*l->parent->wc + l->parent->submwc;
00988         }
00989         
00990         H.resize(3, numJoints());
00991     }else{
00992         rootLink()->calcSubMassCM();
00993         H.resize(3, numJoints()+6);
00994     }
00995     
00996     // compute Jacobian
00997     std::vector<int> sgn(numJoints(), 1);
00998     if (jp) {
00999         for (unsigned int i=0; i<jp->numJoints(); i++) sgn[jp->joint(i)->jointId] = -1;
01000     }
01001     
01002     for (unsigned int i=0; i<numJoints(); i++){
01003         Link *j = joint(i);
01004         switch(j->jointType){
01005         case Link::ROTATIONAL_JOINT:
01006         {
01007             Vector3 omega(sgn[j->jointId]*j->R*j->a);
01008             Vector3 Mcol = M.col(j->jointId);
01009             Matrix33 jsubIw;
01010             j->calcSubMassInertia(jsubIw);
01011             Vector3 dp = jsubIw*omega;
01012             if (j->subm!=0) dp += (j->submwc/j->subm).cross(Mcol);
01013             H.col(j->jointId) = dp;
01014             break;
01015         }
01016         case Link::SLIDE_JOINT:
01017         {
01018           if(j->subm!=0){
01019             Vector3 Mcol =M.col(j->jointId);
01020             Vector3 dp = (j->submwc/j->subm).cross(Mcol);
01021             H.col(j->jointId) = dp;
01022           }
01023           break;
01024         }
01025         default:
01026             std::cerr << "calcCMJacobian() : unsupported jointType("
01027                       << j->jointType << ")" << std::endl;
01028         }
01029     }
01030     if (!base){
01031         int c = numJoints();
01032         H.block(0, c, 3, 3).setZero();
01033         Matrix33 Iw;
01034         rootLink_->calcSubMassInertia(Iw);
01035         H.block(0, c+3, 3, 3) = Iw;
01036         Vector3 cm = calcCM();
01037         Matrix33 cm_cross;
01038         cm_cross <<
01039           0.0, -cm(2), cm(1),
01040           cm(2), 0.0, -cm(0),
01041           -cm(1), cm(0), 0.0;
01042         H.block(0,0,3,c) -= cm_cross * M;
01043     }
01044 }


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:52