Model.hpp

#pragma once
 
#include <string>
#include <vector>
#include <OsqpEigen/OsqpEigen.h>
#include <pinocchio/algorithm/crba.hpp>
#include <pinocchio/algorithm/rnea.hpp>
#include <pinocchio/multibody/data.hpp>
#include <pinocchio/parsers/urdf.hpp>
#include <state_representation/parameters/Parameter.hpp>
#include <state_representation/parameters/ParameterInterface.hpp>
#include <state_representation/space/Jacobian.hpp>
#include <state_representation/space/joint/JointState.hpp>
#include <state_representation/space/cartesian/CartesianState.hpp>
 
using namespace std::chrono_literals;
 
namespace robot_model {
struct InverseKinematicsParameters {
  double damp = 1e-6;
  double alpha = 0.5;
  double gamma = 0.8;
  double margin = 0.07;
  double tolerance = 1e-3;
  unsigned int max_number_of_iterations = 1000;
};
 
struct QPInverseVelocityParameters {
  double alpha = 0.1;
  double proportional_gain = 1.0;
  double linear_velocity_limit = 2.0;
  double angular_velocity_limit = 2.0;
  std::chrono::nanoseconds dt = 1000ns;
};
 
class Model {
private:
  // @format:off
  std::shared_ptr<state_representation::Parameter<std::string>> robot_name_;
  std::shared_ptr<state_representation::Parameter<std::string>> urdf_path_; 
  std::vector<std::string> frames_;                                         
  pinocchio::Model robot_model_;                                            
  pinocchio::Data robot_data_;                                              
  OsqpEigen::Solver solver_;                                                
  Eigen::SparseMatrix<double> hessian_;                                     
  Eigen::VectorXd gradient_;                                                
  Eigen::SparseMatrix<double> constraint_matrix_;                           
  Eigen::VectorXd lower_bound_constraints_;                                 
  Eigen::VectorXd upper_bound_constraints_;                                 
  // @format:on
  void init_model();
 
  bool init_qp_solver();
 
  std::vector<unsigned int> get_frame_ids(const std::vector<std::string>& frames);
 
  unsigned int get_frame_id(const std::string& frame);
 
  state_representation::Jacobian compute_jacobian(const state_representation::JointPositions& joint_positions,
                                                  unsigned int frame_id);
 
  Eigen::MatrixXd compute_jacobian_time_derivative(const state_representation::JointPositions& joint_positions,
                                                   const state_representation::JointVelocities& joint_velocities,
                                                   unsigned int frame_id);
 
  std::vector<state_representation::CartesianPose> forward_kinematics(const state_representation::JointPositions& joint_positions,
                                                                      const std::vector<unsigned int>& frame_ids);
 
  state_representation::CartesianPose forward_kinematics(const state_representation::JointPositions& joint_positions,
                                                         unsigned int frame_id);
 
  static bool in_range(const Eigen::VectorXd& vector,
                       const Eigen::VectorXd& lower_limits,
                       const Eigen::VectorXd& upper_limits);
 
  static Eigen::VectorXd clamp_in_range(const Eigen::VectorXd& vector,
                                        const Eigen::VectorXd& lower_limits,
                                        const Eigen::VectorXd& upper_limits);
 
  Eigen::MatrixXd cwln_weighted_matrix(const state_representation::JointPositions& joint_positions, double margin);
 
  Eigen::VectorXd cwln_repulsive_potential_field(const state_representation::JointPositions& joint_positions,
                                                 double margin);
 
  void check_inverse_velocity_arguments(const std::vector<state_representation::CartesianTwist>& cartesian_twists,
                                        const state_representation::JointPositions& joint_positions,
                                        const std::vector<std::string>& frames);
 
public:
  explicit Model(const std::string& robot_name, const std::string& urdf_path);
 
  Model(const Model& model);
 
  friend void swap(Model& model1, Model& model2);
 
  Model& operator=(const Model& Model);
 
  static bool create_urdf_from_string(const std::string& urdf_string, const std::string& desired_path);
 
  const std::string& get_robot_name() const;
 
  void set_robot_name(const std::string& robot_name);
 
  const std::string& get_urdf_path() const;
 
  unsigned int get_number_of_joints() const;
 
  std::vector<std::string> get_joint_frames() const;
 
  std::vector<std::string> get_frames() const;
 
  const std::string& get_base_frame() const;
 
  Eigen::Vector3d get_gravity_vector() const;
 
  void set_gravity_vector(const Eigen::Vector3d& gravity);
 
  const pinocchio::Model& get_pinocchio_model() const;
 
  state_representation::Jacobian compute_jacobian(const state_representation::JointPositions& joint_positions,
                                                  const std::string& frame = "");
 
  Eigen::MatrixXd compute_jacobian_time_derivative(const state_representation::JointPositions& joint_positions,
                                                   const state_representation::JointVelocities& joint_velocities,
                                                   const std::string& frame = "");
 
  Eigen::MatrixXd compute_inertia_matrix(const state_representation::JointPositions& joint_positions);
 
  state_representation::JointTorques compute_inertia_torques(const state_representation::JointState& joint_state);
 
  Eigen::MatrixXd compute_coriolis_matrix(const state_representation::JointState& joint_state);
 
  state_representation::JointTorques compute_coriolis_torques(const state_representation::JointState& joint_state);
 
  state_representation::JointTorques compute_gravity_torques(const state_representation::JointPositions& joint_positions);
 
  std::vector<state_representation::CartesianPose> forward_kinematics(const state_representation::JointPositions& joint_positions,
                                                                      const std::vector<std::string>& frames);
 
  state_representation::CartesianPose forward_kinematics(const state_representation::JointPositions& joint_positions,
                                                         const std::string& frame = "");
 
  state_representation::JointPositions inverse_kinematics(const state_representation::CartesianPose& cartesian_pose,
                                                          const InverseKinematicsParameters& parameters = InverseKinematicsParameters(),
                                                          const std::string& frame = "");
 
  state_representation::JointPositions inverse_kinematics(const state_representation::CartesianPose& cartesian_pose,
                                                          const state_representation::JointPositions& joint_positions,
                                                          const InverseKinematicsParameters& parameters = InverseKinematicsParameters(),
                                                          const std::string& frame = "");
 
  std::vector<state_representation::CartesianTwist> forward_velocity(const state_representation::JointState& joint_state,
                                                                     const std::vector<std::string>& frames);
 
  state_representation::CartesianTwist forward_velocity(const state_representation::JointState& joint_state,
                                                        const std::string& frame = "");
 
  state_representation::JointVelocities inverse_velocity(const std::vector<state_representation::CartesianTwist>& cartesian_twists,
                                                         const state_representation::JointPositions& joint_positions,
                                                         const std::vector<std::string>& frames);
 
  state_representation::JointVelocities inverse_velocity(const state_representation::CartesianTwist& cartesian_twist,
                                                         const state_representation::JointPositions& joint_positions,
                                                         const std::string& frame = "");
 
  state_representation::JointVelocities inverse_velocity(const std::vector<state_representation::CartesianTwist>& cartesian_twists,
                                                         const state_representation::JointPositions& joint_positions,
                                                         const QPInverseVelocityParameters& parameters,
                                                         const std::vector<std::string>& frames);
 
  state_representation::JointVelocities inverse_velocity(const state_representation::CartesianTwist& cartesian_twist,
                                                         const state_representation::JointPositions& joint_positions,
                                                         const QPInverseVelocityParameters& parameters,
                                                         const std::string& frame = "");
 
  void print_qp_problem();
 
  bool in_range(const state_representation::JointPositions& joint_positions) const;
 
  bool in_range(const state_representation::JointVelocities& joint_velocities) const;
 
  bool in_range(const state_representation::JointTorques& joint_torques) const;
 
  bool in_range(const state_representation::JointState& joint_state) const;
 
  state_representation::JointState clamp_in_range(const state_representation::JointState& joint_state) const;
};
 
inline void swap(Model& model1, Model& model2) {
  std::swap(model1.robot_name_, model2.robot_name_);
  std::swap(model1.urdf_path_, model2.urdf_path_);
  // initialize both models
  model1.init_model();
  model2.init_model();
}
 
inline Model& Model::operator=(const Model& model) {
  Model tmp(model);
  swap(*this, tmp);
  return *this;
}
 
 
inline const std::string& Model::get_robot_name() const {
  return this->robot_name_->get_value();
}
 
inline void Model::set_robot_name(const std::string& robot_name) {
  this->robot_name_->set_value(robot_name);
}
 
inline const std::string& Model::get_urdf_path() const {
  return this->urdf_path_->get_value();
}
 
inline unsigned int Model::get_number_of_joints() const {
  return this->robot_model_.nq;
}
 
inline std::vector<std::string> Model::get_joint_frames() const {
  // model contains a first joint called universe that needs to be discarded
  std::vector<std::string> joint_frames(this->robot_model_.names.begin() + 1, this->robot_model_.names.end());
  return joint_frames;
}
 
inline const std::string& Model::get_base_frame() const {
  return this->frames_.front();
}
 
inline std::vector<std::string> Model::get_frames() const {
  return this->frames_;
}
 
inline Eigen::Vector3d Model::get_gravity_vector() const {
  return this->robot_model_.gravity.linear();
}
 
inline void Model::set_gravity_vector(const Eigen::Vector3d& gravity) {
  this->robot_model_.gravity.linear(gravity);
}
 
inline const pinocchio::Model& Model::get_pinocchio_model() const {
  return this->robot_model_;
}
}// namespace robot_model