NJointCartesianVelocityController.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     Mirko Waechter( mirko.waechter at kit dot edu)
 * @date       2017
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "NJointCartesianVelocityController.h"
 
#include <VirtualRobot/IK/DifferentialIK.h>
#include <VirtualRobot/Nodes/RobotNode.h>
#include <VirtualRobot/Robot.h>
#include <VirtualRobot/RobotNodeSet.h>
 
#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h>
#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerRegistry.h>
#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValue1DoFActuator.h>
 
#define DEFAULT_TCP_STRING "default TCP"
 
namespace armarx
{
 
    NJointControllerRegistration<NJointCartesianVelocityController>
        registrationControllerNJointCartesianVelocityController(
            "NJointCartesianVelocityController");
 
    std::string
    NJointCartesianVelocityController::getClassName(const Ice::Current&) const
    {
        return "NJointCartesianVelocityController";
    }
 
    NJointCartesianVelocityController::NJointCartesianVelocityController(
        RobotUnit* robotUnit,
        const NJointCartesianVelocityControllerConfigPtr& config,
        const VirtualRobot::RobotPtr& r)
    {
        useSynchronizedRtRobot();
        ARMARX_CHECK_EXPRESSION(!config->nodeSetName.empty());
        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 SensorValue1DoFActuatorTorque* torqueSensor =
                sv->asA<SensorValue1DoFActuatorTorque>();
            const SensorValue1DoFGravityTorque* gravityTorqueSensor =
                sv->asA<SensorValue1DoFGravityTorque>();
            if (!torqueSensor)
            {
                ARMARX_WARNING << "No Torque sensor available for " << jointName;
            }
            if (!gravityTorqueSensor)
            {
                ARMARX_WARNING << "No Gravity Torque sensor available for " << jointName;
            }
            torqueSensors.push_back(torqueSensor);
            gravityTorqueSensors.push_back(gravityTorqueSensor);
        };
 
        VirtualRobot::RobotNodePtr tcp =
            (config->tcpName.empty() || config->tcpName == DEFAULT_TCP_STRING)
                ? rns->getTCP()
                : rtGetRobot()->getRobotNode(config->tcpName);
        ARMARX_CHECK_EXPRESSION(tcp) << config->tcpName;
        tcpController.reset(new CartesianVelocityController(rns, tcp));
 
        nodeSetName = config->nodeSetName;
 
        torquePIDs.resize(tcpController->rns->getSize(), SimplePID());
 
        NJointCartesianVelocityControllerControlData initData;
        initData.nullspaceJointVelocities.resize(tcpController->rns->getSize(), 0);
        initData.torqueKp.resize(tcpController->rns->getSize(), 0);
        initData.torqueKd.resize(tcpController->rns->getSize(), 0);
        initData.mode = ModeFromIce(config->mode);
        reinitTripleBuffer(initData);
    }
 
    void
    NJointCartesianVelocityController::rtPreActivateController()
    {
    }
 
    void
    NJointCartesianVelocityController::rtRun(const IceUtil::Time& sensorValuesTimestamp,
                                             const IceUtil::Time& timeSinceLastIteration)
    {
        auto mode = rtGetControlStruct().mode;
 
        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 = Eigen::VectorXf::Zero(tcpController->rns->getSize());
        float jointLimitAvoidanceKp = rtGetControlStruct().avoidJointLimitsKp;
        if (jointLimitAvoidanceKp > 0)
        {
            jnv += jointLimitAvoidanceKp * tcpController->calculateJointLimitAvoidance();
        }
        for (size_t i = 0; i < tcpController->rns->getSize(); i++)
        {
            jnv(i) += rtGetControlStruct().nullspaceJointVelocities.at(i);
            if (torqueSensors.at(i) && gravityTorqueSensors.at(i) &&
                rtGetControlStruct().torqueKp.at(i) != 0)
            {
                torquePIDs.at(i).Kp = rtGetControlStruct().torqueKp.at(i);
                torquePIDs.at(i).Kd = rtGetControlStruct().torqueKd.at(i);
                jnv(i) += torquePIDs.at(i).update(timeSinceLastIteration.toSecondsDouble(),
                                                  torqueSensors.at(i)->torque -
                                                      gravityTorqueSensors.at(i)->gravityTorque);
                //jnv(i) += rtGetControlStruct().torqueKp.at(i) * (torqueSensors.at(i)->torque - gravityTorqueSensors.at(i)->gravityTorque);
            }
            else
            {
                torquePIDs.at(i).lastError = 0;
            }
        }
 
        Eigen::VectorXf jointTargetVelocities = tcpController->calculate(x, jnv, mode);
        for (size_t i = 0; i < targets.size(); ++i)
        {
            targets.at(i)->velocity = jointTargetVelocities(i);
        }
    }
 
    ::armarx::WidgetDescription::WidgetSeq
    NJointCartesianVelocityController::createSliders()
    {
        using namespace armarx::WidgetDescription;
        ::armarx::WidgetDescription::WidgetSeq widgets;
        auto addSlider = [&](const std::string& label, float limit)
        {
            widgets.emplace_back(new Label(false, label));
            {
                FloatSliderPtr c_x = new FloatSlider;
                c_x->name = label;
                c_x->min = -limit;
                c_x->defaultValue = 0.0f;
                c_x->max = limit;
                widgets.emplace_back(c_x);
            }
        };
 
        addSlider("x", 100);
        addSlider("y", 100);
        addSlider("z", 100);
        addSlider("roll", 0.5);
        addSlider("pitch", 0.5);
        addSlider("yaw", 0.5);
        addSlider("avoidJointLimitsKp", 1);
        return widgets;
    }
 
    WidgetDescription::HBoxLayoutPtr
    NJointCartesianVelocityController::createJointSlidersLayout(float min,
                                                                float max,
                                                                float defaultValue) const
    {
        using namespace armarx::WidgetDescription;
        HBoxLayoutPtr layout = new HBoxLayout;
        auto addSlider = [&](const std::string& label)
        {
            layout->children.emplace_back(new Label(false, label));
            FloatSliderPtr floatSlider = new FloatSlider;
            floatSlider->name = label;
            floatSlider->min = min;
            floatSlider->defaultValue = defaultValue;
            floatSlider->max = max;
            layout->children.emplace_back(floatSlider);
        };
 
        for (const VirtualRobot::RobotNodePtr& rn : tcpController->rns->getAllRobotNodes())
        {
            addSlider(rn->getName());
        }
 
        return layout;
    }
 
    WidgetDescription::WidgetPtr
    NJointCartesianVelocityController::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->defaultIndex = 0;
 
        boxMode->options = {"Position", "Orientation", "Both"};
        boxMode->name = "mode";
        layout->children.emplace_back(boxMode);
 
 
        //        auto sliders = createSliders();
        //        layout->children.insert(layout->children.end(),
        //                                sliders.begin(),
        //                                sliders.end());
        layout->children.emplace_back(new HSpacer);
        return layout;
    }
 
    NJointCartesianVelocityControllerConfigPtr
    NJointCartesianVelocityController::GenerateConfigFromVariants(
        const StringVariantBaseMap& values)
    {
        return new NJointCartesianVelocityControllerConfig(
            values.at("nodeSetName")->getString(),
            values.at("tcpName")->getString(),
            IceModeFromString(values.at("mode")->getString()));
    }
 
    VirtualRobot::IKSolver::CartesianSelection
    NJointCartesianVelocityController::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;
    }
 
    NJointCartesianVelocityControllerMode::CartesianSelection
    NJointCartesianVelocityController::IceModeFromString(const std::string mode)
    {
        if (mode == "Position")
        {
            return NJointCartesianVelocityControllerMode::CartesianSelection::ePosition;
        }
        if (mode == "Orientation")
        {
            return NJointCartesianVelocityControllerMode::CartesianSelection::eOrientation;
        }
        if (mode == "Both")
        {
            return NJointCartesianVelocityControllerMode::CartesianSelection::eAll;
        }
        ARMARX_ERROR_S << "invalid mode " << mode;
        return (NJointCartesianVelocityControllerMode::CartesianSelection)0;
    }
 
    VirtualRobot::IKSolver::CartesianSelection
    NJointCartesianVelocityController::ModeFromIce(
        const NJointCartesianVelocityControllerMode::CartesianSelection mode)
    {
        if (mode == NJointCartesianVelocityControllerMode::CartesianSelection::ePosition)
        {
            return VirtualRobot::IKSolver::CartesianSelection::Position;
        }
        if (mode == NJointCartesianVelocityControllerMode::CartesianSelection::eOrientation)
        {
            return VirtualRobot::IKSolver::CartesianSelection::Orientation;
        }
        if (mode == NJointCartesianVelocityControllerMode::CartesianSelection::eAll)
        {
            return VirtualRobot::IKSolver::CartesianSelection::All;
        }
        ARMARX_ERROR_S << "invalid mode " << mode;
        return (VirtualRobot::IKSolver::CartesianSelection)0;
    }
 
    void
    NJointCartesianVelocityController::setVelocities(
        float xVel,
        float yVel,
        float zVel,
        float rollVel,
        float pitchVel,
        float yawVel,
        VirtualRobot::IKSolver::CartesianSelection mode)
    {
        LockGuardType guard{controlDataMutex};
        getWriterControlStruct().xVel = xVel;
        getWriterControlStruct().yVel = yVel;
        getWriterControlStruct().zVel = zVel;
        getWriterControlStruct().rollVel = rollVel;
        getWriterControlStruct().pitchVel = pitchVel;
        getWriterControlStruct().yawVel = yawVel;
        getWriterControlStruct().mode = mode;
        writeControlStruct();
    }
 
    void
    NJointCartesianVelocityController::setAvoidJointLimitsKp(float kp)
    {
        LockGuardType guard{controlDataMutex};
        getWriterControlStruct().avoidJointLimitsKp = kp;
        writeControlStruct();
    }
 
    std::string
    NJointCartesianVelocityController::getNodeSetName() const
    {
        return nodeSetName;
    }
 
    void
    NJointCartesianVelocityController::rtPostDeactivateController()
    {
    }
 
    WidgetDescription::StringWidgetDictionary
    NJointCartesianVelocityController::getFunctionDescriptions(const Ice::Current&) const
    {
        using namespace armarx::WidgetDescription;
        HBoxLayoutPtr layout = new HBoxLayout;
        auto sliders = createSliders();
        layout->children.insert(layout->children.end(), sliders.begin(), sliders.end());
 
        return {{"ControllerTarget", layout},
                {"TorqueKp", createJointSlidersLayout(-1, 1, 0)},
                {"TorqueKd", createJointSlidersLayout(-1, 1, 0)},
                {"NullspaceJointVelocities", createJointSlidersLayout(-1, 1, 0)}};
    }
 
    void
    NJointCartesianVelocityController::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();
            getWriterControlStruct().avoidJointLimitsKp =
                valueMap.at("avoidJointLimitsKp")->getFloat();
            writeControlStruct();
        }
        else if (name == "TorqueKp")
        {
            LockGuardType guard{controlDataMutex};
            for (size_t i = 0; i < tcpController->rns->getSize(); i++)
            {
                getWriterControlStruct().torqueKp.at(i) =
                    valueMap.at(tcpController->rns->getNode(i)->getName())->getFloat();
            }
            writeControlStruct();
        }
        else if (name == "TorqueKd")
        {
            LockGuardType guard{controlDataMutex};
            for (size_t i = 0; i < tcpController->rns->getSize(); i++)
            {
                getWriterControlStruct().torqueKd.at(i) =
                    valueMap.at(tcpController->rns->getNode(i)->getName())->getFloat();
            }
            writeControlStruct();
        }
        else if (name == "NullspaceJointVelocities")
        {
            LockGuardType guard{controlDataMutex};
            for (size_t i = 0; i < tcpController->rns->getSize(); i++)
            {
                getWriterControlStruct().nullspaceJointVelocities.at(i) =
                    valueMap.at(tcpController->rns->getNode(i)->getName())->getFloat();
            }
            writeControlStruct();
        }
        else
        {
            ARMARX_WARNING << "Unknown function name called: " << name;
        }
    }
 
    float
    SimplePID::update(float dt, float error)
    {
        float derivative = (error - lastError) / dt;
        float retVal = Kp * error + Kd * derivative;
        lastError = error;
        return retVal;
    }
 
} // namespace armarx
 
void
armarx::NJointCartesianVelocityController::setControllerTarget(
    float x,
    float y,
    float z,
    float roll,
    float pitch,
    float yaw,
    float avoidJointLimitsKp,
    NJointCartesianVelocityControllerMode::CartesianSelection mode,
    const Ice::Current&)
{
    LockGuardType guard{controlDataMutex};
    getWriterControlStruct().xVel = x;
    getWriterControlStruct().yVel = y;
    getWriterControlStruct().zVel = z;
    getWriterControlStruct().rollVel = roll;
    getWriterControlStruct().pitchVel = pitch;
    getWriterControlStruct().yawVel = yaw;
    getWriterControlStruct().avoidJointLimitsKp = avoidJointLimitsKp;
    getWriterControlStruct().mode = ModeFromIce(mode);
    writeControlStruct();
}
 
void
armarx::NJointCartesianVelocityController::setTorqueKp(const StringFloatDictionary& torqueKp,
                                                       const Ice::Current&)
{
    LockGuardType guard{controlDataMutex};
    for (size_t i = 0; i < tcpController->rns->getSize(); i++)
    {
        getWriterControlStruct().torqueKp.at(i) =
            torqueKp.at(tcpController->rns->getNode(i)->getName());
    }
    writeControlStruct();
}
 
void
armarx::NJointCartesianVelocityController::setNullspaceJointVelocities(
    const StringFloatDictionary& nullspaceJointVelocities,
    const Ice::Current&)
{
    LockGuardType guard{controlDataMutex};
    for (size_t i = 0; i < tcpController->rns->getSize(); i++)
    {
        getWriterControlStruct().nullspaceJointVelocities.at(i) =
            nullspaceJointVelocities.at(tcpController->rns->getNode(i)->getName());
    }
    writeControlStruct();
}