ODE_DynamicsSimulator_impl.h
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2008, AIST, the University of Tokyo and General Robotix Inc.
00003  * All rights reserved. This program is made available under the terms of the
00004  * Eclipse Public License v1.0 which accompanies this distribution, and is
00005  * available at http://www.eclipse.org/legal/epl-v10.html
00006  * Contributors:
00007  * National Institute of Advanced Industrial Science and Technology (AIST)
00008  * General Robotix Inc. 
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         // TODO
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


openhrp3
Author(s): AIST, General Robotix Inc., Nakamura Lab of Dept. of Mechano Informatics at University of Tokyo
autogenerated on Sun Apr 2 2017 03:43:55