model.hpp

Go to the documentation of this file.

 
#ifndef FLEXIV_RDK_MODEL_HPP_
#define FLEXIV_RDK_MODEL_HPP_
 
#include "robot.hpp"
#include <Eigen/Eigen>
#include <memory>
 
namespace flexiv::rdk {
 
struct IKParams
{
    std::array<double, kPoseSize> cartesian_pose = {};
 
    std::vector<double> seed_q = {};
 
    bool free_orientation = false;
};
 
struct IKResult
{
    bool success = false;
 
    std::map<JointGroup, std::vector<double>> solved_q = {};
};
 
class Model
{
public:
    Model(const Robot& robot,
        const Eigen::Vector3d& gravity_vector = Eigen::Vector3d(0.0, 0.0, -9.81));
    virtual ~Model();
 
    std::vector<std::string> link_names() const;
 
    std::vector<std::string> joint_names() const;
 
    void Reload();
 
    void Update(const std::vector<double>& full_q, const std::vector<double>& full_dq);
 
    //========================================== DYNAMICS ==========================================
    Eigen::MatrixXd dJ(const std::string& link_name);
 
    Eigen::MatrixXd M();
 
    Eigen::MatrixXd C();
 
    Eigen::VectorXd g();
 
    Eigen::VectorXd c();
 
    //========================================= KINEMATICS =========================================
    Eigen::MatrixXd J(const std::string& link_name);
 
    Eigen::Isometry3d T(const std::string& link_name);
 
    void SyncURDF(const std::string& template_urdf_path);
 
    size_t SyncKinematicsYAML(const std::string& template_yaml_path);
 
    IKResult SolveConstrainedIK(const std::map<JointGroup, IKParams>& ik_params_by_group);
 
    std::map<JointGroup, std::pair<double, double>> configuration_score() const;
 
    //======================================= MULTI-CONTACT ========================================
    std::map<JointGroup, std::vector<Eigen::Vector3d>> multi_contact_forces() const;
 
    std::map<JointGroup, std::vector<Eigen::Vector3d>> multi_contact_positions() const;
 
private:
    class Impl;
    std::unique_ptr<Impl> pimpl_;
};
 
} /* namespace flexiv::rdk */
 
#endif /* FLEXIV_RDK_MODEL_HPP_ */