SimulatedWorld.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       2014
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
// print who is querying for locks (scoped lock queries are counted)
//#define PERFORMANCE_EVALUATION_LOCKS
 
#define MAX_SIM_TIME_WARNING 25
#define FORCE_TORQUE_TOPIC_NAME "ForceTorqueDynamicSimulationValues"
 
#include "SimulatedWorld.h"
 
#include <algorithm>
#include <memory>
 
#include <VirtualRobot/CollisionDetection/CollisionChecker.h>
#include <VirtualRobot/Nodes/ForceTorqueSensor.h>
#include <VirtualRobot/XML/ObjectIO.h>
#include <VirtualRobot/XML/RobotIO.h>
#include <VirtualRobot/XML/SceneIO.h>
 
#include <ArmarXCore/core/exceptions/LocalException.h>
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/system/ArmarXDataPath.h>
#include <ArmarXCore/core/time/TimeUtil.h>
 
using namespace VirtualRobot;
using namespace armarx;
 
//#define PRINT_TEST_DEBUG_MESSAGES
 
SimulatedWorld::SimulatedWorld()
{
    engineMutex.reset(new MutexType);
    synchronizeMutex.reset(new MutexType);
    simStepExecutionDurationMS = 0.0f;
    synchronizeDurationMS = 0.0f;
    simTimeStepMS = 0.0f;
    currentSimTimeSec = 0.0;
    collectContacts = false;
}
 
void
SimulatedWorld::activateObject(const std::string& objectName)
{
    (void)objectName; // unused
}
 
bool
SimulatedWorld::resetData()
{
    ARMARX_DEBUG_S << "Removing all robots & objects";
    const bool resetRobots = removeRobots();
    const bool resetObstacles = removeObstacles();
 
    return resetRobots && resetObstacles;
}
 
double
SimulatedWorld::getCurrentSimTime()
{
    // Return time of last update of robot/object data instead of last step.
    // => The caller can rely on robot/object data being synced to this time.
 
    // return currentSimTimeSec;
    return currentSyncTimeSec;
}
 
void
SimulatedWorld::resetSimTime()
{
    currentSimTimeSec = 0;
}
 
bool
SimulatedWorld::removeRobots()
{
    ARMARX_DEBUG_S << "Removing all robots";
 
    bool res = true;
 
    std::vector<std::string> names = getRobotNames();
    for (auto n : names)
    {
        res &= removeRobot(n);
    }
 
    return res;
}
 
SimulatedWorldData&
SimulatedWorld::getReportData()
{
    return simReportData;
}
 
SimulatedWorldData
SimulatedWorld::copyReportData()
{
    ScopedRecursiveLockPtr lockSync = getScopedSyncLock(__FUNCTION__);
    return simReportData;
}
 
bool
SimulatedWorld::addRobot(RobotPtr robot,
                         double pid_p,
                         double pid_i,
                         double pid_d,
                         const std::string& filename,
                         bool staticRobot,
                         float scaling,
                         bool colModel,
                         bool selfCollisions)
{
    ARMARX_VERBOSE_S << "Adding robot " << robot->getName() << ", file:" << filename;
    assert(robot);
 
    // lock engine and data
    ScopedRecursiveLockPtr lockEngine = getScopedEngineLock(__FUNCTION__);
    ScopedRecursiveLockPtr lockSync = getScopedSyncLock(__FUNCTION__);
 
    if (scaling != 1.0f)
    {
        ARMARX_INFO_S << "Scaling robot, factor: " << scaling;
        robot = robot->clone(robot->getName(), robot->getCollisionChecker(), scaling);
    }
 
 
    bool ok = addRobotEngine(robot, pid_p, pid_i, pid_d, filename, staticRobot, selfCollisions);
    if (!ok)
    {
        return false;
    }
 
    RobotVisuData robData;
 
    robData.name = robot->getName();
    robData.robotFile = filename;
    robData.updated = true;
    robData.scaling = scaling;
    robData.colModel = colModel;
 
    simVisuData.robots.push_back(robData);
 
    RobotInfo robInfo;
    robInfo.robotName = robot->getName();
 
    try
    {
        // setup robot report data
        robInfo.robotTopicName = "Simulator_Robot_";
        robInfo.robotTopicName += robot->getName();
 
        std::vector<SensorPtr> sensors = robot->getSensors();
 
        for (const auto& sensor : sensors)
        {
            ForceTorqueSensorPtr ft = std::dynamic_pointer_cast<ForceTorqueSensor>(sensor);
 
            if (ft)
            {
                ForceTorqueInfo fti;
                fti.enable = true;
                fti.robotName = robot->getName();
                fti.sensorName = ft->getName();
                fti.ftSensor = ft;
                fti.robotNodeName = ft->getRobotNode()->getName();
                fti.topicName = FORCE_TORQUE_TOPIC_NAME;
 
                robInfo.forceTorqueSensors.push_back(fti);
            }
        }
    }
    catch (...)
    {
        ARMARX_WARNING_S << "addRobot failed";
    }
 
    simReportData.robots.push_back(robInfo);
 
    synchronizeRobots();
    return true;
}
 
void
SimulatedWorld::objectReleased(const std::string& robotName,
                               const std::string& robotNodeName,
                               const std::string& objectName)
{
    ScopedRecursiveLockPtr lockEngine = getScopedEngineLock(__FUNCTION__);
    ARMARX_INFO_S << "Object released, robot " << robotName << ", node:" << robotNodeName
                  << ", object:" << objectName;
 
    if (objectReleasedEngine(robotName, robotNodeName, objectName))
    {
        // check if object is in  grasped list
        auto aoIt = attachedObjects.begin();
        bool objectFound = false;
 
        while (aoIt != attachedObjects.end())
        {
            if (aoIt->robotName == robotName && aoIt->robotNodeName == robotNodeName &&
                aoIt->objectName == objectName)
            {
                attachedObjects.erase(aoIt);
                objectFound = true;
                break;
            }
 
            aoIt++;
        }
 
        if (!objectFound)
        {
            ARMARX_WARNING_S << "Object was not attached to robot: " << objectName;
        }
 
        ARMARX_INFO_S << "Successfully detached object " << objectName;
    }
}
 
void
SimulatedWorld::objectGrasped(const std::string& robotName,
                              const std::string& robotNodeName,
                              const std::string& objectName)
{
    ScopedRecursiveLockPtr lockEngine = getScopedEngineLock(__FUNCTION__);
    ARMARX_INFO_S << "Object grasped, robot " << robotName << ", node:" << robotNodeName
                  << ", object:" << objectName;
 
    // check if object is already grasped
    for (const auto& ao : attachedObjects)
    {
        if (ao.robotName == robotName && ao.robotNodeName == robotNodeName &&
            ao.objectName == objectName)
        {
            ARMARX_INFO_S << "Object already attached, skipping...";
            return;
        }
    }
 
    Eigen::Matrix4f localTransform;
    if (objectGraspedEngine(robotName, robotNodeName, objectName, localTransform))
    {
        ARMARX_INFO_S << "Successfully attached object " << objectName;
        // store grasping information
        GraspingInfo g;
        g.robotName = robotName;
        g.robotNodeName = robotNodeName;
        g.objectName = objectName;
 
        g.node2objectTransformation =
            localTransform; //rn->toLocalCoordinateSystem(o->getSceneObject()->getGlobalPose());
 
        attachedObjects.push_back(g);
    }
}
 
bool
SimulatedWorld::removeObstacles()
{
    ARMARX_VERBOSE_S << "Removing all obstacles";
    bool result = true;
    std::vector<std::string> names = getObstacleNames();
 
    for (auto& n : names)
    {
        if (!removeObstacle(n))
        {
            ARMARX_WARNING_S << "Could not remove object with name " << n;
            result = false;
        }
    }
 
    return result;
}
 
bool
SimulatedWorld::removeObstacle(const std::string& name)
{
    ARMARX_VERBOSE_S << "Removing obstacle " << name;
    ScopedRecursiveLockPtr lockEngine = getScopedEngineLock(__FUNCTION__);
    ScopedRecursiveLockPtr lockData = getScopedSyncLock(__FUNCTION__);
 
    if (!removeObstacleEngine(name))
    {
        return false;
    }
 
    {
        // access data
        bool objectFound = false;
 
        for (auto s = simVisuData.objects.begin(); s != simVisuData.objects.end(); s++)
        {
            if (s->name == name)
            {
                objectFound = true;
                simVisuData.objects.erase(s);
                break;
            }
        }
 
        if (!objectFound)
        {
            ARMARX_WARNING_S << "Object with name " << name
                             << " not in synchronized list, cannot remove";
        }
    }
 
    return true;
}
 
bool
SimulatedWorld::addRobot(std::string& robotInstanceName,
                         const std::string& filename,
                         Eigen::Matrix4f pose,
                         const std::string& filenameLocal,
                         double pid_p,
                         double pid_i,
                         double pid_d,
                         bool staticRobot,
                         float scaling,
                         bool colModel,
                         const std::map<std::string, float>& initConfig,
                         bool selfCollisions)
{
    ARMARX_TRACE;
    ScopedRecursiveLockPtr lockEngine = getScopedEngineLock(__FUNCTION__);
    ARMARX_INFO_S << "Loading Robot from " << filename;
    RobotPtr robot;
 
    try
    {
        ARMARX_TRACE;
        VirtualRobot::RobotIO::RobotDescription loadMode = VirtualRobot::RobotIO::eFull;
        if (colModel)
        {
            loadMode = VirtualRobot::RobotIO::eCollisionModel;
        }
 
        robot = RobotIO::loadRobot(filename, loadMode);
        robot = adaptRobotToWorld(robot);
 
        ARMARX_TRACE;
        // ensure that the robot has the standard name
        if (robotInstanceName.empty())
        {
            robotInstanceName = robot->getType();
        }
        const std::string baseName = robotInstanceName;
        for (std::size_t i = 2; hasRobot(robotInstanceName); ++i)
        {
            robotInstanceName = baseName + "_" + std::to_string(i);
        }
        robot->setName(robotInstanceName);
    }
    catch (VirtualRobotException& e)
    {
        ARMARX_ERROR_S << GetHandledExceptionString()
                       << " ERROR while creating robot (file:" << filename << ")\n"
                       << e.what();
        return false;
    }
 
    if (!robot)
    {
        ARMARX_ERROR_S << " ERROR while creating robot (file:" << filename << ")";
        return false;
    }
 
    ARMARX_INFO_S << "Loaded robot with name " << robot->getName();
 
    /*if (initConfig.size() > 0)
    {
        robot->setJointValues(initConfig);
    }*/
 
    ARMARX_TRACE;
    if (pose.isIdentity() && getFloor())
    {
        ARMARX_TRACE;
        VirtualRobot::BoundingBox bbox = robot->getBoundingBox();
        //pose(2,3) = -bbox.getMin()(2) + 1.0f;
 
        // move down robot until collision
        std::vector<CollisionModelPtr> colModels = robot->getCollisionModels();
        CollisionModelPtr floorCol = getFloor()->getCollisionModel();
 
        bool found = false;
        std::size_t loop = 0;
        float z = -bbox.getMin()(2) + 20.0f;
        pose(2, 3) = z;
        robot->setGlobalPose(pose);
        found = (robot->getCollisionChecker()->checkCollision(colModels, floorCol));
 
        while (loop < 1000 && !found)
        {
            z -= 2.0f;
            pose(2, 3) = z;
            robot->setGlobalPose(pose);
            found = (robot->getCollisionChecker()->checkCollision(colModels, floorCol));
            loop++;
        }
 
        if (found)
        {
            // move up
            z += 2.0f;
            pose(2, 3) = z;
            robot->setGlobalPose(pose);
        }
    }
    ARMARX_TRACE;
 
    robot->setGlobalPose(pose);
    std::string fn = filename;
 
    if (!filenameLocal.empty())
    {
        fn = filenameLocal;
    }
 
    ARMARX_TRACE;
    bool success =
        addRobot(robot, pid_p, pid_i, pid_d, fn, staticRobot, scaling, colModel, selfCollisions);
 
    ARMARX_TRACE;
    if (success && initConfig.size() > 0)
    {
        ARMARX_TRACE;
        actuateRobotJointsPos(robot->getName(), initConfig);
    }
    return success;
}
 
VirtualRobot::RobotPtr
SimulatedWorld::adaptRobotToWorld(VirtualRobot::RobotPtr robot)
{
    return robot;
}
 
bool
SimulatedWorld::removeRobot(const std::string& robotName)
{
    ARMARX_DEBUG_S << "Removing robot " << robotName;
 
    // lock engine and data
    ScopedRecursiveLockPtr lockEngine = getScopedEngineLock(__FUNCTION__);
    ScopedRecursiveLockPtr lockSync = getScopedSyncLock(__FUNCTION__);
 
    bool ok = removeRobotEngine(robotName);
    if (!ok)
    {
        return false;
    }
 
    // access data
    {
        //        if (simVisuData.robots.size() != 1 || simVisuData.robots.at(0).name != robotName)
        //        {g
        //            ARMARX_WARNING_S << "Internal data structure wrong...";
        //        }
 
        ::armarx::RobotVisuList robots;
        for (auto robot : simVisuData.robots)
        {
            if (robot.name != robotName)
            {
                robots.push_back(robot);
            }
        }
 
        std::vector<RobotInfo> robotInfos;
        for (auto robot : simReportData.robots)
        {
            if (robot.robotName != robotName)
            {
                robotInfos.push_back(robot);
            }
        }
 
        simVisuData.robots = robots;
        simReportData.robots = robotInfos;
 
        //simVisuData.robots.clear();
        //simReportData.robots.clear();
    }
 
    // check if there are grasped objects
    auto aoIt = attachedObjects.begin();
 
    while (aoIt != attachedObjects.end())
    {
        if (aoIt->robotName == robotName)
        {
            ARMARX_DEBUG_S << "Removing attached object " << aoIt->objectName;
            aoIt = attachedObjects.erase(aoIt);
        }
        else
        {
            aoIt++;
        }
    }
 
    return true;
}
 
bool
SimulatedWorld::addScene(const std::string& filename,
                         VirtualRobot::SceneObject::Physics::SimulationType simType)
{
    ARMARX_INFO_S << "Loading Scene from " << filename;
    ScenePtr scene;
 
    try
    {
        scene = SceneIO::loadScene(filename);
    }
    catch (VirtualRobotException& e)
    {
        ARMARX_ERROR_S << " ERROR while creating scene (file:" << filename << ")\n" << e.what();
        return false;
    }
 
    if (!scene)
    {
        ARMARX_ERROR_S << " ERROR while creating scene (file:" << filename << ")";
        return false;
    }
 
    return addScene(scene, simType);
}
 
bool
SimulatedWorld::addScene(ScenePtr scene, VirtualRobot::SceneObject::Physics::SimulationType simType)
{
    if (!scene)
    {
        ARMARX_ERROR_S << "No Scene!";
        return false;
    }
 
    int count = 0;
    std::vector<ManipulationObjectPtr> mo = scene->getManipulationObjects();
 
    for (auto& i : mo)
    {
        ARMARX_VERBOSE_S << "Scene: ManipulationObject " << i->getName();
        addObstacle(i, simType, i->getFilename());
        count++;
    }
 
    std::vector<ObstaclePtr> o = scene->getObstacles();
 
    for (auto& i : o)
    {
        ARMARX_VERBOSE_S << "Scene: Obstacle " << i->getName();
        addObstacle(i, simType, i->getFilename());
        count++;
    }
 
    int countrobot = 0;
    std::vector<RobotPtr> ro = scene->getRobots();
 
    for (auto& i : ro)
    {
        // ensure consistent name
        i->setName(i->getType());
        ARMARX_VERBOSE_S << "Scene: Robot " << i->getName() << ", type:" << i->getType();
        addRobot(i, 10.0, 0.0, 0.0, i->getFilename());
        countrobot++;
    }
 
    ARMARX_INFO_S << " Added " << countrobot << " robots and " << count << " objects";
 
    return true;
}
 
bool
SimulatedWorld::addObstacle(const std::string& filename,
                            const Eigen::Matrix4f& pose,
                            VirtualRobot::SceneObject::Physics::SimulationType simType,
                            const std::string& localFilename)
{
    ARMARX_INFO_S << "Loading obstacle from " << filename << ", (local file:" << localFilename
                  << ")";
    ObstaclePtr o;
 
    try
    {
        o = ObjectIO::loadObstacle(filename);
    }
    catch (VirtualRobotException& e)
    {
        ARMARX_ERROR_S << " ERROR while creating obstacle (file:" << filename << ")\n" << e.what();
        return false;
    }
 
    if (!o)
    {
        ARMARX_ERROR_S << " ERROR while creating (file:" << filename << ")";
        return false;
    }
 
    o->setGlobalPose(pose);
    std::string fn = filename;
 
    if (!localFilename.empty())
    {
        std::string absoluteFilename;
        // check if we would find the local file
        bool fileFound = ArmarXDataPath::getAbsolutePath(localFilename, absoluteFilename);
 
        if (fileFound)
        {
            ARMARX_IMPORTANT_S << "found local path:" << localFilename << " -> "
                               << absoluteFilename;
            fn = localFilename;
        }
        else
        {
            ARMARX_IMPORTANT_S << "could not find global path from local path:" << localFilename
                               << " -> usign " << fn;
        }
    }
 
    return addObstacle(o, simType, fn);
}
 
bool
SimulatedWorld::addObstacle(VirtualRobot::SceneObjectPtr o,
                            VirtualRobot::SceneObject::Physics::SimulationType simType,
                            const std::string& filename,
                            const std::string& objectClassName,
                            ObjectVisuPrimitivePtr primitiveData,
                            const std::string& project)
{
    if (!o)
    {
        return false;
    }
 
    ARMARX_VERBOSE_S << "Adding obstacle " << o->getName();
 
    // lock engine and data
    ScopedRecursiveLockPtr lockEngine = getScopedEngineLock(__FUNCTION__);
    ScopedRecursiveLockPtr lockData = getScopedSyncLock(__FUNCTION__);
 
    //VirtualRobot::SceneObjectPtr clonedObj;
    if (hasObject(o->getName()))
    {
        ARMARX_ERROR_S << "Object with name " << o->getName()
                       << " already present! Choose a different instance name";
        return false;
    }
 
    bool ok = addObstacleEngine(o, simType);
    if (!ok)
    {
        return false;
    }
 
    {
        // access data
 
        if (primitiveData && (!filename.empty() || !objectClassName.empty()))
        {
            ARMARX_WARNING_S << "Internal error, either filename or classname or primitive";
        }
 
 
        armarx::ObjectVisuData ovd;
        ovd.name = o->getName();
        ovd.filename = filename;
        ovd.project = project;
        ovd.objectPoses[o->getName()] = new Pose(o->getGlobalPose());
        ovd.objectClassName = objectClassName;
 
 
        if (primitiveData)
        {
            ovd.objectPrimitiveData = primitiveData;
        }
 
        ovd.updated = true;
        simVisuData.objects.push_back(ovd);
    }
 
    return true;
}
 
auto
SimulatedWorld::getScopedEngineLock(const std::string& callStr) -> ScopedRecursiveLockPtr
{
    ScopedRecursiveLockPtr l(new ScopedRecursiveLock(*engineMutex));
#ifdef PERFORMANCE_EVALUATION_LOCKS
    if (engineMtxAccTime.find(callStr) == engineMtxAccTime.end())
    {
        engineMtxAccCalls[callStr] = 1;
        engineMtxAccTime[callStr] = 0;
        engineMtxLastTime[callStr] = IceUtil::Time::now();
    }
    else
    {
        IceUtil::Time delta = IceUtil::Time::now() - engineMtxLastTime[callStr];
        engineMtxAccCalls[callStr] += 1;
        engineMtxAccTime[callStr] += delta.toMilliSecondsDouble();
        engineMtxLastTime[callStr] = IceUtil::Time::now();
        if (engineMtxAccTime[callStr] > 1000)
        {
            ARMARX_INFO << "engine mutex performance [" << callStr
                        << "]: Nr Calls:" << engineMtxAccCalls[callStr] << ", Calls/Sec:"
                        << (float)engineMtxAccCalls[callStr] / engineMtxAccTime[callStr] * 1000.0f;
            engineMtxAccCalls[callStr] = 1;
            engineMtxAccTime[callStr] = 0;
            engineMtxLastTime[callStr] = IceUtil::Time::now();
        }
    }
#else
    (void)callStr; // unused
#endif
    return l;
}
 
auto
SimulatedWorld::getScopedSyncLock(const std::string& callStr) -> ScopedRecursiveLockPtr
{
    ScopedRecursiveLockPtr l(new ScopedRecursiveLock(*synchronizeMutex));
#ifdef PERFORMANCE_EVALUATION_LOCKS
    if (syncMtxAccTime.find(callStr) == syncMtxAccTime.end())
    {
        syncMtxAccCalls[callStr] = 1;
        syncMtxAccTime[callStr] = 0;
        syncMtxLastTime[callStr] = IceUtil::Time::now();
    }
    else
    {
        IceUtil::Time delta = IceUtil::Time::now() - syncMtxLastTime[callStr];
        syncMtxAccCalls[callStr] += 1;
        syncMtxAccTime[callStr] += delta.toMilliSecondsDouble();
        syncMtxLastTime[callStr] = IceUtil::Time::now();
        if (syncMtxAccTime[callStr] > 1000)
        {
            ARMARX_INFO << "snyc mutex performance [" << callStr
                        << "]: Nr Calls:" << syncMtxAccCalls[callStr] << ", Calls/Sec:"
                        << (float)syncMtxAccCalls[callStr] / syncMtxAccTime[callStr] * 1000.0f;
            syncMtxAccCalls[callStr] = 1;
            syncMtxAccTime[callStr] = 0;
            syncMtxLastTime[callStr] = IceUtil::Time::now();
        }
    }
#else
    (void)callStr; // unused
#endif
    return l;
}
 
void
SimulatedWorld::setupFloor(bool enable, const std::string& floorTexture)
{
    setupFloorEngine(enable, floorTexture);
 
    {
        ScopedRecursiveLockPtr lockSync = getScopedSyncLock(__FUNCTION__);
        simVisuData.floor = enable;
        simVisuData.floorTextureFile = floorTexture;
    }
}
 
float
SimulatedWorld::getSimulationStepDuration()
{
    return simTimeStepMS;
}
 
float
SimulatedWorld::getSimulationStepTimeMeasured()
{
    return simStepExecutionDurationMS;
}
 
void
SimulatedWorld::enableLogging(const std::string& robotName, const std::string& logFile)
{
    (void)robotName, (void)logFile; // unused
}
 
float
SimulatedWorld::getSimTime()
{
    return simStepExecutionDurationMS;
}
 
float
SimulatedWorld::getSyncEngineTime()
{
    return synchronizeDurationMS;
}
 
bool
SimulatedWorld::synchronizeRobots()
{
    // lock engine and data
    ScopedRecursiveLockPtr lockEngine = getScopedEngineLock(__FUNCTION__);
    ScopedRecursiveLockPtr lockSync = getScopedSyncLock(__FUNCTION__);
 
    // update visu data
    if (simVisuData.robots.size() == 0)
    {
        return false;
    }
    std::map<std::string, RobotVisuData*> visuRobotPtr;
    for (auto& robot : simVisuData.robots)
    {
        std::string name = robot.name;
 
        if (!synchronizeRobotNodePoses(name, robot.robotNodePoses))
        {
            ARMARX_ERROR_S << "Failed to synchronize robot...";
            return false;
        }
 
        armarx::PosePtr p(new Pose(getRobotPose(name)));
        robot.pose = p;
        robot.updated = true;
        visuRobotPtr[robot.name] = &robot;
    }
 
    // update data for reporting
 
    for (auto& robot : simReportData.robots)
    {
        std::string name = robot.robotName;
 
        robot.pose = getRobotPose(name);
 
        getRobotStatus(name,
                       robot.jointAngles,
                       robot.jointVelocities,
                       robot.jointTorques,
                       robot.linearVelocity,
                       robot.angularVelocity);
        if (visuRobotPtr.count(robot.robotName))
        {
            visuRobotPtr.at(robot.robotName)->jointValues = robot.jointAngles;
        }
 
        // update force torque info
        for (size_t i = 0; i < robot.forceTorqueSensors.size(); i++)
        {
            updateForceTorqueSensor(robot.forceTorqueSensors[i]);
        }
    }
 
    return true;
}
 
bool
SimulatedWorld::synchronizeSimulationData()
{
    IceUtil::Time startTime = IceUtil::Time::now();
 
    // lock engine and data
    ScopedRecursiveLockPtr lockEngine = getScopedEngineLock(__FUNCTION__);
    ScopedRecursiveLockPtr lockSync = getScopedSyncLock(__FUNCTION__);
 
    IceUtil::Time durationlock = IceUtil::Time::now() - startTime;
 
    if (durationlock.toMilliSecondsDouble() > 4)
    {
        ARMARX_INFO << deactivateSpam(10)
                    << "Engine/Sync locking took long :" << durationlock.toMilliSecondsDouble()
                    << " ms";
    }
 
    simVisuData.timestamp = static_cast<Ice::Long>(getCurrentSimTime() * 1000);
    simReportData.timestamp = IceUtil::Time::secondsDouble(getCurrentSimTime());
 
    IceUtil::Time startTimeR = IceUtil::Time::now();
    // ROBOT -> update visu & report data
    synchronizeRobots();
    IceUtil::Time durationR = IceUtil::Time::now() - startTimeR;
    if (durationR.toMilliSecondsDouble() > 4)
    {
        ARMARX_INFO << deactivateSpam(10)
                    << "Robot sync took long:" << durationR.toMilliSecondsDouble() << " ms";
    }
 
    IceUtil::Time startTimeO = IceUtil::Time::now();
    // OBJECTS -> visu & report data update
    synchronizeObjects();
    IceUtil::Time durationO = IceUtil::Time::now() - startTimeO;
    if (durationO.toMilliSecondsDouble() > 4)
    {
        ARMARX_INFO << deactivateSpam(10)
                    << "Object sync took long:" << durationO.toMilliSecondsDouble() << " ms";
    }
 
    // CONTACTS
    synchronizeSimulationDataEngine();
 
    IceUtil::Time duration = IceUtil::Time::now() - startTime;
    synchronizeDurationMS = static_cast<float>(duration.toMilliSecondsDouble());
 
    currentSyncTimeSec = currentSimTimeSec;
 
    return true;
}
 
void
SimulatedWorld::updateContacts(bool enable)
{
    collectContacts = enable;
}
 
SceneVisuData
SimulatedWorld::copySceneVisuData()
{
    ScopedRecursiveLockPtr lock = getScopedSyncLock(__FUNCTION__);
    return simVisuData;
}
 
int
SimulatedWorld::getRobotJointAngleCount()
{
    ScopedRecursiveLockPtr l = getScopedSyncLock(__FUNCTION__);
 
    if (simVisuData.robots.size() == 0)
    {
        return 0;
    }
 
    return static_cast<int>(simVisuData.robots.at(0).robotNodePoses.size());
}
 
int
SimulatedWorld::getContactCount()
{
    ScopedRecursiveLockPtr l = getScopedSyncLock(__FUNCTION__);
    return 0;
}
 
int
SimulatedWorld::getObjectCount()
{
    ScopedRecursiveLockPtr l = getScopedSyncLock(__FUNCTION__);
    return static_cast<int>(simVisuData.objects.size());
}