ColAvoidVel.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 "ColAvoidVel.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>
    NJointTSVelBasedColController<NJointControllerType, CollisionCtrlCfg>::
        NJointTSVelBasedColController(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);
 
        coll = std::make_shared<CollAvoidVelBase>(this->rtGetRobot(), userCfgWithColl.coll);
        collReady.store(true);
        // const std::string rootNodeName = "root";
        // rootNode = this->rtGetRobot()->getRobotNode(rootNodeName);
    }
 
    template <typename NJointControllerType, typename CollisionCtrlCfg>
    void
    NJointTSVelBasedColController<NJointControllerType, CollisionCtrlCfg>::limbRT(
        ArmPtr& arm,
        const double deltaT)
    {
        // limbRTUpdateStatus(arm, deltaT);
 
        arm->controller.run(arm->rtConfig, arm->rtStatus);
        // double time_run_rt = IceUtil::Time::now().toMicroSecondsDouble() - time_measure;
 
        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->rtStatus.jointPosition,
                                      arm->rtStatus.qvelFiltered,
                                      arm->rtConfig.velocityLimit,
                                      arm->rtStatus.desiredJointVelocity,
                                      deltaT);
            /// 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,
                                  arm->rtStatus.desiredJointTorque,
                                  coll->collLimb.at(arm->kinematicChainName).rts.desiredJointVel);
        }
        else
        {
            this->limbRTSetTarget(arm,
                                  arm->rtStatus.nDoFTorque,
                                  arm->rtStatus.nDoFVelocity,
                                  arm->rtStatus.desiredJointTorque,
                                  arm->rtStatus.desiredJointVelocity);
        }
    }
 
    template <typename NJointControllerType, typename CollisionCtrlCfg>
    void
    NJointTSVelBasedColController<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, deltaT);
        }
        if (this->hands)
        {
            this->hands->updateRTStatus(deltaT);
        }
    }
 
    template <typename NJointControllerType, typename CollisionCtrlCfg>
    void
    NJointTSVelBasedColController<NJointControllerType, CollisionCtrlCfg>::
        updateCollisionAvoidanceConfig(const ::armarx::aron::data::dto::DictPtr& dto,
                                       const Ice::Current& iceCurrent)
    {
        ARMARX_CHECK_EXPRESSION(coll);
        auto cfg =
            common::control_law::arondto::ObjectCollisionAvoidanceVelConfigDict::FromAron(dto);
        coll->setUserCfg(cfg);
    }
 
    template <typename NJointControllerType, typename CollisionCtrlCfg>
    void
    NJointTSVelBasedColController<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
    NJointTSVelBasedColController<NJointControllerType, CollisionCtrlCfg>::deleteCollisionObjects(
        const Ice::Current& iceCurrent)
    {
        coll->deleteUserCollisionObjects();
    }
 
    template <typename NJointControllerType, typename CollisionCtrlCfg>
    void
    NJointTSVelBasedColController<NJointControllerType, CollisionCtrlCfg>::updateCollisionObjects(
        const std::string& primitiveSourceName,
        const ::armarx::aron::data::dto::DictPtr& dto,
        const Ice::Current& iceCurrent)
    {
        ARMARX_INFO_S << "updateCollisionObjects";
        auto scene = common::control_law::arondto::CollisionScene::FromAron(dto);
        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 (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 (auto sphere : spheres)
        {
            collisionObjectsOfSource.push_back(simox::control::environment::createCollisionObject(
                simox::control::utils::primitive::Sphere{.radius = sphere.radius},
                Eigen::Isometry3d{sphere.transformation}));
        }
 
        for (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 (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 (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
    NJointTSVelBasedColController<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
    NJointTSVelBasedColController<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
    NJointTSVelBasedColController<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; // TODO check this
        return userCfgWithColl.toAronDTO();
    }
 
    template <typename NJointControllerType, typename CollisionCtrlCfg>
    void
    NJointTSVelBasedColController<NJointControllerType, CollisionCtrlCfg>::collLimbPublish(
        CollAvoidVelBase::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, "desiredJointVel", rtStatus.desiredJointVel);
        common::debugEigenVec(datafields, "selfCollisionTorque", rtStatus.selfCollisionJointVel);
        common::debugEigenVec(datafields, "jointLimitTorque", rtStatus.jointLimitJointVel);
 
        //        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);
 
        // auto& globalPose = rtStatus.globalPose;
        // auto transformVector = [](const Eigen::Matrix4f& pose,
        //                           const Eigen::Vector3f& vec) -> Eigen::Vector3f
        // {
        //     // Eigen::Vector3f homogeneous_vector;
        //     // homogeneous_vector << vec, 1.0;
        //
        //     return common::getOri(pose) * vec + common::getPos(pose);
        //     // return globalPose.head<3>() / globalPose(3);
        // };
 
        //        double timeMeasure = IceUtil::Time::now().toMicroSecondsDouble();
        //        viz::Layer layer = arviz.layer(getClassName() + "_" + 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].point * 1000.0f),
            //                            transformVector(rtStatus.globalPose,
            //                                            rtStatus.collDataVec[i].point * 1000.0f +
            //                                                rtStatus.collDataVec[i].direction * 50.0f *
            //                                                    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) + "_repVel"] =
                new Variant(rtStatus.collDataVec[i].repulsiveVel);
            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);
        }
        //        layer.add(viz::Pose("___root___").pose(rtStatus.globalPose).scale(2));
 
 
        //        /// 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()));
 
        // layer.add(viz::Robot(filename).pose(Eigen::Matrix4f::Identity()))
        //        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);
    }
 
    template <typename NJointControllerType, typename CollisionCtrlCfg>
    void
    NJointTSVelBasedColController<NJointControllerType, CollisionCtrlCfg>::collObjectPublish(
        const std::vector<hpp::fcl::CollisionObject>& objects,
        const DebugObserverInterfacePrx& debugObs,
        const std::string& layerSuffix)
    {
 
        viz::Layer objectLayer = arviz.layer(getName() + layerSuffix);
        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);
                    objectLayer.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);
                    objectLayer.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);
                    objectLayer.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);
                    objectLayer.add(vizObject);
                    objectLayer.add(cap1);
                    objectLayer.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);
                    objectLayer.add(vizObject);
                    break;
                }
                default:;
            }
        }
        arviz.commit(objectLayer);
    }
 
    template <typename NJointControllerType, typename CollisionCtrlCfg>
    void
    NJointTSVelBasedColController<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);
            }
            collObjectPublish(coll->userCollisionObjects, debugObs, "_collisionObjects");
 
            // TODO uses rt collision robot
            const auto& robotObjects = coll->collisionRobot->getCollisionManager()->getObjects();
            std::vector<hpp::fcl::CollisionObject> robotObjectsByValue;
            for (const auto* ptr : robotObjects)
            {
                robotObjectsByValue.push_back(*ptr);
            }
            collObjectPublish(robotObjectsByValue, debugObs, "_collisionRobot");
        }
    }
 
    template <typename NJointControllerType, typename CollisionCtrlCfg>
    void
    NJointTSVelBasedColController<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
    NJointTSVelBasedColController<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 NJointTSVelBasedColController<NJointControllerType, CollisionCtrlCfg>::ConfigPtrT
    NJointTSVelBasedColController<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 NJointTSVelBasedColController<NJointTSVelController,
                                                 law::arondto::TSVelColConfigDict>;
 
    NJointControllerRegistration<NJointTSVelColController>
        registrationControllerNJointTSVelColController("TSVelCol");
 
    NJointTSVelColController::NJointTSVelColController(const RobotUnitPtr& robotUnit,
                                                       const NJointControllerConfigPtr& config,
                                                       const VirtualRobot::RobotPtr& robot) :
        // NJointTSVelController(robotUnit, config, robot),
        NJointTSVelBasedColController<NJointTSVelController, law::arondto::TSVelColConfigDict>(
            robotUnit,
            config,
            robot)
    {
    }
 
    std::string
    NJointTSVelColController::getClassName(const Ice::Current&) const
    {
        return "TSVelCol";
    }
 
} // namespace armarx::control::njoint_controller::task_space