NJointTCPController.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 "NJointTCPController.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<NJointTCPController>
        registrationControllerNJointTCPController("NJointTCPController");
 
    std::string
    NJointTCPController::getClassName(const Ice::Current&) const
    {
        return "NJointTCPController";
    }
 
    void
    NJointTCPController::rtPreActivateController()
    {
        reinitTripleBuffer(NJointTCPControllerControlData());
    }
 
    void
    NJointTCPController::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::MatrixXf jacobiInv = ik->getPseudoInverseJacobianMatrix(tcp, mode);
        Eigen::VectorXf jointTargetVelocities = jacobiInv * x;
 
        for (size_t i = 0; i < targets.size(); ++i)
        {
            targets.at(i)->velocity = jointTargetVelocities(i);
        }
    }
 
    ::armarx::WidgetDescription::WidgetSeq
    NJointTCPController::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);
        return widgets;
    }
 
    WidgetDescription::WidgetPtr
    NJointTCPController::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;
    }
 
    NJointTCPControllerConfigPtr
    NJointTCPController::GenerateConfigFromVariants(const StringVariantBaseMap& values)
    {
        return new NJointTCPControllerConfig{values.at("nodeSetName")->getString(),
                                             values.at("tcpName")->getString()};
    }
 
    NJointTCPController::NJointTCPController(RobotUnit* robotUnit,
                                             const NJointTCPControllerConfigPtr& config,
                                             const VirtualRobot::RobotPtr& r)
    {
        ARMARX_CHECK_EXPRESSION(robotUnit);
        ARMARX_CHECK_EXPRESSION(!config->nodeSetName.empty());
        useSynchronizedRtRobot();
        auto nodeset = rtGetRobot()->getRobotNodeSet(config->nodeSetName);
        ARMARX_CHECK_EXPRESSION(nodeset) << config->nodeSetName;
        for (size_t i = 0; i < nodeset->getSize(); ++i)
        {
            auto jointName = nodeset->getNode(i)->getName();
            ControlTargetBase* ct = useControlTarget(jointName, ControlModes::Velocity1DoF);
            const SensorValueBase* sv = useSensorValue(jointName);
            targets.push_back(ct->asA<ControlTarget1DoFActuatorVelocity>());
            sensors.push_back(sv->asA<SensorValue1DoFActuatorPosition>());
        };
        ik.reset(new VirtualRobot::DifferentialIK(
            nodeset, rtGetRobot()->getRootNode(), VirtualRobot::JacobiProvider::eSVDDamped));
        tcp = (config->tcpName.empty() || config->tcpName == DEFAULT_TCP_STRING)
                  ? nodeset->getTCP()
                  : rtGetRobot()->getRobotNode(config->tcpName);
        ARMARX_CHECK_EXPRESSION(tcp) << config->tcpName;
 
        nodeSetName = config->nodeSetName;
    }
 
    void
    NJointTCPController::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();
    }
 
    std::string
    NJointTCPController::getNodeSetName() const
    {
        return nodeSetName;
    }
 
    void
    NJointTCPController::rtPostDeactivateController()
    {
    }
 
    WidgetDescription::StringWidgetDictionary
    NJointTCPController::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}};
    }
 
    void
    NJointTCPController::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();
            writeControlStruct();
        }
        else
        {
            ARMARX_WARNING << "Unknown function name called: " << name;
        }
    }
} // namespace armarx