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