MujocoPhysicsWorld.cpp

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
 */
 
#ifdef MUJOCO_PHYSICS_WORLD
 
#include "MujocoPhysicsWorld.h"
 
#include <thread>
 
#include <VirtualRobot/math/Helpers.h>
 
#include <ArmarXCore/core/exceptions/user/NotImplementedYetException.h>
#include <ArmarXCore/core/system/ArmarXDataPath.h>
 
#include <MujocoX/libraries/Simulation/utils/eigen_conversion.h>
 
 
namespace fs = std::filesystem;
 
namespace armarx
{
 
    struct FunctionLogger
    {
        FunctionLogger(const std::string& functionName) : functionName(functionName)
        {
            ARMARX_VERBOSE << "Enter: \t" << functionName << "()";
        }
 
        ~FunctionLogger()
        {
            ARMARX_VERBOSE << "Exit:  \t" << functionName << "()";
        }
 
        std::string functionName;
    };
 
    //#define THROW
 
#ifdef THROW
 
#define NOT_YET_IMPLEMENTED() throw exceptions::user::NotImplementedYetException(__FUNCTION__)
#else
 
    //#define NOT_YET_IMPLEMENTED() (void) 0
#define NOT_YET_IMPLEMENTED() ARMARX_WARNING << "Not implemented: \t" << __FUNCTION__ << "()"
 
#endif
 
 
#define DO_LOG_FUNCTIONS
#ifdef DO_LOG_FUNCTIONS
#define LOG_FUNCTION() const FunctionLogger __functionLogger__(__FUNCTION__)
#else
#define LOG_FUNCTION() (void)0
#endif
 
 
    // STATIC
 
    const fs::path MujocoPhysicsWorld::BASE_MJCF_FILENAME =
        "ArmarXSimulation/mujoco_base_model.xml";
 
    const fs::path MujocoPhysicsWorld::TEMP_DIR = fs::temp_directory_path() / "MujocoSimulation";
 
    const fs::path MujocoPhysicsWorld::TEMP_MODEL_FILE = TEMP_DIR / "SimulationModel.xml";
 
    const fs::path MujocoPhysicsWorld::TEMP_MESH_DIR_REL = "meshes";
    const fs::path MujocoPhysicsWorld::TEMP_MESH_DIR = TEMP_DIR / TEMP_MESH_DIR_REL;
 
    const std::string MujocoPhysicsWorld::FLOOR_NAME = "floor";
    const std::string MujocoPhysicsWorld::OBJECT_CLASS_NAME = "object";
 
    // NON-STATIC
 
    MujocoPhysicsWorld::MujocoPhysicsWorld()
    {
        Logging::setTag("MujocoPhysicsWorld");
        sim.addCallbackListener(*this);
    }
 
    void
    MujocoPhysicsWorld::initialize(int stepTimeMs,
                                   bool floorEnabled,
                                   const std::string& floorTexture)
    {
        LOG_FUNCTION();
 
        simMjcf.includeBaseFile(BASE_MJCF_FILENAME, TEMP_DIR);
        simMjcf.addObjectDefaults(OBJECT_CLASS_NAME);
 
        fixedTimeStep = std::chrono::milliseconds(stepTimeMs);
        simMjcf.setTimeStep(fixedTimeStep.count() / 1000.f);
 
        setupFloorEngine(floorEnabled, floorTexture);
 
        ARMARX_VERBOSE << "Activating MuJoCo ... with key file " << MUJOCO_KEY_FILE;
        mujoco::MujocoSim::ensureActivated(MUJOCO_KEY_FILE);
 
        ARMARX_VERBOSE << "Load empty model ...";
        reloadModel();
        sim.simLoopPrepare();
 
        ARMARX_IMPORTANT << "Initialization finished.";
    }
 
    void
    MujocoPhysicsWorld::onLoadModel(mujoco::Model& model)
    {
        LOG_FUNCTION();
 
        model->opt.timestep = fixedTimeStep.count() / 1000.0;
 
        // Fetch object IDs.
 
        floor.update(model);
        floor.sceneObject = makeFloorObject(model, floor.geom);
 
        for (auto& objectItem : objects)
        {
            objectItem.second.update(model);
        }
    }
 
    void
    MujocoPhysicsWorld::onMakeData(mujoco::Model& model, mujoco::Data& data)
    {
        LOG_FUNCTION();
        (void)model, (void)data;
    }
 
    void
    MujocoPhysicsWorld::stepPhysicsRealTime()
    {
        NOT_YET_IMPLEMENTED();
        ScopedRecursiveLockPtr lockEngine = getScopedEngineLock(__FUNCTION__);
    }
 
    void
    MujocoPhysicsWorld::stepPhysicsFixedTimeStep()
    {
        LOG_FUNCTION();
        ScopedRecursiveLockPtr lockEngine = getScopedEngineLock(__FUNCTION__);
 
        sim.handleLoadRequest();
 
        auto start = std::chrono::system_clock::now();
        auto end = start + std::chrono::milliseconds(getFixedTimeStepMS());
 
        sim.step();
        sim.pollEvents();
        sim.render();
 
        currentSimTimeSec = sim.data()->time;
 
        std::chrono::milliseconds sleepTime = std::chrono::duration_cast<std::chrono::milliseconds>(
            end - std::chrono::system_clock::now());
        ARMARX_VERBOSE << "Sleeping for " << sleepTime.count() << "ms";
        std::this_thread::sleep_until(end);
    }
 
    int
    MujocoPhysicsWorld::getFixedTimeStepMS()
    {
        return static_cast<int>(sim.model()->opt.timestep * 1000);
    }
 
    std::vector<std::string>
    MujocoPhysicsWorld::getObstacleNames()
    {
        LOG_FUNCTION();
        std::vector<std::string> result;
        std::transform(obstacles.begin(),
                       obstacles.end(),
                       std::back_inserter(result),
                       [](const auto& item) { return item.first; });
        return result;
    }
 
    std::vector<std::string>
    MujocoPhysicsWorld::getRobotNames()
    {
        LOG_FUNCTION();
        std::vector<std::string> names;
        std::transform(robots.begin(),
                       robots.end(),
                       std::back_inserter(names),
                       [](const auto& item) { return item.first; });
        return names;
    }
 
    void
    MujocoPhysicsWorld::setupFloorEngine(bool enable, const std::string& floorTexture)
    {
        LOG_FUNCTION();
        simMjcf.addFloor(enable, floorTexture, FLOOR_NAME, 20);
        this->floorTextureFile = floorTexture;
    }
 
    bool
    MujocoPhysicsWorld::addObstacleEngine(
        VirtualRobot::SceneObjectPtr object,
        VirtualRobot::SceneObject::Physics::SimulationType simType)
    {
        LOG_FUNCTION();
        simMjcf.addObject(object, simType, OBJECT_CLASS_NAME, TEMP_MESH_DIR);
        obstacles[object->getName()] = object;
 
        reloadModel();
        return true;
    }
 
    bool
    MujocoPhysicsWorld::removeObstacleEngine(const std::string& name)
    {
        NOT_YET_IMPLEMENTED();
        return {};
    }
 
    bool
    MujocoPhysicsWorld::removeRobotEngine(const std::string& robotName)
    {
        NOT_YET_IMPLEMENTED();
        return {};
    }
 
    bool
    MujocoPhysicsWorld::addRobotEngine(VirtualRobot::RobotPtr robot,
                                       double pid_p,
                                       double pid_i,
                                       double pid_d,
                                       const std::string& filename,
                                       bool staticRobot,
                                       bool selfCollisions)
    {
        LOG_FUNCTION();
        ARMARX_IMPORTANT << "Adding robot " << robot->getName() << "\n- filename:   \t" << filename
                         << "\n- (p, i, d):  \t(" << pid_p << " " << pid_i << " " << pid_d << ")"
                         << "\n- static:     \t" << staticRobot << "\n- self coll.: \t"
                         << selfCollisions;
 
        // lock engine and data
        ScopedRecursiveLockPtr lockEngine = getScopedEngineLock(__FUNCTION__);
 
        if (!sim.isModelLoaded())
        {
            ARMARX_ERROR << "Simulation not loaded.";
            return false;
        }
        if (hasRobot(robot->getName()))
        {
            ARMARX_ERROR << "More than one robot with identical name is currently not supported.";
            return false;
        }
 
        if (staticRobot)
        {
            ARMARX_ERROR << "Static robots are currently not supported.";
            return false;
        }
 
        {
 
            // Make MJCF robot model.
            ARMARX_INFO << "Making robot MJCF model.";
 
            mujoco::SimRobot simRobot(robot);
 
            simRobot.mjcf = makeRobotMjcfModel(robot);
 
            // Include Paths must be relative.
            const fs::path robotModelFile =
                ArmarXDataPath::relativeTo(TEMP_DIR, simRobot.mjcf.getOutputFile());
 
            // Include robot in model file.
            mjcfDocument.addInclude(robotModelFile);
 
            // Store robot entry.
            robots[robot->getName()] = std::move(simRobot);
        }
 
        // Request reload.
        reloadModel(false);
 
        return true;
    }
 
    void
    MujocoPhysicsWorld::actuateRobotJoints(const std::string& robotName,
                                           const std::map<std::string, float>& angles,
                                           const std::map<std::string, float>& velocities)
    {
        NOT_YET_IMPLEMENTED();
    }
 
    void
    MujocoPhysicsWorld::actuateRobotJointsPos(const std::string& robotName,
                                              const std::map<std::string, float>& angles)
    {
        NOT_YET_IMPLEMENTED();
    }
 
    void
    MujocoPhysicsWorld::actuateRobotJointsVel(const std::string& robotName,
                                              const std::map<std::string, float>& velocities)
    {
        NOT_YET_IMPLEMENTED();
    }
 
    void
    MujocoPhysicsWorld::actuateRobotJointsTorque(const std::string& robotName,
                                                 const std::map<std::string, float>& torques)
    {
        NOT_YET_IMPLEMENTED();
    }
 
    void
    MujocoPhysicsWorld::applyForceRobotNode(const std::string& robotName,
                                            const std::string& robotNodeName,
                                            const Eigen::Vector3f& force)
    {
        NOT_YET_IMPLEMENTED();
    }
 
    void
    MujocoPhysicsWorld::applyTorqueRobotNode(const std::string& robotName,
                                             const std::string& robotNodeName,
                                             const Eigen::Vector3f& torque)
    {
        NOT_YET_IMPLEMENTED();
    }
 
    void
    MujocoPhysicsWorld::applyForceObject(const std::string& objectName,
                                         const Eigen::Vector3f& force)
    {
        NOT_YET_IMPLEMENTED();
    }
 
    void
    MujocoPhysicsWorld::applyTorqueObject(const std::string& objectName,
                                          const Eigen::Vector3f& torque)
    {
        NOT_YET_IMPLEMENTED();
    }
 
    std::map<std::string, float>
    MujocoPhysicsWorld::getRobotJointAngles(const std::string& robotName)
    {
        NOT_YET_IMPLEMENTED();
        return {};
    }
 
    float
    MujocoPhysicsWorld::getRobotJointAngle(const std::string& robotName,
                                           const std::string& nodeName)
    {
        NOT_YET_IMPLEMENTED();
        return {};
    }
 
    std::map<std::string, float>
    MujocoPhysicsWorld::getRobotJointVelocities(const std::string& robotName)
    {
        NOT_YET_IMPLEMENTED();
        return {};
    }
 
    float
    MujocoPhysicsWorld::getRobotJointVelocity(const std::string& robotName,
                                              const std::string& nodeName)
    {
        NOT_YET_IMPLEMENTED();
        return {};
    }
 
    std::map<std::string, float>
    MujocoPhysicsWorld::getRobotJointTorques(const std::string& robotName)
    {
        NOT_YET_IMPLEMENTED();
        return {};
    }
 
    ForceTorqueDataSeq
    MujocoPhysicsWorld::getRobotForceTorqueSensors(const std::string& robotName)
    {
        NOT_YET_IMPLEMENTED();
        return {};
    }
 
    Eigen::Matrix4f
    MujocoPhysicsWorld::getObjectPose(const std::string& objectName)
    {
        NOT_YET_IMPLEMENTED();
        return {};
    }
 
    void
    MujocoPhysicsWorld::setObjectPose(const std::string& objectName,
                                      const Eigen::Matrix4f& globalPose)
    {
        NOT_YET_IMPLEMENTED();
    }
 
    Eigen::Matrix4f
    MujocoPhysicsWorld::getRobotPose(const std::string& robotName)
    {
        return {};
    }
 
    void
    MujocoPhysicsWorld::setRobotPose(const std::string& robotName,
                                     const Eigen::Matrix4f& globalPose)
    {
        NOT_YET_IMPLEMENTED();
    }
 
    Eigen::Matrix4f
    MujocoPhysicsWorld::getRobotNodePose(const std::string& robotName,
                                         const std::string& robotNodeName)
    {
        NOT_YET_IMPLEMENTED();
        return {};
    }
 
    Eigen::Vector3f
    MujocoPhysicsWorld::getRobotLinearVelocity(const std::string& robotName,
                                               const std::string& nodeName)
    {
        NOT_YET_IMPLEMENTED();
        return {};
    }
 
    Eigen::Vector3f
    MujocoPhysicsWorld::getRobotAngularVelocity(const std::string& robotName,
                                                const std::string& nodeName)
    {
        NOT_YET_IMPLEMENTED();
        return {};
    }
 
    void
    MujocoPhysicsWorld::setRobotLinearVelocity(const std::string& robotName,
                                               const std::string& robotNodeName,
                                               const Eigen::Vector3f& vel)
    {
        NOT_YET_IMPLEMENTED();
    }
 
    void
    MujocoPhysicsWorld::setRobotAngularVelocity(const std::string& robotName,
                                                const std::string& robotNodeName,
                                                const Eigen::Vector3f& vel)
    {
        NOT_YET_IMPLEMENTED();
    }
 
    void
    MujocoPhysicsWorld::setRobotLinearVelocityRobotRootFrame(const std::string& robotName,
                                                             const std::string& robotNodeName,
                                                             const Eigen::Vector3f& vel)
    {
        NOT_YET_IMPLEMENTED();
    }
 
    void
    MujocoPhysicsWorld::setRobotAngularVelocityRobotRootFrame(const std::string& robotName,
                                                              const std::string& robotNodeName,
                                                              const Eigen::Vector3f& vel)
    {
        NOT_YET_IMPLEMENTED();
    }
 
    bool
    MujocoPhysicsWorld::hasObject(const std::string& instanceName)
    {
        LOG_FUNCTION();
        return objects.find(instanceName) != objects.end();
    }
 
    bool
    MujocoPhysicsWorld::hasRobot(const std::string& robotName)
    {
        LOG_FUNCTION();
        return robots.find(robotName) != robots.end();
    }
 
    bool
    MujocoPhysicsWorld::hasRobotNode(const std::string& robotName, const std::string& robotNodeName)
    {
        LOG_FUNCTION();
        return hasRobot(robotName) && getRobot(robotName)->hasRobotNode(robotNodeName);
    }
 
    std::vector<VirtualRobot::SceneObjectPtr>
    MujocoPhysicsWorld::getObjects()
    {
        LOG_FUNCTION();
        std::vector<VirtualRobot::SceneObjectPtr> sceneObjects;
        std::transform(objects.begin(),
                       objects.end(),
                       std::back_inserter(sceneObjects),
                       [](const auto& item) { return item.second.sceneObject; });
        return sceneObjects;
    }
 
    VirtualRobot::RobotPtr
    MujocoPhysicsWorld::getRobot(const std::string& robotName)
    {
        LOG_FUNCTION();
        return robots.at(robotName).robot;
    }
 
    std::map<std::string, VirtualRobot::RobotPtr>
    MujocoPhysicsWorld::getRobots()
    {
        LOG_FUNCTION();
        std::map<std::string, VirtualRobot::RobotPtr> res;
        for (const auto& item : robots)
        {
            res[item.first] = item.second.robot;
        }
        return res;
    }
 
    float
    MujocoPhysicsWorld::getRobotJointLimitLo(const std::string& robotName,
                                             const std::string& nodeName)
    {
        NOT_YET_IMPLEMENTED();
        return {};
    }
 
    float
    MujocoPhysicsWorld::getRobotJointLimitHi(const std::string& robotName,
                                             const std::string& nodeName)
    {
        NOT_YET_IMPLEMENTED();
        return {};
    }
 
    float
    MujocoPhysicsWorld::getRobotMaxTorque(const std::string& robotName, const std::string& nodeName)
    {
        NOT_YET_IMPLEMENTED();
        return {};
    }
 
    void
    MujocoPhysicsWorld::setRobotMaxTorque(const std::string& robotName,
                                          const std::string& nodeName,
                                          float maxTorque)
    {
        NOT_YET_IMPLEMENTED();
    }
 
    float
    MujocoPhysicsWorld::getRobotMass(const std::string& robotName)
    {
        NOT_YET_IMPLEMENTED();
        return {};
    }
 
    DistanceInfo
    MujocoPhysicsWorld::getRobotNodeDistance(const std::string& robotName,
                                             const std::string& robotNodeName1,
                                             const std::string& robotNodeName2)
    {
        NOT_YET_IMPLEMENTED();
        return {};
    }
 
    DistanceInfo
    MujocoPhysicsWorld::getDistance(const std::string& robotName,
                                    const std::string& robotNodeName,
                                    const std::string& worldObjectName)
    {
        NOT_YET_IMPLEMENTED();
        return {};
    }
 
    std::vector<SimDynamics::DynamicsEngine::DynamicsContactInfo>
    MujocoPhysicsWorld::copyContacts()
    {
        LOG_FUNCTION();
        ScopedRecursiveLockPtr lockSync = getScopedSyncLock(__FUNCTION__);
 
        std::vector<SimDynamics::DynamicsEngine::DynamicsContactInfo> copy;
 
        for (const mjContact& contact : contacts)
        {
            SimDynamics::DynamicsEngine::DynamicsContactInfo info;
            info.distance = lengthScaling.meterToMilli(contact.dist);
            info.objectAName = sim.model().id2nameGeom(contact.geom1);
            info.objectBName = sim.model().id2nameGeom(contact.geom2);
 
            Eigen::Vector3f pos = mujoco::utils::toVec3(contact.pos);
            Eigen::Vector3f normalOnB = mujoco::utils::toVec3(contact.frame);
 
            info.normalGlobalB = normalOnB;
            info.posGlobalA = pos + static_cast<float>(contact.dist / 2) * normalOnB;
            info.posGlobalB = pos - static_cast<float>(contact.dist / 2) * normalOnB;
 
            info.posGlobalA = lengthScaling.meterToMilli(info.posGlobalA);
            info.posGlobalB = lengthScaling.meterToMilli(info.posGlobalB);
 
            info.combinedFriction = contact.friction[0];
            copy.push_back(info);
        }
 
        return copy;
    }
 
    FramedPosePtr
    MujocoPhysicsWorld::toFramedPose(const Eigen::Matrix4f& globalPose,
                                     const std::string& robotName,
                                     const std::string& frameName)
    {
        NOT_YET_IMPLEMENTED();
        return {};
    }
 
    void
    MujocoPhysicsWorld::setObjectSimType(const std::string& objectName,
                                         VirtualRobot::SceneObject::Physics::SimulationType simType)
    {
        NOT_YET_IMPLEMENTED();
    }
 
    void
    MujocoPhysicsWorld::setRobotNodeSimType(
        const std::string& robotName,
        const std::string& robotNodeName,
        VirtualRobot::SceneObject::Physics::SimulationType simType)
    {
        NOT_YET_IMPLEMENTED();
    }
 
    // SimulatedWorld interface
 
 
    bool
    MujocoPhysicsWorld::objectGraspedEngine(const std::string& robotName,
                                            const std::string& robotNodeName,
                                            const std::string& objectName,
                                            Eigen::Matrix4f& storeLocalTransform)
    {
        NOT_YET_IMPLEMENTED();
        return {};
    }
 
    bool
    MujocoPhysicsWorld::objectReleasedEngine(const std::string& robotName,
                                             const std::string& robotNodeName,
                                             const std::string& objectName)
    {
        NOT_YET_IMPLEMENTED();
        return {};
    }
 
    VirtualRobot::SceneObjectPtr
    MujocoPhysicsWorld::getFloor()
    {
        LOG_FUNCTION();
 
        if (floor.sceneObject && sim.data())
        {
            floor.updateSceneObjectPose(sim.data());
        }
        return floor.sceneObject;
    }
 
    bool
    MujocoPhysicsWorld::synchronizeSimulationDataEngine()
    {
        LOG_FUNCTION();
        ScopedRecursiveLockPtr lockEngine = getScopedEngineLock(__FUNCTION__);
 
        if (collectContacts)
        {
            contacts.clear();
            for (int i = 0; i < sim.data()->ncon; ++i)
            {
                contacts.push_back(sim.data()->contact[i]);
            }
        }
 
        return true;
    }
 
    int
    MujocoPhysicsWorld::getContactCount()
    {
        return static_cast<int>(contacts.size());
    }
 
    bool
    MujocoPhysicsWorld::synchronizeObjects()
    {
        LOG_FUNCTION();
        ScopedRecursiveLockPtr lockEngine = getScopedEngineLock(__FUNCTION__);
        ScopedRecursiveLockPtr lockSync = getScopedSyncLock(__FUNCTION__);
 
        if (floor.sceneObject && !simVisuData.floor)
        {
            simVisuData.floor = true;
            simVisuData.floorTextureFile = floorTextureFile;
        }
 
        for (auto& item : objects)
        {
            mujoco::SimObject& object = item.second;
 
            if (object.body.id() >= 0 && object.sceneObject && sim.data())
            {
                object.updateSceneObjectPose(sim.data(), lengthScaling.meterToMilli());
            }
 
            VirtualRobot::SceneObjectPtr sceneObject = object.sceneObject;
 
            bool objectFound = false;
 
            for (ObjectVisuData& visuData : simVisuData.objects)
            {
                if (visuData.name == sceneObject->getName())
                {
                    objectFound = true;
                    synchronizeSceneObjectPoses(sceneObject, visuData.objectPoses);
                    visuData.updated = true;
                    break;
                }
            }
 
            if (!objectFound)
            {
                ARMARX_WARNING << "Object '" << sceneObject->getName()
                               << "' not in synchronized list.";
            }
        }
 
        return true;
    }
 
    bool
    MujocoPhysicsWorld::synchronizeSceneObjectPoses(VirtualRobot::SceneObjectPtr sceneObject,
                                                    std::map<std::string, PoseBasePtr>& poseMap)
    {
        LOG_FUNCTION();
 
        if (!sceneObject)
        {
            return false;
        }
 
        poseMap[sceneObject->getName()] = new Pose(sceneObject->getGlobalPose());
 
        for (VirtualRobot::SceneObjectPtr& child : sceneObject->getChildren())
        {
            if (!synchronizeSceneObjectPoses(child, poseMap))
            {
                return false;
            }
        }
 
        return true;
    }
 
    bool
    MujocoPhysicsWorld::synchronizeRobotNodePoses(const std::string& robotName,
                                                  std::map<std::string, PoseBasePtr>& objMap)
    {
        LOG_FUNCTION();
 
        // Lock engine and data.
        ScopedRecursiveLockPtr lockEngine = getScopedEngineLock(__FUNCTION__);
        ScopedRecursiveLockPtr lockSync = getScopedSyncLock(__FUNCTION__);
 
        if (!hasRobot(robotName))
        {
            ARMARX_ERROR << "Robot '" << robotName << "' does not exist in simulation.";
            return false;
        }
 
        mujoco::SimRobot& robot = robots.at(robotName);
 
        for (const std::string& nodeName : robot.robot->getRobotNodeNames())
        {
            const int bodyID = sim.model().name2idBody(nodeName);
            objMap[nodeName] = PosePtr(new Pose(sim.data().getBodyPose(bodyID)));
        }
 
        return true;
    }
 
    bool
    MujocoPhysicsWorld::getRobotStatus(const std::string& robotName,
                                       NameValueMap& jointAngles,
                                       NameValueMap& jointVelocities,
                                       NameValueMap& jointTorques,
                                       Eigen::Vector3f& linearVelocity,
                                       Eigen::Vector3f& angularVelocity)
    {
        LOG_FUNCTION();
 
        mujoco::SimRobot& robot = robots.at(robotName);
 
        for (const auto& node : robot.robot->getConfig()->getNodes())
        {
            const std::string& nodeName = node->getName();
 
            node->isRotationalJoint();
            const int jointID = sim.model().name2id(mjtObj::mjOBJ_JOINT, nodeName);
 
            jointAngles[nodeName] = sim.data().getJointValue1D(jointID);
            jointVelocities[nodeName] = sim.data().getJointVelocity1D(jointID);
 
            const int actuatorID = sim.model().name2id(mjtObj::mjOBJ_ACTUATOR, nodeName);
            jointTorques[nodeName] = sim.data().getActivation(actuatorID);
        }
 
 
        return true;
    }
 
    bool
    MujocoPhysicsWorld::updateForceTorqueSensor(ForceTorqueInfo& ftInfo)
    {
        NOT_YET_IMPLEMENTED();
        return {};
    }
 
    VirtualRobot::ObstaclePtr
    MujocoPhysicsWorld::makeFloorObject(mujoco::Model& model,
                                        const mujoco::SimGeom& floorGeom) const
    {
        Eigen::Vector3f size = lengthScaling.meterToMilli(model.getGeomSize(floorGeom));
 
        VirtualRobot::ObstaclePtr floorObject = VirtualRobot::Obstacle::createBox(
            size.x(), size.y(), 500, VirtualRobot::VisualizationFactory::Color::Gray());
 
        floorObject->setName(FLOOR_NAME);
        floorObject->getVisualization();
        floorObject->setSimulationType(VirtualRobot::SceneObject::Physics::eStatic);
 
        return floorObject;
    }
 
    VirtualRobot::mujoco::RobotMjcf
    MujocoPhysicsWorld::makeRobotMjcfModel(const VirtualRobot::RobotPtr& robot) const
    {
        VirtualRobot::mujoco::RobotMjcf mjcf(robot);
 
        // Use absolute paths to avoid issues with relative paths.
        mjcf.setUseRelativePaths(false);
        // Scale from mm to m.
        mjcf.setLengthScale(0.001f);
 
        mjcf.setOutputFile(TEMP_DIR / (robot->getName() + ".xml"));
        mjcf.setOutputMeshRelativeDirectory(TEMP_MESH_DIR_REL / robot->getName());
 
        mjcf.build(VirtualRobot::mujoco::WorldMountMode::FREE,
                   VirtualRobot::mujoco::ActuatorType::MOTOR);
 
        // Add sensors.
        mjcf::SensorSection sensors = mjcf.getDocument().sensor();
 
        // Needed by getRobotStatus().
        mjcf::VelocimeterSensor velocimeter = sensors.addChild<mjcf::VelocimeterSensor>();
 
        // TODO: continue adding sensors
 
        mjcf.save();
 
        return mjcf;
    }
 
    void
    MujocoPhysicsWorld::reloadModel(bool request)
    {
        // does nothing if it already exists
        fs::create_directory(TEMP_DIR);
        ARMARX_INFO << "Saving temporary model file and requesting reload.\n " << TEMP_MODEL_FILE;
        mjcfDocument.saveFile(TEMP_MODEL_FILE);
 
        if (request)
        {
            sim.loadModelRequest(TEMP_MODEL_FILE);
        }
        else
        {
            sim.loadModel(TEMP_MODEL_FILE);
        }
    }
 
 
} // namespace armarx
 
#endif