00001
00002
00003
00004
00005
00006
00007
00008
00009
00010 #ifndef ODE_DYNAMICS_SIMULATOR_IMPL_H_INCLUDED
00011 #define ODE_DYNAMICS_SIMULATOR_IMPL_H_INCLUDED
00012
00013
00014 #include <hrpCorba/ORBwrap.h>
00015 #include <hrpCorba/ModelLoader.hh>
00016 #include <hrpCorba/CollisionDetector.hh>
00017 #include <hrpCorba/DynamicsSimulator.hh>
00018
00019 #include <hrpModel/World.h>
00020 #include <hrpModel/ConstraintForceSolver.h>
00021 #include <hrpUtil/TimeMeasure.h>
00022
00023 #include <boost/scoped_ptr.hpp>
00024
00025 #include <ode/ode.h>
00026 #include "ODE_World.h"
00027 #include "ODE_Link.h"
00028 using namespace OpenHRP;
00029
00033 class ODE_DynamicsSimulator_impl : virtual public POA_OpenHRP::DynamicsSimulator,
00034 virtual public PortableServer::RefCountServantBase
00035 {
00039 CORBA::ORB_var orb_;
00040
00041 ODE_World world;
00042
00043 CollisionDetector_var collisionDetector;
00044
00045 CollisionSequence_var collisions;
00046 LinkPairSequence_var collidingLinkPairs;
00047
00048 CharacterPositionSequence_var allCharacterPositions;
00049 bool needToUpdatePositions;
00050
00051 SensorStateSequence_var allCharacterSensorStates;
00052 bool needToUpdateSensorStates;
00053
00054 TimeMeasure timeMeasure1;
00055 TimeMeasure timeMeasure2;
00056 TimeMeasure timeMeasure3;
00057 bool timeMeasureFinished;
00058 bool timeMeasureStarted;
00059
00060 void _setupCharacterData();
00061 void _updateCharacterPositions();
00062 void _updateSensorStates();
00063
00064 void registerCollisionPair2CD
00065 (
00066 const std::string &, const std::string &,
00067 const std::string &, const std::string &
00068 );
00069
00070 public:
00071
00072 ODE_DynamicsSimulator_impl(CORBA::ORB_ptr orb);
00073
00074 ~ODE_DynamicsSimulator_impl();
00075
00076
00077 virtual void destroy();
00078
00079 virtual void registerCharacter(const char *name, BodyInfo_ptr binfo);
00080
00081 virtual void init(CORBA::Double timeStep,
00082 OpenHRP::DynamicsSimulator::IntegrateMethod integrateOpt,
00083 OpenHRP::DynamicsSimulator::SensorOption sensorOpt);
00084
00085 virtual void registerCollisionCheckPair
00086 (
00087 const char* char1,
00088 const char* name1,
00089 const char* char2,
00090 const char* name2,
00091 CORBA::Double staticFriction,
00092 CORBA::Double slipFriction,
00093 const DblSequence6& K,
00094 const DblSequence6& C,
00095 const double culling_thresh,
00096 const double restitution);
00097
00098 virtual void registerIntersectionCheckPair
00099 (
00100 const char* char1,
00101 const char* name1,
00102 const char* char2,
00103 const char* name2,
00104 const double tolerance);
00105
00106 virtual void registerExtraJoint
00107 (
00108 const char* charName1,
00109 const char* linkName1,
00110 const char* charName2,
00111 const char* linkName2,
00112 const DblSequence3& link1LocalPos,
00113 const DblSequence3& link2LocalPos,
00114 const ExtraJointType jointType,
00115 const DblSequence3& jointAxis,
00116 const char* extraJointName);
00117
00118
00119 virtual void getExtraJointConstraintForce
00120 (
00121 const char* characterName,
00122 const char* extraJointName,
00123 DblSequence6_out contactForce);
00124
00125 virtual void getCharacterSensorValues
00126 (
00127 const char* characterName,
00128 const char* sensorName,
00129 DblSequence_out values);
00130
00131 virtual void initSimulation();
00132
00133 virtual void stepSimulation();
00134
00135 virtual void setCharacterLinkData
00136 (
00137 const char* characterName,
00138 const char* link,
00139 OpenHRP::DynamicsSimulator::LinkDataType type,
00140 const DblSequence& data);
00141
00142 virtual void getCharacterLinkData
00143 (
00144 const char* characterName,
00145 const char* link,
00146 OpenHRP::DynamicsSimulator::LinkDataType type,
00147 DblSequence_out rdata);
00148
00149 virtual void getCharacterAllLinkData
00150 (
00151 const char* characterName,
00152 OpenHRP::DynamicsSimulator::LinkDataType type,
00153 DblSequence_out wdata);
00154
00155 virtual void setCharacterAllLinkData
00156 (
00157 const char* characterName,
00158 OpenHRP::DynamicsSimulator::LinkDataType type,
00159 const DblSequence& wdata);
00160
00161 virtual void setGVector(const DblSequence3& wdata);
00162
00163 virtual void getGVector(DblSequence3_out wdata);
00164
00165 virtual void setCharacterAllJointModes
00166 (
00167 const char* characterName,
00168 OpenHRP::DynamicsSimulator::JointDriveMode jointMode);
00169
00170 virtual CORBA::Boolean calcCharacterInverseKinematics
00171 (
00172 const char* characterName,
00173 const char* baseLink,
00174 const char* targetLink,
00175 const LinkPosition& target);
00176
00177 virtual void calcCharacterForwardKinematics
00178 (
00179 const char* characterName);
00180
00181 virtual void calcWorldForwardKinematics();
00182
00183 virtual bool checkCollision(bool checkAll);
00184
00185 virtual LinkPairSequence *checkIntersection(CORBA::Boolean checkAll);
00186
00187 virtual DistanceSequence *checkDistance();
00188
00189 virtual void getWorldState(WorldState_out wstate);
00190
00191 virtual void getCharacterSensorState(const char* characterName, SensorState_out sstate);
00192
00193 virtual CORBA::Boolean getCharacterCollidingPairs
00194 (
00195 const char* characterName,
00196 LinkPairSequence_out pairs);
00197
00198 virtual void calcCharacterJacobian
00199 (
00200 const char* characterName,
00201 const char* baseLink,
00202 const char* targetLink,
00203 DblSequence_out jacobian);
00204 };
00205
00213 class DynamicsSimulatorFactory_impl: virtual public POA_OpenHRP::DynamicsSimulatorFactory
00214 {
00215
00216 private:
00220 CORBA::ORB_var orb_;
00221
00222 public:
00223
00229 DynamicsSimulatorFactory_impl(CORBA::ORB_ptr orb);
00230
00234 ~DynamicsSimulatorFactory_impl();
00235
00239 DynamicsSimulator_ptr create();
00240
00241 void shutdown();
00242
00243 };
00244
00245 #endif