model.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 <memory>

#include <franka/robot.h>
#include <franka/robot_state.h>

namespace franka {

enum class Frame {
  kJoint1,
  kJoint2,
  kJoint3,
  kJoint4,
  kJoint5,
  kJoint6,
  kJoint7,
  kFlange,
  kEndEffector,
  kStiffness
};

Frame operator++(Frame& frame, int /* dummy */) noexcept;

class ModelLibrary;
class Network;

class Model {
 public:
  explicit Model(franka::Network& network);

  Model(Model&& model) noexcept;

  Model& operator=(Model&& model) noexcept;

  ~Model() noexcept;

  std::array<double, 16> pose(Frame frame, const franka::RobotState& robot_state) const;

  std::array<double, 16> pose(
      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;

  std::array<double, 42> bodyJacobian(Frame frame, const franka::RobotState& robot_state) const;

  std::array<double, 42> bodyJacobian(
      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;

  std::array<double, 42> zeroJacobian(Frame frame, const franka::RobotState& robot_state) const;

  std::array<double, 42> zeroJacobian(
      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;

  std::array<double, 49> mass(const franka::RobotState& robot_state) const noexcept;

  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 noexcept;

  std::array<double, 7> coriolis(const franka::RobotState& robot_state) const noexcept;

  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 noexcept;

  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 = {{0., 0., -9.81}}) const noexcept;

  std::array<double, 7> gravity(const franka::RobotState& robot_state,
                                const std::array<double, 3>& gravity_earth = {
                                    {0., 0., -9.81}}) const noexcept;

  Model(const Model&) = delete;
  Model& operator=(const Model&) = delete;

 private:
  std::unique_ptr<ModelLibrary> library_;
};

}  // namespace franka