libfranka  0.14.1
FCI C++ API
robot_model.h
1 // Copyright (c) 2024 Franka Robotics GmbH
2 // Use of this source code is governed by the Apache-2.0 license, see LICENSE
3 #pragma once
4 
5 #include <pinocchio/algorithm/centroidal.hpp>
6 #include <pinocchio/algorithm/crba.hpp>
7 #include <pinocchio/algorithm/rnea.hpp>
8 #include <pinocchio/multibody/model.hpp>
9 #include <pinocchio/parsers/urdf.hpp>
10 #include <string>
11 
12 #include "franka/robot_model_base.h"
13 
14 namespace franka {
15 
19 class RobotModel : public RobotModelBase {
20  public:
21  RobotModel(const std::string& urdf);
22  void coriolis(const std::array<double, 7>& q,
23  const std::array<double, 7>& dq,
24  const std::array<double, 9>& i_total,
25  double m_total,
26  const std::array<double, 3>& f_x_ctotal,
27  std::array<double, 7>& c_ne) override;
28  void gravity(const std::array<double, 7>& q,
29  const std::array<double, 3>& g_earth,
30  double m_total,
31  const std::array<double, 3>& f_x_ctotal,
32  std::array<double, 7>& g_ne) override;
33  void mass(const std::array<double, 7>& q,
34  const std::array<double, 9>& i_total,
35  double m_total,
36  const std::array<double, 3>& f_x_ctotal,
37  std::array<double, 49>& m_ne) override;
38 
39  private:
44  void addInertiaToLastLink(const std::array<double, 9>& i_total,
45  double m_total,
46  const std::array<double, 3>& f_x_ctotal);
51  void computeDynamics(
52  const std::array<double, 9>& i_total,
53  double m_total,
54  const std::array<double, 3>& f_x_ctotal,
55  pinocchio::Data& data,
56  const std::function<void(pinocchio::Model&, pinocchio::Data&)>& compute_func);
57 
58  pinocchio::Model pinocchio_model_;
59  pinocchio::Inertia initial_last_link_inertia_;
60  pinocchio::FrameIndex last_link_frame_index_;
61  pinocchio::JointIndex last_joint_index_;
62 };
63 
64 } // namespace franka
Robot dynamic parameters computed from the URDF model with Pinocchio.
Definition: robot_model_base.h:10
Implements RobotModelBase using Pinocchio.
Definition: robot_model.h:19
void gravity(const std::array< double, 7 > &q, 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
Calculates the gravity vector.
void mass(const std::array< double, 7 > &q, 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
Calculates the 7x7 mass matrix.
void coriolis(const std::array< double, 7 > &q, const std::array< double, 7 > &dq, 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
Calculates the Coriolis force vector (state-space equation): , in .