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);
        }
    }
 
 
}
 
#endif