Program Listing for File model.hpp

↰ Return to documentation for file (include/franka_hardware/model.hpp)

// Copyright (c) 2023 Franka Robotics GmbH
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#pragma once

#include <array>

#include <franka/model.h>

namespace franka_hardware {

class Model {  // NOLINT(cppcoreguidelines-pro-type-member-init,
               // cppcoreguidelines-special-member-functions)
 public:
  explicit Model(franka::Model* model) : model_(model) {}
  virtual ~Model() = default;

  [[nodiscard]] std::array<double, 16> pose(
      franka::Frame frame,
      const std::array<double, 7>& q,        // NOLINT(readability-identifier-length)
      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 {
    return model_->pose(frame, q, F_T_EE, EE_T_K);
  }

  [[nodiscard]] std::array<double, 42> bodyJacobian(
      franka::Frame frame,
      const std::array<double, 7>& q,        // NOLINT(readability-identifier-length)
      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 {
    return model_->bodyJacobian(frame, q, F_T_EE, EE_T_K);
  }

  [[nodiscard]] std::array<double, 42> zeroJacobian(
      franka::Frame frame,
      const std::array<double, 7>& q,        // NOLINT(readability-identifier-length)
      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 {
    return model_->zeroJacobian(frame, q, F_T_EE, EE_T_K);
  }

  [[nodiscard]] std::array<double, 49> mass(
      const std::array<double, 7>& q,        // NOLINT(readability-identifier-length)
      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 noexcept {
    return model_->mass(q, I_total, m_total, F_x_Ctotal);
  }

  [[nodiscard]] std::array<double, 7> coriolis(
      const std::array<double, 7>& q,        // NOLINT(readability-identifier-length)
      const std::array<double, 7>& dq,       // NOLINT(readability-identifier-length)
      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 noexcept {
    return model_->coriolis(q, dq, I_total, m_total, F_x_Ctotal);
  }

  [[nodiscard]] std::array<double, 7> gravity(
      const std::array<double, 7>& q,  // NOLINT(readability-identifier-length)
      double m_total,
      const std::array<double, 3>& F_x_Ctotal,  // NOLINT(readability-identifier-naming)
      const std::array<double, 3>& gravity_earth) const noexcept {
    return model_->gravity(q, m_total, F_x_Ctotal, gravity_earth);
  }
  [[nodiscard]] virtual 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);
  }

  [[nodiscard]] virtual 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);
  }

  [[nodiscard]] virtual 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);
  }

  [[nodiscard]] virtual 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);
  }

  [[nodiscard]] virtual 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);
  }

  [[nodiscard]] std::array<double, 7> gravity(
      const std::array<double, 7>& q,  // NOLINT(readability-identifier-length)
      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});
  }

  [[nodiscard]] virtual 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);
  }

  [[nodiscard]] virtual 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
  }

 protected:
  Model() = default;

 private:
  franka::Model* model_;
};

}  // namespace franka_hardware