util/simulator/Simulator.cpp
Go to the documentation of this file.
1 #include "Simulator.h"
2 #include "hrpsys/util/BodyRTC.h"
3 
5  : log(i_log), adjustTime(false)
6 {
7 }
8 
9 void Simulator::init(Project &prj, BodyFactory &factory){
10  initWorld(prj, factory, *this, pairs);
11  initRTS(prj, receivers);
12  std::cout << "number of receivers:" << receivers.size() << std::endl;
13  m_totalTime = prj.totalTime();
14  m_logTimeStep = prj.logTimeStep();
16  realTime(prj.realTime());
17 
18  collisions.length(pairs.size());
19  for(size_t colIndex=0; colIndex < pairs.size(); ++colIndex){
20  hrp::ColdetLinkPairPtr linkPair = pairs[colIndex];
21  hrp::Link *link0 = linkPair->link(0);
22  hrp::Link *link1 = linkPair->link(1);
23  OpenHRP::LinkPair& pair = collisions[colIndex].pair;
24  pair.charName1 = CORBA::string_dup(link0->body->name().c_str());
25  pair.charName2 = CORBA::string_dup(link1->body->name().c_str());
26  pair.linkName1 = CORBA::string_dup(link0->name.c_str());
27  pair.linkName2 = CORBA::string_dup(link1->name.c_str());
28  }
29 
30  m_nextLogTime = 0;
31  appendLog();
32 }
33 
35 {
36  if (log && currentTime() >= m_nextLogTime){
37  state.set(*this, collisions);
38  log->add(state);
40  }
41 }
42 
44 {
46 }
47 
48 void Simulator::checkCollision(OpenHRP::CollisionSequence &collisions)
49 {
50  for (unsigned int i=0; i<numBodies(); i++){
51  body(i)->updateLinkColdetModelPositions();
52  }
53  for(size_t colIndex=0; colIndex < pairs.size(); ++colIndex){
54  hrp::ColdetLinkPairPtr linkPair = pairs[colIndex];
55  OpenHRP::Collision& collision = collisions[colIndex];
56  OpenHRP::CollisionPointSequence* pCollisionPoints = &collision.points;
57  std::vector<hrp::collision_data>& cdata = linkPair->detectCollisions();
58 
59  if(cdata.empty()){
60  pCollisionPoints->length(0);
61  } else {
62  int npoints = 0;
63  for(unsigned int i = 0; i < cdata.size(); i++) {
64  for(int j = 0; j < cdata[i].num_of_i_points; j++){
65  if(cdata[i].i_point_new[j]) npoints++;
66  }
67  }
68  pCollisionPoints->length(npoints);
69  int idx = 0;
70  for (unsigned int i = 0; i < cdata.size(); i++) {
71  hrp::collision_data& cd = cdata[i];
72  for(int j=0; j < cd.num_of_i_points; j++){
73  if (cd.i_point_new[j]){
74  OpenHRP::CollisionPoint& point = (*pCollisionPoints)[idx];
75  for(int k=0; k < 3; k++){
76  point.position[k] = cd.i_points[j][k];
77  }
78  for(int k=0; k < 3; k++){
79  point.normal[k] = cd.n_vector[k];
80  }
81  point.idepth = cd.depth;
82  idx++;
83  }
84  }
85  }
86  }
87  }
88 }
89 
92 
93  if (!currentTime()) gettimeofday(&beginTime, NULL);
94  if (adjustTime){
95  struct timeval tv;
96  gettimeofday(&tv, NULL);
97  startTimes.push_back(tv);
98  if (startTimes.size() > 1.0/timeStep()){
99  startTimes.pop_front();
100  }
101  if (startTimes.size() >= 2){
102  const struct timeval& first = startTimes.front();
103  const struct timeval& last = startTimes.back();
104  int realT = (last.tv_sec - first.tv_sec)*1e6
105  + (last.tv_usec - first.tv_usec);
106  int simT = timeStep()*(startTimes.size()-1)*1e6;
107  int usec = simT - realT;
108  if (usec > 1000){
109  usleep(usec);
110  }
111  }
112  }
113 
114  tm_control.begin();
115  for (unsigned int i=0; i<numBodies(); i++){
116  BodyRTC *bodyrtc = dynamic_cast<BodyRTC *>(body(i).get());
117  bodyrtc->writeDataPorts(currentTime());
118  }
119 
120  for (unsigned int i=0; i<numBodies(); i++){
121  BodyRTC *bodyrtc = dynamic_cast<BodyRTC *>(body(i).get());
122  bodyrtc->readDataPorts();
123  }
124  for (unsigned int i=0; i<numBodies(); i++){
125  BodyRTC *bodyrtc = dynamic_cast<BodyRTC *>(body(i).get());
126  bodyrtc->preOneStep();
127  }
128  for (unsigned int i=0; i<receivers.size(); i++){
129  receivers[i].tick(timeStep());
130  }
131  tm_control.end();
132 
133 #if 1
136  tm_collision.end();
137 #endif
138 
139  tm_dynamics.begin();
141  if (m_kinematicsOnly){
142  for (unsigned int i=0; i<numBodies(); i++){
143  body(i)->calcForwardKinematics();
144  }
145  currentTime_ += timeStep();
146  }else{
148  }
149 
150  for (unsigned int i=0; i<numBodies(); i++){
151  BodyRTC *bodyrtc = dynamic_cast<BodyRTC *>(body(i).get());
152  bodyrtc->postOneStep();
153  }
154  appendLog();
155  tm_dynamics.end();
156 
157  if (m_totalTime && currentTime() > m_totalTime){
158  struct timeval endTime;
159  gettimeofday(&endTime, NULL);
160  double realT = (endTime.tv_sec - beginTime.tv_sec)
161  + (endTime.tv_usec - beginTime.tv_usec)/1e6;
162  printf("total :%8.3f[s], %8.3f[sim/real]\n",
163  realT, m_totalTime/realT);
164  printf("controller:%8.3f[s], %8.3f[ms/frame]\n",
166  printf("collision :%8.3f[s], %8.3f[ms/frame]\n",
168  printf("dynamics :%8.3f[s], %8.3f[ms/frame]\n",
170  for (unsigned int i=0; i<numBodies(); i++){
171  hrp::BodyPtr body = this->body(i);
172  int ntri=0;
173  for (unsigned int j=0; j<body->numLinks(); j++){
174  hrp::Link *l = body->link(j);
175  if (l && l->coldetModel){
176  ntri += l->coldetModel->getNumTriangles();
177  }
178  }
179  printf("num of triangles : %s : %d\n", body->name().c_str(), ntri);
180  }
181  fflush(stdout);
182  return false;
183  }else{
184  return true;
185  }
186 }
187 
189 {
191  for (unsigned int i=0; i<numBodies(); i++){
192  BodyRTC *bodyrtc = dynamic_cast<BodyRTC *>(body(i).get());
193  bodyrtc->exit();
194  }
195  manager->cleanupComponents();
196  clearBodies();
198  setCurrentTime(0.0);
199  pairs.clear();
200  receivers.clear();
201 }
202 
204 {
205  int bodyIndex1 = bodyIndex(bodyPtr1->name());
206  int bodyIndex2 = bodyIndex(bodyPtr2->name());
207 
208  std::vector<hrp::Link*> links1;
209  const hrp::LinkTraverse& traverse1 = bodyPtr1->linkTraverse();
210  links1.resize(traverse1.numLinks());
211  std::copy(traverse1.begin(), traverse1.end(), links1.begin());
212 
213  std::vector<hrp::Link*> links2;
214  const hrp::LinkTraverse& traverse2 = bodyPtr2->linkTraverse();
215  links2.resize(traverse2.numLinks());
216  std::copy(traverse2.begin(), traverse2.end(), links2.begin());
217 
218  for(size_t j=0; j < links1.size(); ++j){
219  for(size_t k=0; k < links2.size(); ++k){
220  hrp::Link* link1 = links1[j];
221  hrp::Link* link2 = links2[k];
222 
223  if(link1 && link2 && link1 != link2){
225  (bodyIndex1, link1, bodyIndex2, link2,
226  0.5, 0.5, 0.01, 0.0, 0.0);
227  pairs.push_back(new hrp::ColdetLinkPair(link1, link2));
228  }
229  }
230  }
231 
232  collisions.length(pairs.size());
233  for(size_t colIndex=0; colIndex < pairs.size(); ++colIndex){
234  hrp::ColdetLinkPairPtr linkPair = pairs[colIndex];
235  hrp::Link *link0 = linkPair->link(0);
236  hrp::Link *link1 = linkPair->link(1);
237  OpenHRP::LinkPair& pair = collisions[colIndex].pair;
238  pair.charName1 = CORBA::string_dup(link0->body->name().c_str());
239  pair.charName2 = CORBA::string_dup(link1->body->name().c_str());
240  pair.linkName1 = CORBA::string_dup(link0->name.c_str());
241  pair.linkName2 = CORBA::string_dup(link1->name.c_str());
242  }
243 }
244 
246 {
247  m_kinematicsOnly = flag;
248 }
struct timeval beginTime
void set(hrp::WorldBase &i_world, OpenHRP::CollisionSequence &i_collisions)
Definition: SceneState.cpp:6
int bodyIndex(const std::string &name)
virtual void calcNextState()
void cleanupComponents()
void setCurrentTime(double tm)
Simulator(RTC::Manager *manager)
Constructor.
manager
png_uint_32 i
TimeMeasure tm_dynamics
void begin()
static Manager & instance()
std::vector< ClockReceiver > receivers
void kinematicsOnly(bool flag)
BodyPtr body(int index)
int gettimeofday(struct timeval *tv, struct timezone *tz)
virtual bool oneStep()
double currentTime(void) const
const std::string & name()
OpenHRP::CollisionSequence collisions
double totalTime()
Definition: Project.h:106
void writeDataPorts(double time)
Definition: BodyRTC.cpp:50
bool realTime()
Definition: Project.h:112
void add(const T &state)
Definition: LogManager.h:17
virtual ReturnCode_t exit()
std::vector< hrp::ColdetLinkPairPtr > pairs
bool addCollisionCheckLinkPair(int bodyIndex1, Link *link1, int bodyIndex2, Link *link2, double muStatic, double muDynamic, double culling_thresh, double restitution, double epsilon)
std::vector< Link *>::const_iterator begin() const
void realTime(bool flag)
bool kinematicsOnly()
Definition: Project.h:111
void initRTS(Project &prj, std::vector< ClockReceiver > &receivers)
hrp::ConstraintForceSolver constraintForceSolver
void addCollisionCheckPair(BodyRTC *b1, BodyRTC *b2)
TimeMeasure tm_collision
LogManager< SceneState > * log
double totalTime()
double logTimeStep()
Definition: Project.h:108
std::deque< struct timeval > startTimes
boost::intrusive_ptr< ColdetLinkPair > ColdetLinkPairPtr
void clearBodies()
bool postOneStep()
Definition: BodyRTC.cpp:569
unsigned int numBodies()
std::vector< Link *>::const_iterator end() const
double currentTime_
void readDataPorts()
Definition: BodyRTC.cpp:57
double timeStep(void) const
boost::function2< hrp::BodyPtr, const std::string &, const ModelItem & > BodyFactory
Definition: ProjectUtil.h:9
TimeMeasure tm_control
void initWorld(Project &prj, BodyFactory &factory, hrp::World< hrp::ConstraintForceSolver > &world, std::vector< hrp::ColdetLinkPairPtr > &pairs)
Definition: ProjectUtil.cpp:9
bool preOneStep()
Definition: BodyRTC.cpp:521
int usleep(useconds_t usec)
double averageTime() const
unsigned int numLinks() const
const LinkTraverse & linkTraverse() const


hrpsys
Author(s): AIST, Fumio Kanehiro
autogenerated on Sat Dec 17 2022 03:52:21