CollisionAvoidanceCore.cpp

Go to the documentation of this file.

#include "CollisionAvoidanceCore.h"
 
namespace armarx::control::njoint_controller::core
{
 
    CollisionAvoidanceBase::CollisionAvoidanceBase(const VirtualRobot::RobotPtr& rtRobotPtr,
                                                   const Config& cfg) :
        rtRobot(rtRobotPtr)
    {
        ARMARX_DEBUG << "creating collison avoidance base";
        ARMARX_CHECK_EXPRESSION(rtRobot);
 
        rtCfg = cfg;
        validateConfigData(rtCfg);
        userCfg = rtCfg;
        bufferCfgUserToRT.reinitAllBuffers(rtCfg);
 
        //        rtCfg.maxCollisionPairs;
        //        rtCfg.actuatedRobotNodeSetName;
        //        rtCfg.primitiveModelID;
        std::vector<std::string> tcpNodeNames;
        for (auto& pair : rtCfg.ctrlCfg)
        {
            const auto nodeset = rtRobot->getRobotNodeSet(pair.first);
            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
        const bool reduceModel = true; // for performance and ignoreCollision detection
        simoxReducedRobotPtr =
            std::make_shared<SimoxRobot>(rtRobot, actuatedJointNames, tcpNodeNames, reduceModel);
        ARMARX_CHECK_EXPRESSION(simoxReducedRobotPtr);
 
        //        std::vector<std::string> primitiveModelIDs{rtCfg.primitiveModelID};
 
        collisionRobotPtr = std::make_unique<CollisionRobot>(
            *simoxReducedRobotPtr, std::string(), true, rtCfg.primitiveModelParts);
        ARMARX_CHECK_EXPRESSION(collisionRobotPtr);
 
        collisionPairs = std::make_shared<DistanceResults>(rtCfg.maxCollisionPairs);
        collisionPairs->reserve(rtCfg.maxCollisionPairs);
 
        /// create not reduced simox control robot for inertia calculation
        simoxControlRobotPtr =
            std::make_shared<SimoxRobot>(rtRobot, actuatedJointNames, tcpNodeNames, false);
        ARMARX_CHECK_EXPRESSION(simoxControlRobotPtr);
 
        // 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)
        {
            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
                ARMARX_INFO << "-- adding translation joint " << nodeName
                            << " with scaling 0.001 from mm to meter";
                translationJointIndex.push_back(jointIdx);
                ++jointIdx;
            }
        }
 
        for (auto& pair : rtCfg.ctrlCfg)
        {
            collLimb.emplace(pair.first,
                             std::make_shared<CollisionAvoidanceArmData>(rtRobot,
                                                                         pair.first,
                                                                         simoxReducedRobotPtr,
                                                                         simoxControlRobotPtr,
                                                                         collisionPairs,
                                                                         rtCfg));
        }
    }
 
    CollisionAvoidanceBase::~CollisionAvoidanceBase()
    {
        ARMARX_IMPORTANT << "destruction of CollisionAvoidanceBase";
    }
 
    CollisionAvoidanceBase::CollisionAvoidanceArmData::CollisionAvoidanceArmData(
        VirtualRobot::RobotPtr rtRobot,
        const std::string& robotNodeSetName,
        SimoxRobotPtr simoxReducedRobot,
        SimoxRobotPtr simoxControlRobot,
        DistanceResultsPtr collisionPairs,
        Config& c) :
        nodeSetName(robotNodeSetName),
        rtRNS(rtRobot->getRobotNodeSet(robotNodeSetName)),
        //        controller(law::CollisionAvoidanceController(rtRNS, simoxReducedRobotPtr)),
        controller(rtRNS),
        collisionPairsPtr(collisionPairs),
        pointsOnArm(std::make_shared<std::vector<int>>(c.maxCollisionPairs, -1)),
        pointsOnArmIndex(0),
        simoxReducedRobotPtr(simoxReducedRobot)
    {
        ARMARX_CHECK_EXPRESSION(rtRNS);
        ARMARX_CHECK_EXPRESSION(collisionPairsPtr);
        ARMARX_CHECK_GREATER(c.maxCollisionPairs, 0);
        ARMARX_CHECK_EXPRESSION(simoxReducedRobotPtr);
 
        tcpName = rtRNS->getTCP()->getName();
        jointNames = rtRNS->getNodeNames();
 
        controller.initialize(simoxReducedRobotPtr);
 
        ARMARX_CHECK_GREATER(controller.numOfJoints, 0);
        rts.reset(
            c.ctrlCfg.at(robotNodeSetName), controller.numOfJoints, c.maxCollisionPairs, rtRNS);
        inertiaPtr = std::make_shared<simox::control::geodesics::metric::Inertia>(
            *simoxControlRobot, robotNodeSetName);
 
        bufferRTtoPublish.reinitAllBuffers(rts);
 
        ARMARX_CHECK_EXPRESSION(inertiaPtr);
    }
 
    void
    CollisionAvoidanceBase::CollisionAvoidanceArmData::reset(Config& c)
    {
        std::fill(pointsOnArm->begin(), pointsOnArm->end(), -1);
        pointsOnArmIndex = 0;
        rts.reset(
            c.ctrlCfg.at(rtRNS->getName()), controller.numOfJoints, 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>();
        for (auto& i : translationJointIndex)
        {
            // convert value from mm to m for simox control robot
            // all translational values of the simox control robots are in meter
            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
        simoxReducedRobotPtr->setConfig(qposActuatedJoints);
        simoxControlRobotPtr->setConfig(qposActuatedJoints);
 
        /// ----------------- calculation of robot collision pairs ---------------------------------
        double timeMeasure = IceUtil::Time::now().toMicroSecondsDouble();
        /// _______TODO_______ the following line is commented out, to avoid allocation of new memory
        collisionPairs->clear(); // empty collision pairs before assigning new ones
 
        /// !!! individualColObjects has to be set to true (second flag, that is passed) !!!
        /// if not set to true, the body and the arms become attractive, be aware
        /// _______TODO_______ consider passing the pre-allocated results to the simox lib.
        DistanceResults results =
            collisionRobotPtr->computeMinimumSelfCollisionDistance(true, true);
 
        collisionPairs->insert(collisionPairs->end(), results.begin(), results.end());
 
        double collisionPairTime = IceUtil::Time::now().toMicroSecondsDouble() - timeMeasure;
 
        for (auto& pair : collLimb)
        {
            /// reset temporary pointsOnArmIndex
            pair.second->pointsOnArmIndex = 0;
            pair.second->rts.collisionPairTime = collisionPairTime;
            pair.second->rts.collisionPairsNum = collisionPairs->size();
        }
 
 
        /// checks whether the point is located on the arm and fills the pointsOnArm vector with the
        /// corresponding indices
        /// the entries in the pointsOnArm vector store the indices of the collision pairs that are
        /// on the arm and which must be taken into account in the control law
 
        timeMeasure = IceUtil::Time::now().toMicroSecondsDouble();
        for (size_t i = 0; i < collisionPairs->size(); ++i)
        {
            ARMARX_CHECK(collisionPairs->at(i).hasPoints());
 
            /// prefilter contact pairs based on distance
            if (static_cast<float>(collisionPairs->at(i).minDistance) <= preFilterDistance)
            {
 
                /// set index in integer vector of contact point on arm below preFilterDistance
                for (auto& pair : collLimb)
                {
 
                    auto& arm = pair.second;
                    if (arm->pointsOnArmIndex >= rtCfg.maxCollisionPairs)
                        continue;
                    if (std::find(arm->jointNames.begin(),
                                  arm->jointNames.end(),
                                  collisionPairs->at(i).node1) != arm->jointNames.end() or
                        arm->tcpName == collisionPairs->at(i).node1)
                    {
                        /// _______TODO_______ what if there are more collision pairs than 100? added above check with maxCollisionPairs
                        // note: if node1 and node2 are on arm, this is handeled in law
                        // here it is only important, whether the coll pair is regarded for the arm
                        arm->pointsOnArm->at(arm->pointsOnArmIndex) = i;
                        arm->pointsOnArmIndex++;
                    }
                    else if (std::find(arm->jointNames.begin(),
                                       arm->jointNames.end(),
                                       collisionPairs->at(i).node2) != arm->jointNames.end() or
                             arm->tcpName == collisionPairs->at(i).node2)
                    {
                        arm->pointsOnArm->at(arm->pointsOnArmIndex) = i;
                        arm->pointsOnArmIndex++;
                    }
                }
            }
        }
        //        ARMARX_INFO << "detected collision pairs: " << collisionPairs->size();
        double preFilterTime = IceUtil::Time::now().toMicroSecondsDouble() - timeMeasure;
 
        if (preFilterTime > 100 or collisionPairTime > 100)
        {
            ARMARX_RT_LOGF_WARN("---- rt too slow: "
                                "preFilterTime: %.2f\n"
                                "collisionPairTime: %.2f\n",
                                preFilterTime, // 3-4 us
                                collisionPairTime)
                .deactivateSpam(1.0f); // 28-31 us
        }
    }
 
    void
    CollisionAvoidanceBase::rtLimbControllerRun(const std::string& robotNodeSetName,
                                                Eigen::VectorXf& qpos,
                                                Eigen::VectorXf& qvelFiltered,
                                                float torqueLimit,
                                                Eigen::VectorXf& impedanceJointTorque)
    {
        auto& arm = collLimb.at(robotNodeSetName);
        /// set all invalid indeces of this rt cycle to -1
        for (size_t i = arm->pointsOnArmIndex; i < arm->pointsOnArm->size(); ++i)
        {
            arm->pointsOnArm->at(i) = -1;
        }
        //        arm->rts.preFilterTime = preFilterTime;
        //        arm->rts.activeCollPairsNum = arm->pointsOnArmIndex;
 
        ARMARX_CHECK_EQUAL(arm->rts.impedanceJointTorque.size(), impedanceJointTorque.size());
        arm->rts.impedanceJointTorque = impedanceJointTorque;
        arm->controller.run(rtCfg.ctrlCfg.at(robotNodeSetName),
                            arm->rts,
                            arm->collisionPairsPtr,
                            arm->pointsOnArm,
                            arm->pointsOnArmIndex,
                            arm->inertiaPtr,
                            arm->jointNames,
                            qpos,
                            qvelFiltered,
                            torqueLimit);
        arm->bufferRTtoPublish.getWriteBuffer() = arm->rts;
        arm->bufferRTtoPublish.commitWrite();
    }
 
    void
    CollisionAvoidanceBase::validateConfigData(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