CartesianPositionControlIK.cpp

Go to the documentation of this file.

/*
 * This file is part of ArmarX.
 *
 * Copyright (C) 2014-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/>.
 *
 * @package    RobotSkillTemplates::MotionControlGroup
 * @author     Peter Kaiser ( peter dot kaiser at kit dot edu )
 * @date       2014
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "CartesianPositionControlIK.h"
 
#include <VirtualRobot/IK/ConstrainedOptimizationIK.h>
#include <VirtualRobot/IK/GenericIKSolver.h>
#include <VirtualRobot/IK/constraints/OrientationConstraint.h>
#include <VirtualRobot/IK/constraints/PositionConstraint.h>
#include <VirtualRobot/LinkedCoordinate.h>
#include <VirtualRobot/MathTools.h>
#include <VirtualRobot/RobotNodeSet.h> // IWYU pragma: keep
#include <VirtualRobot/XML/RobotIO.h>
 
#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
 
#include <RobotAPI/libraries/core/Pose.h>
#include <RobotAPI/libraries/core/RobotStatechartContext.h>
#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
 
#include "../MotionControlGroupStatechartContext.h"
 
using namespace armarx;
using namespace MotionControlGroup;
 
// DO NOT EDIT NEXT LINE
CartesianPositionControlIK::SubClassRegistry
    CartesianPositionControlIK::Registry(CartesianPositionControlIK::GetName(),
                                         &CartesianPositionControlIK::CreateInstance);
 
CartesianPositionControlIK::CartesianPositionControlIK(XMLStateConstructorParams stateData) :
    XMLStateTemplate<CartesianPositionControlIK>(stateData),
    CartesianPositionControlIKGeneratedBase<CartesianPositionControlIK>(stateData)
{
}
 
void
CartesianPositionControlIK::onEnter()
{
    // Create and install timeout condition
    if (in.getTimeout() > 0)
    {
        setTimeoutEvent(in.getTimeout(), createEventTimeout());
    }
}
 
void
CartesianPositionControlIK::run()
{
    MotionControlGroupStatechartContext* context =
        getContext<MotionControlGroupStatechartContext>();
 
    std::string kinChainName = in.getKinematicChainName();
    float positionTolerance = in.getTcpPositionTolerance();
    float orientationTolerance = in.getTcpOrientationTolerance();
 
 
    VirtualRobot::RobotPtr robot = context->getLocalRobot() /*->clone("Armar3")*/;
    RemoteRobot::synchronizeLocalClone(robot,
                                       context->getRobotStateComponent()->getSynchronizedRobot());
 
    //    std::string robotfilepath = context->getRobotStateComponent()->getRobotFilename();
    //    if (!ArmarXDataPath::getAbsolutePath(robotfilepath, robotfilepath))
    //    {
    //        auto packages = context->getRobotStateComponent()->getArmarXPackages();
    //        for (auto & p : packages)
    //        {
    //            CMakePackageFinder finder(p);
    //            ArmarXDataPath::addDataPaths(finder.getDataDir());
    //        }
    //        if (!ArmarXDataPath::getAbsolutePath(robotfilepath, robotfilepath))
    //        {
    //            ARMARX_WARNING << "Could not get absolute robot model filepath for relative path : " << robotfilepath << " - using network clone";
    //            robotfilepath = "";
    //        }
    //    }
    VirtualRobot::RobotPtr localrobot = robot; //VirtualRobot::RobotIO::loadRobot(robotfilepath);
    //    RemoteRobot::synchronizeLocalClone(localrobot, context->getRobotStateComponent()->getSynchronizedRobot());
 
    if (!robot)
    {
        ARMARX_WARNING << "Could not create robot" << flush;
        emitFailure();
    }
 
    if (!robot->hasRobotNodeSet(kinChainName))
    {
        ARMARX_WARNING << "Kinematic Chain '" << kinChainName << "' not available" << flush;
        emitFailure();
    }
 
    auto nodeSet = localrobot->getRobotNodeSet(kinChainName);
 
    FramedPosePtr target(new FramedPose());
    VirtualRobot::IKSolver::CartesianSelection ikType = VirtualRobot::IKSolver::All;
    if (isInputParameterSet("TcpTargetPosition") && isInputParameterSet("TcpTargetOrientation"))
    {
        ikType = VirtualRobot::IKSolver::All;
 
        FramedPositionPtr targetPosition = in.getTcpTargetPosition();
        FramedOrientationPtr targetOrientation = in.getTcpTargetOrientation();
        ARMARX_INFO << "Target position (Frame = " << targetPosition->getFrame()
                    << "): " << targetPosition->toEigen();
        ARMARX_INFO << "Target orientation (Frame = " << targetOrientation->getFrame()
                    << "): " << targetOrientation->toEigen();
        Eigen::Vector3f rpy;
        Eigen::Matrix4f mat = Eigen::Matrix4f::Identity();
        mat.block<3, 3>(0, 0) = targetOrientation->toEigen();
        VirtualRobot::MathTools::eigen4f2rpy(mat, rpy);
        ARMARX_INFO << "rpy in same frame: " << rpy;
 
        targetPosition->changeToGlobal(robot);
        targetOrientation->changeToGlobal(robot);
 
        target = new FramedPose(
            targetOrientation->toEigen(), targetPosition->toEigen(), armarx::GlobalFrame, "");
    }
    else if (isInputParameterSet("TcpTargetPosition"))
    {
        ikType = VirtualRobot::IKSolver::Position;
 
        FramedPositionPtr targetPosition = in.getTcpTargetPosition();
 
        targetPosition->changeToGlobal(robot);
 
        ARMARX_INFO << "Target position (Frame = " << targetPosition->getFrame() << ": "
                    << targetPosition->toEigen();
 
        target = new FramedPose(
            Eigen::Matrix3f::Identity(), targetPosition->toEigen(), armarx::GlobalFrame, "");
    }
    else if (isInputParameterSet("TcpTargetOrientation"))
    {
        ikType = VirtualRobot::IKSolver::Orientation;
 
        FramedOrientationPtr targetOrientation = in.getTcpTargetOrientation();
 
        targetOrientation->changeToGlobal(robot);
 
        ARMARX_INFO << "Target orientation (Frame = " << targetOrientation->getFrame() << ": "
                    << targetOrientation->toEigen();
 
        target = new FramedPose(
            targetOrientation->toEigen(), Eigen::Vector3f(0, 0, 0), armarx::GlobalFrame, "");
    }
    ARMARX_VERBOSE << VAROUT(*target);
    ARMARX_VERBOSE << VAROUT(robot->getGlobalPose());
    context->getDebugDrawerTopicProxy()->setPoseDebugLayerVisu("RobotPose",
                                                               new Pose(robot->getGlobalPose()));
    //    Eigen::Matrix4f globalTargetPose = target->toRootEigen(robot);
    //    robot->setGlobalPose(Eigen::Matrix4f::Identity());
 
    Eigen::Matrix4f globalTargetPose = target->toEigen();
    VirtualRobot::ConstraintPtr positionConstraint(
        new VirtualRobot::PositionConstraint(localrobot,
                                             nodeSet,
                                             nodeSet->getTCP(),
                                             globalTargetPose.block<3, 1>(0, 3),
                                             ikType,
                                             positionTolerance));
    positionConstraint->setOptimizationFunctionFactor(1);
 
    VirtualRobot::ConstraintPtr orientationConstraint(
        new VirtualRobot::OrientationConstraint(localrobot,
                                                nodeSet,
                                                nodeSet->getTCP(),
                                                globalTargetPose.block<3, 3>(0, 0),
                                                ikType,
                                                orientationTolerance));
    orientationConstraint->setOptimizationFunctionFactor(1000);
 
    //    VirtualRobot::GenericIKSolver ikSolver(robot->getRobotNodeSet(kinChainName), VirtualRobot::JacobiProvider::eSVDDamped);
    //    ikSolver.setMaximumError(positionTolerance, orientationTolerance);
    //    ikSolver.setupJacobian(jacobianStepSize, jacobianMaxLoops);
    //    ikSolver.solve(globalTargetPose, ikType, ikMaxLoops);
    // Instantiate solver and generate IK solution
    IceUtil::Time start = IceUtil::Time::now();
    VirtualRobot::ConstrainedOptimizationIK ikSolver(localrobot, nodeSet);
    ARMARX_INFO << "IK " << VAROUT(positionTolerance) << VAROUT(orientationTolerance);
    ikSolver.addConstraint(positionConstraint);
    ikSolver.addConstraint(orientationConstraint);
    ikSolver.initialize();
 
    bool ikSuccess = ikSolver.solve();
    ARMARX_INFO << "Solving IK took: " << (IceUtil::Time::now() - start).toMilliSecondsDouble()
                << " ms " << VAROUT(ikSuccess);
 
    float remainingErrorPos =
        (globalTargetPose.block<3, 1>(0, 3) - nodeSet->getTCP()->getGlobalPose().block<3, 1>(0, 3))
            .norm();
    Eigen::Matrix4f deltaMat = globalTargetPose * nodeSet->getTCP()->getGlobalPose().inverse();
    Eigen::AngleAxisf aa(deltaMat.block<3, 3>(0, 0));
    float remaingErrorOri = fabs(aa.angle());
    //    ikSuccess = (remainingErrorPos < positionTolerance && remaingErrorOri < orientationTolerance && ikType == VirtualRobot::IKSolver::All)
    //                || (ikType == VirtualRobot::IKSolver::Position && remainingErrorPos < positionTolerance)
    //                || (ikType == VirtualRobot::IKSolver::Orientation && remaingErrorOri < orientationTolerance);
    ARMARX_DEBUG << VAROUT(globalTargetPose) << "Delta: " << deltaMat;
    if (ikSuccess)
    {
        ARMARX_INFO << "Found IK solution: remaining position error " << remainingErrorPos
                    << " mm, orientation error: " << remaingErrorOri << " rad ("
                    << (remaingErrorOri * 180 / M_PI) << " deg)";
    }
    else
    {
        ARMARX_WARNING << "IK Solver failed: remaining position error " << remainingErrorPos
                       << " mm, orientation error: " << remaingErrorOri << " rad ("
                       << (remaingErrorOri * 180 / M_PI) << " deg)";
        ARMARX_DEBUG << "Reached Pose: " << nodeSet->getTCP()->getGlobalPose();
        context->getDebugDrawerTopicProxy()->setPoseDebugLayerVisu(
            "UnreachedIKPose", new Pose(target->toGlobalEigen(robot)));
        robot->setGlobalPose(robot->getGlobalPose());
        context->getDebugDrawerTopicProxy()->setPoseDebugLayerVisu(
            "BestFoundIKPose", new Pose(nodeSet->getTCP()->getGlobalPose()));
        if (in.getAlwaysExecuteBestFoundIkSolution())
        {
            ARMARX_WARNING
                << "Using failed IK anyway due to AlwaysExecuteBestFoundIkSolution == true";
        }
    }
 
    if (!ikSuccess && !in.getAlwaysExecuteBestFoundIkSolution())
    {
        emitFailure();
    }
    else
    {
        // Convert solution to state output format
        std::map<std::string, float> jointTargetValues;
        std::map<std::string, float> jointVelocities;
 
        VirtualRobot::RobotNodeSetPtr kinematicChain = localrobot->getRobotNodeSet(kinChainName);
 
        for (unsigned int i = 0; i < kinematicChain->getSize(); i++)
        {
            VirtualRobot::RobotNodePtr node = kinematicChain->getNode(i);
 
            jointTargetValues[node->getName()] = node->getJointValue();
            jointVelocities[node->getName()] = 1.0f;
        }
 
        out.setJointTargetValues(jointTargetValues);
        out.setJointVelocities(jointVelocities);
        out.setJointTargetTolerance(0.1f);
 
        emitSuccess();
    }
}
 
void
CartesianPositionControlIK::onBreak()
{
}
 
void
CartesianPositionControlIK::onExit()
{
}
 
// DO NOT EDIT NEXT FUNCTION
XMLStateFactoryBasePtr
CartesianPositionControlIK::CreateInstance(XMLStateConstructorParams stateData)
{
    return XMLStateFactoryBasePtr(new CartesianPositionControlIK(stateData));
}