robot_env_evaluator.hpp

Go to the documentation of this file.

 
#ifndef ROBOT_ENV_EVALUATOR_ROBOT_ENV_EVALUATOR_HPP
#define ROBOT_ENV_EVALUATOR_ROBOT_ENV_EVALUATOR_HPP
 
#include <pinocchio/multibody/model.hpp>
#include <pinocchio/multibody/data.hpp>
#include <pinocchio/multibody/geometry.hpp>
 
#include <vector>
#include <string>
#include <variant>
 
namespace robot_env_evaluator
{
    using Obstacle = std::variant<coal::Sphere>;
 
    struct obstacleInput{
        Obstacle obstacle;
        Eigen::Matrix4d obstacle_pose;
    };
 
    struct distanceResult{
        int id;
        double distance;
        Eigen::Vector3d normal_vector;
        Eigen::Vector3d nearest_point_on_robot;
        Eigen::Vector3d nearest_point_on_object;
        std::string     contacted_robot_link;
        Eigen::VectorXd projector_jointspace_to_dist;
        Eigen::VectorXd projector_dist_to_jointspace;
    };
 
    class RobotEnvEvaluator{
    public:
        RobotEnvEvaluator(const pinocchio::Model& model,
                          const std::string& ee_name,
                          const std::vector<std::string>& joint_names,
                          const pinocchio::GeometryModel& collision_model,
                          const pinocchio::GeometryModel& visual_model = pinocchio::GeometryModel());
        
        ~RobotEnvEvaluator() = default;
 
        void forwardKinematics(const Eigen::VectorXd& q,
                                     Eigen::Matrix4d& T,
                               const int joint_index = -1);
        
        void forwardKinematicsByFrameName(const Eigen::VectorXd& q,
                                                Eigen::Matrix4d& T,
                                          const std::string& frame_name);
 
        void jacobian(const Eigen::VectorXd& q,
                            Eigen::MatrixXd& J,
                      const int joint_index = -1);
 
        void jacobianByFrameName(const Eigen::VectorXd& q,
                                       Eigen::MatrixXd& J,
                                 const std::string& frame_name);
 
        void computeDistances(const Eigen::VectorXd& q,
                              const std::vector<obstacleInput>& obstacles, 
                                    std::vector<distanceResult>& distances);
 
        // configurations
        bool calculate_self_collision_ = false;                          
        bool projector_dist_to_control_enable_ = false;                  
        bool projector_dist_to_control_with_zero_orientation_  = false;  
        double robust_pinv_lambda_ = 0.001;                              
 
    protected:
        void jacobianFrame(const Eigen::VectorXd& q,
                           const double frame_index,
                                 Eigen::MatrixXd& J);
 
        void computeModelData(const Eigen::VectorXd& q);
 
        // buffers
        Eigen::VectorXd buffered_q_;                 
 
        pinocchio::Model model_;                     
        pinocchio::Data data_;                       
        pinocchio::GeometryModel collision_model_;   
        pinocchio::GeometryModel visual_model_;      
        int ee_index_;                               
        std::vector<double> joint_indices_;          
 
        pinocchio::GeometryModel geom_model_;        
        pinocchio::GeometryData geom_data_;          
    };
} // namespace robot_env_evaluator
 
#endif // ROBOT_ENV_EVALUATOR_ROBOT_ENV_EVALUATOR_HPP