CartesianNaturalPositionController.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     Raphael Grimm (raphael dot grimm at kit dot edu)
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
 
#include "CartesianNaturalPositionController.h"
 
#include <VirtualRobot/IK/DifferentialIK.h>
#include <VirtualRobot/MathTools.h>
#include <VirtualRobot/Nodes/RobotNode.h>
#include <VirtualRobot/math/Helpers.h>
 
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/time/TimeUtil.h>
 
#include <RobotAPI/libraries/core/CartesianVelocityController.h>
 
namespace armarx
{
    CartesianNaturalPositionController::CartesianNaturalPositionController(
        const VirtualRobot::RobotNodeSetPtr& rns,
        const VirtualRobot::RobotNodePtr& elbow,
        float maxPositionAcceleration,
        float maxOrientationAcceleration,
        float maxNullspaceAcceleration,
        const VirtualRobot::RobotNodePtr& tcp) :
        vcTcp(rns,
              Eigen::VectorXf::Constant(rns->getSize(), 0.f),
              VirtualRobot::IKSolver::CartesianSelection::All,
              maxPositionAcceleration,
              maxOrientationAcceleration,
              maxNullspaceAcceleration,
              tcp ? tcp : rns->getTCP()),
        pcTcp(tcp ? tcp : rns->getTCP()),
        vcElb(rns, elbow),
        pcElb(elbow),
        lastJointVelocity(Eigen::VectorXf::Constant(rns->getSize(), 0.f)),
        rns(rns)
    {
        ARMARX_CHECK_NOT_NULL(rns);
        clearNullspaceTarget();
    }
 
    Eigen::VectorXf
    CartesianNaturalPositionController::LimitInfNormTo(const Eigen::VectorXf& vec,
                                                       const std::vector<float>& maxValue)
    {
        if (maxValue.size() == 0)
        {
            return vec;
        }
        if (maxValue.size() != 1 && (int)maxValue.size() != vec.rows())
        {
            throw std::invalid_argument("maxValue.size != 1 and != maxValue.size");
        }
        float scale = 1;
        for (int i = 0; i < vec.rows(); i++)
        {
            int j = maxValue.size() == 1 ? 0 : i;
            if (std::abs(vec(i)) > maxValue.at(j) && maxValue.at(j) >= 0)
            {
                scale = std::min(scale, maxValue.at(j) / std::abs(vec(i)));
            }
        }
        return vec * scale;
    }
 
    const Eigen::VectorXf
    CartesianNaturalPositionController::calculateNullspaceTargetDifference()
    {
        Eigen::VectorXf jvNT = Eigen::VectorXf(rns->getSize());
        for (size_t i = 0; i < rns->getSize(); i++)
        {
            if (std::isnan(nullspaceTarget(i)))
            {
                jvNT(i) = 0;
            }
            else
            {
                float diff = nullspaceTarget(i) - rns->getNode(i)->getJointValue();
                if (rns->getNode(i)->isLimitless())
                {
                    diff = ::math::Helpers::AngleModPI(diff);
                }
                jvNT(i) = diff;
            }
        }
        return jvNT;
    }
 
    const Eigen::VectorXf
    CartesianNaturalPositionController::calculate(float dt,
                                                  VirtualRobot::IKSolver::CartesianSelection mode)
    {
 
        //int posLen = mode & VirtualRobot::IKSolver::Position ? 3 : 0;
        //int oriLen = mode & VirtualRobot::IKSolver::Orientation ? 3 : 0;
        //Eigen::Vector3f pdTcp = posLen ? pcTcp.getPositionDiff(tcpTarget) : Eigen::Vector3f::Zero();
        //Eigen::Vector3f odTcp = oriLen ? pcTcp.getOrientationDiff(tcpTarget) : Eigen::Vector3f::Zero();
        //Eigen::VectorXf cartesianVel(posLen + oriLen);
        //if (posLen)
        //{
        //    cartesianVel.block<3, 1>(0, 0) = pdTcp;
        //}
        //if (oriLen)
        //{
        //    cartesianVel.block<3, 1>(posLen, 0) = odTcp;
        //}
        Eigen::VectorXf cartesianVelTcp = pcTcp.calculate(tcpTarget, mode);
 
        //Eigen::Vector3f pdElb = pcElb.getPositionDiffVec3(elbowTarget);
        ////ARMARX_IMPORTANT << deactivateSpam(0.1) << VAROUT(pdElb) << VAROUT(elbowTarget) << VAROUT(pcElb.getTcp()->getPositionInRootFrame());
        //Eigen::VectorXf cartesianVelElb(3);
        //cartesianVelElb.block<3, 1>(0, 0) = pdElb;
 
        Eigen::VectorXf cartesianVelElb = pcElb.calculatePos(elbowTarget);
        Eigen::VectorXf jnvClamped = Eigen::VectorXf::Zero(rns->getSize());
        if (nullSpaceControlEnabled)
        {
            Eigen::VectorXf jnv = Eigen::VectorXf::Zero(rns->getSize());
            Eigen::VectorXf jvElb =
                vcElb.calculate(cartesianVelElb, VirtualRobot::IKSolver::Position);
 
            Eigen::VectorXf jvLA =
                jointLimitAvoidanceKp * vcTcp.controller.calculateJointLimitAvoidance();
            Eigen::VectorXf jvNT = nullspaceTargetKp * calculateNullspaceTargetDifference();
            //ARMARX_IMPORTANT << deactivateSpam(1) << VAROUT(jvNT);
 
            //ARMARX_IMPORTANT << VAROUT(jvElb);
            jnv = jvElb + jvLA + jvNT;
            jnvClamped = LimitInfNormTo(jnv, maxNullspaceVelocity);
        }
 
        //ARMARX_IMPORTANT << VAROUT(jnv);
        if (vcTcp.getMode() != mode)
        {
            vcTcp.switchMode(lastJointVelocity, mode);
        }
        Eigen::VectorXf jv = vcTcp.calculate(cartesianVelTcp, jnvClamped, dt);
        //ARMARX_IMPORTANT << VAROUT(jv);
 
        Eigen::VectorXf jvClamped = LimitInfNormTo(jv, maxJointVelocity);
 
        if (autoClearFeedForward)
        {
            clearFeedForwardVelocity();
        }
 
        lastJointVelocity = jvClamped;
 
        return jvClamped;
    }
 
    void
    CartesianNaturalPositionController::setTarget(const Eigen::Matrix4f& tcpTarget,
                                                  const Eigen::Vector3f& elbowTarget)
    {
        this->tcpTarget = tcpTarget;
        this->elbowTarget = elbowTarget;
    }
 
    void
    CartesianNaturalPositionController::setFeedForwardVelocity(
        const Eigen::Vector6f& feedForwardVelocity)
    {
        this->feedForwardVelocity = feedForwardVelocity;
    }
 
    void
    CartesianNaturalPositionController::setFeedForwardVelocity(
        const Eigen::Vector3f& feedForwardVelocityPos,
        const Eigen::Vector3f& feedForwardVelocityOri)
    {
        feedForwardVelocity.block<3, 1>(0, 0) = feedForwardVelocityPos;
        feedForwardVelocity.block<3, 1>(3, 0) = feedForwardVelocityOri;
    }
 
    void
    CartesianNaturalPositionController::setNullspaceTarget(const Eigen::VectorXf& nullspaceTarget)
    {
        this->nullspaceTarget = nullspaceTarget;
    }
 
    void
    CartesianNaturalPositionController::setNullspaceTarget(
        const std::vector<float>& nullspaceTarget)
    {
        ARMARX_CHECK_EXPRESSION(rns->getSize() == nullspaceTarget.size());
        for (size_t i = 0; i < rns->getSize(); i++)
        {
            this->nullspaceTarget(i) = nullspaceTarget.at(i);
        }
    }
 
    void
    CartesianNaturalPositionController::clearNullspaceTarget()
    {
        this->nullspaceTarget = Eigen::VectorXf::Constant(rns->getSize(), std::nanf(""));
    }
 
    void
    CartesianNaturalPositionController::clearFeedForwardVelocity()
    {
        feedForwardVelocity = Eigen::Vector6f::Zero();
    }
 
    float
    CartesianNaturalPositionController::getPositionError() const
    {
        return pcTcp.getPositionError(getCurrentTarget());
    }
 
    float
    CartesianNaturalPositionController::getOrientationError() const
    {
        return pcTcp.getOrientationError(getCurrentTarget());
    }
 
    bool
    CartesianNaturalPositionController::isTargetReached() const
    {
        return getPositionError() < thresholdPositionReached &&
               getOrientationError() < thresholdOrientationReached;
    }
 
    bool
    CartesianNaturalPositionController::isTargetNear() const
    {
        return getPositionError() < thresholdPositionNear &&
               getOrientationError() < thresholdOrientationNear;
    }
 
    const Eigen::Matrix4f&
    CartesianNaturalPositionController::getCurrentTarget() const
    {
        return tcpTarget;
    }
 
    const Eigen::Vector3f
    CartesianNaturalPositionController::getCurrentTargetPosition() const
    {
        return ::math::Helpers::GetPosition(tcpTarget);
    }
 
    const Eigen::Vector3f&
    CartesianNaturalPositionController::getCurrentElbowTarget() const
    {
        return elbowTarget;
    }
 
    const Eigen::VectorXf&
    CartesianNaturalPositionController::getCurrentNullspaceTarget() const
    {
        return nullspaceTarget;
    }
 
    bool
    CartesianNaturalPositionController::hasNullspaceTarget() const
    {
        for (int i = 0; i < nullspaceTarget.rows(); i++)
        {
            if (!std::isnan(nullspaceTarget(i)))
            {
                return true;
            }
        }
        return false;
    }
 
    void
    CartesianNaturalPositionController::setNullSpaceControl(bool enabled)
    {
        nullSpaceControlEnabled = enabled;
    }
 
    std::string
    CartesianNaturalPositionController::getStatusText()
    {
        std::stringstream ss;
        ss.precision(2);
        ss << std::fixed << "distance: " << getPositionError() << " mm "
           << VirtualRobot::MathTools::rad2deg(getOrientationError()) << " deg";
        return ss.str();
    }
 
    void
    CartesianNaturalPositionController::setConfig(
        const CartesianNaturalPositionControllerConfig& cfg)
    {
        jointLimitAvoidanceKp = cfg.jointLimitAvoidanceKp;
 
        thresholdOrientationNear = cfg.thresholdOrientationNear;
        thresholdOrientationReached = cfg.thresholdOrientationReached;
        thresholdPositionNear = cfg.thresholdPositionNear;
        thresholdPositionReached = cfg.thresholdPositionReached;
 
        maxJointVelocity = cfg.maxJointVelocity;
        maxNullspaceVelocity = cfg.maxNullspaceVelocity;
 
        nullspaceTargetKp = cfg.nullspaceTargetKp;
 
        pcTcp.maxPosVel = cfg.maxTcpPosVel;
        pcTcp.maxOriVel = cfg.maxTcpOriVel;
        pcTcp.KpOri = cfg.KpOri;
        pcTcp.KpPos = cfg.KpPos;
        pcElb.maxPosVel = cfg.maxElbPosVel;
        pcElb.KpPos = cfg.elbowKp;
 
        vcTcp.setMaxAccelerations(cfg.maxPositionAcceleration,
                                  cfg.maxOrientationAcceleration,
                                  cfg.maxNullspaceAcceleration);
        if (cfg.jointCosts.size() > 0)
        {
            vcTcp.controller.setJointCosts(cfg.jointCosts);
        }
    }
 
    Eigen::VectorXf
    CartesianNaturalPositionController::actualTcpVel(const Eigen::VectorXf& jointVel)
    {
        return (vcTcp.controller.ik->getJacobianMatrix(VirtualRobot::IKSolver::All) * jointVel);
    }
 
    Eigen::VectorXf
    CartesianNaturalPositionController::actualElbVel(const Eigen::VectorXf& jointVel)
    {
        return (vcElb.jacobi * jointVel);
    }
} // namespace armarx