SimMjcf.cpp

Go to the documentation of this file.

#include "SimMjcf.h"
 
#include <VirtualRobot/CollisionDetection/CollisionModel.h>
#include <VirtualRobot/Visualization/TriMeshModel.h>
#include <VirtualRobot/Visualization/VisualizationNode.h>
#include <VirtualRobot/XML/mujoco/Mesh.h>
#include <VirtualRobot/XML/mujoco/MeshConverter.h>
#include <VirtualRobot/math/Helpers.h>
 
#include <ArmarXCore/core/system/ArmarXDataPath.h>
 
#include "LengthScaling.h"
 
 
using namespace armarx;
 
SimMJCF::SimMJCF(mjcf::Document& document,
                 const LengthScaling& lengthScaling,
                 const std::string& loggingTag) :
    document(document), scaling(lengthScaling)
{
    Logging::setTag(loggingTag);
}
 
void
SimMJCF::includeBaseFile(const std::string& path, const std::string& relativeFrom)
{
    std::string absolute = path;
    ArmarXDataPath::getAbsolutePath(path, absolute);
 
    std::filesystem::path absol = absolute;
    absol.relative_path();
 
    if (std::filesystem::exists(absolute))
    {
        std::string relative = ArmarXDataPath::relativeTo(relativeFrom, absolute);
        document.addInclude(relative);
    }
    else
    {
        ARMARX_WARNING << "Base MJCF file does not exist: " << absolute
                       << "\nProceeding without including the base MJCF.";
    }
}
 
void
SimMJCF::addObjectDefaults(const std::string& className)
{
    mjcf::DefaultClass objectClass = document.default_().getClass(className);
 
    mjcf::Geom geom = objectClass.addChild<mjcf::Geom>();
    geom.rgba = Eigen::Vector4f::Ones();
 
    mjcf::Joint joint = objectClass.addChild<mjcf::Joint>();
    joint.frictionloss = 1;
 
    mjcf::Mesh mesh = objectClass.addChild<mjcf::Mesh>();
    mesh.scale = scaling.milliToMeter(Eigen::Vector3f::Ones().eval());
}
 
void
SimMJCF::setTimeStep(float timestep)
{
    document.option().timestep = timestep;
}
 
void
SimMJCF::addFloor(bool enabled, const std::string& texture, const std::string& name, float size)
{
    if (enabled)
    {
        ARMARX_INFO << "Adding floor (texture: " << texture << ")";
 
        // add default class
        mjcf::DefaultClass defaults = document.default_().getClass(name);
        mjcf::Geom defaultGeom = defaults.addChild<mjcf::Geom>();
        defaultGeom.type = "plane";
        defaultGeom.size = Eigen::Vector3f(size, size, 5);
        defaultGeom.material = name;
 
        // add texture
        bool addDefaultTexture = true;
 
        if (!texture.empty())
        {
            std::string absolute = texture;
            ArmarXDataPath::getAbsolutePath(texture, absolute);
 
            if (std::filesystem::exists(absolute))
            {
                document.asset().addTextureFile(name, absolute);
                addDefaultTexture = false;
            }
            else
            {
                ARMARX_WARNING << "Texture file does not exist: " << absolute
                               << "\nProceeding with generated floor texture";
            }
        }
        if (addDefaultTexture)
        {
            mjcf::Texture texture = document.asset().addChild<mjcf::Texture>();
            texture.name = name;
            texture.type = "2d";
            texture.builtin = "checker";
            texture.width = 512;
            texture.height = 512;
            texture.rgb1 = {.35f, .375f, .4f};
            texture.rgb2 = {.10f, .150f, .2f};
        }
 
        // add material
        mjcf::Material material = document.asset().addMaterial(name, name);
        material.reflectance = 0.3f;
        material.texrepeat = {1, 1};
        material.texuniform = true;
 
        // add body and geom
        mjcf::Body body = document.worldbody().addBody(name);
        body.childclass = name;
 
        mjcf::Geom geom = body.addChild<mjcf::Geom>();
        geom.name = name;
        geom.pos = Eigen::Vector3f::Zero();
    }
}
 
void
SimMJCF::addObject(VirtualRobot::SceneObjectPtr object,
                   VirtualRobot::SceneObject::Physics::SimulationType simType,
                   const std::string& className,
                   const std::filesystem::path& meshDir)
{
    const std::string objectName = object->getName();
 
    if (object->getParent())
    {
        ARMARX_WARNING << "Object has parent, which is currently not implemented.";
    }
 
    std::string meshName;
 
    if (false && object->getVisualization())
    {
        ARMARX_INFO << "Has visualization";
        ARMARX_INFO << "Visu filename: " << object->getVisualization()->getFilename();
        ARMARX_INFO << "Num faces: " << object->getVisualization()->getNumFaces();
        ARMARX_INFO << "Has tri mesh?: "
                    << object->getVisualization()->getTriMeshModel().operator bool();
        ARMARX_INFO << "Primitives: " << object->getVisualization()->primitives.size();
    }
 
    if (object->getCollisionModel())
    {
        VirtualRobot::CollisionModelPtr collisionModel = object->getCollisionModel();
 
        const std::string colModelName = collisionModel->getName();
 
        const std::string filename = colModelName + ".msh";
        const std::filesystem::path meshFilePath = meshDir / filename;
 
        try
        {
            // does nothing if already exists
            std::filesystem::create_directory(meshDir);
        }
        catch (const std::filesystem::filesystem_error& e)
        {
            ARMARX_ERROR << "Could not create temporary mesh dir " << meshDir << "."
                         << "\nReason: " << e.what();
        }
 
        if (std::filesystem::is_regular_file(meshFilePath))
        {
            ARMARX_VERBOSE << "Collision model " << colModelName << " already converted.";
        }
        else
        {
            VirtualRobot::TriMeshModelPtr meshModel = collisionModel->getTriMeshModel();
            if (meshModel)
            {
                ARMARX_VERBOSE << "Converting collision model " << colModelName << " ("
                               << meshModel->faces.size() << " faces) ...";
 
                VirtualRobot::mujoco::Mesh mjMesh =
                    VirtualRobot::mujoco::MeshConverter::fromVirtualRobot(*meshModel);
                mjMesh.write(meshFilePath.string());
            }
        }
 
        meshName = colModelName;
 
        // add the mesh as asset
        if (!document.asset().hasChild<mjcf::Mesh>("name", meshName))
        {
            mjcf::Mesh mesh = document.asset().addMesh(meshName, meshFilePath.string());
            mesh.class_ = className;
        }
    }
 
    mjcf::Body body = document.worldbody().addBody(objectName);
    body.childclass = className;
    body.pos = scaling.milliToMeter(math::Helpers::Position(object->getGlobalPose()).eval());
    body.quat = Eigen::Quaternionf(math::Helpers::Orientation(object->getGlobalPose()));
 
    mjcf::Geom geom = body.addGeomMesh(meshName);
    geom.name = objectName;
 
    if (object->getFriction() > 0)
    {
        Eigen::Vector3f friction = geom.friction;
        friction(0) = object->getFriction();
        geom.friction = friction;
    }
 
    mjcf::Inertial inertial = body.addInertial();
    inertial.mass = object->getMass();
    inertial.pos = scaling.milliToMeter(object->getCoMLocal());
    inertial.inertiaFromMatrix(object->getInertiaMatrix());
 
    switch (simType)
    {
        case VirtualRobot::SceneObject::Physics::SimulationType::eKinematic:
            // make it a mocap body
            body.mocap = true;
            break;
 
        case VirtualRobot::SceneObject::Physics::SimulationType::eDynamic:
            // add free joint
            {
                mjcf::Joint joint = body.addJoint();
                joint.name = objectName;
                joint.type = "free";
            }
            break;
 
        case VirtualRobot::SceneObject::Physics::SimulationType::eStatic:
        case VirtualRobot::SceneObject::Physics::SimulationType::eUnknown:
            // do nothing
            break;
    }
}