CollisionAvoidanceVel.cpp

Go to the documentation of this file.

#include "CollisionAvoidanceVel.h"
 
#include <SimoxUtility/math/convert/mat4f_to_quat.h>
#include <VirtualRobot/MathTools.h>
#include <VirtualRobot/Nodes/RobotNode.h>
#include <VirtualRobot/RobotNodeSet.h>
 
#include <simox/control/dynamics/RBDLModel.h>
#include <simox/control/robot/NodeInterface.h>
 
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
 
#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h>
#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValue1DoFActuator.h>
#include <RobotAPI/components/units/RobotUnit/util/ControlThreadOutputBuffer.h>
 
#include <armarx/control/common/utils.h>
 
namespace armarx::control::common::control_law
{
    void
    CollisionAvoidanceVelController::calculateCollisionJointVel(
        CollisionData& collDataVec,
        const Config& c,
        RtStatusForSafetyStrategy& rts,
        const CollisionRobotIndices& collisionRobotIndices,
        const Eigen::VectorXf& qvelFiltered,
        Eigen::VectorXf& jointVel,
        const float distanceThreshold) const
    {
        // calculate jacobian
        {
            const auto* node = collisionRobotIndices.at(collDataVec.node1);
            auto localTransformation = Eigen::Isometry3d::Identity();
            localTransformation.translation() =
                node->getGlobalPose().inverse() * collDataVec.point1.cast<double>();
            const bool result = nodeSet->computeJacobian(
                collDataVec.jacobian,
                *node,
                simox::control::robot::JacobianParams{
                    .mode = simox::control::utils::CartesianSelection::Position,
                    .localTransformation = localTransformation});
            ARMARX_CHECK_EXPRESSION(result);
        }
 
        float distThrSquare = distanceThreshold * distanceThreshold;
        float factor = c.maxRepulsiveVel / distThrSquare;
        float diff = collDataVec.minDistance - distanceThreshold;
        /// calculate common collision pair values
        collDataVec.repulsiveVel = factor * diff * diff;
        collDataVec.localStiffness = -2.0 * factor * diff;
        // minimum distance is always lower than the threshold distance
        // (local stiffness is always positive)
        collDataVec.repulsiveVel = std::clamp(collDataVec.repulsiveVel, 0.0f, c.maxRepulsiveVel);
        ARMARX_CHECK_NONNEGATIVE(collDataVec.repulsiveVel);
        ARMARX_CHECK_NONNEGATIVE(collDataVec.localStiffness);
 
        ARMARX_CHECK_EQUAL(3, collDataVec.direction.rows());
        // projected in direction of collision (1xn), is saved transposed (nx1)
        collDataVec.projectedJacT =
            (collDataVec.direction.transpose() * collDataVec.jacobian.cast<float>()).transpose();
        ARMARX_CHECK_EQUAL(1, collDataVec.projectedJacT.cols());
        ARMARX_CHECK_EQUAL(numNodes(), collDataVec.projectedJacT.rows());
        ARMARX_CHECK_EQUAL(numNodes(), qvelFiltered.rows());
 
        collDataVec.distanceVelocity = collDataVec.projectedJacT.transpose() * qvelFiltered;
 
        ARMARX_CHECK_EQUAL(numNodes(), rts.inertiaInverse.rows());
        ARMARX_CHECK_EQUAL(numNodes(), rts.inertiaInverse.cols());
 
        collDataVec.projectedMass = 1 / (collDataVec.projectedJacT.transpose() *
                                         rts.inertiaInverse * collDataVec.projectedJacT);
 
        ARMARX_CHECK_NONNEGATIVE(collDataVec.projectedMass);
 
        collDataVec.damping = 2 * std::sqrt(collDataVec.localStiffness) *
                              std::sqrt(collDataVec.projectedMass) * c.dampingFactor;
 
        if (rts.inRecoveryMode)
        {
            collDataVec.damping *= c.recoveryDampingFactor;
        }
 
        ARMARX_CHECK_NONNEGATIVE(collDataVec.damping);
        const float projectedJacNorm =
            collDataVec.projectedJacT.transpose() * collDataVec.projectedJacT;
        float scale = 0.0f;
        if (projectedJacNorm > 1e-3)
        {
            scale = 1.0f / projectedJacNorm;
        }
        jointVel += collDataVec.projectedJacT * scale *
                    (collDataVec.repulsiveVel - collDataVec.damping * collDataVec.distanceVelocity);
    }
 
    CollisionAvoidanceVelController::CollisionAvoidanceVelController(
        const simox::control::robot::NodeSetInterface* nodeSet) :
        nodeSet(nodeSet),
        I(nodeSet ? Eigen::MatrixXf::Identity(nodeSet->getSize(), nodeSet->getSize())
                  : Eigen::MatrixXf())
    {
        ARMARX_CHECK_EXPRESSION(nodeSet);
        ARMARX_CHECK_GREATER(numNodes(), 0);
    }
 
    CollisionAvoidanceVelController::~CollisionAvoidanceVelController()
    {
 
        ARMARX_INFO << "Destruction of CollisionAvoidanceVelController";
    }
 
    /// methods for self-collision and joint limit avoidance
    /// avoidance torque methods
    void
    CollisionAvoidanceVelController::calculateSelfCollisionVel(
        const Config& c,
        RtStatusForSafetyStrategy& rts,
        RecoveryState& rState,
        const DistanceResults& collisionPairs,
        const CollisionRobotIndices& collisionRobotIndices,
        const Eigen::VectorXf& qvelFiltered,
        double deltaT) const
    {
        /// self-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 self-collisions -> collisionJointTorque
 
        ARMARX_CHECK_GREATER(collisionPairs.size(), 0);
 
        /// clear values before new cycle
        rts.selfCollisionJointVel.setZero();
        rts.activeCollPairsNum = 0;
 
        for (auto& data : rts.collDataVec)
        {
            data.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.selfDistanceThreshold;
            for (const DistanceResult& collisionPair : collisionPairs)
            {
                float minDist = static_cast<float>(collisionPair.minDistance);
                if (minDist < thresholdInit)
                {
                    thresholdInit = std::max(minDist, 0.0f);
                }
            }
            thresholdInit =
                std::min(c.selfDistanceThreshold, thresholdInit + c.recoveryDistanceElapseMeter);
        }
 
        rState.update(c, rts, deltaT);
 
        /// check collision pairs, which have are below the prefilter distance
        for (const DistanceResult& collisionPair : collisionPairs)
        {
            if (static_cast<float>(collisionPair.minDistance) >= rState.collDistanceThreshold)
            {
                continue;
            }
 
            if (collisionRobotIndices.count(collisionPair.node1) == 0 and
                collisionRobotIndices.count(collisionPair.node2) == 0)
            {
                continue;
            }
 
            ARMARX_CHECK(collisionPair.hasPoints());
 
            auto& selfCollDataVec = rts.collDataVec[rts.activeCollPairsNum++];
 
            selfCollDataVec.minDistance = static_cast<float>(collisionPair.minDistance);
 
            bool flipped = false;
            if (collisionRobotIndices.count(collisionPair.node1) > 0)
            {
                // First node on nodeset
 
                if (collisionRobotIndices.count(collisionPair.node2) > 0)
                {
                    // Check that second node comes later (equals larger index)
                    ARMARX_CHECK_GREATER(collisionPair.node2, collisionPair.node1);
                }
 
                selfCollDataVec.node1 = collisionPair.node1;
                selfCollDataVec.node2 = collisionPair.node2;
                selfCollDataVec.point1 = collisionPair.point1->cast<float>();
                selfCollDataVec.point2 = collisionPair.point2->cast<float>();
            }
            else
            {
                // Second node on nodeset -> flip collision pair
 
                flipped = true;
                selfCollDataVec.node1 = collisionPair.node2;
                selfCollDataVec.node2 = collisionPair.node1;
                selfCollDataVec.point1 = collisionPair.point2->cast<float>();
                selfCollDataVec.point2 = collisionPair.point1->cast<float>();
            }
 
            // direction is pointing away from collision
            selfCollDataVec.direction = selfCollDataVec.point1 - selfCollDataVec.point2;
 
            if (selfCollDataVec.minDistance < 0.0f)
            {
                selfCollDataVec.minDistance = 0.0f;
 
                // check for overlapping spheres or capsules overlapping spheres
                // (returned points are exactly the same)
                if (selfCollDataVec.point1.isApprox(selfCollDataVec.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
                    selfCollDataVec.direction = collisionPair.normalVec->cast<float>();
                    if (flipped)
                    {
                        selfCollDataVec.direction *= -1.0f;
                    }
                }
                else
                {
                    selfCollDataVec.direction *= -1.0f;
                }
            }
 
            selfCollDataVec.direction.normalize();
 
            calculateCollisionJointVel(selfCollDataVec,
                                       c,
                                       rts,
                                       collisionRobotIndices,
                                       qvelFiltered,
                                       rts.selfCollisionJointVel,
                                       rState.collDistanceThreshold);
        }
        rts.objectCollDataIndex = rts.activeCollPairsNum;
    }
 
    void
    CollisionAvoidanceVelController::calculateJointLimitVel(
        const Config& c,
        RtStatusForSafetyStrategy& rts,
        const Eigen::VectorXf& qpos,
        const Eigen::VectorXf& qvelFiltered) const
    {
        /// method:
        ///
        /// computes the joint torques to avoid joint limits -> jointLimitJointVel
 
        rts.jointLimitJointVel.setZero();
        const size_t n = rts.jointLimitData.size();
        ARMARX_CHECK_EQUAL(numNodes(), rts.jointLimitJointVel.rows());
 
        ARMARX_CHECK_EQUAL(numNodes(), n);
        ARMARX_CHECK_EQUAL(numNodes(), rts.inertia.cols());
        ARMARX_CHECK_EQUAL(numNodes(), rts.inertia.rows());
 
        /// iterate over all joints and check the joint positions for not limiteless joints
        for (size_t i = 0; i < n; ++i)
        {
            auto& joint = rts.jointLimitData[i];
            if (joint.isLimitless)
            {
                continue;
            }
 
            float diff = 0.0f;
            float sign = 1.0f;
            // 1. Determine penetration depth and direction
            if (qpos[i] < joint.qposThresholdLow)
            {
                diff = joint.qposThresholdLow - qpos[i];
                sign = 1.0f;
            }
            else if (qpos[i] > joint.qposThresholdHigh)
            {
                diff = qpos[i] - joint.qposThresholdHigh;
                sign = -1.0f;
            }
            else
            {
                continue; // Within safe limits
            }
 
            // 2. Pre-calculate common factors, done in the init process
            // maxRepulsiveJointVel = 1 / (thresholdRange * thresholdRange)
            // kRepulsive = c.maxRepulsiveJointVel * joint.invThresholdSq;
 
            // 3. Compute Repulsive Velocity (Quadratic)
            joint.repulsiveJointVel =
                std::min(joint.kRepulsive * (diff * diff), c.maxRepulsiveJointVel);
 
            // 4. Compute Damping
            // Stiffness is the derivative of the velocity profile
            const float localStiffness = 2.0f * joint.kRepulsive * diff;
            const float jointInertia = rts.inertia(i, i);
 
            // Critical Damping: 2 * sqrt(K * M) * zeta
            const float dampingGain =
                2.0f * std::sqrt(jointInertia * localStiffness) * c.dampingFactor;
 
            joint.totalDamping = dampingGain * qvelFiltered[i];
 
            // 5. Apply result
            // For Low limit:  pos_vel - damping
            // For High limit: -(pos_vel + damping) => -pos_vel - damping
            rts.jointLimitJointVel(i) = (sign * joint.repulsiveJointVel) - joint.totalDamping;
        }
    }
 
    /// caclulate null spaces
    void
    CollisionAvoidanceVelController::calculateSelfCollisionNullspace(
        const common::control_law::arondto::CollisionAvoidanceVelConfig& c,
        RtStatusForSafetyStrategy& rtStatus) const
    {
        rtStatus.selfCollNullSpace.setIdentity();
 
        ARMARX_CHECK_EQUAL(numNodes(), rtStatus.selfCollNullSpace.rows());
        ARMARX_CHECK_EQUAL(numNodes(), rtStatus.selfCollNullSpace.cols());
 
        ARMARX_CHECK_EQUAL(4, rtStatus.selfCollisionNullSpaceWeights.rows());
        rtStatus.selfCollK1 = rtStatus.selfCollisionNullSpaceWeights(0);
        rtStatus.selfCollK2 = rtStatus.selfCollisionNullSpaceWeights(1);
        rtStatus.selfCollK3 = rtStatus.selfCollisionNullSpaceWeights(2);
        rtStatus.selfCollK4 = rtStatus.selfCollisionNullSpaceWeights(3);
 
        ARMARX_TRACE_LITE;
 
        for (unsigned int i = 0; i < rtStatus.objectCollDataIndex; i++)
        {
            auto& active = rtStatus.collDataVec[i];
 
            /// get desired null space value based on proximity to collision
            if (active.minDistance < c.selfCollActivatorZ1)
            {
                /// full locking in direction of collision
                rtStatus.desiredNullSpace = 0.0f;
            }
            else if (c.selfCollActivatorZ1 <= active.minDistance &&
                     active.minDistance <= c.selfCollActivatorZ2)
            {
                /// in transition state
                rtStatus.desiredNullSpace = rtStatus.selfCollK1 * std::pow(active.minDistance, 3) +
                                            rtStatus.selfCollK2 * std::pow(active.minDistance, 2) +
                                            rtStatus.selfCollK3 * active.minDistance +
                                            rtStatus.selfCollK4;
                if (rtStatus.desiredNullSpace > 1.0f)
                {
                    ARMARX_RT_LOGF_WARNING(
                        "desired null space value should not be higher than 1.0");
                    //                    ARMARX_WARNING
                    //                        << "desired null space value should not be higher than 1.0, was "
                    //                        << rtStatus.desiredNullSpace
                    //                        << ", in collision null space calulation, weights: " << rtStatus.selfCollK1
                    //                        << ", " << rtStatus.selfCollK2 << ", " << rtStatus.selfCollK3 << ", "
                    //                        << rtStatus.selfCollK4;
                }
            }
            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.selfCollNormalizedJacT = active.projectedJacT.normalized();
 
            ARMARX_CHECK_EQUAL(numNodes(), rtStatus.selfCollNormalizedJacT.rows());
 
            rtStatus.selfCollTempNullSpaceMatrix =
                I - (rtStatus.selfCollNormalizedJacT * (1.0f - rtStatus.desiredNullSpace) *
                     rtStatus.selfCollNormalizedJacT.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.selfCollTempNullSpaceMatrix(i, i) = 1.0f;
                    }
                }
            }
 
            /// project desired null space in corresponding direction via the Norm-Jacobian
            rtStatus.selfCollNullSpace *= rtStatus.selfCollTempNullSpaceMatrix;
            // todo: clamp the diagonal values between 0 and 1
        }
    }
 
    void
    CollisionAvoidanceVelController::calculateJointLimitNullspace(const Config& c,
                                                                  RtStatusForSafetyStrategy& rts,
                                                                  const Eigen::VectorXf& qpos) const
    {
        /// check fundamentals thesis BA Jan Fenker
        rts.jointLimNullSpace.setIdentity();
 
        ARMARX_CHECK_EQUAL(numNodes(), rts.jointLimNullSpace.rows());
        ARMARX_CHECK_EQUAL(numNodes(), rts.jointLimNullSpace.cols());
 
        ARMARX_CHECK_EQUAL(numNodes(), rts.jointLimitData.size());
 
        for (size_t i = 0; i < rts.jointLimitData.size(); ++i)
        {
            auto& joint = rts.jointLimitData[i];
            ARMARX_CHECK_EQUAL(4, joint.jointLimitNullSpaceWeightsLow.rows());
            ARMARX_CHECK_EQUAL(4, joint.jointLimitNullSpaceWeightsHigh.rows());
            rts.k1Lo = joint.jointLimitNullSpaceWeightsLow(0);
            rts.k2Lo = joint.jointLimitNullSpaceWeightsLow(1);
            rts.k3Lo = joint.jointLimitNullSpaceWeightsLow(2);
            rts.k4Lo = joint.jointLimitNullSpaceWeightsLow(3);
            rts.k1Hi = joint.jointLimitNullSpaceWeightsHigh(0);
            rts.k2Hi = joint.jointLimitNullSpaceWeightsHigh(1);
            rts.k3Hi = joint.jointLimitNullSpaceWeightsHigh(2);
            rts.k4Hi = joint.jointLimitNullSpaceWeightsHigh(3);
 
            ARMARX_TRACE_LITE;
 
            if (not joint.isLimitless)
            {
                /// get desired null space value based on proximity to joint boundaries
                if (qpos[i] < joint.qposZ1Low)
                {
                    /// full locking
                    rts.desiredNullSpace = 0.0f;
                }
                else if (joint.qposZ1Low <= qpos[i] && qpos[i] <= joint.qposZ2Low)
                {
                    /// in transition state
                    rts.desiredNullSpace = rts.k1Lo * std::pow(qpos[i], 3) +
                                           rts.k2Lo * std::pow(qpos[i], 2) + rts.k3Lo * qpos[i] +
                                           rts.k4Lo;
                    if (rts.desiredNullSpace > 1.0f)
                    {
                        ARMARX_WARNING
                            << "desired null space value should not be higher than 1.0, was "
                            << rts.desiredNullSpace
                            << ", approaching lower joint limit, weights: " << rts.k1Lo << ", "
                            << rts.k2Lo << ", " << rts.k3Lo << ", " << rts.k4Lo;
                    }
                }
                else if (joint.qposZ2High <= qpos[i] && qpos[i] <= joint.qposZ1High)
                {
                    /// in transition state
                    rts.desiredNullSpace = rts.k1Hi * std::pow(qpos[i], 3) +
                                           rts.k2Hi * std::pow(qpos[i], 2) + rts.k3Hi * qpos[i] +
                                           rts.k4Hi;
                    if (rts.desiredNullSpace > 1.0f)
                    {
                        ARMARX_WARNING
                            << "desired null space value should not be higher than 1.0, was "
                            << rts.desiredNullSpace
                            << ", approaching upper joint limit, weights: " << rts.k1Hi << ", "
                            << rts.k2Hi << ", " << rts.k3Hi << ", " << rts.k4Hi;
                    }
                }
                else if (joint.qposZ1High < qpos[i])
                {
                    /// full locking
                    rts.desiredNullSpace = 0.0f;
                }
                else
                {
                    /// unrestricted movement
                    rts.desiredNullSpace = 1.0f;
                }
 
 
                joint.desiredNSjointLim = rts.desiredNullSpace;
                rts.desiredNullSpace = std::clamp(rts.desiredNullSpace, 0.0f, 1.0f);
                ARMARX_CHECK_NONNEGATIVE(rts.desiredNullSpace);
 
                /// set desired null space value in null space matrix
                rts.jointLimNullSpace(i, i) = 1.0f - (1.0f - rts.desiredNullSpace);
            }
        }
    }
 
    /// --------------------------------- main rt-loop ------------------------------------------
 
    void
    CollisionAvoidanceVelController::run(const Config& c,
                                         RtStatusForSafetyStrategy& rtStatus,
                                         RecoveryState& rStateSelfColl,
                                         const DistanceResults& collisionPairs,
                                         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.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.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.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.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;
        }
 
        ARMARX_TRACE_LITE;
        /// ----------------------------- hierarchical control  --------------------------------------------------
        /// The following control hierarchies are considered:
        ///
        /// three tasks in hierarchy: "enableSelfCollisionAvoidance": true, "enableJointLimitAvoidance": true
        ///
        /// flag: "samePriority": true
        /// same priority: torque_coll + torque_jl + torque_imp
        ///
        /// flag: "topPrioritySelfCollisionAvoidance": true, "samePriority": false
        /// self-coll avoidance on top: torque_coll -> torque_jl -> torque_imp
        ///
        /// flag: "topPriorityJointLimitAvoidance": true, "samePriority": false
        /// joint limit avoidance on top: torque_jl -> torque_coll -> torque_imp
        ///
        /// flag: "topPrioritySelfCollisionAvoidance": false, "topPriorityJointLimitAvoidance": false,
        /// "samePriority": false
        /// (torque_coll + torque_jl) -> torque_imp
        /// self-coll and joint limit avoidance on same priority level:
        ///
        /// two tasks in hierarchy:
        ///
        /// "enableSelfCollisionAvoidance": true, "enableJointLimitAvoidance": false
        /// self-coll avoidance on top: torque_coll -> torque_imp
        ///
        /// "enableSelfCollisionAvoidance": false, "enableJointLimitAvoidance": true
        /// joint limit avoidance on top: torque_jl -> torque_imp
        ///
        /// one task in hierarchy (impedance control):
        ///
        /// "enableSelfCollisionAvoidance": false, "enableJointLimitAvoidance": false
        /// only impedance control: torque_imp
 
        if (c.samePriority)
        {
            ARMARX_CHECK_EQUAL(numNodes(), rtStatus.selfCollisionJointVel.rows());
            ARMARX_CHECK_EQUAL(numNodes(), rtStatus.jointLimitJointVel.rows());
            ARMARX_CHECK_EQUAL(numNodes(), rtStatus.trajFollowJointVel.rows());
            rtStatus.desiredJointVel = rtStatus.selfCollisionJointVel +
                                       rtStatus.jointLimitJointVel +
                                       rtStatus.trajFollowJointVel /* +
                                       rtStatus.kdImpedanceTorque*/
                ;
            ARMARX_TRACE_LITE;
        }
        else if (c.enableSelfCollisionAvoidance and c.enableJointLimitAvoidance)
        {
            /// three tasks in hierarchy considered
 
            if (c.topPrioritySelfCollisionAvoidance)
            {
 
                ARMARX_CHECK_EQUAL(numNodes(), rtStatus.selfCollNullSpace.rows());
                ARMARX_CHECK_EQUAL(numNodes(), rtStatus.selfCollNullSpace.cols());
                ARMARX_CHECK_EQUAL(rtStatus.trajFollowJointVel.rows(),
                                   rtStatus.selfCollNullSpace.cols());
                ARMARX_CHECK_EQUAL(numNodes(), rtStatus.jointLimNullSpace.rows());
                ARMARX_CHECK_EQUAL(numNodes(), rtStatus.jointLimNullSpace.cols());
                ARMARX_CHECK_EQUAL(rtStatus.jointLimitJointVel.rows(),
                                   rtStatus.selfCollNullSpace.cols());
 
                rtStatus.desiredJointVel =
                    rtStatus.selfCollisionJointVel +
                    rtStatus.selfCollNullSpace *
                        (rtStatus.jointLimitJointVel +
                         rtStatus.jointLimNullSpace * rtStatus.trajFollowJointVel) /* +
                    rtStatus.kdImpedanceTorque*/
                    ;
 
                //                // debug values
                //                rtStatus.projJointLimJointTorque =
                //                    rtStatus.selfCollNullSpace * rtStatus.jointLimitJointVel;
                //                rtStatus.projtrajFollowJointVel =
                //                    rtStatus.selfCollNullSpace *
                //                    (rtStatus.jointLimNullSpace * rtStatus.trajFollowJointVel);
            }
            else if (c.topPriorityJointLimitAvoidance)
            {
                ARMARX_CHECK_EQUAL(numNodes(), rtStatus.selfCollNullSpace.rows());
                ARMARX_CHECK_EQUAL(numNodes(), rtStatus.selfCollNullSpace.cols());
                ARMARX_CHECK_EQUAL(rtStatus.trajFollowJointVel.rows(),
                                   rtStatus.selfCollNullSpace.cols());
                ARMARX_CHECK_EQUAL(numNodes(), rtStatus.jointLimNullSpace.rows());
                ARMARX_CHECK_EQUAL(numNodes(), rtStatus.jointLimNullSpace.cols());
                ARMARX_CHECK_EQUAL(rtStatus.selfCollisionJointVel.rows(),
                                   rtStatus.jointLimNullSpace.cols());
 
                rtStatus.desiredJointVel =
                    rtStatus.jointLimitJointVel +
                    rtStatus.jointLimNullSpace *
                        (rtStatus.selfCollisionJointVel +
                         rtStatus.selfCollNullSpace * rtStatus.trajFollowJointVel) /* +
                    rtStatus.kdImpedanceTorque*/
                    ;
 
                //                // debug values
                //                rtStatus.projCollJointTorque =
                //                    rtStatus.jointLimNullSpace * rtStatus.selfCollisionJointVel;
                //                rtStatus.projtrajFollowJointVel =
                //                    rtStatus.jointLimNullSpace *
                //                    (rtStatus.selfCollNullSpace * rtStatus.trajFollowJointVel);
            }
            else
            {
                ARMARX_CHECK_EQUAL(numNodes(), rtStatus.selfCollNullSpace.rows());
                ARMARX_CHECK_EQUAL(numNodes(), rtStatus.selfCollNullSpace.cols());
                ARMARX_CHECK_EQUAL(rtStatus.trajFollowJointVel.rows(),
                                   rtStatus.selfCollNullSpace.cols());
                ARMARX_CHECK_EQUAL(numNodes(), rtStatus.jointLimNullSpace.rows());
                ARMARX_CHECK_EQUAL(numNodes(), rtStatus.jointLimNullSpace.cols());
                ARMARX_CHECK_EQUAL(rtStatus.selfCollisionJointVel.rows(),
                                   rtStatus.jointLimNullSpace.cols());
                rtStatus.desiredJointVel =
                    rtStatus.jointLimitJointVel + rtStatus.selfCollisionJointVel +
                    (rtStatus.jointLimNullSpace * rtStatus.selfCollNullSpace) *
                        rtStatus.trajFollowJointVel /* +
                    rtStatus.kdImpedanceTorque*/
                    ;
 
                //                // debug values
                //                rtStatus.projtrajFollowJointVel =
                //                    (rtStatus.jointLimNullSpace * rtStatus.selfCollNullSpace) *
                //                    rtStatus.trajFollowJointVel;
            }
        }
        else if (c.enableSelfCollisionAvoidance)
        {
            /// impedance control and self-collision avoidance
 
            ARMARX_CHECK_EQUAL(numNodes(), rtStatus.selfCollNullSpace.rows());
            ARMARX_CHECK_EQUAL(numNodes(), rtStatus.selfCollNullSpace.cols());
            ARMARX_CHECK_EQUAL(rtStatus.trajFollowJointVel.rows(),
                               rtStatus.selfCollNullSpace.cols());
 
            rtStatus.desiredJointVel = rtStatus.selfCollisionJointVel +
                                       rtStatus.selfCollNullSpace * rtStatus.trajFollowJointVel /* +
                                       rtStatus.kdImpedanceTorque*/
                ;
 
            //            // debugging
            //            rtStatus.projtrajFollowJointVel =
            //                rtStatus.selfCollNullSpace * rtStatus.trajFollowJointVel;
        }
        else if (c.enableJointLimitAvoidance)
        {
            /// impedance control and joint limit avoidance
 
            ARMARX_CHECK_EQUAL(numNodes(), rtStatus.jointLimNullSpace.rows());
            ARMARX_CHECK_EQUAL(numNodes(), rtStatus.jointLimNullSpace.cols());
            ARMARX_CHECK_EQUAL(rtStatus.trajFollowJointVel.rows(),
                               rtStatus.jointLimNullSpace.cols());
 
            rtStatus.desiredJointVel = rtStatus.jointLimitJointVel +
                                       rtStatus.jointLimNullSpace * rtStatus.trajFollowJointVel /* +
                                       rtStatus.kdImpedanceTorque*/
                ;
 
            //            // debugging
            //            rtStatus.projtrajFollowJointVel =
            //                rtStatus.jointLimNullSpace * rtStatus.trajFollowJointVel;
        }
        else
        {
            /// plain impedance control
            rtStatus.desiredJointVel =
                rtStatus.trajFollowJointVel /* + rtStatus.kdImpedanceTorque*/;
        }
 
        //        rtStatus.desiredJointVel += rtStatus.jointDampingTorque;
 
        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
    CollisionAvoidanceVelController::RtStatusForSafetyStrategy::reset(
        const Config& c,
        const unsigned int nDoF,
        const unsigned int maxCollisionPairs,
        VirtualRobot::RobotNodeSetPtr& rns)
    {
        trackingError = 0.0;
        dirErrorImp.setZero();
        totalForceImpedance = 0.0;
        projForceImpedance.setZero();
        projTotalForceImpedance = 0.0;
 
        /// intermediate torque results
        trajFollowJointVel.setZero(nDoF);
        //        kdImpedanceTorque.setZero(nDoF);
        selfCollisionJointVel.setZero(nDoF);
        objectCollisionJointVel.setZero(nDoF);
        jointLimitJointVel.setZero(nDoF);
 
        selfCollisionVelFiltered.setZero(nDoF);
        objectCollisionVelFiltered.setZero(nDoF);
        jointLimitVelFiltered.setZero(nDoF);
 
        /// intermediate projected torques via null space matrices
        //        projtrajFollowJointVel.setZero(nDoF);
        //        projCollJointTorque.setZero(nDoF);
        //        projJointLimJointTorque.setZero(nDoF);
 
        impForceRatio = 0.0f;
        impTorqueRatio = 0.0f;
 
        /// intermediate null space matrices (collision and joint limit avoidance)
        selfCollNullSpace.resize(nDoF, nDoF);
        selfCollNullSpace.setZero();
        jointLimNullSpace.resize(nDoF, nDoF);
        jointLimNullSpace.setZero();
        objectCollNullSpace.resize(nDoF, nDoF);
        objectCollNullSpace.setZero();
        impedanceNullSpace.resize(nDoF, nDoF);
        impedanceNullSpace.setIdentity();
        desiredNullSpace = 0.0;
 
        selfCollNullSpaceFiltered.resize(nDoF, nDoF);
        selfCollNullSpaceFiltered.setZero();
        objectCollNullSpaceFiltered.resize(nDoF, nDoF);
        objectCollNullSpaceFiltered.setZero();
        jointLimNullSpaceFiltered.resize(nDoF, nDoF);
        jointLimNullSpaceFiltered.setZero();
 
        /// self-collision avoidance initialization parameters
        /// calculate weights for self-collision nullspace transition function
        /// range between z1 and z2
        /// z1: below this distance [m] the collision direction is fully locked
        /// z2: above this distance collision direction is unrestricted
        //        collisionNullSpaceWeights.setZero();
        selfCollisionNullSpaceWeights =
            helper::calculateTransitionWeights(c.selfCollActivatorZ1, c.selfCollActivatorZ2);
 
        /// distance results
        Eigen::Vector3d point1(0.0, 0.0, 0.0);
        Eigen::Vector3d point2(0.0, 0.0, 0.0);
        Eigen::Vector3d norm(0.0, 0.0, 0.0);
 
        collDataVec.resize(maxCollisionPairs, CollisionAvoidanceVelController::CollisionData(nDoF));
 
        /// self-collision avoidance null space intermediate results
        selfCollK1 = 0.0;
        selfCollK2 = 0.0;
        selfCollK3 = 0.0;
        selfCollK4 = 0.0;
        selfCollNormalizedJacT.setZero(nDoF);
        selfCollTempNullSpaceMatrix.resize(nDoF, nDoF);
        selfCollTempNullSpaceMatrix.setZero();
 
        /// object-collision avoidance null space intermediate results
        objectCollK1 = 0.0;
        objectCollK2 = 0.0;
        objectCollK3 = 0.0;
        objectCollK4 = 0.0;
        objectCollNormalizedJacT.setZero(nDoF);
        objectCollTempNullSpaceMatrix.resize(nDoF, nDoF);
        objectCollTempNullSpaceMatrix.setZero();
 
        /// joint limit avoidance initialization parameters
        CollisionAvoidanceVelController::RtStatusForSafetyStrategy::initJointLimtDataVec(
            c, nDoF, rns);
 
        /// others
        inertia.resize(nDoF, nDoF);
        inertia.setZero();
        inertiaInverse.resize(nDoF, nDoF);
        inertiaInverse.setZero();
 
        /// time status
        collisionPairTime = 0.0;
        preFilterTime = 0.0;
        collisionTorqueTime = 0.0;
        jointLimitTorqueTime = 0.0;
        selfCollNullspaceTime = 0.0;
        jointLimitNullspaceTime = 0.0;
 
        /// collision pair info
        collisionPairsNum = 0;
        activeCollPairsNum = 0;
 
        /// priority accumulators
        nullSpaceAcc.resize(nDoF, nDoF);
        nullSpaceAcc.setIdentity();
        torqueAcc.resize(nDoF);
        torqueAcc.setZero();
        finalTorque.resize(nDoF);
        finalTorque.setZero();
    }
 
    void
    CollisionAvoidanceVelController::RtStatusForSafetyStrategy::rtPreActivate(const Config& c)
    {
        timeSinceStart = 0.0;
        inRecoveryMode = c.enableCollisionRecoveryOnStartup;
    }
 
    void
    CollisionAvoidanceVelController::RtStatusForSafetyStrategy::initJointLimtDataVec(
        const Config& c,
        const unsigned int nDoF,
        VirtualRobot::RobotNodeSetPtr& rns)
    {
        jointVel = 0.0;
        jointInertia = 0.0;
 
        localStiffnessJointLim = 0.0;
        dampingJointLim = 0.0;
 
        k1Lo = 0.0;
        k2Lo = 0.0;
        k3Lo = 0.0;
        k4Lo = 0.0;
        k1Hi = 0.0;
        k2Hi = 0.0;
        k3Hi = 0.0;
        k4Hi = 0.0;
 
        jointLimitData.resize(nDoF);
 
        /// obtain joint information (limits, limitless) from VirtualRobot
        for (size_t i = 0; i < nDoF; ++i)
        {
            jointLimitData[i].jointName = rns->getNode(i)->getName();
            jointLimitData[i].isLimitless = rns->getNode(i)->isLimitless();
 
            if (not jointLimitData[i].isLimitless)
            {
                jointLimitData[i].qLimLow = rns->getNode(i)->getJointLimitLow();
                jointLimitData[i].qLimHigh = rns->getNode(i)->getJointLimitHigh();
            }
        }
 
        //        std::vector<std::string> jointInfo;
        for (auto& joint : jointLimitData)
        {
            if (not joint.isLimitless)
            {
                /// set joint limit avoidance threshold parameters for not limiteless joints
                float qRange = joint.qLimHigh - joint.qLimLow;
                joint.thresholdRange = c.jointRangeBufferZone * qRange;
                joint.invThresholdSq = 1.0f / (joint.thresholdRange * joint.thresholdRange);
                joint.kRepulsive = c.maxRepulsiveJointVel * joint.invThresholdSq;
                joint.qposThresholdLow = joint.qLimLow + joint.thresholdRange;
                joint.qposThresholdHigh = joint.qLimHigh - joint.thresholdRange;
                float qposZ1 = c.jointRangeBufferZoneZ1 * qRange;
                float qposZ2 = c.jointRangeBufferZoneZ2 * qRange;
                joint.qposZ1Low = joint.qLimLow + qposZ1;
                joint.qposZ2Low = joint.qLimLow + qposZ2;
                joint.qposZ1High = joint.qLimHigh - qposZ1;
                joint.qposZ2High = joint.qLimHigh - qposZ2;
                joint.jointLimitNullSpaceWeightsLow =
                    helper::calculateTransitionWeights(joint.qposZ1Low, joint.qposZ2Low);
                joint.jointLimitNullSpaceWeightsHigh =
                    helper::calculateTransitionWeights(joint.qposZ1High, joint.qposZ2High);
 
                /// from here: only the parameter output information is prepared
                //                std::vector<float> params = {joint.qLimLow,
                //                                             joint.qLimHigh,
                //                                             joint.qposThresholdLow,
                //                                             joint.qposThresholdHigh,
                //                                             joint.qposZ1Low,
                //                                             joint.qposZ2Low,
                //                                             joint.qposZ1High,
                //                                             joint.qposZ2High,
                //                                             joint.jointLimitNullSpaceWeightsLow(0),
                //                                             joint.jointLimitNullSpaceWeightsLow(1),
                //                                             joint.jointLimitNullSpaceWeightsLow(2),
                //                                             joint.jointLimitNullSpaceWeightsLow(3),
                //                                             joint.jointLimitNullSpaceWeightsHigh(0),
                //                                             joint.jointLimitNullSpaceWeightsHigh(1),
                //                                             joint.jointLimitNullSpaceWeightsHigh(2),
                //                                             joint.jointLimitNullSpaceWeightsHigh(3)};
                //                std::vector<std::string> stringParams;
                //                // convert float values to string rounded to two digits
                //                for (auto& param : params)
                //                {
                //                    std::stringstream ss;
                //                    ss << std::fixed << std::setprecision(2) << param;
                //                    stringParams.push_back(ss.str());
                //                }
                //                std::string info = joint.jointName + ": LimLo: " + stringParams[0] +
                //                                   " - LimHi: " + stringParams[1] + " - q0L: " + stringParams[2] +
                //                                   " - q0H: " + stringParams[3] + " - z1L: " + stringParams[4] +
                //                                   " z2L: " + stringParams[5] + " z1H: " + stringParams[6] +
                //                                   " z2H: " + stringParams[7] + " k1L: " + stringParams[8] +
                //                                   " k2L: " + stringParams[9] + " k3L: " + stringParams[10] +
                //                                   " k4L: " + stringParams[11] + " k1H: " + stringParams[12] +
                //                                   " k2H: " + stringParams[13] + " k3H: " + stringParams[14] +
                //                                   " k4H: " + stringParams[15];
                //                jointInfo.push_back(info);
            }
        }
 
        //        std::ostringstream oss;
        //        for (const auto& str : jointInfo)
        //        {
        //            oss << str << "\n";
        //        }
        //        std::string initMessage = oss.str();
        //        ARMARX_INFO << "joint limit avoidance parameters initialized for: \n" << initMessage;
    }
 
    void
    collision::validateConfigData(const arondto::CollisionAvoidanceVelConfig& cfg)
    {
        /// collision config parameters
        ARMARX_CHECK_GREATER(cfg.dampingFactor, 0.0); // damping can't be exactly zero
        ARMARX_CHECK_LESS_EQUAL(cfg.dampingFactor, 1.0); // for scaling of the damping
        ARMARX_CHECK_GREATER(cfg.maxRepulsiveVel, 0.0);
        ARMARX_CHECK_NONNEGATIVE(cfg.maxRepulsiveVel);
        ARMARX_CHECK_NONNEGATIVE(cfg.selfDistanceThreshold);
        ARMARX_CHECK_NONNEGATIVE(cfg.selfCollActivatorZ1);
        ARMARX_CHECK_NONNEGATIVE(cfg.selfCollActivatorZ2);
        ARMARX_CHECK_GREATER(cfg.selfCollActivatorZ2, cfg.selfCollActivatorZ1);
 
        /// joint limit avoidance config parameters
        ARMARX_CHECK_NONNEGATIVE(cfg.maxRepulsiveJointVel);
        ARMARX_CHECK_NONNEGATIVE(cfg.jointRangeBufferZone);
        ARMARX_CHECK_LESS_EQUAL(cfg.jointRangeBufferZone, 1.0); // percentage value
        ARMARX_CHECK_NONNEGATIVE(cfg.jointRangeBufferZoneZ1);
        ARMARX_CHECK_LESS_EQUAL(cfg.jointRangeBufferZoneZ1, 1.0); // percentage value
        ARMARX_CHECK_NONNEGATIVE(cfg.jointRangeBufferZoneZ2);
        ARMARX_CHECK_GREATER(cfg.jointRangeBufferZoneZ2, cfg.jointRangeBufferZoneZ1);
        ARMARX_CHECK_LESS_EQUAL(cfg.jointRangeBufferZoneZ2, 1.0); // percentage value
        ARMARX_CHECK_NONNEGATIVE(cfg.safetyValFilter);
        ARMARX_CHECK_LESS_EQUAL(cfg.safetyValFilter, 1.0);
 
        /// check hierarchy flags for valid config
        if (cfg.topPrioritySelfCollisionAvoidance)
        {
            ARMARX_CHECK_EXPRESSION(cfg.enableSelfCollisionAvoidance);
            ARMARX_CHECK_EXPRESSION(
                (cfg.topPrioritySelfCollisionAvoidance == !cfg.topPriorityJointLimitAvoidance));
        }
        if (cfg.topPriorityJointLimitAvoidance)
        {
            ARMARX_CHECK_EXPRESSION(cfg.enableJointLimitAvoidance);
            ARMARX_CHECK_EXPRESSION(
                (cfg.topPrioritySelfCollisionAvoidance == !cfg.topPriorityJointLimitAvoidance));
        }
    }
 
    void
    CollisionAvoidanceVelController::RecoveryState::update(const Config& c,
                                                        RtStatusForSafetyStrategy& rtStatus,
                                                        double deltaT)
    {
        if (not c.enableCollisionRecoveryOnStartup or not rtStatus.inRecoveryMode)
        {
            collDistanceThreshold = c.selfDistanceThreshold;
            return;
        }
 
        /// 1. determine current time since start
        rtStatus.timeSinceStart += deltaT;
        if (rtStatus.timeSinceStart > c.recoveryDurationSec)
            rtStatus.inRecoveryMode = false;
 
        /// 2. update threshold
        collDistanceThreshold =
            std::min(c.selfDistanceThreshold,
                     collDistanceThresholdInit +
                         (c.selfDistanceThreshold - collDistanceThresholdInit) *
                             static_cast<float>(rtStatus.timeSinceStart) / c.recoveryDurationSec);
    }
 
    void
    CollisionAvoidanceVelController::RecoveryState::reset()
    {
        collDistanceThreshold = 0.0;
        collDistanceThresholdInit = 0.0;
    }
 
} // namespace armarx::control::common::control_law