NJointCartesianVelocityControllerWithRamp.cpp

Go to the documentation of this file.

/*
 * This file is part of ArmarX.
 *
 * Copyright (C) 2011-2017, High Performance Humanoid Technologies (H2T), Karlsruhe Institute of Technology (KIT), all rights reserved.
 *
 * 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    ArmarX
 * @author     Sonja Marahrens( sonja.marahrens at kit dot edu)
 * @date       2018
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
#include "NJointCartesianVelocityControllerWithRamp.h"
 
#include <VirtualRobot/IK/DifferentialIK.h>
#include <VirtualRobot/Robot.h>
#include <VirtualRobot/RobotNodeSet.h>
 
#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerRegistry.h>
 
#include "../RobotUnit.h"
 
#define DEFAULT_TCP_STRING "default TCP"
 
namespace armarx
{
 
    NJointControllerRegistration<NJointCartesianVelocityControllerWithRamp>
        registrationControllerNJointCartesianVelocityControllerWithRamp(
            "NJointCartesianVelocityControllerWithRamp");
 
    std::string
    NJointCartesianVelocityControllerWithRamp::getClassName(const Ice::Current&) const
    {
        return "NJointCartesianVelocityControllerWithRamp";
    }
 
    NJointCartesianVelocityControllerWithRamp::NJointCartesianVelocityControllerWithRamp(
        RobotUnit* robotUnit,
        const NJointCartesianVelocityControllerWithRampConfigPtr& config,
        const VirtualRobot::RobotPtr& r)
    {
        ARMARX_CHECK_EXPRESSION(robotUnit);
        ARMARX_CHECK_EXPRESSION(!config->nodeSetName.empty());
        useSynchronizedRtRobot();
        VirtualRobot::RobotNodeSetPtr rns = rtGetRobot()->getRobotNodeSet(config->nodeSetName);
        ARMARX_CHECK_EXPRESSION(rns) << config->nodeSetName;
        for (size_t i = 0; i < rns->getSize(); ++i)
        {
            std::string jointName = rns->getNode(i)->getName();
            ControlTargetBase* ct = useControlTarget(jointName, ControlModes::Velocity1DoF);
            const SensorValueBase* sv = useSensorValue(jointName);
            targets.push_back(ct->asA<ControlTarget1DoFActuatorVelocity>());
 
            const SensorValue1DoFActuatorFilteredVelocity* filteredVelocitySensor =
                sv->asA<SensorValue1DoFActuatorFilteredVelocity>();
            const SensorValue1DoFActuatorVelocity* velocitySensor =
                sv->asA<SensorValue1DoFActuatorVelocity>();
            ARMARX_CHECK_EXPRESSION(filteredVelocitySensor || velocitySensor);
            if (filteredVelocitySensor)
            {
                ARMARX_IMPORTANT << "Using filtered velocity for joint " << jointName;
                velocitySensors.push_back(&filteredVelocitySensor->filteredvelocity);
            }
            else if (velocitySensor)
            {
                ARMARX_IMPORTANT << "Using raw velocity for joint " << jointName;
                velocitySensors.push_back(&velocitySensor->velocity);
            }
        };
 
        VirtualRobot::RobotNodePtr tcp =
            (config->tcpName.empty() || config->tcpName == DEFAULT_TCP_STRING)
                ? rns->getTCP()
                : rtGetRobot()->getRobotNode(config->tcpName);
        ARMARX_CHECK_EXPRESSION(tcp) << config->tcpName;
 
        nodeSetName = config->nodeSetName;
        jointLimitAvoidanceScale = config->jointLimitAvoidanceScale;
        KpJointLimitAvoidance = config->KpJointLimitAvoidance;
        mode = ModeFromIce(config->mode);
 
        controller.reset(
            new CartesianVelocityControllerWithRamp(rns,
                                                    Eigen::VectorXf::Zero(rns->getSize()),
                                                    mode,
                                                    config->maxPositionAcceleration,
                                                    config->maxOrientationAcceleration,
                                                    config->maxNullspaceAcceleration));
    }
 
    void
    NJointCartesianVelocityControllerWithRamp::rtPreActivateController()
    {
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wdeprecated-declarations"
        Eigen::VectorXf currentJointVelocities(velocitySensors.size());
        for (size_t i = 0; i < velocitySensors.size(); i++)
        {
            currentJointVelocities(i) = *velocitySensors.at(i);
        }
        controller->setCurrentJointVelocity(currentJointVelocities);
#pragma GCC diagnostic pop
        ARMARX_IMPORTANT << "initial joint velocities: " << currentJointVelocities.transpose();
    }
 
    void
    NJointCartesianVelocityControllerWithRamp::rtRun(const IceUtil::Time& sensorValuesTimestamp,
                                                     const IceUtil::Time& timeSinceLastIteration)
    {
        Eigen::VectorXf x;
        if (mode == VirtualRobot::IKSolver::All)
        {
            x.resize(6);
            x << rtGetControlStruct().xVel, rtGetControlStruct().yVel, rtGetControlStruct().zVel,
                rtGetControlStruct().rollVel, rtGetControlStruct().pitchVel,
                rtGetControlStruct().yawVel;
        }
        else if (mode == VirtualRobot::IKSolver::Position)
        {
            x.resize(3);
            x << rtGetControlStruct().xVel, rtGetControlStruct().yVel, rtGetControlStruct().zVel;
        }
        else if (mode == VirtualRobot::IKSolver::Orientation)
        {
            x.resize(3);
            x << rtGetControlStruct().rollVel, rtGetControlStruct().pitchVel,
                rtGetControlStruct().yawVel;
        }
        else
        {
            // No mode has been set
            return;
        }
 
        Eigen::VectorXf jnv =
            KpJointLimitAvoidance * controller->controller.calculateJointLimitAvoidance();
        jnv += controller->controller.calculateNullspaceVelocity(x, jointLimitAvoidanceScale, mode);
        Eigen::VectorXf jointTargetVelocities =
            controller->calculate(x, jnv, timeSinceLastIteration.toSecondsDouble());
        for (size_t i = 0; i < targets.size(); ++i)
        {
            targets.at(i)->velocity = jointTargetVelocities(i);
        }
    }
 
    void
    NJointCartesianVelocityControllerWithRamp::setMaxAccelerations(float maxPositionAcceleration,
                                                                   float maxOrientationAcceleration,
                                                                   float maxNullspaceAcceleration,
                                                                   const Ice::Current&)
    {
        controller->setMaxAccelerations(
            maxPositionAcceleration, maxOrientationAcceleration, maxNullspaceAcceleration);
    }
 
    void
    NJointCartesianVelocityControllerWithRamp::setTargetVelocity(float vx,
                                                                 float vy,
                                                                 float vz,
                                                                 float vrx,
                                                                 float vry,
                                                                 float vrz,
                                                                 const Ice::Current&)
    {
        LockGuardType guard{controlDataMutex};
        getWriterControlStruct().xVel = vx;
        getWriterControlStruct().yVel = vy;
        getWriterControlStruct().zVel = vz;
        getWriterControlStruct().rollVel = vrx;
        getWriterControlStruct().pitchVel = vry;
        getWriterControlStruct().yawVel = vrz;
        writeControlStruct();
    }
 
    void
    NJointCartesianVelocityControllerWithRamp::setJointLimitAvoidanceScale(
        float jointLimitAvoidanceScale,
        const Ice::Current&)
    {
        this->jointLimitAvoidanceScale = jointLimitAvoidanceScale;
    }
 
    void
    NJointCartesianVelocityControllerWithRamp::setKpJointLimitAvoidance(float KpJointLimitAvoidance,
                                                                        const Ice::Current&)
    {
        this->KpJointLimitAvoidance = KpJointLimitAvoidance;
    }
 
    void
    NJointCartesianVelocityControllerWithRamp::immediateHardStop(const Ice::Current&)
    {
        LockGuardType guard{controlDataMutex};
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wdeprecated-declarations"
        controller->setCurrentJointVelocity(Eigen::VectorXf::Zero(velocitySensors.size()));
#pragma GCC diagnostic pop
        getWriterControlStruct().xVel = 0;
        getWriterControlStruct().yVel = 0;
        getWriterControlStruct().zVel = 0;
        getWriterControlStruct().rollVel = 0;
        getWriterControlStruct().pitchVel = 0;
        getWriterControlStruct().yawVel = 0;
        writeControlStruct();
    }
 
    void
    NJointCartesianVelocityControllerWithRamp::rtPostDeactivateController()
    {
    }
 
    const std::string&
    NJointCartesianVelocityControllerWithRamp::getNodeSetName() const
    {
        return nodeSetName;
    }
 
    WidgetDescription::StringWidgetDictionary
    NJointCartesianVelocityControllerWithRamp::getFunctionDescriptions(const Ice::Current&) const
    {
        return {{"ControllerTarget", createTargetLayout()},
                {"ControllerParameters", createParameterLayout()}};
    }
 
    void
    NJointCartesianVelocityControllerWithRamp::callDescribedFunction(
        const std::string& name,
        const StringVariantBaseMap& valueMap,
        const Ice::Current&)
    {
        if (name == "ControllerTarget")
        {
            LockGuardType guard{controlDataMutex};
            getWriterControlStruct().xVel = valueMap.at("x")->getFloat();
            getWriterControlStruct().yVel = valueMap.at("y")->getFloat();
            getWriterControlStruct().zVel = valueMap.at("z")->getFloat();
            getWriterControlStruct().rollVel = valueMap.at("roll")->getFloat();
            getWriterControlStruct().pitchVel = valueMap.at("pitch")->getFloat();
            getWriterControlStruct().yawVel = valueMap.at("yaw")->getFloat();
            KpJointLimitAvoidance = valueMap.at("KpJointLimitAvoidance")->getFloat();
            jointLimitAvoidanceScale = valueMap.at("jointLimitAvoidanceScale")->getFloat();
            writeControlStruct();
        }
        else if (name == "ControllerParameters")
        {
            LockGuardType guard{controlDataMutex};
            setMaxAccelerations(valueMap.at("maxPositionAcceleration")->getFloat(),
                                valueMap.at("maxOrientationAcceleration")->getFloat(),
                                valueMap.at("maxNullspaceAcceleration")->getFloat());
        }
        else
        {
            ARMARX_WARNING << "Unknown function name called: " << name;
        }
    }
 
    WidgetDescription::VBoxLayoutPtr
    NJointCartesianVelocityControllerWithRamp::createTargetLayout() const
    {
        LayoutBuilder layout;
        layout.addSlider("x", -100, 100, 0);
        layout.addSlider("y", -100, 100, 0);
        layout.addSlider("z", -100, 100, 0);
        layout.addSlider("roll", -0.5, 0.5, 0);
        layout.addSlider("pitch", -0.5, 0.5, 0);
        layout.addSlider("yaw", -0.5, 0.5, 0);
        layout.newLine();
        layout.addSlider("KpJointLimitAvoidance", -10, 10, KpJointLimitAvoidance);
        layout.addSlider("jointLimitAvoidanceScale", -10, 10, jointLimitAvoidanceScale);
        return layout.getLayout();
    }
 
    WidgetDescription::VBoxLayoutPtr
    NJointCartesianVelocityControllerWithRamp::createParameterLayout() const
    {
        LayoutBuilder layout;
        layout.addSlider(
            "maxPositionAcceleration", 0, 1000, controller->getMaxPositionAcceleration());
        layout.addSlider(
            "maxOrientationAcceleration", 0, 100, controller->getMaxOrientationAcceleration());
        layout.addSlider(
            "maxNullspaceAcceleration", 0, 100, controller->getMaxNullspaceAcceleration());
        return layout.getLayout();
    }
 
    WidgetDescription::WidgetPtr
    NJointCartesianVelocityControllerWithRamp::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;
 
        LabelPtr label = new Label;
        label->text = "nodeset name";
        layout->children.emplace_back(label);
        StringComboBoxPtr box = new StringComboBox;
        box->defaultIndex = 0;
 
        box->options = robot->getRobotNodeSetNames();
        box->name = "nodeSetName";
        layout->children.emplace_back(box);
 
        LabelPtr labelTCP = new Label;
        labelTCP->text = "tcp name";
        layout->children.emplace_back(labelTCP);
        StringComboBoxPtr boxTCP = new StringComboBox;
        boxTCP->defaultIndex = 0;
 
        boxTCP->options = robot->getRobotNodeNames();
        boxTCP->options.insert(boxTCP->options.begin(), DEFAULT_TCP_STRING);
        boxTCP->name = "tcpName";
        layout->children.emplace_back(boxTCP);
 
        LabelPtr labelMode = new Label;
        labelMode->text = "mode";
        layout->children.emplace_back(labelMode);
        StringComboBoxPtr boxMode = new StringComboBox;
 
        boxMode->options = {"Position", "Orientation", "Both"};
        boxMode->name = "mode";
        layout->children.emplace_back(boxMode);
        layout->children.emplace_back(new HSpacer);
        boxMode->defaultIndex = 2;
 
 
        auto addSlider = [&](const std::string& label, float max, float defaultValue)
        {
            layout->children.emplace_back(new Label(false, label));
            FloatSliderPtr floatSlider = new FloatSlider;
            floatSlider->name = label;
            floatSlider->min = 0;
            floatSlider->defaultValue = defaultValue;
            floatSlider->max = max;
            layout->children.emplace_back(floatSlider);
        };
        addSlider("maxPositionAcceleration", 1000, 100);
        addSlider("maxOrientationAcceleration", 10, 1);
        addSlider("maxNullspaceAcceleration", 10, 2);
        addSlider("KpJointLimitAvoidance", 10, 2);
        addSlider("jointLimitAvoidanceScale", 10, 2);
 
        return layout;
    }
 
    NJointCartesianVelocityControllerWithRampConfigPtr
    NJointCartesianVelocityControllerWithRamp::GenerateConfigFromVariants(
        const StringVariantBaseMap& values)
    {
        return new NJointCartesianVelocityControllerWithRampConfig(
            values.at("nodeSetName")->getString(),
            values.at("tcpName")->getString(),
            IceModeFromString(values.at("mode")->getString()),
            values.at("maxPositionAcceleration")->getFloat(),
            values.at("maxOrientationAcceleration")->getFloat(),
            values.at("maxNullspaceAcceleration")->getFloat(),
            values.at("KpJointLimitAvoidance")->getFloat(),
            values.at("jointLimitAvoidanceScale")->getFloat());
    }
 
    VirtualRobot::IKSolver::CartesianSelection
    NJointCartesianVelocityControllerWithRamp::ModeFromString(const std::string mode)
    {
        //ARMARX_IMPORTANT_S << "the mode is " << mode;
        if (mode == "Position")
        {
            return VirtualRobot::IKSolver::CartesianSelection::Position;
        }
        if (mode == "Orientation")
        {
            return VirtualRobot::IKSolver::CartesianSelection::Orientation;
        }
        if (mode == "Both")
        {
            return VirtualRobot::IKSolver::CartesianSelection::All;
        }
        ARMARX_ERROR_S << "invalid mode " << mode;
        return (VirtualRobot::IKSolver::CartesianSelection)0;
    }
 
    CartesianSelectionMode::CartesianSelection
    NJointCartesianVelocityControllerWithRamp::IceModeFromString(const std::string mode)
    {
        if (mode == "Position")
        {
            return CartesianSelectionMode::CartesianSelection::ePosition;
        }
        if (mode == "Orientation")
        {
            return CartesianSelectionMode::CartesianSelection::eOrientation;
        }
        if (mode == "Both")
        {
            return CartesianSelectionMode::CartesianSelection::eAll;
        }
        ARMARX_ERROR_S << "invalid mode " << mode;
        return (CartesianSelectionMode::CartesianSelection)0;
    }
 
    VirtualRobot::IKSolver::CartesianSelection
    NJointCartesianVelocityControllerWithRamp::ModeFromIce(
        const CartesianSelectionMode::CartesianSelection mode)
    {
        if (mode == CartesianSelectionMode::CartesianSelection::ePosition)
        {
            return VirtualRobot::IKSolver::CartesianSelection::Position;
        }
        if (mode == CartesianSelectionMode::CartesianSelection::eOrientation)
        {
            return VirtualRobot::IKSolver::CartesianSelection::Orientation;
        }
        if (mode == CartesianSelectionMode::CartesianSelection::eAll)
        {
            return VirtualRobot::IKSolver::CartesianSelection::All;
        }
        ARMARX_ERROR_S << "invalid mode " << mode;
        return (VirtualRobot::IKSolver::CartesianSelection)0;
    }
 
 
} // namespace armarx