Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012 #ifndef OPENHRP_WORLD_H_INCLUDED
00013 #define OPENHRP_WORLD_H_INCLUDED
00014
00015 #include <vector>
00016 #include <map>
00017 #include <list>
00018 #include <vector>
00019 #include <fMatrix3.h>
00020 #include <hrpCorba/DynamicsSimulator.hh>
00021
00022 class pSim;
00023 class Joint;
00024 class SDContactPair;
00025
00026
00027 class Sensor;
00028 class ForceSensor;
00029 class RateGyroSensor;
00030 class AccelSensor;
00031
00032
00033 class World
00034 {
00035
00036 class CharacterInfo
00037 {
00038 friend class World;
00039 CharacterInfo(Joint* _root, const std::string& _name): name(_name) {
00040 root = _root;
00041 n_joints = 0;
00042 }
00043 std::string name;
00044 std::vector<Joint*> links;
00045 std::vector<int> jointIDs;
00046 int n_joints;
00047 Joint* root;
00048 public:
00049 ~CharacterInfo() {
00050 }
00051
00052 };
00053 public:
00054 World();
00055 ~World();
00056
00057 void clearCollisionPairs();
00058
00059 void setTimeStep(double);
00060 double timeStep(void) const { return timeStep_; }
00061
00062 void setCurrentTime(double);
00063 double currentTime(void) const { return currentTime_; }
00064
00065 void setGravityAcceleration(const fVec3& g);
00066 const fVec3& getGravityAcceleration() { return g; }
00067
00068 void enableSensors(bool on);
00069
00070 void setEulerMethod();
00071 void setRungeKuttaMethod();
00072
00073 void initialize();
00074 void calcNextState(OpenHRP::CollisionSequence& corbaCollisionSequence);
00075
00076
00077
00078 pSim* Chain() {
00079 return chain;
00080 }
00081
00082 int addSensor(Joint* jnt, int sensorType, int id, const std::string name, const fVec3& _localPos, const fMat33& _localR);
00083
00084 Sensor* findSensor(const char* sensorName, const char* charName);
00085
00086 int numSensors(int sensorType, const char* charName);
00087 int numSensors() {
00088 return sensors.size();
00089 }
00090
00091 void getAllCharacterData(const char* name, OpenHRP::DynamicsSimulator::LinkDataType type, OpenHRP::DblSequence_out& rdata);
00092 void setAllCharacterData(const char* name, OpenHRP::DynamicsSimulator::LinkDataType type, const OpenHRP::DblSequence& wdata);
00093 void getAllCharacterPositions(OpenHRP::CharacterPositionSequence& all_char_pos);
00094 void getAllSensorStates(OpenHRP::SensorStateSequence& all_sensor_states);
00095 void calcCharacterJacobian(const char* characterName, const char* baseLink, const char* targetLink, fMat& J);
00096
00097 void addCollisionCheckLinkPair(Joint* jnt1, Joint* jnt2, double staticFriction, double slipFriction, double epsilon);
00098
00099 void addCharacter(Joint* rjoint, const std::string& _name, OpenHRP::LinkInfoSequence_var links);
00100 Joint* rootJoint(int index);
00101 int numLinks(int index) {
00102 return characters[index].links.size();
00103 }
00104 int numJoints(int index) {
00105 return characters[index].n_joints;
00106 }
00107
00108 int numCharacter() {
00109 return characters.size();
00110 }
00111
00112 protected:
00113 pSim* chain;
00114 std::vector<SDContactPair*> contact_pairs;
00115 std::vector<Sensor*> sensors;
00116
00117 std::vector<CharacterInfo> characters;
00118
00119 private:
00120
00121 void _get_all_character_data_sub(Joint* cur, int index, OpenHRP::DynamicsSimulator::LinkDataType type, OpenHRP::DblSequence_out& rdata);
00122 void _set_all_character_data_sub(Joint* cur, int index, OpenHRP::DynamicsSimulator::LinkDataType type, const OpenHRP::DblSequence& wdata);
00123 void _get_all_sensor_states_sub(Joint* cur, int& count, OpenHRP::SensorState& state);
00124
00125 double currentTime_;
00126 double timeStep_;
00127
00128 void update_force_sensor(ForceSensor* fs);
00129 void update_rate_gyro_sensor(RateGyroSensor* rgs);
00130 void update_accel_sensor(AccelSensor* as);
00131 #if 0
00132 typedef std::map<std::string, int> NameToIndexMap;
00133 NameToIndexMap nameToBodyIndexMap;
00134
00135 typedef std::map<BodyPtr, int> BodyToIndexMap;
00136 BodyToIndexMap bodyToIndexMap;
00137
00138 struct LinkPairKey {
00139 BodyPtr body1;
00140 BodyPtr body2;
00141 Link* link1;
00142 Link* link2;
00143 bool operator<(const LinkPairKey& pair2) const;
00144 };
00145 typedef std::map<LinkPairKey, int> LinkPairKeyToIndexMap;
00146 LinkPairKeyToIndexMap linkPairKeyToIndexMap;
00147 #endif
00148
00149 int numRegisteredLinkPairs;
00150
00151 fVec3 g;
00152
00153 bool isEulerMethod;
00154
00155 bool sensorsAreEnabled;
00156
00157 };
00158
00159
00160 #endif