NaturalIK.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 "NaturalIK.h"
 
#include <SimoxUtility/math/convert/deg_to_rad.h>
#include <SimoxUtility/math/convert/rad_to_deg.h>
#include <VirtualRobot/Nodes/RobotNode.h>
#include <VirtualRobot/RobotNodeSet.h>
#include <VirtualRobot/math/Helpers.h>
 
#include <ArmarXCore/core/exceptions/Exception.h>
#include <ArmarXCore/core/logging/Logging.h>
 
using namespace armarx;
 
NaturalIK::NaturalIK(std::string side, Eigen::Vector3f shoulderPos, float scale) :
    side(side), shoulderPos(shoulderPos), scale(scale)
{
}
 
NaturalIK::SoechtingForwardPositions
NaturalIK::solveSoechtingIK(const Eigen::Vector3f& targetPos, std::optional<float> minElbowHeight)
{
    Eigen::Vector3f vtgt = targetPos;
 
    NaturalIK::SoechtingAngles soechtingAngles = CalculateSoechtingAngles(vtgt);
    NaturalIK::SoechtingForwardPositions fwd = forwardKinematics(soechtingAngles);
    for (int i = 1; i < soechtingIterations; i++)
    {
        Eigen::Vector3f err = targetPos - fwd.wrist;
        vtgt = vtgt + err;
        fwd = forwardKinematics(CalculateSoechtingAngles(vtgt));
    }
 
 
    //ARMARX_IMPORTANT << VAROUT(fwd.elbow.z()) << VAROUT(minElbowHeight.value_or(-999));
 
    // this could be solved analytically.
    if (minElbowHeight.has_value() && minElbowHeight.value() > fwd.elbow.z())
    {
        float mEH = minElbowHeight.value();
        float lo = 0;
        float hi = side == "Right" ? -M_PI : M_PI;
        Eigen::Vector3f wri_n = (fwd.wrist - shoulderPos).normalized();
        Eigen::Vector3f elb = fwd.elbow;
        for (int i = 0; i < 20; i++)
        {
            float a = (lo + hi) / 2;
            Eigen::AngleAxisf aa(a, wri_n);
            elb = shoulderPos + aa * (fwd.elbow - shoulderPos);
            if (elb.z() > mEH)
            {
                hi = a;
            }
            else
            {
                lo = a;
            }
        }
        fwd.elbow = elb;
    }
 
    return fwd;
}
 
NaturalDiffIK::Result
NaturalIK::calculateIK(const Eigen::Matrix4f& targetPose,
                       ArmJoints arm,
                       NaturalIK::Parameters params)
{
    Eigen::Vector3f targetPos = math::Helpers::Position(targetPose);
    NaturalIK::SoechtingForwardPositions fwd =
        solveSoechtingIK(targetPos, params.minimumElbowHeight);
    return NaturalDiffIK::CalculateDiffIK(targetPose,
                                          fwd.elbow,
                                          arm.rns,
                                          arm.tcp,
                                          arm.elbow,
                                          NaturalDiffIK::Mode::All,
                                          params.diffIKparams);
}
 
NaturalDiffIK::Result
NaturalIK::calculateIKpos(const Eigen::Vector3f& targetPos,
                          NaturalIK::ArmJoints arm,
                          NaturalIK::Parameters params)
{
    Eigen::Matrix4f targetPose = math::Helpers::Pose(targetPos, Eigen::Matrix3f::Identity());
    NaturalIK::SoechtingForwardPositions fwd =
        solveSoechtingIK(targetPos, params.minimumElbowHeight);
    return NaturalDiffIK::CalculateDiffIK(targetPose,
                                          fwd.elbow,
                                          arm.rns,
                                          arm.tcp,
                                          arm.elbow,
                                          NaturalDiffIK::Mode::Position,
                                          params.diffIKparams);
}
 
NaturalDiffIK::Result
NaturalIK::calculateIK(const Eigen::Matrix4f& targetPose,
                       NaturalIK::ArmJoints arm,
                       NaturalDiffIK::Mode setOri,
                       NaturalIK::Parameters params)
{
    Eigen::Vector3f targetPos = math::Helpers::Position(targetPose);
    NaturalIK::SoechtingForwardPositions fwd =
        solveSoechtingIK(targetPos, params.minimumElbowHeight);
    //VirtualRobot::IKSolver::CartesianSelection mode = setOri ? VirtualRobot::IKSolver::All : VirtualRobot::IKSolver::Position;
    return NaturalDiffIK::CalculateDiffIK(
        targetPose, fwd.elbow, arm.rns, arm.tcp, arm.elbow, setOri, params.diffIKparams);
}
 
Eigen::Vector3f
NaturalIK::getShoulderPos()
{
    return shoulderPos;
}
 
NaturalIK::SoechtingAngles
NaturalIK::CalculateSoechtingAngles(Eigen::Vector3f target)
{
    target = target - shoulderPos;
    if (side == "Right")
    {
        target = target / scale;
    }
    else if (side == "Left")
    {
        target = target / scale;
        target(0) = -target(0);
    }
    else
    {
        throw LocalException("Unsupported side: ") << side << ". supported are Left|Right.";
    }
 
    target = target / 10; // Soechting is defined in cm
    float x = target(0);
    float y = target(1);
    float z = target(2);
 
    float R = target.norm();
    float Chi = simox::math::rad_to_deg(std::atan2(x, y));
    float Psi = simox::math::rad_to_deg(std::atan2(z, std::sqrt(x * x + y * y)));
 
    //ARMARX_IMPORTANT << "target: " << target.transpose() << " " << VAROUT(R) << VAROUT(Chi) << VAROUT(Psi);
 
    // Angles derived from accurate pointing
    //SoechtingAngles sa;
    //sa.SE =  -6.7 + 1.09*R + 1.10*Psi;
    //sa.EE =  47.6 + 0.33*R - 0.95*Psi;
    //sa.EY = -11.5 + 1.27*Chi - 0.54*Psi;
    //sa.SY =  67.7 + 1.00*Chi - 0.68*R;
 
    // Angles derived from pointing in the dark
    SoechtingAngles sa;
    sa.SE = -4.0 + 1.10 * R + 0.90 * Psi;
    sa.EE = 39.4 + 0.54 * R - 1.06 * Psi;
    sa.SY = 13.2 + 0.86 * Chi + 0.11 * Psi;
    sa.EY = -10.0 + 1.08 * Chi - 0.35 * Psi;
 
    sa.SE = simox::math::deg_to_rad(sa.SE);
    sa.SY = simox::math::deg_to_rad(sa.SY);
    sa.EE = simox::math::deg_to_rad(sa.EE);
    sa.EY = simox::math::deg_to_rad(sa.EY);
 
    if (side == "Left")
    {
        sa.SY = -sa.SY;
        sa.EY = -sa.EY;
    }
 
    return sa;
}
 
NaturalIK::SoechtingForwardPositions
NaturalIK::forwardKinematics(SoechtingAngles sa)
{
    Eigen::AngleAxisf aaSE(sa.SE, Eigen::Vector3f::UnitX());
    Eigen::AngleAxisf aaSY(-sa.SY, Eigen::Vector3f::UnitZ());
    Eigen::AngleAxisf aaEE(-sa.EE, Eigen::Vector3f::UnitX());
    Eigen::AngleAxisf aaEY(-sa.EY, Eigen::Vector3f::UnitZ());
    Eigen::Vector3f elb = -Eigen::Vector3f::UnitZ() * upperArmLength;
    elb = aaSY * aaSE * elb;
    Eigen::Vector3f wri = Eigen::Vector3f::UnitZ() * lowerArmLength;
    wri = aaEY * aaEE * wri;
    //ARMARX_IMPORTANT << VAROUT(elb.transpose()) << VAROUT(wri.transpose());
 
    NaturalIK::SoechtingForwardPositions res;
    res.shoulder = shoulderPos;
    res.elbow = shoulderPos + elb;
    res.wrist = shoulderPos + elb + wri;
    res.soechtingAngles = sa;
 
    return res;
}
 
void
NaturalIK::setScale(float scale)
{
    this->scale = scale;
}
 
float
NaturalIK::getScale()
{
    return scale;
}
 
float
NaturalIK::getUpperArmLength() const
{
    return upperArmLength;
}
 
void
NaturalIK::setUpperArmLength(float value)
{
    upperArmLength = value;
}
 
float
NaturalIK::getLowerArmLength() const
{
    return lowerArmLength;
}
 
void
NaturalIK::setLowerArmLength(float value)
{
    lowerArmLength = value;
}
 
NaturalIKProvider::NaturalIKProvider(const NaturalIK& natik,
                                     const NaturalIK::ArmJoints& arm,
                                     const NaturalDiffIK::Mode& setOri,
                                     const NaturalIK::Parameters& params) :
    natik(natik), arm(arm), setOri(setOri), params(params)
{
}
 
DiffIKResult
NaturalIKProvider::SolveAbsolute(const Eigen::Matrix4f& targetPose)
{
    params.diffIKparams.resetRnsValues = true;
    NaturalDiffIK::Result result = natik.calculateIK(targetPose, arm, setOri, params);
    DiffIKResult r;
    r.jointValues = arm.rns->getJointValuesEigen();
    r.oriError = result.oriError;
    r.posError = result.posError;
    r.reachable = result.reached;
    return r;
}
 
DiffIKResult
NaturalIKProvider::SolveRelative(const Eigen::Matrix4f& targetPose,
                                 const Eigen::VectorXf& startJointValues)
{
    params.diffIKparams.resetRnsValues = false;
    arm.rns->setJointValues(startJointValues);
    NaturalDiffIK::Result result = natik.calculateIK(targetPose, arm, setOri, params);
    DiffIKResult r;
    r.jointValues = arm.rns->getJointValuesEigen();
    r.oriError = result.oriError;
    r.posError = result.posError;
    r.reachable = result.reached;
    return r;
}