model_kdl.h

Go to the documentation of this file.

#pragma once

#include <franka/robot_state.h>
#include <franka_hw/model_base.h>
#include <urdf/model.h>
#include <array>
#include <kdl/chaindynparam.hpp>
#include <kdl/chainfksolver.hpp>
#include <kdl/chainjnttojacsolver.hpp>
#include <memory>
#include <string>

namespace franka_gazebo {

class ModelKDL : public franka_hw::ModelBase {
 public:
  ModelKDL(const urdf::Model& model,
           const std::string& root,
           const std::string& tip,
           double singularity_threshold = -1);

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

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

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

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

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

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

  static void augmentFrame(const std::string& name,
                           const std::array<double, 16>& transform,
                           KDL::Chain& chain);

  static void augmentFrame(const std::string& name,
                           const std::array<double, 3>& center_of_mass,
                           double mass,
                           const std::array<double, 9>& inertia,
                           KDL::Chain& chain);

 private:
  static int segment(franka::Frame frame);
  static std::string strError(const int error);
  bool isCloseToSingularity(const KDL::Jacobian& jacobian) const;

  KDL::Chain chain_;
  double singularity_threshold_;
};

}  // namespace franka_gazebo