ZeroTorqueOrVelocityController.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       2024
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "ZeroTorqueOrVelocityController.h"
 
#include <VirtualRobot/MathTools.h>
 
#include <ArmarXCore/core/ArmarXObjectScheduler.h>
#include <ArmarXCore/core/PackagePath.h>
#include <ArmarXCore/core/time/CycleUtil.h>
#include <ArmarXCore/core/time/TimeUtil.h>
#include <ArmarXCore/observers/variant/SingleTypeVariantList.h>
 
#include <armarx/control/common/aron_conversions.h>
#include <armarx/control/common/utils.h>
 
namespace armarx::control::njoint_controller::task_space
{
    NJointControllerRegistration<NJointTaskspaceZeroTorqueOrVelocityController>
        registrationControllerNJointTaskspaceZeroTorqueOrVelocityController(
            "NJointTaskspaceZeroTorqueOrVelocityController");
 
    void
    NJointTaskspaceZeroTorqueOrVelocityController::limbInit(const std::string nodeSetName,
                                                            ArmPtr& arm,
                                                            Config cfg,
                                                            VirtualRobot::RobotPtr& nonRtRobotPtr)
    {
        arm->kinematicChainName = nodeSetName;
        VirtualRobot::RobotNodeSetPtr rtRns =
            rtGetRobot()->getRobotNodeSet(arm->kinematicChainName);
        VirtualRobot::RobotNodeSetPtr nonRtRns =
            nonRtRobotPtr->getRobotNodeSet(arm->kinematicChainName);
        ARMARX_CHECK_EXPRESSION(rtRns) << arm->kinematicChainName;
        ARMARX_CHECK_EXPRESSION(nonRtRns) << arm->kinematicChainName;
        ARMARX_IMPORTANT
            << "Creating Taskspace Zero Torque or Velocity Controller with kinematic chain: "
            << arm->kinematicChainName << " with num of joints: (RT robot) " << rtRns->getSize()
            << ", and (non-RT robot) " << nonRtRns->getSize();
 
        std::vector<size_t> jointIDTorqueMode;
        std::vector<size_t> jointIDVelocityMode;
        arm->jointNames.clear();
        for (size_t i = 0; i < rtRns->getSize(); ++i)
        {
 
            std::string jointName = rtRns->getNode(i)->getName();
            arm->jointNames.push_back(jointName);
 
            bool foundControlDevice = false;
            auto it = std::find(
                cfg.jointNameListTorque.begin(), cfg.jointNameListTorque.end(), jointName);
            if (it != cfg.jointNameListTorque.end())
            {
                ControlTargetBase* ct = useControlTarget(jointName, ControlModes::ZeroTorque1DoF);
                ARMARX_CHECK_EXPRESSION(ct);
                auto* casted_ct = ct->asA<ControlTarget1DoFActuatorZeroTorque>();
                ARMARX_CHECK_EXPRESSION(casted_ct);
                arm->targetsTorque.push_back(casted_ct);
                jointIDTorqueMode.push_back(i);
                foundControlDevice = true;
            }
 
            it = std::find(
                cfg.jointNameListVelocity.begin(), cfg.jointNameListVelocity.end(), jointName);
            if (it != cfg.jointNameListVelocity.end())
            {
                ControlTargetBase* ct = useControlTarget(jointName, ControlModes::ZeroVelocity1DoF);
                ARMARX_CHECK_EXPRESSION(ct);
                auto* casted_ct = ct->asA<ControlTarget1DoFActuatorZeroVelocity>();
                ARMARX_CHECK_EXPRESSION(casted_ct);
                arm->targetsVel.push_back(casted_ct);
                jointIDVelocityMode.push_back(i);
                foundControlDevice = true;
            }
            if (not foundControlDevice)
            {
                ARMARX_INFO << jointName << " is not selected for " << arm->kinematicChainName;
            }
            auto namesTorque = armarx::control::common::sVecToString(cfg.jointNameListTorque);
            auto namesVelocity = armarx::control::common::sVecToString(cfg.jointNameListVelocity);
            ARMARX_INFO << "Robot node set '" << arm->kinematicChainName
                        << "' has torque controlled joints [" << namesTorque
                        << "] and velocity controlled joints [" << namesVelocity << "].";
 
            const SensorValueBase* sv = useSensorValue(jointName);
            ARMARX_CHECK_EXPRESSION(sv);
            const auto* torqueSensor = sv->asA<SensorValue1DoFActuatorTorque>();
            const auto* velocitySensor = sv->asA<SensorValue1DoFActuatorVelocity>();
            const auto* positionSensor = sv->asA<SensorValue1DoFActuatorPosition>();
 
            if (torqueSensor == nullptr)
            {
                ARMARX_WARNING << "No Torque sensor available for " << jointName;
            }
            if (velocitySensor == nullptr)
            {
                ARMARX_WARNING << "No velocity sensor available for " << jointName;
            }
            if (positionSensor == nullptr)
            {
                ARMARX_WARNING << "No position sensor available for " << jointName;
            }
 
 
            arm->sensorDevices.torqueSensors.push_back(torqueSensor);
            arm->sensorDevices.velocitySensors.push_back(velocitySensor);
            arm->sensorDevices.positionSensors.push_back(positionSensor);
        };
        ARMARX_DEBUG << "Number of torque controlled joints " << jointIDTorqueMode.size();
        ARMARX_DEBUG << "Number of velocity controlled joints " << jointIDVelocityMode.size();
        //        ARMARX_CHECK_EQUAL(rtRns->getNodeNames().size(),
        //                           jointIDTorqueMode.size() + jointIDVelocityMode.size());
 
        arm->controller.initialize(nonRtRns, rtRns, jointIDTorqueMode, jointIDVelocityMode);
        arm->controller.ftsensor.initialize(rtRns, robotUnit, cfg.ftConfig);
 
        validateConfigData(cfg, arm);
        arm->rtConfig = cfg;
        arm->nonRtConfig = cfg;
    }
 
    NJointTaskspaceZeroTorqueOrVelocityController::NJointTaskspaceZeroTorqueOrVelocityController(
        const RobotUnitPtr& robotUnit,
        const NJointControllerConfigPtr& config,
        const VirtualRobot::RobotPtr&) :
        robotUnit(robotUnit)
    {
        ARMARX_CHECK_EXPRESSION(robotUnit);
 
        ConfigPtrT cfg = ConfigPtrT::dynamicCast(config);
        ARMARX_CHECK_EXPRESSION(cfg);
        userConfig = ::armarx::fromAronDict<ConfigDict, ConfigDict>(cfg->config);
 
        useSynchronizedRtRobot();
        nonRtRobot = robotUnit->cloneRobot();
        robotUnit->updateVirtualRobot(nonRtRobot);
 
        for (auto& pair : userConfig.limbs)
        {
            limb.emplace(pair.first, std::make_unique<ArmData>());
            limbInit(pair.first, limb.at(pair.first), pair.second, nonRtRobot);
        }
    }
 
    std::string
    NJointTaskspaceZeroTorqueOrVelocityController::getClassName(const Ice::Current&) const
    {
        return "NJointTaskspaceZeroTorqueOrVelocityController";
    }
 
    void
    NJointTaskspaceZeroTorqueOrVelocityController::onInitNJointController()
    {
    }
 
    void
    NJointTaskspaceZeroTorqueOrVelocityController::limbRT(ArmPtr& arm, const double deltaT)
    {
        double timeMeasure = IceUtil::Time::now().toMicroSecondsDouble();
        arm->rtConfig = arm->bufferNonRtToRt.getUpToDateReadBuffer();
        double time_update_non_rt_buffer =
            IceUtil::Time::now().toMicroSecondsDouble() - timeMeasure;
 
        if (arm->rtFirstRun.load())
        {
            arm->rtFirstRun.store(false);
            arm->rtReady.store(true);
        }
        arm->controller.ftsensor.updateStatus(
            arm->rtConfig.ftConfig, arm->rtStatus.currentForceTorque, arm->rtStatus.deltaT, arm->rtFirstRun.load());
 
        double time_update_ft = IceUtil::Time::now().toMicroSecondsDouble() - timeMeasure;
 
        size_t nDoF = arm->sensorDevices.positionSensors.size();
        double time_update_size = IceUtil::Time::now().toMicroSecondsDouble() - timeMeasure;
        for (size_t i = 0; i < nDoF; ++i)
        {
            arm->rtStatus.jointPosition[i] = arm->sensorDevices.positionSensors[i]->position;
            arm->rtStatus.jointVelocity[i] = arm->sensorDevices.velocitySensors[i]->velocity;
            arm->rtStatus.jointTorque[i] = arm->sensorDevices.torqueSensors[i]->torque;
        }
 
        double time_update_js = IceUtil::Time::now().toMicroSecondsDouble() - timeMeasure;
        arm->rtStatus.deltaT = deltaT;
        double time_update_rt_status = IceUtil::Time::now().toMicroSecondsDouble() - timeMeasure;
 
        arm->bufferRtToNonRt.getWriteBuffer() = arm->rtStatus;
        arm->bufferRtToNonRt.commitWrite();
        arm->bufferRtStatusToUser.getWriteBuffer() = arm->rtStatus;
        arm->bufferRtStatusToUser.commitWrite();
        double time_write_rt_buffer = IceUtil::Time::now().toMicroSecondsDouble() - timeMeasure;
 
        arm->controller.run(arm->rtConfig, arm->rtStatus);
        double time_run_rt = IceUtil::Time::now().toMicroSecondsDouble() - timeMeasure;
 
        for (unsigned int i = 0; i < arm->rtStatus.nDoFTorque; i++)
        {
            auto jointIdx = arm->controller.jointIDTorqueMode[i];
            arm->targetsTorque.at(i)->torque = arm->rtStatus.desiredJointTorques(jointIdx);
            if (!arm->targetsTorque.at(i)->isValid())
            {
                arm->targetsTorque.at(i)->torque = 0;
            }
        }
        for (unsigned int i = 0; i < arm->rtStatus.nDoFVelocity; i++)
        {
            auto jointIdx = arm->controller.jointIDVelocityMode[i];
            arm->targetsVel.at(i)->velocity = arm->rtStatus.desiredJointVelocity(jointIdx);
            // if (!targets.at(i)->isValid() || fabs(targets.at(i)->velocity) > cfg->maxJointVelocity)
            if (!arm->targetsVel.at(i)->isValid())
            {
                arm->targetsVel.at(i)->velocity = 0;
            }
        }
        arm->bufferRtStatusToOnPublish.getWriteBuffer() = arm->rtStatus;
        arm->bufferRtStatusToOnPublish.commitWrite();
 
        arm->bufferRtConfigToOnPublish.getWriteBuffer() = arm->rtConfig;
        arm->bufferRtConfigToOnPublish.commitWrite();
 
        arm->bufferRtConfigToUser.getWriteBuffer() = arm->rtConfig;
        arm->bufferRtConfigToUser.commitWrite();
        double time_rt_status_buffer = IceUtil::Time::now().toMicroSecondsDouble() - timeMeasure;
        timeMeasure = IceUtil::Time::now().toMicroSecondsDouble() - timeMeasure;
 
        if (timeMeasure > 100)
        {
            ARMARX_RT_LOGF_WARN("---- rt too slow: unit in (us)"
                                "update_non_rt_buffer: %.2f\n"
                                "update_ft: %.2f\n"
                                "update_size: %.2f\n"
                                "update_js: %.2f\n"
                                "update_rt_status: %.2f\n"
                                "write_rt_buffer: %.2f\n"
                                "run_rt: %.2f\n"
                                "rt_status_buffer: %.2f\n"
                                "time all: %.2f\n",
                                time_update_non_rt_buffer, // 0-1 us
                                time_update_ft - time_update_non_rt_buffer, //
                                time_update_size - time_update_ft, //
                                time_update_js - time_update_size, //
                                time_update_rt_status - time_update_non_rt_buffer, // 1-2 us
                                time_write_rt_buffer - time_update_rt_status, // 0-1 us
                                time_run_rt - time_write_rt_buffer, // 26-33 us
                                time_rt_status_buffer - time_run_rt,
                                timeMeasure); // 0-1 us
        }
    }
 
    void
    NJointTaskspaceZeroTorqueOrVelocityController::rtRun(
        const IceUtil::Time& /*sensorValuesTimestamp*/,
        const IceUtil::Time& timeSinceLastIteration)
    {
        double deltaT = timeSinceLastIteration.toSecondsDouble();
        for (auto& pair : limb)
        {
            limbRT(pair.second, deltaT);
        }
    }
 
    void
    NJointTaskspaceZeroTorqueOrVelocityController::updateConfig(
        const ::armarx::aron::data::dto::DictPtr& dto,
        const Ice::Current& iceCurrent)
    {
        userConfig = userConfig.FromAron(dto);
        //        userConfig = ::armarx::fromAronDict<ConfigDict, ConfigDict>(dto);
 
        for (auto& pair : userConfig.limbs)
        {
            validateConfigData(pair.second, limb.at(pair.first));
            limb.at(pair.first)->bufferUserToNonRt.getWriteBuffer() = pair.second;
            limb.at(pair.first)->bufferUserToNonRt.commitWrite();
        }
    }
 
    ::armarx::aron::data::dto::DictPtr
    NJointTaskspaceZeroTorqueOrVelocityController::getConfig(const Ice::Current& iceCurrent)
    {
        for (auto& pair : limb)
        {
            userConfig.limbs.at(pair.first) =
                pair.second->bufferRtConfigToUser.getUpToDateReadBuffer();
        }
        return userConfig.toAronDTO();
    }
 
    void
    NJointTaskspaceZeroTorqueOrVelocityController::validateConfigData(Config& configData,
                                                                      ArmPtr& arm)
    {
        const auto checkNonNegative = [](const auto& v) { ARMARX_CHECK(v >= 0); };
        const auto checkVecNonNegative = [](const auto& v)
        { ARMARX_CHECK((v.array() >= 0).all()); };
 
        checkVecNonNegative(configData.kpCartesianVel);
        checkVecNonNegative(configData.kdCartesianVel);
 
        checkNonNegative(configData.torqueLimit);
        checkNonNegative(configData.velocityLimit);
        checkNonNegative(configData.qvelFilter);
        checkNonNegative(configData.cartesianLinearVelLimit);
        checkNonNegative(configData.cartesianAngularVelLimit);
    }
 
    void
    NJointTaskspaceZeroTorqueOrVelocityController::limbPublish(
        ArmPtr& arm,
        const DebugObserverInterfacePrx& debugObs)
    {
        StringVariantBaseMap datafields;
        auto rtConfig = arm->bufferRtConfigToOnPublish.getUpToDateReadBuffer();
        auto rtStatus = arm->bufferRtStatusToOnPublish.getUpToDateReadBuffer();
 
        //        datafields[name + "_x"] = new Variant(pose(0, 3));
 
        common::debugEigenVec(datafields, "cartesianVelTarget", rtStatus.cartesianVelTarget);
        datafields["velocityLimit"] = new Variant(rtConfig.velocityLimit);
        common::debugEigenVec(datafields, "desiredJointVelocity", rtStatus.desiredJointVelocity);
        common::debugEigenVec(datafields, "currentForceTorque", rtStatus.currentForceTorque);
        debugObs->setDebugChannel("ZT-ZV_" + arm->kinematicChainName, datafields);
    }
 
    void
    NJointTaskspaceZeroTorqueOrVelocityController::onPublish(
        const SensorAndControl&,
        const DebugDrawerInterfacePrx&,
        const DebugObserverInterfacePrx& debugObs)
    {
        for (auto& pair : limb)
        {
            limbPublish(pair.second, debugObs);
        }
    }
 
    void
    NJointTaskspaceZeroTorqueOrVelocityController::calibrateFTSensor(const Ice::Current&)
    {
        for (auto& pair : limb)
        {
            pair.second->rtReady.store(false);
        }
    }
 
    void
    NJointTaskspaceZeroTorqueOrVelocityController::limbReInit(ArmPtr& arm)
    {
        VirtualRobot::RobotNodeSetPtr rns = rtGetRobot()->getRobotNodeSet(arm->kinematicChainName);
 
        arm->bufferUserToNonRt.reinitAllBuffers(arm->rtConfig);
        {
            arm->nonRtConfig = arm->rtConfig;
            arm->bufferNonRtToRt.reinitAllBuffers(arm->rtConfig);
            arm->bufferRtConfigToOnPublish.reinitAllBuffers(arm->rtConfig);
            arm->bufferRtConfigToUser.reinitAllBuffers(arm->rtConfig);
        }
 
        const auto nDoF = rns->getSize();
        const auto nDoFTorque = arm->controller.jointIDTorqueMode.size();
        const auto nDoFVelocity = arm->controller.jointIDVelocityMode.size();
        {
            arm->rtStatus.nDoFTorque = nDoFTorque;
            arm->rtStatus.nDoFVelocity = nDoFVelocity;
            arm->rtStatus.numJoints = nDoF;
            arm->rtStatus.desiredJointTorques.setZero(nDoF);
            arm->rtStatus.desiredJointVelocity.setZero(nDoF);
            arm->rtStatus.cartesianVelTarget.setZero();
 
            arm->rtStatus.jointPosition.resize(nDoF, 0.);
            arm->rtStatus.jointVelocity.resize(nDoF, 0.);
            arm->rtStatus.jointTorque.resize(nDoF, 0.);
            for (size_t i = 0; i < nDoF; ++i)
            {
                arm->rtStatus.jointPosition[i] = arm->sensorDevices.positionSensors[i]->position;
                arm->rtStatus.jointVelocity[i] = arm->sensorDevices.velocitySensors[i]->velocity;
                arm->rtStatus.jointTorque[i] = arm->sensorDevices.torqueSensors[i]->torque;
            }
 
            arm->rtStatus.qpos = rns->getJointValuesEigen();
            arm->rtStatus.qvel.setZero(nDoF);
            arm->rtStatus.qvelFiltered.setZero(nDoF);
 
            arm->rtStatus.currentTwist.setZero();
            arm->rtStatus.jacobi.setZero(6, nDoF);
 
            arm->rtStatus.deltaT = 0.0;
            arm->rtStatus.rtSafe = false;
 
            arm->rtStatus.currentForceTorque.setZero();
 
            arm->rtStatusInNonRT = arm->rtStatus;
            arm->bufferRtStatusToOnPublish.reinitAllBuffers(arm->rtStatus);
            arm->bufferRtToNonRt.reinitAllBuffers(arm->rtStatus);
            arm->bufferRtStatusToUser.reinitAllBuffers(arm->rtStatus);
        }
 
        arm->controller.preactivateInit(rns);
    }
 
    void
    NJointTaskspaceZeroTorqueOrVelocityController::rtPreActivateController()
    {
        for (auto& pair : limb)
        {
            ARMARX_INFO << "rtPreActivateController for " << pair.first;
            limbReInit(pair.second);
            userConfig.limbs.at(pair.first) = pair.second->rtConfig;
        }
    }
 
    void
    NJointTaskspaceZeroTorqueOrVelocityController::rtPostDeactivateController()
    {
        // for (auto& pair : limb)
        // {
        //     pair.second->controller.isInitialized.store(false);
        // }
    }
 
    WidgetDescription::WidgetPtr
    NJointTaskspaceZeroTorqueOrVelocityController::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;
        auto addSlider = [&](const std::string& label, float min, float max, float defaultValue)
        {
            widgets.emplace_back(new Label(false, label));
            {
                FloatSliderPtr c_x = new FloatSlider;
                c_x->name = label;
                c_x->min = min;
                c_x->defaultValue = defaultValue;
                c_x->max = max;
                widgets.emplace_back(c_x);
            }
        };
 
        /// zero torque mode
        LabelPtr label = new Label;
        label->text = "ZeroTorque (L/R)";
 
        StringComboBoxPtr boxZeroTorqueLeft = new StringComboBox;
        boxZeroTorqueLeft->name = "ZeroTorqueLeft";
        boxZeroTorqueLeft->defaultIndex = 0;
        boxZeroTorqueLeft->multiSelect = true;
 
        StringComboBoxPtr boxZeroTorqueRight = new StringComboBox;
        boxZeroTorqueRight->name = "ZeroTorqueRight";
        boxZeroTorqueRight->defaultIndex = 0;
        boxZeroTorqueRight->multiSelect = true;
 
        /// zero velocity mode
        LabelPtr labelZVMode = new Label;
        labelZVMode->text = "ZeroVelocity (L/R)";
 
        StringComboBoxPtr boxZeroVelocityLeft = new StringComboBox;
        boxZeroVelocityLeft->name = "ZeroVelocityLeft";
        boxZeroVelocityLeft->defaultIndex = 0;
        boxZeroVelocityLeft->multiSelect = true;
 
        StringComboBoxPtr boxZeroVelocityRight = new StringComboBox;
        boxZeroVelocityRight->name = "ZeroVelocityRight";
        boxZeroVelocityRight->defaultIndex = 0;
        boxZeroVelocityRight->multiSelect = true;
 
        ARMARX_INFO_S << "Building check boxes";
        ARMARX_TRACE;
        std::map<std::string, StringComboBoxPtr> boxes;
        boxes.insert({"zv_left", boxZeroVelocityLeft});
        boxes.insert({"zv_right", boxZeroVelocityRight});
        boxes.insert({"zt_left", boxZeroTorqueLeft});
        boxes.insert({"zt_right", boxZeroTorqueRight});
 
        ARMARX_TRACE;
        for (auto& c : controlDevices)
        {
            std::string mode;
            if (c.first.find("ArmL") != std::string::npos)
            {
                mode = "left";
            }
            else if (c.first.find("ArmR") != std::string::npos)
            {
                mode = "right";
            }
            else
            {
                continue;
            }
 
            if (c.second->hasJointController(ControlModes::ZeroTorque1DoF))
            {
                boxes.at("zt_" + mode)->options.push_back(c.first);
            }
            else if (c.second->hasJointController(ControlModes::ZeroVelocity1DoF))
            {
                boxes.at("zv_" + mode)->options.push_back(c.first);
            }
        }
        ARMARX_TRACE;
 
        layout->children.emplace_back(label);
        layout->children.emplace_back(boxZeroTorqueLeft);
        layout->children.emplace_back(boxZeroTorqueRight);
        layout->children.emplace_back(labelZVMode);
        layout->children.emplace_back(boxZeroVelocityLeft);
        layout->children.emplace_back(boxZeroVelocityRight);
 
        addSlider("maxTorque", 0, 100, 10);
        addSlider("maxVeloicty", 0, 3.14, 2.0);
        ARMARX_TRACE;
 
        layout->children.insert(layout->children.end(), widgets.begin(), widgets.end());
        ARMARX_INFO_S << "Layout done";
        return layout;
    }
 
    NJointTaskspaceZeroTorqueOrVelocityController::ConfigPtrT
    NJointTaskspaceZeroTorqueOrVelocityController::GenerateConfigFromVariants(
        const StringVariantBaseMap& values)
    {
        const armarx::PackagePath configPath(
            "armarx_control",
            "controller_config/NJointTaskspaceZeroTorqueOrVelocityController/default.json");
        ARMARX_INFO_S << "Loading config from " << configPath.toSystemPath();
        auto cfgDTO = armarx::readFromJson<ConfigDict>(configPath.toSystemPath());
 
        ARMARX_WARNING_S << "Left arm of armar7 is not yet calibrated, the default is set to "
                            "RightArm for safety reason!";
 
        auto varZTLeft = VariantPtr::dynamicCast(values.at("ZeroTorqueLeft"));
        auto varZTRight = VariantPtr::dynamicCast(values.at("ZeroTorqueRight"));
        auto varZVLeft = VariantPtr::dynamicCast(values.at("ZeroVelocityLeft"));
        auto varZVRight = VariantPtr::dynamicCast(values.at("ZeroVelocityRight"));
        ARMARX_CHECK_EXPRESSION(varZTLeft) << "ZeroTorqueLeft";
        ARMARX_CHECK_EXPRESSION(varZTRight) << "ZeroTorqueRight";
        ARMARX_CHECK_EXPRESSION(varZVLeft) << "ZeroVelocityLeft";
        ARMARX_CHECK_EXPRESSION(varZVRight) << "ZeroVelocityRight";
 
        cfgDTO.limbs.at("RightArm").jointNameListTorque =
            varZTRight->get<SingleTypeVariantList>()->toStdVector<std::string>();
        cfgDTO.limbs.at("RightArm").jointNameListVelocity =
            varZVRight->get<SingleTypeVariantList>()->toStdVector<std::string>();
        cfgDTO.limbs.at("RightArm").torqueLimit = values.at("maxTorque")->getFloat();
        cfgDTO.limbs.at("RightArm").velocityLimit = values.at("maxVeloicty")->getFloat();
 
        auto namesTorque =
            armarx::control::common::sVecToString(cfgDTO.limbs.at("RightArm").jointNameListTorque);
        auto namesVelocity =
            armarx::control::common::sVecToString(cfgDTO.limbs.at("RightArm").jointNameListVelocity);
        if (cfgDTO.limbs.at("RightArm").jointNameListTorque.size() > 0)
        {
            ARMARX_INFO_S << "'RightArm' has torque controlled joints [" << namesTorque << "].";
        }
        if (cfgDTO.limbs.at("RightArm").jointNameListVelocity.size() > 0)
        {
            ARMARX_INFO_S << "'RightArm' has velocity controlled joints [" << namesVelocity << "].";
        }
 
        ARMARX_INFO_S << "torque limit: " << cfgDTO.limbs.at("RightArm").torqueLimit
                      << "velocity limit: " << cfgDTO.limbs.at("RightArm").velocityLimit;
 
        ARMARX_TRACE;
        return new ConfigurableNJointControllerConfig{cfgDTO.toAronDTO()};
    }
} // namespace armarx::control::njoint_controller::task_space