UtDynamicsSimulator/DynamicsSimulator_impl.h

Go to the documentation of this file.

// -*- mode: c++; indent-tabs-mode: t; tab-width: 4; c-basic-offset: 4; -*-
/*
 * Copyright (c) 2008, AIST, the University of Tokyo and General Robotix Inc.
 * All rights reserved. This program is made available under the terms of the
 * Eclipse Public License v1.0 which accompanies this distribution, and is
 * available at http://www.eclipse.org/legal/epl-v10.html
 * Contributors:
 * The University of Tokyo
 * National Institute of Advanced Industrial Science and Technology (AIST)
 * General Robotix Inc. 
 */
#ifndef NEW_DYNAMICSSIMULATOR_IMPL_HEADER
#define NEW_DYNAMICSSIMULATOR_IMPL_HEADER
 
#include <boost/scoped_ptr.hpp>
 
#include <hrpCorba/ORBwrap.h>
#include <hrpCorba/ModelLoader.hh>
#include <hrpCorba/CollisionDetector.hh>
#include <hrpCorba/DynamicsSimulator.hh>
 
#include "World.h"
#include "TimeMeasure.h"
 
using namespace OpenHRP;
        class DynamicsSimulator_impl : virtual public POA_OpenHRP::DynamicsSimulator,
                                                                   virtual public PortableServer::RefCountServantBase
        {
                CORBA::ORB_var orb_;
 
                ::World world;
 
                CollisionDetector_var collisionDetector;
 
                CollisionSequence_var collisions;
                LinkPairSequence_var collidingLinkPairs;
 
                CharacterPositionSequence_var allCharacterPositions;
                bool needToUpdatePositions;
 
                SensorStateSequence_var allCharacterSensorStates;
                bool needToUpdateSensorStates;
 
                bool isFirstSimulationLoop;
 
                TimeMeasure timeMeasure1;
                TimeMeasure timeMeasure2;
                TimeMeasure timeMeasure3;
                bool timeMeasureFinished;
 
                void _setupCharacterData();
                void _updateCharacterPositions();
                void _updateSensorStates();
 
                void registerCollisionPair2CD(
                                const std::string &, const std::string &,
                                const std::string &, const std::string &);
 
        public:
 
                DynamicsSimulator_impl(CORBA::ORB_ptr orb);
 
                ~DynamicsSimulator_impl();
 
 
                virtual void destroy();
 
                virtual void registerCharacter(
                                const char *name,
                                BodyInfo_ptr cinfo);
 
                virtual void init(
                                CORBA::Double timeStep,
                                OpenHRP::DynamicsSimulator::IntegrateMethod integrateOpt,
                                OpenHRP::DynamicsSimulator::SensorOption sensorOpt);
    
                virtual void registerCollisionCheckPair(
                                const char* char1, 
                                const char* name1, 
                                const char* char2,
                                const char* name2,
                                CORBA::Double staticFriction, 
                                CORBA::Double slipFriction,
                                const DblSequence6& K,
                                const DblSequence6& C,
                const double culling_thresh,
                                const double restitution);
 
                virtual void registerIntersectionCheckPair(
                const char* char1, 
                                const char* name1, 
                                const char* char2,
                                const char* name2,
                                const double tolerance);
 
                virtual void registerExtraJoint
                (
                        const char*     charName1,
                        const char*     linkName1,
                        const char*     charName2,
                        const char*     linkName2,
                        const DblSequence3&     link1LocalPos,
                        const DblSequence3&     link2LocalPos,
                        const ExtraJointType jointType,
                        const DblSequence3&     jointAxis,
                        const char*                     extraJointName);
 
                // TODO
                virtual void getExtraJointConstraintForce(
                                const char* characterName, 
                                const char* extraJointName, 
                                DblSequence6_out contactForce);
 
                virtual void getCharacterSensorValues(
                                const char* characterName,
                                const char* sensorName,
                                DblSequence_out values);
 
                virtual void initSimulation();
 
                virtual void stepSimulation();
 
                virtual void setCharacterLinkData(
                                const char* characterName, 
                                const char* link, 
                                OpenHRP::DynamicsSimulator::LinkDataType type, 
                                const DblSequence& data);
 
                virtual void getCharacterLinkData(
                                const char* characterName,
                                const char* link,
                                OpenHRP::DynamicsSimulator::LinkDataType type,
                                DblSequence_out rdata);
 
                virtual void getCharacterAllLinkData(
                                const char* characterName,
                                OpenHRP::DynamicsSimulator::LinkDataType type,
                                DblSequence_out wdata);
   
                virtual void setCharacterAllLinkData(
                                const char* characterName, 
                                OpenHRP::DynamicsSimulator::LinkDataType type,
                                const DblSequence& wdata);
 
                virtual void setGVector(const DblSequence3& wdata);
 
                virtual void getGVector(DblSequence3_out wdata);
 
                virtual void setCharacterAllJointModes(
                                const char* characterName, 
                                OpenHRP::DynamicsSimulator::JointDriveMode jointMode);
 
                virtual CORBA::Boolean calcCharacterInverseKinematics(
                                const char* characterName,
                                const char* baseLink,
                                const char* targetLink,
                                const LinkPosition& target);
 
                virtual void calcCharacterForwardKinematics(
                                const char* characterName);
 
                virtual void calcWorldForwardKinematics();
                
                virtual void getWorldState(WorldState_out state);
 
                virtual void getCharacterSensorState(const char* characterName, SensorState_out sstate);
 
                virtual CORBA::Boolean getCharacterCollidingPairs(
                                const char* characterName, 
                                LinkPairSequence_out pairs);
 
                virtual void calcCharacterJacobian(
                                const char* characterName,
                                const char* baseLink,
                                const char* targetLink,
                                DblSequence_out jacobian);
 
                virtual bool checkCollision(bool checkAll);
 
                virtual LinkPairSequence *checkIntersection(CORBA::Boolean checkAll);
 
                virtual DistanceSequence *checkDistance();
        };
 
        class DynamicsSimulatorFactory_impl: virtual public POA_OpenHRP::DynamicsSimulatorFactory
        {
 
        private:
                CORBA::ORB_var orb_;
    
        public:
 
                DynamicsSimulatorFactory_impl(CORBA::ORB_ptr orb);
 
                ~DynamicsSimulatorFactory_impl();
                
                DynamicsSimulator_ptr create();
                
                void shutdown();
                
        };
 
#endif