ObjectCollisionAvoidanceVel.cpp

Go to the documentation of this file.

#include "ObjectCollisionAvoidanceVel.h"
 
#include <boost/unordered/detail/implementation.hpp>
 
#include <simox/control/dynamics/RBDLModel.h>
 
#include <RobotAPI/components/units/RobotUnit/util/ControlThreadOutputBuffer.h>
 
namespace armarx::control::common::control_law
{
    ObjectCollisionAvoidanceVelController::ObjectCollisionAvoidanceVelController(
        const simox::control::robot::NodeSetInterface* nodeSet) :
        CollisionAvoidanceVelController(nodeSet)
    {
        controllerHierarchy.reserve(4); // number of torque types to consider
    }
 
    void
    ObjectCollisionAvoidanceVelController::calculateObjectCollisionNullspace(
        const common::control_law::arondto::ObjectCollisionAvoidanceVelConfig& c,
        RtStatusForSafetyStrategy& rtStatus) const
    {
        rtStatus.objectCollNullSpace.setIdentity();
 
        ARMARX_CHECK_EQUAL(numNodes(), rtStatus.objectCollNullSpace.rows());
        ARMARX_CHECK_EQUAL(numNodes(), rtStatus.objectCollNullSpace.cols());
 
        ARMARX_CHECK_EQUAL(4, rtStatus.objectCollisionNullSpaceWeights.rows());
        rtStatus.objectCollK1 = rtStatus.objectCollisionNullSpaceWeights(0);
        rtStatus.objectCollK2 = rtStatus.objectCollisionNullSpaceWeights(1);
        rtStatus.objectCollK3 = rtStatus.objectCollisionNullSpaceWeights(2);
        rtStatus.objectCollK4 = rtStatus.objectCollisionNullSpaceWeights(3);
 
        ARMARX_TRACE_LITE;
 
        for (unsigned int i = rtStatus.objectCollDataIndex; i < rtStatus.activeCollPairsNum; i++)
        {
            auto& active = rtStatus.collDataVec[i];
 
            /// get desired null space value based on proximity to collision
            if (active.minDistance < c.objectCollActivatorZ1)
            {
                /// full locking in direction of collision
                rtStatus.desiredNullSpace = 0.0f;
            }
            else if (c.objectCollActivatorZ1 <= active.minDistance &&
                     active.minDistance <= c.objectCollActivatorZ2)
            {
                /// in transition state
                rtStatus.desiredNullSpace =
                    rtStatus.objectCollK1 * std::pow(active.minDistance, 3) +
                    rtStatus.objectCollK2 * std::pow(active.minDistance, 2) +
                    rtStatus.objectCollK3 * active.minDistance + rtStatus.objectCollK4;
                if (rtStatus.desiredNullSpace > 1.0f)
                {
                    ARMARX_WARNING << "desired null space value should not be higher than 1.0, was "
                                   << rtStatus.desiredNullSpace
                                   << ", in collision null space calulation, weights: "
                                   << rtStatus.objectCollK1 << ", " << rtStatus.objectCollK2 << ", "
                                   << rtStatus.objectCollK3 << ", " << rtStatus.objectCollK4;
                }
            }
            else
            {
                /// unrestricted movement in direction of collision
                rtStatus.desiredNullSpace = 1.0f;
            }
 
            active.desiredNSColl = rtStatus.desiredNullSpace;
            rtStatus.desiredNullSpace = std::clamp(rtStatus.desiredNullSpace, 0.0f, 1.0f);
            ARMARX_CHECK_NONNEGATIVE(rtStatus.desiredNullSpace);
            /// normalize projected Jacobian
            rtStatus.objectCollNormalizedJacT = active.projectedJacT.normalized();
 
            ARMARX_CHECK_EQUAL(numNodes(), rtStatus.objectCollNormalizedJacT.rows());
 
            rtStatus.objectCollTempNullSpaceMatrix =
                I - (rtStatus.objectCollNormalizedJacT * (1.0f - rtStatus.desiredNullSpace) *
                     rtStatus.objectCollNormalizedJacT.transpose());
 
            if (c.onlyLimitCollDirection)
            {
                /// check, whether impedance joint torque acts against the collision direction
                ARMARX_CHECK_EQUAL(active.projectedJacT.rows(), rtStatus.trajFollowJointVel.rows());
                for (int i = 0; i < rtStatus.trajFollowJointVel.rows(); ++i)
                {
                    if ((active.projectedJacT(i) < 0.0f and
                         rtStatus.trajFollowJointVel(i) < 0.0f) ||
                        (active.projectedJacT(i) > 0.0f and rtStatus.trajFollowJointVel(i) > 0.0f))
                    {
                        // if both have the same sign (both negative or both positive), do not limit DoF
                        rtStatus.objectCollTempNullSpaceMatrix(i, i) = 1.0f;
                    }
                }
            }
            /// project desired null space in corresponding direction via the Norm-Jacobian
            rtStatus.objectCollNullSpace *= rtStatus.objectCollTempNullSpaceMatrix;
            // todo: clamp the diagonal values between 0 and 1
        }
    }
 
    void
    ObjectCollisionAvoidanceVelController::calculateExternalCollisionVel(
        const Config& c,
        RtStatusForSafetyStrategy& rts,
        RecoveryState& rState,
        const DistanceResults& externalCollisionPairs,
        const CollisionRobotIndices& collisionRobotIndices,
        const Eigen::VectorXf& qvelFiltered,
        double deltaT) const
    {
 
        /// collision avoidance algorithm following the methods in Dietrich et al. (2012):
        ///
        /// A. Dietrich, T. Wimbock, A. Albu-Schaffer and G. Hirzinger, "Integration of Reactive,
        /// Torque-Based Self-Collision Avoidance Into a Task Hierarchy," in IEEE Transactions on
        /// Robotics, vol. 28, no. 6, pp. 1278-1293, Dec. 2012, doi: 10.1109/TRO.2012.2208667.
        ///
        /// see: https://ieeexplore.ieee.org/document/6255795
        ///
        /// method:
        ///
        /// computes the joint torques to avoid collisions -> collisionJointTorque
 
        if (externalCollisionPairs.empty())
        {
            return;
        }
 
        /// clear values before new cycle
        rts.objectCollisionJointVel.setZero();
 
        if (!c.enableSelfCollisionAvoidance)
        {
            rts.objectCollDataIndex = 0;
            rts.activeCollPairsNum = 0;
        }
 
        // clear values before new cycle, starting from object collision data index
        for (size_t i = rts.objectCollDataIndex; i < rts.collDataVec.size(); ++i)
        {
            rts.collDataVec[i].clearValues();
        }
 
        /// when collDistanceThresholdInit is smaller than recoveryDistanceElapseMeter
        /// (usually means it is zero), we need to initilize it to the smallest collision distance
        /// plus a recoveryDistanceElapseMeter.
        float& thresholdInit = rState.collDistanceThresholdInit;
        if (c.enableCollisionRecoveryOnStartup and (thresholdInit < c.recoveryDistanceElapseMeter))
        {
            thresholdInit = c.objectDistanceThreshold;
            for (const DistanceResult& collisionPair : externalCollisionPairs)
            {
                float minDist = static_cast<float>(collisionPair.minDistance);
                if (minDist < thresholdInit)
                {
                    thresholdInit = std::max(minDist, 0.0f);
                }
            }
            thresholdInit =
                std::min(c.objectDistanceThreshold, thresholdInit + c.recoveryDistanceElapseMeter);
        }
 
        rState.update(c, rts, deltaT);
 
 
        /// handling of external collision pairs
        for (const DistanceResult& collisionPair : externalCollisionPairs)
        {
 
            if (rts.activeCollPairsNum >= rts.collDataVec.size())
            {
                ARMARX_RT_LOGF_WARN(
                    "CollisionAvoidanceVelController",
                    "Number of active collision pairs exceeds the allocated memory");
                break;
            }
 
            if (static_cast<float>(collisionPair.minDistance) >= rState.collDistanceThreshold)
            {
                continue;
            }
 
            /// only node1 is on the robot
            if (collisionRobotIndices.count(collisionPair.node1) == 0)
            {
                continue;
            }
 
            ARMARX_CHECK(collisionPair.hasPoints());
 
            auto& externalCollDataVec = rts.collDataVec[rts.activeCollPairsNum++];
 
            externalCollDataVec.minDistance = static_cast<float>(collisionPair.minDistance);
 
            externalCollDataVec.node1 = collisionPair.node1;
            externalCollDataVec.node2 = collisionPair.node2;
            externalCollDataVec.point1 = collisionPair.point1->cast<float>();
            externalCollDataVec.point2 = collisionPair.point2->cast<float>();
            auto node1Type = collisionPair.node1Type;
            //            auto node2Type = collisionPair.node2Type;
 
            // direction is pointing away from collision
            externalCollDataVec.direction = externalCollDataVec.point1 - externalCollDataVec.point2;
 
            if (externalCollDataVec.minDistance < 0.0f)
            {
                externalCollDataVec.minDistance = 0.0f;
 
                // check for overlapping objects (returned points are exactly the same)
                if (externalCollDataVec.point1.isApprox(externalCollDataVec.point2, 1e-8) and
                    collisionPair.normalVec != std::nullopt)
                {
                    /// if the points are the same, normalVec comes with a value
 
                    // todo: how to make sure the normalVec is always pointing away from the
                    // collision, in normally the vector points out from the contact point within
                    // the sphere
                    // issue: if the arm is modeled with a sphere, it will point towards the collision
                    // No faulty behavior has been detected so far, however there is the possibility
                    // to get a direction vector pointing towards the collision
 
                    // solution: check if the node is a sphere, if the repulsive force is
                    // suppose to apply on the sphere, use the negative normal direction
                    if (node1Type == hpp::fcl::NODE_TYPE::GEOM_SPHERE)
                    {
                        externalCollDataVec.direction =
                            -1.0 * collisionPair.normalVec->cast<float>();
                    }
                    else
                    {
                        externalCollDataVec.direction = collisionPair.normalVec->cast<float>();
                    }
                }
                else
                {
                    externalCollDataVec.direction *= -1.0f;
                }
            }
 
            externalCollDataVec.direction.normalize();
 
            this->CollisionAvoidanceVelController::calculateCollisionJointVel(
                externalCollDataVec,
                c,
                rts,
                collisionRobotIndices,
                qvelFiltered,
                rts.objectCollisionJointVel,
                rState.collDistanceThreshold);
        }
    }
 
    /// --------------------------------- main rt-loop ------------------------------------------
 
    void
    ObjectCollisionAvoidanceVelController::run(const Config& c,
                                               RtStatusForSafetyStrategy& rtStatus,
                                               RecoveryState& rStateSelfColl,
                                               RecoveryState& rStateObjColl,
                                               const DistanceResults& collisionPairs,
                                               const DistanceResults& externalCollisionPairs,
                                               const CollisionRobotIndices& collisionRobotIndices,
                                               DynamicsModel& dynamicsModel,
                                               const Eigen::VectorXf& qpos,
                                               const Eigen::VectorXf& qvelFiltered,
                                               float velocityLimit,
                                               double deltaT)
    {
        // /// run in rt thread
 
        /// ----------------------------- inertia calculations --------------------------------------------------
        dynamicsModel.getInertiaMatrix(qpos.cast<double>(), rtStatus.inertia);
 
        // rtStatus.inertiaInverse = rtStatus.inertia.cast<float>().inverse();
        // inertia is positive definite matrix
        rtStatus.inertiaInverse = rtStatus.inertia.cast<float>().llt().solve(I);
 
        /// ----------------------------- safety constraints --------------------------------------------------
        if (c.enableSelfCollisionAvoidance)
        {
            // calculation of self-collision avoidance torque
            calculateSelfCollisionVel(
                c, rtStatus, rStateSelfColl, collisionPairs, collisionRobotIndices, qvelFiltered, deltaT);
        }
        if (c.enableObjectCollisionAvoidance)
        {
            // calculation of external collision avoidance torque
            calculateExternalCollisionVel(
                c, rtStatus, rStateObjColl, externalCollisionPairs, collisionRobotIndices, qvelFiltered, deltaT);
        }
        if (c.enableJointLimitAvoidance)
        {
            // calculation of joint limit avoidance torque
            calculateJointLimitVel(c, rtStatus, qpos, qvelFiltered);
        }
 
        if (!c.samePriority)
        {
            if (c.enableSelfCollisionAvoidance)
            {
                // calculation of null space matrix for self-collision avoidance
                calculateSelfCollisionNullspace(c, rtStatus);
            }
            if (c.enableObjectCollisionAvoidance)
            {
                // calculation of null space matrix for external collision avoidance
                calculateObjectCollisionNullspace(c, rtStatus);
            }
            if (c.enableJointLimitAvoidance)
            {
                // calculation of null space matrix for joint limit avoidance
                calculateJointLimitNullspace(c, rtStatus, qpos);
            }
        }
        ARMARX_TRACE_LITE;
 
        /// --------------------------- apply EMA low pass filter  -----------------------------------------------
        if (c.filterSafetyValues)
        {
            /// filter safety constraint values using EMA low pass filter
            for (int i = 0; i < rtStatus.selfCollisionJointVel.size(); ++i)
            {
                rtStatus.selfCollisionVelFiltered(i) =
                    (1 - c.safetyValFilter) * rtStatus.selfCollisionVelFiltered(i) +
                    c.safetyValFilter * rtStatus.selfCollisionJointVel(i);
            }
            for (int i = 0; i < rtStatus.objectCollisionJointVel.size(); ++i)
            {
                rtStatus.objectCollisionVelFiltered(i) =
                    (1 - c.safetyValFilter) * rtStatus.objectCollisionVelFiltered(i) +
                    c.safetyValFilter * rtStatus.objectCollisionJointVel(i);
            }
            for (int i = 0; i < rtStatus.jointLimitJointVel.size(); ++i)
            {
                rtStatus.jointLimitVelFiltered(i) =
                    (1 - c.safetyValFilter) * rtStatus.jointLimitVelFiltered(i) +
                    c.safetyValFilter * rtStatus.jointLimitJointVel(i);
            }
 
            for (int i = 0; i < rtStatus.selfCollNullSpace.diagonalSize(); ++i)
            {
                // the computed values are only on the diagonal, so only these need to be filtered
                rtStatus.selfCollNullSpaceFiltered(i, i) =
                    (1 - c.safetyValFilter) * rtStatus.selfCollNullSpaceFiltered(i, i) +
                    c.safetyValFilter * rtStatus.selfCollNullSpace(i, i);
            }
            for (int i = 0; i < rtStatus.objectCollNullSpace.diagonalSize(); ++i)
            {
                // the computed values are only on the diagonal, so only these need to be filtered
                rtStatus.objectCollNullSpaceFiltered(i, i) =
                    (1 - c.safetyValFilter) * rtStatus.objectCollNullSpaceFiltered(i, i) +
                    c.safetyValFilter * rtStatus.objectCollNullSpace(i, i);
            }
            for (int i = 0; i < rtStatus.jointLimNullSpace.diagonalSize(); ++i)
            {
                // the computed values are only on the diagonal, so only these need to be filtered
                rtStatus.jointLimNullSpaceFiltered(i, i) =
                    (1 - c.safetyValFilter) * rtStatus.jointLimNullSpaceFiltered(i, i) +
                    c.safetyValFilter * rtStatus.jointLimNullSpace(i, i);
            }
            /// assign filtered values
            rtStatus.selfCollisionJointVel = rtStatus.selfCollisionVelFiltered;
            rtStatus.jointLimitJointVel = rtStatus.jointLimitVelFiltered;
            rtStatus.selfCollNullSpace = rtStatus.selfCollNullSpaceFiltered;
            rtStatus.jointLimNullSpace = rtStatus.jointLimNullSpaceFiltered;
 
            rtStatus.objectCollisionJointVel = rtStatus.objectCollisionVelFiltered;
            rtStatus.objectCollNullSpace = rtStatus.objectCollNullSpaceFiltered;
        }
 
        ARMARX_TRACE_LITE;
        /// ----------------------------- hierarchical control  --------------------------------------------------
        /// The following control hierarchies are considered:
        ///
        /// there are 4 components generating torque:
        /// - SelfCollisionAvoidance
        /// - ObjectCollisionAvoidance
        /// - JointLimitAvoidance
        /// - Impedance
        ///
        /// For each of the enabled components a priority is loaded from the config.
        /// The impedance component is always enabled.
        ///
        /// The components are processed in descending order from high priority value to low priority value
        ///
        /// If two components a and b have the same priority value their torques are added and their null spaces combined:
        /// torque_combined = torque_a + torque_b
        /// null_space_combined = null_space_a * null_space_b
        ///
        /// The torques of two component groups a and b (with a representing all components with a higher priority value
        /// than priority_b and b representing all components with a priority value of priority_b) are calculated like this:
        /// torque_combined = torque_b + (null_space_b * torque_a)
        ///
        /// Iteratively all torques are combined this way.
 
        //        ARMARX_INFO << VAROUT(c.samePriority);
        controllerHierarchy.clear();
 
        if (c.enableSelfCollisionAvoidance)
        {
            controllerHierarchy.emplace_back(c.selfCollisionAvoidancePriority,
                                             std::ref(rtStatus.selfCollisionJointVel),
                                             std::ref(rtStatus.selfCollNullSpace));
        }
        if (c.enableObjectCollisionAvoidance)
        {
            controllerHierarchy.emplace_back(c.objectCollisionAvoidancePriority,
                                             std::ref(rtStatus.objectCollisionJointVel),
                                             std::ref(rtStatus.objectCollNullSpace));
        }
        if (c.enableJointLimitAvoidance)
        {
            controllerHierarchy.emplace_back(c.jointLimitAvoidancePriority,
                                             std::ref(rtStatus.jointLimitJointVel),
                                             std::ref(rtStatus.jointLimNullSpace));
        }
        controllerHierarchy.emplace_back(c.impedancePriority,
                                         std::ref(rtStatus.trajFollowJointVel),
                                         std::ref(rtStatus.impedanceNullSpace));
 
        // sort inplace according to priority
        sortHierarchy();
 
 
        uint32_t index = 0;
 
        rtStatus.finalTorque.setZero();
 
        while (index < controllerHierarchy.size())
        {
            auto& [priority, torque, nullspace] = controllerHierarchy[index];
            rtStatus.nullSpaceAcc = nullspace.get();
            rtStatus.torqueAcc = torque.get();
 
            index++;
            while ((index < controllerHierarchy.size()) &&
                   (std::get<0>(controllerHierarchy[index]) == priority))
            {
                rtStatus.nullSpaceAcc *= std::get<2>(controllerHierarchy[index]).get();
                rtStatus.torqueAcc += std::get<1>(controllerHierarchy[index]).get();
                index++;
            }
            rtStatus.finalTorque =
                (rtStatus.torqueAcc + (rtStatus.nullSpaceAcc * rtStatus.finalTorque));
        }
 
        rtStatus.desiredJointVel = rtStatus.finalTorque /* + rtStatus.kdImpedanceTorque*/;
 
        ARMARX_TRACE_LITE;
 
        /// ------------------------- write impedance forces to buffer  -----------------------------------------
        // this section is purely for visualization purposes in the viewer
        //        rtStatus.dirErrorImp = rtStatus.poseErrorImp.head<3>().normalized();
        //        rtStatus.totalForceImpedance = rtStatus.forceImpedance.head<3>().norm();
        //        rtStatus.projForceImpedance = rtStatus.jtpinv * rtStatus.projtrajFollowJointVel;
        //        rtStatus.projTotalForceImpedance = rtStatus.projForceImpedance.head<3>().norm();
        //        rtStatus.impForceRatio = rtStatus.projTotalForceImpedance / rtStatus.totalForceImpedance;
        //        rtStatus.impTorqueRatio =
        //            rtStatus.projtrajFollowJointVel.norm() / rtStatus.trajFollowJointVel.norm();
 
 
        /// ----------------------------- write torque target  --------------------------------------------------
        for (int i = 0; i < rtStatus.desiredJointVel.rows(); ++i)
        {
            rtStatus.desiredJointVel(i) =
                std::clamp(rtStatus.desiredJointVel(i), -velocityLimit, velocityLimit);
        }
    }
 
    void
    ObjectCollisionAvoidanceVelController::sortHierarchy()
    {
        for (size_t i = 1; i < controllerHierarchy.size(); i++)
        {
            auto element = controllerHierarchy[i];
            const unsigned int priority = std::get<0>(element);
 
            int j = i - 1;
            while (j >= 0 && std::get<0>(controllerHierarchy[j]) < priority)
            {
                controllerHierarchy[j + 1] = controllerHierarchy[j];
                j--;
            }
            controllerHierarchy[j + 1] = element;
        }
    }
 
} // namespace armarx::control::common::control_law