model_base.h

Go to the documentation of this file.

#pragma once
 
#include <franka/model.h>
#include <franka/robot_state.h>
 
namespace franka_hw {
 
class ModelBase {
 public:
  virtual ~ModelBase() noexcept = default;
 
  std::array<double, 16> pose(franka::Frame frame, const franka::RobotState& robot_state) const {
    return pose(frame, robot_state.q, robot_state.F_T_EE, robot_state.EE_T_K);
  }
 
  virtual std::array<double, 16> pose(
      franka::Frame frame,
      const std::array<double, 7>& q,
      const std::array<double, 16>& F_T_EE,  // NOLINT(readability-identifier-naming)
      const std::array<double, 16>& EE_T_K)  // NOLINT(readability-identifier-naming)
      const = 0;
 
  std::array<double, 42> bodyJacobian(franka::Frame frame,
                                      const franka::RobotState& robot_state) const {
    return bodyJacobian(frame, robot_state.q, robot_state.F_T_EE, robot_state.EE_T_K);
  }
 
  virtual std::array<double, 42> bodyJacobian(
      franka::Frame frame,
      const std::array<double, 7>& q,
      const std::array<double, 16>& F_T_EE,  // NOLINT(readability-identifier-naming)
      const std::array<double, 16>& EE_T_K)  // NOLINT(readability-identifier-naming)
      const = 0;
 
  std::array<double, 42> zeroJacobian(franka::Frame frame,
                                      const franka::RobotState& robot_state) const {
    return zeroJacobian(frame, robot_state.q, robot_state.F_T_EE, robot_state.EE_T_K);
  }
 
  virtual std::array<double, 42> zeroJacobian(
      franka::Frame frame,
      const std::array<double, 7>& q,
      const std::array<double, 16>& F_T_EE,  // NOLINT(readability-identifier-naming)
      const std::array<double, 16>& EE_T_K)  // NOLINT(readability-identifier-naming)
      const = 0;
 
  std::array<double, 49> mass(const franka::RobotState& robot_state) const {
    return mass(robot_state.q, robot_state.I_total, robot_state.m_total, robot_state.F_x_Ctotal);
  }
 
  virtual std::array<double, 49> mass(
      const std::array<double, 7>& q,
      const std::array<double, 9>& I_total,  // NOLINT(readability-identifier-naming)
      double m_total,
      const std::array<double, 3>& F_x_Ctotal)  // NOLINT(readability-identifier-naming)
      const = 0;
 
  std::array<double, 7> coriolis(const franka::RobotState& robot_state) const {
    return coriolis(robot_state.q, robot_state.dq, robot_state.I_total, robot_state.m_total,
                    robot_state.F_x_Ctotal);
  }
 
  virtual std::array<double, 7> coriolis(
      const std::array<double, 7>& q,
      const std::array<double, 7>& dq,
      const std::array<double, 9>& I_total,  // NOLINT(readability-identifier-naming)
      double m_total,
      const std::array<double, 3>& F_x_Ctotal)  // NOLINT(readability-identifier-naming)
      const = 0;
 
  std::array<double, 7> gravity(
      const std::array<double, 7>& q,
      double m_total,
      const std::array<double, 3>& F_x_Ctotal  // NOLINT(readability-identifier-naming)
      ) const {
    return gravity(q, m_total, F_x_Ctotal, {0, 0, -9.81});
  }
 
  virtual std::array<double, 7> gravity(
      const std::array<double, 7>& q,
      double m_total,
      const std::array<double, 3>& F_x_Ctotal,  // NOLINT(readability-identifier-naming)
      const std::array<double, 3>& gravity_earth) const = 0;
 
  std::array<double, 7> gravity(const franka::RobotState& robot_state) const {
#ifdef ENABLE_BASE_ACCELERATION
    return gravity(robot_state.q, robot_state.m_total, robot_state.F_x_Ctotal, robot_state.O_ddP_O);
#else
    return gravity(robot_state.q, robot_state.m_total, robot_state.F_x_Ctotal, {0, 0, -9.81});
#endif
  }
 
  std::array<double, 7> gravity(const franka::RobotState& robot_state,
                                const std::array<double, 3>& gravity_earth) const {
    return gravity(robot_state.q, robot_state.m_total, robot_state.F_x_Ctotal, gravity_earth);
  }
};
 
}  // namespace franka_hw