AistDynamicsSimulator/DynamicsSimulator_impl.h

Go to the documentation of this file.

/*
 * 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:
 * National Institute of Advanced Industrial Science and Technology (AIST)
 * General Robotix Inc. 
 */
#ifndef OPENHRP_DYNAMICS_SIMULATOR_IMPL_H_INCLUDED
#define OPENHRP_DYNAMICS_SIMULATOR_IMPL_H_INCLUDED
 
 
#include <hrpCorba/ORBwrap.h>
#include <hrpCorba/ModelLoader.hh>
#include <hrpCorba/CollisionDetector.hh>
#include <hrpCorba/DynamicsSimulator.hh>
 
#include <hrpModel/World.h>
#include <hrpModel/ConstraintForceSolver.h>
#include <hrpUtil/TimeMeasure.h>
 
#include <boost/scoped_ptr.hpp>
 
using namespace OpenHRP;
 
class DynamicsSimulator_impl : virtual public POA_OpenHRP::DynamicsSimulator,
    virtual public PortableServer::RefCountServantBase
{
    CORBA::ORB_var orb_;
 
    hrp::World<hrp::ConstraintForceSolver> world;
 
    CollisionDetector_var collisionDetector;
 
    CollisionSequence_var         collisions;
    LinkPairSequence_var          collidingLinkPairs;
 
    CharacterPositionSequence_var allCharacterPositions;
    bool needToUpdatePositions;
 
    SensorStateSequence_var allCharacterSensorStates;
    bool needToUpdateSensorStates;
 
    TimeMeasure timeMeasure1;
    TimeMeasure timeMeasure2;
    TimeMeasure timeMeasure3;
    bool timeMeasureFinished;
    bool timeMeasureStarted;
 
    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 binfo);
 
    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 bool checkCollision(bool checkAll);
 
    virtual LinkPairSequence *checkIntersection(CORBA::Boolean checkAll);
 
    virtual DistanceSequence *checkDistance();
 
    virtual void getWorldState(WorldState_out wstate);
 
    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);
};
 
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