SimulatedWorld.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       2016
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#pragma once
 
 
#include <map>
#include <memory>
#include <mutex>
#include <string>
#include <vector>
 
#include <Eigen/Core>
#include <Eigen/Geometry>
 
#include <IceUtil/Time.h>
 
#include <VirtualRobot/SceneObject.h>
#include <VirtualRobot/VirtualRobot.h>
 
#include <ArmarXCore/core/logging/Logging.h>
 
#include <RobotAPI/interface/core/NameValueMap.h>
#include <RobotAPI/interface/core/PoseBase.h>
#include <RobotAPI/libraries/core/FramedPose.h>
#include <RobotAPI/libraries/core/Pose.h>
 
#include <ArmarXSimulation/interface/simulator/SimulatorInterface.h>
 
#include <SimDynamics/DynamicsEngine/DynamicsEngine.h>
 
namespace armarx
{
 
    struct ForceTorqueInfo
    {
        ForceTorqueInfo()
        {
            currentForce.setZero();
            currentTorque.setZero();
            enable = true;
        }
 
        std::string robotName;
        std::string robotNodeName;
        std::string sensorName;
        VirtualRobot::ForceTorqueSensorPtr
            ftSensor; // be sure to just access this object when the engine mutex is set!
        bool enable;
        Eigen::Vector3f currentForce;
        Eigen::Vector3f currentTorque;
 
        std::string topicName;
        SimulatorForceTorqueListenerInterfacePrx prx;
    };
 
    using ForceTorqueInfoPtr = std::shared_ptr<ForceTorqueInfo>;
 
    struct RobotInfo
    {
    public:
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
        RobotInfo()
        {
            linearVelocity.setZero();
            angularVelocity.setZero();
            pose.setIdentity();
        }
 
        Eigen::Matrix4f pose;
        std::map<std::string, Eigen::Matrix4f> robotNodePoses;
 
        // the topic on which we publish the robot's data
        std::string robotName;
        std::string robotTopicName;
        SimulatorRobotListenerInterfacePrx simulatorRobotListenerPrx;
 
        // current values to be reported
        NameValueMap jointAngles;
        NameValueMap jointVelocities;
        NameValueMap jointTorques;
 
        Eigen::Vector3f linearVelocity;
        Eigen::Vector3f angularVelocity;
 
        std::vector<ForceTorqueInfo> forceTorqueSensors;
    };
 
    using RobotInfoPtr = std::shared_ptr<RobotInfo>;
 
    /*!
     * \brief The SimulatedWorldData class This data is queried by the simulated in order to offer it via topics.
     */
    class SimulatedWorldData
    {
    public:
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
        std::vector<RobotInfo> robots;
        IceUtil::Time timestamp;
        // todo: objects (currently no need to populate objects via this channel)
    };
 
    using SimualtedWorldDataPtr = std::shared_ptr<SimulatedWorldData>;
 
    template <typename>
    struct argType;
 
    template <typename R, typename A>
    struct argType<R (SimDynamics::DynamicsEngine::*)(A)>
    {
        using type = A;
    };
 
    /*!
     * \brief The SimulatedWorld class encapsulates the whole physics simulation and the corresponding data
     *
     */
    class SimulatedWorld : public Logging
    {
    public:
        SimulatedWorld();
        virtual ~SimulatedWorld() override = default;
 
 
        // needs to be implemented by derived classes
        virtual void actuateRobotJoints(const std::string& robotName,
                                        const std::map<std::string, float>& angles,
                                        const std::map<std::string, float>& velocities) = 0;
        virtual void actuateRobotJointsPos(const std::string& robotName,
                                           const std::map<std::string, float>& angles) = 0;
        virtual void actuateRobotJointsVel(const std::string& robotName,
                                           const std::map<std::string, float>& velocities) = 0;
        virtual void actuateRobotJointsTorque(const std::string& robotName,
                                              const std::map<std::string, float>& torques) = 0;
        virtual void setRobotPose(const std::string& robotName,
                                  const Eigen::Matrix4f& globalPose) = 0;
 
        virtual void applyForceRobotNode(const std::string& robotName,
                                         const std::string& robotNodeName,
                                         const Eigen::Vector3f& force) = 0;
        virtual void applyTorqueRobotNode(const std::string& robotName,
                                          const std::string& robotNodeName,
                                          const Eigen::Vector3f& torque) = 0;
 
        virtual void applyForceObject(const std::string& objectName,
                                      const Eigen::Vector3f& force) = 0;
        virtual void applyTorqueObject(const std::string& objectName,
                                       const Eigen::Vector3f& torque) = 0;
 
        virtual bool hasObject(const std::string& instanceName) = 0;
 
        virtual std::map<std::string, float> getRobotJointAngles(const std::string& robotName) = 0;
        virtual float getRobotJointAngle(const std::string& robotName,
                                         const std::string& nodeName) = 0;
        virtual std::map<std::string, float>
        getRobotJointVelocities(const std::string& robotName) = 0;
        virtual float getRobotJointVelocity(const std::string& robotName,
                                            const std::string& nodeName) = 0;
        virtual std::map<std::string, float> getRobotJointTorques(const std::string& robotName) = 0;
        virtual ForceTorqueDataSeq getRobotForceTorqueSensors(const std::string& robotName) = 0;
 
        virtual float getRobotJointLimitLo(const std::string& robotName,
                                           const std::string& nodeName) = 0;
        virtual float getRobotJointLimitHi(const std::string& robotName,
                                           const std::string& nodeName) = 0;
        virtual Eigen::Matrix4f getRobotPose(const std::string& robotName) = 0;
 
        virtual float getRobotMaxTorque(const std::string& robotName,
                                        const std::string& nodeName) = 0;
        virtual void setRobotMaxTorque(const std::string& robotName,
                                       const std::string& nodeName,
                                       float maxTorque) = 0;
 
        virtual Eigen::Matrix4f getRobotNodePose(const std::string& robotName,
                                                 const std::string& robotNodeName) = 0;
 
        virtual Eigen::Vector3f getRobotLinearVelocity(const std::string& robotName,
                                                       const std::string& nodeName) = 0;
        virtual Eigen::Vector3f getRobotAngularVelocity(const std::string& robotName,
                                                        const std::string& nodeName) = 0;
 
        virtual void setRobotLinearVelocity(const std::string& robotName,
                                            const std::string& robotNodeName,
                                            const Eigen::Vector3f& vel) = 0;
        virtual void setRobotAngularVelocity(const std::string& robotName,
                                             const std::string& robotNodeName,
                                             const Eigen::Vector3f& vel) = 0;
        virtual void setRobotLinearVelocityRobotRootFrame(const std::string& robotName,
                                                          const std::string& robotNodeName,
                                                          const Eigen::Vector3f& vel) = 0;
        virtual void setRobotAngularVelocityRobotRootFrame(const std::string& robotName,
                                                           const std::string& robotNodeName,
                                                           const Eigen::Vector3f& vel) = 0;
 
        virtual Eigen::Matrix4f getObjectPose(const std::string& objectName) = 0;
 
        virtual void setObjectPose(const std::string& objectName,
                                   const Eigen::Matrix4f& globalPose) = 0;
        virtual void activateObject(const std::string& objectName);
 
 
        virtual VirtualRobot::RobotPtr getRobot(const std::string& robotName) = 0;
        virtual std::map<std::string, VirtualRobot::RobotPtr> getRobots() = 0;
        virtual bool hasRobot(const std::string& robotName) = 0;
        virtual bool hasRobotNode(const std::string& robotName,
                                  const std::string& robotNodeName) = 0;
        virtual float getRobotMass(const std::string& robotName) = 0;
 
        virtual armarx::DistanceInfo getRobotNodeDistance(const std::string& robotName,
                                                          const std::string& robotNodeName1,
                                                          const std::string& robotNodeName2) = 0;
        virtual armarx::DistanceInfo getDistance(const std::string& robotName,
                                                 const std::string& robotNodeName,
                                                 const std::string& worldObjectName) = 0;
 
 
        //! create a joint
        virtual void objectGrasped(const std::string& robotName,
                                   const std::string& robotNodeName,
                                   const std::string& objectName);
 
        //! remove a joint
        virtual void objectReleased(const std::string& robotName,
                                    const std::string& robotNodeName,
                                    const std::string& objectName);
 
 
        virtual std::vector<SimDynamics::DynamicsEngine::DynamicsContactInfo> copyContacts() = 0;
 
        /**
        * Load and add a robot
        */
        /*!
         * \brief addRobot Load and add int to the scene
         * \param filename The absolute filename
         * \param pose The initial pose (4x4matrix, in MM)
         * \param filenameLocal The local filename (i.e. not the absolute one)
         * \param pid_p PID control paramters, currently all joints use the same pid values
         * \param pid_i PID control paramters, currently all joints use the same pid values
         * \param pid_d PID control paramters, currently all joints use the same pid values
         * \param staticRobot If set, the robot is added as a static/fixed kinematic object which does not move due to physics (it may be actuated by setting the position of the joints)
         * \param scaling The scaling of the robot (1 = no scaling).
         * \param colModel Use the collision model for visualization.
         * \return
         */
        virtual bool addRobot(std::string& robotInstanceName,
                              const std::string& filename,
                              Eigen::Matrix4f pose = Eigen::Matrix4f::Identity(),
                              const std::string& filenameLocal = "",
                              double pid_p = 10.0,
                              double pid_i = 0,
                              double pid_d = 0,
                              bool staticRobot = false,
                              float scaling = 1.0f,
                              bool colModel = false,
                              const std::map<std::string, float>& initConfig = {},
                              bool selfCollisions = false);
        virtual bool addRobot(VirtualRobot::RobotPtr robot,
                              double pid_p,
                              double pid_i,
                              double pid_d,
                              const std::string& filename,
                              bool staticRobot = false,
                              float scaling = 1.0f,
                              bool colModel = false,
                              bool selfCollisions = false);
        virtual VirtualRobot::RobotPtr adaptRobotToWorld(VirtualRobot::RobotPtr);
 
        /*!
         * \brief toFramedPose Constructs a framed pose
         * \param globalPose
         * \param robotName
         * \param frameName
         * \return
         */
        virtual FramedPosePtr toFramedPose(const Eigen::Matrix4f& globalPose,
                                           const std::string& robotName,
                                           const std::string& frameName) = 0;
 
        /**
        * Load and add an Obstacle (VirtualRobot xml file).
        * \param filename The obstacle xml file
        * \param pose The inital pose (mm)
        * \param simType eUnknown:  use sim type of SceneObject,
                                    to overwrite internal simType use:
                                        eStatic: cannot move, but collide;
                                        eKinematic: can be moved, but no dynamics;
                                        eDynamic: full dynamic simulation
        */
        virtual bool addObstacle(const std::string& filename,
                                 const Eigen::Matrix4f& pose = Eigen::Matrix4f::Identity(),
                                 VirtualRobot::SceneObject::Physics::SimulationType simType =
                                     VirtualRobot::SceneObject::Physics::eUnknown,
                                 const std::string& localFilename = "");
 
        virtual bool addObstacle(VirtualRobot::SceneObjectPtr o,
                                 VirtualRobot::SceneObject::Physics::SimulationType simType =
                                     VirtualRobot::SceneObject::Physics::eUnknown,
                                 const std::string& filename = "",
                                 const std::string& objectClassName = "",
                                 ObjectVisuPrimitivePtr primitiveData = {},
                                 const std::string& project = "");
 
        /**
        * Load and add an Scene (VirtualRobot xml file).
        * \param filename The scene xml fil
        * \param simType eStatic: cannot move, but collide; eKinematic: can be moved, but no dynamics; eDynamic: full dynamic simulation
        */
        virtual bool addScene(const std::string& filename,
                              VirtualRobot::SceneObject::Physics::SimulationType simType =
                                  VirtualRobot::SceneObject::Physics::eUnknown);
        virtual bool addScene(VirtualRobot::ScenePtr scene,
                              VirtualRobot::SceneObject::Physics::SimulationType simType =
                                  VirtualRobot::SceneObject::Physics::eUnknown);
 
        virtual void
        setRobotNodeSimType(const std::string& robotName,
                            const std::string& robotNodeName,
                            VirtualRobot::SceneObject::Physics::SimulationType simType) = 0;
        virtual void
        setObjectSimType(const std::string& objectName,
                         VirtualRobot::SceneObject::Physics::SimulationType simType) = 0;
        virtual std::vector<VirtualRobot::SceneObjectPtr> getObjects() = 0;
 
        /**
        * Perform one simulation step. Calculates the delta update time from the time that has passed since the last call.
        * This updates all models.
        */
        virtual void stepPhysicsRealTime() = 0;
 
        /**
        * Perform one simulation step.
        * This updates all models.
        */
        virtual void stepPhysicsFixedTimeStep() = 0;
 
        /*!
         * \brief
         * \return The number of steps * the timestep in MS
         */
        virtual int getFixedTimeStepMS() = 0;
 
        using setMutexFunc = decltype(&SimDynamics::DynamicsEngine::setMutex);
        using MutexPtrType = typename argType<setMutexFunc>::type;
        using MutexType = MutexPtrType::element_type;
 
        using ScopedRecursiveLock = std::scoped_lock<MutexType>;
        using ScopedRecursiveLockPtr = std::shared_ptr<ScopedRecursiveLock>;
 
 
        virtual ScopedRecursiveLockPtr getScopedEngineLock(const std::string& callStr);
 
        virtual ScopedRecursiveLockPtr getScopedSyncLock(const std::string& callStr);
 
        virtual void enableLogging(const std::string& robotName, const std::string& logFile);
 
        virtual float getSimTime();
        virtual float getSyncEngineTime();
 
        virtual bool removeObstacles();
        virtual bool removeRobots();
        virtual bool removeRobot(const std::string& robotName);
 
        virtual bool removeObstacle(const std::string& name);
 
 
        virtual double getCurrentSimTime();
        virtual void resetSimTime();
 
        virtual int getRobotJointAngleCount(); //Data->robotJointAngles.size();
        virtual int getContactCount(); //contacts.size();
        virtual int getObjectCount(); //objects.size();
 
 
        // protect access with mutex or use copyReportData
        virtual SimulatedWorldData& getReportData();
 
        virtual SimulatedWorldData copyReportData();
 
        /*!
         * \brief resetData Clears all data
         */
        virtual bool resetData();
 
        /*!
         * \brief copySceneVisuData Creates a copy of the visualization data
         * \return The copy
         */
        virtual SceneVisuData copySceneVisuData();
 
        /*!
         * \brief synchronizeSimulationData Update the sim data according to the internal physics models.
         * \return true on success.
         */
        virtual bool synchronizeSimulationData();
 
        virtual void updateContacts(bool enable);
 
        virtual void setupFloor(bool enable, const std::string& floorTexture);
 
        /*!
         * \brief getSimulationStepDuration
         * \return The requested duration of the last simulation step (in ms)
         */
        virtual float getSimulationStepDuration();
 
        /*!
         * \brief getSimulationStepTimeMeasured
         * \return  How long it took to simulate the last sim step.
         */
        virtual float getSimulationStepTimeMeasured();
 
        virtual std::vector<std::string> getObstacleNames() = 0;
        virtual std::vector<std::string> getRobotNames() = 0;
 
 
    protected:
        // needs to be implemented in derived classes
        virtual bool
        addObstacleEngine(VirtualRobot::SceneObjectPtr o,
                          VirtualRobot::SceneObject::Physics::SimulationType simType) = 0;
        virtual bool removeObstacleEngine(const std::string& name) = 0;
 
        virtual bool addRobotEngine(VirtualRobot::RobotPtr robot,
                                    double pid_p,
                                    double pid_i,
                                    double pid_d,
                                    const std::string& filename,
                                    bool staticRobot,
                                    bool selfCollisions) = 0;
        virtual bool removeRobotEngine(const std::string& robotName) = 0;
 
 
        //! create a joint
        virtual bool objectGraspedEngine(const std::string& robotName,
                                         const std::string& robotNodeName,
                                         const std::string& objectName,
                                         Eigen::Matrix4f& storeLocalTransform) = 0;
        //! remove a joint
        virtual bool objectReleasedEngine(const std::string& robotName,
                                          const std::string& robotNodeName,
                                          const std::string& objectName) = 0;
 
        virtual VirtualRobot::SceneObjectPtr getFloor() = 0;
        virtual void setupFloorEngine(bool enable, const std::string& floorTexture) = 0;
        virtual bool synchronizeSimulationDataEngine() = 0;
 
 
        // synchronize visu data
        virtual bool synchronizeObjects() = 0;
 
        virtual bool
        synchronizeRobotNodePoses(const std::string& robotName,
                                  std::map<std::string, armarx::PoseBasePtr>& objMap) = 0;
 
        virtual bool getRobotStatus(const std::string& robotName,
                                    NameValueMap& jointAngles,
                                    NameValueMap& jointVelocities,
                                    NameValueMap& jointTorques,
                                    Eigen::Vector3f& linearVelocity,
                                    Eigen::Vector3f& angularVelocity) = 0;
 
        virtual bool updateForceTorqueSensor(ForceTorqueInfo& ftInfo) = 0;
 
        virtual bool
        synchronizeSceneObjectPoses(VirtualRobot::SceneObjectPtr currentObjEngine,
                                    std::map<std::string, armarx::PoseBasePtr>& objMap) = 0;
 
        // synchronize visu data (and robot report data for topics)
        virtual bool synchronizeRobots();
 
        double currentSimTimeSec = 0.0;
        double currentSyncTimeSec = 0.0;
 
        MutexPtrType engineMutex;
        MutexPtrType synchronizeMutex;
 
        bool collectContacts;
 
        struct GraspingInfo
        {
            std::string robotName;
            std::string robotNodeName;
            std::string objectName;
            Eigen::Matrix4f node2objectTransformation;
        };
 
        std::vector<GraspingInfo> attachedObjects;
 
        SceneVisuData simVisuData;
        SimulatedWorldData simReportData;
        float maxRealTimeSimSpeed = 1;
        float simTimeStepMS; // simulation step in MS
 
 
        float simStepExecutionDurationMS;
 
        float synchronizeDurationMS;
 
 
        std::map<std::string, int> engineMtxAccCalls;
        std::map<std::string, float> engineMtxAccTime;
        std::map<std::string, IceUtil::Time> engineMtxLastTime;
        std::map<std::string, int> syncMtxAccCalls;
        std::map<std::string, float> syncMtxAccTime;
        std::map<std::string, IceUtil::Time> syncMtxLastTime;
    };
 
    using SimulatedWorldPtr = std::shared_ptr<SimulatedWorld>;
} // namespace armarx