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