World.h
Go to the documentation of this file.
00001 // -*- mode: c++; indent-tabs-mode: nil; tab-width: 4; c-basic-offset: 4; -*-
00002 /*
00003  * Copyright (c) 2008, AIST, the University of Tokyo and General Robotix Inc.
00004  * All rights reserved. This program is made available under the terms of the
00005  * Eclipse Public License v1.0 which accompanies this distribution, and is
00006  * available at http://www.eclipse.org/legal/epl-v10.html
00007  * Contributors:
00008  * The University of Tokyo
00009  * National Institute of Advanced Industrial Science and Technology (AIST)
00010  * General Robotix Inc. 
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 //              friend class CharacterInfo;
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 //              std::pair<int,bool> getIndexOfLinkPairs(BodyPtr body1, Link* link1, BodyPtr body2, Link* link2);
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; // Euler or Runge Kutta ?
00154 
00155                 bool sensorsAreEnabled;
00156                 
00157         };
00158 
00159 
00160 #endif


openhrp3
Author(s): AIST, General Robotix Inc., Nakamura Lab of Dept. of Mechano Informatics at University of Tokyo
autogenerated on Thu Apr 11 2019 03:30:19