ODE_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 ODE_DYNAMICS_SIMULATOR_IMPL_H_INCLUDED
#define ODE_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>

#include <ode/ode.h>
#include "ODE_World.h"
#include "ODE_Link.h"
using namespace OpenHRP;

class ODE_DynamicsSimulator_impl : virtual public POA_OpenHRP::DynamicsSimulator,
    virtual public PortableServer::RefCountServantBase
{
    CORBA::ORB_var orb_;

    ODE_World 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:

    ODE_DynamicsSimulator_impl(CORBA::ORB_ptr orb);

    ~ODE_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