ZeroTorqueOrVelocity.cpp

Go to the documentation of this file.

#include "ZeroTorqueOrVelocity.h"
 
#include <VirtualRobot/Nodes/RobotNode.h>
#include <VirtualRobot/Robot.h>
 
#include <ArmarXCore/core/logging/Logging.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<NJointZeroTorqueOrVelocityController>
        registrationControllerNJointZeroTorqueOrVelocityController(
            "NJointZeroTorqueOrVelocityController");
 
    std::string
    NJointZeroTorqueOrVelocityController::getClassName(const Ice::Current&) const
    {
        return "NJointZeroTorqueOrVelocityController";
    }
 
    NJointZeroTorqueOrVelocityController::NJointZeroTorqueOrVelocityController(
        RobotUnitPtr prov,
        const NJointZeroTorqueOrVelocityControllerConfigPtr& 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(
            not(config->jointNamesZeroTorque.empty() and config->jointNamesZeroVelocity.empty()));
        //        ARMARX_CHECK_EXPRESSION(!config->jointNamesZeroVelocity.empty());
 
        for (auto& jointName : config->jointNamesZeroTorque)
        {
            if (std::find(config->jointNamesZeroVelocity.begin(),
                          config->jointNamesZeroVelocity.end(),
                          jointName) != config->jointNamesZeroVelocity.end())
            {
                ARMARX_ERROR << jointName << " exists in both torque and velocity controlled mode";
            }
            auto node = r->getRobotNode(jointName);
            ARMARX_CHECK_EXPRESSION(node) << jointName;
 
            ControlTargetBase* ct = useControlTarget(jointName, ControlModes::ZeroTorque1DoF);
            ARMARX_CHECK_NOT_NULL(ct);
            targetsTorque.push_back(ct->asA<ControlTarget1DoFActuatorZeroTorque>());
            torqueControlled.push_back(true);
        }
        for (auto& jointName : config->jointNamesZeroVelocity)
        {
            auto node = r->getRobotNode(jointName);
            ARMARX_CHECK_EXPRESSION(node) << jointName;
 
            ControlTargetBase* ct = useControlTarget(jointName, ControlModes::ZeroVelocity1DoF);
            ARMARX_CHECK_NOT_NULL(ct);
            targetsVel.push_back(ct->asA<ControlTarget1DoFActuatorZeroVelocity>());
            torqueControlled.push_back(false);
        }
        size_t numJoints =
            config->jointNamesZeroTorque.size() + config->jointNamesZeroVelocity.size();
        ARMARX_CHECK_EQUAL(numJoints, torqueControlled.size());
 
        maxTorque = config->maxTorque;
        maxVelocity = config->maxVelocity;
 
        NJointZeroTorqueOrVelocityControllerTarget target;
        target.targetTorqueOrVelocity = Ice::FloatSeq(numJoints, 0.0f);
        this->reinitTripleBuffer(target);
    }
 
    void
    NJointZeroTorqueOrVelocityController::rtPreActivateController()
    {
 
        //    targetTorque = 0.0;
    }
 
    void
    NJointZeroTorqueOrVelocityController::rtRun(const IceUtil::Time& sensorValuesTimestamp,
                                                const IceUtil::Time& timeSinceLastIteration)
    {
        size_t torqueIndex = 0;
        size_t velIndex = 0;
        for (size_t i = 0; i < torqueControlled.size(); ++i)
        {
            if (torqueControlled.at(i))
            {
                targetsTorque.at(torqueIndex)->torque = math::MathUtils::LimitTo(
                    rtGetControlStruct().targetTorqueOrVelocity.at(i), maxTorque);
                torqueIndex++;
            }
            else
            {
                targetsVel.at(velIndex)->velocity = math::MathUtils::LimitTo(
                    rtGetControlStruct().targetTorqueOrVelocity.at(i), maxVelocity);
                velIndex++;
            }
        }
    }
 
    WidgetDescription::WidgetPtr
    NJointZeroTorqueOrVelocityController::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);
            }
        };
 
        /// joint names
        LabelPtr label = new Label;
        label->text = "zero torque joints";
        layout->children.emplace_back(label);
 
        StringComboBoxPtr box = new StringComboBox;
        box->name = "jointNamesZeroTorque";
        box->defaultIndex = 0;
        box->multiSelect = true;
 
        for (auto& c : controlDevices)
        {
            if (c.second->hasJointController(ControlModes::ZeroTorque1DoF))
            {
                box->options.push_back(c.first);
            }
        }
        layout->children.emplace_back(box);
 
 
        /// joint control mode
        LabelPtr labelControlMode = new Label;
        labelControlMode->text = "zero velocity joints";
        layout->children.emplace_back(labelControlMode);
 
        StringComboBoxPtr boxControlMode = new StringComboBox;
        boxControlMode->name = "jointNamesZeroVelocity";
        boxControlMode->defaultIndex = 0;
        boxControlMode->multiSelect = true;
 
        for (auto& c : controlDevices)
        {
            if (c.second->hasJointController(ControlModes::ZeroVelocity1DoF))
            {
                boxControlMode->options.push_back(c.first);
            }
        }
        layout->children.emplace_back(boxControlMode);
 
 
        addSlider("maxTorque", 0, 100, 10);
        addSlider("maxVeloicty", 0, 3.14, 2.0);
 
        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;
    }
 
    NJointZeroTorqueOrVelocityControllerConfigPtr
    NJointZeroTorqueOrVelocityController::GenerateConfigFromVariants(
        const StringVariantBaseMap& values)
    {
        auto varZeroTorque = VariantPtr::dynamicCast(values.at("jointNamesZeroTorque"));
        ARMARX_CHECK_EXPRESSION(varZeroTorque) << "jointNamesZeroTorque";
        auto varZeroVelocity = VariantPtr::dynamicCast(values.at("jointNamesZeroVelocity"));
        ARMARX_CHECK_EXPRESSION(varZeroVelocity) << "jointNamesZeroVelocity";
        for (auto& pair : values)
        {
            std::cout << pair.first;
        }
 
        ARMARX_TRACE;
        return new NJointZeroTorqueOrVelocityControllerConfig{
            varZeroTorque->get<SingleTypeVariantList>()->toStdVector<std::string>(),
            varZeroVelocity->get<SingleTypeVariantList>()->toStdVector<std::string>(),
            values.at("maxTorque")->getFloat(),
            values.at("maxVeloicty")->getFloat()};
    }
 
    void
    NJointZeroTorqueOrVelocityController::rtPostDeactivateController()
    {
    }
 
    //::armarx::WidgetDescription::WidgetSeq NJointZeroTorqueOrVelocityController::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 NJointZeroTorqueOrVelocityController::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("NJointZeroTorqueOrVelocityController", map);
    //}
 
    //WidgetDescription::StringWidgetDictionary NJointZeroTorqueOrVelocityController::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 NJointZeroTorqueOrVelocityController::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
    NJointZeroTorqueOrVelocityController::setControllerTarget(const Ice::FloatSeq& targets,
                                                              const Ice::Current&)
    {
        LockGuardType guard{controlDataMutex};
        this->getWriterControlStruct().targetTorqueOrVelocity = targets;
        writeControlStruct();
    }
} // namespace armarx::control::njoint_controller::joint_space