SimpleGridReachability.cpp

Go to the documentation of this file.

#include "SimpleGridReachability.h"
 
#include "CartesianVelocityController.h"
 
namespace armarx
{
    SimpleGridReachability::Result
    SimpleGridReachability::calculateDiffIK(const Eigen::Matrix4f targetPose,
                                            const Parameters& params)
    {
        VirtualRobot::RobotNodePtr rns = params.nodeSet;
        VirtualRobot::RobotNodeSetPtr tcp = params.tcp ? params.tcp : rns->getTCP();
        CartesianVelocityController velocityController(rns);
        CartesianPositionController positionController(tcp);
 
        Eigen::VectorXf initialJV = rns->getJointValuesEigen();
        for (size_t i = 0; i <= params.stepsInitial + params.stepsFineTune; i++)
        {
            Eigen::Vector3f posDiff = positionController.getPositionDiff(targetPose);
            Eigen::Vector3f oriDiff = positionController.getOrientationDiff(targetPose);
 
            //ARMARX_IMPORTANT << VAROUT(posDiff) << VAROUT(oriDiff);
 
            Eigen::VectorXf cartesialVel(6);
            cartesialVel << posDiff(0), posDiff(1), posDiff(2), oriDiff(0), oriDiff(1), oriDiff(2);
            Eigen::VectorXf jnv =
                params.jointLimitAvoidanceKp * velocityController.calculateJointLimitAvoidance();
            Eigen::VectorXf jv =
                velocityController.calculate(cartesialVel, jnv, VirtualRobot::IKSolver::All);
 
 
            float stepLength =
                i < params.stepsInitial ? params.ikStepLengthInitial : params.ikStepLengthFineTune;
            jv = jv * stepLength;
 
            for (size_t n = 0; n < rns->getSize(); n++)
            {
                rns->getNode(n)->setJointValue(rns->getNode(n)->getJointValue() + jv(n));
            }
        }
 
        Result result;
        result.jointValues = rns->getJointValuesEigen();
        result.posError = positionController.getPositionDiff(targetPose);
        result.oriError = positionController.getOrientationError(targetPose);
        result.reached =
            result.posError < params.maxPosError && result.oriError < params.maxOriError;
 
        return result;
    }
} // namespace armarx