ZeroTorque.cpp

Go to the documentation of this file.

#include "ZeroTorque.h"
 
#include <VirtualRobot/Nodes/RobotNode.h>
#include <VirtualRobot/Robot.h>
 
#include <ArmarXCore/observers/variant/SingleTypeVariantList.h>
 
#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/core/math/MathUtils.h>
 
namespace armarx::control::njoint_controller::joint_space
{
 
    NJointControllerRegistration<NJointZeroTorqueController>
        registrationControllerNJointZeroTorqueController("NJointZeroTorqueController");
 
    std::string
    NJointZeroTorqueController::getClassName(const Ice::Current&) const
    {
        return "NJointZeroTorqueController";
    }
 
    NJointZeroTorqueController::NJointZeroTorqueController(
        RobotUnitPtr prov,
        const NJointZeroTorqueControllerConfigPtr& config,
        const VirtualRobot::RobotPtr& r)
    {
        ARMARX_CHECK_EXPRESSION(prov);
        RobotUnitPtr robotUnit = RobotUnitPtr::dynamicCast(prov);
        ARMARX_CHECK_NOT_NULL(robotUnit);
        ARMARX_CHECK_NOT_NULL(config);
        ARMARX_CHECK_EXPRESSION(!config->jointNames.empty());
        for (auto& jointName : config->jointNames)
        {
            auto node = r->getRobotNode(jointName);
            ARMARX_CHECK_EXPRESSION(node) << jointName;
 
            ControlTargetBase* ct = useControlTarget(jointName, ControlModes::ZeroTorque1DoF);
            ARMARX_CHECK_NOT_NULL(ct) << jointName;
            targets.push_back(ct->asA<ControlTarget1DoFActuatorZeroTorque>());
        }
        maxTorque = config->maxTorque;
        NJointZeroTorqueControllerTarget target;
        target.targetTorques = Ice::FloatSeq(config->jointNames.size(), 0.0f);
        this->reinitTripleBuffer(target);
    }
 
    void
    NJointZeroTorqueController::rtPreActivateController()
    {
 
        //    targetTorque = 0.0;
    }
 
    void
    NJointZeroTorqueController::rtRun(const IceUtil::Time& sensorValuesTimestamp,
                                      const IceUtil::Time& timeSinceLastIteration)
    {
        for (size_t i = 0; i < targets.size(); ++i)
        {
 
            targets.at(i)->torque =
                math::MathUtils::LimitTo(rtGetControlStruct().targetTorques.at(i), maxTorque);
        }
    }
 
    WidgetDescription::WidgetPtr
    NJointZeroTorqueController::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);
            }
        };
 
        LabelPtr label = new Label;
        label->text = "joint names";
        layout->children.emplace_back(label);
        StringComboBoxPtr box = new StringComboBox;
        box->defaultIndex = 0;
        box->multiSelect = true;
 
        for (auto& c : controlDevices)
        {
            if (c.second->hasJointController(ControlModes::ZeroTorque1DoF))
            {
                box->options.push_back(c.first);
            }
        }
        box->name = "jointNames";
        layout->children.emplace_back(box);
 
        addSlider("maxTorque", 0, 100, 10);
 
        layout->children.insert(layout->children.end(), widgets.begin(), widgets.end());
 
 
        //        auto sliders = createSliders(0, 0, 0, 1, 0.1, 0.01, 10.0f, 10.0f, 0.0f);
        //        layout->children.insert(layout->children.end(),
        //                                sliders.begin(),
        //                                sliders.end());
        //        layout->children.emplace_back(new HSpacer);
        return layout;
    }
 
    NJointZeroTorqueControllerConfigPtr
    NJointZeroTorqueController::GenerateConfigFromVariants(const StringVariantBaseMap& values)
    {
        auto var = VariantPtr::dynamicCast(values.at("jointNames"));
        ARMARX_CHECK_EXPRESSION(var) << "jointNames";
        return new NJointZeroTorqueControllerConfig{
            var->get<SingleTypeVariantList>()->toStdVector<std::string>(),
            values.at("maxTorque")->getFloat()};
    }
 
    void
    NJointZeroTorqueController::rtPostDeactivateController()
    {
    }
 
    //::armarx::WidgetDescription::WidgetSeq NJointZeroTorqueController::createSliders(float Kp, float Ki, float Kd, float accelerationGain,
    //        float deadZone, float decay, float maxAcceleration, float maxJerk, float torqueToCurrent)
    //{
    //    using namespace armarx::WidgetDescription;
    //    ::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);
    //        }
    //    };
 
    //    addSlider("Kp", 0, 20, Kp);
    //    addSlider("Ki", 0, 10, Ki);
    //    addSlider("Kd", 0, 4, Kd);
    //    addSlider("accelerationGain", 0, 10, accelerationGain);
    //    addSlider("deadZone", 0, 2, deadZone);
    //    addSlider("decay", 0, 0.05, decay);
    //    addSlider("maxAcceleration", 0, 40, maxAcceleration);
    //    addSlider("maxJerk", 0, 400, maxJerk);
    //    addSlider("torqueToCurrent", 0, 2, torqueToCurrent);
    //    return widgets;
    //}
 
    //void NJointZeroTorqueController::onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx& dd)
    //{
    //    StringVariantBaseMap map;
    //    map["currentTarget"] = new Variant((float)currentTarget * 0.001);
    //    map["velocityTarget"] = new Variant((float)velocityTarget);
    //    map["currentPIDError"] = new Variant((float)currentPIDError);
    //    map["currentPIDIntegral"] = new Variant((float)currentPIDIntegral);
    //    map["currentAcceleration"] = new Variant((float)currentAcceleration);
    //    map["currentJerk"] = new Variant((float)currentJerk);
    //    dd->setDebugChannel("NJointZeroTorqueController", map);
    //}
 
    //WidgetDescription::StringWidgetDictionary NJointZeroTorqueController::getFunctionDescriptions(const Ice::Current&) const
    //{
    //    using namespace armarx::WidgetDescription;
 
 
    //    HBoxLayoutPtr configLayout = new HBoxLayout;
    //    auto sliders = createSliders(Kp, Ki, Kd, accelerationGain, deadZone, decay, maxAcceleration, maxJerk, torqueToCurrent);
    //    configLayout->children.insert(configLayout->children.end(),
    //                                  sliders.begin(),
    //                                  sliders.end());
 
 
    //    HBoxLayoutPtr targetsLayout = 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);
    //        }
    //    };
 
    //    addSlider("targetTorque", -5, 5, 0);
    //    targetsLayout->children.insert(targetsLayout->children.end(),
    //                                   widgets.begin(),
    //                                   widgets.end());
    //    return {{"ControllerConfig", configLayout},
    //        {"ControllerTarget", targetsLayout}
    //    };
    //}
 
    //void NJointZeroTorqueController::callDescribedFunction(const std::string& name, const StringVariantBaseMap& valueMap, const Ice::Current&)
    //{
    //    if (name == "ControllerConfig")
    //    {
    //        Kp = valueMap.at("Kp")->getFloat();
    //        ARMARX_INFO << VAROUT(Kp);
    //        Ki = valueMap.at("Ki")->getFloat();
    //        Kd = valueMap.at("Kd")->getFloat();
    //        accelerationGain = valueMap.at("accelerationGain")->getFloat();
    //        deadZone = valueMap.at("deadZone")->getFloat();
    //        decay = valueMap.at("decay")->getFloat();
    //        maxAcceleration = valueMap.at("maxAcceleration")->getFloat();
    //        maxJerk = valueMap.at("maxJerk")->getFloat();
    //        torqueToCurrent = valueMap.at("torqueToCurrent")->getFloat();
    //        std::stringstream s;
    //        for (auto& p : valueMap)
    //        {
    //            auto var = VariantPtr::dynamicCast(p.second);
    //            s << p.first << ": " << (var ? var->getOutputValueOnly() : "NULL") << "\n";
    //        }
    //        ARMARX_INFO << "Setting new config: " << s.str();
    //    }
    //    else if (name == "ControllerTarget")
    //    {
    //        targetTorque = valueMap.at("targetTorque")->getFloat();
    //    }
    //    else
    //    {
    //        ARMARX_WARNING << "Unknown function name called: " << name;
    //    }
    //}
 
    void
    NJointZeroTorqueController::setControllerTarget(const Ice::FloatSeq& targets,
                                                    const Ice::Current&)
    {
        LockGuardType guard{controlDataMutex};
        this->getWriterControlStruct().targetTorques = targets;
        writeControlStruct();
    }
} // namespace armarx::control::njoint_controller::joint_space