KinematicsWorld.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       2017
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "KinematicsWorld.h"
 
#include <VirtualRobot/Visualization/VisualizationFactory.h>
#include <VirtualRobot/CollisionDetection/CollisionChecker.h>
#include <VirtualRobot/Nodes/ForceTorqueSensor.h>
#include <VirtualRobot/RobotFactory.h>
#include <VirtualRobot/RobotNodeSet.h>
#include <VirtualRobot/IK/DifferentialIK.h>
#include <VirtualRobot/IK/IKSolver.h>
#include <VirtualRobot/Obstacle.h>
 
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/system/ArmarXDataPath.h>
#include <algorithm>
#include <ArmarXCore/core/exceptions/LocalException.h>
 
using namespace VirtualRobot;
using namespace armarx;
 
 
KinematicsWorld::KinematicsWorld() : SimulatedWorld()
{
    synchronizeDurationMS = 0.0f;
    currentSimTimeSec = 0.0f;
 
    floorPos.setZero();
    floorUp.setZero();
    floorDepthMM = 500.0f;
    floorExtendMM = 50000.0f;
}
 
KinematicsWorld::~KinematicsWorld()
    = default;
 
void KinematicsWorld::initialize(int stepSizeMS, float maxRealTimeSimSpeed, bool floorPlane, std::string floorTexture)
{
    ARMARX_INFO_S << "Init KinematicsWorld...";
 
    simVisuData.robots.clear();
    simVisuData.objects.clear();
    simVisuData.floorTextureFile.clear();
    simVisuData.floor = false;
 
    simReportData.robots.clear();
 
    this->maxRealTimeSimSpeed = maxRealTimeSimSpeed;
    this->stepSizeMs = stepSizeMS;
 
    setupFloor(floorPlane, floorTexture);
}
 
 
void KinematicsWorld::createFloorPlane(const Eigen::Vector3f& pos, const Eigen::Vector3f& up)
{
    ScopedRecursiveLockPtr lockEngine = getScopedEngineLock(__FUNCTION__);
    floorPos = pos;
    floorUp = up;
    float size = float(floorExtendMM);
    float sizeSmall = float(floorDepthMM);
    float w = size;
    float h = size;
    float d = sizeSmall;
 
    if (up(1) == 0 && up(2) == 0)
    {
        w = sizeSmall;
        h = size;
        d = size;
    }
    else if (up(0) == 0 && up(2) == 0)
    {
        w = size;
        h = sizeSmall;
        d = size;
    }
 
    groundObject = VirtualRobot::Obstacle::createBox(w, h, d, VirtualRobot::VisualizationFactory::Color::Gray());
    std::string name("Floor");
    groundObject->setName(name);
    Eigen::Matrix4f gp;
    gp.setIdentity();
    gp(2, 3) = -sizeSmall * 0.5f;
    groundObject->setGlobalPose(gp);
 
    groundObject->getVisualization();
    groundObject->setSimulationType(VirtualRobot::SceneObject::Physics::eStatic);
 
    //addObstacleEngine(groundObject, VirtualRobot::SceneObject::Physics::eStatic);
}
 
void KinematicsWorld::actuateRobotJoints(const std::string& robotName, const std::map< std::string, float>& angles, const std::map< std::string, float>& velocities)
{
    ScopedRecursiveLockPtr lockEngine = getScopedEngineLock(__FUNCTION__);
 
    ARMARX_DEBUG_S << "actuateRobotJoints: first:" << angles.begin()->first << ", ang: " << angles.begin()->second << ", vel:" << velocities.begin()->second;
    VirtualRobot::RobotPtr kinRobot = getRobot(robotName);
    if (!kinRobot)
    {
        ARMARX_WARNING_S << "No robot available";
        return;
    }
    kinRobot->setJointValues(angles);
 
    // Set velocities to zero
    KinematicRobotInfo& ri = kinematicRobots[robotName];
    for (auto& entry : angles)
    {
        ri.targetVelocities[entry.first] = 0;
    }
}
 
void KinematicsWorld::actuateRobotJointsPos(const std::string& robotName, const std::map< std::string, float>& angles)
{
    ScopedRecursiveLockPtr lockEngine = getScopedEngineLock(__FUNCTION__);
    if (angles.size() == 0)
    {
        return;
    }
    ARMARX_DEBUG_S << "actuateRobotJointsPos: first:" << angles.begin()->first << ", ang: " << angles.begin()->second;
 
    VirtualRobot::RobotPtr kinRobot = getRobot(robotName);
    if (!kinRobot)
    {
        ARMARX_WARNING_S <<  "No robot available";
        return;
    }
    kinRobot->setJointValues(angles);
 
    // Set velocities to zero
    KinematicRobotInfo& ri = kinematicRobots[robotName];
    for (auto& entry : angles)
    {
        ri.targetVelocities[entry.first] = 0;
    }
}
 
void KinematicsWorld::actuateRobotJointsVel(const std::string& robotName, const std::map< std::string, float>& velocities)
{
    ScopedRecursiveLockPtr lockEngine = getScopedEngineLock(__FUNCTION__);
    ARMARX_DEBUG_S << "actuateRobotJointsVel: first: vel:" << velocities.begin()->second;
 
    VirtualRobot::RobotPtr kinRobot = getRobot(robotName);
    if (!kinRobot)
    {
        ARMARX_WARNING_S <<  "No robot available";
        return;
    }
 
    // store velocities for static robots
    KinematicRobotInfo& ri = kinematicRobots[robotName];
    // preserve all values of velocities, and add values of robotInfo
    std::map<std::string, float> res = velocities;
    res.insert(ri.targetVelocities.begin(), ri.targetVelocities.end());
    ri.targetVelocities = res;
}
 
void KinematicsWorld::actuateRobotJointsTorque(const std::string& robotName, const std::map< std::string, float>& torques)
{
    ScopedRecursiveLockPtr lockEngine = getScopedEngineLock(__FUNCTION__);
    ARMARX_DEBUG_S << "actuateRobotJointsTorque: first:" << torques.begin()->first << "," << torques.begin()->second;
 
    ARMARX_DEBUG_S << "No torque mode available...";
}
 
void KinematicsWorld::setRobotPose(const std::string& robotName, const Eigen::Matrix4f& globalPose)
{
    ScopedRecursiveLockPtr lockEngine = getScopedEngineLock(__FUNCTION__);
    ARMARX_DEBUG << "setRobotPose:" << globalPose(0, 3) << "," << globalPose(1, 3) << ", " << globalPose(2, 3);
 
    VirtualRobot::RobotPtr kinRobot = getRobot(robotName);
    if (!kinRobot)
    {
        ARMARX_WARNING_S <<  "No robot available";
        return;
    }
 
    kinRobot->setGlobalPose(globalPose);
}
 
void KinematicsWorld::applyForceRobotNode(const std::string& robotName, const std::string& robotNodeName, const Eigen::Vector3f& force)
{
    ScopedRecursiveLockPtr lockEngine = getScopedEngineLock(__FUNCTION__);
    ARMARX_DEBUG_S << "ApplyForce not available";
}
 
void KinematicsWorld::applyTorqueRobotNode(const std::string& robotName, const std::string& robotNodeName, const Eigen::Vector3f& torque)
{
    ScopedRecursiveLockPtr lockEngine = getScopedEngineLock(__FUNCTION__);
    ARMARX_DEBUG_S << "ApplyTorque not available";
}
 
void KinematicsWorld::applyForceObject(const std::string& objectName, const Eigen::Vector3f& force)
{
    ScopedRecursiveLockPtr lockEngine = getScopedEngineLock(__FUNCTION__);
    ARMARX_DEBUG_S << "applyForceObject not available";
}
 
void KinematicsWorld::applyTorqueObject(const std::string& objectName, const Eigen::Vector3f& torque)
{
    ScopedRecursiveLockPtr lockEngine = getScopedEngineLock(__FUNCTION__);
    ARMARX_DEBUG_S << "applyTorqueObject not available";
}
 
 
bool KinematicsWorld::hasObject(const std::string& instanceName)
{
    return getObject(instanceName).get();
}
 
void KinematicsWorld::setObjectPose(const std::string& objectName, const Eigen::Matrix4f& globalPose)
{
    ScopedRecursiveLockPtr lockEngine = getScopedEngineLock(__FUNCTION__);
    VirtualRobot::SceneObjectPtr o = getObject(objectName);
 
    if (!o)
    {
        ARMARX_WARNING_S << "No object found with name " << objectName;
        return;
    }
 
    o->setGlobalPose(globalPose);
}
 
 
float KinematicsWorld::getRobotMass(const std::string& robotName)
{
    ScopedRecursiveLockPtr lockEngine = getScopedEngineLock(__FUNCTION__);
 
    VirtualRobot::RobotPtr kinRobot = getRobot(robotName);
    if (!kinRobot)
    {
        ARMARX_WARNING_S <<  "No robot available";
        return 0;
    }
 
    return kinRobot->getMass();
}
 
std::map< std::string, float> KinematicsWorld::getRobotJointAngles(const std::string& robotName)
{
    ScopedRecursiveLockPtr lockEngine = getScopedEngineLock(__FUNCTION__);
    std::map<std::string, float> res;
 
    VirtualRobot::RobotPtr kinRob = getRobot(robotName);
    if (!kinRob)
    {
        ARMARX_WARNING_S <<  "No robot available";
        return std::map< std::string, float>();
    }
 
    std::vector<RobotNodePtr> rn = kinRob->getRobotNodes();
 
    for (auto& i : rn)
    {
        if (i->isJoint())
        {
            res[i->getName()] = i->getJointValue();
        }
    }
 
    ARMARX_DEBUG_S << "getRobotJointAngles:" << res.begin()->second;
 
    return res;
}
 
float KinematicsWorld::getRobotJointAngle(const std::string& robotName, const std::string& nodeName)
{
    ScopedRecursiveLockPtr lockEngine = getScopedEngineLock(__FUNCTION__);
 
    VirtualRobot::RobotPtr kinRob = getRobot(robotName);
    if (!kinRob)
    {
        ARMARX_WARNING_S <<  "No robot available";
        return 0;
    }
 
    if (!kinRob->hasRobotNode(nodeName))
    {
        ARMARX_WARNING_S << "No robotNode available with name " << nodeName;
        return 0;
    }
 
    float res = kinRob->getRobotNode(nodeName)->getJointValue();
    ARMARX_DEBUG_S << "getRobotJointAngle:" << res;
    return res;
}
 
std::map< std::string, float> KinematicsWorld::getRobotJointVelocities(const std::string& robotName)
{
    ScopedRecursiveLockPtr lockEngine = getScopedEngineLock(__FUNCTION__);
    std::map< std::string, float> res;
 
    VirtualRobot::RobotPtr kinRobot = getRobot(robotName);
    if (!kinRobot)
    {
        ARMARX_WARNING_S <<  "No robot available";
        return res;
    }
 
    KinematicRobotInfo& ri = kinematicRobots[robotName];
    res = ri.targetVelocities;
 
    ARMARX_DEBUG_S << "getRobotJointVelocities:" << res.begin()->second;
    return res;
}
 
float KinematicsWorld::getRobotJointVelocity(const std::string& robotName, const std::string& nodeName)
{
    ScopedRecursiveLockPtr lockEngine = getScopedEngineLock(__FUNCTION__);
 
    VirtualRobot::RobotPtr kinRobot = getRobot(robotName);
    if (!kinRobot)
    {
        ARMARX_WARNING_S <<  "No robot available";
        return 0;
    }
 
    if (!kinRobot->hasRobotNode(nodeName))
    {
        ARMARX_WARNING_S << "No robotNode available with name " << nodeName;
        return 0;
    }
    KinematicRobotInfo& ri = kinematicRobots[robotName];
 
    float res = 0.0f;
    if (ri.targetVelocities.find(nodeName) != ri.targetVelocities.end())
    {
        res = ri.targetVelocities[nodeName];
    }
 
    ARMARX_DEBUG_S << "getRobotJointVelocity:" << res;
    return res;
}
 
std::map<std::string, float> KinematicsWorld::getRobotJointTorques(const std::string&)
{
    return std::map< std::string, float>();
}
 
ForceTorqueDataSeq KinematicsWorld::getRobotForceTorqueSensors(const std::string&)
{
    return {};
}
 
float KinematicsWorld::getRobotJointLimitLo(const std::string& robotName, const std::string& nodeName)
{
    ScopedRecursiveLockPtr lockEngine = getScopedEngineLock(__FUNCTION__);
 
    VirtualRobot::RobotPtr kinRobot = getRobot(robotName);
    if (!kinRobot)
    {
        ARMARX_WARNING_S <<  "No robot available";
        return 0;
    }
 
    if (!kinRobot->hasRobotNode(nodeName))
    {
        ARMARX_WARNING_S << "No robotNode available with name " << nodeName;
        return 0;
    }
 
    return kinRobot->getRobotNode(nodeName)->getJointLimitLo();
}
 
float KinematicsWorld::getRobotJointLimitHi(const std::string& robotName, const std::string& nodeName)
{
    ScopedRecursiveLockPtr lockEngine = getScopedEngineLock(__FUNCTION__);
 
    VirtualRobot::RobotPtr kinRobot = getRobot(robotName);
    if (!kinRobot)
    {
        ARMARX_WARNING_S <<  "No robot available";
        return 0;
    }
 
    if (!kinRobot->hasRobotNode(nodeName))
    {
        ARMARX_WARNING_S << "No robotNode available with name " << nodeName;
        return 0;
    }
 
    return kinRobot->getRobotNode(nodeName)->getJointLimitHi();
}
 
Eigen::Matrix4f KinematicsWorld::getRobotPose(const std::string& robotName)
{
    ScopedRecursiveLockPtr lockEngine = getScopedEngineLock(__FUNCTION__);
    ARMARX_DEBUG_S << "get robot pose";
 
    VirtualRobot::RobotPtr kinRobot = getRobot(robotName);
    if (!kinRobot)
    {
        ARMARX_WARNING_S <<  "No robot available";
        return Eigen::Matrix4f::Identity();
    }
 
    return kinRobot->getGlobalPose();
}
 
float KinematicsWorld::getRobotMaxTorque(const std::string& robotName, const std::string& nodeName)
{
    ScopedRecursiveLockPtr lockEngine = getScopedEngineLock(__FUNCTION__);
    ARMARX_DEBUG_S << "getRobotMaxTorque";
 
    VirtualRobot::RobotPtr kinRobot = getRobot(robotName);
    if (!kinRobot)
    {
        ARMARX_WARNING_S <<  "Robot '" << robotName << "' not found";
        return 0.0f;
    }
 
    if (!kinRobot->hasRobotNode(nodeName))
    {
        ARMARX_WARNING_S << "Robot node '" << nodeName << "' does not exist";
        return 0.0f;
    }
 
    return kinRobot->getRobotNode(nodeName)->getMaxTorque();
}
 
void KinematicsWorld::setRobotMaxTorque(const std::string& robotName, const std::string& nodeName, float maxTorque)
{
    ScopedRecursiveLockPtr lockEngine = getScopedEngineLock(__FUNCTION__);
    ARMARX_DEBUG_S << "setRobotMaxTorque";
 
    VirtualRobot::RobotPtr kinRobot = getRobot(robotName);
    if (!kinRobot)
    {
        ARMARX_WARNING_S <<  "Robot '" << robotName << "' not found";
        return;
    }
 
    if (!kinRobot->hasRobotNode(nodeName))
    {
        ARMARX_WARNING_S << "Robot node '" << nodeName << "' does not exist";
        return;
    }
 
    kinRobot->getRobotNode(nodeName)->setMaxTorque(maxTorque);
}
 
Eigen::Matrix4f KinematicsWorld::getRobotNodePose(const std::string& robotName, const std::string& robotNodeName)
{
    ScopedRecursiveLockPtr lockEngine = getScopedEngineLock(__FUNCTION__);
    ARMARX_DEBUG_S << "get robot node pose";
 
    VirtualRobot::RobotPtr kinRobot = getRobot(robotName);
    if (!kinRobot)
    {
        ARMARX_WARNING_S <<  "No robot available";
        return Eigen::Matrix4f::Identity();
    }
 
    if (!kinRobot->hasRobotNode(robotNodeName))
    {
        ARMARX_WARNING_S << "No robotNode available with name " << robotNodeName;
        return Eigen::Matrix4f::Identity();
    }
 
    return kinRobot->getRobotNode(robotNodeName)->getGlobalPose();
}
 
 
Eigen::Vector3f KinematicsWorld::getRobotLinearVelocity(const std::string& robotName, const std::string& nodeName)
{
    ScopedRecursiveLockPtr lockEngine = getScopedEngineLock(__FUNCTION__);
    ARMARX_DEBUG_S << "get robot linear velocity";
 
    VirtualRobot::RobotPtr kinRobot = getRobot(robotName);
    if (!kinRobot)
    {
        ARMARX_WARNING_S <<  "No robot available";
        return Eigen::Vector3f::Identity();
    }
    KinematicRobotInfo& ri = kinematicRobots[robotName];
 
    return ri.velTransActual;
}
 
 
Eigen::Vector3f KinematicsWorld::getRobotAngularVelocity(const std::string& robotName, const std::string& nodeName)
{
    ScopedRecursiveLockPtr lockEngine = getScopedEngineLock(__FUNCTION__);
    ARMARX_DEBUG << "get robot angular vel";
 
    VirtualRobot::RobotPtr kinRobot = getRobot(robotName);
    if (!kinRobot)
    {
        ARMARX_WARNING <<  "No robot available";
        return Eigen::Vector3f::Identity();
    }
    KinematicRobotInfo& ri = kinematicRobots[robotName];
 
    RobotNodePtr rn = kinRobot->getRobotNode(nodeName);
    if (!rn)
    {
        ARMARX_WARNING << "No rn found with name " << nodeName;
        return Eigen::Vector3f::Identity();
    }
 
    // Get jacobian
    RobotNodeSetPtr rns = RobotNodeSet::createRobotNodeSet(kinRobot, "All_Nodes", kinRobot->getRobotNodes());
    DifferentialIKPtr j(new DifferentialIK(rns));
    Eigen::MatrixXf jac = j->getJacobianMatrix(rn, IKSolver::Orientation);
 
    // Get joint velocities
    std::vector<std::string> nodeNames = rns->getNodeNames();
    Eigen::VectorXf joint_vel(nodeNames.size());
    int i = 0;
    for (std::vector<std::string>::iterator it = nodeNames.begin(); it != nodeNames.end(); ++it, ++i)
    {
        joint_vel(i) = ri.actualVelocities[*it];
    }
 
    Eigen::Vector3f omega;
    omega = jac * joint_vel; // vel induced by robot joints
 
    return ri.velRotActual + omega;
}
 
void KinematicsWorld::setRobotLinearVelocity(const std::string& robotName, const std::string& robotNodeName, const Eigen::Vector3f& vel)
{
    ScopedRecursiveLockPtr lockEngine = getScopedEngineLock(__FUNCTION__);
    ARMARX_DEBUG_S << "set robot lin vel";
 
    VirtualRobot::RobotPtr kinRobot = getRobot(robotName);
    if (!kinRobot)
    {
        ARMARX_WARNING_S <<  "No robot available";
        return ;
    }
    KinematicRobotInfo& ri = kinematicRobots[robotName];
 
    ri.velTransTarget = vel * 1000.0f; // m -> mm
    ARMARX_DEBUG_S << "set robot lin vel end";
}
 
void KinematicsWorld::setRobotAngularVelocity(const std::string& robotName, const std::string& robotNodeName, const Eigen::Vector3f& vel)
{
    ScopedRecursiveLockPtr lockEngine = getScopedEngineLock(__FUNCTION__);
    ARMARX_DEBUG_S << "set robot ang vel";
 
    ARMARX_DEBUG_S << "set robot lin vel";
 
    VirtualRobot::RobotPtr kinRobot = getRobot(robotName);
    if (!kinRobot)
    {
        ARMARX_WARNING_S <<  "No robot available";
        return;
    }
    KinematicRobotInfo& ri = kinematicRobots[robotName];
 
    ri.velRotTarget = vel;
    ARMARX_DEBUG_S << "set robot ang vel end";
}
 
void KinematicsWorld::setRobotLinearVelocityRobotRootFrame(const std::string& robotName, const std::string& robotNodeName, const Eigen::Vector3f& vel)
{
    ScopedRecursiveLockPtr lockEngine = getScopedEngineLock(__FUNCTION__);
    ARMARX_DEBUG_S << "set robot lin vel";
 
    VirtualRobot::RobotPtr kinRobot = getRobot(robotName);
    if (!kinRobot)
    {
        ARMARX_WARNING_S <<  "No robot available";
        return;
    }
    KinematicRobotInfo& ri = kinematicRobots[robotName];
 
    Eigen::Matrix4f newPose;
    newPose.setIdentity();
    newPose.block<3, 1>(0, 3) = vel;
    auto globalPose = kinRobot->getGlobalPose();
    globalPose(0, 3) = 0;
    globalPose(1, 3) = 0;
    globalPose(2, 3) = 0;
    Eigen::Matrix4f globalNewPose = globalPose * newPose;
    ri.velTransTarget = globalNewPose.block<3, 1>(0, 3) * 1000.0f; // m -> mm
    ARMARX_DEBUG_S << "set robot lin vel end";
}
 
void KinematicsWorld::setRobotAngularVelocityRobotRootFrame(const std::string& robotName, const std::string& robotNodeName, const Eigen::Vector3f& vel)
{
    ScopedRecursiveLockPtr lockEngine = getScopedEngineLock(__FUNCTION__);
    ARMARX_DEBUG_S << "set robot ang vel";
 
    VirtualRobot::RobotPtr kinRobot = getRobot(robotName);
    if (!kinRobot)
    {
        ARMARX_WARNING_S <<  "No robot available";
        return;
    }
    KinematicRobotInfo& ri = kinematicRobots[robotName];
 
    Eigen::Matrix4f newPose;
    newPose.setIdentity();
    newPose.block<3, 1>(0, 3) = vel;
    Eigen::Matrix4f globalPose = kinRobot->getGlobalPose();
    globalPose(0, 3) = 0;
    globalPose(1, 3) = 0;
    globalPose(2, 3) = 0;
    Eigen::Matrix4f globalNewPose = globalPose * newPose;
    ri.velRotTarget = globalNewPose.block<3, 1>(0, 3);
}
 
bool KinematicsWorld::hasRobot(const std::string& robotName)
{
    ScopedRecursiveLockPtr lockEngine = getScopedEngineLock(__FUNCTION__);
 
    std::map<std::string, KinematicRobotInfo>::iterator it = kinematicRobots.begin();
    while (it != kinematicRobots.end())
    {
        if (it->first == robotName)
        {
            return true;
        }
        it++;
    }
 
    return false;
}
 
bool KinematicsWorld::hasRobotNode(const std::string& robotName, const std::string& robotNodeName)
{
    ScopedRecursiveLockPtr lockEngine = getScopedEngineLock(__FUNCTION__);
 
    VirtualRobot::RobotPtr kinRobot = getRobot(robotName);
    if (!kinRobot)
    {
        ARMARX_WARNING_S <<  "No robot available";
        return false;
    }
 
    return (kinRobot->hasRobotNode(robotNodeName));
}
 
 
VirtualRobot::RobotPtr KinematicsWorld::getRobot(const std::string& robotName)
{
    ScopedRecursiveLockPtr lockEngine = getScopedEngineLock(__FUNCTION__);
 
    if (!hasRobot(robotName))
    {
        ARMARX_WARNING_S << "No robot with name " << robotName;
        return VirtualRobot::RobotPtr();
    }
 
    return kinematicRobots[robotName].robot;
}
 
std::map<std::string, RobotPtr> KinematicsWorld::getRobots()
{
    std::map<std::string, RobotPtr> result;
    ScopedRecursiveLockPtr lockEngine = getScopedEngineLock(__FUNCTION__);
    for (auto& elem : kinematicRobots)
    {
        const KinematicRobotInfo& info = elem.second;
        result[elem.first] = (info.robot);
    }
    return result;
}
 
VirtualRobot::SceneObjectPtr KinematicsWorld::getObject(const std::string& objectName)
{
    VirtualRobot::SceneObjectPtr o;
    std::vector<VirtualRobot::SceneObjectPtr>::const_iterator it;
 
    for (it = kinematicObjects.begin(); it != kinematicObjects.end(); it++)
    {
        if ((*it)->getName() == objectName)
        {
            o = *it;
            break;
        }
    }
 
    return o;
}
 
Eigen::Matrix4f KinematicsWorld::getObjectPose(const std::string& objectName)
{
    ScopedRecursiveLockPtr lockEngine = getScopedEngineLock(__FUNCTION__);
    ARMARX_DEBUG_S << "get object pose";
 
    VirtualRobot::SceneObjectPtr o = getObject(objectName);
    if (!o)
    {
        ARMARX_WARNING_S << "No object found with name " << objectName;
        return Eigen::Matrix4f::Identity();
    }
 
    return o->getGlobalPose();
}
 
bool KinematicsWorld::objectReleasedEngine(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;
    VirtualRobot::RobotPtr kinRobot = getRobot(robotName);
    if (!kinRobot)
    {
        ARMARX_WARNING_S <<  "No robot available";
        return false;
    }
 
    if (!kinRobot->hasRobotNode(robotNodeName))
    {
        ARMARX_WARNING_S << "No robotNode available with name " << robotNodeName;
        return false;
    }
 
    /*VirtualRobot::SceneObjectPtr o = getObject(objectName);
 
    if (!o)
    {
        ARMARX_WARNING_S << "No object found with name " << objectName;
        return false;
    }*/
    RobotNodePtr rn = kinRobot->getRobotNode(objectName);
    if (!rn)
    {
        ARMARX_WARNING_S << "No rn found with name " << objectName;
        return false;
    }
 
    bool res = RobotFactory::detach(kinRobot, rn);
    return res;
}
 
bool KinematicsWorld::objectGraspedEngine(const std::string& robotName, const std::string& robotNodeName, const std::string& objectName, Eigen::Matrix4f& storeLocalTransform)
{
    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 false;
        }
    }
    VirtualRobot::RobotPtr kinRobot = getRobot(robotName);
    if (!kinRobot)
    {
        ARMARX_WARNING_S <<  "No robot available";
        return false;
    }
 
    if (!kinRobot->hasRobotNode(robotNodeName))
    {
        ARMARX_WARNING_S << "No robotNode available with name " << robotNodeName;
        return false;
    }
 
    RobotNodePtr rn = kinRobot->getRobotNode(robotNodeName);
 
    VirtualRobot::SceneObjectPtr o = getObject(objectName);
 
    if (!o)
    {
        ARMARX_WARNING_S << "No object found with name " << objectName;
        return false;
    }
 
    storeLocalTransform = rn->toLocalCoordinateSystem(o->getGlobalPose());
    bool res = RobotFactory::attach(kinRobot, o, rn, storeLocalTransform);
    return res;
}
 
 
SceneObjectPtr KinematicsWorld::getFloor()
{
    return groundObject;
}
 
 
 
bool KinematicsWorld::removeRobotEngine(const std::string& robotName)
{
    // lock engine and data
    ScopedRecursiveLockPtr lockEngine = getScopedEngineLock(__FUNCTION__);
 
    VirtualRobot::RobotPtr kinRobot = getRobot(robotName);
    if (!kinRobot)
    {
        ARMARX_WARNING_S <<  "No robot available";
        return false;
    }
 
    ARMARX_DEBUG_S << "Removing robot " << robotName;
 
    kinematicRobots.erase(robotName);
 
    return true;
}
 
 
bool KinematicsWorld::addRobotEngine(RobotPtr robot, double pid_p, double pid_i, double pid_d, const std::string& filename, bool staticRobot, bool selfCollisions)
{
    ARMARX_VERBOSE_S << "Adding robot " << robot->getName() << ", file:" << filename;
 
    // lock engine and data
    ScopedRecursiveLockPtr lockEngine = getScopedEngineLock(__FUNCTION__);
 
    //access engine
    ARMARX_DEBUG_S << "add robot";
 
    if (hasRobot(robot->getName()))
    {
        ARMARX_ERROR_S << "More than robot with identical name is currently not supported!";
        return false;
    }
 
    KinematicRobotInfo robInfo;
 
    ARMARX_INFO_S << "Building static robot...";
    /*std::vector< RobotNodePtr > nodes = robot->getRobotNodes();
    for (auto n : nodes)
    {
        n->setSimulationType(SceneObject::Physics::eKinematic);
    }*/
 
    RobotPtr r = robot->clone(robot->getName());
 
    // since we do not visualize this robot we can skip visualization updates
    r->setThreadsafe(false); // we already got a mutex protection
    r->setUpdateVisualization(false);
    r->setPropagatingJointValuesEnabled(false);
 
    robInfo.robot = r;
 
    kinematicRobots[r->getName()] = robInfo;
 
    return true;
}
 
armarx::FramedPosePtr KinematicsWorld::toFramedPose(const Eigen::Matrix4f& globalPose, const std::string& robotName, const std::string& frameName)
{
    ScopedRecursiveLockPtr lockEngine = getScopedEngineLock(__FUNCTION__);
 
    RobotPtr kinRobot = getRobot(robotName);
    if (!kinRobot)
    {
        ARMARX_WARNING_S << "No robot or wrong name:" << robotName;
        return FramedPosePtr();
    }
 
    RobotNodePtr rn = kinRobot->getRobotNode(frameName);
 
    if (!rn)
    {
        ARMARX_WARNING_S << "No robot node found (" << frameName << ")";
        return FramedPosePtr();
    }
 
    Eigen::Matrix4f localPose = rn->toLocalCoordinateSystem(globalPose);
 
    armarx::FramedPosePtr framedPose = new armarx::FramedPose(localPose, frameName, kinRobot->getName());
    return framedPose;
}
 
bool KinematicsWorld::synchronizeRobotNodePoses(const std::string& robotName, std::map<std::string, armarx::PoseBasePtr>& objMap)
{
    // lock engine and data
    ScopedRecursiveLockPtr lockEngine = getScopedEngineLock(__FUNCTION__);
    ScopedRecursiveLockPtr lockSync = getScopedSyncLock(__FUNCTION__);
 
    RobotPtr kinRobot = getRobot(robotName);
    if (!kinRobot)
    {
        ARMARX_WARNING_S << "No robot or wrong name:" << robotName;
        return false;
    }
 
    VirtualRobot::SceneObjectPtr currentObjEngine = kinRobot->getRootNode();
 
    if (!synchronizeSceneObjectPoses(currentObjEngine, objMap))
    {
        ARMARX_ERROR_S << "Failed to synchronize objects...";
        return false;
    }
    return true;
}
 
void KinematicsWorld::calculateActualVelocities()
{
 
    ScopedRecursiveLockPtr lockEngine = getScopedEngineLock(__FUNCTION__);
    for (const auto& data : kinematicRobots)
    {
        RobotPtr kinRobot = data.second.robot;
        auto robotName = data.first;
        if (!kinRobot)
        {
            ARMARX_WARNING_S << "No robot or wrong name:" << robotName;
            continue;
        }
 
        NameValueMap jointAngles;
        std::vector<RobotNodePtr> rn = kinRobot->getRobotNodes();
        KinematicRobotInfo& ri = kinematicRobots[robotName];
        NameValueMap& jointVelocities = ri.actualVelocities;
        IceUtil::Time timestamp = IceUtil::Time::secondsDouble(currentSimTimeSec);
        for (auto& i : rn)
        {
            if ((i->isJoint()))
            {
                std::string n = i->getName();
 
                jointAngles[n] = i->getJointValue();
                auto filterIt = jointAngleDerivationFilters.find(i);
                if (filterIt == jointAngleDerivationFilters.end())
                {
                    //                    DerivationFilterBasePtr filter(new ) ;
                    ARMARX_INFO << "Creating filter for " << n;
                    filterIt = jointAngleDerivationFilters.insert(std::make_pair(i, std::make_pair(timestamp, i->getJointValue()))).first;
                    //                    filterIt->second.windowFilterSize = 25;
                }
                else
                {
                    jointVelocities[n] = (i->getJointValue() - filterIt->second.second) / (timestamp - filterIt->second.first).toSecondsDouble();
                    filterIt->second.second = i->getJointValue();
                    filterIt->second.first = timestamp;
                }
                //                filterIt->second.update(timestamp.toMicroSeconds(), new Variant(jointAngles[n]));
                //            if (ri.targetVelocities.find(n) != ri.targetVelocities.end())
                //            {
                //                jointVelocities[n] = ri.targetVelocities[n];
                //            }
                //            else
                {
                    //                    jointVelocities[n] = filterIt->second.getValue()->getFloat();
 
                }
            }
        }
        //        ARMARX_INFO << deactivateSpam(1) << jointVelocities;
 
        // calculate linear velocity of robot
        Eigen::Vector3f linearVelocity;
        auto filterIt = linearVelocityFilters.find(kinRobot);
        Eigen::Vector3f pos = kinRobot->getGlobalPose().block<3, 1>(0, 3);
        if (filterIt == linearVelocityFilters.end())
        {
            filterIt = linearVelocityFilters.insert(std::make_pair(kinRobot, std::make_pair(timestamp, pos))).first;
        }
        linearVelocity = (pos - filterIt->second.second) / (timestamp - filterIt->second.first).toSecondsDouble();
        filterIt->second.second = pos;
        filterIt->second.first = timestamp;
        ri.velTransActual = linearVelocity;
        //ARMARX_INFO << deactivateSpam(1) << linearVelocity;
 
 
 
        // calculate angular velocity of robot
        Eigen::Vector3f angularVelocity;
        angularVelocity.setZero();
        auto filterVelIt = angularVelocityFilters.find(kinRobot);
        if (filterVelIt == angularVelocityFilters.end()) // store previous orientation (TODO: add filtering, e.g. on rpy)
        {
            //Eigen::Vector3f rpy;
            //MathTools::eigen4f2rpy(kinRobot->getGlobalPose(), rpy);
            filterVelIt = angularVelocityFilters.insert(std::make_pair(kinRobot, std::make_pair(timestamp, kinRobot->getGlobalPose()))).first;
        }
 
        double deltaTimestamp = (timestamp - filterVelIt->second.first).toSecondsDouble();
 
 
        // old method
        //        Eigen::Matrix4f deltaMat = kinRobot->getGlobalPose() * filterVelIt->second.second.inverse();
        //        MathTools::eigen4f2rpy(deltaMat, angularVelocity);
        //        if (deltaTimestamp > 0)
        //        {
        //            angularVelocity /= deltaTimestamp;
        //        }
        //        ri.velRotActual = angularVelocity;
 
        // method from http://math.stackexchange.com/questions/668866/how-do-you-find-angular-velocity-given-a-pair-of-3x3-rotation-matrices
        Eigen::Matrix3f Rot_prev, Rot_new, A, Omega_mat;
        float theta;
        Rot_prev = filterVelIt->second.second.block(0, 0, 3, 3);
        Rot_new  = kinRobot->getGlobalPose().block(0, 0, 3, 3);
        A = Rot_new * Rot_prev.transpose();
        theta = acos((A.trace() - 1) / 2.);
 
        if (theta == 0.)
        {
            ri.velRotActual.setZero();
            return;
        }
 
        Omega_mat = theta * (A - A.transpose()) / (2 * deltaTimestamp * sin(theta));
        ri.velRotActual[0] = Omega_mat(2, 1);
        ri.velRotActual[1] = Omega_mat(0, 2);
        ri.velRotActual[2] = Omega_mat(1, 0);
 
        if (deltaTimestamp > 0.1)
        {
            angularVelocityFilters[kinRobot] = std::make_pair(timestamp, kinRobot->getGlobalPose());
        }
 
        //ARMARX_INFO << deactivateSpam(1) << VAROUT(angularVelocity);
    }
}
 
bool KinematicsWorld::getRobotStatus(const std::string& robotName, NameValueMap& jointAngles, NameValueMap& jointVelocities, NameValueMap& jointTorques, Eigen::Vector3f& linearVelocity, Eigen::Vector3f& angularVelocity)
{
    ScopedRecursiveLockPtr lockEngine = getScopedEngineLock(__FUNCTION__);
    RobotPtr kinRobot = getRobot(robotName);
    if (!kinRobot)
    {
        ARMARX_WARNING_S << "No robot or wrong name:" << robotName;
        return false;
    }
 
    std::vector<RobotNodePtr> rn = kinRobot->getRobotNodes();
    KinematicRobotInfo& ri = kinematicRobots[robotName];
 
    //    float simTimeFactor = 1.0;
    //    if (simStepExecutionTimeMS > 0)
    //    {
    //        simTimeFactor = simTimeStepMS / simStepExecutionTimeMS;
    //    }
 
    //    IceUtil::Time timestamp = IceUtil::Time::secondsDouble(currentSimTimeSec);
    jointVelocities = ri.actualVelocities;
    for (auto& i : rn)
    {
        if ((i->isJoint()))
        {
            std::string n = i->getName();
 
            jointAngles[n] = i->getJointValue();
            //            auto filterIt = jointAngleDerivationFilters.find(rn[i]);
            //            if (filterIt == jointAngleDerivationFilters.end())
            //            {
            //                filterIt = jointAngleDerivationFilters.insert(std::make_pair(rn[i], new filters::DerivationFilter())).first;
            //                filterIt->second->windowFilterSize = 25;
            //            }
            //            filterIt->second->update(timestamp.toMicroSeconds(), new Variant(jointAngles[n]));
            //            //            if (ri.targetVelocities.find(n) != ri.targetVelocities.end())
            //            //            {
            //            //                jointVelocities[n] = ri.targetVelocities[n];
            //            //            }
            //            //            else
            //            {
            //                jointVelocities[n] = filterIt->second->getValue()->getFloat() * simTimeFactor;
            //            }
            //            ARMARX_INFO << deactivateSpam(1, n) << n << ": " << jointVelocities[n];
            jointTorques[n] = 0;
        }
    }
 
    linearVelocity = ri.velTransActual;
 
 
    //    filterIt = angularVelocityFilters.find(kinRobot);
    //    if (filterIt == angularVelocityFilters.end())
    //    {
    //        filterIt = angularVelocityFilters.insert(std::make_pair(kinRobot, new filters::DerivationFilter())).first;
    //        filterIt->second->windowFilterSize = 25;
    //    }
    //    filterIt->second->update(timestamp.toMicroSeconds(), new Variant(jointAngles[n]));
    angularVelocity = ri.velRotTarget;
 
    return true;
}
 
bool KinematicsWorld::updateForceTorqueSensor(ForceTorqueInfo& ftInfo)
{
    ScopedRecursiveLockPtr lockEngine = getScopedEngineLock(__FUNCTION__);
 
    // not implemented
 
    return true;
}
 
 
bool KinematicsWorld::synchronizeObjects()
{
    // lock engine and data
    ScopedRecursiveLockPtr lockEngine = getScopedEngineLock(__FUNCTION__);
    ScopedRecursiveLockPtr lockSync = getScopedSyncLock(__FUNCTION__);
 
    for (auto oEngine : kinematicObjects)
    {
        bool objectFound = false;
 
        for (auto& s : simVisuData.objects)
        {
            if (s.name == oEngine->getName())
            {
                objectFound = true;
                synchronizeSceneObjectPoses(oEngine, s.objectPoses);
                s.updated = true;
                break;
            }
        }
 
        if (!objectFound)
        {
            ARMARX_WARNING_S << "Object with name " << oEngine->getName() << " not in synchronized list";
        }
    }
 
    return true;
}
 
bool KinematicsWorld::addObstacleEngine(VirtualRobot::SceneObjectPtr o, VirtualRobot::SceneObject::Physics::SimulationType simType)
{
    ScopedRecursiveLockPtr lockEngine = getScopedEngineLock(__FUNCTION__);
 
    ARMARX_DEBUG_S << "Adding obstacle " << o->getName();
    VirtualRobot::SceneObjectPtr clonedObj = o->clone(o->getName());
 
    kinematicObjects.push_back(clonedObj);
 
    // since we do not visualize this object we can skip visualization updates
    clonedObj->setUpdateVisualization(false);
 
    return true;
}
 
std::vector<std::string> KinematicsWorld::getRobotNames()
{
    std::vector<std::string> names;
    {
        ScopedRecursiveLockPtr lockEngine = getScopedEngineLock(__FUNCTION__);
        std::map<std::string, KinematicRobotInfo>::iterator it = kinematicRobots.begin();
        while (it != kinematicRobots.end())
        {
            names.push_back(it->first);
            it++;
        }
    }
    return names;
}
 
void KinematicsWorld::setObjectSimType(const std::string& objectName, SceneObject::Physics::SimulationType simType)
{
    ScopedRecursiveLockPtr lockEngine = getScopedEngineLock(__FUNCTION__);
    SceneObjectPtr o = getObject(objectName);
    if (!o)
    {
        ARMARX_WARNING_S << "Could not find object with name " << objectName;
        return;
    }
    //o->setSimType(simType);
}
 
void KinematicsWorld::setRobotNodeSimType(const std::string& robotName, const std::string& robotNodeName, SceneObject::Physics::SimulationType simType)
{
    ScopedRecursiveLockPtr lockEngine = getScopedEngineLock(__FUNCTION__);
    RobotPtr r = getRobot(robotName);
    if (!r)
    {
        ARMARX_WARNING_S << "Could not find robot with name " << robotName;
        return;
    }
    RobotNodePtr rn = r->getRobotNode(robotNodeName);
    if (!rn)
    {
        ARMARX_WARNING_S << "Could not find robot node with name " << robotNodeName;
        return;
    }
    //rn->setSimType(simType);
}
 
std::vector<std::string> KinematicsWorld::getObstacleNames()
{
    std::vector<std::string> names;
    {
        ScopedRecursiveLockPtr lockEngine = getScopedEngineLock(__FUNCTION__);
 
        for (auto& o : kinematicObjects)
        {
            names.push_back(o->getName());
        }
    }
    return names;
}
 
 
bool KinematicsWorld::removeObstacleEngine(const std::string& name)
{
    ARMARX_VERBOSE_S << "Removing obstacle " << name;
    ScopedRecursiveLockPtr lockEngine = getScopedEngineLock(__FUNCTION__);
    {
        SceneObjectPtr o = getObject(name);
 
        if (!o)
        {
            return false;
        }
 
        std::vector<SceneObjectPtr>::iterator it = find(kinematicObjects.begin(), kinematicObjects.end(), o);
 
        if (it != kinematicObjects.end())
        {
            kinematicObjects.erase(it);
        }
        else
        {
            ARMARX_WARNING_S << "Sync error";
        }
    }
 
    return true;
}
 
 
 
void KinematicsWorld::stepPhysicsRealTime()
{
    ScopedRecursiveLockPtr lockEngine = getScopedEngineLock(__FUNCTION__);
    ARMARX_DEBUG << "start sim physics";
 
 
    //    IceUtil::Time startTime = IceUtil::Time::now();
    float dt1 = getDeltaTimeMilli();
 
    // the engine has to be stepped with seconds
    // max 100 internal loops
    ARMARX_DEBUG << deactivateSpam(1) << "dt1: " << dt1 << " ms";
    simTimeStepMS = dt1;
    // step static objects and robots
    stepStaticObjects(dt1 / 1000.0);
    stepStaticRobots(dt1 / 1000.0);
    calculateActualVelocities();
 
 
    currentSimTimeSec += dt1 / 1000.0;
    //    IceUtil::Time duration = IceUtil::Time::now() - startTime;
    simStepExecutionDurationMS = dt1;
    ARMARX_DEBUG << "end sim physics, simTime ms:" << simStepExecutionDurationMS;
}
 
 
void KinematicsWorld::stepPhysicsFixedTimeStep()
{
    auto startTime = IceUtil::Time::now(); // this should always be real time and not virtual time
    {
        ScopedRecursiveLockPtr lockEngine = getScopedEngineLock(__FUNCTION__);
 
        float stepSize = (float)(stepSizeMs) / 1000.0f;
        ARMARX_DEBUG_S << "start sim physics: " << VAROUT(stepSize) << VAROUT(stepSizeMs);
 
        simTimeStepMS = stepSizeMs;
 
        // step static robots
        stepStaticObjects(stepSize);
        stepStaticRobots(stepSize);
        calculateActualVelocities();
 
        currentSimTimeSec += stepSize;
    }
 
 
    if (maxRealTimeSimSpeed > 0)
    {
        auto duration = (IceUtil::Time::now() - startTime).toMilliSecondsDouble();
        const double minStepSizeMs = static_cast<double>(stepSizeMs) / maxRealTimeSimSpeed;
        if (duration < minStepSizeMs)
        {
            ARMARX_DEBUG << "Sim calculation took " << duration  << " - Sleeping now for " << (stepSizeMs - duration);
            usleep(1000 * (minStepSizeMs - duration));
        }
    }
 
    IceUtil::Time duration = IceUtil::Time::now() - startTime;
    simStepExecutionDurationMS = (float)duration.toMilliSecondsDouble();
    ARMARX_DEBUG << "end sim physics, simTime ms:" << simStepExecutionDurationMS;
}
 
void KinematicsWorld::stepStaticRobots(double deltaInSeconds)
{
    // step through all static robots and apply velocities
    for (auto dr : kinematicRobots)
    {
        KinematicRobotInfo ri = dr.second;
        std::map<std::string, float> targetPos;
        VirtualRobot::RobotPtr kinRob = ri.robot;
        ARMARX_DEBUG << "Applying velocities for static robot " << kinRob->getName();
        for (auto nv : ri.targetVelocities)
        {
            if (!kinRob->hasRobotNode(nv.first))
            {
                ARMARX_ERROR  << deactivateSpam(4, nv.first) << "No rn with name " << nv.first;
                continue;
            }
            double change = (nv.second * deltaInSeconds);
 
            //double randomNoiseValue = distribution(generator);
            //change += randomNoiseValue;
            double newAngle = kinRob->getRobotNode(nv.first)->getJointValue() + change;
            targetPos[nv.first] = (float)newAngle;
        }
        if (targetPos.size() > 0)
        {
            ARMARX_DEBUG << "Target joint angles:" << targetPos;
            kinRob->setJointValues(targetPos);
        }
        if (ri.velTransTarget.norm() > 0 || ri.velRotTarget.norm() > 0)
        {
            Eigen::Matrix4f gp = kinRob->getGlobalPose();
            gp.block(0, 3, 3, 1) += ri.velTransTarget * deltaInSeconds;
            Eigen::Matrix3f m3;
            m3 = Eigen::AngleAxisf(ri.velRotTarget(0) * deltaInSeconds, Eigen::Vector3f::UnitX())
                 * Eigen::AngleAxisf(ri.velRotTarget(1) * deltaInSeconds, Eigen::Vector3f::UnitY())
                 * Eigen::AngleAxisf(ri.velRotTarget(2) * deltaInSeconds, Eigen::Vector3f::UnitZ());
            gp.block(0, 0, 3, 3) *= m3;
            setRobotPose(kinRob->getName(), gp);
        }
    }
}
 
 
void KinematicsWorld::stepStaticObjects(double deltaInSeconds)
{
    // Step attached objects
    for (auto& o : attachedObjects)
    {
        if (kinematicRobots.find(o.robotName) == kinematicRobots.end())
        {
            // Robot not found
            continue;
        }
 
        VirtualRobot::RobotPtr kinRob = kinematicRobots[o.robotName].robot;
 
        if (!kinRob->hasRobotNode(o.objectName))
        {
            // Object is not attached
            continue;
        }
 
        Eigen::Matrix4f pose = kinRob->getRobotNode(o.objectName)->getGlobalPose();
 
        for (auto& kino : kinematicObjects)
        {
            if (kino->getName() == o.objectName)
            {
                kino->setGlobalPose(pose);
                break;
            }
        }
    }
}
 
int KinematicsWorld::getFixedTimeStepMS()
{
    return stepSizeMs;
}
 
std::vector<SimDynamics::DynamicsEngine::DynamicsContactInfo> KinematicsWorld::copyContacts()
{
    ScopedRecursiveLockPtr lockSync = getScopedSyncLock(__FUNCTION__);
    return std::vector<SimDynamics::DynamicsEngine::DynamicsContactInfo>();
}
 
std::vector<SceneObjectPtr> KinematicsWorld::getObjects()
{
    return kinematicObjects;
}
 
void KinematicsWorld::setupFloorEngine(bool enable, const std::string& floorTexture)
{
 
    ScopedRecursiveLockPtr lockEngine = getScopedEngineLock(__FUNCTION__);
 
    std::string textureFile = floorTexture;
 
    if (enable)
    {
        ARMARX_INFO_S << "Creating floor plane...";
        Eigen::Vector3f pos;
        pos.setZero();
        Eigen::Vector3f up = Eigen::Vector3f::UnitZ();
        createFloorPlane(pos, up);
    }
}
 
void KinematicsWorld::enableLogging(const std::string& robotName, const std::string& logFile)
{
    ScopedRecursiveLockPtr lockEngine = getScopedEngineLock(__FUNCTION__);
 
}
 
 
 
bool KinematicsWorld::synchronizeSceneObjectPoses(SceneObjectPtr currentObjEngine, std::map<std::string, armarx::PoseBasePtr>& objMap)
{
    if (!currentObjEngine)
    {
        return false;
    }
 
    PosePtr p(new Pose(currentObjEngine->getGlobalPose()));
    objMap[currentObjEngine->getName()] = p;
    std::vector<SceneObjectPtr> childrenE = currentObjEngine->getChildren();
 
    for (const auto& i : childrenE)
    {
        if (!synchronizeSceneObjectPoses(i, objMap))
        {
            return false;
        }
    }
 
    return true;
}
 
 
bool KinematicsWorld::synchronizeSimulationDataEngine()
{
    // lock engine and data
    ScopedRecursiveLockPtr lockEngine = getScopedEngineLock(__FUNCTION__);
 
    // CONTACTS
    /*if (collectContacts)
    {
        contacts = dynamicsWorld->getEngine()->getContacts();
        std::stringstream ss;
        for (auto c : contacts)
        {
            ss << c.objectAName << " < - > " << c.objectBName;
        }
        ARMARX_DEBUG << "Contacts:" << endl << ss.str();
    }*/
    return true;
}
 
float KinematicsWorld::getDeltaTimeMilli()
{
    ScopedRecursiveLockPtr lockEngine = getScopedEngineLock(__FUNCTION__);
 
    auto newEndTime = IceUtil::Time::now();
    auto frameTime = newEndTime - lastTime;
    lastTime = newEndTime;
 
    return (float)(frameTime).toMilliSecondsDouble();;
}
 
/*
int KinematicsWorld::getContactCont()
{
    ScopedRecursiveLockPtr l = getScopedSyncLock(__FUNCTION__);
    return contacts.size();
}*/
 
DistanceInfo KinematicsWorld::getRobotNodeDistance(const std::string& robotName, const std::string& robotNodeName1, const std::string& robotNodeName2)
{
    ScopedRecursiveLockPtr lockEngine = getScopedEngineLock(__FUNCTION__);
 
    VirtualRobot::RobotPtr robot = getRobot(robotName);
    if (!robot)
    {
        throw LocalException("Wrong robot name. '") << robotName << "'";
    }
 
    if (!robot->hasRobotNode(robotNodeName1))
    {
        throw LocalException("Wrong robot node name. '") << robotNodeName1 << "'";
    }
    if (!robot->hasRobotNode(robotNodeName2))
    {
        throw LocalException("Wrong robot node name. '") << robotNodeName2 << "'";
    }
 
    // Compute distance from node to the rest of the robot (does that make sense?!)
    Eigen::Vector3f p1, p2;
    auto model1 = robot->getRobotNode(robotNodeName1)->getCollisionModel();
    auto model2 = robot->getRobotNode(robotNodeName2)->getCollisionModel();
    float d = robot->getCollisionChecker()->calculateDistance(model1, model2, p1, p2);
    DistanceInfo di;
    di.distance = d;
    di.p1 = new Vector3(p1);
    di.p2 = new Vector3(p2);
 
    return di;
}
 
DistanceInfo KinematicsWorld::getDistance(const std::string& robotName, const std::string& robotNodeName, const std::string& worldObjectName)
{
    ScopedRecursiveLockPtr lockEngine = getScopedEngineLock(__FUNCTION__);
 
    VirtualRobot::RobotPtr robot = getRobot(robotName);
    if (!robot)
    {
        throw LocalException("Wrong robot name. '") << robotName << "'";
    }
 
    if (!robot->hasRobotNode(robotNodeName))
    {
        throw LocalException("Wrong robot node name. '") << robotNodeName << "'";
    }
    VirtualRobot::SceneObjectPtr so = getObject(worldObjectName);
    if (!so)
    {
        throw LocalException("No object with name '") << worldObjectName << "'";
    }
    Eigen::Vector3f p1, p2;
 
    float d = robot->getCollisionChecker()->calculateDistance(robot->getRobotNode(robotNodeName)->getCollisionModel(), so->getCollisionModel(), p1, p2);
    DistanceInfo di;
    di.distance = d;
    di.p1 = new Vector3(p1);
    di.p2 = new Vector3(p2);
 
    return di;
}