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>
#include "Qontrol/Problem/ReferenceFrame.hpp"
 
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);
    }
  };
 
  enum ModelImpl
  {
    PINOCCHIO,
    KDL
  };
  namespace Model
  {
 
    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 getJointFrictionAndDamping() -> Eigen::VectorXd = 0;
      void setVelocityGain(Eigen::VectorXd v);
 
      auto getVelocityGain() -> Eigen::VectorXd;
 
      auto getVelocityGainDiag() -> Eigen::MatrixXd;
 
      auto getVelocityGainDiagInv() -> Eigen::MatrixXd;
 
      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 getJacobian(const std::string &body_name, ReferenceFrame ref_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 getJacobianTimeDerivativeQdot(const std::string &body_name, ReferenceFrame ref_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;
 
      Eigen::VectorXd velocity_gains;
      Eigen::MatrixXd velocity_gains_diag;
      Eigen::MatrixXd velocity_gains_diag_inv;
      // Private methods
    private:
    };
 
    template <ModelImpl model_lib>
    class RobotModel
    {
    };
 
  } // namespace Model
} // namespace Qontrol