NJointTaskSpaceImpedanceController.cpp

Go to the documentation of this file.

/*
 * This file is part of ArmarX.
 *
 * 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    TaskSpaceActiveImpedanceControl::ArmarXObjects::NJointTaskSpaceImpedanceController
 * @author     zhou ( you dot zhou at kit dot edu )
 * @date       2018
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "NJointTaskSpaceImpedanceController.h"
 
#include <SimoxUtility/math/convert/mat4f_to_pos.h>
#include <SimoxUtility/math/convert/mat4f_to_quat.h>
#include <VirtualRobot/IK/DifferentialIK.h>
#include <VirtualRobot/Robot.h>
#include <VirtualRobot/RobotNodeSet.h>
 
#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerRegistry.h>
 
using namespace armarx;
 
NJointControllerRegistration<NJointTaskSpaceImpedanceController>
    registrationControllerDSRTController("NJointTaskSpaceImpedanceController");
 
NJointTaskSpaceImpedanceController::NJointTaskSpaceImpedanceController(
    const RobotUnitPtr& robotUnit,
    const NJointControllerConfigPtr& config,
    const VirtualRobot::RobotPtr&)
{
 
    NJointTaskSpaceImpedanceControlConfigPtr cfg =
        NJointTaskSpaceImpedanceControlConfigPtr::dynamicCast(config);
 
    ARMARX_CHECK_EXPRESSION(cfg);
    ARMARX_CHECK_EXPRESSION(robotUnit);
    ARMARX_CHECK_EXPRESSION(!cfg->nodeSetName.empty());
    useSynchronizedRtRobot();
    VirtualRobot::RobotNodeSetPtr rns = rtGetRobot()->getRobotNodeSet(cfg->nodeSetName);
 
    jointNames.clear();
    ARMARX_CHECK_EXPRESSION(rns) << cfg->nodeSetName;
    for (size_t i = 0; i < rns->getSize(); ++i)
    {
        std::string jointName = rns->getNode(i)->getName();
        jointNames.push_back(jointName);
        ControlTargetBase* ct = useControlTarget(jointName, ControlModes::Torque1DoF);
        ARMARX_CHECK_EXPRESSION(ct) << jointName;
        const SensorValueBase* sv = useSensorValue(jointName);
        ARMARX_CHECK_EXPRESSION(sv) << jointName;
        auto casted_ct = ct->asA<ControlTarget1DoFActuatorTorque>();
        ARMARX_CHECK_EXPRESSION(casted_ct) << jointName;
        targets.push_back(casted_ct);
 
        const SensorValue1DoFActuatorTorque* torqueSensor =
            sv->asA<SensorValue1DoFActuatorTorque>();
        const SensorValue1DoFActuatorVelocity* velocitySensor =
            sv->asA<SensorValue1DoFActuatorVelocity>();
        const SensorValue1DoFActuatorPosition* positionSensor =
            sv->asA<SensorValue1DoFActuatorPosition>();
        if (!torqueSensor)
        {
            ARMARX_WARNING << "No Torque sensor available for " << jointName;
        }
 
 
        torqueSensors.push_back(torqueSensor);
        velocitySensors.push_back(velocitySensor);
        positionSensors.push_back(positionSensor);
    };
 
    controller.initialize(rns);
 
    CartesianImpedanceController::Config initData;
    initData.desiredOrientation = cfg->desiredOrientation;
    initData.desiredPosition = cfg->desiredPosition;
 
    initData.Kpos = cfg->Kpos;
    initData.Dpos = cfg->Dpos;
    initData.Kori = cfg->Kori;
    initData.Dori = cfg->Dori;
 
    initData.Knull = cfg->Knull;
    initData.Dnull = cfg->Dnull;
    initData.desiredJointPosition = cfg->desiredJointPositions;
 
    initData.torqueLimit = cfg->torqueLimit;
    reinitTripleBuffer(initData);
}
 
void
NJointTaskSpaceImpedanceController::rtRun(const IceUtil::Time& /*sensorValuesTimestamp*/,
                                          const IceUtil::Time& /*timeSinceLastIteration*/)
{
    rtUpdateControlStruct();
    const auto& jointDesiredTorques =
        controller.run(rtGetControlStruct(), torqueSensors, velocitySensors, positionSensors);
    ARMARX_CHECK_EQUAL(targets.size(), static_cast<size_t>(jointDesiredTorques.size()));
 
    for (size_t i = 0; i < targets.size(); ++i)
    {
        targets.at(i)->torque = jointDesiredTorques(i);
        if (!targets.at(i)->isValid())
        {
            targets.at(i)->torque = 0;
        }
    }
 
    debugDataInfo.getWriteBuffer().desiredForce_x = controller.dbg.tcpDesiredForce(0);
    debugDataInfo.getWriteBuffer().desiredForce_y = controller.dbg.tcpDesiredForce(1);
    debugDataInfo.getWriteBuffer().desiredForce_z = controller.dbg.tcpDesiredForce(2);
    debugDataInfo.getWriteBuffer().tcpDesiredTorque_x = controller.dbg.tcpDesiredTorque(0);
    debugDataInfo.getWriteBuffer().tcpDesiredTorque_y = controller.dbg.tcpDesiredTorque(1);
    debugDataInfo.getWriteBuffer().tcpDesiredTorque_z = controller.dbg.tcpDesiredTorque(2);
    debugDataInfo.getWriteBuffer().quatError = controller.dbg.errorAngle;
    debugDataInfo.getWriteBuffer().posiError = controller.dbg.posiError;
    debugDataInfo.commitWrite();
}
 
void
NJointTaskSpaceImpedanceController::onPublish(const SensorAndControl&,
                                              const DebugDrawerInterfacePrx&,
                                              const DebugObserverInterfacePrx& debugObs)
{
    StringVariantBaseMap datafields;
    auto values = debugDataInfo.getUpToDateReadBuffer().desired_torques;
    for (auto& pair : values)
    {
        datafields[pair.first] = new Variant(pair.second);
    }
 
    datafields["desiredForce_x"] =
        new Variant(debugDataInfo.getUpToDateReadBuffer().desiredForce_x);
    datafields["desiredForce_y"] =
        new Variant(debugDataInfo.getUpToDateReadBuffer().desiredForce_y);
    datafields["desiredForce_z"] =
        new Variant(debugDataInfo.getUpToDateReadBuffer().desiredForce_z);
 
    datafields["tcpDesiredTorque_x"] =
        new Variant(debugDataInfo.getUpToDateReadBuffer().tcpDesiredTorque_x);
    datafields["tcpDesiredTorque_y"] =
        new Variant(debugDataInfo.getUpToDateReadBuffer().tcpDesiredTorque_y);
    datafields["tcpDesiredTorque_z"] =
        new Variant(debugDataInfo.getUpToDateReadBuffer().tcpDesiredTorque_z);
 
    datafields["quatError"] = new Variant(debugDataInfo.getUpToDateReadBuffer().quatError);
    datafields["posiError"] = new Variant(debugDataInfo.getUpToDateReadBuffer().posiError);
 
    debugObs->setDebugChannel("DSControllerOutput", datafields);
}
 
void
NJointTaskSpaceImpedanceController::setPosition(const Eigen::Vector3f& target, const Ice::Current&)
{
    LockGuardType guard{controlDataMutex};
    getWriterControlStruct().desiredPosition = target;
    writeControlStruct();
}
 
void
NJointTaskSpaceImpedanceController::setOrientation(const Eigen::Quaternionf& target,
                                                   const Ice::Current&)
{
    LockGuardType guard{controlDataMutex};
    getWriterControlStruct().desiredOrientation = target;
    writeControlStruct();
}
 
void
NJointTaskSpaceImpedanceController::setPositionOrientation(const Eigen::Vector3f& pos,
                                                           const Eigen::Quaternionf& ori,
                                                           const Ice::Current&)
{
    LockGuardType guard{controlDataMutex};
    getWriterControlStruct().desiredPosition = pos;
    getWriterControlStruct().desiredOrientation = ori;
    writeControlStruct();
}
 
void
NJointTaskSpaceImpedanceController::setPose(const Eigen::Matrix4f& mat, const Ice::Current&)
{
    LockGuardType guard{controlDataMutex};
    getWriterControlStruct().desiredPosition = simox::math::mat4f_to_pos(mat);
    getWriterControlStruct().desiredOrientation = simox::math::mat4f_to_quat(mat);
    writeControlStruct();
}
 
void
NJointTaskSpaceImpedanceController::setImpedanceParameters(const std::string& name,
                                                           const Ice::FloatSeq& vals,
                                                           const Ice::Current&)
{
    LockGuardType guard{controlDataMutex};
    if (name == "Kpos")
    {
        ARMARX_CHECK_EQUAL(vals.size(), 3);
 
        Eigen::Vector3f vec;
        for (size_t i = 0; i < 3; ++i)
        {
            vec(i) = vals.at(i);
        }
        getWriterControlStruct().Kpos = vec;
    }
    else if (name == "Kori")
    {
        ARMARX_CHECK_EQUAL(vals.size(), 3);
 
        Eigen::Vector3f vec;
        for (size_t i = 0; i < 3; ++i)
        {
            vec(i) = vals.at(i);
        }
        getWriterControlStruct().Kori = vec;
    }
    else if (name == "Dpos")
    {
        ARMARX_CHECK_EQUAL(vals.size(), 3);
 
        Eigen::Vector3f vec;
        for (size_t i = 0; i < 3; ++i)
        {
            vec(i) = vals.at(i);
        }
        getWriterControlStruct().Dpos = vec;
    }
    else if (name == "Dori")
    {
        ARMARX_CHECK_EQUAL(vals.size(), 3);
 
        Eigen::Vector3f vec;
        for (size_t i = 0; i < 3; ++i)
        {
            vec(i) = vals.at(i);
        }
        getWriterControlStruct().Dori = vec;
    }
    else if (name == "Knull")
    {
        ARMARX_CHECK_EQUAL(vals.size(), 8);
 
        Eigen::VectorXf vec;
        vec.setZero(8);
        for (size_t i = 0; i < 8; ++i)
        {
            vec(i) = vals.at(i);
        }
        getWriterControlStruct().Knull = vec;
    }
    else if (name == "Dnull")
    {
        ARMARX_CHECK_EQUAL(vals.size(), 8);
 
        Eigen::VectorXf vec;
        vec.setZero(8);
        for (size_t i = 0; i < 8; ++i)
        {
            vec(i) = vals.at(i);
        }
        getWriterControlStruct().Dnull = vec;
    }
    writeControlStruct();
}
 
void
NJointTaskSpaceImpedanceController::setNullspaceConfig(const Eigen::VectorXf& joint,
                                                       const Eigen::VectorXf& knull,
                                                       const Eigen::VectorXf& dnull,
                                                       const Ice::Current&)
{
    LockGuardType guard{controlDataMutex};
    ARMARX_CHECK_EQUAL(static_cast<std::size_t>(joint.size()), targets.size());
    ARMARX_CHECK_EQUAL(static_cast<std::size_t>(knull.size()), targets.size());
    ARMARX_CHECK_EQUAL(static_cast<std::size_t>(dnull.size()), targets.size());
    getWriterControlStruct().Knull = knull;
    getWriterControlStruct().Dnull = dnull;
    getWriterControlStruct().desiredJointPosition = joint;
    writeControlStruct();
}
 
void
NJointTaskSpaceImpedanceController::setConfig(
    const NJointTaskSpaceImpedanceControlRuntimeConfig& cfg,
    const Ice::Current&)
{
    LockGuardType guard{controlDataMutex};
    ARMARX_CHECK_EQUAL(static_cast<std::size_t>(cfg.Knull.size()), targets.size());
    ARMARX_CHECK_EQUAL(static_cast<std::size_t>(cfg.Dnull.size()), targets.size());
    ARMARX_CHECK_EQUAL(static_cast<std::size_t>(cfg.desiredJointPositions.size()), targets.size());
 
    getWriterControlStruct().Kpos = cfg.Kpos;
    getWriterControlStruct().Dpos = cfg.Dpos;
    getWriterControlStruct().Kori = cfg.Kori;
    getWriterControlStruct().Dori = cfg.Dori;
 
    getWriterControlStruct().Knull = cfg.Knull;
    getWriterControlStruct().Dnull = cfg.Dnull;
    getWriterControlStruct().desiredJointPosition = cfg.desiredJointPositions;
 
    getWriterControlStruct().torqueLimit = cfg.torqueLimit;
    writeControlStruct();
}
 
void
armarx::NJointTaskSpaceImpedanceController::rtPreActivateController()
{
}