PositionControllerHelper.cpp

/*
 * 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>

using 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 CartesianPositionControllerConfigBasePtr& 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::update()
{
    updateRead();
    updateWrite();
}

void PositionControllerHelper::updateRead()
{
    if (!isLastWaypoint() && isCurrentTargetNear())
    {
        currentWaypointIndex++;
    }
}

void PositionControllerHelper::updateWrite()
{
    Eigen::VectorXf cv = posController.calculate(getCurrentTarget(), 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;
    }
}