server/UtDynamicsSimulator/World.cpp
Go to the documentation of this file.
1 // -*- mode: c++; indent-tabs-mode: t; tab-width: 4; c-basic-offset: 4; -*-
2 /*
3  * Copyright (c) 2008, AIST, the University of Tokyo and General Robotix Inc.
4  * All rights reserved. This program is made available under the terms of the
5  * Eclipse Public License v1.0 which accompanies this distribution, and is
6  * available at http://www.eclipse.org/legal/epl-v10.html
7  * Contributors:
8  * The University of Tokyo
9  * National Institute of Advanced Industrial Science and Technology (AIST)
10  * General Robotix Inc.
11  */
16 #include <string>
17 
18 #include "World.h"
19 #include "psim.h"
20 #include "Sensor.h"
21 
22 static const double DEFAULT_GRAVITY_ACCELERATION = 9.80665;
23 
24 static const bool debugMode = false;
25 
26 #include <fstream>
27 //static std::ofstream logfile("world.log");
28 static std::ofstream logfile;
29 
30 #include <sdcontact.h>
31 
32 #if 0
33 bool World::LinkPairKey::operator<(const LinkPairKey& pair2) const
34 {
35  if((body1 == pair2.body1 && body2 == pair2.body2) ||
36  (body2 == pair2.body1 && body1 == pair2.body2 )){
37  if(link1 < link2){
38  if(pair2.link1 < pair2.link2){
39  return (link1 < pair2.link1) ? true : (link2 < pair2.link2);
40  } else {
41  return (link1 < pair2.link2) ? true : (link2 < pair2.link1);
42  }
43  } else {
44  if(pair2.link1 < pair2.link2){
45  return (link2 < pair2.link1) ? true : (link1 < pair2.link2);
46  } else {
47  return (link2 < pair2.link2) ? true : (link1 < pair2.link1);
48  }
49  }
50  } else {
51  if(body1 < body2){
52  if(pair2.body1 < pair2.body2){
53  return (body1 < pair2.body1) ? true : (body2 < pair2.body2);
54  } else {
55  return (body1 < pair2.body2) ? true : (body2 < pair2.body1);
56  }
57  } else {
58  if(pair2.body1 < pair2.body2){
59  return (body2 < pair2.body1) ? true : (body1 < pair2.body2);
60  } else {
61  return (body2 < pair2.body2) ? true : (body1 < pair2.body1);
62  }
63  }
64  }
65 }
66 #endif
67 
69 {
70  currentTime_ = 0.0;
71  timeStep_ = 0.005;
72 
73  isEulerMethod = false;
74  sensorsAreEnabled = false;
76 
77  chain = new pSim;
78 }
79 
80 
82 {
83  if(chain) delete chain;
84  int n_sensors = sensors.size();
85  for(int i=0; i<n_sensors; i++)
86  {
88  }
89  sensors.clear();
90  int n_pairs = contact_pairs.size();
91  for(int i=0; i<n_pairs; i++)
92  {
93  delete contact_pairs[i];
94  }
95  contact_pairs.clear();
96 }
97 
98 
99 void World::setTimeStep(double ts)
100 {
101  timeStep_ = ts;
102 }
103 
104 
105 void World::setCurrentTime(double time)
106 {
107  currentTime_ = time;
108 }
109 
110 
112 {
113  g.set(_g);
114  if(chain->Root())
115  chain->Root()->loc_lin_acc.set(_g);
116 }
117 
118 
119 void World::enableSensors(bool on)
120 {
121  sensorsAreEnabled = on;
122 }
123 
124 
126 {
127 // logfile << "initialize" << endl;
128  chain->Schedule();
129  chain->CalcPosition();
130  chain->CalcVelocity();
131 // logfile << "initialize end" << endl;
132 #if 0
133  if(isEulerMethod){
134  pForwardDynamics->setEulerMethod();
135  } else {
136  pForwardDynamics->setRungeKuttaMethod();
137  }
138  pForwardDynamics->setGravityAcceleration(g);
139  pForwardDynamics->setTimeStep(timeStep_);
140  pForwardDynamics->enableSensors(sensorsAreEnabled);
141  pForwardDynamics->initialize();
142 #endif
143 }
144 
145 
146 void World::calcNextState(OpenHRP::CollisionSequence& corbaCollisionSequence)
147 {
148  if(debugMode){
149  cout << "World current time = " << currentTime_ << endl;
150  }
151 // logfile << "calcNextState" << endl;
152  // corbaCollisionSequence->contact_pairs
153  int n_pair = contact_pairs.size();
154  for(int i=0; i<n_pair; i++)
155  {
156  contact_pairs[i]->Clear();
157  OpenHRP::Collision& col = corbaCollisionSequence[i];
158  int n_point = col.points.length();
159  if(n_point == 0) continue;
160  Joint* joint0 = contact_pairs[i]->GetJoint(0);
161  static fVec3 pos0, norm0, pos, norm;
162  for(int j=0; j<n_point; j++)
163  {
164  OpenHRP::CollisionPoint& point = col.points[j];
165  // inertia frame -> joint0 frame
166  pos0(0) = point.position[0];
167  pos0(1) = point.position[1];
168  pos0(2) = point.position[2];
169  norm0(0) = point.normal[0];
170  norm0(1) = point.normal[1];
171  norm0(2) = point.normal[2];
172  pos0 -= joint0->abs_pos;
173  pos.mul(pos0, joint0->abs_att);
174  norm.mul(norm0, joint0->abs_att);
175  contact_pairs[i]->AddPoint(pos.data(), norm.data(), point.idepth);
176  }
177  }
178  //
179  if(isEulerMethod)
180  {
183  chain->CalcPosition();
184  chain->CalcVelocity();
185  }
186  else
187  {
190  chain->CalcPosition();
191  chain->CalcVelocity();
192 
193  chain->Update();
195  chain->CalcPosition();
196  chain->CalcVelocity();
197 
198  chain->Update();
200  chain->CalcPosition();
201  chain->CalcVelocity();
202 
203  chain->Update();
205  chain->CalcPosition();
206  chain->CalcVelocity();
207  }
208  // update sensors
209  logfile << "update sensors ->" << endl;
210  int n;
211  n = numSensors();
212  for(int i=0; i<n; i++)
213  {
214  if(sensors[i]->type == Sensor::FORCE)
215  {
216  ForceSensor* fs = (ForceSensor*)sensors[i];
218  }
219  else if(sensors[i]->type == Sensor::RATE_GYRO)
220  {
223  }
224  else if(sensors[i]->type == Sensor::ACCELERATION)
225  {
226  AccelSensor* as = (AccelSensor*)sensors[i];
228  }
229  else
230  {
231  }
232  }
233  logfile << "<- update sensors" << endl;
235 }
236 
238 {
239  logfile << "force sensor: " << fs->name << endl;
240  logfile << "joint = " << fs->joint->name << endl;
241  logfile << "f = " << fs->joint->joint_f << endl;
242  logfile << "n = " << fs->joint->joint_n << endl;
243  logfile << "localR = " << fs->localR << endl;
244  logfile << "localPos = " << fs->localPos << endl;
245  fVec3 local_f, local_n, n;
246  local_f.mul(fs->joint->joint_f, fs->localR);
247  n.cross(fs->joint->joint_f, fs->localPos);
248  n += fs->joint->joint_n;
249  local_n.mul(n, fs->localR);
250  fs->f.neg(local_f);
251  fs->tau.neg(local_n);
252 }
253 
255 {
256  rgs->w.mul(rgs->joint->loc_ang_vel, rgs->localR);
257 }
258 
260 {
262  fVec3 acc_j, tmp1;
263  tmp1.cross(as->joint->loc_ang_vel, as->localPos);
264  acc_j.cross(as->joint->loc_ang_vel, tmp1);
265  tmp1.cross(as->joint->loc_ang_acc, as->localPos);
266  acc_j += tmp1;
267  acc_j += as->joint->loc_lin_acc;
268  as->dv.mul(acc_j, as->localR);
269 }
270 
272 {
273  isEulerMethod = true;
274 }
275 
276 
278 {
279  isEulerMethod = false;
280 }
281 
282 
283 #if 0
284 std::pair<int,bool> World::getIndexOfLinkPairs(BodyPtr body1, Link* link1, BodyPtr body2, Link* link2)
285 {
286  int index = -1;
287  int isRegistered = false;
288 
289  if(link1 != link2){
290 
291  LinkPairKey linkPair;
292  linkPair.body1 = body1;
293  linkPair.link1 = link1;
294  linkPair.body2 = body2;
295  linkPair.link2 = link2;
296 
297  LinkPairKeyToIndexMap::iterator p = linkPairKeyToIndexMap.find(linkPair);
298 
299  if(p != linkPairKeyToIndexMap.end()){
300  index = p->second;
301  isRegistered = true;
302  } else {
303  index = numRegisteredLinkPairs++;
304  linkPairKeyToIndexMap[linkPair] = index;
305  }
306  }
307 
308  return std::make_pair(index, isRegistered);
309 }
310 #endif
311 
312 int World::addSensor(Joint* _joint, int sensorType, int _id, const std::string _name, const fVec3& _localPos, const fMat33& _localR)
313 {
314  Sensor* sensor = Sensor::create(sensorType);
315  sensor->joint = _joint;
316  sensor->id = _id;
317  sensor->name = _name;
318  sensor->localPos.set(_localPos);
319  sensor->localR.set(_localR);
320  sensors.push_back(sensor);
321  return 0;
322 }
323 
324 
325 Sensor* World::findSensor(const char* sensorName, const char* charName)
326 {
327  int n_sensors = sensors.size();
328  for(int i=0; i<n_sensors; i++)
329  {
330  if(!strcmp(sensors[i]->name.c_str(), sensorName) && sensors[i]->joint &&
331  !strcmp(sensors[i]->joint->CharName(), charName))
332  {
333  return sensors[i];
334  }
335  }
336  return 0;
337 }
338 
339 int World::numSensors(int sensorType, const char* charName)
340 {
341  int count = 0;
342  int n_sensors = sensors.size();
343  for(int i=0; i<n_sensors; i++)
344  {
345  if(sensors[i]->type == sensorType &&
346  (!charName || !strcmp(sensors[i]->joint->CharName(), charName)))
347  {
348  count++;
349  }
350  }
351  return count;
352 }
353 
354 void World::getAllCharacterData(const char* name, OpenHRP::DynamicsSimulator::LinkDataType type, OpenHRP::DblSequence_out& rdata)
355 {
356  int index = -1, nchar = characters.size();
357  for(int i=0; i<nchar; i++)
358  {
359  if(!strcmp(name, characters[i].name.c_str()))
360  {
361  index = i;
362  break;
363  }
364  }
365  if(index < 0) return;
366  CharacterInfo& cinfo = characters[index];
367  int n_joints = cinfo.n_joints, n_links = cinfo.links.size();
368  rdata->length(n_joints);
369  for(int i=0; i<n_links; i++)
370  {
371  if(cinfo.jointIDs[i] >= 0)
372  {
373  _get_all_character_data_sub(cinfo.links[i], cinfo.jointIDs[i], type, rdata);
374  }
375  }
376 }
377 
378 void World::_get_all_character_data_sub(Joint* cur, int index, OpenHRP::DynamicsSimulator::LinkDataType type, OpenHRP::DblSequence_out& rdata)
379 {
380  if(cur->j_type == ::JROTATE || cur->j_type == ::JSLIDE)
381  {
382  switch(type) {
384  (*rdata)[index] = cur->q;
385  break;
386 
388  (*rdata)[index] = cur->qd;
389  break;
390 
392  (*rdata)[index] = cur->qdd;
393  break;
394 
396  (*rdata)[index] = cur->tau;
397  break;
398 
399  default:
400 // cerr << "ERROR - Invalid type: " << getLabelOfGetLinkDataType(type) << endl;
401  break;
402  }
403  }
404 }
405 
406 void World::setAllCharacterData(const char* name, OpenHRP::DynamicsSimulator::LinkDataType type, const OpenHRP::DblSequence& wdata)
407 {
408  int index = -1, nchar = characters.size();
409  for(int i=0; i<nchar; i++)
410  {
411  if(!strcmp(name, characters[i].name.c_str()))
412  {
413  index = i;
414  break;
415  }
416  }
417  if(index < 0) return;
418  CharacterInfo& cinfo = characters[index];
419  int n_links = cinfo.links.size();
420  for(int i=0; i<n_links; i++)
421  {
422  if(cinfo.jointIDs[i] >= 0)
423  {
424  _set_all_character_data_sub(cinfo.links[i], cinfo.jointIDs[i], type, wdata);
425  }
426  }
427 }
428 
429 void World::_set_all_character_data_sub(Joint* cur, int index, OpenHRP::DynamicsSimulator::LinkDataType type, const OpenHRP::DblSequence& wdata)
430 {
431  if(index < (int)wdata.length() &&
432  (cur->j_type == ::JROTATE || cur->j_type == ::JSLIDE))
433  {
434 // logfile << "set[" << index << "]: " << cur->name << ": " << wdata[index] << endl;
435  switch(type) {
437  cur->SetJointValue(wdata[index]);
438  break;
439 
441  cur->SetJointVel(wdata[index]);
442  break;
443 
445  cur->SetJointAcc(wdata[index]);
446  break;
447 
449  cur->SetJointForce(wdata[index]);
450  break;
451 
452  default:
453 // cerr << "ERROR - Invalid type: " << getLabelOfGetLinkDataType(type) << endl;
454  break;
455  }
456  }
457 }
458 
459 static void vec3_to_array(const fVec3& vec, double* a, int offset = 0)
460 {
461  a[offset++] = vec(0);
462  a[offset++] = vec(1);
463  a[offset] = vec(2);
464 }
465 
466 static void mat33_to_array(const fMat33& mat, OpenHRP::DblArray9& a)
467 {
468  a[0] = mat(0,0); a[1] = mat(0,1); a[2] = mat(0,2);
469  a[3] = mat(1,0); a[4] = mat(1,1); a[5] = mat(1,2);
470  a[6] = mat(2,0); a[7] = mat(2,1); a[8] = mat(2,2);
471 }
472 
473 void World::getAllCharacterPositions(OpenHRP::CharacterPositionSequence& all_char_pos)
474 {
475  int nchar = characters.size();
476  for(int i=0; i<nchar; i++)
477  {
478  CharacterInfo& cinfo = characters[i];
479  OpenHRP::CharacterPosition& char_pos = all_char_pos[i];
480  int nlink = cinfo.links.size();
481  for(int j=0; j<nlink; j++)
482  {
483  OpenHRP::LinkPosition& link_pos = char_pos.linkPositions[j];
484  Joint* cur = cinfo.links[j];
485 // logfile << "[" << j << "] " << cur->name << ": " << cur->abs_pos << endl;
486  vec3_to_array(cur->abs_pos, link_pos.p);
487  mat33_to_array(cur->abs_att, link_pos.R);
488  }
489  }
490 }
491 
492 void World::getAllSensorStates(OpenHRP::SensorStateSequence& all_sensor_states)
493 {
494  int nchar = characters.size();
495  for(int i=0; i<nchar; i++)
496  {
497  OpenHRP::SensorState& state = all_sensor_states[i];
498  // joint angles and torques
499  CharacterInfo& cinfo = characters[i];
500  int n_links = cinfo.links.size();
501  for(int j=0; j<n_links; j++)
502  {
503  int index = cinfo.jointIDs[j];
504  if(index >= 0)
505  {
506  Joint* joint = cinfo.links[j];
507  if(joint->j_type == ::JROTATE || joint->j_type == ::JSLIDE){
508  state.q[index] = joint->q;
509  state.dq[index] = joint->qd;
510  state.u[index] = joint->tau;
511  }
512  }
513  }
514  // other sensors
515  Joint* r = rootJoint(i);
516  int n_sensors = sensors.size();
517  for(int i=0; i<n_sensors; i++)
518  {
519  if(strcmp(sensors[i]->joint->CharName(), r->CharName())) continue;
520  switch(sensors[i]->type)
521  {
522  case Sensor::FORCE:
523  {
524  ForceSensor* _fs = (ForceSensor*)sensors[i];
525  vec3_to_array(_fs->f, state.force[sensors[i]->id], 0);
526  vec3_to_array(_fs->tau, state.force[sensors[i]->id], 3);
527  break;
528  }
529 
530  case Sensor::RATE_GYRO:
531  {
533  vec3_to_array(_gs->w, state.rateGyro[sensors[i]->id]);
534  break;
535  }
536 
538  {
539  AccelSensor* _as = (AccelSensor*)sensors[i];
540  vec3_to_array(_as->dv, state.accel[sensors[i]->id]);
541  break;
542  }
543 
544  default:
545  break;
546  }
547  }
548  }
549 }
550 
551 #if 0
552 void World::_get_all_sensor_states_sub(Joint* cur, int& count, SensorState& state)
553 {
554  if(!cur || cur->real) return;
555  if(cur->j_type == ::JROTATE || cur->j_type == ::JSLIDE)
556  {
557  state.q[count] = cur->q;
558  state.u[count] = cur->tau;
559  count++;
560  }
561  _get_all_sensor_states_sub(cur->child, count, state);
562  _get_all_sensor_states_sub(cur->brother, count, state);
563 }
564 #endif
566  const char* characterName,
567  const char* baseLink,
568  const char* targetLink,
569  fMat& J)
570 {
571  Joint* basej = chain->FindJoint(baseLink, characterName);
572  if(!basej) return;
573  Joint* targetj = chain->FindJoint(targetLink, characterName);
574  if(!targetj) return;
575 
576  fMat tempJ(6, chain->NumDOF());
577  tempJ.zero();
578  targetj->CalcJacobian(tempJ);
579 
580  int n_dof = 0;
581  for(Joint* j=targetj; j!=basej; j=j->parent)
582  {
583  n_dof += j->n_dof;
584  }
585 
586  J.resize(6, n_dof);
587  J.zero();
588  int count = 0;
589  for(Joint* j=targetj; j!=basej; j=j->parent)
590  {
591  for(int m=0; m<j->n_dof; m++)
592  {
593  J(0, count+m) = tempJ(0, j->i_dof+m);
594  J(1, count+m) = tempJ(1, j->i_dof+m);
595  J(2, count+m) = tempJ(2, j->i_dof+m);
596  J(3, count+m) = tempJ(3, j->i_dof+m);
597  J(4, count+m) = tempJ(4, j->i_dof+m);
598  J(5, count+m) = tempJ(5, j->i_dof+m);
599  }
600  count += j->n_dof;
601  }
602 }
603 
604 
605 void World::addCollisionCheckLinkPair(Joint* jnt1, Joint* jnt2, double staticFriction, double slipFriction, double epsilon)
606 {
607  double default_spring = 1e5;
608  double default_damper = 1e4;
609  double default_slip_p = 2000.0;
610  double default_slip_d = 700.0;
611  double default_slip_func_coef_base = 0.1;
612  SDContactPair* sd_pair = new SDContactPair(jnt1, jnt2, default_spring, default_damper, staticFriction, slipFriction, default_slip_p, default_slip_d, default_slip_func_coef_base);
613  contact_pairs.push_back(sd_pair);
614 }
615 
616 void World::addCharacter(Joint* rjoint, const std::string& name, OpenHRP::LinkInfoSequence_var links)
617 {
618  CharacterInfo cinfo(rjoint, name);
619  int n_links = links->length();
620  for(int i=0; i<n_links; i++)
621  {
622  OpenHRP::LinkInfo linfo = links[i];
623  Joint* jnt = chain->FindJoint(linfo.name, name.c_str());
624  if(jnt)
625  {
626  cinfo.links.push_back(jnt);
627  cinfo.jointIDs.push_back(linfo.jointId);
628  if(linfo.jointId >= 0) cinfo.n_joints++;
629  }
630  }
631 // logfile << "character: " << name << ", root = " << rjoint->name << ", n_joints = " << cinfo.n_joints << endl;
632  characters.push_back(cinfo);
633 }
634 
636 {
637  return characters[index].root;
638 }
Sensor.h
AccelSensor::dv
fVec3 dv
Definition: server/UtDynamicsSimulator/Sensor.h:98
World::setGravityAcceleration
void setGravityAcceleration(const fVec3 &g)
Definition: server/UtDynamicsSimulator/World.cpp:111
Joint::parent
Joint * parent
pointer to the parent joint
Definition: chain.h:683
i
png_uint_32 i
Definition: png.h:2732
Joint::abs_pos
fVec3 abs_pos
absolute position
Definition: chain.h:741
World::setTimeStep
void setTimeStep(double)
Definition: server/UtDynamicsSimulator/World.cpp:99
fMat33::set
void set(const fMat33 &mat)
Copies a matrix.
Definition: fMatrix3.cpp:623
Joint::joint_f
fVec3 joint_f
Definition: chain.h:755
fVec3
3-element vector class.
Definition: fMatrix3.h:206
World.h
Chain::CalcVelocity
void CalcVelocity()
Definition: fk.cpp:41
Sensor::joint
Joint * joint
Definition: server/UtDynamicsSimulator/Sensor.h:55
World::getAllSensorStates
void getAllSensorStates(OpenHRP::SensorStateSequence &all_sensor_states)
Definition: server/UtDynamicsSimulator/World.cpp:492
fVec3::cross
void cross(const fVec3 &vec1, const fVec3 &vec2)
Cross product.
Definition: fMatrix3.cpp:944
Sensor::ACCELERATION
@ ACCELERATION
Definition: server/UtDynamicsSimulator/Sensor.h:32
World::addCharacter
void addCharacter(Joint *rjoint, const std::string &_name, OpenHRP::LinkInfoSequence_var links)
Definition: server/UtDynamicsSimulator/World.cpp:616
Sensor
Definition: server/UtDynamicsSimulator/Sensor.h:24
Joint
The class for representing a joint.
Definition: chain.h:538
pSim::Schedule
int Schedule()
Creates default schedule, which is optimized for serial computation.
Definition: schedule.cpp:1060
test_modelloader.norm
def norm(a)
Definition: test_modelloader.py:56
World::_get_all_character_data_sub
void _get_all_character_data_sub(Joint *cur, int index, OpenHRP::DynamicsSimulator::LinkDataType type, OpenHRP::DblSequence_out &rdata)
Definition: server/UtDynamicsSimulator/World.cpp:378
Sensor::FORCE
@ FORCE
Definition: server/UtDynamicsSimulator/Sensor.h:30
Joint::SetJointAcc
int SetJointAcc(double _qdd)
Definition: joint.cpp:580
Joint::SetJointForce
int SetJointForce(double _tau)
Definition: joint.cpp:783
World::CharacterInfo::links
std::vector< Joint * > links
Definition: server/UtDynamicsSimulator/World.h:44
ForceSensor
Definition: server/UtDynamicsSimulator/Sensor.h:64
autoplay.n
n
Definition: autoplay.py:12
Sensor::destroy
static void destroy(Sensor *sensor)
Definition: server/UtDynamicsSimulator/Sensor.cpp:73
Joint::j_type
JointType j_type
joint type
Definition: chain.h:694
sdcontact.h
ForceSensor::tau
fVec3 tau
Definition: server/UtDynamicsSimulator/Sensor.h:71
Joint::qd
double qd
joint velocity (for 1DOF joints)
Definition: chain.h:725
Joint::CalcJacobian
int CalcJacobian(fMat &J)
The Jacobian matrix of position/orientation w.r.t. the joint values.
Definition: jacobi.cpp:119
Sensor::RATE_GYRO
@ RATE_GYRO
Definition: server/UtDynamicsSimulator/Sensor.h:31
mat33_to_array
static void mat33_to_array(const fMat33 &mat, OpenHRP::DblArray9 &a)
Definition: server/UtDynamicsSimulator/World.cpp:466
SDContactPair
Definition: sdcontact.h:23
Sensor::localPos
fVec3 localPos
Definition: server/UtDynamicsSimulator/Sensor.h:57
type
png_infop png_charp png_int_32 png_int_32 int * type
Definition: png.h:2330
World::getAllCharacterData
void getAllCharacterData(const char *name, OpenHRP::DynamicsSimulator::LinkDataType type, OpenHRP::DblSequence_out &rdata)
Definition: server/UtDynamicsSimulator/World.cpp:354
Joint::child
Joint * child
pointer to the child joint
Definition: chain.h:685
Chain::NumDOF
int NumDOF()
Total degrees of freedom.
Definition: chain.h:343
World::enableSensors
void enableSensors(bool on)
Definition: server/UtDynamicsSimulator/World.cpp:119
RateGyroSensor
Definition: server/UtDynamicsSimulator/Sensor.h:78
JROTATE
@ JROTATE
rotational (1DOF)
Definition: chain.h:41
World::setRungeKuttaMethod
void setRungeKuttaMethod()
Definition: server/UtDynamicsSimulator/World.cpp:277
Sensor::name
std::string name
Definition: server/UtDynamicsSimulator/Sensor.h:52
World::~World
~World()
Definition: server/UtDynamicsSimulator/World.cpp:81
fMat33
3x3 matrix class.
Definition: fMatrix3.h:29
RateGyroSensor::w
fVec3 w
Definition: server/UtDynamicsSimulator/Sensor.h:84
fMat
Matrix of generic size. The elements are stored in a one-dimensional array in row-major order.
Definition: fMatrix.h:46
fMat::resize
int resize(int i, int j)
Changes the matrix size.
Definition: fMatrix.h:133
fMat::zero
void zero()
Creates a zero matrix.
Definition: fMatrix.h:249
fVec3::set
void set(double *v)
Set element values from array or three values.
Definition: fMatrix3.h:314
Chain::CalcPosition
void CalcPosition()
Forward kinematics.
Definition: fk.cpp:16
World::World
World()
Definition: server/UtDynamicsSimulator/World.cpp:68
Joint::SetJointValue
int SetJointValue(double _q)
Definition: joint.cpp:538
World::findSensor
Sensor * findSensor(const char *sensorName, const char *charName)
Definition: server/UtDynamicsSimulator/World.cpp:325
Joint::qdd
double qdd
joint acceleration (for 1DOF joints)
Definition: chain.h:726
World::setEulerMethod
void setEulerMethod()
Definition: server/UtDynamicsSimulator/World.cpp:271
World::CharacterInfo::n_joints
int n_joints
Definition: server/UtDynamicsSimulator/World.h:46
Joint::q
double q
joint value (for 1DOF joints)
Definition: chain.h:724
World::calcNextState
void calcNextState(OpenHRP::CollisionSequence &corbaCollisionSequence)
Definition: server/UtDynamicsSimulator/World.cpp:146
Chain::Root
Joint * Root()
Definition: chain.h:151
logfile
static std::ofstream logfile
Definition: server/UtDynamicsSimulator/World.cpp:28
Joint::CharName
char * CharName() const
Returns the character name.
Definition: chain.h:648
psim.h
Forward dynamics computation based on Assembly-Disassembly Algorithm.
World::numRegisteredLinkPairs
int numRegisteredLinkPairs
Definition: server/UtDynamicsSimulator/World.h:149
World::getAllCharacterPositions
void getAllCharacterPositions(OpenHRP::CharacterPositionSequence &all_char_pos)
Definition: server/UtDynamicsSimulator/World.cpp:473
World::numSensors
int numSensors()
Definition: server/UtDynamicsSimulator/World.h:87
Sensor::id
int id
Definition: server/UtDynamicsSimulator/Sensor.h:54
World::_set_all_character_data_sub
void _set_all_character_data_sub(Joint *cur, int index, OpenHRP::DynamicsSimulator::LinkDataType type, const OpenHRP::DblSequence &wdata)
Definition: server/UtDynamicsSimulator/World.cpp:429
hrp::BodyPtr
boost::shared_ptr< Body > BodyPtr
Definition: Body.h:315
JOINT_VALUE
@ JOINT_VALUE
Definition: BridgeConf.h:36
World::update_rate_gyro_sensor
void update_rate_gyro_sensor(RateGyroSensor *rgs)
Definition: server/UtDynamicsSimulator/World.cpp:254
name
png_infop png_charpp name
Definition: png.h:2379
Joint::tau
double tau
joint force/torque (for 1DOF joints)
Definition: chain.h:733
World::characters
std::vector< CharacterInfo > characters
Definition: server/UtDynamicsSimulator/World.h:117
World::currentTime_
double currentTime_
Definition: server/UtDynamicsSimulator/World.h:125
fVec3::data
double * data()
Pointer to the first element.
Definition: fMatrix3.h:259
fVec3::neg
void neg(const fVec3 &vec)
Definition: fMatrix3.cpp:881
debugMode
static const bool debugMode
Definition: server/UtDynamicsSimulator/World.cpp:24
World::chain
pSim * chain
Definition: server/UtDynamicsSimulator/World.h:113
Chain::IntegrateRK4
int IntegrateRK4(double timestep, int step)
Performs 4-th order Runge-Kutta integration.
Definition: integ.cpp:214
Joint::abs_att
fMat33 abs_att
absolute orientation
Definition: chain.h:742
JSLIDE
@ JSLIDE
prismatic (1DOF)
Definition: chain.h:42
JOINT_TORQUE
@ JOINT_TORQUE
Definition: BridgeConf.h:39
Sensor::localR
fMat33 localR
Definition: server/UtDynamicsSimulator/Sensor.h:56
ForceSensor::f
fVec3 f
Definition: server/UtDynamicsSimulator/Sensor.h:70
Joint::real
Joint * real
pointer to the real (connected) joint; for closed chains.
Definition: chain.h:687
World::rootJoint
Joint * rootJoint(int index)
Definition: server/UtDynamicsSimulator/World.cpp:635
Chain::FindJoint
Joint * FindJoint(const char *jname, const char *charname=0)
Find a joint from name.
Definition: chain.cpp:391
World::_get_all_sensor_states_sub
void _get_all_sensor_states_sub(Joint *cur, int &count, OpenHRP::SensorState &state)
pSim::Update
int Update()
Compute joint accelerations.
Definition: update.cpp:40
World::update_force_sensor
void update_force_sensor(ForceSensor *fs)
Definition: server/UtDynamicsSimulator/World.cpp:237
World::CharacterInfo::jointIDs
std::vector< int > jointIDs
Definition: server/UtDynamicsSimulator/World.h:45
World::setCurrentTime
void setCurrentTime(double)
Definition: server/UtDynamicsSimulator/World.cpp:105
JOINT_ACCELERATION
@ JOINT_ACCELERATION
Definition: BridgeConf.h:38
Joint::loc_ang_acc
fVec3 loc_ang_acc
angular acceleration in local frame
Definition: chain.h:747
Joint::SetJointVel
int SetJointVel(double _qd)
Definition: joint.cpp:562
World::sensorsAreEnabled
bool sensorsAreEnabled
Definition: server/UtDynamicsSimulator/World.h:155
World::addSensor
int addSensor(Joint *jnt, int sensorType, int id, const std::string name, const fVec3 &_localPos, const fMat33 &_localR)
Definition: server/UtDynamicsSimulator/World.cpp:312
Joint::name
char * name
joint name (including the character name)
Definition: chain.h:691
JOINT_VELOCITY
@ JOINT_VELOCITY
Definition: BridgeConf.h:37
test.a
int a
Definition: test.py:1
Chain::CalcAcceleration
void CalcAcceleration()
Definition: fk.cpp:75
World::addCollisionCheckLinkPair
void addCollisionCheckLinkPair(Joint *jnt1, Joint *jnt2, double staticFriction, double slipFriction, double epsilon)
Definition: server/UtDynamicsSimulator/World.cpp:605
World::timeStep_
double timeStep_
Definition: server/UtDynamicsSimulator/World.h:126
Joint::brother
Joint * brother
pointer to the brother joint
Definition: chain.h:684
vec3_to_array
static void vec3_to_array(const fVec3 &vec, double *a, int offset=0)
Definition: server/UtDynamicsSimulator/World.cpp:459
World::update_accel_sensor
void update_accel_sensor(AccelSensor *as)
Definition: server/UtDynamicsSimulator/World.cpp:259
Chain::Integrate
int Integrate(double timestep)
Performs Euler integration with timestep timestep [s].
Definition: integ.cpp:110
World::contact_pairs
std::vector< SDContactPair * > contact_pairs
Definition: server/UtDynamicsSimulator/World.h:114
World::initialize
void initialize()
Definition: server/UtDynamicsSimulator/World.cpp:125
DEFAULT_GRAVITY_ACCELERATION
static const double DEFAULT_GRAVITY_ACCELERATION
Definition: server/UtDynamicsSimulator/World.cpp:22
Sensor::create
static Sensor * create(int type)
Definition: server/UtDynamicsSimulator/Sensor.cpp:25
World::isEulerMethod
bool isEulerMethod
Definition: server/UtDynamicsSimulator/World.h:153
World::CharacterInfo
Definition: server/UtDynamicsSimulator/World.h:36
World::calcCharacterJacobian
void calcCharacterJacobian(const char *characterName, const char *baseLink, const char *targetLink, fMat &J)
Definition: server/UtDynamicsSimulator/World.cpp:565
World::g
fVec3 g
Definition: server/UtDynamicsSimulator/World.h:151
Joint::loc_ang_vel
fVec3 loc_ang_vel
angular velocity in local frame
Definition: chain.h:744
World::setAllCharacterData
void setAllCharacterData(const char *name, OpenHRP::DynamicsSimulator::LinkDataType type, const OpenHRP::DblSequence &wdata)
Definition: server/UtDynamicsSimulator/World.cpp:406
Joint::loc_lin_acc
fVec3 loc_lin_acc
linear acceleration in local frame
Definition: chain.h:746
fVec3::mul
void mul(const fVec3 &vec, double d)
Definition: fMatrix3.cpp:916
Joint::joint_n
fVec3 joint_n
Definition: chain.h:756
AccelSensor
Definition: server/UtDynamicsSimulator/Sensor.h:91
World::sensors
std::vector< Sensor * > sensors
Definition: server/UtDynamicsSimulator/World.h:115
pSim
Main class for forward dynamics computation.
Definition: psim.h:446


openhrp3
Author(s): AIST, General Robotix Inc., Nakamura Lab of Dept. of Mechano Informatics at University of Tokyo
autogenerated on Wed Sep 7 2022 02:51:04