MujocoPhysicsWorld.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     Rainer Kartmann ( rainer dot kartmann at student dot kit dot edu )
 * @date       2019
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#pragma once
 
#include <filesystem>
 
#include <VirtualRobot/XML/mujoco/RobotMjcf.h>
 
#include "SimulatedWorld.h"
#include "mujoco/LengthScaling.h"
#include "mujoco/SimMjcf.h"
#include "mujoco/SimObject.h"
#include "mujoco/SimRobot.h"
#include <MujocoX/libraries/MJCF/Document.h>
#include <MujocoX/libraries/Simulation/MujocoSim.h>
#include <MujocoX/libraries/Simulation/SimCallbackListener.h>
#include <MujocoX/libraries/Simulation/SimEntity.h>
 
namespace armarx
{
    /**
     * @brief The MujocoPhysicsWorld class encapsulates the whole physics
     * simulation and the corresponding data.
     */
    class MujocoPhysicsWorld : public SimulatedWorld, mujoco::SimCallbackListener
    {
    public:
        MujocoPhysicsWorld();
        virtual ~MujocoPhysicsWorld() override = default;
 
 
    public:
        /**
         * @brief Initialize `*this`. To be called after construction.
         *
         * Sets up simulation model document and requests a reload.
         *
         * @param stepTimeMs The step time in ms.
         * @param floorEnabled Whether to add a floor.
         * @param floorTexture The floor texture file.
         */
        void initialize(int stepTimeMs, bool floorEnabled, const std::string& floorTexture);
 
 
        // SimulatedWorld interface
 
        virtual void stepPhysicsRealTime() override;
        virtual void stepPhysicsFixedTimeStep() override;
        virtual int getFixedTimeStepMS() override;
 
        virtual std::vector<std::string> getObstacleNames() override;
        virtual std::vector<std::string> getRobotNames() override;
 
 
        virtual void actuateRobotJoints(const std::string& robotName,
                                        const std::map<std::string, float>& angles,
                                        const std::map<std::string, float>& velocities) override;
        virtual void actuateRobotJointsPos(const std::string& robotName,
                                           const std::map<std::string, float>& angles) override;
        virtual void actuateRobotJointsVel(const std::string& robotName,
                                           const std::map<std::string, float>& velocities) override;
        virtual void actuateRobotJointsTorque(const std::string& robotName,
                                              const std::map<std::string, float>& torques) override;
 
        virtual void applyForceRobotNode(const std::string& robotName,
                                         const std::string& robotNodeName,
                                         const Eigen::Vector3f& force) override;
        virtual void applyTorqueRobotNode(const std::string& robotName,
                                          const std::string& robotNodeName,
                                          const Eigen::Vector3f& torque) override;
 
        virtual void applyForceObject(const std::string& objectName,
                                      const Eigen::Vector3f& force) override;
        virtual void applyTorqueObject(const std::string& objectName,
                                       const Eigen::Vector3f& torque) override;
 
        virtual std::map<std::string, float>
        getRobotJointAngles(const std::string& robotName) override;
        virtual float getRobotJointAngle(const std::string& robotName,
                                         const std::string& nodeName) override;
 
        virtual std::map<std::string, float>
        getRobotJointVelocities(const std::string& robotName) override;
        virtual float getRobotJointVelocity(const std::string& robotName,
                                            const std::string& nodeName) override;
 
        virtual std::map<std::string, float>
        getRobotJointTorques(const std::string& robotName) override;
        virtual ForceTorqueDataSeq
        getRobotForceTorqueSensors(const std::string& robotName) override;
 
 
        virtual Eigen::Matrix4f getObjectPose(const std::string& objectName) override;
        virtual void setObjectPose(const std::string& objectName,
                                   const Eigen::Matrix4f& globalPose) override;
 
        virtual Eigen::Matrix4f getRobotPose(const std::string& robotName) override;
        virtual void setRobotPose(const std::string& robotName,
                                  const Eigen::Matrix4f& globalPose) override;
 
        virtual Eigen::Matrix4f getRobotNodePose(const std::string& robotName,
                                                 const std::string& robotNodeName) override;
        virtual Eigen::Vector3f getRobotLinearVelocity(const std::string& robotName,
                                                       const std::string& nodeName) override;
        virtual Eigen::Vector3f getRobotAngularVelocity(const std::string& robotName,
                                                        const std::string& nodeName) override;
 
        virtual void setRobotLinearVelocity(const std::string& robotName,
                                            const std::string& robotNodeName,
                                            const Eigen::Vector3f& vel) override;
        virtual void setRobotAngularVelocity(const std::string& robotName,
                                             const std::string& robotNodeName,
                                             const Eigen::Vector3f& vel) override;
 
        virtual void setRobotLinearVelocityRobotRootFrame(const std::string& robotName,
                                                          const std::string& robotNodeName,
                                                          const Eigen::Vector3f& vel) override;
        virtual void setRobotAngularVelocityRobotRootFrame(const std::string& robotName,
                                                           const std::string& robotNodeName,
                                                           const Eigen::Vector3f& vel) override;
 
        virtual bool hasObject(const std::string& instanceName) override;
 
        virtual bool hasRobot(const std::string& robotName) override;
        virtual bool hasRobotNode(const std::string& robotName,
                                  const std::string& robotNodeName) override;
 
        virtual std::vector<VirtualRobot::SceneObjectPtr> getObjects() override;
 
        virtual VirtualRobot::RobotPtr getRobot(const std::string& robotName) override;
        virtual std::map<std::string, VirtualRobot::RobotPtr> getRobots() override;
 
        virtual float getRobotJointLimitLo(const std::string& robotName,
                                           const std::string& nodeName) override;
        virtual float getRobotJointLimitHi(const std::string& robotName,
                                           const std::string& nodeName) override;
 
        virtual float getRobotMaxTorque(const std::string& robotName,
                                        const std::string& nodeName) override;
        virtual void setRobotMaxTorque(const std::string& robotName,
                                       const std::string& nodeName,
                                       float maxTorque) override;
 
        virtual float getRobotMass(const std::string& robotName) override;
 
        virtual DistanceInfo getRobotNodeDistance(const std::string& robotName,
                                                  const std::string& robotNodeName1,
                                                  const std::string& robotNodeName2) override;
        virtual DistanceInfo getDistance(const std::string& robotName,
                                         const std::string& robotNodeName,
                                         const std::string& worldObjectName) override;
 
        virtual std::vector<SimDynamics::DynamicsEngine::DynamicsContactInfo>
        copyContacts() override;
 
        virtual FramedPosePtr toFramedPose(const Eigen::Matrix4f& globalPose,
                                           const std::string& robotName,
                                           const std::string& frameName) override;
 
        virtual void
        setObjectSimType(const std::string& objectName,
                         VirtualRobot::SceneObject::Physics::SimulationType simType) override;
 
        virtual void
        setRobotNodeSimType(const std::string& robotName,
                            const std::string& robotNodeName,
                            VirtualRobot::SceneObject::Physics::SimulationType simType) override;
 
 
    protected:
        // SimulatedWorld interface
 
        virtual bool
        addObstacleEngine(VirtualRobot::SceneObjectPtr o,
                          VirtualRobot::SceneObject::Physics::SimulationType simType) override;
        virtual bool removeObstacleEngine(const std::string& name) override;
 
        virtual bool addRobotEngine(VirtualRobot::RobotPtr robot,
                                    double pid_p,
                                    double pid_i,
                                    double pid_d,
                                    const std::string& filename,
                                    bool staticRobot,
                                    bool selfCollisions) override;
        virtual bool removeRobotEngine(const std::string& robotName) override;
 
        virtual bool objectGraspedEngine(const std::string& robotName,
                                         const std::string& robotNodeName,
                                         const std::string& objectName,
                                         Eigen::Matrix4f& storeLocalTransform) override;
        virtual bool objectReleasedEngine(const std::string& robotName,
                                          const std::string& robotNodeName,
                                          const std::string& objectName) override;
 
        // floor
        virtual VirtualRobot::SceneObjectPtr getFloor() override;
        virtual void setupFloorEngine(bool enable, const std::string& floorTexture) override;
 
        /// According to other SimulatedWorlds, this method's only job is to update contacts.
        virtual bool synchronizeSimulationDataEngine() override;
        virtual int getContactCount() override;
 
        virtual bool synchronizeObjects() override;
        virtual bool
        synchronizeSceneObjectPoses(VirtualRobot::SceneObjectPtr currentObjEngine,
                                    std::map<std::string, PoseBasePtr>& objMap) override;
 
        virtual bool synchronizeRobotNodePoses(const std::string& robotName,
                                               std::map<std::string, PoseBasePtr>& objMap) override;
 
        virtual bool getRobotStatus(const std::string& robotName,
                                    NameValueMap& jointAngles,
                                    NameValueMap& jointVelocities,
                                    NameValueMap& jointTorques,
                                    Eigen::Vector3f& linearVelocity,
                                    Eigen::Vector3f& angularVelocity) override;
 
        virtual bool updateForceTorqueSensor(ForceTorqueInfo& ftInfo) override;
 
 
    protected:
        // mujoco::SimCallbackListener
        /// Sets time step and fetches entity IDs.
        virtual void onLoadModel(mujoco::Model& model) override;
        virtual void onMakeData(mujoco::Model& model, mujoco::Data& data) override;
 
 
    private:
        /// Name of base file included by model.
        static const std::filesystem::path BASE_MJCF_FILENAME;
        /// Temporary directory to store models.
        static const std::filesystem::path TEMP_DIR;
        /// File where model is stored for loading.
        static const std::filesystem::path TEMP_MODEL_FILE;
        /// Directory where object meshes are placed (relative to TEMP_DIR).
        static const std::filesystem::path TEMP_MESH_DIR_REL;
        /// Directory where object meshes are placed.
        static const std::filesystem::path TEMP_MESH_DIR;
 
        /// Name of floor entities and class.
        static const std::string FLOOR_NAME;
        /// Name of object defaults class.
        static const std::string OBJECT_CLASS_NAME;
 
 
    private:
        VirtualRobot::ObstaclePtr makeFloorObject(mujoco::Model& model,
                                                  const mujoco::SimGeom& floorGeom) const;
 
        /// Make a MJCF model for the given robot and return its absolute path.
        VirtualRobot::mujoco::RobotMjcf
        makeRobotMjcfModel(const VirtualRobot::RobotPtr& robot) const;
 
 
        /// Save the MJCF document and request a reload.
        void reloadModel(bool request = true);
 
 
        // MJCF
 
        /// Length scaling manager.
        LengthScaling lengthScaling;
 
        /// The model document.
        mjcf::Document mjcfDocument;
 
        /// The model document helper.
        SimMJCF simMjcf{mjcfDocument, lengthScaling, "MujocoPhysicsWorld"};
 
 
        /// The simulation.
        mujoco::MujocoSim sim;
 
        /// The fixed time step.
        std::chrono::milliseconds fixedTimeStep;
 
 
        // Entities & Objects
 
        /// The floor object.
        mujoco::SimObject floor{FLOOR_NAME};
        /// The floor texture file.
        std::filesystem::path floorTextureFile;
 
 
        /// The simulation obstacles.
        std::map<std::string, mujoco::SimObject> obstacles;
 
        /// The simulation objects.
        std::map<std::string, mujoco::SimObject> objects;
 
        /// The robots (key is name).
        std::map<std::string, mujoco::SimRobot> robots;
 
 
        // Simulation data
 
        /// Contacts.
        std::vector<mjContact> contacts;
    };
 
    using MujocoPhysicsWorldPtr = std::shared_ptr<MujocoPhysicsWorld>;
} // namespace armarx