ColAvoid.cpp

Go to the documentation of this file.

/*
 * This file is part of ArmarX.
 *
 * ArmarX is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License version 2 as
 * published by the Free Software Foundation.
 *
 * ArmarX is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program. If not, see <http://www.gnu.org/licenses/>.
 *
 * @package    ...
 * @author     Jianfeng Gao ( jianfeng dot gao at kit dot edu )
 * @date       2025
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "ColAvoid.h"
 
#include <SimoxUtility/color/Color.h>
#include <VirtualRobot/MathTools.h>
#include <VirtualRobot/RobotNodeSet.h>
 
#include <simox/control/environment/collision.h>
#include <simox/control/geodesics/util.h>
#include <simox/control/robot/NodeInterface.h>
#include <simox/control/utils/primitive.h>
 
#include <ArmarXCore/core/ArmarXObjectScheduler.h>
#include <ArmarXCore/core/PackagePath.h> // for GUI
#include <ArmarXCore/core/time/CycleUtil.h>
#include <ArmarXCore/core/time/TimeUtil.h>
#include <ArmarXCore/observers/variant/SingleTypeVariantList.h> // for GUI
 
#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.h>
#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerRegistry.h>
#include <RobotAPI/components/units/RobotUnit/RobotUnit.h>
 
#include <armarx/control/common/aron_conversions.h> // for GUI
#include <armarx/control/common/control_law/aron/CollisionPrimitives.aron.generated.h>
#include <armarx/control/common/utils.h>
 
namespace armarx::control::njoint_controller::task_space
{
    template <typename NJointControllerType, typename CollisionCtrlCfg>
    NJointTSColController<NJointControllerType, CollisionCtrlCfg>::NJointTSColController(
        const RobotUnitPtr& robotUnit,
        const NJointControllerConfigPtr& config,
        const VirtualRobot::RobotPtr& robot) :
        NJointControllerType{robotUnit, config, robot}
    {
        auto cfg = ConfigPtrT::dynamicCast(config);
        ARMARX_CHECK_EXPRESSION(cfg);
        userCfgWithColl_ = CollisionCtrlCfg::FromAron(cfg->config);
 
        std::map<std::string, std::vector<size_t>> velCtrlIndices;
        for (auto& arm : this->limb)
        {
            velCtrlIndices.emplace(arm.first, arm.second->rtStatus.jointIDVelocityMode);
        }
        coll = std::make_shared<CollAvoidBase>(
            this->rtGetRobot(), userCfgWithColl_.coll, velCtrlIndices);
        collReady_.store(true);
        // const std::string rootNodeName = "root";
        // rootNode = this->rtGetRobot()->getRobotNode(rootNodeName);
    }
 
    template <typename NJointControllerType, typename CollisionCtrlCfg>
    void
    NJointTSColController<NJointControllerType, CollisionCtrlCfg>::limbRT(ArmPtr& arm)
    {
        arm->controller.run(arm->rtConfig, arm->rtStatus);
 
        if (collReady_.load())
        {
            ARMARX_CHECK_EXPRESSION(coll);
            // const std::string rootNodeName = "root";
            // // coll->collLimb.at(arm->kinematicChainName)->rts.globalPose = rootNode->getGlobalPose();
            // coll->collLimb.at(arm->kinematicChainName).rts.globalPose =
            //     this->rtGetRobot()->getGlobalPose();
            coll->rtLimbControllerRun(arm->kinematicChainName,
                                      arm->rtConfig.torqueLimit,
                                      arm->rtConfig.velocityLimit,
                                      arm->rtStatus);
            //            /// experimental implementation of admittance interface
            //            arm->rtStatus.inertia = coll->collLimb.at(arm->kinematicChainName).rts.inertia;
        }
 
        //        if (collReady.load())
        //        {
        //            this->limbRTSetTarget(
        //                arm,
        //                arm->rtStatus.nDoFTorque,
        //                arm->rtStatus.nDoFVelocity,
        //                coll->collLimb.at(arm->kinematicChainName).rts.desiredJointTorques,
        //                arm->rtStatus.desiredJointVelocity);
        //        }
        //        else
        //        {
        //            this->limbRTSetTarget(arm,
        //                                  arm->rtStatus.nDoFTorque,
        //                                  arm->rtStatus.nDoFVelocity,
        //                                  arm->rtStatus.desiredJointTorque,
        //                                  arm->rtStatus.desiredJointVelocity);
        //        }
        this->limbRTSetTarget(arm,
                              arm->rtStatus.nDoFTorque,
                              arm->rtStatus.nDoFVelocity,
                              arm->rtStatus.desiredJointTorque,
                              arm->rtStatus.desiredJointVelocity);
    }
 
    template <typename NJointControllerType, typename CollisionCtrlCfg>
    void
    NJointTSColController<NJointControllerType, CollisionCtrlCfg>::rtRun(
        const IceUtil::Time& /*sensorValuesTimestamp*/,
        const IceUtil::Time& timeSinceLastIteration)
    {
        double deltaT = timeSinceLastIteration.toSecondsDouble();
        // globalPose = rtGetRobot()->getRobotNode("root")->getGlobalPose();
 
        if (collReady_.load())
        {
            ARMARX_CHECK_EXPRESSION(coll);
            coll->updateRtConfigFromUser();
            coll->updateRtCollisionObjects();
            coll->rtCollisionChecking();
        }
        for (auto& pair : this->limb)
        {
            this->limbRTUpdateStatus(pair.second, deltaT);
        }
 
        this->rtRunCoordinator(deltaT);
 
        for (auto& pair : this->limb)
        {
            limbRT(pair.second);
        }
        if (this->hands)
        {
            this->hands->updateRTStatus(deltaT);
        }
    }
 
    template <typename NJointControllerType, typename CollisionCtrlCfg>
    void
    NJointTSColController<NJointControllerType, CollisionCtrlCfg>::updateCollisionAvoidanceConfig(
        const ::armarx::aron::data::dto::DictPtr& dto,
        const Ice::Current& /*iceCurrent*/)
    {
        ARMARX_CHECK_EXPRESSION(coll);
        auto cfg = common::control_law::arondto::ObjectCollisionAvoidanceConfigDict::FromAron(dto);
        coll->setUserCfg(cfg);
    }
 
    template <typename NJointControllerType, typename CollisionCtrlCfg>
    void
    NJointTSColController<NJointControllerType, CollisionCtrlCfg>::sendCollisionObjects()
    {
        size_t numCollisionObjects = 0;
        for (const auto& pair : collisionObjects)
        {
            numCollisionObjects += pair.second.size();
        }
 
        std::vector<hpp::fcl::CollisionObject> allCollisionObjects;
        allCollisionObjects.reserve(numCollisionObjects);
        for (const auto& pair : collisionObjects)
        {
            allCollisionObjects.insert(
                allCollisionObjects.end(), pair.second.begin(), pair.second.end());
        }
 
        coll->updateUserCollisionObjects(allCollisionObjects);
    }
 
    template <typename NJointControllerType, typename CollisionCtrlCfg>
    void
    NJointTSColController<NJointControllerType, CollisionCtrlCfg>::deleteCollisionObjects(
        const Ice::Current& /*iceCurrent*/)
    {
        coll->deleteUserCollisionObjects();
    }
 
    template <typename NJointControllerType, typename CollisionCtrlCfg>
    void
    NJointTSColController<NJointControllerType, CollisionCtrlCfg>::updateCollisionObjects(
        const std::string& primitiveSourceName,
        const ::armarx::aron::data::dto::DictPtr& dtoScene,
        const Ice::Current& /*iceCurrent*/)
    {
        ARMARX_INFO_S << "updateCollisionObjects";
        auto scene = common::control_law::arondto::CollisionScene::FromAron(dtoScene);
        auto boxes = scene.boxes;
        auto spheres = scene.spheres;
        auto cylinders = scene.cylinders;
        auto capsules = scene.capsules;
        auto ellipsoids = scene.ellipsoids;
 
        if (collisionObjects.count(primitiveSourceName) == 0)
        {
            collisionObjects.emplace(primitiveSourceName, std::vector<hpp::fcl::CollisionObject>());
        }
        std::vector<hpp::fcl::CollisionObject>& collisionObjectsOfSource =
            collisionObjects[primitiveSourceName];
        collisionObjectsOfSource.clear();
        collisionObjectsOfSource.reserve(boxes.size() + spheres.size() + cylinders.size() +
                                         capsules.size() + ellipsoids.size());
 
        for (const auto& box : boxes)
        {
            collisionObjectsOfSource.push_back(simox::control::environment::createCollisionObject(
                simox::control::utils::primitive::Box{
                    .lengthX = box.lengthX, .lengthY = box.lengthY, .lengthZ = box.lengthZ},
                Eigen::Isometry3d{box.transformation}));
        }
 
        for (const auto& sphere : spheres)
        {
            collisionObjectsOfSource.push_back(simox::control::environment::createCollisionObject(
                simox::control::utils::primitive::Sphere{.radius = sphere.radius},
                Eigen::Isometry3d{sphere.transformation}));
        }
 
        for (const auto& cylinder : cylinders)
        {
            collisionObjectsOfSource.push_back(simox::control::environment::createCollisionObject(
                simox::control::utils::primitive::Cylinder{.radius = cylinder.radius,
                                                           .lengthY = cylinder.lengthY},
                Eigen::Isometry3d{cylinder.transformation}));
        }
 
        for (const auto& capsule : capsules)
        {
            collisionObjectsOfSource.push_back(simox::control::environment::createCollisionObject(
                simox::control::utils::primitive::Capsule{.radius = capsule.radius,
                                                          .lengthY = capsule.lengthY},
                Eigen::Isometry3d{capsule.transformation}));
        }
 
        for (const auto& ellipsoid : ellipsoids)
        {
            collisionObjectsOfSource.push_back(simox::control::environment::createCollisionObject(
                simox::control::utils::primitive::Ellipsoid{.lengthX = ellipsoid.lengthX,
                                                            .lengthY = ellipsoid.lengthY,
                                                            .lengthZ = ellipsoid.lengthZ},
                Eigen::Isometry3d{ellipsoid.transformation}));
        }
 
        sendCollisionObjects();
    }
 
    template <typename NJointControllerType, typename CollisionCtrlCfg>
    ::armarx::aron::data::dto::DictPtr
    NJointTSColController<NJointControllerType, CollisionCtrlCfg>::getCollisionAvoidanceConfig(
        const Ice::Current& /*iceCurrent*/)
    {
        if (not collReady_.load())
        {
            return nullptr;
        }
 
        ARMARX_CHECK_EXPRESSION(coll);
        return coll->userCfg.toAronDTO();
    }
 
    template <typename NJointControllerType, typename CollisionCtrlCfg>
    void
    NJointTSColController<NJointControllerType, CollisionCtrlCfg>::updateConfig(
        const ::armarx::aron::data::dto::DictPtr& dto,
        const Ice::Current& /*iceCurrent*/)
    {
        NJointControllerType::updateConfig(dto);
        userCfgWithColl_ = CollisionCtrlCfg::FromAron(dto);
        ARMARX_CHECK_EXPRESSION(coll);
        coll->setUserCfg(userCfgWithColl_.coll);
    }
 
    template <typename NJointControllerType, typename CollisionCtrlCfg>
    ::armarx::aron::data::dto::DictPtr
    NJointTSColController<NJointControllerType, CollisionCtrlCfg>::getConfig(
        const Ice::Current& /*iceCurrent*/)
    {
        // for (auto& pair : this->limb)
        // {
        //     this->userConfig.limbs.at(pair.first) =
        //         pair.second->bufferConfigRtToUser.getUpToDateReadBuffer();
        // }
        NJointControllerType::getConfig();
        userCfgWithColl_.limbs = this->userConfig.limbs;
        userCfgWithColl_.hands = this->userConfig.hands;
        return userCfgWithColl_.toAronDTO();
    }
 
    template <typename NJointControllerType, typename CollisionCtrlCfg>
    void
    NJointTSColController<NJointControllerType, CollisionCtrlCfg>::onPublish(
        const SensorAndControl& sc,
        const DebugDrawerInterfacePrx& drawer,
        const DebugObserverInterfacePrx& debugObs)
    {
        NJointControllerType::onPublish(sc, drawer, debugObs);
        if (coll)
        {
            for (auto& pair : coll->collLimb)
            {
                collLimbPublish(pair.second, debugObs);
            }
        }
    }
 
    template <typename NJointControllerType, typename CollisionCtrlCfg>
    void
    NJointTSColController<NJointControllerType, CollisionCtrlCfg>::collLimbPublish(
        CollAvoidBase::NodeSetData& arm,
        const DebugObserverInterfacePrx& debugObs)
    {
        //        double limbTimeMeasure = IceUtil::Time::now().toMicroSecondsDouble();
 
        StringVariantBaseMap datafields;
        auto rtStatus = arm.bufferRTtoPublish.getUpToDateReadBuffer();
        auto recoveryState = arm.bufferRecoveryStateSelfColl.getUpToDateReadBuffer();
 
        datafields["collDistanceThreshold"] = new Variant(recoveryState.collDistanceThreshold);
        datafields["collDistanceThresholdInit"] =
            new Variant(recoveryState.collDistanceThresholdInit);
 
        datafields["trackingError"] = new Variant(rtStatus.trackingError);
        common::debugEigenVec(datafields, "desiredJointTorques", rtStatus.desiredJointTorques);
        common::debugEigenVec(datafields, "selfCollisionTorque", rtStatus.selfCollisionJointTorque);
        common::debugEigenVec(datafields, "jointLimitTorque", rtStatus.jointLimitJointTorque);
 
        datafields["selfColAdmInterface.active"] =
            new Variant(static_cast<float>(rtStatus.selfColAdmInterface.active));
        common::debugEigenVec(
            datafields, "selfColAdmInterface.jointVel", rtStatus.selfColAdmInterface.jointVel);
        datafields["selfColAdmInterface.nDoFVel"] =
            new Variant(static_cast<float>(rtStatus.selfColAdmInterface.nDoFVel));
 
 
        datafields["jointLimAdmInterface.active"] =
            new Variant(static_cast<float>(rtStatus.jointLimAdmInterface.active));
        common::debugEigenVec(
            datafields, "jointLimAdmInterface.jointVel", rtStatus.jointLimAdmInterface.jointVel);
        datafields["jointLimAdmInterface.nDoFVel"] =
            new Variant(static_cast<float>(rtStatus.jointLimAdmInterface.nDoFVel));
 
        common::debugEigenVec(
            datafields, "projectedImpedanceTorque", rtStatus.projImpedanceJointTorque);
        //                common::debugEigenVec(datafields, "projectedJointLimitTorque", rtStatus.projJointLimTorque);
        //                common::debugEigenVec(datafields, "projectedForceImpedance", rtStatus.projForceImpedance);
        //       common::debugEigenVec(
        //            datafields, "projectedSelfCollisionTorque", rtStatus.projSelfCollTorque);
 
        const Eigen::VectorXf selfCollNullspace = rtStatus.selfCollNullSpace.diagonal();
        const Eigen::VectorXf jointLimNullspace = rtStatus.jointLimNullSpace.diagonal();
        common::debugEigenVec(datafields, "selfCollNullspaceDiagonal", selfCollNullspace);
        common::debugEigenVec(datafields, "jointLimNullspaceDiagonal", jointLimNullspace);
 
        //        if (rtData.enableJointLimitAvoidance)
        //        {
        //            for (size_t i = 0; i < rtStatus.jointLimitData.size(); ++i)
        //            {
        //                if (not rtStatus.jointLimitData[i].isLimitless)
        //                {
        //                    datafields["n_des(q)_" + std::to_string(i) + "_" +
        //                               rtStatus.jointLimitData[i].jointName] =
        //                        new Variant(rtStatus.jointLimitData[i].desiredNSjointLim);
        //                    datafields["q_damping" + std::to_string(i) + "_" +
        //                               rtStatus.jointLimitData[i].jointName] =
        //                        new Variant(rtStatus.jointLimitData[i].totalDamping);
        //                    datafields["q_repTorque" + std::to_string(i) + "_" +
        //                               rtStatus.jointLimitData[i].jointName] =
        //                        new Variant(rtStatus.jointLimitData[i].repulsiveTorque);
        //                }
        //            }
        //        }
 
        datafields["collisionPairsNum"] = new Variant(rtStatus.collisionPairsNum);
        datafields["activeCollPairsNum"] = new Variant(rtStatus.activeCollPairsNum);
 
        datafields["collisionPairTime"] = new Variant(rtStatus.collisionPairTime);
        datafields["collisionTorqueTime"] = new Variant(rtStatus.collisionTorqueTime);
        datafields["jointLimitTorqueTime"] = new Variant(rtStatus.jointLimitTorqueTime);
        datafields["selfCollNullspaceTime"] = new Variant(rtStatus.selfCollNullspaceTime);
        datafields["jointLimitNullspaceTime"] = new Variant(rtStatus.jointLimitNullspaceTime);
 
        datafields["impForceRatio"] = new Variant(rtStatus.impForceRatio);
        datafields["impTorqueRatio"] = new Variant(rtStatus.impTorqueRatio);
 
        for (unsigned int i = 0; i < rtStatus.activeCollPairsNum; ++i)
        {
            //size_t index = rtStatus.distanceIndexPairs[i].second;
            datafields[std::to_string(i) + "_minDistance"] =
                new Variant(rtStatus.collDataVec[i].minDistance);
            datafields[std::to_string(i) + "_repForce"] =
                new Variant(rtStatus.collDataVec[i].repulsiveForce);
            datafields[std::to_string(i) + "_dampingForce"] = new Variant(
                -1.0f * rtStatus.collDataVec[i].damping * rtStatus.collDataVec[i].distanceVelocity);
            datafields[std::to_string(i) + "_n_des(d)"] =
                new Variant(rtStatus.collDataVec[i].desiredNSColl);
            common::debugEigenVec(
                datafields, std::to_string(i) + "_point", rtStatus.collDataVec[i].point1);
            common::debugEigenVec(
                datafields, std::to_string(i) + "_dir", rtStatus.collDataVec[i].direction);
        }
 
 
        //        /// prepare test case config
        //        const float tolerance = 1e-9f;
        //        if (rtData.testConfig == 1)
        //        {
        //            if (rtStatus.evalData[0].nodeName == "ArmL8_Wri2")
        //            {
        //                // set distance
        //                datafields["ArmL8_Wri2_" + rtStatus.evalData[0].otherName + "_dist"] =
        //                    new Variant(rtStatus.evalData[0].minDistance);
        //                // set f_rep, damping, desired NS space
 
 
        //                datafields["ArmL8_Wri2_" + rtStatus.evalData[0].otherName + "_repF"] =
        //                    new Variant(rtStatus.evalData[0].repulsiveForce);
        //                datafields["ArmL8_Wri2_" + rtStatus.evalData[0].otherName + "_projMass"] =
        //                    new Variant(rtStatus.evalData[0].projectedMass);
        //                datafields["ArmL8_Wri2_" + rtStatus.evalData[0].otherName + "_locStiffness"] =
        //                    new Variant(rtStatus.evalData[0].localStiffness);
        //                datafields["ArmL8_Wri2_" + rtStatus.evalData[0].otherName + "_dampFactor"] =
        //                    new Variant(rtStatus.evalData[0].damping);
        //                datafields["ArmL8_Wri2_" + rtStatus.evalData[0].otherName + "_distVel"] =
        //                    new Variant(rtStatus.evalData[0].distanceVelocity);
        //                if (fabs(rtStatus.evalData[0].damping + 1.0f) < tolerance)
        //                {
        //                    datafields["ArmL8_Wri2_" + rtStatus.evalData[0].otherName + "_dampForce"] =
        //                        new Variant(0.0);
        //                }
        //                else
        //                {
        //                    datafields["ArmL8_Wri2_" + rtStatus.evalData[0].otherName + "_dampForce"] =
        //                        new Variant(-1.0 * rtStatus.evalData[0].damping *
        //                                    rtStatus.evalData[0].distanceVelocity);
        //                }
 
        //                datafields["ArmL8_Wri2_" + rtStatus.evalData[0].otherName + "_n_des(d)"] =
        //                    new Variant(rtStatus.evalData[0].desiredNSColl);
        //            }
        //            if (rtStatus.evalData[1].nodeName == "ArmL8_Wri2")
        //            {
        //                // set distance
        //                datafields["ArmL8_Wri2_" + rtStatus.evalData[1].otherName + "_dist"] =
        //                    new Variant(rtStatus.evalData[1].minDistance);
        //                // set f_rep, damping, desired NS space
 
        //                datafields["ArmL8_Wri2_" + rtStatus.evalData[1].otherName + "_repF"] =
        //                    new Variant(rtStatus.evalData[1].repulsiveForce);
        //                datafields["ArmL8_Wri2_" + rtStatus.evalData[1].otherName + "_projMass"] =
        //                    new Variant(rtStatus.evalData[1].projectedMass);
        //                datafields["ArmL8_Wri2_" + rtStatus.evalData[1].otherName + "_locStiffness"] =
        //                    new Variant(rtStatus.evalData[1].localStiffness);
        //                datafields["ArmL8_Wri2_" + rtStatus.evalData[1].otherName + "_dampFactor"] =
        //                    new Variant(rtStatus.evalData[1].damping);
        //                datafields["ArmL8_Wri2_" + rtStatus.evalData[1].otherName + "_distVel"] =
        //                    new Variant(rtStatus.evalData[1].distanceVelocity);
        //                if (fabs(rtStatus.evalData[1].damping + 1.0f) < tolerance)
        //                {
        //                    datafields["ArmL8_Wri2_" + rtStatus.evalData[1].otherName + "_dampForce"] =
        //                        new Variant(0.0);
        //                }
        //                else
        //                {
        //                    datafields["ArmL8_Wri2_" + rtStatus.evalData[1].otherName + "_dampForce"] =
        //                        new Variant(-1.0 * rtStatus.evalData[1].damping *
        //                                    rtStatus.evalData[1].distanceVelocity);
        //                }
 
        //                datafields["ArmL8_Wri2_" + rtStatus.evalData[1].otherName + "_n_des(d)"] =
        //                    new Variant(rtStatus.evalData[1].desiredNSColl);
        //            }
        //            if (rtStatus.evalData[2].nodeName == "ArmL7_Wri1")
        //            {
        //                // set distance
        //                datafields["ArmL7_Wri1_" + rtStatus.evalData[2].otherName + "_dist"] =
        //                    new Variant(rtStatus.evalData[2].minDistance);
        //                // set f_rep, damping, desired NS space
 
        //                datafields["ArmL7_Wri1_" + rtStatus.evalData[2].otherName + "_repF"] =
        //                    new Variant(rtStatus.evalData[2].repulsiveForce);
 
        //                datafields["ArmL7_Wri1_" + rtStatus.evalData[2].otherName + "_projMass"] =
        //                    new Variant(rtStatus.evalData[2].projectedMass);
        //                datafields["ArmL7_Wri1_" + rtStatus.evalData[2].otherName + "_locStiffness"] =
        //                    new Variant(rtStatus.evalData[2].localStiffness);
        //                datafields["ArmL7_Wri1_" + rtStatus.evalData[2].otherName + "_dampFactor"] =
        //                    new Variant(rtStatus.evalData[2].damping);
        //                datafields["ArmL7_Wri1_" + rtStatus.evalData[2].otherName + "_distVel"] =
        //                    new Variant(rtStatus.evalData[2].distanceVelocity);
        //                if (fabs(rtStatus.evalData[2].damping + 1.0f) < tolerance)
        //                {
        //                    datafields["ArmL7_Wri1_" + rtStatus.evalData[2].otherName + "_dampForce"] =
        //                        new Variant(0.0);
        //                }
        //                else
        //                {
        //                    datafields["ArmL7_Wri1_" + rtStatus.evalData[2].otherName + "_dampForce"] =
        //                        new Variant(-1.0 * rtStatus.evalData[2].damping *
        //                                    rtStatus.evalData[2].distanceVelocity);
        //                }
 
 
        //                datafields["ArmL7_Wri1_" + rtStatus.evalData[2].otherName + "_n_des(d)"] =
        //                    new Variant(rtStatus.evalData[2].desiredNSColl);
        //            }
        //            if (rtStatus.evalData[3].nodeName == "ArmL8_Wri2")
        //            {
        //                // set distance
        //                datafields["ArmL8_Wri2_" + rtStatus.evalData[3].otherName + "_dist"] =
        //                    new Variant(rtStatus.evalData[3].minDistance);
        //                // set f_rep, damping, desired NS space
 
        //                datafields["ArmL8_Wri2_" + rtStatus.evalData[3].otherName + "_repF"] =
        //                    new Variant(rtStatus.evalData[3].repulsiveForce);
 
        //                datafields["ArmL8_Wri2_" + rtStatus.evalData[3].otherName + "_projMass"] =
        //                    new Variant(rtStatus.evalData[3].projectedMass);
        //                datafields["ArmL8_Wri2_" + rtStatus.evalData[3].otherName + "_locStiffness"] =
        //                    new Variant(rtStatus.evalData[3].localStiffness);
        //                datafields["ArmL8_Wri2_" + rtStatus.evalData[3].otherName + "_dampFactor"] =
        //                    new Variant(rtStatus.evalData[3].damping);
        //                datafields["ArmL8_Wri2_" + rtStatus.evalData[3].otherName + "_distVel"] =
        //                    new Variant(rtStatus.evalData[3].distanceVelocity);
        //                if (fabs(rtStatus.evalData[3].damping + 1.0f) < tolerance)
        //                {
        //                    datafields["ArmL8_Wri2_" + rtStatus.evalData[3].otherName + "_dampForce"] =
        //                        new Variant(0.0);
        //                }
        //                else
        //                {
        //                    datafields["ArmL8_Wri2_" + rtStatus.evalData[3].otherName + "_dampForce"] =
        //                        new Variant(-1.0 * rtStatus.evalData[3].damping *
        //                                    rtStatus.evalData[3].distanceVelocity);
        //                }
 
 
        //                datafields["ArmL8_Wri2_" + rtStatus.evalData[3].otherName + "_n_des(d)"] =
        //                    new Variant(rtStatus.evalData[3].desiredNSColl);
        //            }
 
        //            if (rtStatus.evalData[4].nodeName == "ArmL8_Wri2")
        //            {
        //                // set distance
        //                datafields["ArmL8_Wri2_" + rtStatus.evalData[4].otherName + "_dist"] =
        //                    new Variant(rtStatus.evalData[4].minDistance);
        //                // set f_rep, damping, desired NS space
 
        //                // values have been calculated
        //                datafields["ArmL8_Wri2_" + rtStatus.evalData[4].otherName + "_repF"] =
        //                    new Variant(rtStatus.evalData[4].repulsiveForce);
 
        //                datafields["ArmL8_Wri2_" + rtStatus.evalData[4].otherName + "_projMass"] =
        //                    new Variant(rtStatus.evalData[4].projectedMass);
        //                datafields["ArmL8_Wri2_" + rtStatus.evalData[4].otherName + "_locStiffness"] =
        //                    new Variant(rtStatus.evalData[4].localStiffness);
        //                datafields["ArmL8_Wri2_" + rtStatus.evalData[4].otherName + "_dampFactor"] =
        //                    new Variant(rtStatus.evalData[4].damping);
        //                datafields["ArmL8_Wri2_" + rtStatus.evalData[4].otherName + "_distVel"] =
        //                    new Variant(rtStatus.evalData[4].distanceVelocity);
        //                if (fabs(rtStatus.evalData[4].damping + 1.0f) < tolerance)
        //                {
        //                    datafields["ArmL8_Wri2_" + rtStatus.evalData[4].otherName + "_dampForce"] =
        //                        new Variant(0.0);
        //                }
        //                else
        //                {
        //                    datafields["ArmL8_Wri2_" + rtStatus.evalData[4].otherName + "_dampForce"] =
        //                        new Variant(-1.0 * rtStatus.evalData[4].damping *
        //                                    rtStatus.evalData[4].distanceVelocity);
        //                }
 
 
        //                datafields["ArmL8_Wri2_" + rtStatus.evalData[4].otherName + "_n_des(d)"] =
        //                    new Variant(rtStatus.evalData[4].desiredNSColl);
        //            }
        //            if (rtStatus.evalData[5].nodeName == "ArmL8_Wri2")
        //            {
        //                // set distance
        //                datafields["ArmL8_Wri2_" + rtStatus.evalData[5].otherName + "_dist"] =
        //                    new Variant(rtStatus.evalData[5].minDistance);
 
        //                // values have been calculated
        //                datafields["ArmL8_Wri2_" + rtStatus.evalData[5].otherName + "_repF"] =
        //                    new Variant(rtStatus.evalData[5].repulsiveForce);
 
        //                datafields["ArmL8_Wri2_" + rtStatus.evalData[5].otherName + "_projMass"] =
        //                    new Variant(rtStatus.evalData[5].projectedMass);
        //                datafields["ArmL8_Wri2_" + rtStatus.evalData[5].otherName + "_locStiffness"] =
        //                    new Variant(rtStatus.evalData[5].localStiffness);
        //                datafields["ArmL8_Wri2_" + rtStatus.evalData[5].otherName + "_dampFactor"] =
        //                    new Variant(rtStatus.evalData[5].damping);
        //                datafields["ArmL8_Wri2_" + rtStatus.evalData[5].otherName + "_distVel"] =
        //                    new Variant(rtStatus.evalData[5].distanceVelocity);
        //                if (fabs(rtStatus.evalData[5].damping + 1.0f) < tolerance)
        //                {
        //                    datafields["ArmL8_Wri2_" + rtStatus.evalData[5].otherName + "_dampForce"] =
        //                        new Variant(0.0);
        //                }
        //                else
        //                {
        //                    datafields["ArmL8_Wri2_" + rtStatus.evalData[5].otherName + "_dampForce"] =
        //                        new Variant(-1.0 * rtStatus.evalData[5].damping *
        //                                    rtStatus.evalData[5].distanceVelocity);
        //                }
 
 
        //                datafields["ArmL8_Wri2_" + rtStatus.evalData[5].otherName + "_n_des(d)"] =
        //                    new Variant(rtStatus.evalData[5].desiredNSColl);
        //            }
        //            if (rtStatus.evalData[6].nodeName == "ArmL5_Elb1")
        //            {
        //                // set distance
        //                datafields["ArmL5_Elb1_" + rtStatus.evalData[6].otherName + "_dist"] =
        //                    new Variant(rtStatus.evalData[6].minDistance);
 
        //                // values have been calculated
        //                datafields["ArmL5_Elb1_" + rtStatus.evalData[6].otherName + "_repF"] =
        //                    new Variant(rtStatus.evalData[6].repulsiveForce);
 
        //                datafields["ArmL5_Elb1_" + rtStatus.evalData[6].otherName + "_projMass"] =
        //                    new Variant(rtStatus.evalData[6].projectedMass);
        //                datafields["ArmL5_Elb1_" + rtStatus.evalData[6].otherName + "_locStiffness"] =
        //                    new Variant(rtStatus.evalData[6].localStiffness);
        //                datafields["ArmL5_Elb1_" + rtStatus.evalData[6].otherName + "_dampFactor"] =
        //                    new Variant(rtStatus.evalData[6].damping);
        //                datafields["ArmL5_Elb1_" + rtStatus.evalData[6].otherName + "_distVel"] =
        //                    new Variant(rtStatus.evalData[6].distanceVelocity);
        //                if (fabs(rtStatus.evalData[6].damping + 1.0f) < tolerance)
        //                {
        //                    datafields["ArmL5_Elb1_" + rtStatus.evalData[6].otherName + "_dampForce"] =
        //                        new Variant(0.0);
        //                }
        //                else
        //                {
        //                    datafields["ArmL5_Elb1_" + rtStatus.evalData[6].otherName + "_dampForce"] =
        //                        new Variant(-1.0 * rtStatus.evalData[6].damping *
        //                                    rtStatus.evalData[6].distanceVelocity);
        //                }
 
 
        //                datafields["ArmL5_Elb1_" + rtStatus.evalData[6].otherName + "_n_des(d)"] =
        //                    new Variant(rtStatus.evalData[6].desiredNSColl);
        //            }
        //        }
 
 
        //double selfCollDebugTime = IceUtil::Time::now().toMicroSecondsDouble() - timeMeasure;
        //datafields["selfCollDebugTime"] = new Variant(selfCollDebugTime);
 
        // debug value assignment torso, neck joints
        //        datafields["torsoJointValue_float"] = new Variant(rtStatus.torsoJointValuef);
        //        datafields["neck1JointValue_float"] = new Variant(rtStatus.neck1JointValuef);
        //        datafields["neck2JointValue_float"] = new Variant(rtStatus.neck2JointValuef);
 
        //        datafields["torsoJointValue_double"] = new Variant(rtStatus.torsoJointValued);
        //        datafields["neck1JointValue_double"] = new Variant(rtStatus.neck1JointValued);
        //        datafields["neck2JointValue_double"] = new Variant(rtStatus.neck2JointValued);
 
        //        common::debugEigenVec(
        //            datafields, "set_ActuatedJointValues", rtStatus.setActuatedJointValues.cast<float>());
        //        common::debugEigenVec(
        //            datafields, "get_ActuatedJointValues", rtStatus.getActuatedJointValues.cast<float>());
 
        //        double limbTime = IceUtil::Time::now().toMicroSecondsDouble() - limbTimeMeasure;
        //        datafields["limbOnPublishTime"] = new Variant(limbTime);
 
        debugObs->setDebugChannel("CollAvoid_ImpCtrl_" + arm.nodeSetName, datafields);
    }
 
    template <typename NJointControllerType, typename CollisionCtrlCfg>
    void
    NJointTSColController<NJointControllerType, CollisionCtrlCfg>::AddArvizObjects(
        viz::Layer& layer,
        const std::vector<hpp::fcl::CollisionObject>& objects)
    {
        for (size_t i = 0; i < objects.size(); ++i)
        {
            auto object = objects.at(i);
            const auto position = object.getTranslation().cast<float>() * 1000;
            const Eigen::Matrix3f rotation = object.getRotation().cast<float>();
 
            switch (object.getNodeType())
            {
                case hpp::fcl::NODE_TYPE::GEOM_BOX:
                {
                    const hpp::fcl::Box* box =
                        std::static_pointer_cast<hpp::fcl::Box>(object.collisionGeometry()).get();
                    const viz::Box vizObject = viz::Box("box " + std::to_string(i))
                                                   .size(box->halfSide.cast<float>() * 1000 * 2)
                                                   .orientation(rotation)
                                                   .position(position)
                                                   .color(255, 0, 0, 128);
                    layer.add(vizObject);
                    break;
                }
                case hpp::fcl::NODE_TYPE::GEOM_SPHERE:
                {
                    const hpp::fcl::Sphere* sphere =
                        std::static_pointer_cast<hpp::fcl::Sphere>(object.collisionGeometry())
                            .get();
                    const viz::Sphere vizObject =
                        viz::Sphere("sphere " + std::to_string(i))
                            .radius(static_cast<float>(sphere->radius) * 1000)
                            .orientation(rotation)
                            .position(position)
                            .color(0, 0, 255, 128);
                    layer.add(vizObject);
                    break;
                }
                case hpp::fcl::NODE_TYPE::GEOM_CYLINDER:
                {
                    const hpp::fcl::Cylinder* cylinder =
                        std::static_pointer_cast<hpp::fcl::Cylinder>(object.collisionGeometry())
                            .get();
                    const viz::Cylinder vizObject =
                        viz::Cylinder("cylinder " + std::to_string(i))
                            .height(static_cast<float>(cylinder->halfLength) * 1000 * 2)
                            .radius(static_cast<float>(cylinder->radius) * 1000)
                            .orientation(rotation *
                                         Eigen::AngleAxisf(M_PI / 2., Eigen::Vector3f::UnitX()))
                            .position(position)
                            .color(0, 255, 0, 128);
                    layer.add(vizObject);
                    break;
                }
                case hpp::fcl::NODE_TYPE::GEOM_CAPSULE:
                {
                    const hpp::fcl::Capsule* capsule =
                        std::static_pointer_cast<hpp::fcl::Capsule>(object.collisionGeometry())
                            .get();
                    const auto _rotation =
                        rotation * Eigen::AngleAxisf(M_PI / 2., Eigen::Vector3f::UnitX());
                    Eigen::Vector3f offset;
                    offset << 0, static_cast<float>(capsule->halfLength) * 1000, 0;
                    offset = _rotation * offset;
 
                    const viz::Sphere cap1 = viz::Sphere("capsule_cap1 " + std::to_string(i))
                                                 .radius(static_cast<float>(capsule->radius) * 1000)
                                                 .position(position + offset)
                                                 .orientation(_rotation)
                                                 .color(255, 200, 0, 128);
                    const viz::Sphere cap2 = viz::Sphere("capsule_cap2 " + std::to_string(i))
                                                 .radius(static_cast<float>(capsule->radius) * 1000)
                                                 .position(position - offset)
                                                 .orientation(_rotation)
                                                 .color(255, 200, 0, 128);
                    const viz::Cylinder vizObject =
                        viz::Cylinder("capsule_mid " + std::to_string(i))
                            .height(static_cast<float>(capsule->halfLength) * 1000 * 2)
                            .radius(static_cast<float>(capsule->radius) * 1000)
                            .color(255, 200, 0, 128)
                            .orientation(_rotation)
                            .position(position);
                    layer.add(vizObject);
                    layer.add(cap1);
                    layer.add(cap2);
                    break;
                }
                case hpp::fcl::NODE_TYPE::GEOM_ELLIPSOID:
                {
                    const hpp::fcl::Ellipsoid* ellipsoid =
                        std::static_pointer_cast<hpp::fcl::Ellipsoid>(object.collisionGeometry())
                            .get();
                    const viz::Ellipsoid vizObject =
                        viz::Ellipsoid("ellipsoid " + std::to_string(i))
                            // axis lengths seem to be radii instead of axis lengths
                            .axisLengths(ellipsoid->radii.cast<float>() * 1000)
                            .orientation(rotation)
                            .position(position)
                            .color(255, 0, 200, 128);
                    layer.add(vizObject);
                    break;
                }
                default:;
            }
        }
    }
 
    template <typename NJointControllerType, typename CollisionCtrlCfg>
    void
    NJointTSColController<NJointControllerType, CollisionCtrlCfg>::collectArviz(
        viz::StagedCommit& stage) const
    {
        NJointControllerType::collectArviz(stage);
 
        auto transformVector = [](const Eigen::Matrix4f& pose,
                                  const Eigen::Vector3f& vec) -> Eigen::Vector3f
        { return common::getOri(pose) * vec + common::getPos(pose); };
 
        if (coll)
        {
            for (const auto& [_, arm] : coll->collLimb)
            {
                const auto& rtStatus = arm.bufferRTtoPublish.getUpToDateReadBuffer();
 
                viz::Layer layer = this->scopedArviz->layer("ColStatus_" + arm.nodeSetName);
 
                for (unsigned int i = 0; i < rtStatus.activeCollPairsNum; ++i)
                {
                    // visualize impedance force
                    //            layer.add(viz::Arrow("projImpForce_" + std::to_string(i))
                    //                          .fromTo(rtStatus.collDataVec[i].point * 1000.0,
                    //                                  rtStatus.collDataVec[i].point * 1000.0 +
                    //                                      rtStatus.collDataVec[i].projectedImpedanceForce *
                    //                                          rtStatus.collDataVec[i].direction * 5.0)
                    //                          .color(simox::Color::purple()));
 
                    // layer.add(viz::Arrow("impForce_" + std::to_string(i))
                    //               .fromTo(rtStatus.collDataVec[i].point * 1000.0,
                    //                       rtStatus.collDataVec[i].point * 1000.0 +
                    //                           rtStatus.forceImpedance.head<3>().dot(
                    //                               rtStatus.collDataVec[i].direction) *
                    //                               rtStatus.collDataVec[i].direction * 5.0)
                    //               .color(simox::Color::yellow())); // project forceImp in dir of collision
 
                    layer.add(
                        viz::Arrow("Force_" + std::to_string(i))
                            .fromTo(transformVector(rtStatus.globalPose,
                                                    rtStatus.collDataVec[i].point1 * 1000.0F),
                                    transformVector(rtStatus.globalPose,
                                                    rtStatus.collDataVec[i].point1 * 1000.0F +
                                                        rtStatus.collDataVec[i].direction * 50.0F *
                                                            rtStatus.collDataVec[i].repulsiveForce))
                            .color(simox::Color::blue())
                            .width(5.0f));
                }
 
                layer.add(viz::Pose("RootPose").pose(rtStatus.globalPose).scale(2));
 
                // visualize impedance force
                //        layer.add(viz::Arrow("impForce")
                //                      .fromTo(transformVector(rtStatus.globalPose,
                //                                              rtStatus.currentPose.block<3, 1>(0, 3)),
                //                              transformVector(rtStatus.globalPose,
                //                                              rtStatus.currentPose.block<3, 1>(0, 3) +
                //                                                  rtStatus.forceImpedance.head<3>() * 1000.0))
                //                      .color(simox::Color::yellow()));
                //        layer.add(viz::Arrow("projImpForce")
                //                      .fromTo(rtStatus.currentPose.block<3, 1>(0, 3),
                //                              rtStatus.currentPose.block<3, 1>(0, 3) +
                //                                  rtStatus.projForceImpedance.head<3>() * 1000.0)
                //                      .color(simox::Color::purple()));
 
                stage.add(layer);
            }
 
            if (not coll->userCollisionObjects.empty())
            {
                viz::Layer layer = this->scopedArviz->layer("ColStatus_objectModels");
                AddArvizObjects(layer, coll->userCollisionObjects);
                stage.add(layer);
            }
            const bool enableRobotPrimitiveVis = true;
            if (enableRobotPrimitiveVis)
            {
                // TODO uses rt collision robot
                const auto& robotObjects =
                    coll->collisionRobot->getCollisionManager()->getObjects();
                std::vector<hpp::fcl::CollisionObject> robotObjectsByValue;
                robotObjectsByValue.reserve(robotObjects.size());
                for (const auto* ptr : robotObjects)
                {
                    robotObjectsByValue.push_back(*ptr);
                }
                viz::Layer layer = this->scopedArviz->layer("ColStatus_robotModel");
                AddArvizObjects(layer, robotObjectsByValue);
                stage.add(layer);
            }
        }
    }
 
    template <typename NJointControllerType, typename CollisionCtrlCfg>
    void
    NJointTSColController<NJointControllerType, CollisionCtrlCfg>::rtPreActivateController()
    {
        // ARMARX_RT_LOGF_INFO("rt Preactivate controller "
        //                     "NJointTaskspaceCollisionAvoidanceMixedImpedanceVelocityController");
        NJointControllerType::rtPreActivateController();
        ARMARX_TRACE;
        if (collReady_.load())
        {
            ARMARX_CHECK_EXPRESSION(coll);
            coll->rtPreActivate();
        }
    }
 
    template <typename NJointControllerType, typename CollisionCtrlCfg>
    void
    NJointTSColController<NJointControllerType, CollisionCtrlCfg>::rtPostDeactivateController()
    {
        NJointControllerType::rtPostDeactivateController();
        if (collReady_.load())
        {
            ARMARX_CHECK_EXPRESSION(coll);
            coll->rtPostDeactivate();
        }
        // ARMARX_RT_LOGF_INFO("-- post deactivate: "
        //                     "NJointTaskspaceCollisionAvoidanceMixedImpedanceVelocityController");
    }
 
    template <typename NJointControllerType, typename CollisionCtrlCfg>
    typename NJointTSColController<NJointControllerType, CollisionCtrlCfg>::ConfigPtrT
    NJointTSColController<NJointControllerType, CollisionCtrlCfg>::GenerateConfigFromVariants(
        const StringVariantBaseMap& values)
    {
        auto cfgName = values.at("config_box")->getString();
        const armarx::PackagePath configPath(
            "armarx_control",
            "controller_config/" + std::string(NJointControllerType::ControlType::TypeName) +
                "Col/" + cfgName + ".json");
        ARMARX_INFO_S << "Loading config from " << configPath.toSystemPath();
        ARMARX_CHECK(std::filesystem::exists(configPath.toSystemPath()));
 
        auto cfgDTO = armarx::readFromJson<CollisionCtrlCfg>(configPath.toSystemPath());
 
        ARMARX_TRACE;
        return new ConfigurableNJointControllerConfig{cfgDTO.toAronDTO()};
    }
 
    /// ================================== TSMixImpVelCol ==================================
    template class NJointTSColController<NJointTSMixImpVelController,
                                         law::arondto::TSMixImpVelColConfigDict>;
 
    NJointControllerRegistration<NJointTSMixImpVelColController>
        registrationControllerNJointTSMixImpVelColController("TSMixImpVelCol");
 
    NJointTSMixImpVelColController::NJointTSMixImpVelColController(
        const RobotUnitPtr& robotUnit,
        const NJointControllerConfigPtr& config,
        const VirtualRobot::RobotPtr& robot) :
        // NJointTSMixImpVelController(robotUnit, config, robot),
        NJointTSColController<NJointTSMixImpVelController, law::arondto::TSMixImpVelColConfigDict>(
            robotUnit,
            config,
            robot)
    {
    }
 
    std::string
    NJointTSMixImpVelColController::getClassName(const Ice::Current& /*unused*/) const
    {
        return "TSMixImpVelCol";
    }
 
    /// ================================== TSImpCol ==================================
 
    template class NJointTSColController<NJointTSImpController, law::arondto::TSImpColConfigDict>;
 
    NJointControllerRegistration<NJointTSImpColController>
        registrationControllerNJointTSImpColController("TSImpCol");
 
    NJointTSImpColController::NJointTSImpColController(const RobotUnitPtr& robotUnit,
                                                       const NJointControllerConfigPtr& config,
                                                       const VirtualRobot::RobotPtr& robot) :
        // NJointTSImpController(robotUnit, config, robot),
        NJointTSColController<NJointTSImpController, law::arondto::TSImpColConfigDict>(robotUnit,
                                                                                       config,
                                                                                       robot)
    {
    }
 
    std::string
    NJointTSImpColController::getClassName(const Ice::Current& /*unused*/) const
    {
        return "TSImpCol";
    }
 
    /// ================================== TSVeloCol ==================================
 
    template class NJointTSColController<NJointTSVelController, law::arondto::TSVeloColConfigDict>;
 
    NJointControllerRegistration<NJointTSVeloColController>
        registrationControllerNJointTSVeloColController("TSVeloCol");
 
    NJointTSVeloColController::NJointTSVeloColController(const RobotUnitPtr& robotUnit,
                                                         const NJointControllerConfigPtr& config,
                                                         const VirtualRobot::RobotPtr& robot) :
        // NJointTSVelController(robotUnit, config, robot),
        NJointTSColController<NJointTSVelController, law::arondto::TSVeloColConfigDict>(robotUnit,
                                                                                        config,
                                                                                        robot)
    {
    }
 
    std::string
    NJointTSVeloColController::getClassName(const Ice::Current& /*unused*/) const
    {
        return "TSVeloCol";
    }
 
 
} // namespace armarx::control::njoint_controller::task_space