ObjectCollisionAvoidance.cpp

Go to the documentation of this file.

#include "ObjectCollisionAvoidance.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
{
    ObjectCollisionAvoidanceController::ObjectCollisionAvoidanceController(
        const simox::control::robot::NodeSetInterface* nodeSet) :
        CollisionAvoidanceController(nodeSet)
    {
        controllerHierarchy.reserve(4); // number of torque types to consider
    }
 
    void
    ObjectCollisionAvoidanceController::calculateObjectCollisionNullspace(
        const common::control_law::arondto::ObjectCollisionAvoidanceConfig& 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 =
                getIdentityMat() -
                (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.impedanceJointTorque.rows());
                for (int i = 0; i < rtStatus.impedanceJointTorque.rows(); ++i)
                {
                    if ((active.projectedJacT(i) < 0.0f and
                         rtStatus.impedanceJointTorque(i) < 0.0f) ||
                        (active.projectedJacT(i) > 0.0f and
                         rtStatus.impedanceJointTorque(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
    ObjectCollisionAvoidanceController::calculateExternalCollisionTorque(
        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.objectCollisionJointTorque.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(
                    "CollisionAvoidanceController",
                    "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->CollisionAvoidanceController::calculateCollisionTorque(
                externalCollDataVec,
                c,
                rts,
                collisionRobotIndices,
                qvelFiltered,
                rts.objectCollisionJointTorque,
                c.objectDistanceThreshold,
                rState.collDistanceThreshold);
        }
    }
 
    /// --------------------------------- main rt-loop ------------------------------------------
 
    void
    ObjectCollisionAvoidanceController::run(const Config& c,
                                            RtStatusForSafetyStrategy& rtStatus,
                                            RecoveryState& rStateSelfColl,
                                            RecoveryState& rStateObjColl,
                                            const DistanceResults& collisionPairs,
                                            const DistanceResults& externalCollisionPairs,
                                            const CollisionRobotIndices& collisionRobotIndices,
                                            DynamicsModel& dynamicsModel,
                                            float torqueLimit,
                                            float jointVelLimit,
                                            TSCtrlRtStatus& rts)
    {
        /// run in rt thread
        /// ----------------------------- inertia calculations --------------------------------------------------
        dynamicsModel.getInertiaMatrix(rts.jointPosition.cast<double>(), rtStatus.inertia);
        rts.inertia = rtStatus.inertia;
 
        // rtStatus.inertiaInverse = rtStatus.inertia.cast<float>().inverse();
        // inertia is positive definite matrix
        rtStatus.inertiaInverse = rtStatus.inertia.cast<float>().llt().solve(getIdentityMat());
 
        /// ----------------------------- safety constraints --------------------------------------------------
        if (c.enableSelfCollisionAvoidance)
        {
            // calculation of self-collision avoidance torque
            const double timeMeasure = IceUtil::Time::now().toMicroSecondsDouble();
            calculateSelfCollisionTorque(c,
                                         rtStatus,
                                         rStateSelfColl,
                                         collisionPairs,
                                         collisionRobotIndices,
                                         rts.qvelFiltered,
                                         rts.deltaT);
            rtStatus.collisionTorqueTime =
                IceUtil::Time::now().toMicroSecondsDouble() - timeMeasure;
        }
        if (c.enableObjectCollisionAvoidance)
        {
            // calculation of external collision avoidance torque
            const double timeMeasure = IceUtil::Time::now().toMicroSecondsDouble();
            calculateExternalCollisionTorque(c,
                                             rtStatus,
                                             rStateObjColl,
                                             externalCollisionPairs,
                                             collisionRobotIndices,
                                             rts.qvelFiltered,
                                             rts.deltaT);
            rtStatus.collisionTorqueTime +=
                IceUtil::Time::now().toMicroSecondsDouble() - timeMeasure;
        }
        if (c.enableJointLimitAvoidance)
        {
            // calculation of joint limit avoidance torque
            const double timeMeasure = IceUtil::Time::now().toMicroSecondsDouble();
            calculateJointLimitTorque(c, rtStatus, rts.jointPosition, rts.qvelFiltered);
            rtStatus.jointLimitTorqueTime =
                IceUtil::Time::now().toMicroSecondsDouble() - timeMeasure;
        }
 
        if (!c.samePriority)
        {
            if (c.enableSelfCollisionAvoidance)
            {
                // calculation of null space matrix for self-collision avoidance
                const double timeMeasure = IceUtil::Time::now().toMicroSecondsDouble();
                calculateSelfCollisionNullspace(c, rtStatus);
                rtStatus.selfCollNullspaceTime =
                    IceUtil::Time::now().toMicroSecondsDouble() - timeMeasure;
            }
            if (c.enableObjectCollisionAvoidance)
            {
                // calculation of null space matrix for external collision avoidance
                const double timeMeasure = IceUtil::Time::now().toMicroSecondsDouble();
                calculateObjectCollisionNullspace(c, rtStatus);
                rtStatus.objectCollNullspaceTime =
                    IceUtil::Time::now().toMicroSecondsDouble() - timeMeasure;
            }
            if (c.enableJointLimitAvoidance)
            {
                // calculation of null space matrix for joint limit avoidance
                const double timeMeasure = IceUtil::Time::now().toMicroSecondsDouble();
                calculateJointLimitNullspace(c, rtStatus, rts.jointPosition);
                rtStatus.jointLimitNullspaceTime =
                    IceUtil::Time::now().toMicroSecondsDouble() - timeMeasure;
            }
        }
        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.selfCollisionJointTorque.size(); ++i)
            {
                rtStatus.selfCollisionTorqueFiltered(i) =
                    (1 - c.safetyValFilter) * rtStatus.selfCollisionTorqueFiltered(i) +
                    c.safetyValFilter * rtStatus.selfCollisionJointTorque(i);
            }
            for (int i = 0; i < rtStatus.objectCollisionJointTorque.size(); ++i)
            {
                rtStatus.objectCollisionTorqueFiltered(i) =
                    (1 - c.safetyValFilter) * rtStatus.objectCollisionTorqueFiltered(i) +
                    c.safetyValFilter * rtStatus.objectCollisionJointTorque(i);
            }
            for (int i = 0; i < rtStatus.jointLimitJointTorque.size(); ++i)
            {
                rtStatus.jointLimitTorqueFiltered(i) =
                    (1 - c.safetyValFilter) * rtStatus.jointLimitTorqueFiltered(i) +
                    c.safetyValFilter * rtStatus.jointLimitJointTorque(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.selfCollisionJointTorque = rtStatus.selfCollisionTorqueFiltered;
            rtStatus.jointLimitJointTorque = rtStatus.jointLimitTorqueFiltered;
            rtStatus.selfCollNullSpace = rtStatus.selfCollNullSpaceFiltered;
            rtStatus.jointLimNullSpace = rtStatus.jointLimNullSpaceFiltered;
 
            rtStatus.objectCollisionJointTorque = rtStatus.objectCollisionTorqueFiltered;
            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
        /// low priority (high priority value) ===> high priority (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)
        {
            if (rtStatus.selfColAdmInterface.active)
            {
                rtStatus.selfColAdmInterface.run(
                    rtStatus.selfCollisionJointTorque, jointVelLimit, rts);
                controllerHierarchy.emplace_back(c.selfCollisionAvoidancePriority,
                                                 std::ref(rtStatus.selfColAdmInterface.jointVel),
                                                 std::ref(rtStatus.selfCollNullSpace));
            }
            else
            {
                controllerHierarchy.emplace_back(c.selfCollisionAvoidancePriority,
                                                 std::ref(rtStatus.selfCollisionJointTorque),
                                                 std::ref(rtStatus.selfCollNullSpace));
            }
        }
        if (c.enableObjectCollisionAvoidance)
        {
            if (rtStatus.objColAdmInterface.active)
            {
                rtStatus.objColAdmInterface.run(
                    rtStatus.objectCollisionJointTorque, jointVelLimit, rts);
                controllerHierarchy.emplace_back(c.objectCollisionAvoidancePriority,
                                                 std::ref(rtStatus.objColAdmInterface.jointVel),
                                                 std::ref(rtStatus.objectCollNullSpace));
            }
            else
            {
                controllerHierarchy.emplace_back(c.objectCollisionAvoidancePriority,
                                                 std::ref(rtStatus.objectCollisionJointTorque),
                                                 std::ref(rtStatus.objectCollNullSpace));
            }
        }
        if (c.enableJointLimitAvoidance)
        {
            if (rtStatus.jointLimAdmInterface.active)
            {
                rtStatus.jointLimAdmInterface.run(
                    rtStatus.jointLimitJointTorque, jointVelLimit, rts);
                controllerHierarchy.emplace_back(c.jointLimitAvoidancePriority,
                                                 std::ref(rtStatus.jointLimAdmInterface.jointVel),
                                                 std::ref(rtStatus.jointLimNullSpace));
            }
            else
            {
                controllerHierarchy.emplace_back(c.jointLimitAvoidancePriority,
                                                 std::ref(rtStatus.jointLimitJointTorque),
                                                 std::ref(rtStatus.jointLimNullSpace));
            }
        }
 
        // TODO find a better way to check velocity mode
        if (rtStatus.selfColAdmInterface.active)
        {
            controllerHierarchy.emplace_back(c.impedancePriority,
                                             std::ref(rts.desiredJointVelocity),
                                             std::ref(rtStatus.impedanceNullSpace));
        }
        else
        {
            controllerHierarchy.emplace_back(c.impedancePriority,
                                             std::ref(rts.desiredJointTorque),
                                             std::ref(rtStatus.impedanceNullSpace));
        }
 
        // sort inplace according to priority
        sortHierarchy();
 
 
        uint32_t index = 0;
 
        // TODO rename the finalTorque ... variables
        rtStatus.finalTorque.setZero();
 
        while (index < controllerHierarchy.size())
        {
            auto& [priority, torque, nullspace] = controllerHierarchy[index];
            rtStatus.nullSpaceAcc = nullspace.get();
            rtStatus.torqueAcc = torque.get();
 
            index++;
            /// If two tasks have the exact same priority, combine them
            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++;
            }
 
            /// Otherwise, do the nullspace projection
            rtStatus.finalTorque =
                (rtStatus.torqueAcc + (rtStatus.nullSpaceAcc * rtStatus.finalTorque));
        }
 
        if (rtStatus.selfColAdmInterface.active)
        {
            rts.desiredJointVelocity = rtStatus.finalTorque;
            /// ----------------------------- write torque target  --------------------------------------------------
            for (int i = 0; i < rts.desiredJointVelocity.rows(); ++i)
            {
                rts.desiredJointVelocity(i) =
                    std::clamp(rts.desiredJointVelocity(i), -jointVelLimit, jointVelLimit);
            }
            //            rtStatus.desiredJointVel = rtStatus.finalTorque;
            //            /// ----------------------------- write torque target  --------------------------------------------------
            //            for (int i = 0; i < rtStatus.desiredJointVel.rows(); ++i)
            //            {
            //                rtStatus.desiredJointVel(i) =
            //                    std::clamp(rtStatus.desiredJointVel(i), -jointVelLimit, jointVelLimit);
            //            }
        }
        else
        {
            rts.desiredJointTorque = rtStatus.finalTorque;
            /// ----------------------------- write torque target  --------------------------------------------------
            for (int i = 0; i < rts.desiredJointTorque.rows(); ++i)
            {
                rts.desiredJointTorque(i) =
                    std::clamp(rts.desiredJointTorque(i), -torqueLimit, torqueLimit);
            }
            //            rtStatus.desiredJointTorques = rtStatus.finalTorque;
            //            /// ----------------------------- write torque target  --------------------------------------------------
            //            for (int i = 0; i < rtStatus.desiredJointTorques.rows(); ++i)
            //            {
            //                rtStatus.desiredJointTorques(i) =
            //                    std::clamp(rtStatus.desiredJointTorques(i), -torqueLimit, torqueLimit);
            //            }
        }
        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.projImpedanceJointTorque;
        //        rtStatus.projTotalForceImpedance = rtStatus.projForceImpedance.head<3>().norm();
        //        rtStatus.impForceRatio = rtStatus.projTotalForceImpedance / rtStatus.totalForceImpedance;
        //        rtStatus.impTorqueRatio =
        //            rtStatus.projImpedanceJointTorque.norm() / rtStatus.impedanceJointTorque.norm();
    }
 
    void
    ObjectCollisionAvoidanceController::sortHierarchy()
    {
        /// This sorts the controllerHierarchy vector in Descending Order based on the
        /// priority value (integer). This means controllers with Lower Priority are
        /// placed at the beginning
        ///
        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