BulletPhysicsWorld.cpp

Go to the documentation of this file.

/*
 * This file is part of ArmarX.
 *
 * Copyright (C) 2013-2016, High Performance Humanoid Technologies (H2T), Karlsruhe Institute of Technology (KIT), all rights reserved.
 *
 * ArmarX is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License version 2 as
 * published by the Free Software Foundation.
 *
 * ArmarX is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program. If not, see <http://www.gnu.org/licenses/>.
 *
 * @package    ArmarXSimulation::Simulator
 * @author     Nikolaus ( vahrenkamp at kit dot edu )
 * @date       2014
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "BulletPhysicsWorld.h"
 
#include <SimDynamics/DynamicsEngine/BulletEngine/BulletRobot.h>
#include <VirtualRobot/Robot.h>
#include <VirtualRobot/Nodes/ForceTorqueSensor.h>
#include <VirtualRobot/CollisionDetection/CollisionChecker.h>
#include <VirtualRobot/RobotFactory.h>
 
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/system/ArmarXDataPath.h>
#include <algorithm>
 
#include <memory>
#include <ArmarXCore/core/exceptions/LocalException.h>
 
using namespace VirtualRobot;
using namespace SimDynamics;
using namespace armarx;
 
 
BulletPhysicsWorld::BulletPhysicsWorld() : SimulatedWorld()
{
    synchronizeDurationMS = 0.0f;
    currentSimTimeSec = 0.0;
    collectContacts = false;
}
 
BulletPhysicsWorld::~BulletPhysicsWorld()
{
    bulletEngine.reset();
    dynamicsWorld.reset();
}
 
void BulletPhysicsWorld::initialize(int stepSizeMS, int bulletFixedTimeStepMS, int bulletFixedTimeStepMaxNrLoops, float maxRealTimeSimSpeed, bool floorPlane, std::string floorTexture)
{
    ARMARX_INFO_S << "Init BulletPhysicsWorld. Creating bullet engine...";
 
    simVisuData.robots.clear();
    simVisuData.objects.clear();
    simVisuData.floorTextureFile.clear();
    simVisuData.floor = false;
 
    simReportData.robots.clear();
 
    contacts.clear();
    this->maxRealTimeSimSpeed = maxRealTimeSimSpeed;
    this->stepSizeMs = stepSizeMS;
    this->bulletFixedTimeStepMS = bulletFixedTimeStepMS;
    this->bulletFixedTimeStepMaxNrLoops = bulletFixedTimeStepMaxNrLoops;
 
    dynamicsWorld = SimDynamics::DynamicsWorld::Init();
 
    bulletEngine = std::dynamic_pointer_cast<BulletEngine>(dynamicsWorld->getEngine());
 
    if (!bulletEngine)
    {
        ARMARX_ERROR_S << "Could not cast engine to BULLET engine, non-bullet engines not yet supported!";
        throw UserException("Bullet engine missing");
    }
 
    bulletEngine->setMutex(engineMutex);
 
    setupFloor(floorPlane, floorTexture);
 
    m_clock.reset();
}
 
 
void BulletPhysicsWorld::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;
    SimDynamics::DynamicsRobotPtr dynamicsRobot = getDynamicRobot(robotName);
    if (!dynamicsRobot)
    {
        ARMARX_WARNING_S << "No robot available";
        return;
    }
 
    RobotPtr kinRob = dynamicsRobot->getRobot();
 
    if (!kinRob || kinRob->getName() != robotName)
    {
        ARMARX_WARNING_S << "No robot available with name " << robotName;
 
        if (kinRob)
        {
            ARMARX_WARNING_S << "available robot:" << kinRob->getName();
        }
 
        return;
    }
 
    bool isStatic = dynamicRobots[robotName].isStatic;
 
    for (const auto& angle : angles)
    {
        // target values
        std::string targetJointName = angle.first;
        float targetJointPos = angle.second;
        auto velIt = velocities.find(angle.first);
 
        if (isStatic)
        {
            RobotNodePtr rn = kinRob->getRobotNode(targetJointName);
            BulletRobot* br = dynamic_cast<BulletRobot*>(dynamicsRobot.get());
            if (br && br->hasLink(rn))
            {
                kinRob->setJointValue(rn, targetJointPos);
                BulletRobot::LinkInfo link = br->getLink(rn);
                RobotNodePtr nodeCloneA;
                RobotNodePtr nodeCloneB;
 
                if (link.nodeA && link.dynNode1)
                {
                    nodeCloneA = kinRob->getRobotNode(link.nodeA->getName());
                    link.dynNode1->setPose(nodeCloneA->getGlobalPose());
                }
                if (link.nodeB && link.dynNode2)
                {
                    nodeCloneB = kinRob->getRobotNode(link.nodeB->getName());
                    link.dynNode1->setPose(nodeCloneB->getGlobalPose());
                }
            }
        }
        else
        {
            if (kinRob->hasRobotNode(targetJointName) && dynamicsRobot->getDynamicsRobotNode(targetJointName) && velIt != velocities.end())
            {
                VR_ASSERT(not kinRob->isPassive());
                dynamicsRobot->actuateNode(dynamicsRobot->getRobot()->getRobotNode(targetJointName),
                                           static_cast<double>(targetJointPos),
                                           static_cast<double>(velIt->second));
            }
            else
            {
                ARMARX_WARNING_S << deactivateSpam(5, targetJointName) << "No node with name " << targetJointName;
            }
        }
    }
}
 
void BulletPhysicsWorld::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;
 
    // ARMARX_WARNING_S << "actuateRobotJointsPos: " << VAROUT(angles);
 
    SimDynamics::DynamicsRobotPtr dynamicsRobot = getDynamicRobot(robotName);
    if (!dynamicsRobot)
    {
        ARMARX_WARNING_S <<  "No robot available with name " << robotName;
        return;
    }
    bool isStatic = dynamicRobots[robotName].isStatic;
 
    RobotPtr kinRob = dynamicsRobot->getRobot();
 
    if (!kinRob || kinRob->getName() != robotName)
    {
        ARMARX_WARNING_S << "No robot available with name " << robotName;
        return;
    }
 
    for (const auto& angle : angles)
    {
        // target values
        std::string targetJointName = angle.first;
        float targetJoint = angle.second;
 
        if (kinRob->hasRobotNode(targetJointName) /*&& dynamicsRobot->getDynamicsRobotNode(targetJointName)*/)
        {
            if (isStatic or kinRob->isPassive())
            {
                RobotNodePtr rn = kinRob->getRobotNode(targetJointName);
                BulletRobot* br = dynamic_cast<BulletRobot*>(dynamicsRobot.get());
                if (br && br->hasLink(rn))
                {
                    kinRob->setJointValue(rn, targetJoint);
                    BulletRobot::LinkInfo link = br->getLink(rn);
                    RobotNodePtr nodeCloneA;
                    RobotNodePtr nodeCloneB;
 
                    if (link.nodeA && link.dynNode1)
                    {
                        nodeCloneA = kinRob->getRobotNode(link.nodeA->getName());
                        link.dynNode1->setPose(nodeCloneA->getGlobalPose());
                    }
                    if (link.nodeB && link.dynNode2)
                    {
                        nodeCloneB = kinRob->getRobotNode(link.nodeB->getName());
                        link.dynNode1->setPose(nodeCloneB->getGlobalPose());
                    }
                }
            }
            else
            {
                VR_ASSERT(not kinRob->isPassive());
                dynamicsRobot->actuateNode(targetJointName,
                                           static_cast<double>(targetJoint));
            }
        }
        else
        {
            ARMARX_WARNING_S << deactivateSpam(5, targetJointName) << "No node with name " << targetJointName;
        }
    }
}
 
void BulletPhysicsWorld::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;
 
    SimDynamics::DynamicsRobotPtr dynamicsRobot = getDynamicRobot(robotName);
    if (!dynamicsRobot)
    {
        ARMARX_WARNING_S << "No robot available";
        return;
    }
 
    RobotPtr kinRob = dynamicsRobot->getRobot();
 
    if (!kinRob || kinRob->getName() != robotName)
    {
        ARMARX_WARNING_S << "No robot available with name " << robotName;
        return;
    }
 
    // store velocities for static robots
    DynamicsRobotInfo& ri = dynamicRobots[robotName];
    if (ri.isStatic)
    {
        // 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;
    }
    else
    {
 
        for (const auto& velocitie : velocities)
        {
            // target values
            std::string targetJointName = velocitie.first;
            float targetJointVel = velocitie.second;
 
            if (kinRob->hasRobotNode(targetJointName))
            {
                VR_ASSERT(not kinRob->isPassive());
 
                dynamicsRobot->actuateNodeVel(targetJointName, static_cast<double>(targetJointVel));
            }
            else
            {
                ARMARX_WARNING_S << "No node with name " << targetJointName;
            }
        }
    }
}
 
void BulletPhysicsWorld::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;
 
    SimDynamics::DynamicsRobotPtr dynamicsRobot = getDynamicRobot(robotName);
    if (!dynamicsRobot)
    {
        ARMARX_WARNING_S << "No robot available";
        return;
    }
 
    RobotPtr kinRob = dynamicsRobot->getRobot();
 
    if (!kinRob || kinRob->getName() != robotName)
    {
        ARMARX_WARNING_S << "No robot available with name " << robotName;
        return;
    }
 
    for (const auto& torque : torques)
    {
        // target values
        std::string targetJointName = torque.first;
        float targetJointTorque = torque.second;
 
        if (kinRob->hasRobotNode(targetJointName))
        {
            VR_ASSERT(not kinRob->isPassive());
 
            dynamicsRobot->actuateNodeTorque(targetJointName, static_cast<double>(targetJointTorque));
        }
        else
        {
            ARMARX_WARNING_S << "No node with name " << targetJointName;
        }
    }
}
 
void BulletPhysicsWorld::setRobotPose(const std::string& robotName, const Eigen::Matrix4f& globalPose)
{
    ScopedRecursiveLockPtr lockEngine = getScopedEngineLock(__FUNCTION__);
    ARMARX_DEBUG_S << "setRobotPose:" << globalPose(0, 3) << "," << globalPose(1, 3) << ", " << globalPose(2, 3);
 
    SimDynamics::DynamicsRobotPtr dynamicsRobot = getDynamicRobot(robotName);
    if (!dynamicsRobot)
    {
        ARMARX_WARNING_S << "No robot available";
        return;
    }
 
    RobotPtr kinRob = dynamicsRobot->getRobot();
 
    if (!kinRob || kinRob->getName() != robotName)
    {
        ARMARX_WARNING_S << "No robot available with name " << robotName;
        return;
    }
 
    Eigen::Matrix4f gp = globalPose; // just a hack to convert const to non-const (fixed in newest simox version)
    dynamicsRobot->setGlobalPose(gp);
}
 
void BulletPhysicsWorld::applyForceRobotNode(const std::string& robotName, const std::string& robotNodeName, const Eigen::Vector3f& force)
{
    ScopedRecursiveLockPtr lockEngine = getScopedEngineLock(__FUNCTION__);
    ARMARX_DEBUG_S << "ApplyForce";
 
    SimDynamics::DynamicsRobotPtr dynamicsRobot = getDynamicRobot(robotName);
    if (!dynamicsRobot)
    {
        ARMARX_WARNING_S << "No robot available";
        return;
    }
 
    RobotPtr kinRob = dynamicsRobot->getRobot();
 
    if (!kinRob || kinRob->getName() != robotName)
    {
        ARMARX_WARNING_S << "No robot available with name " << robotName;
        return;
    }
 
    RobotNodePtr rn = kinRob->getRobotNode(robotNodeName);
 
    if (!rn)
    {
        ARMARX_WARNING_S << "No robotnode available with name " << robotNodeName;
        return;
    }
 
    dynamicsRobot->getDynamicsRobotNode(rn)->applyForce(force);
}
 
void BulletPhysicsWorld::applyTorqueRobotNode(const std::string& robotName, const std::string& robotNodeName, const Eigen::Vector3f& torque)
{
    ScopedRecursiveLockPtr lockEngine = getScopedEngineLock(__FUNCTION__);
    ARMARX_DEBUG_S << "ApplyTorque";
 
    SimDynamics::DynamicsRobotPtr dynamicsRobot = getDynamicRobot(robotName);
 
    if (!dynamicsRobot)
    {
        ARMARX_WARNING_S << "No robot available with name " << robotName;
        return;
    }
 
    RobotPtr kinRob = dynamicsRobot->getRobot();
 
    if (!kinRob || kinRob->getName() != robotName)
    {
        ARMARX_WARNING_S << "No robot available with name " << robotName;
        return;
    }
 
    RobotNodePtr rn = kinRob->getRobotNode(robotNodeName);
 
    if (!rn)
    {
        ARMARX_WARNING_S << "No robotnode available with name " << robotNodeName;
        return;
    }
 
    dynamicsRobot->getDynamicsRobotNode(rn)->applyTorque(torque);
}
 
void BulletPhysicsWorld::applyForceObject(const std::string& objectName, const Eigen::Vector3f& force)
{
    ScopedRecursiveLockPtr lockEngine = getScopedEngineLock(__FUNCTION__);
    SimDynamics::DynamicsObjectPtr o = getObject(objectName);
 
    if (!o)
    {
        ARMARX_WARNING_S << "No object found with name " << objectName;
        return;
    }
 
    o->applyForce(force);
}
 
void BulletPhysicsWorld::applyTorqueObject(const std::string& objectName, const Eigen::Vector3f& torque)
{
    ScopedRecursiveLockPtr lockEngine = getScopedEngineLock(__FUNCTION__);
    SimDynamics::DynamicsObjectPtr o = getObject(objectName);
 
    if (!o)
    {
        ARMARX_WARNING_S << "No object found with name " << objectName;
        return;
    }
 
    o->applyTorque(torque);
}
 
 
SimDynamics::DynamicsObjectPtr BulletPhysicsWorld::getObject(const std::string& objectName)
{
    ScopedRecursiveLockPtr lockEngine = getScopedEngineLock(__FUNCTION__);
    SimDynamics::DynamicsObjectPtr o;
    std::vector<SimDynamics::DynamicsObjectPtr>::const_iterator it;
 
    for (it = dynamicsObjects.begin(); it != dynamicsObjects.end(); it++)
    {
        if ((*it)->getName() == objectName)
        {
            o = *it;
            break;
        }
    }
 
    return o;
}
 
bool BulletPhysicsWorld::hasObject(const std::string& instanceName)
{
    return getObject(instanceName).get();
}
 
void BulletPhysicsWorld::setObjectPose(const std::string& objectName, const Eigen::Matrix4f& globalPose)
{
    ScopedRecursiveLockPtr lockEngine = getScopedEngineLock(__FUNCTION__);
    SimDynamics::DynamicsObjectPtr o = getObject(objectName);
 
    if (!o)
    {
        ARMARX_WARNING_S << "No object found with name " << objectName;
        return;
    }
 
    o->setPose(globalPose);
}
 
void BulletPhysicsWorld::activateObject(const std::string& objectName)
{
    ScopedRecursiveLockPtr lockEngine = getScopedEngineLock(__FUNCTION__);
    SimDynamics::DynamicsObjectPtr o = getObject(objectName);
 
    if (!o)
    {
        ARMARX_WARNING_S << "No object found with name " << objectName;
        return;
    }
 
    // need newest simox version for this (2.3.48)
    //o->activate();
 
    // this works for earlier versions but activates all objects
    bulletEngine->activateAllObjects(); // avoid sleeping objects
}
 
 
float BulletPhysicsWorld::getRobotMass(const std::string& robotName)
{
    ScopedRecursiveLockPtr lockEngine = getScopedEngineLock(__FUNCTION__);
 
    SimDynamics::DynamicsRobotPtr dynamicsRobot = getDynamicRobot(robotName);
    if (!dynamicsRobot)
    {
        ARMARX_WARNING_S << "No robot available";
        return 0;
    }
 
    RobotPtr kinRob = dynamicsRobot->getRobot();
 
    if (!kinRob || kinRob->getName() != robotName)
    {
        ARMARX_WARNING_S << "No robot available with name " << robotName;
        return 0;
    }
 
    return kinRob->getMass();
}
 
std::map< std::string, float> BulletPhysicsWorld::getRobotJointAngles(const std::string& robotName)
{
    ScopedRecursiveLockPtr lockEngine = getScopedEngineLock(__FUNCTION__);
    std::map< std::string, float> res;
 
    SimDynamics::DynamicsRobotPtr dynamicsRobot = getDynamicRobot(robotName);
    if (!dynamicsRobot)
    {
        ARMARX_WARNING_S << "No robot available";
        return res;
    }
 
    RobotPtr kinRob = dynamicsRobot->getRobot();
 
    if (!kinRob || kinRob->getName() != robotName)
    {
        ARMARX_WARNING_S << "No robot available with name " << robotName;
        return res;
    }
 
    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 BulletPhysicsWorld::getRobotJointAngle(const std::string& robotName, const std::string& nodeName)
{
    ScopedRecursiveLockPtr lockEngine = getScopedEngineLock(__FUNCTION__);
 
    SimDynamics::DynamicsRobotPtr dynamicsRobot = getDynamicRobot(robotName);
    if (!dynamicsRobot)
    {
        ARMARX_WARNING_S << "No robot available";
        return 0;
    }
 
    RobotPtr kinRob = dynamicsRobot->getRobot();
 
    if (!kinRob || kinRob->getName() != robotName)
    {
        ARMARX_WARNING_S << "No robot available with name " << robotName;
        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> BulletPhysicsWorld::getRobotJointVelocities(const std::string& robotName)
{
    ScopedRecursiveLockPtr lockEngine = getScopedEngineLock(__FUNCTION__);
    std::map< std::string, float> res;
 
    SimDynamics::DynamicsRobotPtr dynamicsRobot = getDynamicRobot(robotName);
    if (!dynamicsRobot)
    {
        ARMARX_WARNING_S << "No robot available";
        return res;
    }
 
    RobotPtr kinRob = dynamicsRobot->getRobot();
 
    if (!kinRob || kinRob->getName() != robotName)
    {
        ARMARX_WARNING_S << "No robot available with name " << robotName;
        return res;
    }
 
    std::vector<RobotNodePtr> rn = kinRob->getRobotNodes();
 
    for (auto& i : rn)
    {
        if (i->isJoint())
        {
            float vel = dynamicsRobot->getJointSpeed(i);
            res[i->getName()] = vel;
        }
    }
 
    ARMARX_DEBUG_S << "getRobotJointVelocities:" << res.begin()->second;
    return res;
}
 
float BulletPhysicsWorld::getRobotJointVelocity(const std::string& robotName, const std::string& nodeName)
{
    ScopedRecursiveLockPtr lockEngine = getScopedEngineLock(__FUNCTION__);
 
    SimDynamics::DynamicsRobotPtr dynamicsRobot = getDynamicRobot(robotName);
    if (!dynamicsRobot)
    {
        ARMARX_WARNING_S << "No robot available";
        return 0;
    }
 
    RobotPtr kinRob = dynamicsRobot->getRobot();
 
    if (!kinRob || kinRob->getName() != robotName)
    {
        ARMARX_WARNING_S << "No robot available with name " << robotName;
        return 0;
    }
 
    if (!kinRob->hasRobotNode(nodeName))
    {
        ARMARX_WARNING_S << "No robotNode available with name " << nodeName;
        return 0;
    }
 
    double res = dynamicsRobot->getJointSpeed(kinRob->getRobotNode(nodeName));
    ARMARX_DEBUG_S << "getRobotJointVelocity:" << res;
    return static_cast<float>(res);
}
 
std::map<std::string, float> BulletPhysicsWorld::getRobotJointTorques(const std::string& robotName)
{
    ScopedRecursiveLockPtr lockEngine = getScopedEngineLock(__FUNCTION__);
    std::map< std::string, float> res;
 
    SimDynamics::DynamicsRobotPtr dynamicsRobot = getDynamicRobot(robotName);
    if (!dynamicsRobot)
    {
        ARMARX_WARNING_S << "No robot available";
        return res;
    }
 
    VirtualRobot::RobotPtr kinRob = dynamicsRobot->getRobot();
 
    if (!kinRob || kinRob->getName() != robotName)
    {
        ARMARX_WARNING_S << "No robot available with name " << robotName;
        return res;
    }
 
    SimDynamics::BulletRobot* br = dynamic_cast<BulletRobot*>(dynamicsRobot.get());
    if (!br)
    {
        ARMARX_WARNING_S << "No bullet robot available with name " << robotName;
        return res;
    }
    std::vector<RobotNodePtr> rn = kinRob->getRobotNodes();
 
    for (auto& i : rn)
    {
        if (i->isJoint() && br)
        {
            if (dynamicsRobot->isNodeActuated(i))
            {
                res[i->getName()] = br->getJointTorque(i);
            }
        }
    }
    return res;
}
 
ForceTorqueDataSeq BulletPhysicsWorld::getRobotForceTorqueSensors(const std::string& robotName)
{
    ScopedRecursiveLockPtr lockEngine = getScopedEngineLock(__FUNCTION__);
    DynamicsRobotPtr dynamicsRobot = getDynamicRobot(robotName);
    RobotPtr virtualRobot = getRobot(robotName);
    if (!dynamicsRobot || !virtualRobot)
    {
        static bool printed = false;
        if (!printed)
        {
            ARMARX_WARNING_S << "No robot available with name " << robotName;
            printed = true;
        }
    }
    const auto ftsensors = virtualRobot->getSensors<ForceTorqueSensor>();
    ForceTorqueDataSeq result;
    result.reserve(ftsensors.size());
    for (const VirtualRobot::ForceTorqueSensorPtr& sensor : ftsensors)
    {
        ForceTorqueData data;
        Eigen::Vector3f force = sensor->getForce();
        Eigen::Vector3f torque = sensor->getTorque();
        data.nodeName = sensor->getRobotNode()->getName();
        data.sensorName = sensor->getName();
 
        if (!robotName.empty() && !data.nodeName.empty())
        {
            RobotNodePtr rn = dynamicsRobot->getRobot()->getRobotNode(data.nodeName);
            Eigen::Vector3f forceGlobal = force;
            Eigen::Vector3f torqueGlobal = torque;
            if (rn)
            {
                Eigen::Matrix3f rotMat = rn->getGlobalPose().block(0, 0, 3, 3);
                force = rotMat.inverse() * forceGlobal;
                torque = rotMat.inverse() * torqueGlobal;
            }
        }
        data.force  = new Vector3 {force};
        data.torque = new Vector3 {torque};
 
        result.emplace_back(std::move(data));
    }
    return result;
}
 
float BulletPhysicsWorld::getRobotJointLimitLo(const std::string& robotName, const std::string& nodeName)
{
    ScopedRecursiveLockPtr lockEngine = getScopedEngineLock(__FUNCTION__);
 
    SimDynamics::DynamicsRobotPtr dynamicsRobot = getDynamicRobot(robotName);
    if (!dynamicsRobot)
    {
        ARMARX_WARNING_S << "No robot available";
        return 0;
    }
 
    RobotPtr kinRob = dynamicsRobot->getRobot();
 
    if (!kinRob || kinRob->getName() != robotName)
    {
        ARMARX_WARNING_S << "No robot available with name " << robotName;
        return 0;
    }
 
    if (!kinRob->hasRobotNode(nodeName))
    {
        ARMARX_WARNING_S << "No robotNode available with name " << nodeName;
        return 0;
    }
 
    return kinRob->getRobotNode(nodeName)->getJointLimitLo();
}
 
float BulletPhysicsWorld::getRobotJointLimitHi(const std::string& robotName, const std::string& nodeName)
{
    ScopedRecursiveLockPtr lockEngine = getScopedEngineLock(__FUNCTION__);
 
    SimDynamics::DynamicsRobotPtr dynamicsRobot = getDynamicRobot(robotName);
    if (!dynamicsRobot)
    {
        ARMARX_WARNING_S << "No robot available";
        return 0;
    }
 
    RobotPtr kinRob = dynamicsRobot->getRobot();
 
    if (!kinRob || kinRob->getName() != robotName)
    {
        ARMARX_WARNING_S << "No robot available with name " << robotName;
        return 0;
    }
 
    if (!kinRob->hasRobotNode(nodeName))
    {
        ARMARX_WARNING_S << "No robotNode available with name " << nodeName;
        return 0;
    }
 
    return kinRob->getRobotNode(nodeName)->getJointLimitHi();
}
 
Eigen::Matrix4f BulletPhysicsWorld::getRobotPose(const std::string& robotName)
{
    ScopedRecursiveLockPtr lockEngine = getScopedEngineLock(__FUNCTION__);
    ARMARX_DEBUG_S << "get robot pose";
 
    SimDynamics::DynamicsRobotPtr dynamicsRobot = getDynamicRobot(robotName);
    if (!dynamicsRobot)
    {
        ARMARX_WARNING_S << "No robot available";
        return Eigen::Matrix4f::Identity();
    }
 
    RobotPtr kinRob = dynamicsRobot->getRobot();
 
    if (!kinRob || kinRob->getName() != robotName)
    {
        ARMARX_WARNING_S << "No robot available with name " << robotName;
 
        if (kinRob)
        {
            ARMARX_WARNING_S << "available robot:" << kinRob->getName();
        }
 
        return Eigen::Matrix4f::Identity();
    }
 
    return kinRob->getGlobalPose();
}
 
float BulletPhysicsWorld::getRobotMaxTorque(const std::string& robotName, const std::string& nodeName)
{
    ScopedRecursiveLockPtr lockEngine = getScopedEngineLock(__FUNCTION__);
    ARMARX_DEBUG_S << "getRobotMaxTorque";
 
    SimDynamics::DynamicsRobotPtr dynamicsRobot = getDynamicRobot(robotName);
    if (!dynamicsRobot)
    {
        ARMARX_WARNING_S << "Robot '" << robotName << "' does not exist";
        return 0.0f;
    }
 
    RobotPtr kinRob = dynamicsRobot->getRobot();
 
    if (!kinRob || kinRob->getName() != robotName)
    {
        ARMARX_WARNING_S << "No robot available with name " << robotName;
 
        if (kinRob)
        {
            ARMARX_WARNING_S << "available robot:" << kinRob->getName();
        }
 
        return 0.0f;
    }
 
    if (!kinRob->hasRobotNode(nodeName))
    {
        ARMARX_WARNING_S << "Robot node '" << nodeName << "' does not exist";
        return 0.0f;
    }
 
    return kinRob->getRobotNode(nodeName)->getMaxTorque();
}
 
void BulletPhysicsWorld::setRobotMaxTorque(const std::string& robotName, const std::string& nodeName, float maxTorque)
{
    ScopedRecursiveLockPtr lockEngine = getScopedEngineLock(__FUNCTION__);
    ARMARX_DEBUG_S << "setRobotMaxTorque";
 
    SimDynamics::DynamicsRobotPtr dynamicsRobot = getDynamicRobot(robotName);
    if (!dynamicsRobot)
    {
        ARMARX_WARNING_S << "Robot '" << robotName << "' does not exist";
        return;
    }
 
    RobotPtr kinRob = dynamicsRobot->getRobot();
 
    if (!kinRob || kinRob->getName() != robotName)
    {
        ARMARX_WARNING_S << "No robot available with name " << robotName;
 
        if (kinRob)
        {
            ARMARX_WARNING_S << "available robot:" << kinRob->getName();
        }
 
        return;
    }
 
    if (!kinRob->hasRobotNode(nodeName))
    {
        ARMARX_WARNING_S << "Robot node '" << nodeName << "' does not exist";
        return;
    }
 
    kinRob->getRobotNode(nodeName)->setMaxTorque(maxTorque);
}
 
Eigen::Matrix4f BulletPhysicsWorld::getRobotNodePose(const std::string& robotName, const std::string& robotNodeName)
{
    ScopedRecursiveLockPtr lockEngine = getScopedEngineLock(__FUNCTION__);
    ARMARX_DEBUG_S << "get robot node pose";
 
    SimDynamics::DynamicsRobotPtr dynamicsRobot = getDynamicRobot(robotName);
    if (!dynamicsRobot)
    {
        ARMARX_WARNING_S << "No robot available with name '" << robotName << "'";
        return Eigen::Matrix4f::Identity();
    }
 
    RobotPtr kinRob = dynamicsRobot->getRobot();
 
    if (!kinRob || kinRob->getName() != robotName)
    {
        ARMARX_WARNING_S << "No robot available with name " << robotName;
        return Eigen::Matrix4f::Identity();
    }
 
    if (!kinRob->hasRobotNode(robotNodeName))
    {
        ARMARX_WARNING_S << "No robotNode available with name " << robotNodeName;
        return Eigen::Matrix4f::Identity();
    }
 
    return kinRob->getRobotNode(robotNodeName)->getGlobalPose();
}
 
 
Eigen::Vector3f BulletPhysicsWorld::getRobotLinearVelocity(const std::string& robotName, const std::string& nodeName)
{
    ScopedRecursiveLockPtr lockEngine = getScopedEngineLock(__FUNCTION__);
    ARMARX_DEBUG_S << "get robot linear velocity";
 
    DynamicsObjectPtr dynamicsRobotNode = this->getFirstDynamicsObject(robotName, nodeName);
 
    if (!dynamicsRobotNode)
    {
        return Eigen::Vector3f::Identity();
    }
    return dynamicsRobotNode->getLinearVelocity();
}
 
 
SimDynamics::DynamicsObjectPtr BulletPhysicsWorld::getFirstDynamicsObject(const std::string& robotName, const std::string& nodeName)
{
    SimDynamics::DynamicsRobotPtr dynamicsRobot = getDynamicRobot(robotName);
    if (!dynamicsRobot)
    {
        Ice::StringSeq robots;
        for (auto& elem : getRobots())
        {
            robots.push_back(elem.first);
        }
 
        ARMARX_WARNING_S << deactivateSpam(1) << "No robot available with name '" << robotName << "' - available robots: " << robots;
 
        return {};
 
    }
 
    RobotPtr kinRob = dynamicsRobot->getRobot();
 
    if (!kinRob || kinRob->getName() != robotName)
    {
        ARMARX_WARNING_S << "No robot available with name " << robotName;
        return {};
    }
 
    if (!kinRob->hasRobotNode(nodeName))
    {
        ARMARX_WARNING_S << "No robotNode available with name " << nodeName;
        return {};
    }
 
 
    RobotNodePtr robotNode = kinRob->getRobotNode(nodeName);
    DynamicsObjectPtr dynamicsRobotNode = dynamicsRobot->getDynamicsRobotNode(robotNode);
    // search first parent with dynamic object
 
    while (!dynamicsRobotNode)
    {
        robotNode = std::dynamic_pointer_cast<RobotNode>(robotNode->getParent());
        if (!robotNode)
        {
            ARMARX_WARNING_S << "unable to get parent robot node";
            return {};
 
        }
        dynamicsRobotNode = dynamicsRobot->getDynamicsRobotNode(robotNode);
    }
 
    return dynamicsRobotNode;
}
 
 
Eigen::Vector3f BulletPhysicsWorld::getRobotAngularVelocity(const std::string& robotName, const std::string& nodeName)
{
    ScopedRecursiveLockPtr lockEngine = getScopedEngineLock(__FUNCTION__);
    ARMARX_DEBUG_S << "get robot angular vel";
 
    DynamicsObjectPtr dynamicsRobotNode = this->getFirstDynamicsObject(robotName, nodeName);
 
    if (!dynamicsRobotNode)
    {
        return Eigen::Vector3f::Identity();
    }
 
    return dynamicsRobotNode->getAngularVelocity();
}
 
void BulletPhysicsWorld::setRobotLinearVelocity(const std::string& robotName, const std::string& robotNodeName, const Eigen::Vector3f& vel)
{
    ScopedRecursiveLockPtr lockEngine = getScopedEngineLock(__FUNCTION__);
    ARMARX_DEBUG_S << "set robot lin vel";
 
    SimDynamics::DynamicsRobotPtr dynamicsRobot = getDynamicRobot(robotName);
    if (!dynamicsRobot)
    {
        ARMARX_WARNING_S << "No robot available";
        return;
    }
 
    RobotPtr kinRob = dynamicsRobot->getRobot();
 
    if (!kinRob || kinRob->getName() != robotName)
    {
        ARMARX_WARNING_S << "No robot available with name " << robotName;
        return;
    }
 
    if (!kinRob->hasRobotNode(robotNodeName))
    {
        ARMARX_WARNING_S << "No robotNode available with name " << robotNodeName;
        return;
    }
 
    dynamicsRobot->getDynamicsRobotNode(kinRob->getRobotNode(robotNodeName))->setLinearVelocity(vel);
    ARMARX_DEBUG_S << "set robot lin vel end";
}
 
void BulletPhysicsWorld::setRobotAngularVelocity(const std::string& robotName, const std::string& robotNodeName, const Eigen::Vector3f& vel)
{
    ScopedRecursiveLockPtr lockEngine = getScopedEngineLock(__FUNCTION__);
    ARMARX_DEBUG_S << "set robot ang vel";
 
    SimDynamics::DynamicsRobotPtr dynamicsRobot = getDynamicRobot(robotName);
    if (!dynamicsRobot)
    {
        ARMARX_WARNING_S << "No robot available";
        return;
    }
 
    RobotPtr kinRob = dynamicsRobot->getRobot();
 
    if (!kinRob || kinRob->getName() != robotName)
    {
        ARMARX_WARNING_S << "No robot available with name " << robotName;
        return;
    }
 
    if (!kinRob->hasRobotNode(robotNodeName))
    {
        ARMARX_WARNING_S << "No robotNode available with name " << robotNodeName;
        return;
    }
 
    dynamicsRobot->getDynamicsRobotNode(kinRob->getRobotNode(robotNodeName))->setAngularVelocity(vel);
    ARMARX_DEBUG_S << "set robot ang vel end";
}
 
void BulletPhysicsWorld::setRobotLinearVelocityRobotRootFrame(const std::string& robotName, const std::string& robotNodeName, const Eigen::Vector3f& vel)
{
    ScopedRecursiveLockPtr lockEngine = getScopedEngineLock(__FUNCTION__);
    ARMARX_DEBUG_S << "set robot lin vel";
 
    SimDynamics::DynamicsRobotPtr dynamicsRobot = getDynamicRobot(robotName);
    if (!dynamicsRobot)
    {
        ARMARX_WARNING_S << "No robot available";
        return;
    }
 
    RobotPtr kinRob = dynamicsRobot->getRobot();
 
    if (!kinRob || kinRob->getName() != robotName)
    {
        ARMARX_WARNING_S << "No robot available with name '" << robotName << "'";
        return;
    }
 
    if (!kinRob->hasRobotNode(robotNodeName))
    {
        ARMARX_WARNING_S << "No robotNode available with name '" << robotNodeName << "'";
        return;
    }
 
    if (!dynamicsRobot->hasDynamicsRobotNode(kinRob->getRobotNode(robotNodeName)))
    {
        ARMARX_WARNING_S << "No dynamic robotNode available with name '" << robotNodeName << "'";
        return;
    }
 
    Eigen::Matrix4f newPose;
    newPose.setIdentity();
    newPose.block<3, 1>(0, 3) = vel;
    auto globalPose = dynamicsRobot->getRobot()->getGlobalPose();
    globalPose(0, 3) = 0;
    globalPose(1, 3) = 0;
    globalPose(2, 3) = 0;
    auto globalNewPose = globalPose * newPose;
    dynamicsRobot->getDynamicsRobotNode(kinRob->getRobotNode(robotNodeName))->setLinearVelocity(globalNewPose.block<3, 1>(0, 3));
    ARMARX_DEBUG_S << "set robot lin vel end";
}
 
void BulletPhysicsWorld::setRobotAngularVelocityRobotRootFrame(const std::string& robotName, const std::string& robotNodeName, const Eigen::Vector3f& vel)
{
    ScopedRecursiveLockPtr lockEngine = getScopedEngineLock(__FUNCTION__);
    ARMARX_DEBUG_S << "set robot ang vel";
 
    SimDynamics::DynamicsRobotPtr dynamicsRobot = getDynamicRobot(robotName);
    if (!dynamicsRobot)
    {
        ARMARX_WARNING_S << "No robot available";
        return;
    }
 
    RobotPtr kinRob = dynamicsRobot->getRobot();
 
    if (!kinRob || kinRob->getName() != robotName)
    {
        ARMARX_WARNING_S << "No robot available with name '" << robotName << "'";
        return;
    }
 
    if (!kinRob->hasRobotNode(robotNodeName))
    {
        ARMARX_WARNING_S << "No robotNode available with name '" << robotNodeName << "'";
        return;
    }
 
    if (!dynamicsRobot->hasDynamicsRobotNode(kinRob->getRobotNode(robotNodeName)))
    {
        ARMARX_WARNING_S << "No dynamic robotNode available with name '" << robotNodeName << "'";
        return;
    }
 
    Eigen::Matrix4f newPose;
    newPose.setIdentity();
    newPose.block<3, 1>(0, 3) = vel;
    auto globalPose = dynamicsRobot->getRobot()->getGlobalPose();
    globalPose(0, 3) = 0;
    globalPose(1, 3) = 0;
    globalPose(2, 3) = 0;
    auto globalNewPose = globalPose * newPose;
 
    RobotNodePtr robotNode = kinRob->getRobotNode(robotNodeName);
    DynamicsObjectPtr dynamicsRobotNode = dynamicsRobot->getDynamicsRobotNode(robotNode);
    dynamicsRobotNode->setAngularVelocity(globalNewPose.block<3, 1>(0, 3));
}
 
bool BulletPhysicsWorld::hasRobot(const std::string& robotName)
{
    ScopedRecursiveLockPtr lockEngine = getScopedEngineLock(__FUNCTION__);
 
    std::map<std::string, DynamicsRobotInfo>::iterator it = dynamicRobots.begin();
    while (it != dynamicRobots.end())
    {
        if (it->first == robotName)
        {
            return true;
        }
        it++;
    }
 
    return false;
}
 
 
bool BulletPhysicsWorld::hasRobotNode(const std::string& robotName, const std::string& robotNodeName)
{
    ScopedRecursiveLockPtr lockEngine = getScopedEngineLock(__FUNCTION__);
 
    SimDynamics::DynamicsRobotPtr dynRob = getDynamicRobot(robotName);
    if (!dynRob)
    {
        return false;
    }
 
    return (dynRob->getRobot()->hasRobotNode(robotNodeName));
}
 
 
VirtualRobot::RobotPtr BulletPhysicsWorld::getRobot(const std::string& robotName)
{
    ScopedRecursiveLockPtr lockEngine = getScopedEngineLock(__FUNCTION__);
    SimDynamics::DynamicsRobotPtr r = getDynamicRobot(robotName);
    if (!r)
    {
        return VirtualRobot::RobotPtr();
    }
 
    return r->getRobot();
}
 
std::map<std::string, RobotPtr> BulletPhysicsWorld::getRobots()
{
    std::map<std::string, RobotPtr> result;
    ScopedRecursiveLockPtr lockEngine = getScopedEngineLock(__FUNCTION__);
    for (auto& elem : dynamicRobots)
    {
        const DynamicsRobotInfo& info = elem.second;
        result[elem.first] = (info.robot);
    }
    return result;
}
 
SimDynamics::DynamicsRobotPtr BulletPhysicsWorld::getDynamicRobot(const std::string& robotName)
{
    ScopedRecursiveLockPtr lockEngine = getScopedEngineLock(__FUNCTION__);
 
    if (!hasRobot(robotName))
    {
        return SimDynamics::DynamicsRobotPtr();
    }
 
    return dynamicRobots[robotName].dynamicsRobot;
}
 
SimDynamics::DynamicsObjectPtr BulletPhysicsWorld::getDynamicsObject(const std::string& objectName)
{
    SimDynamics::DynamicsObjectPtr o;
    std::vector<SimDynamics::DynamicsObjectPtr>::const_iterator it;
 
    for (it = dynamicsObjects.begin(); it != dynamicsObjects.end(); it++)
    {
        if ((*it)->getName() == objectName)
        {
            o = *it;
            break;
        }
    }
 
    return o;
}
 
Eigen::Matrix4f BulletPhysicsWorld::getObjectPose(const std::string& objectName)
{
    ScopedRecursiveLockPtr lockEngine = getScopedEngineLock(__FUNCTION__);
    ARMARX_DEBUG_S << "get object pose";
    SimDynamics::DynamicsObjectPtr o = getDynamicsObject(objectName);
 
    if (!o)
    {
        ARMARX_WARNING_S << "No object found with name " << objectName;
        return Eigen::Matrix4f::Identity();
    }
 
    return o->getSceneObject()->getGlobalPose();
}
 
bool BulletPhysicsWorld::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;
    SimDynamics::DynamicsRobotPtr dynamicsRobot = getDynamicRobot(robotName);
    if (!dynamicsRobot)
    {
        ARMARX_WARNING_S << "No robot available with name " << robotName;
        return false;
    }
    RobotPtr kinRob = dynamicsRobot->getRobot();
 
    if (!kinRob || kinRob->getName() != robotName)
    {
        ARMARX_WARNING_S << "No robot available with name " << robotName;
        return false;
    }
 
    if (!kinRob->hasRobotNode(robotNodeName))
    {
        ARMARX_WARNING_S << "No robotNode available with name " << robotNodeName;
        return false;
    }
 
    SimDynamics::DynamicsObjectPtr o = getObject(objectName);
 
    if (!o)
    {
        ARMARX_WARNING_S << "No object found with name " << objectName;
        return false;
    }
 
    bool res = dynamicsWorld->getEngine()->detachObjectFromRobot(robotName, o);
 
    return res;
}
 
bool BulletPhysicsWorld::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;
        }
    }
    SimDynamics::DynamicsRobotPtr dynamicsRobot = getDynamicRobot(robotName);
    RobotPtr kinRob = dynamicsRobot->getRobot();
 
    if (!kinRob || kinRob->getName() != robotName)
    {
        ARMARX_WARNING_S << "No robot available with name " << robotName;
        return false;
    }
 
    if (!kinRob->hasRobotNode(robotNodeName))
    {
        ARMARX_WARNING_S << "No robotNode available with name " << robotNodeName;
        return false;
    }
 
    RobotNodePtr rn = kinRob->getRobotNode(robotNodeName);
 
 
    SimDynamics::DynamicsObjectPtr o = getObject(objectName);
 
    if (!o)
    {
        ARMARX_WARNING_S << "No object found with name " << objectName;
        return false;
    }
 
    bool res = dynamicsWorld->getEngine()->attachObjectToRobot(robotName, robotNodeName, o);
    storeLocalTransform = rn->toLocalCoordinateSystem(o->getSceneObject()->getGlobalPose());
    return res;
}
 
 
SceneObjectPtr BulletPhysicsWorld::getFloor()
{
    if (!dynamicsWorld || !dynamicsWorld->getEngine()->getFloor())
    {
        return SceneObjectPtr();
    }
    return dynamicsWorld->getEngine()->getFloor()->getSceneObject();
}
 
 
 
bool BulletPhysicsWorld::removeRobotEngine(const std::string& robotName)
{
    // lock engine and data
    ScopedRecursiveLockPtr lockEngine = getScopedEngineLock(__FUNCTION__);
 
    SimDynamics::DynamicsRobotPtr dynamicsRobot = getDynamicRobot(robotName);
    if (!dynamicsWorld || !dynamicsRobot)
    {
        return false;
    }
 
    ARMARX_DEBUG_S << "Removing robot " << robotName;
 
 
    //access engine
    {
        if (robotName != dynamicsRobot->getRobot()->getName())
        {
            ARMARX_WARNING_S << "Robot names differ...";
        }
 
        dynamicsWorld->removeRobot(dynamicsRobot);
        dynamicRobots.erase(robotName);
    }
 
    return true;
}
 
 
bool BulletPhysicsWorld::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 (!dynamicsWorld)
    {
        ARMARX_ERROR_S << "No physics world!";
        return false;
    }
 
    if (hasRobot(robot->getName()))
    {
        ARMARX_ERROR_S << "More than one robot with identical name is currently not supported!";
        return false;
    }
 
    DynamicsRobotInfo robInfo;
    if (staticRobot)
    {
        ARMARX_INFO_S << "Building static robot...";
        std::vector< RobotNodePtr > nodes = robot->getRobotNodes();
        for (auto n : nodes)
        {
            n->setSimulationType(SceneObject::Physics::eKinematic);
        }
    }
 
    SimDynamics::DynamicsRobotPtr dynamicsRobot;
    try
    {
        dynamicsRobot = dynamicsWorld->CreateDynamicsRobot(robot);
        ARMARX_DEBUG_S << "add dynamicsRobot" << dynamicsRobot->getName();
 
        auto& controllers = dynamicsRobot->getControllers();
        std::vector<RobotNodePtr> nodes = robot->getRobotNodes();
 
        if (not robot->isPassive())
        {
            ARMARX_INFO_S << "Configuring controllers for robot " << dynamicsRobot->getName();
 
            for (auto node : nodes)
            {
                if (node->isRotationalJoint())
                {
                    if (controllers.find(node) == controllers.end())
                    {
                        controllers[node] = VelocityMotorController(
                                                static_cast<double>(node->getMaxVelocity()),
                                                static_cast<double>(node->getMaxAcceleration()));
                    }
                    controllers[node].reset(pid_p, pid_i, pid_d);
                }
            }
        }
 
        ARMARX_INFO_S << "The robot " << dynamicsRobot->getName() << " has " << controllers.size() << " controllers";
 
        if (not selfCollisions)
        {
            dynamicsRobot->enableSelfCollisions(false);    // do not pass the value selfCollisions in here, because it would reset the specifically set ingored-collisions.
        }
 
        dynamicsWorld->addRobot(dynamicsRobot);
        ARMARX_DEBUG_S << "add dynamicsRobot2 " << dynamicsRobot->getName();
    }
    catch (VirtualRobotException& e)
    {
        ARMARX_ERROR_S << " ERROR while building dynamic robot";
        ARMARX_ERROR_S << e.what();
        return false;
    }
 
 
 
    // since we do not visualize this robot we can skip visualization updates
    dynamicsRobot->getRobot()->setThreadsafe(false); // we already got a mutex protection
    dynamicsRobot->getRobot()->setUpdateVisualization(false);
 
 
    robInfo.isStatic = staticRobot;
    robInfo.robot = robot;
    robInfo.dynamicsRobot = dynamicsRobot;
 
    dynamicRobots[robot->getName()] = robInfo;
 
    return true;
}
 
armarx::FramedPosePtr BulletPhysicsWorld::toFramedPose(const Eigen::Matrix4f& globalPose, const std::string& robotName, const std::string& frameName)
{
    ScopedRecursiveLockPtr lockEngine = getScopedEngineLock(__FUNCTION__);
 
    SimDynamics::DynamicsRobotPtr dynamicsRobot = getDynamicRobot(robotName);
 
    if (!dynamicsRobot || dynamicsRobot->getRobot()->getName() != robotName)
    {
        ARMARX_WARNING_S << "No robot or wrong name: " << robotName;
        return FramedPosePtr();
    }
 
    RobotNodePtr rn = dynamicsRobot->getRobot()->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, dynamicsRobot->getRobot()->getName());
    return framedPose;
}
 
bool BulletPhysicsWorld::synchronizeRobotNodePoses(const std::string& robotName, std::map<std::string, armarx::PoseBasePtr>& objMap)
{
    // lock engine and data
    ScopedRecursiveLockPtr lockEngine = getScopedEngineLock(__FUNCTION__);
    ScopedRecursiveLockPtr lockSync = getScopedSyncLock(__FUNCTION__);
 
    SimDynamics::DynamicsRobotPtr dynamicsRobot = getDynamicRobot(robotName);
    // update visu data
    if (!dynamicsRobot || robotName != dynamicsRobot->getName())
    {
        //ARMARX_ERROR_S << "no robot data";
        return false;
    }
 
    VirtualRobot::RobotPtr kinRobEngine = dynamicsRobot->getRobot();
    VirtualRobot::SceneObjectPtr currentObjEngine = kinRobEngine->getRootNode();
 
    if (!synchronizeSceneObjectPoses(currentObjEngine, objMap))
    {
        ARMARX_ERROR_S << "Failed to synchronize objects...";
        return false;
    }
    return true;
}
 
bool BulletPhysicsWorld::getRobotStatus(const std::string& robotName, NameValueMap& jointAngles, NameValueMap& jointVelocities, NameValueMap& jointTorques, Eigen::Vector3f& linearVelocity, Eigen::Vector3f& angularVelocity)
{
    ScopedRecursiveLockPtr lockEngine = getScopedEngineLock(__FUNCTION__);
    SimDynamics::DynamicsRobotPtr dynamicsRobot = getDynamicRobot(robotName);
 
    if (!dynamicsRobot || robotName != dynamicsRobot->getName())
    {
        return false;
    }
 
    double simTimeFactor = 1.0;
    if (simStepExecutionDurationMS > 0)
    {
        simTimeFactor = static_cast<double>(simTimeStepMS / simStepExecutionDurationMS);
    }
 
    VirtualRobot::RobotPtr kinRob = dynamicsRobot->getRobot();
 
    SimDynamics::BulletRobotPtr br = std::dynamic_pointer_cast<BulletRobot>(dynamicsRobot);
    std::vector<RobotNodePtr> rn = kinRob->getRobotNodes();
 
    for (auto& i : rn)
    {
        if (i->isJoint())
        {
            jointAngles[i->getName()] = i->getJointValue();
 
            // TODO: check if isNodeActuated() is needed as well as else clause.
            //       jointSpeed should also be valid / meaningful for a passive robot
            if (dynamicsRobot->isNodeActuated(i))
            {
                jointVelocities[i->getName()] = dynamicsRobot->getJointSpeed(i) * simTimeFactor;
 
                if (br)
                {
                    jointTorques[i->getName()] = br->getJointTorque(i);
                }
            }
            else
            {
                jointVelocities[i->getName()] = 0;
                jointTorques[i->getName()] = 0;
            }
        }
    }
 
    DynamicsObjectPtr rootDyn = dynamicsRobot->getDynamicsRobotNode(kinRob->getRootNode());
 
    if (rootDyn)
    {
        linearVelocity = rootDyn->getLinearVelocity();
        angularVelocity = rootDyn->getAngularVelocity();
    }
    else
    {
        linearVelocity.setZero();
        angularVelocity.setZero();
    }
 
    return true;
}
 
bool BulletPhysicsWorld::updateForceTorqueSensor(ForceTorqueInfo& ftInfo)
{
    ScopedRecursiveLockPtr lockEngine = getScopedEngineLock(__FUNCTION__);
    SimDynamics::DynamicsRobotPtr dynamicsRobot = getDynamicRobot(ftInfo.robotName);
    if (!dynamicsRobot)
    {
        static bool printed = false;
        if (!printed)
        {
            ARMARX_WARNING_S << "No robot available with name " << ftInfo.robotName;
            printed = true;
        }
 
        return false;
    }
 
    // update force torque info
    //for (size_t i = 0; i < simReportData.robots.at(0).forceTorqueSensors.size(); i++)
    //{
    if (ftInfo.enable)
    {
        ftInfo.currentForce = ftInfo.ftSensor->getForce();
        ftInfo.currentTorque = ftInfo.ftSensor->getTorque();
 
        // transform to local coordinates
        std::string nodeName = ftInfo.robotNodeName;
        std::string robotName = ftInfo.robotName;
 
        if (!robotName.empty() && !nodeName.empty() && dynamicsRobot)
        {
            RobotNodePtr rn = dynamicsRobot->getRobot()->getRobotNode(nodeName);
            Eigen::Vector3f forceLocal;
            Eigen::Vector3f forceGlobal = ftInfo.currentForce;
            Eigen::Vector3f torqueLocal;
            Eigen::Vector3f torqueGlobal = ftInfo.currentTorque;
 
            if (rn)
            {
                Eigen::Matrix3f rotMat = rn->getGlobalPose().block(0, 0, 3, 3);
                forceLocal = rotMat.inverse() * forceGlobal;
                torqueLocal = rotMat.inverse() * torqueGlobal;
                ftInfo.currentForce = forceLocal;
                ftInfo.currentTorque = torqueLocal;
            }
        }
    }
    //}
 
    return true;
}
 
 
bool BulletPhysicsWorld::synchronizeObjects()
{
    // lock engine and data
    ScopedRecursiveLockPtr lockEngine = getScopedEngineLock(__FUNCTION__);
    ScopedRecursiveLockPtr lockSync = getScopedSyncLock(__FUNCTION__);
 
    for (auto& dynamicsObject : dynamicsObjects)
    {
        SceneObjectPtr oEngine = dynamicsObject->getSceneObject();
        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;
}
 
 
std::vector<DynamicsEngine::DynamicsContactInfo> BulletPhysicsWorld::copyContacts()
{
    ScopedRecursiveLockPtr lockSync = getScopedSyncLock(__FUNCTION__);
    return contacts;
}
 
bool BulletPhysicsWorld::addObstacleEngine(
    VirtualRobot::SceneObjectPtr o,
    VirtualRobot::SceneObject::Physics::SimulationType simType)
{
    ScopedRecursiveLockPtr lockEngine = getScopedEngineLock(__FUNCTION__);
 
    ARMARX_DEBUG_S << "Adding obstacle " << o->getName();
    //clonedObj = o->clone(o->getName());
 
    DynamicsObjectPtr dynamicsObject;
 
    try
    {
        if (simType != VirtualRobot::SceneObject::Physics::eUnknown)
        {
            o->setSimulationType(simType);    //clonedObj
        }
 
        dynamicsObject = dynamicsWorld->CreateDynamicsObject(o);//clonedObj
        dynamicsWorld->addObject(dynamicsObject);
 
    }
    catch (VirtualRobotException& e)
    {
        cout << " ERROR while building dynamic object";
        cout << e.what();
        return false;
    }
 
    dynamicsObjects.push_back(dynamicsObject);
 
    // since we do not visualize this object we can skip visualization updates
    dynamicsObject->getSceneObject()->setUpdateVisualization(false);
 
    return true;
}
 
std::vector<std::string> BulletPhysicsWorld::getRobotNames()
{
    std::vector<std::string> names;
    {
        ScopedRecursiveLockPtr lockEngine = getScopedEngineLock(__FUNCTION__);
        std::map<std::string, DynamicsRobotInfo>::iterator it = dynamicRobots.begin();
        while (it != dynamicRobots.end())
        {
            names.push_back(it->first);
            it++;
        }
    }
    return names;
}
 
void BulletPhysicsWorld::setObjectSimType(const std::string& objectName, SceneObject::Physics::SimulationType simType)
{
    ScopedRecursiveLockPtr lockEngine = getScopedEngineLock(__FUNCTION__);
    SimDynamics::DynamicsObjectPtr o = getObject(objectName);
    if (!o)
    {
        ARMARX_WARNING_S << "Could not find object with name " << objectName;
        return;
    }
    o->setSimType(simType);
}
 
void BulletPhysicsWorld::setRobotNodeSimType(const std::string& robotName, const std::string& robotNodeName, SceneObject::Physics::SimulationType simType)
{
    ScopedRecursiveLockPtr lockEngine = getScopedEngineLock(__FUNCTION__);
    SimDynamics::DynamicsRobotPtr r = getDynamicRobot(robotName);
    if (!r)
    {
        ARMARX_WARNING_S << "Could not find robot with name " << robotName;
        return;
    }
    DynamicsObjectPtr rn = r->getDynamicsRobotNode(robotNodeName);
    if (!rn)
    {
        ARMARX_WARNING_S << "Could not find robot node with name " << robotNodeName;
        return;
    }
    rn->setSimType(simType);
}
 
std::vector<std::string> BulletPhysicsWorld::getObstacleNames()
{
    std::vector<std::string> names;
    {
        ScopedRecursiveLockPtr lockEngine = getScopedEngineLock(__FUNCTION__);
 
        for (auto& o : dynamicsObjects)
        {
            names.push_back(o->getName());
        }
    }
    return names;
}
 
 
bool BulletPhysicsWorld::removeObstacleEngine(const std::string& name)
{
    ARMARX_VERBOSE_S << "Removing obstacle " << name;
    ScopedRecursiveLockPtr lockEngine = getScopedEngineLock(__FUNCTION__);
    {
        SimDynamics::DynamicsObjectPtr o = getObject(name);
 
        if (!o)
        {
            return false;
        }
 
        std::vector<SimDynamics::DynamicsObjectPtr>::iterator it = find(dynamicsObjects.begin(), dynamicsObjects.end(), o);
 
        if (it != dynamicsObjects.end())
        {
            dynamicsObjects.erase(it);
        }
        else
        {
            ARMARX_WARNING_S << "Sync error";
        }
 
        dynamicsWorld->removeObject(o);
    }
 
    return true;
}
 
 
 
void BulletPhysicsWorld::stepPhysicsRealTime()
{
    ScopedRecursiveLockPtr lockEngine = getScopedEngineLock(__FUNCTION__);
    ARMARX_DEBUG_S << "start sim physics";
 
 
    //    IceUtil::Time startTime = IceUtil::Time::now();
    btScalar dt1 = btScalar(getDeltaTimeMicroseconds() / 1000.0);
 
    //    bulletEngine->activateAllObjects(); // avoid sleeping objects
 
    // the engine has to be stepped with seconds
    // max 100 internal loops
    ARMARX_DEBUG << deactivateSpam(1) << "dt1: " << (dt1 / 1000.0) << " max substeps: " << bulletFixedTimeStepMaxNrLoops << " internal step size: " << 0.001 * bulletFixedTimeStepMS;
    bulletEngine->stepSimulation(dt1 / 1000.0, bulletFixedTimeStepMaxNrLoops, 0.001 * bulletFixedTimeStepMS);
    simTimeStepMS = static_cast<float>(dt1);
    // step static robots
    stepStaticRobots(dt1 / 1000.0);
 
 
    currentSimTimeSec += dt1 / 1000.0;
    //    IceUtil::Time duration = IceUtil::Time::now() - startTime;
    simStepExecutionDurationMS = static_cast<float>(dt1);
    ARMARX_DEBUG_S << "end sim physics, sim duration [ms] : " << simStepExecutionDurationMS;
 
}
 
 
void BulletPhysicsWorld::stepPhysicsFixedTimeStep()
{
    ScopedRecursiveLockPtr lockEngine = getScopedEngineLock(__FUNCTION__);
    auto startTime = IceUtil::Time::now(); // this should always be real time and not virtual time
 
    //    bulletEngine->activateAllObjects(); // avoid sleeping objects
    //    bulletFixedTimeStepMaxNrLoops = 1;
 
    float stepSize = static_cast<float>(stepSizeMs / 1000.0);
    ARMARX_DEBUG_S << "start sim physics: " << VAROUT(stepSize) << VAROUT(stepSizeMs);
    //    int maxSubSteps = 10;
    // the engine has to be stepped with seconds
    // better results when stepping engine by hand
    //    for (int i = 0; i < bulletFixedTimeStepMaxNrLoops; i++)
    {
        bulletEngine->stepSimulation(static_cast<double>(stepSize),
                                     bulletFixedTimeStepMaxNrLoops, 0.001 * bulletFixedTimeStepMS);
    }
    simTimeStepMS = stepSizeMs;///*bulletFixedTimeStepMaxNrLoops **/ stepSize * 1000.0f;
 
    // step static robots
    stepStaticRobots(/*bulletFixedTimeStepMaxNrLoops **/ static_cast<double>(stepSize));
 
 
    currentSimTimeSec += static_cast<double>(stepSize) /** (float)bulletFixedTimeStepMaxNrLoops*/;
 
 
    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(static_cast<unsigned int>(1000 * (minStepSizeMs - duration)));
        }
    }
 
    IceUtil::Time duration = IceUtil::Time::now() - startTime;
    simStepExecutionDurationMS = static_cast<float>(duration.toMilliSecondsDouble());
    ARMARX_DEBUG_S << "end sim physics, sim duration [ms] : " << simStepExecutionDurationMS;
}
 
void BulletPhysicsWorld::stepStaticRobots(double deltaInSeconds)
{
    // step through all static robots and apply velocities
    for (auto dr : dynamicRobots)
    {
        DynamicsRobotInfo ri = dr.second;
        if (ri.isStatic)
        {
            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 << "No rn with name " << kinRob->hasRobotNode(nv.first);
                    continue;
                }
                double change = static_cast<double>(nv.second) * deltaInSeconds;
 
                //double randomNoiseValue = distribution(generator);
                //change += randomNoiseValue;
                double newAngle = static_cast<double>(kinRob->getRobotNode(nv.first)->getJointValue()) + change;
                targetPos[nv.first] = static_cast<float>(newAngle);
            }
 
            if (not targetPos.empty())
            {
                ARMARX_DEBUG << "Target joint angles : " << targetPos;
                actuateRobotJointsPos(kinRob->getName(), targetPos);
            }
        }
    }
}
 
int BulletPhysicsWorld::getFixedTimeStepMS()
{
    return stepSizeMs;
}
 
 
SimDynamics::BulletEnginePtr BulletPhysicsWorld::getEngine()
{
    return bulletEngine;
}
 
 
std::vector<VirtualRobot::SceneObjectPtr> BulletPhysicsWorld::getObjects()
{
    std::vector<VirtualRobot::SceneObjectPtr> res;
    for (auto o : dynamicsObjects)
    {
        res.push_back(o->getSceneObject());
    }
    return res;
}
 
std::vector<SimDynamics::DynamicsObjectPtr> BulletPhysicsWorld::getDynamicObjects()
{
    return dynamicsObjects;
}
 
void BulletPhysicsWorld::setupFloorEngine(bool enable, const std::string& floorTexture)
{
 
    ScopedRecursiveLockPtr lockEngine = getScopedEngineLock(__FUNCTION__);
 
    if (!dynamicsWorld)
    {
        return;
    }
 
    std::string textureFile = floorTexture;
 
    if (enable)
    {
        ARMARX_INFO_S << "Creating floor plane...";
        dynamicsWorld->createFloorPlane();
    }
}
 
void BulletPhysicsWorld::enableLogging(const std::string& robotName, const std::string& logFile)
{
    ScopedRecursiveLockPtr lockEngine = getScopedEngineLock(__FUNCTION__);
    SimDynamics::DynamicsRobotPtr dynamicsRobot = getDynamicRobot(robotName);
    BulletRobotPtr br = std::dynamic_pointer_cast<BulletRobot>(dynamicsRobot);
 
    if (!br)
    {
        ARMARX_ERROR_S << "no bullet robot to log...";
        return;
    }
 
    // create rns with all bodies
    std::vector<RobotNodePtr> rnsBodies;
    std::vector<RobotNodePtr> rnsJoints;
    std::vector<RobotNodePtr> rnAll = br->getRobot()->getRobotNodes();
    std::string nameJoints = "RNS_Joints_All_logging";
    std::string nameBodies = "RNS_Bodies_All_logging";
 
    for (auto r : rnAll)
    {
        if (r->isJoint())
        {
            rnsJoints.push_back(r);
        }
 
        if (r->getMass() > 0)
        {
            rnsBodies.push_back(r);
        }
    }
 
    RobotNodeSetPtr rnsJ = RobotNodeSet::createRobotNodeSet(br->getRobot(), nameJoints, rnsJoints);
    RobotNodeSetPtr rnsB = RobotNodeSet::createRobotNodeSet(br->getRobot(), nameBodies, rnsBodies);
 
    robotLogger.reset(new BulletRobotLogger(bulletEngine, br, rnsJ, rnsB));
    robotLogger->setLogPath(logFile);
    robotLogger->startLogging();
}
 
 
 
bool BulletPhysicsWorld::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 BulletPhysicsWorld::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;
}
 
btScalar BulletPhysicsWorld::getDeltaTimeMicroseconds()
{
    ScopedRecursiveLockPtr lockEngine = getScopedEngineLock(__FUNCTION__);
    btScalar dt = static_cast<btScalar>(m_clock.getTimeMicroseconds());
    m_clock.reset();
    return dt;
}
 
 
int BulletPhysicsWorld::getContactCount()
{
    ScopedRecursiveLockPtr l = getScopedSyncLock(__FUNCTION__);
    return static_cast<int>(contacts.size());
}
 
DistanceInfo BulletPhysicsWorld::getRobotNodeDistance(const std::string& robotName, const std::string& robotNodeName1, const std::string& robotNodeName2)
{
    ScopedRecursiveLockPtr lockEngine = getScopedEngineLock(__FUNCTION__);
 
    SimDynamics::DynamicsRobotPtr dynamicsRobot = getDynamicRobot(robotName);
    if (!dynamicsRobot)
    {
        throw LocalException("No robot");
    }
    if (dynamicsRobot->getName() != robotName)
    {
        throw LocalException("Wrong robot name. '") << robotName << "' != '" << dynamicsRobot->getName() << "'";
    }
    VirtualRobot::RobotPtr robot = dynamicsRobot->getRobot();
    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 BulletPhysicsWorld::getDistance(const std::string& robotName, const std::string& robotNodeName, const std::string& worldObjectName)
{
    ScopedRecursiveLockPtr lockEngine = getScopedEngineLock(__FUNCTION__);
 
    SimDynamics::DynamicsRobotPtr dynamicsRobot = getDynamicRobot(robotName);
    if (!dynamicsRobot)
    {
        throw LocalException("No robot");
    }
    if (dynamicsRobot->getName() != robotName)
    {
        throw LocalException("Wrong robot name. '") << robotName << "' != '" << dynamicsRobot->getName() << "'";
    }
    VirtualRobot::RobotPtr robot = dynamicsRobot->getRobot();
    if (!robot->hasRobotNode(robotNodeName))
    {
        throw LocalException("Wrong robot node name. '") << robotNodeName << "'";
    }
    SimDynamics::DynamicsObjectPtr object = getObject(worldObjectName);
    if (!object)
    {
        throw LocalException("No object with name '") << worldObjectName << "'";
    }
    VirtualRobot::SceneObjectPtr so = object->getSceneObject();
    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;
}
 
RobotPtr BulletPhysicsWorld::adaptRobotToWorld(VirtualRobot::RobotPtr robot)
{
    return RobotFactory::createFlattenedModel(*robot);
}