Program Listing for File robot_model.h

↰ Return to documentation for file (include/franka/robot_model.h)

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

#include <array>
#include <string>

#include <pinocchio/multibody/data.hpp>
#include <pinocchio/multibody/model.hpp>

#include "franka/robot_model_base.h"

namespace franka {

class RobotModel : public RobotModelBase {
 public:
  RobotModel(const std::string& urdf);

  void 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,
                double m_total,
                const std::array<double, 3>& f_x_ctotal,
                std::array<double, 7>& c_ne) override;

  void 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,
                double m_total,
                const std::array<double, 3>& f_x_ctotal,
                const std::array<double, 3>& g_earth,
                std::array<double, 7>& c_ne) override;

  void gravity(const std::array<double, 7>& q,  // NOLINT(readability-identifier-length)
               const std::array<double, 3>& g_earth,
               double m_total,
               const std::array<double, 3>& f_x_ctotal,
               std::array<double, 7>& g_ne) override;

  void mass(const std::array<double, 7>& q,  // NOLINT(readability-identifier-length)
            const std::array<double, 9>& i_total,
            double m_total,
            const std::array<double, 3>& f_x_ctotal,
            std::array<double, 49>& m_ne) override;
  std::array<double, 16> pose(
      const std::array<double, 7>& q,  // NOLINT(readability-identifier-length)
      int joint_index) override;
  std::array<double, 16> poseEe(
      const std::array<double, 7>& q,  // NOLINT(readability-identifier-length)
      const std::array<double, 16>& f_t_ee) override;
  std::array<double, 16> poseFlange(
      const std::array<double, 7>& q) override;  // NOLINT(readability-identifier-length)
  std::array<double, 16> poseStiffness(
      const std::array<double, 7>& q,  // NOLINT(readability-identifier-length)
      const std::array<double, 16>& f_t_ee,
      const std::array<double, 16>& ee_t_k) override;
  std::array<double, 42> bodyJacobian(
      const std::array<double, 7>& q,  // NOLINT(readability-identifier-length)
      int joint_index) override;
  std::array<double, 42> bodyJacobianFlange(
      const std::array<double, 7>& q) override;  // NOLINT(readability-identifier-length)
  std::array<double, 42> bodyJacobianEe(
      const std::array<double, 7>& q,  // NOLINT(readability-identifier-length)
      const std::array<double, 16>& f_t_ee) override;
  std::array<double, 42> bodyJacobianStiffness(
      const std::array<double, 7>& q,  // NOLINT(readability-identifier-length)
      const std::array<double, 16>& f_t_ee,
      const std::array<double, 16>& ee_t_k) override;
  std::array<double, 42> zeroJacobian(
      const std::array<double, 7>& q,  // NOLINT(readability-identifier-length)
      int joint_index) override;
  std::array<double, 42> zeroJacobianFlange(
      const std::array<double, 7>& q) override;  // NOLINT(readability-identifier-length)
  std::array<double, 42> zeroJacobianEe(
      const std::array<double, 7>& q,  // NOLINT(readability-identifier-length)
      const std::array<double, 16>& f_t_ee) override;
  std::array<double, 42> zeroJacobianStiffness(
      const std::array<double, 7>& q,  // NOLINT(readability-identifier-length)
      const std::array<double, 16>& f_t_ee,
      const std::array<double, 16>& ee_t_k) override;

 private:
  mutable pinocchio::Data data_;
  mutable pinocchio::Data data_gravity_;

  mutable Eigen::Matrix<double, 7, 1> q_eigen_;
  mutable Eigen::Matrix<double, 7, 1> dq_eigen_;
  mutable Eigen::Matrix<double, 7, 1> tau_eigen_;
  mutable Eigen::Matrix<double, 7, 1> ddq_temp_eigen_;
  mutable Eigen::Vector3d com_eigen_;
  mutable Eigen::Matrix3d inertia_eigen_;

  mutable std::array<double, 9> cached_i_total_;
  mutable double cached_m_total_{-1.0};
  mutable std::array<double, 3> cached_f_x_ctotal_;
  mutable bool inertia_cache_valid_{false};

  mutable pinocchio::Model pinocchio_model_;
  pinocchio::Data computeForwardKinematics(
      const std::array<double, 7>& q) const;  // NOLINT(readability-identifier-length)

  static std::array<double, 16> eigenToArray(const Eigen::Matrix4d& matrix);

  static std::array<double, 42> eigenToArray(const Eigen::Matrix<double, 6, 7>& matrix);

  pinocchio::Data initializeModelAndReturnData(const std::string& urdf);

  pinocchio::FrameIndex addFrame(const std::string& name,
                                 pinocchio::FrameIndex parent_frame_id,
                                 const pinocchio::SE3& placement);

  std::array<double, 42> computeJacobian(
      const std::array<double, 7>& q,  // NOLINT(readability-identifier-length)
      int frame_or_joint_index,
      bool is_joint,
      pinocchio::ReferenceFrame reference_frame);

  std::array<double, 42> computeEeJacobian(
      const std::array<double, 7>& q,  // NOLINT(readability-identifier-length)
      const std::array<double, 16>& f_t_ee,
      pinocchio::ReferenceFrame reference_frame);

  std::array<double, 42> computeStiffnessJacobian(
      const std::array<double, 7>& q,  // NOLINT(readability-identifier-length)
      const std::array<double, 16>& f_t_ee,
      const std::array<double, 16>& ee_t_k,
      pinocchio::ReferenceFrame reference_frame);

  void updateFramePlacements(const std::array<double, 16>& f_t_ee,
                             const std::array<double, 16>& ee_t_k = {1, 0, 0, 0, 0, 1, 0, 0, 0, 0,
                                                                     1, 0, 0, 0, 0, 1},
                             bool update_stiffness = false);

  void updateInertiaIfNeeded(const std::array<double, 9>& i_total,
                             double m_total,
                             const std::array<double, 3>& f_x_ctotal) const;

  void restoreOriginalInertia() const;

  void computeGravityVector(
      const std::array<double, 7>& q,  // NOLINT(readability-identifier-length)
      const std::array<double, 3>& g_earth,
      std::array<double, 7>& g_ne) const;

  static constexpr const char* kLastLinkName = "link8";

  pinocchio::Inertia initial_last_link_inertia_;
  pinocchio::FrameIndex last_link_frame_index_;
  pinocchio::JointIndex last_joint_index_;
  void copyToEigenQ(const std::array<double, 7>& q) const;  // NOLINT(readability-identifier-length)
  void copyToEigenDQ(
      const std::array<double, 7>& dq) const;  // NOLINT(readability-identifier-length)
  static void copyFromEigen(const Eigen::VectorXd& src, std::array<double, 7>& dst);
  static void copyFromEigenMatrix(const Eigen::MatrixXd& src, std::array<double, 49>& dst);
  pinocchio::FrameIndex ee_frame_index_;
  pinocchio::FrameIndex stiffness_frame_index_;
};

}  // namespace franka