SimpleGridReachability.h

Go to the documentation of this file.

#pragma once
#include "CartesianPositionController.h"
 
namespace armarx
{
    class SimpleGridReachability
    {
    public:
        struct Parameters
        {
            VirtualRobot::RobotNodePtr tcp;
            VirtualRobot::RobotNodeSetPtr nodeSet;
 
            // IK params
            float ikStepLengthInitial = 0.2f;
            float ikStepLengthFineTune = 0.5f;
            size_t stepsInitial = 25;
            size_t stepsFineTune = 5;
            float maxPosError = 10.f;
            float maxOriError = 0.05f;
            float jointLimitAvoidanceKp = 2.0f;
        };
 
        struct Result
        {
            Eigen::VectorXf jointValues;
            float posError;
            float oriError;
            bool reached;
        };
 
        static Result CalculateDiffIK(const Eigen::Matrix4f targetPose, const Parameters& params);
    };
 
} // namespace armarx