CompositeDiffIK.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 "CompositeDiffIK.h"
 
#include <cfloat>
 
#include <VirtualRobot/IK/DifferentialIK.h>
#include <VirtualRobot/Nodes/RobotNode.h>
#include <VirtualRobot/Robot.h>
#include <VirtualRobot/RobotNodeSet.h>
#include <VirtualRobot/math/Helpers.h>
 
#include <ArmarXCore/core/exceptions/Exception.h>
 
using namespace armarx;
 
CompositeDiffIK::CompositeDiffIK(const VirtualRobot::RobotNodeSetPtr& rns) : rns(rns)
{
    ik.reset(new VirtualRobot::DifferentialIK(
        rns, rns->getRobot()->getRootNode(), VirtualRobot::JacobiProvider::eSVDDamped));
}
 
void
CompositeDiffIK::addTarget(const TargetPtr& target)
{
    targets.emplace_back(target);
}
 
CompositeDiffIK::TargetPtr
CompositeDiffIK::addTarget(const VirtualRobot::RobotNodePtr& tcp,
                           const Eigen::Matrix4f& target,
                           VirtualRobot::IKSolver::CartesianSelection mode)
{
    TargetPtr t = std::make_shared<Target>(rns, tcp, target, mode);
    addTarget(t);
    return t;
}
 
void
CompositeDiffIK::addNullspaceGradient(const CompositeDiffIK::NullspaceGradientPtr& gradient)
{
    nullspaceGradients.emplace_back(gradient);
}
 
CompositeDiffIK::NullspaceTargetPtr
CompositeDiffIK::addNullspacePositionTarget(const VirtualRobot::RobotNodePtr& tcp,
                                            const Eigen::Vector3f& target)
{
    CompositeDiffIK::NullspaceTargetPtr nst(new CompositeDiffIK::NullspaceTarget(
        rns,
        tcp,
        math::Helpers::CreatePose(target, Eigen::Matrix3f::Identity()),
        VirtualRobot::IKSolver::CartesianSelection::Position));
    addNullspaceGradient(nst);
    return nst;
}
 
CompositeDiffIK::Result
CompositeDiffIK::solve(Parameters params)
{
    //ARMARX_IMPORTANT << "###";
    //ARMARX_IMPORTANT << VAROUT(rns->getJointValuesEigen().transpose());
    if (params.resetRnsValues)
    {
        for (VirtualRobot::RobotNodePtr rn : rns->getAllRobotNodes())
        {
            if (rn->isLimitless())
            {
                rn->setJointValue(0);
            }
            else
            {
                rn->setJointValue((rn->getJointLimitHi() + rn->getJointLimitLo()) / 2);
            }
        }
    }
    //ARMARX_IMPORTANT << VAROUT(rns->getJointValuesEigen().transpose());
 
    SolveState s;
 
    s.rows = 0;
    s.cols = rns->getSize();
 
    for (const TargetPtr& target : targets)
    {
        s.rows += CartesianSelectionToSize(target->mode);
    }
 
    s.jointRegularization = Eigen::VectorXf::Zero(s.cols);
    for (size_t i = 0; i < rns->getSize(); i++)
    {
        s.jointRegularization(i) = rns->getNode(i)->isTranslationalJoint()
                                       ? params.jointRegularizationTranslation
                                       : params.jointRegularizationRotation;
    }
 
    s.cartesianRegularization = Eigen::VectorXf::Zero(s.rows);
    {
        CartesianVelocityController tmpVC(rns);
        int row = 0;
        for (const TargetPtr& target : targets)
        {
            int h = CartesianSelectionToSize(target->mode);
            target->jacobi = Eigen::MatrixXf::Zero(h, s.cols);
            s.cartesianRegularization.block(row, 0, h, 1) =
                tmpVC.calculateRegularization(target->mode);
            row += h;
        }
    }
    //ARMARX_IMPORTANT << "START";
    //ARMARX_INFO << VAROUT(regularization.transpose());
 
    s.jointValues = rns->getJointValuesEigen();
 
    for (size_t i = 0; i <= params.steps; i++)
    {
        step(params, s, i);
    }
 
    bool allReached = true;
    for (const TargetPtr& target : targets)
    {
        allReached = allReached &&
                     target->pCtrl.reached(
                         target->target, target->mode, target->maxPosError, target->maxOriError);
    }
 
    //ARMARX_IMPORTANT << "END";
 
 
    //ARMARX_IMPORTANT << ss.str();
 
    Result result;
    result.jointValues = rns->getJointValuesEigen();
    result.reached = allReached;
    result.jointLimitMargins = Eigen::VectorXf::Zero(rns->getSize());
    result.minimumJointLimitMargin = FLT_MAX;
    for (size_t i = 0; i < rns->getSize(); i++)
    {
        VirtualRobot::RobotNodePtr rn = rns->getNode(i);
        if (rn->isLimitless())
        {
            result.jointLimitMargins(i) = M_PI;
        }
        else
        {
            result.jointLimitMargins(i) = std::min(rn->getJointValue() - rn->getJointLimitLo(),
                                                   rn->getJointLimitHi() - rn->getJointValue());
            result.minimumJointLimitMargin =
                std::min(result.minimumJointLimitMargin, result.jointLimitMargins(i));
        }
    }
 
    return result;
}
 
Eigen::MatrixXf
CompositeDiffIK::CalculateNullspaceSVD(const Eigen::Matrix4f& jacobi)
{
    Eigen::FullPivLU<Eigen::MatrixXf> lu_decomp(jacobi);
    Eigen::MatrixXf nullspaceLU = lu_decomp.kernel();
    return nullspaceLU;
}
 
Eigen::MatrixXf
CompositeDiffIK::CalculateNullspaceLU(const Eigen::Matrix4f& jacobi)
{
    // cols >= rows
    int rows = jacobi.rows();
    int cols = jacobi.cols();
    Eigen::JacobiSVD<Eigen::MatrixXf> svd(jacobi, Eigen::ComputeFullU | Eigen::ComputeFullV);
    ///
    /// \brief V contains right singular vector and nullspace:
    /// V.shape: (cols,cols)
    /// singular vectors: V[:,0:rows]
    /// nullspace: V[:,rows:cols-rows]
    ///
    Eigen::MatrixXf V = svd.matrixV();
    Eigen::MatrixXf nullspaceSVD = V.block(0, rows, cols, cols - rows);
    return nullspaceSVD;
}
 
void
CompositeDiffIK::step(CompositeDiffIK::Parameters& params, SolveState& s, int stepNr)
{
    //ARMARX_IMPORTANT << VAROUT(i);
    s.jacobi = Eigen::MatrixXf::Zero(s.rows, s.cols);
    s.invJac = Eigen::MatrixXf::Zero(s.cols, s.rows);
 
    Eigen::VectorXf cartesianVel = Eigen::VectorXf::Zero(s.rows);
    {
        int row = 0;
        for (const TargetPtr& target : targets)
        {
            ik->updateJacobianMatrix(target->jacobi, target->tcp, target->mode);
            int h = CartesianSelectionToSize(target->mode);
            s.jacobi.block(row, 0, h, s.cols) = target->jacobi;
            Eigen::VectorXf cv = target->pCtrl.calculate(target->target, target->mode);
            //ARMARX_INFO << VAROUT(cv.transpose());
            cartesianVel.block(row, 0, h, 1) = cv;
            row += h;
            if (params.returnIKSteps)
            {
                TargetStep step;
                step.tcpPose = target->tcp->getPoseInRootFrame();
                step.cartesianVel = cv;
                step.posDiff = target->pCtrl.getPositionDiff(target->target);
                step.oriDiff = target->pCtrl.getOrientationDiff(target->target);
                target->ikSteps.emplace_back(step);
            }
        }
    }
    //ARMARX_INFO << VAROUT(cartesianVel.transpose());
    if (stepNr == 0)
    {
        //ARMARX_IMPORTANT << VAROUT(s.jacobi);
    }
 
    for (int row = 0; row < s.rows; row++)
    {
        s.jacobi.block(row, 0, 1, s.cols) =
            s.jacobi.block(row, 0, 1, s.cols).cwiseProduct(s.jointRegularization.transpose());
    }
    if (stepNr == 0)
    {
        //ARMARX_IMPORTANT << VAROUT(s.jacobi);
    }
 
    ik->updatePseudoInverseJacobianMatrix(s.invJac, s.jacobi, 0, s.cartesianRegularization);
 
    Eigen::VectorXf nullspaceVel = Eigen::VectorXf::Zero(s.cols);
 
    for (const NullspaceGradientPtr& nsGradient : nullspaceGradients)
    {
        Eigen::VectorXf nsgrad = nsGradient->kP * nsGradient->getGradient(params);
        //ARMARX_INFO << VAROUT(nsgrad.transpose());
        nullspaceVel += nsgrad;
        if (stepNr == 0)
        {
            //ARMARX_IMPORTANT << VAROUT(nsgrad.transpose());
        }
    }
    //LimitInfNormTo(nullspaceVel, params.maxJointAngleStep);
 
 
    Eigen::JacobiSVD<Eigen::MatrixXf> svd(s.jacobi, Eigen::ComputeFullU | Eigen::ComputeFullV);
    Eigen::MatrixXf V = svd.matrixV();
    Eigen::MatrixXf nullspaceSVD = V.block(0, s.rows, s.cols, s.cols - s.rows);
 
    s.nullspace = nullspaceSVD; // CalculateNullspaceSVD(s.jacobi);
 
    Eigen::VectorXf nsv = Eigen::VectorXf::Zero(s.cols);
    for (int i = 0; i < s.nullspace.cols(); i++)
    {
        float squaredNorm = s.nullspace.col(i).squaredNorm();
        // Prevent division by zero
        if (squaredNorm > 1.0e-32f)
        {
            nsv += s.nullspace.col(i) * s.nullspace.col(i).dot(nullspaceVel) /
                   s.nullspace.col(i).squaredNorm();
        }
    }
 
    Eigen::VectorXf jv = s.invJac * cartesianVel;
    //ARMARX_INFO << VAROUT(jv.transpose());
    //ARMARX_INFO << VAROUT(nsv.transpose());
    jv = jv + nsv;
    jv = jv * params.stepSize;
    jv = LimitInfNormTo(jv, params.maxJointAngleStep);
    jv = jv.cwiseProduct(s.jointRegularization);
 
    //ARMARX_INFO << VAROUT(jv.transpose());
 
    Eigen::VectorXf newJointValues = s.jointValues + jv;
    if (stepNr == 0)
    {
        //ARMARX_IMPORTANT << VAROUT(cartesianVel.transpose());
        //ARMARX_IMPORTANT << VAROUT(nullspaceVel.transpose());
 
        //ARMARX_IMPORTANT << VAROUT(currentJointValues.transpose());
        //ARMARX_IMPORTANT << VAROUT(newJointValues.transpose());
    }
    rns->setJointValues(newJointValues);
    s.jointValues = newJointValues;
}
 
int
CompositeDiffIK::CartesianSelectionToSize(VirtualRobot::IKSolver::CartesianSelection mode)
{
    switch (mode)
    {
        case VirtualRobot::IKSolver::CartesianSelection::Position:
            return 3;
        case VirtualRobot::IKSolver::CartesianSelection::Orientation:
            return 3;
        case VirtualRobot::IKSolver::CartesianSelection::All:
            return 6;
        default:
            throw LocalException("mode not supported: ") << mode;
    }
}
 
CompositeDiffIK::Target::Target(const VirtualRobot::RobotNodeSetPtr& rns,
                                const VirtualRobot::RobotNodePtr& tcp,
                                const Eigen::Matrix4f& target,
                                VirtualRobot::IKSolver::CartesianSelection mode) :
    tcp(tcp), target(target), mode(mode), pCtrl(tcp)
{
    jacobi = Eigen::MatrixXf::Zero(0, 0);
}
 
CompositeDiffIK::NullspaceTarget::NullspaceTarget(const VirtualRobot::RobotNodeSetPtr& rns,
                                                  const VirtualRobot::RobotNodePtr& tcp,
                                                  const Eigen::Matrix4f& target,
                                                  VirtualRobot::IKSolver::CartesianSelection mode) :
    tcp(tcp), target(target), mode(mode), pCtrl(tcp), vCtrl(rns, tcp)
{
}
 
void
CompositeDiffIK::NullspaceTarget::init(Parameters&)
{
    ikSteps.clear();
}
 
Eigen::VectorXf
CompositeDiffIK::NullspaceTarget::getGradient(Parameters& params)
{
    Eigen::VectorXf cv = pCtrl.calculate(target, mode);
    Eigen::VectorXf jv = vCtrl.calculate(cv, mode);
    if (params.returnIKSteps)
    {
        NullspaceTargetStep step;
        step.tcpPose = tcp->getPoseInRootFrame();
        step.posDiff = pCtrl.getPositionDiff(target);
        step.oriDiff = pCtrl.getOrientationDiff(target);
        step.cartesianVel = cv;
        step.jointVel = jv;
        ikSteps.emplace_back(step);
    }
    return jv;
}
 
Eigen::VectorXf
CompositeDiffIK::LimitInfNormTo(Eigen::VectorXf vec, float maxValue)
{
    float infNorm = vec.lpNorm<Eigen::Infinity>();
    if (infNorm > maxValue)
    {
        vec = vec / infNorm * maxValue;
    }
    return vec;
}
 
CompositeDiffIK::NullspaceJointTarget::NullspaceJointTarget(
    const VirtualRobot::RobotNodeSetPtr& rns) :
    rns(rns), target(rns->getJointValuesEigen()), weight(Eigen::VectorXf::Zero(rns->getSize()))
{
}
 
CompositeDiffIK::NullspaceJointTarget::NullspaceJointTarget(
    const VirtualRobot::RobotNodeSetPtr& rns,
    const Eigen::VectorXf& target,
    const Eigen::VectorXf& weight) :
    rns(rns), target(target), weight(weight)
{
}
 
void
CompositeDiffIK::NullspaceJointTarget::set(int index, float target, float weight)
{
    this->target(index) = target;
    this->weight(index) = weight;
}
 
void
CompositeDiffIK::NullspaceJointTarget::set(const std::string& jointName, float target, float weight)
{
    int index = rns->getRobotNodeIndex(jointName);
    if (index < 0)
    {
        throw LocalException("RobotNodeSet has no joint ") << jointName;
    }
    set(index, target, weight);
}
 
void
CompositeDiffIK::NullspaceJointTarget::set(const VirtualRobot::RobotNodePtr& rn,
                                           float target,
                                           float weight)
{
    int index = rns->getRobotNodeIndex(rn);
    if (index < 0)
    {
        throw LocalException("RobotNodeSet has no joint ") << rn->getName();
    }
    set(index, target, weight);
}
 
void
CompositeDiffIK::NullspaceJointTarget::init(CompositeDiffIK::Parameters&)
{
}
 
Eigen::VectorXf
CompositeDiffIK::NullspaceJointTarget::getGradient(Parameters& params)
{
    return (target - rns->getJointValuesEigen()).cwiseProduct(weight);
}
 
CompositeDiffIK::NullspaceJointLimitAvoidance::NullspaceJointLimitAvoidance(
    const VirtualRobot::RobotNodeSetPtr& rns) :
    rns(rns), weight(Eigen::VectorXf::Constant(rns->getSize(), 1))
{
}
 
CompositeDiffIK::NullspaceJointLimitAvoidance::NullspaceJointLimitAvoidance(
    const VirtualRobot::RobotNodeSetPtr& rns,
    const Eigen::VectorXf& weight) :
    rns(rns), weight(weight)
{
}
 
void
CompositeDiffIK::NullspaceJointLimitAvoidance::setWeight(int index, float weight)
{
    this->weight(index) = weight;
}
 
void
CompositeDiffIK::NullspaceJointLimitAvoidance::setWeight(const std::string& jointName, float weight)
{
    int index = rns->getRobotNodeIndex(jointName);
    if (index < 0)
    {
        throw LocalException("RobotNodeSet has no joint ") << jointName;
    }
    setWeight(index, weight);
}
 
void
CompositeDiffIK::NullspaceJointLimitAvoidance::setWeight(const VirtualRobot::RobotNodePtr& rn,
                                                         float weight)
{
    int index = rns->getRobotNodeIndex(rn);
    if (index < 0)
    {
        throw LocalException("RobotNodeSet has no joint ") << rn->getName();
    }
    setWeight(index, weight);
}
 
void
CompositeDiffIK::NullspaceJointLimitAvoidance::setWeights(const VirtualRobot::RobotNodeSetPtr& rns,
                                                          float weight)
{
    for (const VirtualRobot::RobotNodePtr& rn : rns->getAllRobotNodes())
    {
        setWeight(rn, weight);
    }
}
 
void
CompositeDiffIK::NullspaceJointLimitAvoidance::init(CompositeDiffIK::Parameters&)
{
}
 
Eigen::VectorXf
CompositeDiffIK::NullspaceJointLimitAvoidance::getGradient(Parameters& params)
{
    Eigen::VectorXf r(rns->getSize());
    for (size_t i = 0; i < rns->getSize(); i++)
    {
        VirtualRobot::RobotNodePtr rn = rns->getNode(i);
        if (rn->isLimitless())
        {
            r(i) = 0;
        }
        else
        {
            float f = math::Helpers::ILerp(
                rn->getJointLimitLo(), rn->getJointLimitHi(), rn->getJointValue());
            r(i) = cos(f * M_PI);
            //r(i) = math::MathUtils::Lerp(1.f, -1.f, f);
        }
    }
    return r.cwiseProduct(weight);
}
 
Eigen::Matrix4f
armarx::CompositeDiffIK::Target::getPoseError() const
{
    return tcp->getPoseInRootFrame().inverse() * target;
}
 
Eigen::Vector3f
armarx::CompositeDiffIK::Target::getPosError() const
{
    return (tcp->getPoseInRootFrame().inverse() * target).topRightCorner<3, 1>();
}
 
auto
armarx::CompositeDiffIK::Target::getOriError() const
{
    return simox::math::delta_angle(tcp->getPoseInRootFrame(), target);
}