ObjectCollisionAvoidanceVelCore.cpp

Go to the documentation of this file.

#include "ObjectCollisionAvoidanceVelCore.h"
 
#include <VirtualRobot/Nodes/RobotNode.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>
 
#include <RobotAPI/components/units/RobotUnit/util/ControlThreadOutputBuffer.h>
 
#include <armarx/control/common/control_law/CollisionAvoidanceVel.h>
#include <armarx/control/common/utils.h>
 
namespace armarx::control::njoint_controller::core
{
 
    ObjectCollisionAvoidanceVelBase::ObjectCollisionAvoidanceVelBase(
        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.selfDistanceThreshold > preFilterDistance)
            {
                preFilterDistance = pair.second.selfDistanceThreshold;
            }
            if (pair.second.objectDistanceThreshold > preFilterDistance)
            {
                preFilterDistance = pair.second.objectDistanceThreshold;
            }
        }
 
        /// 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);
        }
 
        // 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_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);
    }
 
    ObjectCollisionAvoidanceVelBase::~ObjectCollisionAvoidanceVelBase()
    {
        ARMARX_IMPORTANT << "destruction of ObjectCollisionAvoidanceVelBase";
    }
 
    ObjectCollisionAvoidanceVelBase::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
    ObjectCollisionAvoidanceVelBase::NodeSetData::reset(const Config& c)
    {
        rts.reset(
            c.ctrlCfg.at(rtRNS->getName()), controller.numNodes(), c.maxCollisionPairs, rtRNS);
        rts.objectCollisionNullSpaceWeights =
            armarx::control::common::control_law::helper::calculateTransitionWeights(
                c.ctrlCfg.at(rtRNS->getName()).objectCollActivatorZ1,
                c.ctrlCfg.at(rtRNS->getName()).objectCollActivatorZ2);
        recoveryStateObjColl.reset();
        recoveryStateSelfColl.reset();
    }
 
    void
    ObjectCollisionAvoidanceVelBase::rtPreActivate()
    {
        for (auto& pair : collLimb)
        {
            pair.second.rts.rtPreActivate(userCfg.ctrlCfg.at(pair.first));
            pair.second.recoveryStateObjColl.reset();
            pair.second.recoveryStateSelfColl.reset();
        }
    }
 
    void
    ObjectCollisionAvoidanceVelBase::rtPostDeactivate()
    {
        for (auto& pair : collLimb)
        {
            pair.second.reset(rtCfg);
        }
    }
 
    void
    ObjectCollisionAvoidanceVelBase::updateRtConfigFromUser()
    {
        /// Running in rt thread.
        rtCfg = bufferCfgUserToRT.getUpToDateReadBuffer();
        validateConfigData(rtCfg);
    }
 
    void
    ObjectCollisionAvoidanceVelBase::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);
 
        // Directly set Simox robot inside and then update the joints of simox control robot
        scRobotNodeSet->setJointValues(rtRobotNodeSet->getJointValuesEigen());
        scRobot->update();
        globalPose = rtRobot->getGlobalPose().matrix().cast<double>();
        globalPose.block<3, 1>(0, 3) /= 1000.0;
        scRobot->setGlobalPose(simox::control::Pose(globalPose), true);
 
        /// ----------------- calculation of robot collision pairs ---------------------------------
        const auto time = IceUtil::Time::now();
        // TODO update is also called in computeMinimumSelfCollisionDistance if updatePose = true in selfColAvoidanceArgs
        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();
        }
 
        /// ----------------- calculation of collision pairs between the robot and the environment -----------------
        collisionRobot->computeMinimumDistance(
            rtCollisionObjects, externalCollisionPairs, compDistArgs);
    }
 
    void
    ObjectCollisionAvoidanceVelBase::rtLimbControllerRun(const std::string& robotNodeSetName,
                                                         const Eigen::VectorXf& qpos,
                                                         const Eigen::VectorXf& qvelFiltered,
                                                         float velocityLimit,
                                                         const Eigen::VectorXf& trajFollowJointVel,
                                                         double deltaT)
    {
        auto& arm = collLimb.at(robotNodeSetName);
 
        ARMARX_CHECK_EQUAL(arm.rts.trajFollowJointVel.size(), trajFollowJointVel.size());
        arm.rts.trajFollowJointVel = trajFollowJointVel;
        arm.controller.run(rtCfg.ctrlCfg.at(robotNodeSetName),
                           arm.rts,
                           arm.recoveryStateSelfColl,
                           arm.recoveryStateObjColl,
                           arm.collisionPairs,
                           externalCollisionPairs,
                           arm.collisionRobotIndices,
                           *arm.dynamicsModel,
                           qpos,
                           qvelFiltered,
                           velocityLimit,
                           deltaT);
        arm.bufferRTtoPublish.getWriteBuffer() = arm.rts;
        arm.bufferRTtoPublish.commitWrite();
 
        arm.bufferRecoveryStateSelfColl.getWriteBuffer() = arm.recoveryStateSelfColl;
        arm.bufferRecoveryStateSelfColl.commitWrite();
    }
 
    void
    ObjectCollisionAvoidanceVelBase::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
    ObjectCollisionAvoidanceVelBase::setUserCfg(const Config& cfg)
    {
        userCfg = cfg;
        validateConfigData(userCfg);
        bufferCfgUserToRT.getWriteBuffer() = userCfg;
        bufferCfgUserToRT.commitWrite();
    }
 
    void
    ObjectCollisionAvoidanceVelBase::updateRtCollisionObjects()
    {
        /// Running in rt thread.
        std::tie(externalCollisionPairs, rtCollisionObjects) =
            bufferCollisionObjectsUserToRt.getUpToDateReadBuffer();
    }
 
    void
    ObjectCollisionAvoidanceVelBase::deleteUserCollisionObjects()
    {
        userCollisionObjects.clear();
        auto& writeBuffer = bufferCollisionObjectsUserToRt.getWriteBuffer();
        std::get<0>(writeBuffer).reserve(0);
        std::get<1>(writeBuffer) = userCollisionObjects;
        bufferCollisionObjectsUserToRt.commitWrite();
    }
 
    void
    ObjectCollisionAvoidanceVelBase::updateUserCollisionObjects(
        const std::vector<hpp::fcl::CollisionObject>& collisionObjects)
    {
        const unsigned int nElements =
            collisionRobot->computeNElements(compDistArgs, collisionObjects);
        ARMARX_INFO << "There are " << collisionObjects.size() << " collision objects";
        ARMARX_INFO << "Reserving space for " << nElements << " collision pairs";
 
        userCollisionObjects = collisionObjects;
        auto& writeBuffer = bufferCollisionObjectsUserToRt.getWriteBuffer();
        std::get<0>(writeBuffer).reserve(nElements);
        std::get<1>(writeBuffer) = collisionObjects;
        bufferCollisionObjectsUserToRt.commitWrite();
    }
 
 
} // namespace armarx::control::njoint_controller::core