server/UtDynamicsSimulator/World.h
Go to the documentation of this file.
1 // -*- mode: c++; indent-tabs-mode: nil; 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  */
12 #ifndef OPENHRP_WORLD_H_INCLUDED
13 #define OPENHRP_WORLD_H_INCLUDED
14 
15 #include <vector>
16 #include <map>
17 #include <list>
18 #include <vector>
19 #include <fMatrix3.h>
20 #include <hrpCorba/DynamicsSimulator.hh>
21 
22 class pSim;
23 class Joint;
24 class SDContactPair;
25 
26 
27  class Sensor;
28  class ForceSensor;
29  class RateGyroSensor;
30  class AccelSensor;
31 
32 
33  class World
34  {
35 // friend class CharacterInfo;
37  {
38  friend class World;
39  CharacterInfo(Joint* _root, const std::string& _name): name(_name) {
40  root = _root;
41  n_joints = 0;
42  }
43  std::string name;
44  std::vector<Joint*> links;
45  std::vector<int> jointIDs;
46  int n_joints;
48  public:
50  }
51 
52  };
53  public:
54  World();
55  ~World();
56 
57  void clearCollisionPairs();
58 
59  void setTimeStep(double);
60  double timeStep(void) const { return timeStep_; }
61 
62  void setCurrentTime(double);
63  double currentTime(void) const { return currentTime_; }
64 
65  void setGravityAcceleration(const fVec3& g);
66  const fVec3& getGravityAcceleration() { return g; }
67 
68  void enableSensors(bool on);
69 
70  void setEulerMethod();
71  void setRungeKuttaMethod();
72 
73  void initialize();
74  void calcNextState(OpenHRP::CollisionSequence& corbaCollisionSequence);
75 
76 // std::pair<int,bool> getIndexOfLinkPairs(BodyPtr body1, Link* link1, BodyPtr body2, Link* link2);
77 
78  pSim* Chain() {
79  return chain;
80  }
81 
82  int addSensor(Joint* jnt, int sensorType, int id, const std::string name, const fVec3& _localPos, const fMat33& _localR);
83 
84  Sensor* findSensor(const char* sensorName, const char* charName);
85 
86  int numSensors(int sensorType, const char* charName);
87  int numSensors() {
88  return sensors.size();
89  }
90 
91  void getAllCharacterData(const char* name, OpenHRP::DynamicsSimulator::LinkDataType type, OpenHRP::DblSequence_out& rdata);
92  void setAllCharacterData(const char* name, OpenHRP::DynamicsSimulator::LinkDataType type, const OpenHRP::DblSequence& wdata);
93  void getAllCharacterPositions(OpenHRP::CharacterPositionSequence& all_char_pos);
94  void getAllSensorStates(OpenHRP::SensorStateSequence& all_sensor_states);
95  void calcCharacterJacobian(const char* characterName, const char* baseLink, const char* targetLink, fMat& J);
96 
97  void addCollisionCheckLinkPair(Joint* jnt1, Joint* jnt2, double staticFriction, double slipFriction, double epsilon);
98 
99  void addCharacter(Joint* rjoint, const std::string& _name, OpenHRP::LinkInfoSequence_var links);
100  Joint* rootJoint(int index);
101  int numLinks(int index) {
102  return characters[index].links.size();
103  }
104  int numJoints(int index) {
105  return characters[index].n_joints;
106  }
107 
108  int numCharacter() {
109  return characters.size();
110  }
111 
112  protected:
114  std::vector<SDContactPair*> contact_pairs;
115  std::vector<Sensor*> sensors;
116 
117  std::vector<CharacterInfo> characters;
118 
119  private:
120 
121  void _get_all_character_data_sub(Joint* cur, int index, OpenHRP::DynamicsSimulator::LinkDataType type, OpenHRP::DblSequence_out& rdata);
122  void _set_all_character_data_sub(Joint* cur, int index, OpenHRP::DynamicsSimulator::LinkDataType type, const OpenHRP::DblSequence& wdata);
123  void _get_all_sensor_states_sub(Joint* cur, int& count, OpenHRP::SensorState& state);
124 
125  double currentTime_;
126  double timeStep_;
127 
131 #if 0
132  typedef std::map<std::string, int> NameToIndexMap;
133  NameToIndexMap nameToBodyIndexMap;
134 
135  typedef std::map<BodyPtr, int> BodyToIndexMap;
136  BodyToIndexMap bodyToIndexMap;
137 
138  struct LinkPairKey {
139  BodyPtr body1;
140  BodyPtr body2;
141  Link* link1;
142  Link* link2;
143  bool operator<(const LinkPairKey& pair2) const;
144  };
145  typedef std::map<LinkPairKey, int> LinkPairKeyToIndexMap;
146  LinkPairKeyToIndexMap linkPairKeyToIndexMap;
147 #endif
148 
150 
152 
153  bool isEulerMethod; // Euler or Runge Kutta ?
154 
156 
157  };
158 
159 
160 #endif
void setAllCharacterData(const char *name, OpenHRP::DynamicsSimulator::LinkDataType type, const OpenHRP::DblSequence &wdata)
3x3 matrix class.
Definition: fMatrix3.h:29
std::vector< Sensor * > sensors
png_infop png_charp png_int_32 png_int_32 int * type
Definition: png.h:2332
double currentTime(void) const
void setGravityAcceleration(const fVec3 &g)
void _get_all_character_data_sub(Joint *cur, int index, OpenHRP::DynamicsSimulator::LinkDataType type, OpenHRP::DblSequence_out &rdata)
bool operator<(const EchoSample &, const EchoSample &)
void addCharacter(Joint *rjoint, const std::string &_name, OpenHRP::LinkInfoSequence_var links)
CharacterInfo(Joint *_root, const std::string &_name)
void getAllCharacterData(const char *name, OpenHRP::DynamicsSimulator::LinkDataType type, OpenHRP::DblSequence_out &rdata)
int numJoints(int index)
Sensor * findSensor(const char *sensorName, const char *charName)
boost::shared_ptr< Body > BodyPtr
Definition: Body.h:315
double timeStep(void) const
void calcNextState(OpenHRP::CollisionSequence &corbaCollisionSequence)
void _set_all_character_data_sub(Joint *cur, int index, OpenHRP::DynamicsSimulator::LinkDataType type, const OpenHRP::DblSequence &wdata)
const fVec3 & getGravityAcceleration()
list index
void update_rate_gyro_sensor(RateGyroSensor *rgs)
int addSensor(Joint *jnt, int sensorType, int id, const std::string name, const fVec3 &_localPos, const fMat33 &_localR)
void getAllCharacterPositions(OpenHRP::CharacterPositionSequence &all_char_pos)
3x3 matrix and 3-element vector classes.
std::vector< CharacterInfo > characters
Joint * rootJoint(int index)
void _get_all_sensor_states_sub(Joint *cur, int &count, OpenHRP::SensorState &state)
void calcCharacterJacobian(const char *characterName, const char *baseLink, const char *targetLink, fMat &J)
3-element vector class.
Definition: fMatrix3.h:206
void addCollisionCheckLinkPair(Joint *jnt1, Joint *jnt2, double staticFriction, double slipFriction, double epsilon)
std::vector< SDContactPair * > contact_pairs
void update_accel_sensor(AccelSensor *as)
Main class for forward dynamics computation.
Definition: psim.h:446
The class for representing a joint.
Definition: chain.h:538
void update_force_sensor(ForceSensor *fs)
void clearCollisionPairs()
Matrix of generic size. The elements are stored in a one-dimensional array in row-major order...
Definition: fMatrix.h:46
void getAllSensorStates(OpenHRP::SensorStateSequence &all_sensor_states)


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