robot_model.h

Go to the documentation of this file.

// 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 <pinocchio/algorithm/centroidal.hpp>
#include <pinocchio/algorithm/compute-all-terms.hpp>
#include <pinocchio/algorithm/crba.hpp>
#include <pinocchio/algorithm/frames.hpp>
#include <pinocchio/algorithm/jacobian.hpp>
#include <pinocchio/algorithm/kinematics.hpp>
#include <pinocchio/algorithm/rnea.hpp>
#include <pinocchio/multibody/model.hpp>
#include <pinocchio/parsers/urdf.hpp>
 
#include <string>
 
#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;
 
  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