NJointCartesianTorqueController.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 "NJointCartesianTorqueController.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<NJointCartesianTorqueController>
        registrationControllerNJointCartesianTorqueController("NJointCartesianTorqueController");
 
    std::string
    NJointCartesianTorqueController::getClassName(const Ice::Current&) const
    {
        return "NJointCartesianTorqueController";
    }
 
    NJointCartesianTorqueController::NJointCartesianTorqueController(
        RobotUnit*,
        NJointCartesianTorqueControllerConfigPtr config,
        const VirtualRobot::RobotPtr& r)
    {
        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::Torque1DoF);
            //const SensorValueBase* sv = useSensorValue(jointName);
            targets.push_back(ct->asA<ControlTarget1DoFActuatorTorque>());
 
            /*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);*/
        };
 
        tcp = (config->tcpName.empty() || config->tcpName == DEFAULT_TCP_STRING)
                  ? rns->getTCP()
                  : rtGetRobot()->getRobotNode(config->tcpName);
        ARMARX_CHECK_EXPRESSION(tcp) << config->tcpName;
 
        nodeSetName = config->nodeSetName;
 
        ik.reset(new VirtualRobot::DifferentialIK(
            rns, rns->getRobot()->getRootNode(), VirtualRobot::JacobiProvider::eSVDDamped));
 
 
        /*NJointCartesianTorqueControllerControlData initData;
        initData.nullspaceJointVelocities.resize(tcpController->rns->getSize(), 0);
        initData.torqueKp.resize(tcpController->rns->getSize(), 0);
        initData.mode = ModeFromIce(config->mode);
        reinitTripleBuffer(initData);*/
    }
 
    void
    NJointCartesianTorqueController::rtPreActivateController()
    {
    }
 
    void
    NJointCartesianTorqueController::rtRun(const IceUtil::Time& sensorValuesTimestamp,
                                           const IceUtil::Time& timeSinceLastIteration)
    {
        Eigen::VectorXf cartesianFT(6);
        cartesianFT << rtGetControlStruct().forceX / 1000, rtGetControlStruct().forceY / 1000,
            rtGetControlStruct().forceZ / 1000, rtGetControlStruct().torqueX,
            rtGetControlStruct().torqueY, rtGetControlStruct().torqueZ;
 
        Eigen::MatrixXf jacobi =
            ik->getJacobianMatrix(tcp, VirtualRobot::IKSolver::CartesianSelection::All);
        Eigen::MatrixXf jacobiT = jacobi.transpose();
 
 
        Eigen::VectorXf jointTargetTorques = jacobiT * cartesianFT;
 
        ARMARX_IMPORTANT << deactivateSpam(0.5) << VAROUT(jointTargetTorques.transpose());
 
        for (size_t i = 0; i < targets.size(); ++i)
        {
            targets.at(i)->torque = jointTargetTorques(i);
        }
    }
 
    WidgetDescription::HBoxLayoutPtr
    NJointCartesianTorqueController::createSliders()
    {
        using namespace armarx::WidgetDescription;
        HBoxLayoutPtr layout = new HBoxLayout;
        auto addSlider = [&](const std::string& label, float min, float max, float defaultValue)
        {
            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);
        };
 
        addSlider("forceX", -10, 10, 0);
        addSlider("forceY", -10, 10, 0);
        addSlider("forceZ", -10, 10, 0);
        addSlider("torqueX", -1, 1, 0);
        addSlider("torqueY", -1, 1, 0);
        addSlider("torqueZ", -1, 1, 0);
 
        return layout;
    }
 
    /*WidgetDescription::HBoxLayoutPtr NJointCartesianTorqueController::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
    NJointCartesianTorqueController::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);
 
 
        layout->children.emplace_back(new HSpacer);
        return layout;
    }
 
    NJointCartesianTorqueControllerConfigPtr
    NJointCartesianTorqueController::GenerateConfigFromVariants(const StringVariantBaseMap& values)
    {
        return new NJointCartesianTorqueControllerConfig(values.at("nodeSetName")->getString(),
                                                         values.at("tcpName")->getString());
    }
 
    std::string
    NJointCartesianTorqueController::getNodeSetName() const
    {
        return nodeSetName;
    }
 
    void
    NJointCartesianTorqueController::rtPostDeactivateController()
    {
    }
 
    WidgetDescription::StringWidgetDictionary
    NJointCartesianTorqueController::getFunctionDescriptions(const Ice::Current&) const
    {
        using namespace armarx::WidgetDescription;
        HBoxLayoutPtr layout = createSliders();
 
        return {{"ControllerTarget", layout}};
    }
 
    void
    NJointCartesianTorqueController::callDescribedFunction(const std::string& name,
                                                           const StringVariantBaseMap& valueMap,
                                                           const Ice::Current&)
    {
        if (name == "ControllerTarget")
        {
            LockGuardType guard{controlDataMutex};
            getWriterControlStruct().forceX = valueMap.at("forceX")->getFloat();
            getWriterControlStruct().forceY = valueMap.at("forceY")->getFloat();
            getWriterControlStruct().forceZ = valueMap.at("forceZ")->getFloat();
            getWriterControlStruct().torqueX = valueMap.at("torqueX")->getFloat();
            getWriterControlStruct().torqueY = valueMap.at("torqueY")->getFloat();
            getWriterControlStruct().torqueZ = valueMap.at("torqueZ")->getFloat();
            writeControlStruct();
        }
        else
        {
            ARMARX_WARNING << "Unknown function name called: " << name;
        }
    }
} // namespace armarx
 
void
armarx::NJointCartesianTorqueController::setControllerTarget(float forceX,
                                                             float forceY,
                                                             float forceZ,
                                                             float torqueX,
                                                             float torqueY,
                                                             float torqueZ,
                                                             const Ice::Current&)
{
    LockGuardType guard{controlDataMutex};
    getWriterControlStruct().forceX = forceX;
    getWriterControlStruct().forceY = forceY;
    getWriterControlStruct().forceZ = forceZ;
    getWriterControlStruct().torqueX = torqueX;
    getWriterControlStruct().torqueY = torqueY;
    getWriterControlStruct().torqueZ = torqueZ;
    writeControlStruct();
}