Simulator.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     Nikolaus ( vahrenkamp at kit dot edu )
 * @date       2015
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include <filesystem>
 
#include <VirtualRobot/MathTools.h>
#include <VirtualRobot/XML/BaseIO.h>
#include <VirtualRobot/Obstacle.h>
#include <VirtualRobot/ManipulationObject.h>
#include <VirtualRobot/XML/RobotIO.h>
 
#include "ArmarXCore/core/PackagePath.h"
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/util/StringHelpers.h>
#define MAX_SIM_TIME_WARNING 25
 
#include "Simulator.h"
 
#include <ArmarXCore/libraries/DecoupledSingleComponent/Decoupled.h>
 
#ifdef MUJOCO_PHYSICS_WORLD
#include "MujocoPhysicsWorld.h"
#endif
 
#include <SimoxUtility/algorithm/string/string_tools.h>
#include <SimoxUtility/json.h>
#include <VirtualRobot/Scene.h>
#include <VirtualRobot/SceneObject.h>
#include <VirtualRobot/XML/ObjectIO.h>
#include <VirtualRobot/math/Helpers.h>
 
#include <ArmarXCore/core/ArmarXManager.h>
#include <ArmarXCore/core/system/ArmarXDataPath.h>
#include <ArmarXCore/util/CPPUtility/trace.h>
 
#include <RobotAPI/interface/core/GeometryBase.h>
#include <RobotAPI/libraries/ArmarXObjects/ObjectID.h>
#include <RobotAPI/libraries/ArmarXObjects/Scene.h>
#include <RobotAPI/libraries/ArmarXObjects/json_conversions.h>
#include <RobotAPI/libraries/core/FramedPose.h>
#include <RobotAPI/libraries/core/Pose.h>
 
#include <MemoryX/core/MemoryXCoreObjectFactories.h>
#include <MemoryX/libraries/memorytypes/MemoryXTypesObjectFactories.h>
#include <MemoryX/libraries/memorytypes/entity/AgentInstance.h>
#include <MemoryX/libraries/memorytypes/entity/ObjectClass.h>
#include <MemoryX/libraries/memorytypes/entity/ObjectInstance.h>
#include <SimDynamics/DynamicsEngine/DynamicsObject.h>
#include <SimDynamics/DynamicsEngine/DynamicsRobot.h>
 
#ifdef ENABLE_SDF_FORMAT
#include <sdf/sdf.hh>
#endif
 
using namespace armarx;
using namespace memoryx;
 
SimulatorPropertyDefinitions::SimulatorPropertyDefinitions(std::string prefix) :
    ComponentPropertyDefinitions(prefix)
{
    defineOptionalProperty<std::string>(
        "SimoxSceneFileName", "", "Simox/VirtualRobot scene file name, e.g. myScene.xml");
    defineOptionalProperty<bool>("FloorPlane", true, "Enable floor plane.");
    defineOptionalProperty<std::string>("FloorTexture", "", "Texture file for floor.");
    defineOptionalProperty<std::string>(
        "LongtermMemory.SnapshotName", "", "Name of snapshot to load the scene")
        .setCaseInsensitive(true);
    defineOptionalProperty<bool>(
        "LoadSnapshotToWorkingMemory", true, "Load the snapshot also into the WorkingMemory");
    defineOptionalProperty<bool>("LoadAgentsFromSnapshot",
                                 false,
                                 "Also load the agents from the snapshot into the WorkingMemory");
 
    defineOptionalProperty<std::string>(
        "WorkingMemoryName", "WorkingMemory", "Name of WorkingMemory")
        .setCaseInsensitive(true);
    defineOptionalProperty<std::string>(
        "CommonStorageName", "CommonStorage", "Name of CommonStorage")
        .setCaseInsensitive(true);
    defineOptionalProperty<std::string>(
        "PriorKnowledgeName", "PriorKnowledge", "Name of PriorKnowledge")
        .setCaseInsensitive(true);
    defineOptionalProperty<std::string>(
        "LongtermMemoryName", "LongtermMemory", "Name of LongtermMemory")
        .setCaseInsensitive(true);
 
    defineOptionalProperty<std::string>("Scene.ObjectPackage",
                                        "PriorKnowledgeData",
                                        "Package to search for object models."
                                        "Used when loading scenes from JSON files.");
    defineOptionalProperty<std::string>("Scene.Path",
                                        "",
                                        "Path to one or multiple JSON scene file(s) to load.\n"
                                        "Example: 'PriorKnowledgeData/scenes/MyScene.json'.\n"
                                        "Multiple paths are separated by semicolons (';').\n"
                                        "The package to load a scene from is automatically"
                                        "extracted from its first path segment.");
 
    defineOptionalProperty<SimulatorType>("SimulationType",
                                          SimulatorType::Bullet,
                                          "Simulation type (Supported: Bullet, Kinematics, Mujoco)")
        .map("bullet", SimulatorType::Bullet)
        .map("kinematics", SimulatorType::Kinematics)
        .map("mujoco", SimulatorType::Mujoco)
        .setCaseInsensitive(true);
 
    for (int i = -1; i < MAX_INITIAL_ROBOT_COUNT - 1; ++i)
    {
        std::string postfix;
        if (i >= 0)
        {
            postfix = "_" + ValueToString(i);
        }
 
        defineOptionalProperty<std::string>(
            "RobotFileName" + postfix,
            "",
            "Simox/VirtualRobot robot file name, e.g. robot_model.xml");
        defineOptionalProperty<std::string>(
            "RobotInstanceName" + postfix, "", "Name of this robot instance");
        defineOptionalProperty<bool>(
            "RobotIsStatic" + postfix,
            false,
            "A static robot does not move due to the physical environment (i.e. no gravity). \n"
            "It is a static (but kinematic) object in the world.");
        defineOptionalProperty<bool>("ReportRobotPose", false, "Report the global robot pose.");
        defineOptionalProperty<bool>(
            "RobotCollisionModel" + postfix, false, "Show the collision model of the robot.");
        defineOptionalProperty<float>(
            "InitialRobotPose.x" + postfix, 0.f, "x component of initial robot position (mm)");
        defineOptionalProperty<float>(
            "InitialRobotPose.y" + postfix, 0.f, "y component of initial robot position (mm)");
        defineOptionalProperty<float>(
            "InitialRobotPose.z" + postfix, 0.f, "z component of initial robot position (mm)");
        defineOptionalProperty<float>("InitialRobotPose.roll" + postfix,
                                      0.f,
                                      "Initial robot pose: roll component of RPY angles (radian)");
        defineOptionalProperty<float>("InitialRobotPose.pitch" + postfix,
                                      0.f,
                                      "Initial robot pose: pitch component of RPY angles (radian)");
        defineOptionalProperty<float>("InitialRobotPose.yaw" + postfix,
                                      0.f,
                                      "Initial robot pose: yaw component of RPY angles (radian)");
        defineOptionalProperty<std::string>(
            "InitialRobotConfig" + postfix,
            "",
            "Initial robot config as comma separated list (RobotNodeName:value)");
 
        defineOptionalProperty<std::string>(
            "InitialNamedRobotConfig" + postfix,
            "",
            "Initial named robot config, e.g., `Home`.");
 
        defineOptionalProperty<float>(
            "RobotScaling" + postfix, 1.0f, "Scaling of the robot (1 = no scaling).");
        defineOptionalProperty<double>(
            "RobotControllerPID.p" + postfix, 10.0, "Setup robot controllers: PID paramter p.");
        defineOptionalProperty<double>(
            "RobotControllerPID.i" + postfix, 0.0, "Setup robot controllers: PID paramter i.");
        defineOptionalProperty<double>(
            "RobotControllerPID.d" + postfix, 0.0, "Setup robot controllers: PID paramter d.");
        defineOptionalProperty<bool>(
            "RobotSelfCollisions" + postfix,
            false,
            "If false, the robot bodies will not collide with other bodies of this \n"
            "robot but with all other bodies in the world. \n"
            "If true, the robot bodies will collide with all other bodies \n"
            "(This needs an accurately modelled robot model without self collisions \n"
            "if the robot does not move.)");
    }
    defineOptionalProperty<int>(
        "FixedTimeStepLoopNrSteps",
        10,
        "The maximum number of internal simulation loops (fixed time step mode).\n"
        "After max number of time steps, the remaining time is interpolated.");
    defineOptionalProperty<int>(
        "FixedTimeStepStepTimeMS",
        16,
        "The simulation's internal timestep (fixed time step mode). \n"
        "This property determines the precision of the simulation. \n"
        "Needs to be set to bigger values for slow PCs, but the simulation results might be bad. "
        "Smaller value for higher precision, but slower calculation.");
    defineOptionalProperty<int>("StepTimeMS", 25, "The simulation's time progress per step. ");
    defineOptionalProperty<bool>(
        "RealTimeMode",
        false,
        "If true the timestep is adjusted to the computing power of the host and \n"
        "the time progress in real time. Can result in bad results on slow PCs. \n"
        "The property StepTimeMS has then no effect. FixedTimeStepStepTimeMS needs \n"
        "to be adjusted if real time cannot be achieved on slower PCs.");
    defineOptionalProperty<float>(
        "MaxRealTime",
        1,
        "If not zero and the real time mode is off, the simulator will not run \n"
        "\tfaster than real-time * MaxRealTime even if the machine is fast enough \n"
        "\t(a sleep is inserted to slow down the simulator - the precision remains unchanged).\n"
        "\tIf set to 2, the simulator will not run faster than double speed. \n"
        "\tIf set to 0.5, the simulator will not run faster than half speed. \n"
        "\tIf set to 0, the simulator will run as fast as possible.\n");
 
    defineOptionalProperty<bool>(
        "LogRobot",
        false,
        "Enable robot logging. If true, the complete robot state is logged to a file each step.");
 
    defineOptionalProperty<int>("ReportVisuFrequency",
                                30,
                                "How often should the visualization data be published. Value is "
                                "given in Hz. (0 to disable)");
    defineOptionalProperty<std::string>(
        "ReportVisuTopicName",
        "SimulatorVisuUpdates",
        "The topic on which the visualization updates are published.");
    defineOptionalProperty<int>(
        "ReportDataFrequency",
        30,
        "How often should the robot data be published. Value is given in Hz. (0 to disable)");
    defineOptionalProperty<std::string>(
        "FixedObjects",
        "",
        "Fixate objects or parts of a robot in the world. Comma separated list. \n"
        "Define objects by their name, robots can be defined with robotName:RobotNodeName");
    defineOptionalProperty<bool>("DrawCoMVisu",
                                 true,
                                 "If true the CoM of each robot is drawn as a circle on the ground "
                                 "after each simulation cycle.");
}
 
SimulatorPropertyDefinitions::~SimulatorPropertyDefinitions()
{
}
 
Simulator::Simulator() : SimulatorInterface(), Component()
{
    // create data object
    //physicsWorld.reset(new BulletPhysicsWorld());
    //physicsWorldData.reset(new ArmarXPhysicsWorldData());
 
    currentComTimeMS = 0;
    currentSimTimeMS = 0;
    currentSyncTimeMS = 0;
    reportVisuTimeMS = 0;
    timeServerSpeed = 1;
 
    publishContacts = false;
    contactLayerName = "contacts";
    lastPublishedContacts = 0;
}
 
Simulator::~Simulator()
{
    ARMARX_VERBOSE << "~Simulator";
}
 
std::string
Simulator::getDefaultName() const
{
    return GetDefaultName();
}
 
std::string
Simulator::GetDefaultName()
{
    return "Simulator";
}
 
void
Simulator::onInitComponent()
{
    priorKnowledgeName = getProperty<std::string>("PriorKnowledgeName").getValue();
    commonStorageName = getProperty<std::string>("CommonStorageName").getValue();
    ARMARX_INFO << "Init component.";
 
    std::string snapshotName = getProperty<std::string>("LongtermMemory.SnapshotName").getValue();
 
    if (!snapshotName.empty())
    {
        ARMARX_LOG << "UsingProxy LongtermMemory for snapshot " << snapshotName;
        usingProxy(getProperty<std::string>("WorkingMemoryName").getValue());
        usingProxy(getProperty<std::string>("LongtermMemoryName").getValue());
        usingProxy(getProperty<std::string>("CommonStorageName").getValue());
        usingProxy(getProperty<std::string>("PriorKnowledgeName").getValue());
    }
    else
    {
        ARMARX_LOG << "Not using LongtermMemoryProxy";
    }
 
    bool floorPlane = getProperty<bool>("FloorPlane").getValue();
    std::string floorTexture = getProperty<std::string>("FloorTexture");
 
    SimulatorType simulatorType = SimulatorType::Bullet;
    try
    {
        simulatorType = getProperty<SimulatorType>("SimulationType");
    }
    catch (...)
    {
    }
 
    offeringTopic("GlobalRobotPoseLocalization");
    offeringTopic("SimulatorResetEvent");
    getProperty(reportRobotPose, "ReportRobotPose");
 
    const int stepTimeMs = getProperty<int>("StepTimeMS");
    const float maxRealTime = getProperty<float>("MaxRealTime");
    ARMARX_CHECK_NONNEGATIVE(maxRealTime);
 
 
    switch (simulatorType)
    {
        case SimulatorType::Kinematics:
        {
            KinematicsWorldPtr kw(new KinematicsWorld());
            kw->initialize(stepTimeMs, maxRealTime, floorPlane, floorTexture);
            physicsWorld = kw;
        }
        break;
        case SimulatorType::Bullet:
        {
            int bulletFixedTimeStepMS = getProperty<int>("FixedTimeStepStepTimeMS").getValue();
            int bulletFixedTimeStepMaxNrLoops =
                getProperty<int>("FixedTimeStepLoopNrSteps").getValue();
            BulletPhysicsWorldPtr bw(new BulletPhysicsWorld());
            bw->initialize(stepTimeMs,
                           bulletFixedTimeStepMS,
                           bulletFixedTimeStepMaxNrLoops,
                           maxRealTime,
                           floorPlane,
                           floorTexture);
            physicsWorld = bw;
        }
        break;
        case SimulatorType::Mujoco:
        {
#ifdef MUJOCO_PHYSICS_WORLD
            MujocoPhysicsWorldPtr mw(new MujocoPhysicsWorld());
            // initialize mujoco
 
            mw->initialize(stepTimeMs, floorPlane, floorTexture);
 
            physicsWorld = mw;
#else
            ARMARX_ERROR << "Simulator type 'mujoco' is not supported, since simulator was built "
                            "without MujocoX."
                         << "\nGet the package MujocoX from: https://gitlab.com/h2t/mujoco .";
            return;
#endif
        }
        break;
    }
 
    timeserverProxy = new SimulatorTimeServerProxy(this);
    getArmarXManager()->addObject(timeserverProxy);
    offeringTopic(TIME_TOPIC_NAME);
}
 
void
Simulator::addRobotsFromProperties()
{
    for (int i = -1; i < MAX_INITIAL_ROBOT_COUNT - 1; ++i)
    {
        std::string postfix;
        if (i >= 0)
        {
            postfix = "_" + ValueToString(i);
        }
        if (!getProperty<std::string>("RobotFileName" + postfix).isSet() ||
            getProperty<std::string>("RobotFileName" + postfix).getValue().empty())
        {
            continue;
        }
 
        std::string robotFile = getProperty<std::string>("RobotFileName" + postfix).getValue();
        Eigen::Matrix4f gp = Eigen::Matrix4f::Identity();
 
        VirtualRobot::MathTools::rpy2eigen4f(
            getProperty<float>("InitialRobotPose.roll" + postfix).getValue(),
            getProperty<float>("InitialRobotPose.pitch" + postfix).getValue(),
            getProperty<float>("InitialRobotPose.yaw" + postfix).getValue(),
            gp);
 
        math::Helpers::Position(gp) =
            Eigen::Vector3f(getProperty<float>("InitialRobotPose.x" + postfix).getValue(),
                            getProperty<float>("InitialRobotPose.y" + postfix).getValue(),
                            getProperty<float>("InitialRobotPose.z" + postfix).getValue());
 
        ARMARX_INFO << "Initial robot pose:\n" << gp;
        std::map<std::string, float> initConfig;
        
        if(getProperty<std::string>("InitialNamedRobotConfig" + postfix).isSet())
        {
            std::string confStr =
                getProperty<std::string>("InitialNamedRobotConfig" + postfix).getValue();
            
            ARMARX_INFO << "Using named robot config: " << QUOTED(confStr);
 
            ARMARX_CHECK_NOT_EMPTY(robotFile);
            std::string robFileGlobal = robotFile;
 
            if (!ArmarXDataPath::getAbsolutePath(robotFile, robFileGlobal))
            {
                ARMARX_ERROR << "Could not find robot file " << robotFile;
            }
 
            const auto robot = VirtualRobot::RobotIO::loadRobot(robFileGlobal, VirtualRobot::BaseIO::eStructure);
            ARMARX_CHECK_NOT_NULL(robot) << "Could not load robot from file " << QUOTED(robFileGlobal);
 
            const std::optional<std::map<std::string, float>> namedConfig =
                robot->getConfiguration(confStr);
 
            ARMARX_CHECK_NOT_NULL(namedConfig)
                << "Could not find named robot config " << QUOTED(confStr) << " in robot file "
                << QUOTED(robFileGlobal);
 
            initConfig = namedConfig.value();
 
            if (getProperty<std::string>("InitialRobotConfig" + postfix).isSet())
            {
                ARMARX_WARNING << "Ignoring InitialRobotConfig entry "
                               << getProperty<std::string>("InitialRobotConfig" + postfix).getValue()
                               << " because InitialNamedRobotConfig is set.";
            }
                
        }
        else if (getProperty<std::string>("InitialRobotConfig" + postfix).isSet())
        {
 
            std::string confStr =
                getProperty<std::string>("InitialRobotConfig" + postfix).getValue();
            std::vector<std::string> confList = Split(confStr, ",", true, true);
            for (auto& pos : confList)
            {
                ARMARX_INFO << "Processing '" << pos << "'";
                std::vector<std::string> d = Split(pos, ":", true, true);
                if (d.size() != 2)
                {
                    ARMARX_WARNING << "Ignoring InitialRobotConfig entry " << pos;
                    continue;
                }
                std::string name = d.at(0);
 
                initConfig[name] = static_cast<float>(atof(d.at(1).c_str()));
                ARMARX_INFO << "1:'" << name << "', 2:" << initConfig[name];
            }
        }
 
 
        double pid_p = getProperty<double>("RobotControllerPID.p" + postfix).getValue();
        double pid_i = getProperty<double>("RobotControllerPID.i" + postfix).getValue();
        double pid_d = getProperty<double>("RobotControllerPID.d" + postfix).getValue();
 
        std::string robotInstanceName =
            getProperty<std::string>("RobotInstanceName" + postfix).getValue();
 
        bool staticRobot = getProperty<bool>("RobotIsStatic" + postfix).getValue();
        bool colModelRobot = getProperty<bool>("RobotCollisionModel" + postfix).getValue();
        float scaling = getProperty<float>("RobotScaling" + postfix).getValue();
        bool selfCollisions = getProperty<bool>("RobotSelfCollisions" + postfix);
        if (!robotFile.empty())
        {
            std::string robFileGlobal = robotFile;
 
            if (!ArmarXDataPath::getAbsolutePath(robotFile, robFileGlobal))
            {
                ARMARX_ERROR << "Could not find robot file " << robotFile;
            }
            else
            {
                ARMARX_INFO << "Adding robot with name '" << robotInstanceName << "' from file "
                            << robFileGlobal;
                addRobot(robotInstanceName,
                         robFileGlobal,
                         gp,
                         robotFile,
                         pid_p,
                         pid_i,
                         pid_d,
                         staticRobot,
                         scaling,
                         colModelRobot,
                         initConfig,
                         selfCollisions);
            }
        }
    }
}
 
void
Simulator::addSceneFromFile(const std::string& sceneFile)
{
    std::string sf;
    if (!ArmarXDataPath::getAbsolutePath(sceneFile, sf))
    {
        ARMARX_ERROR << "Could not find scene file " << sceneFile;
    }
    else
    {
        ARMARX_INFO << "Loading scene " << sf << "...";
        physicsWorld->addScene(sf);
        // setup robot topics
        updateRobotTopics();
    }
}
 
void
Simulator::addSceneFromMemorySnapshot(const std::string& snapshotName)
{
    requestFileManager();
    if (!fileManager)
    {
        ARMARX_ERROR << "No connection to memory - cannot add snapshot!";
        return;
    }
    memoryPrx = getProxy<WorkingMemoryInterfacePrx>(
        getProperty<std::string>("WorkingMemoryName").getValue());
 
 
    priorKnowledgePrx = getProxy<PriorKnowledgeInterfacePrx>(priorKnowledgeName);
    longtermMemoryPrx = getProxy<LongtermMemoryInterfacePrx>(
        getProperty<std::string>("LongtermMemoryName").getValue());
    WorkingMemorySnapshotInterfacePrx snapshotInterfacePrx;
 
    if (longtermMemoryPrx && priorKnowledgePrx && memoryPrx)
    {
        ARMARX_LOG << "Loading snapshot " << snapshotName;
 
        try
        {
            snapshotInterfacePrx =
                longtermMemoryPrx->getWorkingMemorySnapshotListSegment()->openSnapshot(
                    snapshotName);
            if (getProperty<bool>("LoadSnapshotToWorkingMemory").getValue())
            {
                if (snapshotInterfacePrx)
                {
                    longtermMemoryPrx->loadWorkingMemorySnapshot(snapshotName, memoryPrx);
                }
                else
                {
                    ARMARX_ERROR << "Could not get snapshot segment from longterm memory...";
                }
            }
        }
        catch (memoryx::SnapshotNotFoundException&)
        {
            ARMARX_ERROR << "Could not find snapshot: " << snapshotName;
        }
        catch (...)
        {
            ARMARX_ERROR << "Failed to load snapshot with name:" << snapshotName;
        }
    }
    else
    {
        ARMARX_ERROR << "Could not get PriorKnowledge/LongtermMemory/WorkingMemory Proxy";
    }
 
    if (longtermMemoryPrx && priorKnowledgePrx && fileManager && snapshotInterfacePrx)
    {
        ARMARX_LOG << "Found and opened snapshot " << snapshotName;
        addSnapshot(snapshotInterfacePrx);
    }
    else
    {
        ARMARX_ERROR << "Failed to open snapshot " << snapshotName;
    }
}
 
void
Simulator::addSceneFromSdfFile(const std::string& scenePath)
{
#ifdef ENABLE_SDF_FORMAT
    std::string scenePackage = simox::alg::split(scenePath, "/").at(0);
    CMakePackageFinder cmakeFinder(scenePackage);
    if (!cmakeFinder.packageFound())
    {
        ARMARX_WARNING << "Could not find CMake package " << scenePackage << ".";
        return;
    }
 
    std::filesystem::path dataDir = cmakeFinder.getDataDir();
    std::filesystem::path path = dataDir / scenePath;
 
 
    sdf::SDFPtr sdfFile(new sdf::SDF());
    sdf::init(sdfFile);
 
    std::filesystem::path modelDirectory = path.parent_path();
 
    const std::string modelURI = "model://";
 
    auto findFileCallback = [this, &modelURI](const std::string& uri) -> std::string
    {
        std::stringstream ss;
        ss << "Find '" << uri << "' ... -> ";
 
        const std::string classIDStr = simox::alg::remove_prefix(uri, modelURI);
        const ObjectID classID = ObjectID::FromString(classIDStr);
 
        if (std::optional<ObjectInfo> info = objectFinder.findObject(classID))
        {
            if (std::optional<PackageFileLocation> modelPath = info->getModel())
            {
                std::filesystem::path result = modelPath->absolutePath.parent_path();
                ss << result;
                ARMARX_VERBOSE << ss.str();
 
                return result;
            }
        }
        ss << uri;
        ARMARX_VERBOSE << ss.str();
 
        return uri;
    };
 
    sdf::ParserConfig parser;
    parser.AddURIPath(modelURI, modelDirectory.string());
    parser.SetFindCallback(findFileCallback);
 
    sdf::Root root;
    sdf::Errors errors = root.Load(path.string(), parser);
 
    for (auto const& error : errors)
    {
        ARMARX_ERROR << "Error on reading SDF: " << error;
    }
 
    if (!errors.empty())
    {
        ARMARX_WARNING << "Could not load SDF from '" << path << "'";
        return;
    }
 
    if (root.WorldCount() == 0)
    {
        ARMARX_WARNING << "SDF does not contain a world";
        return;
    }
 
    if (root.WorldCount() > 1)
    {
        ARMARX_WARNING << "SDF contains multiple worlds";
        return;
    }
 
    sdf::World* world = root.WorldByIndex(0);
    auto addSceneObject = [&](sdf::Model* model)
    {
        const std::string name = model->Name();
        if (physicsWorld->hasObject(name))
        {
            ARMARX_WARNING << "Object with name \"" << name << "\" already exists.";
            return;
        }
 
        const std::string uri = model->Uri();
        if (uri.empty())
        {
            ARMARX_WARNING << "No URI set for object \"" << name << "\" in SDF.";
            return;
        }
 
 
        // Not clear where the result of the callback called during loading is stored.
        const std::filesystem::path modelPath = findFileCallback(uri);
 
        gz::math::Pose3d pose = model->RawPose();
 
        const float meterToMillimeter = 1000.0F;
 
        Eigen::Vector3f position;
        position.x() = static_cast<float>(pose.Pos().X() * meterToMillimeter);
        position.y() = static_cast<float>(pose.Pos().Y() * meterToMillimeter);
        position.z() = static_cast<float>(pose.Pos().Z() * meterToMillimeter);
 
        Eigen::Quaternionf orientation;
        orientation.x() = static_cast<float>(pose.Rot().X());
        orientation.y() = static_cast<float>(pose.Rot().Y());
        orientation.z() = static_cast<float>(pose.Rot().Z());
        orientation.w() = static_cast<float>(pose.Rot().W());
 
        auto isStatic = model->Static();
 
        std::string robotFilename = modelPath.filename().generic_string() + ".urdf";
        std::filesystem::path robotPath = modelPath / robotFilename;
 
        std::string xmlFilename = modelPath.filename().generic_string() + ".xml";
        std::filesystem::path xmlPath = modelPath / xmlFilename;
 
        if (std::filesystem::exists(xmlPath))
        {
            ARMARX_INFO << "Loading object '" << name << "' from object XML file " << xmlPath;
            VirtualRobot::ManipulationObjectPtr simoxObject =
                VirtualRobot::ObjectIO::loadManipulationObject(xmlPath.string());
 
            simoxObject->setGlobalPose(simox::math::pose(position, orientation));
 
            VirtualRobot::SceneObject::Physics::SimulationType simType =
                isStatic ? VirtualRobot::SceneObject::Physics::eKinematic
                         : VirtualRobot::SceneObject::Physics::eDynamic;
 
            simoxObject->setSimulationType(simType);
            simoxObject->setFilename(xmlPath.string());
            simoxObject->setName(name);
 
            physicsWorld->addObstacle(
                simoxObject, simType, xmlPath.string(), name, {}, scenePackage);
        }
        else if (std::filesystem::exists(robotPath))
        {
            ARMARX_INFO << "Loading object '" << name << "' from robot URDF file " << xmlPath;
            VirtualRobot::RobotPtr robot = VirtualRobot::RobotIO::loadRobot(robotPath);
 
            robot->setGlobalPose(simox::math::pose(position, orientation));
 
            VirtualRobot::SceneObject::Physics::SimulationType simType =
                isStatic ? VirtualRobot::SceneObject::Physics::eKinematic
                         : VirtualRobot::SceneObject::Physics::eDynamic;
 
            robot->setSimulationType(simType);
            robot->setFilename(robotPath.string());
            robot->setName(name);
 
            physicsWorld->addRobot(robot, 10.0, 0.0, 0.0, robotPath.string(), isStatic);
        }
        else
        {
            ARMARX_INFO
                << "Not adding object '" << name
                << "' because neither object XML nor robot URDF file exist. (Tried object XML = "
                << xmlPath << " and robot URDF = " << robotPath << ")";
        }
    };
 
    for (size_t index = 0; index < world->ModelCount(); index++)
    {
        addSceneObject(world->ModelByIndex(index));
    }
#else
    ARMARX_ERROR << "Loading from SDF file was not compiled";
#endif
}
 
void
Simulator::addSceneFromJsonFile(const std::string& scenePath)
{
    std::string mutScenePath = scenePath;
    if (!simox::alg::ends_with(mutScenePath, ".json"))
    {
        mutScenePath += ".json";
    }
 
    std::string scenePackage = simox::alg::split(scenePath, "/").at(0);
    CMakePackageFinder cmakeFinder(scenePackage);
    if (!cmakeFinder.packageFound())
    {
        ARMARX_WARNING << "Could not find CMake package " << scenePackage << ".";
        return;
    }
 
    std::filesystem::path dataDir = cmakeFinder.getDataDir();
    std::filesystem::path path = dataDir / scenePath;
 
    armarx::objects::Scene scene;
    try
    {
        scene = simox::json::read<armarx::objects::Scene>(path);
    }
    catch (const simox::json::error::JsonError& e)
    {
        ARMARX_WARNING << "Loading scene snapshot failed: \n" << e.what();
        return;
    }
 
    std::map<ObjectID, int> idCounters;
    for (const auto& object : scene.objects)
    {
        if (simox::alg::starts_with(object.className, "#"))
        {
            // we ignore stuff that is out commented in json
            continue;
        }
 
        addJsonSceneObject(object, idCounters);
    }
}
 
void
Simulator::addJsonSceneObject(const armarx::objects::SceneObject object,
                              std::map<ObjectID, int>& idCounters)
{
    const ObjectID classID = object.getClassID();
    const std::string name =
        object.getObjectID()
            .withInstanceName(object.instanceName.empty() ? std::to_string(idCounters[classID]++)
                                                          : object.instanceName)
            .str();
 
    if (physicsWorld->hasObject(name))
    {
        ARMARX_INFO << "Object with name \"" << name << "\" already exists.";
        return;
    }
 
    ARMARX_INFO << "Adding object with name \"" << name << "\" to scene.";
 
    std::optional<ObjectInfo> objectInfo = objectFinder.findObject(classID);
    VirtualRobot::ManipulationObjectPtr simoxObject =
        VirtualRobot::ObjectIO::loadManipulationObject(objectInfo->simoxXML().absolutePath);
    simoxObject->setGlobalPose(simox::math::pose(object.position, object.orientation));
 
    VirtualRobot::SceneObject::Physics::SimulationType simType;
    if (object.isStatic.has_value())
    {
        simType = *object.isStatic ? VirtualRobot::SceneObject::Physics::eKinematic
                                   : VirtualRobot::SceneObject::Physics::eDynamic;
    }
    else
    {
        simType = VirtualRobot::SceneObject::Physics::eKinematic;
    }
 
    PackageFileLocation simoxXml = objectInfo->simoxXML();
    simoxObject->setSimulationType(simType);
    simoxObject->setFilename(simoxXml.absolutePath);
    simoxObject->setName(name);
 
    physicsWorld->addObstacle(
        simoxObject, simType, simoxXml.relativePath, object.className, {}, simoxXml.package);
}
 
void
Simulator::fixateRobotNode(const std::string& robotName, const std::string& robotNodeName)
{
    ARMARX_VERBOSE << "Fixing robot node" << robotName << "." << robotNodeName;
    physicsWorld->setRobotNodeSimType(
        robotName, robotNodeName, VirtualRobot::SceneObject::Physics::eStatic);
}
 
void
Simulator::fixateObject(const std::string& objectName)
{
    ARMARX_VERBOSE << "Fixing object" << objectName;
    physicsWorld->setObjectSimType(objectName, VirtualRobot::SceneObject::Physics::eStatic);
}
 
void
Simulator::initializeData()
{
    ARMARX_INFO << "Initializing simulator...";
 
    addRobotsFromProperties();
 
    std::string sceneFile = getProperty<std::string>("SimoxSceneFileName").getValue();
    if (!sceneFile.empty())
    {
        addSceneFromFile(sceneFile);
    }
 
    std::string snapshotName = getProperty<std::string>("LongtermMemory.SnapshotName").getValue();
    if (!snapshotName.empty())
    {
        addSceneFromMemorySnapshot(snapshotName);
    }
 
    std::string objectPackage = getProperty<std::string>("Scene.ObjectPackage").getValue();
    objectFinder = ObjectFinder(objectPackage);
 
    std::string scenePaths = getProperty<std::string>("Scene.Path").getValue();
    if (!scenePaths.empty())
    {
        bool trim = true;
        for (const std::string& scenePath : simox::alg::split(scenePaths, ";", trim))
        {
            std::filesystem::path path = scenePath;
            std::string sdfExtension = ".sdf";
 
            if (path.has_extension() && path.extension().string() == sdfExtension)
            {
                ARMARX_INFO << "Loading scene from SDF file '" << scenePath << "' ...";
                addSceneFromSdfFile(scenePath);
            }
            else
            {
                ARMARX_INFO << "Loading scene from JSON file '" << scenePath << "' ...";
                addSceneFromJsonFile(scenePath);
            }
        }
    }
 
    //check for fixed objects
    std::string fixedObjects = getProperty<std::string>("FixedObjects").getValue();
    if (!fixedObjects.empty())
    {
        std::vector<std::string> objects = Split(fixedObjects, ",;");
        for (auto o : objects)
        {
            std::vector<std::string> rob = Split(o, ":");
            if (rob.size() == 2)
            {
                // robot
                fixateRobotNode(rob.at(0), rob.at(1));
            }
            else
            {
                // object
                fixateObject(o);
            }
        }
    }
 
    physicsWorld->resetSimTime();
}
 
SimulatedRobotState
Simulator::stateFromRobotInfo(RobotInfo const& r, IceUtil::Time timestamp)
{
    SimulatedRobotState state;
 
    state.hasRobot = true;
    state.timestampInMicroSeconds = timestamp.toMicroSeconds();
 
    state.jointAngles = std::move(r.jointAngles);
    state.jointVelocities = std::move(r.jointVelocities);
    state.jointTorques = std::move(r.jointTorques);
 
    state.pose = new Pose(r.pose);
    state.linearVelocity = new Vector3(r.linearVelocity);
    state.angularVelocity = new Vector3(r.angularVelocity);
 
    for (auto& forceTorqueSensor : r.forceTorqueSensors)
    {
        if (forceTorqueSensor.enable)
        {
            ForceTorqueData& ftData = state.forceTorqueValues.emplace_back();
            ftData.force = new Vector3(forceTorqueSensor.currentForce);
            ftData.torque = new Vector3(forceTorqueSensor.currentTorque);
            ftData.sensorName = forceTorqueSensor.sensorName;
            ftData.nodeName = forceTorqueSensor.robotNodeName;
        }
    }
 
    return state;
}
 
memoryx::GridFileManagerPtr
Simulator::requestFileManager()
{
    if (fileManager)
    {
        ARMARX_INFO << deactivateSpam(10)
                    << "Was able to find a reference to the CommonStorage. Continue with existing "
                       "connection to memory.";
        return fileManager;
    }
    CommonStorageInterfacePrx commonStoragePrx =
        getProxy<CommonStorageInterfacePrx>(commonStorageName, false, "", false);
 
    if (commonStoragePrx)
    {
        ARMARX_INFO << deactivateSpam(10)
                    << "Was able to find a reference to the CommonStorage. Reset GridFSManager and "
                       "continue with memory.";
        fileManager.reset(new GridFileManager(commonStoragePrx));
    }
    else
    {
        ARMARX_INFO << deactivateSpam(10)
                    << "Could not get CommonStorage Proxy - continuing without memory";
    }
    return fileManager;
}
 
void
Simulator::onConnectComponent()
{
    ARMARX_VERBOSE << "Connecting ArmarX Simulator";
 
    getTopic(globalRobotLocalization, "GlobalRobotPoseLocalization");
    getTopic(simulatorResetEventTopic, "SimulatorResetEvent");
 
    entityDrawerPrx = getTopic<memoryx::EntityDrawerInterfacePrx>("DebugDrawerUpdates");
 
    int hzVisu = getProperty<int>("ReportVisuFrequency").getValue();
    int hzData = getProperty<int>("ReportDataFrequency").getValue();
    std::string top = getProperty<std::string>("ReportVisuTopicName").getValue();
 
    initializeData();
 
    ARMARX_DEBUG << "Starting periodic simulation task in ArmarX Simulator";
 
    timeTopicPrx = ManagedIceObject::getTopic<TimeServerListenerPrx>(TIME_TOPIC_NAME);
    start();
 
    ARMARX_DEBUG << "Starting periodic visualization report task in ArmarX Simulator";
 
    if (reportVisuTask)
    {
        reportVisuTask->stop();
    }
 
    if (hzVisu > 0)
    {
        offeringTopic(top);
        ARMARX_INFO << "Connecting to topic " << top;
        simulatorVisuUpdateListenerPrx = getTopic<SimulatorListenerInterfacePrx>(top);
 
        ARMARX_DEBUG << "Creating report visu task";
        int cycleTime = 1000 / hzVisu;
        reportVisuTask = new PeriodicTask<Simulator>(
            this, &Simulator::reportVisuLoop, cycleTime, false, "ArmarXSimulatorReportVisuTask");
        reportVisuTask->start();
        reportVisuTask->setDelayWarningTolerance(100);
    }
 
    ARMARX_DEBUG << "Starting periodic data report task in ArmarX Simulator";
 
    if (reportDataTask)
    {
        reportDataTask->stop();
    }
 
    if (hzData > 0)
    {
        int cycleTime = 1000 / hzData;
        reportDataTask = new PeriodicTask<Simulator>(
            this, &Simulator::reportDataLoop, cycleTime, false, "ArmarXSimulatorReportDataTask");
        reportDataTask->start();
        reportDataTask->setDelayWarningTolerance(100);
    }
 
    ARMARX_DEBUG << "finished connection";
}
 
void
Simulator::onDisconnectComponent()
{
    ARMARX_VERBOSE << "disconnecting Simulator";
 
    if (reportDataTask)
    {
        reportDataTask->stop();
    }
 
    if (reportVisuTask)
    {
        reportVisuTask->stop();
    }
 
    shutdownSimulationLoop();
 
    resetData();
}
 
void
Simulator::onExitComponent()
{
    ARMARX_VERBOSE << "exiting Simulator";
 
    if (reportDataTask)
    {
        ARMARX_VERBOSE << "stopping report task";
        reportDataTask->stop();
        ARMARX_VERBOSE << "stopping report task...Done!";
    }
 
    if (reportVisuTask)
    {
        ARMARX_VERBOSE << "stopping visu task";
        reportVisuTask->stop();
        ARMARX_VERBOSE << "stopping visu task...Done!";
    }
 
    shutdownSimulationLoop();
    timeserverProxy =
        nullptr; // delete the timeserver proxy as it holds a handle to this object, preventing it from being deleted
    ARMARX_VERBOSE << "exiting Simulator...Done";
}
 
void
Simulator::shutdownSimulationLoop()
{
    ARMARX_VERBOSE << "shutting down simulation loop";
    simulatorThreadRunning = true;
    simulatorThreadShutdown = true;
    condSimulatorThreadRunning.notify_all();
    ARMARX_VERBOSE << "---- get thread control";
    IceUtil::ThreadControl threadControl = getThreadControl();
    ARMARX_VERBOSE << "---- get thread control...Done!";
    ARMARX_VERBOSE << "---- joining thread control";
    if (isAlive())
    {
        threadControl.join();
    }
    ARMARX_VERBOSE << "---- joining thread control...Done!";
    ARMARX_VERBOSE << "shutting down simulation loop...Done!";
}
 
void
Simulator::resetData(bool clearWorkingMemory)
{
    ARMARX_INFO << "Deleting data";
 
    auto lockEngine = physicsWorld->getScopedEngineLock(__FUNCTION__);
    auto lockData = physicsWorld->getScopedSyncLock(__FUNCTION__);
 
    {
        ARMARX_INFO << "Clearing class map...";
        classInstanceMap.clear();
 
        physicsWorld->resetData();
    }
 
    if (clearWorkingMemory && memoryPrx)
    {
        ARMARX_INFO << "Clearing working memory...";
 
        try
        {
            memoryPrx->clear();
        }
        catch (...)
        {
            ARMARX_WARNING << "Clearing working memory failed...";
        }
    }
    ARMARX_VERBOSE << "Deleting data...Done!";
}
 
bool
Simulator::addSnapshot(WorkingMemorySnapshotInterfacePrx snapshotInterfacePrx)
{
    ARMARX_VERBOSE << "Adding snapshot from memory";
 
    if (!snapshotInterfacePrx || !priorKnowledgePrx)
    {
        ARMARX_ERROR << "Null data?!";
        return false;
    }
 
    const std::string segmentName = "objectInstances";
 
    try
    {
        PersistentEntitySegmentBasePrx segInstances = snapshotInterfacePrx->getSegment(segmentName);
 
        if (!segInstances)
        {
            ARMARX_ERROR << "Segment not found in snapshot: " << segmentName;
            return false;
        }
 
        // get classes for IV file loading (instances refer to classes, and classes store IV files)
        PersistentObjectClassSegmentBasePrx classesSegmentPrx =
            priorKnowledgePrx->getObjectClassesSegment();
 
        if (!classesSegmentPrx)
        {
            ARMARX_ERROR << "Classes Segment not found in PriorKnowledge";
            return false;
        }
 
 
        EntityIdList ids = segInstances->getAllEntityIds();
 
        int c = 0;
 
        for (EntityIdList::const_iterator it = ids.begin(); it != ids.end(); ++it)
        {
            EntityBasePtr snapEntity = segInstances->getEntityById(*it);
            ObjectInstancePtr snapInstance = ObjectInstancePtr::dynamicCast(snapEntity);
            std::string id = snapInstance->getId();
            bool ok = snapInstance->hasAttribute("classes");
 
            if (!ok)
            {
                ARMARX_ERROR << "SnapInstance " << id
                             << " has no classes attribute, could not load iv files, skipping";
                continue;
            }
 
            std::string classesName = snapInstance->getAttributeValue("classes")->getString();
 
            EntityBasePtr classesEntity = classesSegmentPrx->getEntityByName(classesName);
 
            if (!classesEntity)
            {
                ARMARX_ERROR << "Classes Segment does not know class with name : " << classesName;
                continue;
            }
 
            ObjectClassPtr objectClass = ObjectClassPtr::dynamicCast(classesEntity);
 
            if (!objectClass)
            {
                ARMARX_ERROR << "Could not cast class entity : " << classesName;
                continue;
            }
 
 
            Eigen::Matrix4f m = Eigen::Matrix4f::Identity();
 
            // Position
            FramedPositionPtr objPos = snapInstance->getPosition();
 
            if (objPos)
            {
                m.block<3,1>(0, 3) = objPos->toEigen();
            }
 
            // Orientation
            FramedOrientationPtr objOrient = snapInstance->getOrientation();
 
            if (objOrient)
            {
                m.block<3,3>(0, 0) = objOrient->toEigen();
            }
 
            PosePtr poseGlobal(new Pose(m));
            memoryx::EntityWrappers::SimoxObjectWrapperPtr sw =
                objectClass->addWrapper(new EntityWrappers::SimoxObjectWrapper(fileManager));
            VirtualRobot::ManipulationObjectPtr mo = sw->getManipulationObject()->clone(
                sw->getManipulationObject()
                    ->getName()); // maybe not needed: cloning the object to avoid changing content of priorKnowledge
 
            if (mo)
            {
                std::string instanceName = snapInstance->getName();
                int i = 2;
                while (hasObject(instanceName))
                {
                    instanceName = snapInstance->getName() + "_" + to_string(i);
                    i++;
                }
                if (instanceName != snapInstance->getName())
                {
                    ARMARX_INFO << "Object instance with name '" << snapInstance->getName()
                                << "'' already exists - using '" << instanceName << "'";
                }
                bool isStatic =
                    mo->getSimulationType() == VirtualRobot::SceneObject::Physics::eStatic;
                addObject(objectClass, instanceName, poseGlobal, isStatic);
            }
        }
 
        ARMARX_LOG << "Added " << c << " objects";
    }
    catch (...)
    {
        ARMARX_WARNING << "addSnapshot failed...";
        return false;
    }
    if (getProperty<bool>("LoadAgentsFromSnapshot").getValue())
    {
        return loadAgentsFromSnapshot(snapshotInterfacePrx);
    }
    else
    {
        return true;
    }
}
 
bool
Simulator::loadAgentsFromSnapshot(WorkingMemorySnapshotInterfacePrx snapshotInterfacePrx)
{
    const std::string segmentName = "agentInstances";
    bool result = true;
    try
    {
        PersistentEntitySegmentBasePrx segInstances = snapshotInterfacePrx->getSegment(segmentName);
 
        if (!segInstances)
        {
            ARMARX_ERROR << "Segment not found in snapshot: " << segmentName;
            return false;
        }
 
        EntityIdList ids = segInstances->getAllEntityIds();
 
 
        for (EntityIdList::const_iterator it = ids.begin(); it != ids.end(); ++it)
        {
            EntityBasePtr snapEntity = segInstances->getEntityById(*it);
            AgentInstancePtr snapAgent = AgentInstancePtr::dynamicCast(snapEntity);
 
            auto agentName = snapAgent->getName();
            std::string agentModelFile = snapAgent->getAgentFilePath();
            std::string absModelFile;
            if (hasRobot(agentName))
            {
                ARMARX_INFO << "Robot with name '" << agentName << "' already exists.";
                continue;
            }
            if (!ArmarXDataPath::getAbsolutePath(agentModelFile, absModelFile))
            {
                ARMARX_WARNING << "Could not load agent '" << agentName
                               << "'- file not found: " << agentModelFile;
                result = false;
                continue;
            }
 
 
            if (addRobot(agentName, absModelFile, snapAgent->getPose()->toEigen(), agentModelFile)
                    .empty())
            {
                result = false;
            }
        }
    }
    catch (...)
    {
        handleExceptions();
    }
 
    return result;
}
 
void
Simulator::actuateRobotJoints(const std::string& robotName,
                              const NameValueMap& angles,
                              const NameValueMap& velocities,
                              const Ice::Current&)
{
    physicsWorld->actuateRobotJoints(robotName, angles, velocities);
}
 
void
Simulator::actuateRobotJointsPos(const std::string& robotName,
                                 const NameValueMap& angles,
                                 const Ice::Current&)
{
    physicsWorld->actuateRobotJointsPos(robotName, angles);
}
 
void
Simulator::actuateRobotJointsVel(const std::string& robotName,
                                 const NameValueMap& velocities,
                                 const Ice::Current&)
{
    physicsWorld->actuateRobotJointsVel(robotName, velocities);
}
 
void
Simulator::actuateRobotJointsTorque(const std::string& robotName,
                                    const NameValueMap& torques,
                                    const Ice::Current&)
{
    physicsWorld->actuateRobotJointsTorque(robotName, torques);
}
 
void
Simulator::setRobotPose(const std::string& robotName,
                        const PoseBasePtr& globalPose,
                        const Ice::Current&)
{
    VirtualRobot::MathTools::Quaternion qa;
    qa.x = globalPose->orientation->qx;
    qa.y = globalPose->orientation->qy;
    qa.z = globalPose->orientation->qz;
    qa.w = globalPose->orientation->qw;
    Eigen::Matrix4f gp = VirtualRobot::MathTools::quat2eigen4f(qa);
    gp(0, 3) = globalPose->position->x;
    gp(1, 3) = globalPose->position->y;
    gp(2, 3) = globalPose->position->z;
    physicsWorld->setRobotPose(robotName, gp);
}
 
void
Simulator::applyForceRobotNode(const std::string& robotName,
                               const std::string& robotNodeName,
                               const Vector3BasePtr& force,
                               const Ice::Current&)
{
    Vector3Ptr v3p = Vector3Ptr::dynamicCast(force);
    physicsWorld->applyForceRobotNode(robotName, robotNodeName, v3p->toEigen());
}
 
void
Simulator::applyTorqueRobotNode(const std::string& robotName,
                                const std::string& robotNodeName,
                                const Vector3BasePtr& torque,
                                const Ice::Current&)
{
    Vector3Ptr v3p = Vector3Ptr::dynamicCast(torque);
    physicsWorld->applyTorqueRobotNode(robotName, robotNodeName, v3p->toEigen());
}
 
void
Simulator::applyForceObject(const std::string& objectName,
                            const Vector3BasePtr& force,
                            const Ice::Current&)
{
    Vector3Ptr v3p = Vector3Ptr::dynamicCast(force);
    physicsWorld->applyForceObject(objectName, v3p->toEigen());
}
 
void
Simulator::applyTorqueObject(const std::string& objectName,
                             const Vector3BasePtr& torque,
                             const Ice::Current&)
{
    Vector3Ptr v3p = Vector3Ptr::dynamicCast(torque);
    physicsWorld->applyTorqueObject(objectName, v3p->toEigen());
}
 
void
Simulator::addObject(const memoryx::ObjectClassBasePtr& objectClassBase,
                     const std::string& instanceName,
                     const PoseBasePtr& globalPose,
                     bool isStatic,
                     const Ice::Current&)
{
    ARMARX_VERBOSE << "Add object from memory " << instanceName;
    if (!requestFileManager())
    {
        ARMARX_WARNING << "Cannot add object - no connection to common storage";
        return;
    }
    auto lockEngine = physicsWorld->getScopedEngineLock(__FUNCTION__);
    auto lockData = physicsWorld->getScopedSyncLock(__FUNCTION__);
 
    try
    {
 
        ObjectClassPtr objectClass = ObjectClassPtr::dynamicCast(objectClassBase);
 
        if (physicsWorld->hasObject(instanceName))
        {
            ARMARX_ERROR << "Object with instance name \"" << instanceName << "\" already exists";
            return;
        }
 
        if (!objectClass)
        {
            ARMARX_ERROR << "Invalid object class; could not create object with instance name "
                         << instanceName;
            return;
        }
 
        memoryx::EntityWrappers::SimoxObjectWrapperPtr sw =
            objectClass->addWrapper(new EntityWrappers::SimoxObjectWrapper(fileManager));
 
        VirtualRobot::ManipulationObjectPtr mo =
            sw->getManipulationObject(); //->clone(sw->getManipulationObject()->getName()); // maybe not needed: cloning the object to avoid changing content of priorKnowledge
 
        if (!mo)
        {
            ARMARX_ERROR << "Could not retrieve manipulation object of object class "
                         << objectClass->getName();
            return;
        }
 
        //ARMARX_INFO << "Filename mo:" << mo->getVisualization()->getFilename();
        //ARMARX_INFO << "Filename sw:" << sw->getManipulationObjectFileName();
 
        mo->setName(instanceName);
 
        ARMARX_LOG << "Adding manipulation object " << mo->getName() << " of class "
                   << objectClass->getName();
        classInstanceMap[objectClass->getName()].push_back(instanceName);
 
        VirtualRobot::MathTools::Quaternion qa;
        qa.x = globalPose->orientation->qx;
        qa.y = globalPose->orientation->qy;
        qa.z = globalPose->orientation->qz;
        qa.w = globalPose->orientation->qw;
        Eigen::Matrix4f gp = VirtualRobot::MathTools::quat2eigen4f(qa);
        gp(0, 3) = globalPose->position->x;
        gp(1, 3) = globalPose->position->y;
        gp(2, 3) = globalPose->position->z;
        mo->setGlobalPose(gp);
        VirtualRobot::SceneObject::Physics::SimulationType simType =
            (isStatic) ? VirtualRobot::SceneObject::Physics::eKinematic
                       : VirtualRobot::SceneObject::Physics::eDynamic;
        physicsWorld->addObstacle(mo, simType, std::string(), objectClassBase->getName());
    }
    catch (...)
    {
        ARMARX_WARNING << "addObject failed: " << GetHandledExceptionString();
    }
}
 
void
Simulator::addObjectFromFile(const armarx::data::PackagePath& packagePath,
                             const std::string& instanceName,
                             const PoseBasePtr& globalPose,
                             bool isStatic,
                             const Ice::Current&)
{
    ARMARX_INFO << "Add object from packagePath " << packagePath << ": " << instanceName;
    //if (!requestFileManager())
    //{
    //    ARMARX_WARNING << "Cannot add object - no connection to common storage";
    //    return;
    //}
    auto lockEngine = physicsWorld->getScopedEngineLock(__FUNCTION__);
    auto lockData = physicsWorld->getScopedSyncLock(__FUNCTION__);
 
    ArmarXDataPath::FindPackageAndAddDataPath(packagePath.package);
    std::string filename;
    if (!ArmarXDataPath::SearchReadableFile(packagePath.path, filename))
    {
        ARMARX_ERROR << "Could not find file: \"" << packagePath.path << "\".";
        return;
    }
    // armarx::PackagePath boPackagePath(packagePath);
    // std::string filename = boPackagePath.toSystemPath();
 
    try
    {
        if (physicsWorld->hasObject(instanceName))
        {
            ARMARX_ERROR << "Object with instance name \"" << instanceName << "\" already exists";
            return;
        }
 
 
        VirtualRobot::ManipulationObjectPtr mo =
            VirtualRobot::ObjectIO::loadManipulationObject(filename);
 
        if (!mo)
        {
            ARMARX_ERROR << "Could not retrieve manipulation object from file " << filename;
            return;
        }
 
        //ARMARX_INFO << "Filename mo:" << mo->getVisualization()->getFilename();
        //ARMARX_INFO << "Filename sw:" << sw->getManipulationObjectFileName();
 
        mo->setName(instanceName);
 
        // ARMARX_LOG << "Adding manipulation object " << mo->getName() << " of class " << objectClass->getName();
        // classInstanceMap[objectClass->getName()].push_back(instanceName);
 
        VirtualRobot::MathTools::Quaternion qa;
        qa.x = globalPose->orientation->qx;
        qa.y = globalPose->orientation->qy;
        qa.z = globalPose->orientation->qz;
        qa.w = globalPose->orientation->qw;
        Eigen::Matrix4f gp = VirtualRobot::MathTools::quat2eigen4f(qa);
        gp(0, 3) = globalPose->position->x;
        gp(1, 3) = globalPose->position->y;
        gp(2, 3) = globalPose->position->z;
        mo->setGlobalPose(gp);
        VirtualRobot::SceneObject::Physics::SimulationType simType =
            (isStatic) ? VirtualRobot::SceneObject::Physics::eKinematic
                       : VirtualRobot::SceneObject::Physics::eDynamic;
        physicsWorld->addObstacle(mo, simType, filename, instanceName, {}, packagePath.package);
    }
    catch (...)
    {
        ARMARX_WARNING << "addObject failed: " << GetHandledExceptionString();
    }
}
 
void
Simulator::addBox(float width,
                  float height,
                  float depth,
                  float massKG,
                  const DrawColor& color,
                  const std::string& instanceName,
                  const PoseBasePtr& globalPose,
                  bool isStatic,
                  const Ice::Current&)
{
    ARMARX_VERBOSE << "Add box " << instanceName;
 
    auto lockEngine = physicsWorld->getScopedEngineLock(__FUNCTION__);
    auto lockData = physicsWorld->getScopedSyncLock(__FUNCTION__);
 
    try
    {
 
        if (physicsWorld->hasObject(instanceName))
        {
            ARMARX_ERROR << "Object with instance name \"" << instanceName << "\" already exists";
            return;
        }
 
        if (depth <= 0 || width <= 0 || height <= 0)
        {
            ARMARX_ERROR << "Invalid properties; could not create object with instance name "
                         << instanceName;
            return;
        }
 
        VirtualRobot::VisualizationFactory::Color c;
        c.r = color.r;
        c.g = color.g;
        c.b = color.b;
        c.transparency = color.a;
        VirtualRobot::ObstaclePtr o = VirtualRobot::Obstacle::createBox(width, height, depth, c);
        o->setMass(massKG);
 
 
        if (!o)
        {
            ARMARX_ERROR << "Could not create box";
            return;
        }
 
        o->setName(instanceName);
 
        ARMARX_LOG << "Adding box object " << o->getName();
 
        // not a memoryX object
        //classInstanceMap[objectClass->getName()].push_back(instanceName);
 
        VirtualRobot::MathTools::Quaternion qa;
        qa.x = globalPose->orientation->qx;
        qa.y = globalPose->orientation->qy;
        qa.z = globalPose->orientation->qz;
        qa.w = globalPose->orientation->qw;
        Eigen::Matrix4f gp = VirtualRobot::MathTools::quat2eigen4f(qa);
        gp(0, 3) = globalPose->position->x;
        gp(1, 3) = globalPose->position->y;
        gp(2, 3) = globalPose->position->z;
        o->setGlobalPose(gp);
        VirtualRobot::SceneObject::Physics::SimulationType simType =
            (isStatic) ? VirtualRobot::SceneObject::Physics::eStatic
                       : VirtualRobot::SceneObject::Physics::eDynamic;
 
        BoxVisuPrimitivePtr bp(new BoxVisuPrimitive());
        bp->type = Box;
        bp->width = width;
        bp->height = height;
        bp->depth = depth;
        bp->massKG = massKG;
        bp->color = color;
 
        physicsWorld->addObstacle(o, simType, std::string(), std::string(), bp);
    }
    catch (...)
    {
        ARMARX_WARNING << "addObject failed: " << GetHandledExceptionString();
    }
}
 
Ice::StringSeq
Simulator::getRobotNames(const Ice::Current&)
{
    auto lockEngine =
        physicsWorld->getScopedEngineLock(__FUNCTION__); // we are accessing objects and robots
    auto lockData = physicsWorld->getScopedSyncLock(__FUNCTION__);
    return physicsWorld->getRobotNames();
}
 
void
Simulator::removeObject(const std::string& instanceName, const Ice::Current&)
{
    ARMARX_VERBOSE << "Remove object " << instanceName;
 
    auto lockEngine = physicsWorld->getScopedEngineLock(__FUNCTION__);
    auto lockData = physicsWorld->getScopedSyncLock(__FUNCTION__);
 
    try
    {
        if (!physicsWorld->hasObject(instanceName))
        {
            ARMARX_ERROR << "Object with instance name \"" << instanceName << "\" does not exist";
            return;
        }
 
        ARMARX_LOG << "Removing object " << instanceName;
 
        if (!physicsWorld->removeObstacle(instanceName))
        {
            ARMARX_ERROR << "Could not remove object with instance name \"" << instanceName << "\"";
            return;
        }
    }
    catch (...)
    {
        ARMARX_WARNING << "removeObject failed...";
    }
}
 
void
Simulator::setObjectPose(const std::string& objectName,
                         const PoseBasePtr& globalPose,
                         const Ice::Current&)
{
    VirtualRobot::MathTools::Quaternion q;
    q.x = globalPose->orientation->qx;
    q.y = globalPose->orientation->qy;
    q.z = globalPose->orientation->qz;
    q.w = globalPose->orientation->qw;
    Eigen::Matrix4f gp = VirtualRobot::MathTools::quat2eigen4f(q);
    gp(0, 3) = globalPose->position->x;
    gp(1, 3) = globalPose->position->y;
    gp(2, 3) = globalPose->position->z;
 
    physicsWorld->setObjectPose(objectName, gp);
}
 
void
Simulator::setObjectSimulationType(const std::string& objectName,
                                   SimulationType type,
                                   const Ice::Current&)
{
    VirtualRobot::SceneObject::Physics::SimulationType st =
        VirtualRobot::SceneObject::Physics::eDynamic;
    if (type == armarx::Kinematic)
    {
        st = VirtualRobot::SceneObject::Physics::eKinematic;
    }
    ARMARX_DEBUG << "setting simulation type of " << objectName << " to " << st;
    physicsWorld->setObjectSimType(objectName, st);
}
 
void
Simulator::activateObject(const std::string& objectName, const Ice::Current&)
{
    physicsWorld->activateObject(objectName);
}
 
int
Simulator::getFixedTimeStepMS(const Ice::Current&)
{
    return physicsWorld->getFixedTimeStepMS();
}
 
float
Simulator::getRobotMass(const std::string& robotName, const Ice::Current&)
{
    return physicsWorld->getRobotMass(robotName);
}
 
NameValueMap
Simulator::getRobotJointAngles(const std::string& robotName, const Ice::Current&)
{
    return physicsWorld->getRobotJointAngles(robotName);
}
 
float
Simulator::getRobotJointAngle(const std::string& robotName,
                              const std::string& nodeName,
                              const Ice::Current&)
{
    return physicsWorld->getRobotJointAngle(robotName, nodeName);
}
 
NameValueMap
Simulator::getRobotJointVelocities(const std::string& robotName, const Ice::Current&)
{
    return physicsWorld->getRobotJointVelocities(robotName);
}
 
float
Simulator::getRobotJointVelocity(const std::string& robotName,
                                 const std::string& nodeName,
                                 const Ice::Current&)
{
    return physicsWorld->getRobotJointVelocity(robotName, nodeName);
}
 
NameValueMap
Simulator::getRobotJointTorques(const std::string& robotName, const Ice::Current&)
{
    return physicsWorld->getRobotJointTorques(robotName);
}
 
ForceTorqueDataSeq
Simulator::getRobotForceTorqueSensors(const std::string& robotName, const Ice::Current&)
{
    return physicsWorld->getRobotForceTorqueSensors(robotName);
}
 
float
Simulator::getRobotJointLimitLo(const std::string& robotName,
                                const std::string& nodeName,
                                const Ice::Current&)
{
    return physicsWorld->getRobotJointLimitLo(robotName, nodeName);
}
 
float
Simulator::getRobotJointLimitHi(const std::string& robotName,
                                const std::string& nodeName,
                                const Ice::Current&)
{
    return physicsWorld->getRobotJointLimitHi(robotName, nodeName);
}
 
PoseBasePtr
Simulator::getRobotPose(const std::string& robotName, const Ice::Current&)
{
    PosePtr p(new Pose(physicsWorld->getRobotPose(robotName)));
    return p;
}
 
float
Simulator::getRobotMaxTorque(const std::string& robotName,
                             const std::string& nodeName,
                             const Ice::Current&)
{
    return physicsWorld->getRobotMaxTorque(robotName, nodeName);
}
 
void
Simulator::setRobotMaxTorque(const std::string& robotName,
                             const std::string& nodeName,
                             float maxTorque,
                             const Ice::Current&)
{
    physicsWorld->setRobotMaxTorque(robotName, nodeName, maxTorque);
}
 
PoseBasePtr
Simulator::getRobotNodePose(const std::string& robotName,
                            const std::string& robotNodeName,
                            const Ice::Current&)
{
    PosePtr p(new Pose(physicsWorld->getRobotNodePose(robotName, robotNodeName)));
    return p;
}
 
Vector3BasePtr
Simulator::getRobotLinearVelocity(const std::string& robotName,
                                  const std::string& nodeName,
                                  const Ice::Current&)
{
    Vector3Ptr v(new Vector3(physicsWorld->getRobotLinearVelocity(robotName, nodeName)));
    return v;
}
 
Vector3BasePtr
Simulator::getRobotAngularVelocity(const std::string& robotName,
                                   const std::string& nodeName,
                                   const Ice::Current&)
{
    Vector3Ptr v(new Vector3(physicsWorld->getRobotAngularVelocity(robotName, nodeName)));
    return v;
}
 
void
Simulator::setRobotLinearVelocity(const std::string& robotName,
                                  const std::string& robotNodeName,
                                  const Vector3BasePtr& vel,
                                  const Ice::Current&)
{
    Eigen::Vector3f newVel = Vector3Ptr::dynamicCast(vel)->toEigen();
    physicsWorld->setRobotLinearVelocity(robotName, robotNodeName, newVel);
}
 
void
Simulator::setRobotAngularVelocity(const std::string& robotName,
                                   const std::string& robotNodeName,
                                   const Vector3BasePtr& vel,
                                   const Ice::Current&)
{
    Vector3Ptr newVel = Vector3Ptr::dynamicCast(vel);
    physicsWorld->setRobotAngularVelocity(robotName, robotNodeName, newVel->toEigen());
}
 
void
Simulator::setRobotLinearVelocityRobotRootFrame(const std::string& robotName,
                                                const std::string& robotNodeName,
                                                const Vector3BasePtr& vel,
                                                const Ice::Current&)
{
    Eigen::Vector3f newVel = Vector3Ptr::dynamicCast(vel)->toEigen();
    physicsWorld->setRobotLinearVelocityRobotRootFrame(robotName, robotNodeName, newVel);
}
 
void
Simulator::setRobotAngularVelocityRobotRootFrame(const std::string& robotName,
                                                 const std::string& robotNodeName,
                                                 const Vector3BasePtr& vel,
                                                 const Ice::Current&)
{
    Eigen::Vector3f newVel = Vector3Ptr::dynamicCast(vel)->toEigen();
    physicsWorld->setRobotAngularVelocityRobotRootFrame(robotName, robotNodeName, newVel);
}
 
bool
Simulator::hasRobot(const std::string& robotName, const Ice::Current&)
{
    return physicsWorld->hasRobot(robotName);
}
 
bool
Simulator::hasRobotNode(const std::string& robotName,
                        const std::string& robotNodeName,
                        const Ice::Current&)
{
 
    return physicsWorld->hasRobotNode(robotName, robotNodeName);
}
 
PoseBasePtr
Simulator::getObjectPose(const std::string& objectName, const Ice::Current&)
{
    PosePtr p(new Pose(physicsWorld->getObjectPose(objectName)));
    return p;
}
 
void
Simulator::objectReleased(const std::string& robotName,
                          const std::string& robotNodeName,
                          const std::string& objectName,
                          const Ice::Current&)
{
    ARMARX_VERBOSE << "Object released " << robotName << "," << objectName;
    physicsWorld->objectReleased(robotName, robotNodeName, objectName);
}
 
void
Simulator::objectGrasped(const std::string& robotName,
                         const std::string& robotNodeName,
                         const std::string& objectName,
                         const Ice::Current&)
{
    ARMARX_VERBOSE << "Object grasped" << robotName << "," << objectName;
    physicsWorld->objectGrasped(robotName, robotNodeName, objectName);
}
 
float
Simulator::getSimTime(const Ice::Current&)
{
    return static_cast<float>(physicsWorld->getCurrentSimTime());
}
 
SceneVisuData
Simulator::getScene(const Ice::Current&)
{
    SceneVisuData res = physicsWorld->copySceneVisuData();
    res.priorKnowledgeName = priorKnowledgeName;
    res.commonStorageName = commonStorageName;
    return res;
}
 
SimulatorInformation
Simulator::getSimulatorInformation(const Ice::Current&)
{
    auto lockEngine =
        physicsWorld->getScopedEngineLock(__FUNCTION__); // we are accessing objects and robots
    auto lockData = physicsWorld->getScopedSyncLock(__FUNCTION__);
    SimulatorInformation res;
    res.comTimeMS = currentComTimeMS;
    //    res.simTimeStepMeasuredMS = currentSimTimeMS; <-- this is wrong?!
    res.syncEngineTimeMS = currentSyncTimeMS;
 
    std::vector<std::string> robNames = physicsWorld->getRobotNames();
    res.nrRobots = static_cast<int>(robNames.size());
 
    res.nrActuatedJoints = physicsWorld->getRobotJointAngleCount();
    res.nrObjects = physicsWorld->getObjectCount();
    res.nrContacts = physicsWorld->getContactCount();
    res.currentSimulatorTimeSec = getSimTime();
    res.simTimeStepMeasuredMS = physicsWorld->getSimulationStepTimeMeasured();
    res.simTimeStepDurationMS = physicsWorld->getSimulationStepDuration();
 
    res.simTimeFactor = 0;
    if (res.simTimeStepMeasuredMS > 0)
    {
        res.simTimeFactor = res.simTimeStepDurationMS / res.simTimeStepMeasuredMS;
    }
 
    std::vector<VirtualRobot::SceneObjectPtr> objects = physicsWorld->getObjects();
 
    for (VirtualRobot::SceneObjectPtr& o : objects)
    {
        //ARMARX_INFO << "Object " << o->getName() << ", pose:" << o->getSceneObject()->getGlobalPose();
        ObjectVisuData ovd;
        ovd.name = o->getName();
        PosePtr p(new Pose(o->getGlobalPose()));
        ovd.objectPoses[ovd.name] = p;
        VirtualRobot::Obstacle* ob = dynamic_cast<VirtualRobot::Obstacle*>(o.get());
 
        if (ob)
        {
            ovd.filename = ob->getFilename();
        }
 
        if (o->getSimulationType() == VirtualRobot::SceneObject::Physics::eStatic)
        {
            ovd.staticObject = true;
        }
        else
        {
            ovd.staticObject = false;
        }
 
        res.objects.push_back(ovd);
    }
 
    for (auto& r : robNames)
    {
        RobotVisuData ovd;
        ovd.name = r;
        VirtualRobot::RobotPtr rob = physicsWorld->getRobot(r);
        if (rob)
        {
            PosePtr p(new Pose(rob->getGlobalPose()));
            ovd.pose = p;
            ovd.robotFile = rob->getFilename();
        }
 
        res.robots.push_back(ovd);
    }
 
    return res;
}
 
std::string
Simulator::addRobotFromFile(const armarx::data::PackagePath& packagePath, const Ice::Current& c)
{
    ArmarXDataPath::FindPackageAndAddDataPath(packagePath.package);
 
    // legacy
    {
        std::string filename;
        if (ArmarXDataPath::SearchReadableFile(packagePath.path, filename))
        {
            return addRobot(filename);
        }
    }
 
    const PackagePath pp(packagePath);
    if (std::filesystem::exists(pp.toSystemPath()))
    {
        return addRobot(pp.toSystemPath());
    }
 
    ARMARX_ERROR << "Could not find file: \"" << packagePath.path << "\".";
    return "";
}
 
std::string
Simulator::addRobot(std::string robotInstanceName,
                    const std::string& robFileGlobal,
                    Eigen::Matrix4f gp,
                    const std::string& robFile,
                    double pid_p,
                    double pid_i,
                    double pid_d,
                    bool isStatic,
                    float scaling,
                    bool colModel,
                    std::map<std::string, float> initConfig,
                    bool selfCollisions)
{
    ARMARX_DEBUG << "Add robot from file " << robFileGlobal;
    auto lockEngine = physicsWorld->getScopedEngineLock(__FUNCTION__);
    auto lockData = physicsWorld->getScopedSyncLock(__FUNCTION__);
 
    if (physicsWorld->addRobot(robotInstanceName,
                               robFileGlobal,
                               gp,
                               robFile,
                               pid_p,
                               pid_i,
                               pid_d,
                               isStatic,
                               scaling,
                               colModel,
                               initConfig,
                               selfCollisions))
    {
        // update report topics
        if (!updateRobotTopics())
        {
            ARMARX_ERROR << "topic setup failed";
        }
    }
    else
    {
        ARMARX_WARNING << "addRobot failed";
        return "";
    }
 
    return robotInstanceName;
}
 
bool
Simulator::updateRobotTopics()
{
    SimulatedWorldData& syncData = physicsWorld->getReportData();
 
    try
    {
        for (auto& rob : syncData.robots)
        {
            // offer robot
            std::string top = rob.robotTopicName;
            ARMARX_INFO << "Offering topic " << top;
            offeringTopic(top);
            //if (this->getState() == eManagedIceObjectStarted)
            {
                ARMARX_INFO << "Connecting to topic " << top;
                rob.simulatorRobotListenerPrx = getTopic<SimulatorRobotListenerInterfacePrx>(top);
            }
 
            for (auto& fti : rob.forceTorqueSensors)
            {
                ARMARX_INFO << "Offering topic " << fti.topicName;
                offeringTopic(fti.topicName);
 
                //if (this->getState() == eManagedIceObjectStarted)
                {
                    ARMARX_INFO << "Connecting to topic " << fti.topicName;
                    fti.prx = getTopic<SimulatorForceTorqueListenerInterfacePrx>(fti.topicName);
                }
            }
        }
    }
    catch (...)
    {
        ARMARX_WARNING << "failed: exception";
        return false;
    }
 
    return true;
}
 
void
Simulator::simulationLoop()
{
    step();
}
 
ObjectClassInformationSequence
Simulator::getObjectClassPoses(const std::string& robotName,
                               const std::string& frameName,
                               const std::string& className,
                               const Ice::Current&)
{
    auto lockEngine = physicsWorld->getScopedEngineLock(__FUNCTION__);
    auto lockData = physicsWorld->getScopedSyncLock(__FUNCTION__);
 
    ARMARX_DEBUG << "get object class poses";
 
    ObjectClassInformationSequence result;
    std::vector<std::string> objects = classInstanceMap[className];
 
    for (const auto& object : objects)
    {
        ObjectClassInformation inf;
        // Conversion from seconds to microseconds is 10^6 = 1.0e6 != 10e6 = 1.0e7!!!
        inf.timestampMicroSeconds =
            static_cast<Ice::Long>(physicsWorld->getCurrentSimTime() * 1.0e6);
        inf.className = className;
        inf.manipulationObjectName = object;
        Eigen::Matrix4f worldPose = physicsWorld->getObjectPose(object);
 
        if (frameName == armarx::GlobalFrame)
        {
            inf.pose = new FramedPose(worldPose, armarx::GlobalFrame, "");
        }
        else
        {
            inf.pose = physicsWorld->toFramedPose(worldPose, robotName, frameName);
        }
 
        result.push_back(inf);
    }
 
    return result;
}
 
DistanceInfo
Simulator::getDistance(const std::string& robotName,
                       const std::string& robotNodeName,
                       const std::string& worldObjectName,
                       const Ice::Current&)
{
    return physicsWorld->getDistance(robotName, robotNodeName, worldObjectName);
}
 
DistanceInfo
Simulator::getRobotNodeDistance(const std::string& robotName,
                                const std::string& robotNodeName1,
                                const std::string& robotNodeName2,
                                const Ice::Current&)
{
    return physicsWorld->getRobotNodeDistance(robotName, robotNodeName1, robotNodeName2);
}
 
void
Simulator::showContacts(bool enable, const std::string& layerName, const Ice::Current&)
{
    auto lockData = physicsWorld->getScopedSyncLock(__FUNCTION__);
    ARMARX_INFO << "showcontacts:" << enable;
    publishContacts = enable;
    physicsWorld->updateContacts(enable);
 
    if (enable)
    {
        contactLayerName = layerName;
    }
    else
    {
        lastPublishedContacts = 0;
 
        try
        {
            entityDrawerPrx->clearLayer(layerName);
            entityDrawerPrx->removeLayer(layerName);
        }
        catch (...)
        {
        }
    }
}
 
void
Simulator::reInitialize(const Ice::Current&)
{
    resetData(true);
 
    initializeData();
    simulatorResetEventTopic->simulatorWasReset();
}
 
bool
Simulator::hasObject(const std::string& instanceName, const Ice::Current&)
{
    return physicsWorld->hasObject(instanceName);
}
 
void
Simulator::start(const Ice::Current&)
{
    simulatorThreadShutdown = false;
    bool notify = !simulatorThreadRunning && this->isAlive();
    simulatorThreadRunning = true;
 
    if (notify)
    {
        condSimulatorThreadRunning.notify_all();
    }
 
    if (!isAlive())
    {
        Thread::start();
    }
}
 
void
Simulator::run()
{
    IceUtil::Time startTime;
    int sleepTimeMS;
    int loopTimeMS = physicsWorld->getFixedTimeStepMS();
 
    std::unique_lock lock(simulatorRunningMutex);
 
    while (!simulatorThreadShutdown)
    {
        while (!simulatorThreadRunning)
        {
            condSimulatorThreadRunning.wait(lock);
        }
 
        while (simulatorThreadRunning && !simulatorThreadShutdown)
        {
            startTime = IceUtil::Time::now();
            step();
            sleepTimeMS = static_cast<int>(
                loopTimeMS * (1 / timeServerSpeed) -
                static_cast<float>((IceUtil::Time::now() - startTime).toMilliSeconds()));
 
            if (sleepTimeMS < 0)
            {
                ARMARX_DEBUG << deactivateSpam(5) << "Simulation step took " << -sleepTimeMS
                             << " milliseconds too long!";
            }
            else
            {
                usleep(static_cast<unsigned int>(sleepTimeMS * 1000)); //TODO: check for EINTR?
            }
        }
    }
    ARMARX_VERBOSE << "Exiting Simulator::run()";
}
 
void
Simulator::pause(const Ice::Current&)
{
    simulatorThreadRunning = false;
}
 
void
Simulator::stop(const Ice::Current& c)
{
    pause(c);
}
 
void
Simulator::setSpeed(Ice::Float newSpeed, const Ice::Current&)
{
    timeServerSpeed = static_cast<float>(newSpeed);
}
 
Ice::Float
Simulator::getSpeed(const Ice::Current&)
{
    return Ice::Float(timeServerSpeed);
}
 
Ice::Int
Simulator::getStepTimeMS(const ::Ice::Current&)
{
    return physicsWorld->getFixedTimeStepMS();
}
 
Ice::Long
Simulator::getTime(const Ice::Current&)
{
    return static_cast<long>(physicsWorld->getCurrentSimTime() * 1000);
}
 
void
Simulator::step(const Ice::Current&)
{
    IceUtil::Time startTime = IceUtil::Time::now();
 
    if (getProperty<bool>("RealTimeMode"))
    {
        physicsWorld->stepPhysicsRealTime();
    }
    else
    {
        physicsWorld->stepPhysicsFixedTimeStep();
    }
 
    IceUtil::Time durationSim = IceUtil::Time::now() - startTime;
 
    if (durationSim.toMilliSecondsDouble() > MAX_SIM_TIME_WARNING)
    {
        ARMARX_INFO << deactivateSpam(5) << "*** Simulation slow !! Simulation time in ms:"
                    << durationSim.toMilliSecondsDouble();
    }
 
    ARMARX_DEBUG << "*** Step Physics, MS:" << durationSim.toMilliSecondsDouble();
 
    startTime = IceUtil::Time::now();
    // copy bullet content to data objects
    physicsWorld->synchronizeSimulationData();
    IceUtil::Time durationSync = IceUtil::Time::now() - startTime;
 
    {
        auto lockData = physicsWorld->getScopedSyncLock(__FUNCTION__);
        currentSimTimeMS = static_cast<float>(durationSim.toMilliSecondsDouble());
        currentSyncTimeMS = static_cast<float>(durationSync.toMilliSecondsDouble());
    }
    timeTopicPrx->reportTime(getTime());
}
 
bool
Simulator::isRunning(const Ice::Current&)
{
    return simulatorThreadRunning;
}
 
void
Simulator::reportVisuLoop()
{
    ARMARX_DEBUG << "report visu updates";
 
    try
    {
        IceUtil::Time startTime = IceUtil::Time::now();
 
        SceneVisuData reportData = physicsWorld->copySceneVisuData();
        reportData.priorKnowledgeName = priorKnowledgeName;
        reportData.commonStorageName = commonStorageName;
 
        if (simulatorVisuUpdateListenerPrx)
        {
            simulatorVisuUpdateListenerPrx->begin_reportSceneUpdated(reportData);
        }
        else
        {
            ARMARX_ERROR << deactivateSpam() << "No visu update proxy to send data (so far)";
        }
 
        {
            //lock both here!
            //otherwise we have the lock order sync->engine which causes a deadlock!
            const auto lockEngine = physicsWorld->getScopedEngineLock(__FUNCTION__);
            const auto lockSync = physicsWorld->getScopedSyncLock(__FUNCTION__);
 
            if (publishContacts && entityDrawerPrx)
            {
                //entityDrawerPrx->clearLayer(contactLayerName);
                std::vector<SimDynamics::DynamicsEngine::DynamicsContactInfo> contacts =
                    physicsWorld->copyContacts();
                VirtualRobot::ColorMap cm(VirtualRobot::ColorMap::eHot);
                float maxImpulse = 2.0f;
                float arrowLength = 50.0f;
                int i = 0;
                DrawColor dc;
 
                for (auto& c : contacts)
                {
                    std::stringstream ss;
                    ss << "Contact_" << i;
                    /*if (c.objectA)
                        ss << "_" << c.objectA->getName();
                    if (c.objectB)
                        ss << "_" << c.objectB->getName();*/
                    std::string name = ss.str();
                    std::string name2 = name + "_2";
                    std::string objA;
 
                    if (!c.objectAName.empty())
                    {
                        objA = c.objectAName; //->getName();
                    }
                    else
                    {
                        objA = "<unknown>";
                    }
 
                    std::string objB;
 
                    if (!c.objectBName.empty())
                    {
                        objB = c.objectBName; //->getName();
                    }
                    else
                    {
                        objB = "<unknown>";
                    }
 
                    ARMARX_DEBUG << "CONTACT " << name << ", objects: " << objA << "-" << objB
                                 << ", impulse:" << c.appliedImpulse;
                    float intensity = static_cast<float>(c.appliedImpulse);
 
                    if (intensity > maxImpulse)
                    {
                        intensity = maxImpulse;
                    }
 
                    intensity /= maxImpulse;
                    VirtualRobot::VisualizationFactory::Color colSimox = cm.getColor(intensity);
                    dc.r = colSimox.r;
                    dc.g = colSimox.g;
                    dc.b = colSimox.b;
                    dc.a = 1.0f;
                    //Eigen::Vector3f posEndB = c.posGlobalB + c.normalGlobalB.normalized() * arrowLength;
                    Vector3Ptr p1(new Vector3(c.posGlobalA));
                    Eigen::Vector3f dir2 = -(c.normalGlobalB);
                    Vector3Ptr d1(new Vector3(dir2));
                    Vector3Ptr p2(new Vector3(c.posGlobalB));
                    Vector3Ptr d2(new Vector3(c.normalGlobalB));
 
                    entityDrawerPrx->setArrowVisu(
                        contactLayerName, name, p1, d1, dc, arrowLength, 5.0f);
                    entityDrawerPrx->setArrowVisu(
                        contactLayerName, name2, p2, d2, dc, arrowLength, 5.0f);
                    i++;
                }
 
                // remove old contacts
                for (int j = i; j < lastPublishedContacts; j++)
                {
                    std::stringstream ss;
                    ss << "Contact_" << j;
                    std::string name = ss.str();
                    std::string name2 = name + "_2";
                    entityDrawerPrx->removeArrowVisu(contactLayerName, name);
                    entityDrawerPrx->removeArrowVisu(contactLayerName, name2);
                }
 
                lastPublishedContacts = i;
            }
 
            if (getProperty<bool>("DrawCoMVisu"))
            {
                for (auto name : physicsWorld->getRobotNames())
                {
                    auto pos = new Vector3(physicsWorld->getRobot(name)->getCoMGlobal());
                    pos->z = 0.0f;
                    entityDrawerPrx->setSphereVisu(
                        "CoM", "GlobalCoM_" + name, pos, DrawColor{0.f, 0.f, 1.f, 0.5f}, 20);
                }
            }
        }
        IceUtil::Time duration = IceUtil::Time::now() - startTime;
        reportVisuTimeMS = static_cast<float>(duration.toMilliSecondsDouble());
    }
    catch (...)
    {
        ARMARX_WARNING << "report updates failed";
        ARMARX_WARNING << "Reason" << GetHandledExceptionString();
        return;
    }
 
    ARMARX_DEBUG << "report visu updates done, time (ms):" << reportVisuTimeMS;
    if (reportVisuTimeMS > 5)
    {
        ARMARX_INFO << deactivateSpam(10)
                    << "report visu updates took long, time (ms):" << reportVisuTimeMS;
    }
}
 
void
Simulator::reportDataLoop()
{
    ARMARX_DEBUG_S << "report data updates";
 
    try
    {
        IceUtil::Time startTime = IceUtil::Time::now();
 
        SimulatedWorldData reportData = physicsWorld->copyReportData();
 
        for (RobotInfo& r : reportData.robots)
        {
            SimulatedRobotState state = stateFromRobotInfo(r, reportData.timestamp);
 
            if (reportRobotPose)
            {
                Ice::Context ctx;
                ctx["origin"] = "simulation";
 
                FrameHeader header;
                header.parentFrame = GlobalFrame;
                header.frame = physicsWorld->getRobot(r.robotName)->getRootNode()->getName();
                header.agent = r.robotName;
                header.timestampInMicroSeconds = reportData.timestamp.toMicroSeconds();
 
                TransformStamped globalRobotPose;
                globalRobotPose.header = header;
                globalRobotPose.transform = r.pose;
 
                if (globalRobotLocalization)
                {
                    globalRobotLocalization->reportGlobalRobotPose(globalRobotPose, ctx);
                }
            }
 
            if (r.simulatorRobotListenerPrx)
            {
                r.simulatorRobotListenerPrx->reportState(state);
            }
        }
 
        // report world objects
        // todo...
 
        IceUtil::Time duration = IceUtil::Time::now() - startTime;
        currentComTimeMS = static_cast<float>(duration.toMilliSecondsDouble());
    }
    catch (...)
    {
        ARMARX_WARNING_S << "report updates failed";
        ARMARX_WARNING << "Reason" << GetHandledExceptionString();
        return;
    }
 
    ARMARX_DEBUG_S << "report data updates done, time (ms):" << currentComTimeMS;
}
 
ContactInfoSequence
Simulator::getContacts(const Ice::Current&)
{
    ContactInfoSequence result;
    auto contacts = this->physicsWorld->copyContacts();
    for (auto& contact : contacts)
    {
        ContactInformation contactInfo;
        contactInfo.objectNameA = contact.objectAName;
        contactInfo.objectNameB = contact.objectBName;
        contactInfo.posGlobalA = new Vector3(contact.posGlobalA);
        contactInfo.posGlobalB = new Vector3(contact.posGlobalB);
        result.push_back(contactInfo);
    }
    return result;
}
 
void
Simulator::updateContacts(bool enable, const Ice::Current&)
{
    this->physicsWorld->updateContacts(enable);
}
 
std::string
Simulator::addScaledRobot(const std::string& filename, float scaling, const Ice::Current&)
{
    ARMARX_TRACE;
    std::string instanceName;
    this->physicsWorld->addRobot(
        instanceName, filename, Eigen::Matrix4f::Identity(), "", 10.0, 0, 0, false, scaling);
    return instanceName;
}
 
std::string
Simulator::addScaledRobotName(const std::string& instanceName,
                              const std::string& filename,
                              float scaling,
                              const Ice::Current&)
{
    ARMARX_TRACE;
    std::string name = instanceName;
    this->physicsWorld->addRobot(
        name, filename, Eigen::Matrix4f::Identity(), "", 10.0, 0, 0, false, scaling);
    return name;
}
 
bool
Simulator::removeRobot(const std::string& robotName, const Ice::Current&)
{
    return this->physicsWorld->removeRobot(robotName);
}
 
std::string
Simulator::addRobot(const std::string& filename, const Ice::Current&)
{
    ARMARX_TRACE;
    std::string instanceName;
    this->physicsWorld->addRobot(
        instanceName, filename, Eigen::Matrix4f::Identity(), "", 10.0, 0, 0, false, 1.0f);
    return instanceName;
}
 
SimulatedRobotState
armarx::Simulator::getRobotState(const std::string& robotName, const Ice::Current&)
{
    SimulatedWorldData report = physicsWorld->getReportData();
    for (auto& robot : report.robots)
    {
        if (robot.robotName == robotName)
        {
            return stateFromRobotInfo(robot, report.timestamp);
        }
    }
 
    return SimulatedRobotState();
}
 
ARMARX_REGISTER_COMPONENT_EXECUTABLE(::armarx::Simulator, ::armarx::Simulator::GetDefaultName());