Simulator.h

Go to the documentation of this file.

/*
 * This file is part of ArmarX.
 *
 * Copyright (C) 2013-2016, High Performance Humanoid Technologies (H2T), Karlsruhe Institute of Technology (KIT), all rights reserved.
 *
 * ArmarX is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License version 2 as
 * published by the Free Software Foundation.
 *
 * ArmarX is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program. If not, see <http://www.gnu.org/licenses/>.
 *
 * @package    ArmarXSimulation::Simulator
 * @author     Nikolaus Vahrenkamp ( vahrenkamp at kit dot edu )
 * @date       2015
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#pragma once
 
#include <condition_variable>
#include <mutex>
 
#include <Eigen/Core>
#include <Eigen/Geometry>
 
#include <ArmarXCore/core/Component.h>
#include <ArmarXCore/core/application/properties/Properties.h>
#include <ArmarXCore/core/services/sharedmemory/IceSharedMemoryProvider.h>
#include <ArmarXCore/core/services/tasks/PeriodicTask.h>
#include <ArmarXCore/interface/core/PackagePath.h>
#include <ArmarXCore/interface/core/TimeServerInterface.h>
#include <ArmarXCore/interface/events/SimulatorResetEvent.h>
 
#include "RobotAPI/libraries/ArmarXObjects/forward_declarations.h"
#include <RobotAPI/interface/core/RobotState.h>
#include <RobotAPI/libraries/ArmarXObjects/ObjectFinder.h>
#include <RobotAPI/libraries/core/Pose.h>
 
#include <ArmarXSimulation/interface/simulator/SimulatorInterface.h>
 
#include "BulletPhysicsWorld.h"
#include "KinematicsWorld.h"
#include "SimulatedWorld.h"
#include "SimulatorTimeServerProxy.h"
#include <MemoryX/core/GridFileManager.h>
#include <MemoryX/interface/components/LongtermMemoryInterface.h>
#include <MemoryX/interface/components/PriorKnowledgeInterface.h>
#include <MemoryX/interface/components/WorkingMemoryInterface.h>
#include <MemoryX/interface/gui/EntityDrawerInterface.h>
#include <MemoryX/libraries/helpers/VirtualRobotHelpers/SimoxObjectWrapper.h>
#include <MemoryX/libraries/memorytypes/entity/ObjectClass.h>
 
 
#define MAX_INITIAL_ROBOT_COUNT 4
 
namespace armarx
{
 
    class SimulatorTimeServerProxy;
    using SimulatorTimeServerProxyPtr = IceInternal::Handle<SimulatorTimeServerProxy>;
 
    enum class SimulatorType
    {
        Kinematics,
        Bullet,
        Mujoco
    };
 
    /**
     * @class SimulatorPropertyDefinitions
     * @brief
     */
    class SimulatorPropertyDefinitions : public ComponentPropertyDefinitions
    {
    public:
        SimulatorPropertyDefinitions(std::string prefix);
        ~SimulatorPropertyDefinitions() override;
    };
 
    /**
     * @brief The Simulator class holds an instance of the AmrarXPhysicsWorld and communicates to ArmarX.
     *
     * The following optional parameters can be used.
     * - RobotFileName<br>
     *   Simox/VirtualRobot robot file name, e.g. robot_model.xml.
     * - InitialRobotPose.x/y/z/roll/pitch/yaw<br>
     *   The initial pose of the robot
     * - LogRobot<br>
     *   Default: false, If true, the complete robot state is logged to a file.
     * - RobotControllerPID.p/i/d<br>
     *   Setup robot controllers: PID paramters
     * - LongtermMemory.SnapshotName<br>
     *   Name of snapshot to load the scene
     * - SceneFileName<br>
     *   Simox/VirtualRobot scene file name, e.g. myScene.xml. Usually, sceneas are loaded via MemoryX-snapshots, but in order to test a setup, a scene can be imported directly.
     * - FloorPlane<br>
     *   Indicates if the should be created or not. Default: true.
     * - FloorTexture<br>
     *   The texture file for the floor. By default, a standard texture is used
     * - FixedTimeStepLoopNrSteps<br>
     *   The maximal number of internal simulation loops (fixed time step mode). After the maximum number of internal steps is performed, the simulator interpolates the remaining time.
     * - FixedTimeStepStepTimeMS<br>
     *   The simulation's internal timestep (fixed time step mode)
     * - ReportVisuFrequency<br>
     *   How often should the visualization data be published. Value is given in Hz. (0 to disable, default: 30)
     * - ReportVisuTopicName<br>
     *   The topic on which the visualization updates are published. Default: ReportVisuTopicName
     * - ReportDataFrequency<br>
     *   How often should the robot data be published. Value is given in Hz. (0 to disable, default: 30)
     */
    class Simulator :
        virtual public SimulatorInterface,
        virtual public Component,
        virtual protected IceUtil::Thread,
        virtual public TimeServerInterface
    {
    public:
        Simulator();
        ~Simulator() override;
 
 
        // inherited from Component
        static std::string GetDefaultName();
        std::string getDefaultName() const override;
 
        void onInitComponent() override;
        void onConnectComponent() override;
        void onDisconnectComponent() override;
        void onExitComponent() override;
 
        // inherited from Thread
        void run() override;
 
        // inherited from TimeServerInterface
        void stop(const Ice::Current& c = Ice::emptyCurrent) override;
 
        /**
         * @brief setSpeed sets the scaling factor for the speed of passing of time
         * e.g. setSpeed(2) leads to 2 virtual seconds passing every 1 real second
         * (provided the simulation is able to keep up)
         * @param newSpeed new scaling factor for speed
         */
        void setSpeed(Ice::Float newSpeed, const Ice::Current& c = Ice::emptyCurrent) override;
        Ice::Float getSpeed(const Ice::Current& c = Ice::emptyCurrent) override;
        Ice::Long getTime(const Ice::Current& c = Ice::emptyCurrent) override;
        Ice::Int getStepTimeMS(const ::Ice::Current& = Ice::emptyCurrent) override;
 
        // inherited from SimulatorDataInterface
        void actuateRobotJoints(const std::string& robotName,
                                const NameValueMap& angles,
                                const NameValueMap& velocities,
                                const Ice::Current& c = Ice::emptyCurrent) override;
        void actuateRobotJointsPos(const std::string& robotName,
                                   const NameValueMap& angles,
                                   const Ice::Current& c = Ice::emptyCurrent) override;
        void actuateRobotJointsVel(const std::string& robotName,
                                   const NameValueMap& velocities,
                                   const Ice::Current& c = Ice::emptyCurrent) override;
        void actuateRobotJointsTorque(const std::string& robotName,
                                      const NameValueMap& torques,
                                      const Ice::Current& c = Ice::emptyCurrent) override;
        void setRobotPose(const std::string& robotName,
                          const PoseBasePtr& globalPose,
                          const Ice::Current& c = Ice::emptyCurrent) override;
 
        void applyForceRobotNode(const std::string& robotName,
                                 const std::string& robotNodeName,
                                 const Vector3BasePtr& force,
                                 const Ice::Current& c = Ice::emptyCurrent) override;
        void applyTorqueRobotNode(const std::string& robotName,
                                  const std::string& robotNodeName,
                                  const Vector3BasePtr& torque,
                                  const Ice::Current& c = Ice::emptyCurrent) override;
 
        void applyForceObject(const std::string& objectName,
                              const Vector3BasePtr& force,
                              const Ice::Current& c = Ice::emptyCurrent) override;
        void applyTorqueObject(const std::string& objectName,
                               const Vector3BasePtr& torque,
                               const Ice::Current& c = Ice::emptyCurrent) override;
 
        // "old api => should be removed"
        std::string addRobot(const std::string& filename,
                             const Ice::Current& c = Ice::emptyCurrent) override;
        std::string addScaledRobot(const std::string& filename,
                                   float scale,
                                   const Ice::Current& c = Ice::emptyCurrent) override;
        std::string addScaledRobotName(const std::string& instanceName,
                                       const std::string& filename,
                                       float scale,
                                       const Ice::Current& c = Ice::emptyCurrent) override;
        bool removeRobot(const std::string& robotName, const Ice::Current& c = Ice::emptyCurrent);
        void addObject(const memoryx::ObjectClassBasePtr& objectClassBase,
                       const std::string& instanceName,
                       const PoseBasePtr& globalPose,
                       bool isStatic = false,
                       const Ice::Current& c = Ice::emptyCurrent) override;
 
        // "new api" =>
        std::string addRobotFromFile(const armarx::data::PackagePath& packagePath,
                                     const Ice::Current& c = Ice::emptyCurrent) override;
        void addObjectFromFile(const armarx::data::PackagePath& packagePath,
                               const std::string& instanceName,
                               const PoseBasePtr& globalPose,
                               bool isStatic = false,
                               const Ice::Current& c = Ice::emptyCurrent) override;
 
 
        void addBox(float width,
                    float height,
                    float depth,
                    float massKG,
                    const DrawColor& color,
                    const std::string& instanceName,
                    const PoseBasePtr& globalPose,
                    bool isStatic = false,
                    const Ice::Current& c = Ice::emptyCurrent) override;
 
        Ice::StringSeq getRobotNames(const Ice::Current&) override;
        void removeObject(const std::string& instanceName, const Ice::Current&) override;
 
        bool hasObject(const std::string& instanceName,
                       const Ice::Current& c = Ice::emptyCurrent) override;
        void setObjectPose(const std::string& objectName,
                           const PoseBasePtr& globalPose,
                           const Ice::Current& c = Ice::emptyCurrent) override;
        void setObjectSimulationType(const std::string& objectName,
                                     armarx::SimulationType type,
                                     const Ice::Current& c = Ice::emptyCurrent) override;
        void activateObject(const std::string& objectName,
                            const Ice::Current& c = Ice::emptyCurrent) override;
 
        bool hasRobot(const std::string& robotName,
                      const Ice::Current& c = Ice::emptyCurrent) override;
        bool hasRobotNode(const std::string& robotName,
                          const std::string& robotNodeName,
                          const Ice::Current& c = Ice::emptyCurrent) override;
        float getRobotMass(const std::string& robotName,
                           const Ice::Current& c = Ice::emptyCurrent) override;
 
        NameValueMap getRobotJointAngles(const std::string& robotName,
                                         const Ice::Current& c = Ice::emptyCurrent) override;
        float getRobotJointAngle(const std::string& robotName,
                                 const std::string& nodeName,
                                 const Ice::Current& c = Ice::emptyCurrent) override;
        NameValueMap getRobotJointVelocities(const std::string& robotName,
                                             const Ice::Current& c = Ice::emptyCurrent) override;
        float getRobotJointVelocity(const std::string& robotName,
                                    const std::string& nodeName,
                                    const Ice::Current& c = Ice::emptyCurrent) override;
        NameValueMap getRobotJointTorques(const std::string& robotName,
                                          const Ice::Current& c = Ice::emptyCurrent) override;
        ForceTorqueDataSeq
        getRobotForceTorqueSensors(const std::string& robotName,
                                   const Ice::Current& c = Ice::emptyCurrent) override;
 
        float getRobotJointLimitLo(const std::string& robotName,
                                   const std::string& nodeName,
                                   const Ice::Current& c = Ice::emptyCurrent) override;
        float getRobotJointLimitHi(const std::string& robotName,
                                   const std::string& nodeName,
                                   const Ice::Current& c = Ice::emptyCurrent) override;
        PoseBasePtr getRobotPose(const std::string& robotName,
                                 const Ice::Current& c = Ice::emptyCurrent) override;
        SimulatedRobotState getRobotState(const std::string& robotName,
                                          const Ice::Current&) override;
 
        float getRobotMaxTorque(const std::string& robotName,
                                const std::string& nodeName,
                                const Ice::Current& c = Ice::emptyCurrent) override;
        void setRobotMaxTorque(const std::string& robotName,
                               const std::string& nodeName,
                               float maxTorque,
                               const Ice::Current& c = Ice::emptyCurrent) override;
 
        PoseBasePtr getRobotNodePose(const std::string& robotName,
                                     const std::string& robotNodeName,
                                     const Ice::Current& c = Ice::emptyCurrent) override;
 
        Vector3BasePtr getRobotLinearVelocity(const std::string& robotName,
                                              const std::string& nodeName,
                                              const Ice::Current& c = Ice::emptyCurrent) override;
        Vector3BasePtr getRobotAngularVelocity(const std::string& robotName,
                                               const std::string& nodeName,
                                               const Ice::Current& c = Ice::emptyCurrent) override;
 
        void setRobotLinearVelocity(const std::string& robotName,
                                    const std::string& robotNodeName,
                                    const Vector3BasePtr& vel,
                                    const Ice::Current& c = Ice::emptyCurrent) override;
        void setRobotAngularVelocity(const std::string& robotName,
                                     const std::string& robotNodeName,
                                     const Vector3BasePtr& vel,
                                     const Ice::Current& c = Ice::emptyCurrent) override;
        void
        setRobotLinearVelocityRobotRootFrame(const std::string& robotName,
                                             const std::string& robotNodeName,
                                             const Vector3BasePtr& vel,
                                             const Ice::Current& c = Ice::emptyCurrent) override;
        void
        setRobotAngularVelocityRobotRootFrame(const std::string& robotName,
                                              const std::string& robotNodeName,
                                              const Vector3BasePtr& vel,
                                              const Ice::Current& c = Ice::emptyCurrent) override;
 
        //! create a joint
        void objectGrasped(const std::string& robotName,
                           const std::string& robotNodeName,
                           const std::string& objectName,
                           const Ice::Current& c = Ice::emptyCurrent) override;
 
        //! remove a joint
        void objectReleased(const std::string& robotName,
                            const std::string& robotNodeName,
                            const std::string& objectName,
                            const Ice::Current& c = Ice::emptyCurrent) override;
 
        PoseBasePtr getObjectPose(const std::string& objectName,
                                  const Ice::Current& c = Ice::emptyCurrent) override;
 
        DistanceInfo getDistance(const std::string& robotName,
                                 const std::string& robotNodeName,
                                 const std::string& worldObjectName,
                                 const Ice::Current& c = Ice::emptyCurrent) override;
        DistanceInfo getRobotNodeDistance(const std::string& robotName,
                                          const std::string& robotNodeName1,
                                          const std::string& robotNodeName2,
                                          const Ice::Current& c = Ice::emptyCurrent) override;
        void showContacts(bool enable,
                          const std::string& layerName,
                          const Ice::Current& c = Ice::emptyCurrent) override;
 
        /*!
         * \brief Returns a list of all contacts. Note that you must call updateContacts() first to enable
         * contacts handling.
         */
        ContactInfoSequence getContacts(const Ice::Current& c = Ice::emptyCurrent) override;
 
        /*!
         * \brief Enables the handling of contacts. If you intend to use getContacts(), call this method
         * with enabled = true first.
         */
        void updateContacts(bool enable, const Ice::Current& c = Ice::emptyCurrent) override;
 
        ObjectClassInformationSequence
        getObjectClassPoses(const std::string& robotName,
                            const std::string& frameName,
                            const std::string& className,
                            const Ice::Current& c = Ice::emptyCurrent) override;
 
 
        /*!
         * \brief reInitialize Re-initializes the scene.
         * Removes all robots and objects (and, in case the scene was loaded via a MemoryX snapshot, the working memory is cleared)
         * Then, the setup is re-loaded similar to the startup procedure.
         */
        void reInitialize(const Ice::Current& c = Ice::emptyCurrent) override;
 
 
        float getSimTime(const Ice::Current& c = Ice::emptyCurrent) override;
 
 
        SceneVisuData getScene(const Ice::Current& c = Ice::emptyCurrent) override;
        SimulatorInformation
        getSimulatorInformation(const ::Ice::Current& = Ice::emptyCurrent) override;
 
        /*!
         * \brief
         * \return The number of steps * the timestep in MS
         */
        int getFixedTimeStepMS(const Ice::Current& c = Ice::emptyCurrent) override;
 
        void start(const Ice::Current& c = Ice::emptyCurrent) override;
        void pause(const Ice::Current& c = Ice::emptyCurrent) override;
        void step(const Ice::Current& c = Ice::emptyCurrent) override;
        bool isRunning(const Ice::Current& c = Ice::emptyCurrent) override;
 
        bool addSnapshot(memoryx::WorkingMemorySnapshotInterfacePrx snapshotInterfacePrx);
        bool
        loadAgentsFromSnapshot(memoryx::WorkingMemorySnapshotInterfacePrx snapshotInterfacePrx);
 
        /**
         * @see PropertyUser::createPropertyDefinitions()
         */
        PropertyDefinitionsPtr
        createPropertyDefinitions() override
        {
            return PropertyDefinitionsPtr(new SimulatorPropertyDefinitions(getConfigIdentifier()));
        }
 
 
    protected:
        void simulationLoop();
        void reportVisuLoop();
        void reportDataLoop();
        /**
         * @brief stop the simulation and join the simulation thread
         */
        void shutdownSimulationLoop();
 
        /*!
         * \brief resetData Clears all data
         * \param clearWorkingMemory If set, the working memory is also cleared.
         */
        void resetData(bool clearWorkingMemory = false);
 
        void initializeData();
 
        SimulatedRobotState stateFromRobotInfo(RobotInfo const& robot, IceUtil::Time timestamp);
 
        /// Returns the name. On failure an empty string is returned.
        std::string
        addRobot(std::string robotInstanceName,
                 const std::string& robFileGlobal,
                 Eigen::Matrix4f gp,
                 const std::string& robFile,
                 double pid_p = 10.0,
                 double pid_i = 0.0,
                 double pid_d = 0.0,
                 bool isStatic = false,
                 float scaling = 1,
                 bool colModel = false,
                 std::map<std::string, float> initConfig = std::map<std::string, float>(),
                 bool selfCollisions = false);
        bool updateRobotTopics();
 
        memoryx::GridFileManagerPtr requestFileManager();
 
 
    protected:
        /// The sim task
        PeriodicTask<Simulator>::pointer_type simulationTask;
        /// The report visu task
        PeriodicTask<Simulator>::pointer_type reportVisuTask;
        /// The report data task
        PeriodicTask<Simulator>::pointer_type reportDataTask;
 
        // simulator thread control
        bool simulatorThreadRunning = false;
        bool simulatorThreadShutdown = false;
        std::mutex simulatorRunningMutex;
        std::condition_variable condSimulatorThreadRunning;
 
        SimulatedWorldPtr physicsWorld;
 
 
        /// Stores the time that was needed for communictaion
        float currentComTimeMS = 0;
        /// Stores the time that was needed to perform the last simulation loop
        float currentSimTimeMS = 0;
        /// stores the time that was needed to sync the data
        float currentSyncTimeMS = 0;
 
        /// Scaling factor for the passing of time
        float timeServerSpeed = 0;
 
        TimeServerListenerPrx timeTopicPrx;
 
        GlobalRobotPoseLocalizationListenerPrx globalRobotLocalization;
        bool reportRobotPose = false;
 
        SimulatorListenerInterfacePrx simulatorVisuUpdateListenerPrx;
 
        std::atomic<float> reportVisuTimeMS = 0;
 
        std::string priorKnowledgeName;
        std::string commonStorageName;
        memoryx::WorkingMemoryInterfacePrx memoryPrx;
        memoryx::PriorKnowledgeInterfacePrx priorKnowledgePrx;
        memoryx::LongtermMemoryInterfacePrx longtermMemoryPrx;
        memoryx::GridFileManagerPtr fileManager;
 
        /// Constructed once based on the `ObjectPackage` property
        ObjectFinder objectFinder;
 
        /// Drawing contacts
        memoryx::EntityDrawerInterfacePrx entityDrawerPrx;
 
        bool publishContacts = false;
        std::string contactLayerName;
 
        int lastPublishedContacts = 0;
 
        /// Stores all instances that belong to a class
        std::map<std::string, std::vector<std::string>> classInstanceMap;
 
        /**
         * Proxy object to offer the Timeserver proxy
         *
         * This is a hack to allow offering the proxies "Simulator" and "MasterTimeServer" at once
         */
        SimulatorTimeServerProxyPtr timeserverProxy;
        SimulatorResetEventPrx simulatorResetEventTopic;
 
 
    private:
        void addRobotsFromProperties();
        void addSceneFromFile(const std::string& sceneFile);
        void addSceneFromMemorySnapshot(const std::string& snapshotName);
 
        /**
         * Add a scene defined in the ArmarX JSON format to the simulation.
         */
        void addSceneFromJsonFile(const std::string& scenePath);
 
        /**
         * Add a scene defined in the SDF format to the simulation.
         */
        void addSceneFromSdfFile(const std::string& scenePath);
 
        /**
         * Add one object from an ArmarX JSON scene to the simulation.
         */
        void addJsonSceneObject(const armarx::objects::SceneObject object,
                                std::map<ObjectID, int>& idCounters);
 
        void fixateRobotNode(const std::string& robotName, const std::string& robotNodeName);
        void fixateObject(const std::string& objectName);
    };
 
    using SimulatorPtr = IceInternal::Handle<Simulator>;
} // namespace armarx