CollisionAvoidanceImpedanceController.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       2021
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "CollisionAvoidanceImpedanceController.h"
 
#include <VirtualRobot/MathTools.h>
#include <VirtualRobot/RobotNodeSet.h>
 
#include <simox/control/geodesics/util.h>
#include <simox/control/robot/NodeInterface.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/ControlTargets/ControlTarget1DoFActuator.h>
#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.h>
#include <RobotAPI/components/units/RobotUnit/RobotUnit.h>
#include <RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.h>
 
#include <armarx/control/common/aron_conversions.h> // for GUI
#include <armarx/control/common/utils.h>
#include <armarx/control/ethercat/RTUtility.h>
 
namespace armarx::control::njoint_controller::task_space
{
    NJointControllerRegistration<NJointTaskspaceCollisionAvoidanceImpedanceController>
        registrationControllerNJointTaskspaceCollisionAvoidanceImpedanceController(
            "NJointTaskspaceCollisionAvoidanceImpedanceController");
 
    NJointTaskspaceCollisionAvoidanceImpedanceController::
        NJointTaskspaceCollisionAvoidanceImpedanceController(
            const RobotUnitPtr& robotUnit,
            const NJointControllerConfigPtr& config,
            const VirtualRobot::RobotPtr& robot) :
        NJointTaskspaceImpedanceController{robotUnit, config, robot}
    {
        ConfigPtrT cfg = ConfigPtrT::dynamicCast(config);
        ARMARX_CHECK_EXPRESSION(cfg);
        userCfgWithColl = CollisionCtrlCfg::FromAron(cfg->config);
 
        std::map<std::string, std::vector<size_t>> velCtrlIndices;
 
        coll = std::make_shared<core::CollisionAvoidanceBase>(
            rtGetRobot(), userCfgWithColl.coll, velCtrlIndices);
        collReady.store(true);
        ARMARX_INFO << "-- " << getClassName() << " initialized --";
    }
 
    std::string
    NJointTaskspaceCollisionAvoidanceImpedanceController::getClassName(const Ice::Current&) const
    {
        return "NJointTaskspaceCollisionAvoidanceImpedanceController";
    }
 
    void
    NJointTaskspaceCollisionAvoidanceImpedanceController::limbRT(ArmPtr& arm, const double deltaT)
    {
        double time_measure = IceUtil::Time::now().toMicroSecondsDouble();
        //        limbRTUpdateStatus(arm, deltaT);
        double time_update_status = IceUtil::Time::now().toMicroSecondsDouble() - time_measure;
 
        arm->controller.run(arm->rtConfig, arm->rtStatus);
        double time_run_rt = IceUtil::Time::now().toMicroSecondsDouble() - time_measure;
 
        if (collReady.load())
        {
            ARMARX_CHECK_EXPRESSION(coll);
            coll->rtLimbControllerRun(arm->kinematicChainName,
                                      arm->rtStatus.jointPosition,
                                      arm->rtStatus.qvelFiltered,
                                      arm->rtConfig.torqueLimit,
                                      arm->rtStatus.desiredJointTorque,
                                      deltaT);
        }
        double time_coll_run = IceUtil::Time::now().toMicroSecondsDouble() - time_measure;
 
        if (collReady.load())
        {
            limbRTSetTarget(arm,
                            coll->collLimb.at(arm->kinematicChainName).rts.desiredJointTorques);
        }
        else
        {
            limbRTSetTarget(arm, arm->rtStatus.desiredJointTorque);
        }
        double time_set_target = IceUtil::Time::now().toMicroSecondsDouble() - time_measure;
 
        time_measure = IceUtil::Time::now().toMicroSecondsDouble() - time_measure;
 
        if (time_measure > 200)
        {
            ARMARX_RT_LOGF_WARN("---- rt too slow: "
                                "time_update_status: %.2f\n"
                                "run_rt_limb: %.2f\n"
                                "run_coll_rt_limb: %.2f\n"
                                "set_target_limb: %.2f\n"
                                "time all: %.2f\n",
                                time_update_status, // 0-1 us
                                time_run_rt - time_update_status, //
                                time_coll_run - time_run_rt, //
                                time_set_target - time_coll_run, //
                                time_measure)
                .deactivateSpam(1.0f); // 0-1 us
        }
    }
 
    void
    NJointTaskspaceCollisionAvoidanceImpedanceController::rtRun(
        const IceUtil::Time& sensorValuesTimestamp,
        const IceUtil::Time& timeSinceLastIteration)
    {
        double deltaT = timeSinceLastIteration.toSecondsDouble();
 
        if (collReady.load())
        {
            ARMARX_CHECK_EXPRESSION(coll);
            coll->updateRtConfigFromUser();
            coll->rtCollisionChecking();
        }
        for (auto& pair : limb)
        {
            limbRTUpdateStatus(pair.second, deltaT);
        }
 
        rtRunCoordinator(deltaT);
 
        for (auto& pair : limb)
        {
            limbRT(pair.second, deltaT);
        }
        if (hands)
        {
            hands->updateRTStatus(deltaT);
        }
    }
 
    void
    NJointTaskspaceCollisionAvoidanceImpedanceController::updateCollisionAvoidanceConfig(
        const ::armarx::aron::data::dto::DictPtr& dto,
        const Ice::Current& iceCurrent)
    {
        ARMARX_CHECK_EXPRESSION(coll);
        auto cfg = common::control_law::arondto::CollisionAvoidanceConfigDict::FromAron(dto);
        coll->setUserCfg(cfg);
    }
 
    ::armarx::aron::data::dto::DictPtr
    NJointTaskspaceCollisionAvoidanceImpedanceController::getCollisionAvoidanceConfig(
        const Ice::Current& iceCurrent)
    {
        if (not collReady.load())
            return nullptr;
 
        ARMARX_CHECK_EXPRESSION(coll);
        return coll->userCfg.toAronDTO();
    }
 
    void
    NJointTaskspaceCollisionAvoidanceImpedanceController::updateConfig(
        const ::armarx::aron::data::dto::DictPtr& dto,
        const Ice::Current& iceCurrent)
    {
        NJointTaskspaceImpedanceController::updateConfig(dto);
        userCfgWithColl = CollisionCtrlCfg::FromAron(dto);
        ARMARX_CHECK_EXPRESSION(coll);
        coll->setUserCfg(userCfgWithColl.coll);
    }
 
    ::armarx::aron::data::dto::DictPtr
    NJointTaskspaceCollisionAvoidanceImpedanceController::getConfig(const Ice::Current& iceCurrent)
    {
        NJointTaskspaceImpedanceController::getConfig();
        userCfgWithColl.limbs = userConfig.limbs;
        userCfgWithColl.hands = userConfig.hands;
        return userCfgWithColl.toAronDTO();
    }
 
    void
    NJointTaskspaceCollisionAvoidanceImpedanceController::collLimbPublish(
        core::CollisionAvoidanceBase::NodeSetData& arm,
        const DebugObserverInterfacePrx& debugObs)
    {
        //        double limbTimeMeasure = IceUtil::Time::now().toMicroSecondsDouble();
 
        StringVariantBaseMap datafields;
        auto rtStatus = arm.bufferRTtoPublish.getUpToDateReadBuffer();
 
        datafields["trackingError"] = new Variant(rtStatus.trackingError);
        common::debugEigenVec(datafields, "desiredJointTorques", rtStatus.desiredJointTorques);
        common::debugEigenVec(datafields, "selfCollisionTorque", rtStatus.selfCollisionJointTorque);
        common::debugEigenVec(datafields, "jointLimitTorque", rtStatus.jointLimitJointTorque);
 
        common::debugEigenVec(
            datafields, "projectedImpedanceTorque", rtStatus.projImpedanceJointTorque);
        //                common::debugEigenVec(datafields, "projectedJointLimitTorque", rtStatus.projJointLimTorque);
        //                common::debugEigenVec(datafields, "projectedForceImpedance", rtStatus.projForceImpedance);
        //       common::debugEigenVec(
        //            datafields, "projectedSelfCollisionTorque", rtStatus.projSelfCollTorque);
 
        Eigen::VectorXf selfCollNullspace = rtStatus.selfCollNullSpace.diagonal();
        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["activeCollPairsNum"] = new Variant(rtStatus.activeCollPairsNum);
        //        datafields["collisionPairsNum"] = new Variant(rtStatus.collisionPairsNum);
        //        datafields["jointLimitNullspaceTime"] = new Variant(rtStatus.jointLimitNullspaceTime);
        //        datafields["jointLimitTorqueTime"] = new Variant(rtStatus.jointLimitTorqueTime);
        //        datafields["collisionTorqueTime"] = new Variant(rtStatus.collisionTorqueTime);
        //        datafields["collNullspaceTime"] = new Variant(rtStatus.collNullspaceTime);
        //        datafields["collisionPairTime"] = new Variant(rtStatus.collisionPairTime);
        //        datafields["impForceRatio"] = new Variant(rtStatus.impForceRatio);
        //        datafields["impTorqueRatio"] = new Variant(rtStatus.impTorqueRatio);
 
        //        double timeMeasure = IceUtil::Time::now().toMicroSecondsDouble();
        //        viz::Layer layer = arviz.layer(getName() + "_" + arm->nodeSetName);
        //        for (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(rtStatus.collDataVec[i].point * 1000.0,
        //                                  rtStatus.collDataVec[i].point * 1000.0 +
        //                                      rtStatus.collDataVec[i].direction * 50.0 *
        //                                          rtStatus.collDataVec[i].repulsiveForce)
        //                          .color(simox::Color::blue()));
 
        //            //
 
        //            //            //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].point);
        //            //            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);
        //            }
        //        }
 
        // visualize impedance force
 
        //        layer.add(viz::Arrow("impForce")
        //                      .fromTo(rtStatus.currentPose.block<3, 1>(0, 3),
        //                              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()));
 
        //        arviz.commit(layer);
 
 
        //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);
    }
 
    void
    NJointTaskspaceCollisionAvoidanceImpedanceController::onPublish(
        const SensorAndControl& sc,
        const DebugDrawerInterfacePrx& drawer,
        const DebugObserverInterfacePrx& debugObs)
    {
        NJointTaskspaceImpedanceController::onPublish(sc, drawer, debugObs);
        if (coll)
        {
            for (auto& pair : coll->collLimb)
            {
                collLimbPublish(pair.second, debugObs);
            }
        }
    }
 
    void
    NJointTaskspaceCollisionAvoidanceImpedanceController::rtPreActivateController()
    {
        ARMARX_RT_LOGF_INFO(
            "rt Preactivate controller NJointTaskspaceCollisionAvoidanceImpedanceController");
        NJointTaskspaceImpedanceController::rtPreActivateController();
        ARMARX_TRACE;
        if (collReady.load())
        {
            ARMARX_CHECK_EXPRESSION(coll);
            coll->rtPreActivate();
        }
    }
 
    void
    NJointTaskspaceCollisionAvoidanceImpedanceController::rtPostDeactivateController()
    {
        NJointTaskspaceImpedanceController::rtPostDeactivateController();
        if (collReady.load())
        {
            ARMARX_CHECK_EXPRESSION(coll);
            coll->rtPostDeactivate();
        }
        ARMARX_RT_LOGF_INFO(
            "post deactivate: NJointTaskspaceCollisionAvoidanceImpedanceController");
    }
 
    WidgetDescription::WidgetPtr
    NJointTaskspaceCollisionAvoidanceImpedanceController::GenerateConfigDescription(
        const VirtualRobot::RobotPtr& robot,
        const std::map<std::string, ConstControlDevicePtr>& controlDevices,
        const std::map<std::string, ConstSensorDevicePtr>&)
    {
        using namespace armarx::WidgetDescription;
        HBoxLayoutPtr layout = new HBoxLayout;
 
 
        ::armarx::WidgetDescription::WidgetSeq widgets;
 
        /// select default config
        LabelPtr label = new Label;
        label->text = "select a controller config";
 
        StringComboBoxPtr cfgBox = new StringComboBox;
        cfgBox->name = "config_box";
        cfgBox->defaultIndex = 0;
        cfgBox->multiSelect = false;
 
        cfgBox->options =
            std::vector<std::string>{"default", "default_a7_right", "default_a7_right_zero_torque"};
        ARMARX_TRACE;
 
        layout->children.emplace_back(label);
        layout->children.emplace_back(cfgBox);
        ARMARX_TRACE;
 
        layout->children.insert(layout->children.end(), widgets.begin(), widgets.end());
        ARMARX_INFO_S << "Layout done";
        return layout;
    }
 
    NJointTaskspaceCollisionAvoidanceImpedanceController::ConfigPtrT
    NJointTaskspaceCollisionAvoidanceImpedanceController::GenerateConfigFromVariants(
        const StringVariantBaseMap& values)
    {
        auto cfgName = values.at("config_box")->getString();
        const armarx::PackagePath configPath(
            "armarx_control",
            "controller_config/NJointTaskspaceCollisionAvoidanceImpedanceController/" + 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()};
    }
 
} // namespace armarx::control::njoint_controller::task_space