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 <algorithm>
 
#include <VirtualRobot/CollisionDetection/CollisionChecker.h>
#include <VirtualRobot/IK/DifferentialIK.h>
#include <VirtualRobot/IK/IKSolver.h>
#include <VirtualRobot/Nodes/ForceTorqueSensor.h>
#include <VirtualRobot/Nodes/RobotNode.h>
#include <VirtualRobot/Obstacle.h>
#include <VirtualRobot/Robot.h>
#include <VirtualRobot/RobotFactory.h>
#include <VirtualRobot/RobotNodeSet.h>
#include <VirtualRobot/Visualization/VisualizationFactory.h>
 
#include <ArmarXCore/core/exceptions/LocalException.h>
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/system/ArmarXDataPath.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;
}