PositionControllerHelper.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     Simon Ottenhaus (simon dot ottenhaus at kit dot edu)
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "PositionControllerHelper.h"
 
#include <ArmarXCore/core/time/TimeUtil.h>
 
#include <RobotAPI/libraries/core/CartesianVelocityController.h>
 
#include <VirtualRobot/math/Helpers.h>
#include <VirtualRobot/MathTools.h>
 
namespace armarx
{
    PositionControllerHelper::PositionControllerHelper(const VirtualRobot::RobotNodePtr& tcp, const VelocityControllerHelperPtr& velocityControllerHelper, const Eigen::Matrix4f& target)
        : posController(tcp), velocityControllerHelper(velocityControllerHelper), currentWaypointIndex(0)
    {
        waypoints.push_back(target);
    }
 
    PositionControllerHelper::PositionControllerHelper(const VirtualRobot::RobotNodePtr& tcp, const VelocityControllerHelperPtr& velocityControllerHelper, const std::vector<Eigen::Matrix4f>& waypoints)
        : posController(tcp), velocityControllerHelper(velocityControllerHelper), waypoints(waypoints), currentWaypointIndex(0)
    {
    }
 
    PositionControllerHelper::PositionControllerHelper(const VirtualRobot::RobotNodePtr& tcp, const VelocityControllerHelperPtr& velocityControllerHelper, const std::vector<PosePtr>& waypoints)
        : posController(tcp), velocityControllerHelper(velocityControllerHelper), currentWaypointIndex(0)
    {
        for (const PosePtr& pose : waypoints)
        {
            this->waypoints.push_back(pose->toEigen());
        }
    }
 
    void PositionControllerHelper::readConfig(const CartesianPositionControllerConfigBase &config)
    {
        thresholdOrientationNear = config.thresholdOrientationNear;
        thresholdOrientationReached = config.thresholdOrientationReached;
        thresholdPositionNear = config.thresholdPositionNear;
        thresholdPositionReached = config.thresholdPositionReached;
        posController.KpOri = config.KpOri;
        posController.KpPos = config.KpPos;
        posController.maxOriVel = config.maxOriVel;
        posController.maxPosVel = config.maxPosVel;
    }
 
    void PositionControllerHelper::readConfig(const CartesianPositionControllerConfigBasePtr& config)
    {
        readConfig(*config);
    }
 
    void PositionControllerHelper::update()
    {
        updateRead();
        updateWrite();
    }
 
    void PositionControllerHelper::updateRead()
    {
        if (!isLastWaypoint() && isCurrentTargetNear())
        {
            currentWaypointIndex++;
        }
    }
 
    void PositionControllerHelper::updateWrite()
    {
        auto target = getCurrentTarget();
        Eigen::VectorXf cv = posController.calculate(target, VirtualRobot::IKSolver::All);
        velocityControllerHelper->setTargetVelocity(cv + feedForwardVelocity);
        if (autoClearFeedForward)
        {
            clearFeedForwardVelocity();
        }
    }
 
    void PositionControllerHelper::setNewWaypoints(const std::vector<Eigen::Matrix4f>& waypoints)
    {
        this->waypoints = waypoints;
        currentWaypointIndex = 0;
    }
 
    void PositionControllerHelper::setNewWaypoints(const std::vector<PosePtr>& waypoints)
    {
        this->waypoints.clear();
        for (const PosePtr& pose : waypoints)
        {
            this->waypoints.push_back(pose->toEigen());
        }
        currentWaypointIndex = 0;
    }
 
    void PositionControllerHelper::addWaypoint(const Eigen::Matrix4f& waypoint)
    {
        this->waypoints.push_back(waypoint);
    }
 
    void PositionControllerHelper::setTarget(const Eigen::Matrix4f& target)
    {
        waypoints.clear();
        waypoints.push_back(target);
        currentWaypointIndex = 0;
    }
 
    void PositionControllerHelper::setFeedForwardVelocity(const Eigen::Vector6f& feedForwardVelocity)
    {
        this->feedForwardVelocity = feedForwardVelocity;
    }
 
    void PositionControllerHelper::setFeedForwardVelocity(const Eigen::Vector3f& feedForwardVelocityPos, const Eigen::Vector3f& feedForwardVelocityOri)
    {
        feedForwardVelocity.block<3, 1>(0, 0) = feedForwardVelocityPos;
        feedForwardVelocity.block<3, 1>(3, 0) = feedForwardVelocityOri;
    }
 
    void PositionControllerHelper::clearFeedForwardVelocity()
    {
        feedForwardVelocity = Eigen::Vector6f::Zero();
    }
 
    void PositionControllerHelper::immediateHardStop(bool clearTargets)
    {
        velocityControllerHelper->controller->immediateHardStop();
        if (clearTargets)
        {
            setTarget(posController.getTcp()->getPoseInRootFrame());
        }
    }
 
    float PositionControllerHelper::getPositionError() const
    {
        return posController.getPositionError(getCurrentTarget());
    }
 
    float PositionControllerHelper::getOrientationError() const
    {
        return posController.getOrientationError(getCurrentTarget());
    }
 
    bool PositionControllerHelper::isCurrentTargetReached() const
    {
        return getPositionError() < thresholdPositionReached && getOrientationError() < thresholdOrientationReached;
    }
 
    bool PositionControllerHelper::isCurrentTargetNear() const
    {
        return getPositionError() < thresholdPositionNear && getOrientationError() < thresholdOrientationNear;
    }
 
    bool PositionControllerHelper::isFinalTargetReached() const
    {
        return isLastWaypoint() && isCurrentTargetReached();
    }
 
    bool PositionControllerHelper::isFinalTargetNear() const
    {
        return isLastWaypoint() && isCurrentTargetNear();
    }
 
    bool PositionControllerHelper::isLastWaypoint() const
    {
        return currentWaypointIndex + 1 >= waypoints.size();
    }
 
    const Eigen::Matrix4f& PositionControllerHelper::getCurrentTarget() const
    {
        return waypoints.at(currentWaypointIndex);
    }
 
    const Eigen::Vector3f PositionControllerHelper::getCurrentTargetPosition() const
    {
        return math::Helpers::GetPosition(waypoints.at(currentWaypointIndex));
    }
 
    size_t PositionControllerHelper::skipToClosestWaypoint(float rad2mmFactor)
    {
        float dist = FLT_MAX;
        size_t minIndex = 0;
        for (size_t i = 0; i < waypoints.size(); i++)
        {
            float posErr = posController.getPositionError(waypoints.at(i));
            float oriErr = posController.getOrientationError(waypoints.at(i));
            float d = posErr + oriErr * rad2mmFactor;
            if (d < dist)
            {
                minIndex = i;
                dist = d;
            }
        }
        currentWaypointIndex = minIndex;
        return currentWaypointIndex;
    }
 
    void PositionControllerHelper::setNullSpaceControl(bool enabled)
    {
        velocityControllerHelper->setNullSpaceControl(enabled);
    }
 
    std::string PositionControllerHelper::getStatusText()
    {
        std::stringstream ss;
        ss.precision(2);
        ss << std::fixed << "Waypoint: " << (currentWaypointIndex + 1) << "/" << waypoints.size() << " distance: " << getPositionError() << " mm " << VirtualRobot::MathTools::rad2deg(getOrientationError()) << " deg";
        return ss.str();
    }
 
    bool PositionControllerHelper::OptimizeNullspace(const VirtualRobot::RobotNodePtr& tcp, const VirtualRobot::RobotNodeSetPtr& rns, const Eigen::Matrix4f& target, const NullspaceOptimizationArgs& args)
    {
        CartesianPositionController posHelper(tcp);
 
        CartesianVelocityControllerPtr cartesianVelocityController;
        cartesianVelocityController.reset(new CartesianVelocityController(rns, tcp, args.invJacMethod));
 
        float errorOriInitial = posHelper.getOrientationError(target);
        float errorPosInitial = posHelper.getPositionError(target);
 
        float stepLength = args.stepLength;
        float eps = args.eps;
 
        std::vector<float> initialJointAngles =  rns->getJointValues();
 
        for (int i = 0; i < args.loops; i++)
        {
            Eigen::VectorXf tcpVelocities = posHelper.calculate(target, args.cartesianSelection);
            Eigen::VectorXf nullspaceVel = cartesianVelocityController->calculateJointLimitAvoidance(); // calculateNullspaceVelocity(tcpVelocities, 1.0f, vrmode);
            float nullspaceLen = nullspaceVel.norm();
            if (nullspaceLen > stepLength)
            {
                nullspaceVel = nullspaceVel / nullspaceLen * stepLength;
            }
            Eigen::VectorXf jointVelocities = cartesianVelocityController->calculate(tcpVelocities, nullspaceVel, args.cartesianSelection);
 
            //jointVelocities = jointVelocities * stepLength;
            float len = jointVelocities.norm();
            if (len > stepLength)
            {
                jointVelocities = jointVelocities / len * stepLength;
            }
 
            for (size_t n = 0; n < rns->getSize(); n++)
            {
                rns->getNode(n)->setJointValue(rns->getNode(n)->getJointValue() + jointVelocities(n));
            }
            if (len < eps)
            {
                break;
            }
        }
 
        float errorOriAfter = posHelper.getOrientationError(target);
        float errorPosAfter = posHelper.getPositionError(target);
        if (errorOriAfter < errorOriInitial + 1.f / 180 * M_PI && errorPosAfter < errorPosInitial + 1.f)
        {
            return true;
        }
        else
        {
            rns->setJointValues(initialJointAngles);
            return false;
        }
    }
}