GenericModel.hpp

// This file is part of Qontrol, a quadratic optimization library to
// control robot.
//
// Copyright (C) 2023 Lucas Joseph <lucas.joseph@inria.fr>
//
// Qontrol is free software; you can redistribute it and/or
// modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation; either
// version 3 of the License, or (at your option) any later version.
//
// Alternatively, you can redistribute it and/or
// modify it under the terms of the GNU General Public License as
// published by the Free Software Foundation; either version 3 of
// the License, or (at your option) any later version.
//
// Qontrol is distributed in the hope that it will be useful, but WITHOUT ANY
// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
// FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License or the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public
// License and a copy of the GNU General Public License along with
// Qontrol. If not, see <http://www.gnu.org/licenses/>.
 
#pragma once
#include "Qontrol/Utils/Log.hpp"
#include "Qontrol/Utils/Exception.hpp"
#include <Eigen/Dense>
namespace Qontrol {
 
class RobotState {
public:
  Eigen::VectorXd joint_position;
  Eigen::VectorXd joint_velocity;
  Eigen::VectorXd joint_acceleration;
  Eigen::VectorXd joint_torque;
  Eigen::VectorXd external_joint_torque;
 
  void resize(int size) {
    joint_position.resize(size);
    joint_velocity.resize(size);
    joint_acceleration.resize(size);
    joint_torque.resize(size);
    external_joint_torque.resize(size);
  }
};
 
namespace Model {
 
enum RobotModelImplType { PINOCCHIO, KDL };
 
class GenericModel {
private:
  std::string robot_name_;
  RobotState robot_state_;
 
protected:
  GenericModel(){};
  // Public methods
  ~GenericModel(){};
public:
  // static GenericModel loadModelFromFile(const std::string &urdf_path,
  //                                  std::string root_link = "",
  //                                  std::string tip_link = "") ;
 
  // static GenericModel loadModelFromString(const std::string &path,
  //                                  std::string root_link = "",
  //                                  std::string tip_link = "");
 
  // virtual bool loadFromString(const std::string &path,
  //                                  std::string root_link = "",
  //                                  std::string tip_link = "");
 
  // virtual bool loadFromFile(const std::string &path,
  //                                  std::string root_link = "",
  //                                  std::string tip_link = "");
 
  void setRobotState(const RobotState &robot_state);
 
  auto getRobotState() -> RobotState;
 
  virtual auto getTipFrameName() -> std::string = 0;
 
  virtual auto getRootFrameName() -> std::string = 0;
 
  virtual auto getNrOfDegreesOfFreedom() -> int = 0;
 
  virtual void setJointPosition(Eigen::VectorXd joint_position);
 
  virtual auto getJointPosition() -> Eigen::VectorXd;
 
  void setLowerJointPositionLimits(Eigen::VectorXd lim);
 
  void setUpperJointPositionLimits(Eigen::VectorXd lim);
 
  virtual auto getLowerJointPositionLimits() -> Eigen::VectorXd;
 
  virtual auto getUpperJointPositionLimits() -> Eigen::VectorXd;
 
  virtual void setJointVelocity(Eigen::VectorXd joint_velocity);
 
  virtual auto getJointVelocity() -> Eigen::VectorXd;
 
  void setJointVelocityLimits(Eigen::VectorXd lim);
 
  auto getJointVelocityLimits() -> Eigen::VectorXd;
 
  virtual void setJointAcceleration(Eigen::VectorXd joint_acceleration);
 
  virtual auto getJointAcceleration() -> Eigen::VectorXd;
 
  virtual void setJointTorque(Eigen::VectorXd joint_torque);
 
  virtual auto getJointTorque() -> Eigen::VectorXd;
 
  void setJointTorqueLimits(Eigen::VectorXd lim);
 
  auto getJointTorqueLimits() -> Eigen::VectorXd;
 
  virtual void setExternalJointTorque(Eigen::VectorXd external_joint_torque);
 
  virtual auto getExternalJointTorque() -> Eigen::VectorXd;
 
  virtual auto getJointGravityTorques() -> Eigen::VectorXd = 0;
 
  virtual auto getJointCoriolisTorques() -> Eigen::VectorXd = 0;
 
  virtual auto getJointInertiaMatrix() -> Eigen::MatrixXd = 0;
 
  virtual auto getInverseJointInertiaMatrix() -> Eigen::MatrixXd = 0;
 
  virtual auto getFramePose(const std::string &body_name)
      -> Eigen::Isometry3d = 0;
 
  virtual auto getTipFramePose() -> Eigen::Isometry3d = 0;
 
  virtual auto getJacobian(const std::string &body_name) -> Eigen::MatrixXd = 0;
 
  virtual auto getJacobian(const std::string &body_name,
                           const Eigen::Isometry3d &frame)
      -> Eigen::MatrixXd = 0;
 
  virtual auto getJacobianTimeDerivativeQdot(const std::string &body_name)
      -> Eigen::VectorXd = 0;
 
  virtual auto getJacobianTimeDerivativeQdot(const std::string &body_name,
                                             const Eigen::Isometry3d &frame)
      -> Eigen::VectorXd = 0;
 
  virtual auto getFrameVelocity(const std::string &body_name)
      -> Eigen::Matrix<double, 6, 1> = 0;
 
  virtual auto getFrameVelocity(const std::string &body_name,
                                const Eigen::Isometry3d &frame)
      -> Eigen::Matrix<double, 6, 1> = 0;
 
  virtual auto getTipFrameVelocity() -> Eigen::Matrix<double, 6, 1> = 0;
 
  virtual auto getJointAccelerationFromTorques(Eigen::VectorXd torque) -> Eigen::VectorXd = 0;
 
  virtual auto
  getFrameAccelerationFromJointAcceleration(const std::string &body_name)
      -> Eigen::Matrix<double, 6, 1> = 0;
 
  virtual auto
  getFrameAccelerationFromJointAcceleration(const std::string &body_name,
                                            const Eigen::Isometry3d &frame)
      -> Eigen::Matrix<double, 6, 1> = 0;
 
  virtual auto getFrameAccelerationFromTorques(const std::string &body_name)
      -> Eigen::Matrix<double, 6, 1> = 0;
 
  virtual auto getFrameAccelerationFromTorques(const std::string &body_name,
                                               const Eigen::Isometry3d &frame)
      -> Eigen::Matrix<double, 6, 1> = 0;
 
  // virtual Eigen::Matrix<double, 6, 1> getTipFrameAcceleration() = 0;
 
  virtual auto getExternalWrench(const std::string &body_name)
      -> Eigen::Matrix<double, 6, 1> = 0;
 
  virtual auto getExternalWrench(const std::string &body_name,
                                 const Eigen::Isometry3d &frame)
      -> Eigen::Matrix<double, 6, 1> = 0;
 
  static auto skew(Eigen::Vector3d vector) -> Eigen::Matrix3d;
 
  auto integrate(Eigen::VectorXd q,Eigen::VectorXd v,double dt) -> Eigen::VectorXd;
  // Protected methods
protected:
  Eigen::VectorXd lower_joint_position_limits;
  Eigen::VectorXd upper_joint_position_limits;
  Eigen::VectorXd joint_velocity_limits;
  Eigen::VectorXd joint_torque_limits;
  // Private methods
private:
};
 
template <RobotModelImplType model_lib> class RobotModel {};
 
} // namespace Model
} // namespace Qontrol