ObjectCollisionAvoidanceImpedanceController.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 "ObjectCollisionAvoidanceImpedanceController.h"
 
#include <VirtualRobot/MathTools.h>
#include <VirtualRobot/RobotNodeSet.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>
 
#include <simox/control/geodesics/util.h>
#include <simox/control/robot/NodeInterface.h>
#include <simox/control/environment/collision.h>
#include <simox/control/utils/primitive.h>
 
#include <ArmarXCore/core/logging/Logging.h>
 
namespace armarx::control::njoint_controller::task_space
{
    NJointControllerRegistration<NJointTaskspaceObjectCollisionAvoidanceImpedanceController>
        registrationControllerNJointTaskspaceObjectCollisionAvoidanceImpedanceController(
            "NJointTaskspaceObjectCollisionAvoidanceImpedanceController");
 
    NJointTaskspaceObjectCollisionAvoidanceImpedanceController::
        NJointTaskspaceObjectCollisionAvoidanceImpedanceController(
            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);
 
        coll = std::make_shared<core::ObjectCollisionAvoidanceBase>(rtGetRobot(), userCfgWithColl.coll);
        collReady.store(true);
        ARMARX_INFO << "-- " << getClassName() << " initialized --";
    }
 
    std::string
    NJointTaskspaceObjectCollisionAvoidanceImpedanceController::getClassName(const Ice::Current&) const
    {
        return "NJointTaskspaceObjectCollisionAvoidanceImpedanceController";
    }
 
    void
    NJointTaskspaceObjectCollisionAvoidanceImpedanceController::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);
        }
        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
    NJointTaskspaceObjectCollisionAvoidanceImpedanceController::rtRun(
        const IceUtil::Time& sensorValuesTimestamp,
        const IceUtil::Time& timeSinceLastIteration)
    {
        double deltaT = timeSinceLastIteration.toSecondsDouble();
 
        if (collReady.load())
        {
            ARMARX_CHECK_EXPRESSION(coll);
            coll->updateRtConfigFromUser();
            coll->updateRtCollisionObjects();
            coll->rtCollisionChecking();
        }
        for (auto& pair : limb)
        {
            this->limbRT(pair.second, deltaT);
        }
        if (hands)
        {
            hands->updateRTStatus(deltaT);
            hands->setTargets();
        }
    }
 
    Eigen::Isometry3d
    NJointTaskspaceObjectCollisionAvoidanceImpedanceController::matrixToIsometry(Eigen::Matrix<double, 4, 4> transformation)
    {
        Eigen::Matrix3d rotation = transformation.block<3,3>(0,0);
        Eigen::Vector3d translation = transformation.block<3,1>(0,3);
        Eigen::Isometry3d transform;
        transform.linear() = rotation;
        transform.translation() = translation;
        return transform;
    }
 
    void
    NJointTaskspaceObjectCollisionAvoidanceImpedanceController::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);
    }
 
    void
    NJointTaskspaceObjectCollisionAvoidanceImpedanceController::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);
    }
 
    void
    NJointTaskspaceObjectCollisionAvoidanceImpedanceController::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},
                            matrixToIsometry(box.transformation)
                            )
                    );
        }
 
        for (auto sphere : spheres)
        {
            collisionObjectsOfSource.push_back(
                    simox::control::environment::createCollisionObject(
                            simox::control::utils::primitive::Sphere{.radius = sphere.radius},
                            matrixToIsometry(sphere.transformation)
                    )
            );
        }
 
        for (auto cylinder : cylinders)
        {
            collisionObjectsOfSource.push_back(
                    simox::control::environment::createCollisionObject(
                            simox::control::utils::primitive::Cylinder{.radius = cylinder.radius, .lengthY = cylinder.lengthY},
                            matrixToIsometry(cylinder.transformation)
                    )
            );
        }
 
        for (auto capsule : capsules)
        {
            collisionObjectsOfSource.push_back(
                    simox::control::environment::createCollisionObject(
                            simox::control::utils::primitive::Capsule{.radius = capsule.radius, .lengthY = capsule.lengthY},
                            matrixToIsometry(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},
                            matrixToIsometry(ellipsoid.transformation)
                    )
            );
        }
 
        sendCollisionObjects();
    }
 
 
 
    ::armarx::aron::data::dto::DictPtr
    NJointTaskspaceObjectCollisionAvoidanceImpedanceController::getCollisionAvoidanceConfig(
        const Ice::Current& iceCurrent)
    {
        if (not collReady.load())
            return nullptr;
 
        ARMARX_CHECK_EXPRESSION(coll);
        return coll->userCfg.toAronDTO();
    }
 
    void
    NJointTaskspaceObjectCollisionAvoidanceImpedanceController::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
    NJointTaskspaceObjectCollisionAvoidanceImpedanceController::getConfig(const Ice::Current& iceCurrent)
    {
        NJointTaskspaceImpedanceController::getConfig();
        userCfgWithColl.limbs = userConfig.limbs;
        userCfgWithColl.hands = userConfig.hands;
        return userCfgWithColl.toAronDTO();
    }
 
    void
    NJointTaskspaceObjectCollisionAvoidanceImpedanceController::collLimbPublish(
        core::ObjectCollisionAvoidanceBase::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("force_" + std::to_string(i))
                .fromTo(rtStatus.collDataVec[i].point1 * 1000.0,
                        rtStatus.collDataVec[i].point1 * 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].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);
        //            }
        //        }
 
        // visualize impedance force
 
        layer.add(viz::Arrow("projImpForce")
            .fromTo(rtStatus.globalPose.block<3, 1>(0, 3),
                    rtStatus.globalPose.block<3, 1>(0, 3) +
                    rtStatus.projForceImpedance.head(3) * 1000.0)
            .color(simox::Color::red()));
 
        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
    NJointTaskspaceObjectCollisionAvoidanceImpedanceController::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);
            auto position = object.getTranslation().cast<float>() * 1000;
            auto 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:
                {
                    // hpp fcl has height as z for cylinders, arviz y axis
                    const auto _rotation = rotation * Eigen::AngleAxisf(-M_PI / 2., Eigen::Vector3f::UnitX());
                    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)
                        .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();
                    Eigen::Vector3f offset;
                    offset << 0, 0, static_cast<float>(capsule->halfLength) * 1000;
                    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 auto _rotation = rotation * Eigen::AngleAxisf(-M_PI / 2., Eigen::Vector3f::UnitX());
                    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);
    }
 
    void
    NJointTaskspaceObjectCollisionAvoidanceImpedanceController::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);
            }
            collObjectPublish(coll->userCollisionObjects, debugObs, "_collisionObjects");
 
            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");
        }
    }
 
    void
    NJointTaskspaceObjectCollisionAvoidanceImpedanceController::rtPreActivateController()
    {
        ARMARX_RT_LOGF_INFO(
            "rt Preactivate controller NJointTaskspaceObjectCollisionAvoidanceImpedanceController");
        NJointTaskspaceImpedanceController::rtPreActivateController();
        ARMARX_TRACE;
        if (collReady.load())
        {
            ARMARX_CHECK_EXPRESSION(coll);
            coll->rtPreActivate();
        }
    }
 
    void
    NJointTaskspaceObjectCollisionAvoidanceImpedanceController::rtPostDeactivateController()
    {
        NJointTaskspaceImpedanceController::rtPostDeactivateController();
        if (collReady.load())
        {
            ARMARX_CHECK_EXPRESSION(coll);
            coll->rtPostDeactivate();
        }
        ARMARX_RT_LOGF_INFO(
            "post deactivate: NJointTaskspaceObjectCollisionAvoidanceImpedanceController");
    }
 
    WidgetDescription::WidgetPtr
    NJointTaskspaceObjectCollisionAvoidanceImpedanceController::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;
    }
 
    NJointTaskspaceObjectCollisionAvoidanceImpedanceController::ConfigPtrT
    NJointTaskspaceObjectCollisionAvoidanceImpedanceController::GenerateConfigFromVariants(
        const StringVariantBaseMap& values)
    {
        auto cfgName = values.at("config_box")->getString();
        const armarx::PackagePath configPath(
            "armarx_control",
            "controller_config/NJointTaskspaceObjectCollisionAvoidanceImpedanceController/" + 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