Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
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
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 }