World.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  */
00014 #include <iostream>
00015 #include "World.h"
00016 #include "Link.h"
00017 #include "Sensor.h"
00018 #include "ForwardDynamicsABM.h"
00019 #include "ForwardDynamicsCBM.h"
00020 #include <string>
00021 
00022 using namespace std;
00023 using namespace hrp;
00024 
00025 static const double DEFAULT_GRAVITY_ACCELERATION = 9.80665;
00026 
00027 static const bool debugMode = false;
00028 
00029 
00030 WorldBase::WorldBase()
00031 {
00032     currentTime_ = 0.0;
00033     timeStep_ = 0.005;
00034 
00035     g << 0.0, 0.0, DEFAULT_GRAVITY_ACCELERATION;
00036 
00037     isEulerMethod =false;
00038     sensorsAreEnabled = false;
00039     numRegisteredLinkPairs = 0;
00040 }
00041 
00042 
00043 WorldBase::~WorldBase()
00044 {
00045 
00046 }
00047 
00048 
00049 int WorldBase::bodyIndex(const std::string& name)
00050 {
00051     NameToIndexMap::iterator p = nameToBodyIndexMap.find(name);
00052     return (p != nameToBodyIndexMap.end()) ? p->second : -1;
00053 }
00054 
00055 
00056 BodyPtr WorldBase::body(int index)
00057 {
00058     if(index < 0 || (int)bodyInfoArray.size() <= index)
00059         return BodyPtr();
00060 
00061     return bodyInfoArray[index].body; 
00062 }
00063 
00064 
00065 BodyPtr WorldBase::body(const std::string& name)
00066 {
00067     int idx = bodyIndex(name);
00068     if(idx < 0 || (int)bodyInfoArray.size() <= idx)
00069         return BodyPtr();
00070 
00071     return bodyInfoArray[idx].body;
00072 }
00073 
00074 
00075 void WorldBase::setTimeStep(double ts)
00076 {
00077     timeStep_ = ts;
00078 }
00079 
00080 
00081 void WorldBase::setCurrentTime(double time)
00082 {
00083     currentTime_ = time;
00084 }
00085 
00086 
00087 void WorldBase::setGravityAcceleration(const Vector3& g)
00088 {
00089     this->g = g;
00090 }
00091 
00092 
00093 void WorldBase::enableSensors(bool on)
00094 {
00095     sensorsAreEnabled = on;
00096 }
00097 
00098 
00099 void WorldBase::initialize()
00100 {
00101     const int n = bodyInfoArray.size();
00102 
00103     for(int i=0; i < n; ++i){
00104 
00105         BodyInfo& info = bodyInfoArray[i];
00106         BodyPtr body = info.body;
00107 
00108         bool hasHighGainModeJoints = false;
00109         int nL = body->numLinks();
00110         for(int j=0; j < nL; ++j){
00111             if(body->link(j)->isHighGainMode){
00112                 hasHighGainModeJoints = true;
00113                 break;
00114             }
00115         }
00116 
00117         if(hasHighGainModeJoints){
00118             info.forwardDynamics.reset(new ForwardDynamicsMM(body));
00119         } else {
00120             info.forwardDynamics.reset(new ForwardDynamicsABM(body));
00121         }
00122         if(isEulerMethod){
00123             info.forwardDynamics->setEulerMethod();
00124         } else {
00125             info.forwardDynamics->setRungeKuttaMethod();
00126         }
00127         info.forwardDynamics->setGravityAcceleration(g);
00128         info.forwardDynamics->setTimeStep(timeStep_);
00129         info.forwardDynamics->enableSensors(sensorsAreEnabled);
00130         info.forwardDynamics->initialize();
00131     }
00132 }
00133 
00134 
00135 void WorldBase::calcNextState()
00136 {
00137     if(debugMode){
00138         cout << "World current time = " << currentTime_ << endl;
00139     }
00140     const int n = bodyInfoArray.size();
00141 
00142 //#pragma omp parallel for num_threads(3) schedule(static)
00143 #pragma omp parallel for num_threads(3) schedule(dynamic)
00144     for(int i=0; i < n; ++i){
00145         BodyInfo& info = bodyInfoArray[i];
00146         info.forwardDynamics->calcNextState();
00147     }
00148     if (sensorsAreEnabled) updateRangeSensors();
00149     currentTime_ += timeStep_;
00150 }
00151 
00152 
00153 int WorldBase::addBody(BodyPtr body)
00154 {
00155     if(!body->name().empty()){
00156         nameToBodyIndexMap[body->name()] = bodyInfoArray.size();
00157     }
00158     BodyInfo info;
00159     info.body = body;
00160     bodyInfoArray.push_back(info);
00161 
00162     return bodyInfoArray.size() - 1;
00163 }
00164 
00165 
00166 void WorldBase::clearBodies()
00167 {
00168     nameToBodyIndexMap.clear();
00169     bodyInfoArray.clear();
00170 }
00171 
00172 
00173 void WorldBase::clearCollisionPairs()
00174 {
00175     linkPairKeyToIndexMap.clear();
00176     numRegisteredLinkPairs = 0;
00177 }
00178 
00179 
00180 void WorldBase::setEulerMethod()
00181 {
00182     isEulerMethod = true;
00183 }
00184 
00185 
00186 void WorldBase::setRungeKuttaMethod()
00187 {
00188     isEulerMethod = false;
00189 }
00190 
00191 
00192 std::pair<int,bool> WorldBase::getIndexOfLinkPairs(Link* link1, Link* link2)
00193 {
00194     int index = -1;
00195     int isRegistered = false;
00196 
00197     if(link1 != link2){
00198 
00199         LinkPairKey linkPair;
00200         if(link1 < link2){
00201             linkPair.link1 = link1;
00202             linkPair.link2 = link2;
00203         } else {
00204             linkPair.link1 = link2;
00205             linkPair.link2 = link1;
00206         }
00207 
00208         LinkPairKeyToIndexMap::iterator p = linkPairKeyToIndexMap.find(linkPair);
00209 
00210         if(p != linkPairKeyToIndexMap.end()){
00211             index = p->second;
00212             isRegistered = true;
00213         } else {
00214             index = numRegisteredLinkPairs++;
00215             linkPairKeyToIndexMap[linkPair] = index;
00216         }
00217     }
00218 
00219     return std::make_pair(index, isRegistered);
00220 }
00221 
00222 
00223 bool WorldBase::LinkPairKey::operator<(const LinkPairKey& pair2) const
00224 {
00225     if(link1 < pair2.link1){
00226         return true;
00227     } else if(link1 == pair2.link1){
00228         return (link2 < pair2.link2);
00229     } else {
00230         return false;
00231     }
00232 }
00233 
00234 void WorldBase::updateRangeSensors()
00235 {
00236     for (unsigned int bodyIndex = 0; bodyIndex<numBodies(); bodyIndex++){
00237         BodyPtr bodyptr = body(bodyIndex);
00238         int n = bodyptr->numSensors(Sensor::RANGE);
00239         for(int i=0; i < n; ++i){
00240             RangeSensor *s = bodyptr->sensor<RangeSensor>(i);
00241             if (s->isEnabled){
00242                 updateRangeSensor(s);
00243             }
00244         }
00245     }
00246 }
00247 
00248 void WorldBase::updateRangeSensor(RangeSensor *sensor)
00249 {
00250     if (currentTime() >= sensor->nextUpdateTime){
00251         Vector3 p(sensor->link->p + (sensor->link->R)*sensor->localPos);
00252         Matrix33 R(sensor->link->R*sensor->localR);
00253         int scan_half = (int)(sensor->scanAngle/2/sensor->scanStep);
00254         sensor->distances.resize(scan_half*2+1);
00255         double th;
00256         Vector3 v, dir;
00257         v[1] = 0.0;
00258         for (int i = -scan_half; i<= scan_half; i++){
00259             th = i*sensor->scanStep;
00260             v[0] = -sin(th); v[2] = -cos(th); 
00261             dir = R*v;
00262             double D, minD=0;
00263             for (unsigned int bodyIndex=0; bodyIndex<numBodies(); bodyIndex++){
00264                 BodyPtr bodyptr = body(bodyIndex);
00265                 for (unsigned int linkIndex=0; linkIndex<bodyptr->numLinks(); linkIndex++){
00266                     Link *link = bodyptr->link(linkIndex); 
00267                     if (link->coldetModel){
00268                         D = link->coldetModel->computeDistanceWithRay(p.data(), dir.data());
00269                         if ((minD==0&&D>0)||(minD>0&&D>0&&minD>D)) minD = D;
00270                     }
00271                 }
00272             }
00273             sensor->distances[i+scan_half] = minD;
00274         }
00275         sensor->nextUpdateTime += 1.0/sensor->scanRate;
00276         sensor->isUpdated = true;
00277     }
00278 }


openhrp3
Author(s): AIST, General Robotix Inc., Nakamura Lab of Dept. of Mechano Informatics at University of Tokyo
autogenerated on Thu Apr 11 2019 03:30:19