CollisionAvoidanceCore.cpp

Go to the documentation of this file.

#include "CollisionAvoidanceCore.h"
 
 
#include <VirtualRobot/Nodes/RobotNode.h>
#include <VirtualRobot/Robot.h>
#include <VirtualRobot/RobotNodeSet.h>
 
#include <simox/control/environment/CollisionRobot.h>
#include <simox/control/impl/simox/robot/Robot.h>
 
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/util/CPPUtility/trace.h>
 
namespace armarx::control::njoint_controller::core
{
 
    CollisionAvoidanceBase::CollisionAvoidanceBase(const VirtualRobot::RobotPtr& rtRobot,
                                                   const Config& cfg) :
        rtRobot(rtRobot)
    {
        ARMARX_CHECK_EXPRESSION(rtRobot);
 
        rtCfg = cfg;
        validateConfigData(rtCfg);
        userCfg = rtCfg;
        bufferCfgUserToRT.reinitAllBuffers(rtCfg);
 
        // Collect tcp node names
        std::vector<std::string> tcpNodeNames;
        for (const auto& pair : rtCfg.ctrlCfg)
        {
            const auto nodeset = rtRobot->getRobotNodeSet(pair.first);
            const std::string tcpName = nodeset->getTCP()->getName();
            tcpNodeNames.push_back(tcpName);
            if (pair.second.distanceThreshold > preFilterDistance)
            {
                preFilterDistance = pair.second.distanceThreshold;
            }
        }
 
        /// add actuated robot joints
        /// use a robot nodeset with all actuated joints, arms including head and torso joints, which change
        /// the position of the robot body parts for correct collision checking
        actuatedJointNames =
            rtRobot->getRobotNodeSet(rtCfg.actuatedRobotNodeSetName)->getNodeNames();
 
        /// initialize simox control robot for self-collision checking
        scRobot = std::make_unique<simox::control::simox::robot::Robot>(
            rtRobot, actuatedJointNames, tcpNodeNames, reduceModel);
        scRobot->setGlobalPose(Eigen::Isometry3d::Identity());
 
        ARMARX_INFO << "Creating robot model for collision computation using default primitive models and models with id " << rtCfg.primitiveModelParts;
 
        collisionRobot = std::make_unique<simox::control::environment::CollisionRobot<hpp::fcl::OBBRSS>>(
            *scRobot,
            simox::control::environment::CollisionRobotParams {
                .loadDefaultPrimitiveModel = true,
                .primitiveModelIDs = rtCfg.primitiveModelParts,
                .sortNodes = true});
 
        // initialize vector to store the current joint values of all actuated joints to update
        // the simox robots with the most recent values, so the simox control robots display the
        // current joint configuration of the real world model
        qposActuatedJoints.resize(actuatedJointNames.size());
        qposActuatedJoints.setZero();
 
        size_t jointIdx = 0;
        for (const auto& nodeName : actuatedJointNames)
        {
            const auto node = rtRobot->getRobotNode(nodeName);
            if (node->isTranslationalJoint())
            {
                // convert value from mm to m for simox control robot
                // all translational values of the simox control robots are in meter
                translationJointIndex.push_back(jointIdx);
            }
            jointIdx++;
        }
 
        for (const auto& pair : rtCfg.ctrlCfg)
        {
            collLimb.try_emplace(pair.first, rtRobot, pair.first, *scRobot, collisionPairs, rtCfg);
        }
 
        // Store all indices
        ARMARX_INFO << "Creating index to node mapping for collision robot:";
        std::unordered_set<unsigned int> consideredIndices;
        for (auto& [name, data] : collLimb)
        {
            collisionRobot->getIndices(data.controller.getNodeSet(), data.collisionRobotIndices);
            for (const auto& [index, node] : data.collisionRobotIndices)
            {
                ARMARX_INFO << "Mapping index " << index << " to node " << node->getName()
                            << " for limb " << name;
                consideredIndices.insert(index);
            }
        }
 
        ARMARX_INFO << "Initial computation of all self collision pairs";
 
        selfColAvoidanceArgs = simox::control::environment::CompDistArgs{
            .updatePose = false,
            .nearestPoints = true,
            .individualColObjects = true,
            .allowIndices = consideredIndices};
 
        // Compute number of collision pairs
        // This will not change if no objects are attached and the args are not changed
        collisionRobot->update(false); // update pose
        const DistanceResults distanceResults = 
            collisionRobot->computeMinimumSelfCollisionDistance(selfColAvoidanceArgs);
        const int nElements = distanceResults.size();
 
        for (const auto& distanceResult : distanceResults)
        {
            ARMARX_INFO 
                << "Considering distance between " 
                << collisionRobot->getNodes().at(distanceResult.node1).getName() << " and " 
                << collisionRobot->getNodes().at(distanceResult.node2).getName()
                << ". Initial distance: " << distanceResult.minDistance << "m";
        }
 
        ARMARX_INFO << "Reserving space for " << nElements << " collision pairs";
        ARMARX_CHECK_EXPRESSION(nElements < rtCfg.maxCollisionPairs);
        collisionPairs.reserve(nElements);
    }
 
    CollisionAvoidanceBase::~CollisionAvoidanceBase()
    {
        ARMARX_IMPORTANT << "destruction of CollisionAvoidanceBase";
    }
 
    CollisionAvoidanceBase::NodeSetData::NodeSetData(
        const VirtualRobot::RobotPtr& rtRobot,
        const std::string& robotNodeSetName,
        const SCRobot& scRobot,
        const DistanceResults& collisionPairs,
        const Config& c) :
        nodeSetName(robotNodeSetName),
        rtRNS(rtRobot->getRobotNodeSet(robotNodeSetName)),
        controller(scRobot.getSubNodeSet(robotNodeSetName)),
        collisionPairs(collisionPairs)
    {
        ARMARX_CHECK_EXPRESSION(rtRNS);
        ARMARX_CHECK_GREATER(c.maxCollisionPairs, 0);
 
        reset(c);
 
        dynamicsModel = scRobot.createDynamicsModel(robotNodeSetName);
 
        bufferRTtoPublish.reinitAllBuffers(rts);
    }
 
    void
    CollisionAvoidanceBase::NodeSetData::reset(const Config& c)
    {
        rts.reset(
            c.ctrlCfg.at(rtRNS->getName()), controller.numNodes(), c.maxCollisionPairs, rtRNS);
    }
 
    void
    CollisionAvoidanceBase::rtPreActivate()
    {
        for (auto& pair : collLimb)
        {
            pair.second.rts.rtPreActivate();
        }
    }
 
    void
    CollisionAvoidanceBase::rtPostDeactivate()
    {
        for (auto& pair : collLimb)
        {
            pair.second.reset(rtCfg);
        }
    }
 
    void
    CollisionAvoidanceBase::updateRtConfigFromUser()
    {
        /// Running in rt thread.
        rtCfg = bufferCfgUserToRT.getUpToDateReadBuffer();
        validateConfigData(rtCfg);
    }
 
    void
    CollisionAvoidanceBase::rtCollisionChecking()
    {
        /// ----------------- update joint values for simox Robots ---------------------------------
        qposActuatedJoints = rtRobot->getRobotNodeSet(rtCfg.actuatedRobotNodeSetName)
                                 ->getJointValuesEigen()
                                 .cast<double>();
 
        // convert value from mm to m for simox control robot
        // all translational values of the simox control robots are in meter
        for (unsigned int i : translationJointIndex)
        {
            qposActuatedJoints(i) *= 0.001;
        }
 
        /// ----------------- update Simox robots --------------------------------------------------
        ///
        /// a VectorXd is needed with nodeSet size (size of actuated joints in order of vector,
        /// which has been used for initialization in init
        ///
        /// It is very important that the simox robots map the current joint configuration of the
        /// real-world robot
        scRobot->setConfig(qposActuatedJoints);
 
        /// ----------------- calculation of robot collision pairs ---------------------------------
        const auto time = IceUtil::Time::now();
        collisionRobot->update(false); // update pose
        collisionRobot->computeMinimumSelfCollisionDistance(collisionPairs, selfColAvoidanceArgs);
        const double collisionPairTime = (IceUtil::Time::now() - time).toMicroSecondsDouble();
 
        for (auto& pair : collLimb)
        {
            pair.second.rts.collisionPairTime = collisionPairTime;
            pair.second.rts.collisionPairsNum = collisionPairs.size();
        }
    }
 
    void
    CollisionAvoidanceBase::rtLimbControllerRun(const std::string& robotNodeSetName,
                                                const Eigen::VectorXf& qpos,
                                                const Eigen::VectorXf& qvelFiltered,
                                                float torqueLimit,
                                                const Eigen::VectorXf& impedanceJointTorque)
    {
        auto& arm = collLimb.at(robotNodeSetName);
 
        ARMARX_CHECK_EQUAL(arm.rts.impedanceJointTorque.size(), impedanceJointTorque.size());
        arm.rts.impedanceJointTorque = impedanceJointTorque;
        arm.controller.run(rtCfg.ctrlCfg.at(robotNodeSetName),
                            arm.rts,
                            arm.collisionPairs,
                            arm.collisionRobotIndices,
                            *arm.dynamicsModel,
                            qpos,
                            qvelFiltered,
                            torqueLimit);
        arm.bufferRTtoPublish.getWriteBuffer() = arm.rts;
        arm.bufferRTtoPublish.commitWrite();
    }
 
    void
    CollisionAvoidanceBase::validateConfigData(const Config& cfg)
    {
        ARMARX_CHECK_NONNEGATIVE(cfg.maxCollisionPairs);
        ARMARX_CHECK_NOT_EMPTY(cfg.primitiveModelParts);
        ARMARX_CHECK_NOT_EMPTY(cfg.actuatedRobotNodeSetName);
        for (auto& pair : cfg.ctrlCfg)
        {
            ::armarx::control::common::control_law::collision::validateConfigData(pair.second);
        }
    }
 
    void
    CollisionAvoidanceBase::setUserCfg(const Config& cfg)
    {
        userCfg = cfg;
        validateConfigData(userCfg);
        bufferCfgUserToRT.getWriteBuffer() = userCfg;
        bufferCfgUserToRT.commitWrite();
    }
 
} // namespace armarx::control::njoint_controller::core