CartesianNaturalPositionControllerProxy.cpp

Go to the documentation of this file.

/*
 * This file is part of ArmarX.
 *
 * Copyright (C) 2012-2016, 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/>.
 *
 * @author     armar-user (armar-user at kit dot edu)
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "CartesianNaturalPositionControllerProxy.h"
 
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/logging/Logging.h>
 
#include <RobotAPI/libraries/core/CartesianPositionController.h>
//#include <RobotAPI/libraries/aron/core/navigator/Navigator.h>
 
#include <VirtualRobot/MathTools.h>
#include <VirtualRobot/Nodes/RobotNode.h>
#include <VirtualRobot/RobotNodeSet.h>
#include <VirtualRobot/math/Helpers.h>
 
using namespace armarx;
 
CartesianNaturalPositionControllerProxy::CartesianNaturalPositionControllerProxy(
    const NaturalIK& ik,
    const NaturalIK::ArmJoints& arm,
    const RobotUnitInterfacePrx& robotUnit,
    const std::string& controllerName,
    NJointCartesianNaturalPositionControllerConfigPtr config) :
    _ik(ik), _robotUnit(robotUnit), _controllerName(controllerName), _config(config), _arm(arm)
{
    _runCfg = _config->runCfg;
    _velocityBaseSettings.basePosVel = _runCfg.maxTcpPosVel;
    _velocityBaseSettings.baseOriVel = _runCfg.maxTcpOriVel;
    _velocityBaseSettings.baseJointVelocity =
        CalculateMaxJointVelocity(arm.rns, _runCfg.maxTcpPosVel);
    _userNullspaceTargets = std::vector<float>(arm.rns->getSize(), std::nanf(""));
    _naturalNullspaceTargets = std::vector<float>(arm.rns->getSize(), std::nanf(""));
    updateBaseKpValues(_runCfg);
    VirtualRobot::RobotNodeSetPtr rns = arm.rns;
    for (size_t i = 0; i < rns->getSize(); i++)
    {
        VirtualRobot::RobotNodePtr rn = rns->getNode(i);
        if (rn == arm.elbow)
        {
            break;
        }
        _rnsToElb.emplace_back(rn);
    }
 
    {
        WaypointConfig wpc;
        wpc.referencePosVel = 80;
        wpc.thresholdTcpPosReached = 5;
        _defaultWaypointConfigs["default"] = wpc;
    }
    _ftOffset.force = Eigen::Vector3f::Zero();
    _ftOffset.torque = Eigen::Vector3f::Zero();
}
 
NJointCartesianNaturalPositionControllerConfigPtr
CartesianNaturalPositionControllerProxy::MakeConfig(const std::string& rns,
                                                    const std::string& elbowNode)
{
    NJointCartesianNaturalPositionControllerConfigPtr cfg =
        new NJointCartesianNaturalPositionControllerConfig();
    cfg->rns = rns;
    cfg->elbowNode = elbowNode;
    return cfg;
}
 
void
CartesianNaturalPositionControllerProxy::init()
{
    NJointControllerInterfacePrx ctrl = _robotUnit->getNJointController(_controllerName);
    if (ctrl)
    {
        _controllerCreated = false;
        _controller = NJointCartesianNaturalPositionControllerInterfacePrx::checkedCast(ctrl);
        _controller->setConfig(_runCfg);
    }
    else
    {
        ctrl = _robotUnit->createNJointController(
            "NJointCartesianNaturalPositionController", _controllerName, _config);
        _controller = NJointCartesianNaturalPositionControllerInterfacePrx::checkedCast(ctrl);
        _controllerCreated = true;
    }
    if (_activateControllerOnInit)
    {
        _controller->activateController();
    }
}
 
bool
CartesianNaturalPositionControllerProxy::setTarget(const Eigen::Matrix4f& tcpTarget,
                                                   NaturalDiffIK::Mode setOri,
                                                   std::optional<float> minElbowHeight)
{
    ScopedJointValueRestore jvr(_arm.rns);
    _tcpTarget = tcpTarget;
    _setOri = setOri;
    _fwd = _ik.solveSoechtingIK(math::Helpers::Position(tcpTarget), minElbowHeight);
    //VirtualRobot::IKSolver::CartesianSelection mode = setOri ? VirtualRobot::IKSolver::All : VirtualRobot::IKSolver::Position;
    NaturalDiffIK::Result ikResult = NaturalDiffIK::CalculateDiffIK(
        tcpTarget, _fwd.elbow, _arm.rns, _arm.tcp, _arm.elbow, setOri, _natikParams.diffIKparams);
    if (!ikResult.reached)
    {
        ARMARX_ERROR << "could not solve natural IK for target: " << tcpTarget;
        return false;
    }
    _elbTarget = _arm.elbow->getPoseInRootFrame();
    _controller->setTarget(
        _tcpTarget, _arm.elbow->getPositionInRootFrame(), setOri == NaturalDiffIK::Mode::All);
    if (_setJointNullspaceFromNaturalIK)
    {
        for (size_t i = 0; i < _rnsToElb.size(); i++)
        {
            _naturalNullspaceTargets.at(i) = ikResult.jointValues(i);
        }
    }
    updateNullspaceTargets();
    updateDynamicKp();
    return true;
}
 
void
CartesianNaturalPositionControllerProxy::setNullspaceTarget(
    const std::vector<float>& nullspaceTargets)
{
    ARMARX_CHECK(_arm.rns->getSize() == nullspaceTargets.size());
    _userNullspaceTargets = nullspaceTargets;
    updateNullspaceTargets();
}
 
Eigen::Vector3f
CartesianNaturalPositionControllerProxy::getCurrentTargetPosition()
{
    return math::Helpers::Position(_tcpTarget);
}
 
Eigen::Vector3f
CartesianNaturalPositionControllerProxy::getCurrentElbowTargetPosition()
{
    return math::Helpers::Position(_elbTarget);
}
 
float
CartesianNaturalPositionControllerProxy::getTcpPositionError()
{
    return CartesianPositionController::GetPositionError(_tcpTarget, _arm.tcp);
}
 
float
CartesianNaturalPositionControllerProxy::getTcpOrientationError()
{
    if (_setOri == NaturalDiffIK::Mode::Position)
    {
        return 0;
    }
    return CartesianPositionController::GetOrientationError(_tcpTarget, _arm.tcp);
}
 
float
CartesianNaturalPositionControllerProxy::getElbPositionError()
{
    return CartesianPositionController::GetPositionError(_elbTarget, _arm.elbow);
}
 
bool
CartesianNaturalPositionControllerProxy::isFinalWaypointReached()
{
    return isLastWaypoint() && currentWaypoint().reached(_arm.tcp);
}
 
void
CartesianNaturalPositionControllerProxy::useCurrentFTasOffset()
{
    _ftOffset = _controller->getAverageFTValue();
    _controller->setFTOffset(_ftOffset);
}
 
void
CartesianNaturalPositionControllerProxy::enableFTLimit(float force,
                                                       float torque,
                                                       bool useCurrentFTasOffset)
{
    if (useCurrentFTasOffset)
    {
        this->useCurrentFTasOffset();
    }
    _controller->setFTLimit(force, torque);
}
 
void
CartesianNaturalPositionControllerProxy::disableFTLimit()
{
    _controller->clearFTLimit();
    //_controller->resetFTOffset();
}
 
FTSensorValue
CartesianNaturalPositionControllerProxy::getCurrentFTValue(bool substactOffset)
{
    FTSensorValue ft = _controller->getCurrentFTValue();
    if (substactOffset)
    {
        ft.force = ft.force - _ftOffset.force;
        ft.torque = ft.torque - _ftOffset.torque;
    }
    return ft;
}
 
armarx::FTSensorValue
armarx::CartesianNaturalPositionControllerProxy::getAverageFTValue(bool substactOffset)
{
    FTSensorValue ft = _controller->getAverageFTValue();
    if (substactOffset)
    {
        ft.force = ft.force - _ftOffset.force;
        ft.torque = ft.torque - _ftOffset.torque;
    }
    return ft;
}
 
void
CartesianNaturalPositionControllerProxy::stopClear()
{
    _controller->stopMovement();
    _controller->clearFTLimit();
    _controller->setNullspaceControlEnabled(true);
    clearWaypoints();
}
 
void
CartesianNaturalPositionControllerProxy::updateDynamicKp()
{
    if (_dynamicKp.enabled)
    {
        float error = (math::Helpers::Position(_tcpTarget) - _fwd.wrist).norm();
        float KpElb, KpJla;
        ARMARX_IMPORTANT << VAROUT(error);
        _dynamicKp.calculate(error, KpElb, KpJla);
        //ARMARX_IMPORTANT << VAROUT()
        _runCfg.elbowKp = KpElb;
        _runCfg.jointLimitAvoidanceKp = KpJla;
        _controller->setConfig(_runCfg);
        ARMARX_IMPORTANT << VAROUT(_runCfg.elbowKp) << VAROUT(_runCfg.jointLimitAvoidanceKp);
    }
}
 
void
CartesianNaturalPositionControllerProxy::updateNullspaceTargets()
{
    std::vector<float> nsTargets = _userNullspaceTargets;
    for (size_t i = 0; i < _naturalNullspaceTargets.size(); i++)
    {
        if (std::isnan(nsTargets.at(i)))
        {
            nsTargets.at(i) = _naturalNullspaceTargets.at(i);
        }
    }
    _controller->setNullspaceTarget(nsTargets);
}
 
void
CartesianNaturalPositionControllerProxy::DynamicKp::calculate(float error,
                                                              float& KpElb,
                                                              float& KpJla)
{
    float f = std::exp(-0.5f * (error * error) / (sigmaMM * sigmaMM));
    KpElb = f * maxKp;
    KpJla = (1 - f) * maxKp;
}
 
void
CartesianNaturalPositionControllerProxy::setRuntimeConfig(
    CartesianNaturalPositionControllerConfig runCfg)
{
    _controller->setConfig(runCfg);
    this->_runCfg = runCfg;
}
 
CartesianNaturalPositionControllerConfig
CartesianNaturalPositionControllerProxy::getRuntimeConfig()
{
    return _runCfg;
}
 
void
CartesianNaturalPositionControllerProxy::addWaypoint(
    const CartesianNaturalPositionControllerProxy::Waypoint& waypoint)
{
    if (_waypoints.size() == 0)
    {
        _waypointChanged = true;
    }
    _waypoints.emplace_back(waypoint);
}
 
CartesianNaturalPositionControllerProxy::Waypoint
CartesianNaturalPositionControllerProxy::createWaypoint(
    const Eigen::Vector3f& tcpTarget,
    const std::vector<float>& userNullspaceTargets,
    std::optional<float> minElbowHeight)
{
    return createWaypoint(tcpTarget, minElbowHeight).setNullspaceTargets(userNullspaceTargets);
}
 
CartesianNaturalPositionControllerProxy::Waypoint
CartesianNaturalPositionControllerProxy::createWaypoint(const Eigen::Vector3f& tcpTarget,
                                                        std::optional<float> minElbowHeight)
{
    Waypoint w;
    w.config = _defaultWaypointConfigs["default"];
    w.targets.tcpTarget = math::Helpers::CreatePose(tcpTarget, Eigen::Matrix3f::Identity());
    w.targets.setOri = NaturalDiffIK::Mode::Position;
    w.targets.minElbowHeight = minElbowHeight;
    w.targets.userNullspaceTargets = std::vector<float>(_arm.rns->getSize(), std::nanf(""));
    return w;
}
 
CartesianNaturalPositionControllerProxy::Waypoint
CartesianNaturalPositionControllerProxy::createWaypoint(const Eigen::Matrix4f& tcpTarget,
                                                        std::optional<float> minElbowHeight)
{
    Waypoint w;
    w.config = _defaultWaypointConfigs["default"];
    w.targets.tcpTarget = tcpTarget;
    w.targets.setOri = NaturalDiffIK::Mode::All;
    w.targets.minElbowHeight = minElbowHeight;
    w.targets.userNullspaceTargets = std::vector<float>(_arm.rns->getSize(), std::nanf(""));
    return w;
}
 
void
CartesianNaturalPositionControllerProxy::clearWaypoints()
{
    _waypoints.clear();
    _currentWaypointIndex = 0;
}
 
void
CartesianNaturalPositionControllerProxy::setDefaultWaypointConfig(
    const CartesianNaturalPositionControllerProxy::WaypointConfig& config)
{
    _defaultWaypointConfigs["default"] = config;
}
 
std::string
CartesianNaturalPositionControllerProxy::getStatusText()
{
    std::stringstream ss;
    ss.precision(2);
    ss << std::fixed << "Waypoint: " << (_currentWaypointIndex + 1) << "/" << _waypoints.size()
       << " distance: " << getTcpPositionError() << " mm "
       << VirtualRobot::MathTools::rad2deg(getTcpOrientationError()) << " deg";
    return ss.str();
}
 
std::vector<float>
CartesianNaturalPositionControllerProxy::CalculateMaxJointVelocity(
    const VirtualRobot::RobotNodeSetPtr& rns,
    float maxPosVel)
{
    size_t len = rns->getSize();
    std::vector<Eigen::Vector3f> positions;
    for (size_t i = 0; i < len; i++)
    {
        positions.push_back(rns->getNode(i)->getPositionInRootFrame());
    }
    positions.push_back(rns->getTCP()->getPositionInRootFrame());
 
    std::vector<float> dists;
    for (size_t i = 0; i < len; i++)
    {
        dists.push_back((positions.at(i) - positions.at(i + 1)).norm());
    }
 
    std::vector<float> result(len, 0);
    float dist = 0;
    for (int i = len - 1; i >= 0; i--)
    {
        dist += dists.at(i);
        result.at(i) = maxPosVel / dist;
    }
    return result;
}
 
std::vector<float>
CartesianNaturalPositionControllerProxy::ScaleVec(const std::vector<float>& vec, float scale)
{
    std::vector<float> result(vec.size(), 0);
    for (size_t i = 0; i < vec.size(); i++)
    {
        result.at(i) = vec.at(i) * scale;
    }
    return result;
}
 
void
CartesianNaturalPositionControllerProxy::setActivateControllerOnInit(bool value)
{
    _activateControllerOnInit = value;
}
 
void
CartesianNaturalPositionControllerProxy::setMaxVelocities(float referencePosVel)
{
    VelocityBaseSettings& v = _velocityBaseSettings;
    KpBaseSettings& k = _kpBaseSettings;
    float scale = referencePosVel / v.basePosVel;
    _runCfg.maxTcpPosVel = v.basePosVel * v.scaleTcpPosVel * scale;
    _runCfg.maxTcpOriVel = v.baseOriVel * v.scaleTcpOriVel * scale;
    _runCfg.maxElbPosVel = v.basePosVel * v.scaleElbPosVel * scale;
    _runCfg.maxJointVelocity = ScaleVec(v.baseJointVelocity, v.scaleJointVelocities * scale);
    _runCfg.maxNullspaceVelocity =
        ScaleVec(v.baseJointVelocity, v.scaleNullspaceVelocities * scale);
    _runCfg.KpPos = k.baseKpTcpPos; // * scale;
    _runCfg.KpOri = k.baseKpTcpOri; // * scale;
    _runCfg.elbowKp = k.baseKpElbPos; // * scale;
    _runCfg.jointLimitAvoidanceKp = k.baseKpJla; // * scale;
    _runCfg.nullspaceTargetKp = k.baseKpNs; // * scale;
    _runCfg.maxNullspaceAcceleration = k.maxNullspaceAcceleration; // * scale;
    _runCfg.maxPositionAcceleration = k.maxPositionAcceleration; // * scale;
    _runCfg.maxOrientationAcceleration = k.maxOrientationAcceleration; // * scale;
 
    _controller->setConfig(_runCfg);
}
 
void
CartesianNaturalPositionControllerProxy::updateBaseKpValues(
    const CartesianNaturalPositionControllerConfig& runCfg)
{
    _kpBaseSettings.baseKpTcpPos = _runCfg.KpPos;
    _kpBaseSettings.baseKpTcpOri = _runCfg.KpOri;
    _kpBaseSettings.baseKpElbPos = _runCfg.elbowKp;
    _kpBaseSettings.baseKpJla = _runCfg.jointLimitAvoidanceKp;
    _kpBaseSettings.baseKpNs = _runCfg.nullspaceTargetKp;
    _kpBaseSettings.maxNullspaceAcceleration = _runCfg.maxNullspaceAcceleration;
    _kpBaseSettings.maxPositionAcceleration = _runCfg.maxPositionAcceleration;
    _kpBaseSettings.maxOrientationAcceleration = _runCfg.maxOrientationAcceleration;
}
 
void
CartesianNaturalPositionControllerProxy::cleanup()
{
    if (_controllerCreated)
    {
        // delete controller only if it was created
        _controller->deactivateAndDeleteController();
    }
    else
    {
        // if the controller existed, only deactivate it
        _controller->deactivateController();
    }
    _controllerCreated = false;
}
 
bool
CartesianNaturalPositionControllerProxy::update()
{
    if (_waypoints.size() == 0)
    {
        return true;
    }
 
    Waypoint& w = currentWaypoint();
    if (!isLastWaypoint() && w.reached(_arm.tcp))
    {
        _currentWaypointIndex++;
        _waypointChanged = true;
    }
    if (_waypointChanged)
    {
        _waypointChanged = false;
        ARMARX_IMPORTANT << "Waypoint changed. Setting new target: ";
        return onWaypointChanged();
    }
    return true;
}
 
bool
CartesianNaturalPositionControllerProxy::onWaypointChanged()
{
    Waypoint& w = currentWaypoint();
    setMaxVelocities(w.config.referencePosVel);
    _userNullspaceTargets = w.targets.userNullspaceTargets;
    ARMARX_IMPORTANT << "Waypoint target position: "
                     << math::Helpers::GetPosition(w.targets.tcpTarget).transpose();
    return setTarget(w.targets.tcpTarget, w.targets.setOri, w.targets.minElbowHeight);
}
 
CartesianNaturalPositionControllerProxy::Waypoint&
CartesianNaturalPositionControllerProxy::currentWaypoint()
{
    ARMARX_CHECK(_waypoints.size() > 0);
    return _waypoints.at(_currentWaypointIndex);
}
 
bool
CartesianNaturalPositionControllerProxy::isLastWaypoint()
{
    return _waypoints.size() == 0 || _currentWaypointIndex == _waypoints.size() - 1;
}
 
NJointCartesianNaturalPositionControllerInterfacePrx
CartesianNaturalPositionControllerProxy::getInternalController()
{
    return _controller;
}
 
void
CartesianNaturalPositionControllerProxy::setDynamicKp(DynamicKp dynamicKp)
{
    _dynamicKp = dynamicKp;
    updateDynamicKp();
}
 
CartesianNaturalPositionControllerProxy::DynamicKp
CartesianNaturalPositionControllerProxy::getDynamicKp()
{
    return _dynamicKp;
}
 
bool
CartesianNaturalPositionControllerProxy::Waypoint::reached(const VirtualRobot::RobotNodePtr& tcp)
{
    return CartesianPositionController::Reached(targets.tcpTarget,
                                                tcp,
                                                targets.setOri == NaturalDiffIK::Mode::All,
                                                config.thresholdTcpPosReached,
                                                config.thresholdTcpPosReached /
                                                    config.rad2mmFactor);
}
 
CartesianNaturalPositionControllerProxy::Waypoint&
CartesianNaturalPositionControllerProxy::Waypoint::setConfig(
    const CartesianNaturalPositionControllerProxy::WaypointConfig& config)
{
    this->config = config;
    return *this;
}
 
CartesianNaturalPositionControllerProxy::Waypoint&
CartesianNaturalPositionControllerProxy::Waypoint::setNullspaceTargets(
    const std::vector<float>& userNullspaceTargets)
{
    ARMARX_CHECK(this->targets.userNullspaceTargets.size() == userNullspaceTargets.size());
    this->targets.userNullspaceTargets = userNullspaceTargets;
    return *this;
}
 
/*
aron::AronObjectPtr CartesianNaturalPositionControllerProxy::Waypoint::toAron()
{
    aron::AronObjectPtr obj = new aron::AronObject();
    aron::Navigator nav(obj);
    nav.appendMatrix4f("tcpTarget", tcpTarget);
    nav.appendVector3f("elbTarget", elbTarget);
    nav.appendVec("userNullspaceTargets", userNullspaceTargets);
    nav.appendVec("naturalNullspaceTargets", naturalNullspaceTargets);
    nav.appendBool("setOri", setOri == NaturalDiffIK::Mode::All);
    nav.appendFloat("referencePosVel", referencePosVel);
    nav.appendFloat("thresholdTcpPosReached", thresholdTcpPosReached);
    nav.appendFloat("thresholdTcpOriReached", thresholdTcpOriReached);
    nav.appendBool("resetFTonEnter", resetFTonEnter);
    nav.appendBool("resetFTonExit", resetFTonExit);
    nav.appendFloat("forceTheshold", forceTheshold);
    return obj;
}
 
void CartesianNaturalPositionControllerProxy::Waypoint::fromAron(aron::AronObjectPtr aron)
{
    aron::Navigator nav(aron);
    tcpTarget = nav.atKey("tcpTarget").asMatrix4f();
    elbTarget = nav.atKey("elbTarget").asVector3f();
    userNullspaceTargets = nav.atKey("userNullspaceTargets").asVecFloat();
    naturalNullspaceTargets = nav.atKey("naturalNullspaceTargets").asVecFloat();
    setOri = nav.atKey("setOri").asBool() ? NaturalDiffIK::Mode::All : NaturalDiffIK::Mode::Position;
    referencePosVel = nav.atKey("referencePosVel").asFloat();
    thresholdTcpPosReached = nav.atKey("thresholdTcpPosReached").asFloat();
    thresholdTcpOriReached = nav.atKey("thresholdTcpOriReached").asFloat();
    resetFTonEnter = nav.atKey("resetFTonEnter").asBool();
    resetFTonExit = nav.atKey("resetFTonExit").asBool();
    forceTheshold = nav.atKey("forceTheshold").asFloat();
 
}*/
 
CartesianNaturalPositionControllerProxy::ScopedJointValueRestore::ScopedJointValueRestore(
    const VirtualRobot::RobotNodeSetPtr& rns) :
    jointValues(rns->getJointValues()), rns(rns)
{
}
 
CartesianNaturalPositionControllerProxy::ScopedJointValueRestore::~ScopedJointValueRestore()
{
    //ARMARX_IMPORTANT << "restoring joint values";
    rns->setJointValues(jointValues);
}