franka_model_interface.h

Go to the documentation of this file.

// Copyright (c) 2017 Franka Emika GmbH
// Use of this source code is governed by the Apache-2.0 license, see LICENSE
#pragma once

#include <array>
#include <string>

#include <franka/model.h>
#include <franka/robot_state.h>
#include <hardware_interface/internal/hardware_resource_manager.h>

namespace franka_hw {

class FrankaModelHandle {
 public:
  FrankaModelHandle() = delete;

  FrankaModelHandle(const std::string& name, franka::Model& model, franka::RobotState& robot_state)
      : name_(name), model_(&model), robot_state_(&robot_state) {}

  std::string getName() const noexcept { return name_; }

  std::array<double, 49> getMass() const { return model_->mass(*robot_state_); }

  std::array<double, 49> getMass(
      const std::array<double, 7>& q,
      const std::array<double, 9>& total_inertia,
      double total_mass,
      const std::array<double, 3>& F_x_Ctotal) const {  // NOLINT (readability-identifier-naming)
    return model_->mass(q, total_inertia, total_mass, F_x_Ctotal);
  }

  std::array<double, 7> getCoriolis() const { return model_->coriolis(*robot_state_); }

  std::array<double, 7> getCoriolis(
      const std::array<double, 7>& q,
      const std::array<double, 7>& dq,
      const std::array<double, 9>& total_inertia,
      double total_mass,
      const std::array<double, 3>& F_x_Ctotal) const {  // NOLINT (readability-identifier-naming)
    return model_->coriolis(q, dq, total_inertia, total_mass, F_x_Ctotal);
  }

  std::array<double, 7> getGravity(const std::array<double, 3>& gravity_earth = {
                                       {0., 0., -9.81}}) const {
    return model_->gravity(*robot_state_, gravity_earth);
  }

  std::array<double, 7> getGravity(
      const std::array<double, 7>& q,
      double total_mass,
      const std::array<double, 3>& F_x_Ctotal,  // NOLINT (readability-identifier-naming)
      const std::array<double, 3>& gravity_earth = {{0., 0., -9.81}}) const {
    return model_->gravity(q, total_mass, F_x_Ctotal, gravity_earth);
  }

  std::array<double, 16> getPose(const franka::Frame& frame) const {
    return model_->pose(frame, *robot_state_);
  }

  std::array<double, 16> getPose(
      const 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 {
    return model_->pose(frame, q, F_T_EE, EE_T_K);
  }

  std::array<double, 42> getBodyJacobian(const franka::Frame& frame) const {
    return model_->bodyJacobian(frame, *robot_state_);
  }

  std::array<double, 42> getBodyJacobian(
      const 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 {
    return model_->bodyJacobian(frame, q, F_T_EE, EE_T_K);
  }

  std::array<double, 42> getZeroJacobian(const franka::Frame& frame) const {
    return model_->zeroJacobian(frame, *robot_state_);
  }

  std::array<double, 42> getZeroJacobian(
      const 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 {
    return model_->zeroJacobian(frame, q, F_T_EE, EE_T_K);
  }

 private:
  std::string name_;
  const franka::Model* model_;
  const franka::RobotState* robot_state_;
};

class FrankaModelInterface : public hardware_interface::HardwareResourceManager<FrankaModelHandle> {
};

}  // namespace franka_hw