libfranka  0.14.1
FCI C++ API
model.h
Go to the documentation of this file.
1 // Copyright (c) 2023 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 <array>
6 #include <memory>
7 
8 #include <franka/robot.h>
9 #include <franka/robot_model_base.h>
10 #include <franka/robot_state.h>
11 
17 namespace franka {
18 
22 enum class Frame {
23  kJoint1,
24  kJoint2,
25  kJoint3,
26  kJoint4,
27  kJoint5,
28  kJoint6,
29  kJoint7,
30  kFlange,
31  kEndEffector,
32  kStiffness
33 };
34 
44 Frame operator++(Frame& frame, int /* dummy */) noexcept;
45 
46 class ModelLibrary;
47 class Network;
48 
52 class Model {
53  public:
65  explicit Model(franka::Network& network, const std::string& urdf_model);
66 
76  explicit Model(franka::Network& network, std::unique_ptr<RobotModelBase> robot_model);
77 
83  Model(Model&& model) noexcept;
84 
92  Model& operator=(Model&& model) noexcept;
93 
97  ~Model() noexcept;
98 
109  std::array<double, 16> pose(Frame frame, const franka::RobotState& robot_state) const;
110 
123  std::array<double, 16> pose(
124  Frame frame,
125  const std::array<double, 7>& q,
126  const std::array<double, 16>& F_T_EE, // NOLINT(readability-identifier-naming)
127  const std::array<double, 16>& EE_T_K) // NOLINT(readability-identifier-naming)
128  const;
129 
140  std::array<double, 42> bodyJacobian(Frame frame, const franka::RobotState& robot_state) const;
141 
154  std::array<double, 42> bodyJacobian(
155  Frame frame,
156  const std::array<double, 7>& q,
157  const std::array<double, 16>& F_T_EE, // NOLINT(readability-identifier-naming)
158  const std::array<double, 16>& EE_T_K) // NOLINT(readability-identifier-naming)
159  const;
160 
171  std::array<double, 42> zeroJacobian(Frame frame, const franka::RobotState& robot_state) const;
172 
185  std::array<double, 42> zeroJacobian(
186  Frame frame,
187  const std::array<double, 7>& q,
188  const std::array<double, 16>& F_T_EE, // NOLINT(readability-identifier-naming)
189  const std::array<double, 16>& EE_T_K) // NOLINT(readability-identifier-naming)
190  const;
191 
199  std::array<double, 49> mass(const franka::RobotState& robot_state) const noexcept;
200 
214  std::array<double, 49> mass(
215  const std::array<double, 7>& q,
216  const std::array<double, 9>& I_total, // NOLINT(readability-identifier-naming)
217  double m_total,
218  const std::array<double, 3>& F_x_Ctotal) // NOLINT(readability-identifier-naming)
219  const noexcept;
220 
229  std::array<double, 7> coriolis(const franka::RobotState& robot_state) const noexcept;
230 
246  std::array<double, 7> coriolis(
247  const std::array<double, 7>& q,
248  const std::array<double, 7>& dq,
249  const std::array<double, 9>& I_total, // NOLINT(readability-identifier-naming)
250  double m_total,
251  const std::array<double, 3>& F_x_Ctotal) // NOLINT(readability-identifier-naming)
252  const noexcept;
253 
267  std::array<double, 7> gravity(
268  const std::array<double, 7>& q,
269  double m_total,
270  const std::array<double, 3>& F_x_Ctotal, // NOLINT(readability-identifier-naming)
271  const std::array<double, 3>& gravity_earth = {{0., 0., -9.81}}) const noexcept;
272 
281  std::array<double, 7> gravity(const franka::RobotState& robot_state,
282  const std::array<double, 3>& gravity_earth) const noexcept;
283 
291  std::array<double, 7> gravity(const franka::RobotState& robot_state) const noexcept;
292 
294  Model(const Model&) = delete;
295  Model& operator=(const Model&) = delete;
297 
298  private:
299  std::unique_ptr<ModelLibrary> library_;
300  std::unique_ptr<RobotModelBase> robot_model_;
301 };
302 
303 } // namespace franka
Calculates poses of joints and dynamic properties of the robot.
Definition: model.h:52
~Model() noexcept
Unloads the model library.
std::array< double, 16 > pose(Frame frame, const franka::RobotState &robot_state) const
Gets the 4x4 pose matrix for the given frame in base frame.
Model & operator=(Model &&model) noexcept
Move-assigns this Model from another Model instance.
std::array< double, 42 > zeroJacobian(Frame frame, const franka::RobotState &robot_state) const
Gets the 6x7 Jacobian for the given joint relative to the base frame.
std::array< double, 7 > gravity(const franka::RobotState &robot_state, const std::array< double, 3 > &gravity_earth) const noexcept
Calculates the gravity vector.
Model(franka::Network &network, std::unique_ptr< RobotModelBase > robot_model)
Creates a new Model instance only for the tests.
Model(Model &&model) noexcept
Move-constructs a new Model instance.
Model(franka::Network &network, const std::string &urdf_model)
Creates a new Model instance.
std::array< double, 7 > coriolis(const franka::RobotState &robot_state) const noexcept
Calculates the Coriolis force vector (state-space equation): , in .
std::array< double, 42 > bodyJacobian(Frame frame, const franka::RobotState &robot_state) const
Gets the 6x7 Jacobian for the given frame, relative to that frame.
std::array< double, 7 > gravity(const franka::RobotState &robot_state) const noexcept
Calculates the gravity vector using the robot state.
std::array< double, 7 > gravity(const std::array< double, 7 > &q, double m_total, const std::array< double, 3 > &F_x_Ctotal, const std::array< double, 3 > &gravity_earth={{0., 0., -9.81}}) const noexcept
Calculates the gravity vector.
std::array< double, 49 > mass(const franka::RobotState &robot_state) const noexcept
Calculates the 7x7 mass matrix.
Frame
Enumerates the seven joints, the flange, and the end effector of a robot.
Definition: model.h:22
Frame operator++(Frame &frame, int) noexcept
Post-increments the given Frame by one.
Contains the franka::Robot type.
Contains the franka::RobotState types.
Describes the robot state.
Definition: robot_state.h:34