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,
        const std::map<std::string, std::vector<size_t>>& velCtrlIndices) :
        rtRobot(rtRobot)
    {
        ARMARX_CHECK_EXPRESSION(rtRobot);
 
        rtCfg = cfg;
        validateConfigData(rtCfg);
        userCfg = rtCfg;
        bufferCfgUserToRT.reinitAllBuffers(rtCfg);
        velCtrlIdx = velCtrlIndices;
 
        // 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.selfDistanceThreshold > preFilterDistance)
            {
                preFilterDistance = pair.second.selfDistanceThreshold;
            }
        }
 
        /// 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
        ARMARX_CHECK(rtRobot->hasRobotNodeSet(cfg.actuatedRobotNodeSetName));
        rtRobotNodeSet = rtRobot->getRobotNodeSet(cfg.actuatedRobotNodeSetName);
        actuatedJointNames = rtRobotNodeSet->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_CHECK(scRobot->getRobot()->hasRobotNodeSet(cfg.actuatedRobotNodeSetName));
        scRobotNodeSet = scRobot->getRobot()->getRobotNodeSet(cfg.actuatedRobotNodeSetName);
 
        ARMARX_DEBUG << "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,
                                 velCtrlIdx.at(pair.first));
        }
 
        // Store all indices
        ARMARX_DEBUG << "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_DEBUG << "Mapping index " << index << " to node " << node->getName()
                             << " for limb " << name;
                consideredIndices.insert(index);
            }
        }
 
        ARMARX_DEBUG << "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_DEBUG << "Considering distance between "
                         << collisionRobot->getNodes().at(distanceResult.node1).getName() << " and "
                         << collisionRobot->getNodes().at(distanceResult.node2).getName()
                         << ". Initial distance: " << distanceResult.minDistance << "m";
        }
 
        ARMARX_DEBUG << "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,
                                                     const std::vector<size_t>& velCtrlIndex) :
        nodeSetName(robotNodeSetName),
        rtRNS(rtRobot->getRobotNodeSet(robotNodeSetName)),
        controller(scRobot.getSubNodeSet(robotNodeSetName)),
        collisionPairs(collisionPairs)
    {
        ARMARX_CHECK_EXPRESSION(rtRNS);
        ARMARX_CHECK_GREATER(c.maxCollisionPairs, 0);
 
        reset(c, velCtrlIndex);
 
        dynamicsModel = scRobot.createDynamicsModel(robotNodeSetName);
 
        bufferRTtoPublish.reinitAllBuffers(rts);
    }
 
    void
    CollisionAvoidanceBase::NodeSetData::reset(const Config& c,
                                               const std::vector<size_t>& velCtrlIndex)
    {
        rts.reset(c.ctrlCfg.at(rtRNS->getName()),
                  controller.numNodes(),
                  c.maxCollisionPairs,
                  rtRNS,
                  velCtrlIndex);
        rStateSelfColl.reset();
    }
 
    void
    CollisionAvoidanceBase::rtPreActivate()
    {
        for (auto& pair : collLimb)
        {
            pair.second.rts.rtPreActivate(userCfg.ctrlCfg.at(pair.first));
            pair.second.rStateSelfColl.reset();
        }
    }
 
    void
    CollisionAvoidanceBase::rtPostDeactivate()
    {
        for (auto& pair : collLimb)
        {
            pair.second.reset(rtCfg, velCtrlIdx.at(pair.first));
        }
    }
 
    void
    CollisionAvoidanceBase::updateRtConfigFromUser()
    {
        /// Running in rt thread.
        rtCfg = bufferCfgUserToRT.getUpToDateReadBuffer();
        validateConfigData(rtCfg);
    }
 
    void
    CollisionAvoidanceBase::rtCollisionChecking()
    {
        // Directly set Simox robot inside and then update the joints of simox control robot
        scRobotNodeSet->setJointValues(rtRobotNodeSet->getJointValuesEigen());
        scRobot->update();
 
        /// ----------------- 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,
                                                double deltaT)
    {
        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.rStateSelfColl,
                           arm.collisionPairs,
                           arm.collisionRobotIndices,
                           *arm.dynamicsModel,
                           qpos,
                           qvelFiltered,
                           torqueLimit,
                           deltaT);
        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