00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012 #ifndef NEW_DYNAMICSSIMULATOR_IMPL_HEADER
00013 #define NEW_DYNAMICSSIMULATOR_IMPL_HEADER
00014
00019 #include <boost/scoped_ptr.hpp>
00020
00021 #include <hrpCorba/ORBwrap.h>
00022 #include <hrpCorba/ModelLoader.hh>
00023 #include <hrpCorba/CollisionDetector.hh>
00024 #include <hrpCorba/DynamicsSimulator.hh>
00025
00026 #include "World.h"
00027 #include "TimeMeasure.h"
00028
00029 using namespace OpenHRP;
00033 class DynamicsSimulator_impl : virtual public POA_OpenHRP::DynamicsSimulator,
00034 virtual public PortableServer::RefCountServantBase
00035 {
00039 CORBA::ORB_var orb_;
00040
00041 ::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 bool isFirstSimulationLoop;
00055
00056 TimeMeasure timeMeasure1;
00057 TimeMeasure timeMeasure2;
00058 TimeMeasure timeMeasure3;
00059 bool timeMeasureFinished;
00060
00061 void _setupCharacterData();
00062 void _updateCharacterPositions();
00063 void _updateSensorStates();
00064
00065 void registerCollisionPair2CD(
00066 const std::string &, const std::string &,
00067 const std::string &, const std::string &);
00068
00069 public:
00070
00071 DynamicsSimulator_impl(CORBA::ORB_ptr orb);
00072
00073 ~DynamicsSimulator_impl();
00074
00075
00076 virtual void destroy();
00077
00078 virtual void registerCharacter(
00079 const char *name,
00080 BodyInfo_ptr cinfo);
00081
00082 virtual void init(
00083 CORBA::Double timeStep,
00084 OpenHRP::DynamicsSimulator::IntegrateMethod integrateOpt,
00085 OpenHRP::DynamicsSimulator::SensorOption sensorOpt);
00086
00087 virtual void registerCollisionCheckPair(
00088 const char* char1,
00089 const char* name1,
00090 const char* char2,
00091 const char* name2,
00092 CORBA::Double staticFriction,
00093 CORBA::Double slipFriction,
00094 const DblSequence6& K,
00095 const DblSequence6& C,
00096 const double culling_thresh,
00097 const double restitution);
00098
00099 virtual void registerIntersectionCheckPair(
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 const char* characterName,
00121 const char* extraJointName,
00122 DblSequence6_out contactForce);
00123
00124 virtual void getCharacterSensorValues(
00125 const char* characterName,
00126 const char* sensorName,
00127 DblSequence_out values);
00128
00129 virtual void initSimulation();
00130
00131 virtual void stepSimulation();
00132
00133 virtual void setCharacterLinkData(
00134 const char* characterName,
00135 const char* link,
00136 OpenHRP::DynamicsSimulator::LinkDataType type,
00137 const DblSequence& data);
00138
00139 virtual void getCharacterLinkData(
00140 const char* characterName,
00141 const char* link,
00142 OpenHRP::DynamicsSimulator::LinkDataType type,
00143 DblSequence_out rdata);
00144
00145 virtual void getCharacterAllLinkData(
00146 const char* characterName,
00147 OpenHRP::DynamicsSimulator::LinkDataType type,
00148 DblSequence_out wdata);
00149
00150 virtual void setCharacterAllLinkData(
00151 const char* characterName,
00152 OpenHRP::DynamicsSimulator::LinkDataType type,
00153 const DblSequence& wdata);
00154
00155 virtual void setGVector(const DblSequence3& wdata);
00156
00157 virtual void getGVector(DblSequence3_out wdata);
00158
00159 virtual void setCharacterAllJointModes(
00160 const char* characterName,
00161 OpenHRP::DynamicsSimulator::JointDriveMode jointMode);
00162
00164 virtual CORBA::Boolean calcCharacterInverseKinematics(
00165 const char* characterName,
00166 const char* baseLink,
00167 const char* targetLink,
00168 const LinkPosition& target);
00169
00170 virtual void calcCharacterForwardKinematics(
00171 const char* characterName);
00172
00173 virtual void calcWorldForwardKinematics();
00174
00175 virtual void getWorldState(WorldState_out state);
00176
00177 virtual void getCharacterSensorState(const char* characterName, SensorState_out sstate);
00178
00179 virtual CORBA::Boolean getCharacterCollidingPairs(
00180 const char* characterName,
00181 LinkPairSequence_out pairs);
00182
00183 virtual void calcCharacterJacobian(
00184 const char* characterName,
00185 const char* baseLink,
00186 const char* targetLink,
00187 DblSequence_out jacobian);
00188
00189 virtual bool checkCollision(bool checkAll);
00190
00191 virtual LinkPairSequence *checkIntersection(CORBA::Boolean checkAll);
00192
00193 virtual DistanceSequence *checkDistance();
00194 };
00195
00203 class DynamicsSimulatorFactory_impl: virtual public POA_OpenHRP::DynamicsSimulatorFactory
00204 {
00205
00206 private:
00210 CORBA::ORB_var orb_;
00211
00212 public:
00213
00218 DynamicsSimulatorFactory_impl(CORBA::ORB_ptr orb);
00219
00223 ~DynamicsSimulatorFactory_impl();
00224
00228 DynamicsSimulator_ptr create();
00229
00230 void shutdown();
00231
00232 };
00233
00234 #endif