hrplib/hrpModel/World.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2008, AIST, the University of Tokyo and General Robotix Inc.
3  * All rights reserved. This program is made available under the terms of the
4  * Eclipse Public License v1.0 which accompanies this distribution, and is
5  * available at http://www.eclipse.org/legal/epl-v10.html
6  * Contributors:
7  * National Institute of Advanced Industrial Science and Technology (AIST)
8  */
14 #include <iostream>
15 #include "World.h"
16 #include "Link.h"
17 #include "Sensor.h"
18 #include "ForwardDynamicsABM.h"
19 #include "ForwardDynamicsCBM.h"
20 #include <string>
21 
22 using namespace std;
23 using namespace hrp;
24 
25 static const double DEFAULT_GRAVITY_ACCELERATION = 9.80665;
26 
27 static const bool debugMode = false;
28 
29 
30 WorldBase::WorldBase()
31 {
32  currentTime_ = 0.0;
33  timeStep_ = 0.005;
34 
35  g << 0.0, 0.0, DEFAULT_GRAVITY_ACCELERATION;
36 
37  isEulerMethod =false;
38  sensorsAreEnabled = false;
39  numRegisteredLinkPairs = 0;
40 }
41 
42 
43 WorldBase::~WorldBase()
44 {
45 
46 }
47 
48 
49 int WorldBase::bodyIndex(const std::string& name)
50 {
51  NameToIndexMap::iterator p = nameToBodyIndexMap.find(name);
52  return (p != nameToBodyIndexMap.end()) ? p->second : -1;
53 }
54 
55 
56 BodyPtr WorldBase::body(int index)
57 {
58  if(index < 0 || (int)bodyInfoArray.size() <= index)
59  return BodyPtr();
60 
61  return bodyInfoArray[index].body;
62 }
63 
64 
65 BodyPtr WorldBase::body(const std::string& name)
66 {
67  int idx = bodyIndex(name);
68  if(idx < 0 || (int)bodyInfoArray.size() <= idx)
69  return BodyPtr();
70 
71  return bodyInfoArray[idx].body;
72 }
73 
74 
75 void WorldBase::setTimeStep(double ts)
76 {
77  timeStep_ = ts;
78 }
79 
80 
81 void WorldBase::setCurrentTime(double time)
82 {
83  currentTime_ = time;
84 }
85 
86 
87 void WorldBase::setGravityAcceleration(const Vector3& g)
88 {
89  this->g = g;
90 }
91 
92 
93 void WorldBase::enableSensors(bool on)
94 {
95  sensorsAreEnabled = on;
96 }
97 
98 
99 void WorldBase::initialize()
100 {
101  const int n = bodyInfoArray.size();
102 
103  for(int i=0; i < n; ++i){
104 
105  BodyInfo& info = bodyInfoArray[i];
106  BodyPtr body = info.body;
107 
108  bool hasHighGainModeJoints = false;
109  int nL = body->numLinks();
110  for(int j=0; j < nL; ++j){
111  if(body->link(j)->isHighGainMode){
112  hasHighGainModeJoints = true;
113  break;
114  }
115  }
116 
117  if(hasHighGainModeJoints){
118  info.forwardDynamics.reset(new ForwardDynamicsMM(body));
119  } else {
120  info.forwardDynamics.reset(new ForwardDynamicsABM(body));
121  }
122  if(isEulerMethod){
123  info.forwardDynamics->setEulerMethod();
124  } else {
125  info.forwardDynamics->setRungeKuttaMethod();
126  }
127  info.forwardDynamics->setGravityAcceleration(g);
128  info.forwardDynamics->setTimeStep(timeStep_);
129  info.forwardDynamics->enableSensors(sensorsAreEnabled);
130  info.forwardDynamics->initialize();
131  }
132 }
133 
134 
135 void WorldBase::calcNextState()
136 {
137  if(debugMode){
138  cout << "World current time = " << currentTime_ << endl;
139  }
140  const int n = bodyInfoArray.size();
141 
142 //#pragma omp parallel for num_threads(3) schedule(static)
143 #pragma omp parallel for num_threads(3) schedule(dynamic)
144  for(int i=0; i < n; ++i){
145  BodyInfo& info = bodyInfoArray[i];
146  info.forwardDynamics->calcNextState();
147  }
148  if (sensorsAreEnabled) updateRangeSensors();
149  currentTime_ += timeStep_;
150 }
151 
152 
153 int WorldBase::addBody(BodyPtr body)
154 {
155  if(!body->name().empty()){
156  nameToBodyIndexMap[body->name()] = bodyInfoArray.size();
157  }
158  BodyInfo info;
159  info.body = body;
160  bodyInfoArray.push_back(info);
161 
162  return bodyInfoArray.size() - 1;
163 }
164 
165 
166 void WorldBase::clearBodies()
167 {
168  nameToBodyIndexMap.clear();
169  bodyInfoArray.clear();
170 }
171 
172 
173 void WorldBase::clearCollisionPairs()
174 {
175  linkPairKeyToIndexMap.clear();
176  numRegisteredLinkPairs = 0;
177 }
178 
179 
180 void WorldBase::setEulerMethod()
181 {
182  isEulerMethod = true;
183 }
184 
185 
186 void WorldBase::setRungeKuttaMethod()
187 {
188  isEulerMethod = false;
189 }
190 
191 
192 std::pair<int,bool> WorldBase::getIndexOfLinkPairs(Link* link1, Link* link2)
193 {
194  int index = -1;
195  int isRegistered = false;
196 
197  if(link1 != link2){
198 
199  LinkPairKey linkPair;
200  if(link1 < link2){
201  linkPair.link1 = link1;
202  linkPair.link2 = link2;
203  } else {
204  linkPair.link1 = link2;
205  linkPair.link2 = link1;
206  }
207 
208  LinkPairKeyToIndexMap::iterator p = linkPairKeyToIndexMap.find(linkPair);
209 
210  if(p != linkPairKeyToIndexMap.end()){
211  index = p->second;
212  isRegistered = true;
213  } else {
214  index = numRegisteredLinkPairs++;
215  linkPairKeyToIndexMap[linkPair] = index;
216  }
217  }
218 
219  return std::make_pair(index, isRegistered);
220 }
221 
222 
223 bool WorldBase::LinkPairKey::operator<(const LinkPairKey& pair2) const
224 {
225  if(link1 < pair2.link1){
226  return true;
227  } else if(link1 == pair2.link1){
228  return (link2 < pair2.link2);
229  } else {
230  return false;
231  }
232 }
233 
234 void WorldBase::updateRangeSensors()
235 {
236  for (unsigned int bodyIndex = 0; bodyIndex<numBodies(); bodyIndex++){
237  BodyPtr bodyptr = body(bodyIndex);
238  int n = bodyptr->numSensors(Sensor::RANGE);
239  for(int i=0; i < n; ++i){
240  RangeSensor *s = bodyptr->sensor<RangeSensor>(i);
241  if (s->isEnabled){
242  updateRangeSensor(s);
243  }
244  }
245  }
246 }
247 
248 void WorldBase::updateRangeSensor(RangeSensor *sensor)
249 {
250  if (currentTime() >= sensor->nextUpdateTime){
251  Vector3 p(sensor->link->p + (sensor->link->R)*sensor->localPos);
252  Matrix33 R(sensor->link->R*sensor->localR);
253  int scan_half = (int)(sensor->scanAngle/2/sensor->scanStep);
254  sensor->distances.resize(scan_half*2+1);
255  double th;
256  Vector3 v, dir;
257  v[1] = 0.0;
258  for (int i = -scan_half; i<= scan_half; i++){
259  th = i*sensor->scanStep;
260  v[0] = -sin(th); v[2] = -cos(th);
261  dir = R*v;
262  double D, minD=0;
263  for (unsigned int bodyIndex=0; bodyIndex<numBodies(); bodyIndex++){
264  BodyPtr bodyptr = body(bodyIndex);
265  for (unsigned int linkIndex=0; linkIndex<bodyptr->numLinks(); linkIndex++){
266  Link *link = bodyptr->link(linkIndex);
267  if (link->coldetModel){
268  D = link->coldetModel->computeDistanceWithRay(p.data(), dir.data());
269  if ((minD==0&&D>0)||(minD>0&&D>0&&minD>D)) minD = D;
270  }
271  }
272  }
273  sensor->distances[i+scan_half] = minD;
274  }
275  sensor->nextUpdateTime += 1.0/sensor->scanRate;
276  sensor->isUpdated = true;
277  }
278 }
std::vector< double > distances
ForwardDynamicsPtr forwardDynamics
static const double DEFAULT_GRAVITY_ACCELERATION
png_infop png_charpp name
Definition: png.h:2382
png_uint_32 i
Definition: png.h:2735
boost::shared_ptr< Body > BodyPtr
Definition: Body.h:315
Eigen::Vector3d Vector3
Definition: EigenTypes.h:11
Eigen::Matrix3d Matrix33
Definition: EigenTypes.h:12
static const bool debugMode
list index
typedef int
Definition: png.h:1113
backing_store_ptr info
Definition: jmemsys.h:181


openhrp3
Author(s): AIST, General Robotix Inc., Nakamura Lab of Dept. of Mechano Informatics at University of Tokyo
autogenerated on Thu Sep 8 2022 02:24:06