Class Model
Defined in File model.h
Class Documentation
-
class Model
Calculates poses of joints and dynamic properties of the robot.
Public Functions
-
explicit Model(franka::Network &network, const std::string &urdf_model)
Creates a new Model instance.
This constructor is for internal use only.
See also
- Parameters:
network – [in] For internal use.
- Throws:
ModelException – if the model library cannot be loaded.
-
explicit Model(franka::Network &network, std::unique_ptr<RobotModelBase> robot_model)
Creates a new Model instance only for the tests.
This constructor is for the unittests for enabling mocks.
- Parameters:
network – [in] For internal use.
robot_model – [in] unique pointer to the mocked robot_model
-
Model(Model &&model) noexcept
Move-constructs a new Model instance.
- Parameters:
model – [in] Other Model instance.
-
~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.
The pose is represented as a 4x4 matrix in column-major format.
- Parameters:
frame – [in] The desired frame.
robot_state – [in] State from which the pose should be calculated.
- Returns:
Vectorized 4x4 pose matrix, column-major.
-
std::array<double, 16> pose(Frame frame, const std::array<double, 7> &q, const std::array<double, 16> &F_T_EE, const std::array<double, 16> &EE_T_K) const
Gets the 4x4 pose matrix for the given frame in base frame.
The pose is represented as a 4x4 matrix in column-major format.
- Parameters:
frame – [in] The desired frame.
q – [in] Joint position.
F_T_EE – [in] End effector in flange frame.
EE_T_K – [in] Stiffness frame K in the end effector frame.
- Returns:
Vectorized 4x4 pose matrix, column-major.
-
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.
The Jacobian is represented as a 6x7 matrix in column-major format.
- Parameters:
frame – [in] The desired frame.
robot_state – [in] State from which the pose should be calculated.
- Returns:
Vectorized 6x7 Jacobian, column-major.
-
std::array<double, 42> bodyJacobian(Frame frame, const std::array<double, 7> &q, const std::array<double, 16> &F_T_EE, const std::array<double, 16> &EE_T_K) const
Gets the 6x7 Jacobian for the given frame, relative to that frame.
The Jacobian is represented as a 6x7 matrix in column-major format.
- Parameters:
frame – [in] The desired frame.
q – [in] Joint position.
F_T_EE – [in] End effector in flange frame.
EE_T_K – [in] Stiffness frame K in the end effector frame.
- Returns:
Vectorized 6x7 Jacobian, column-major.
-
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.
The Jacobian is represented as a 6x7 matrix in column-major format.
- Parameters:
frame – [in] The desired frame.
robot_state – [in] State from which the pose should be calculated.
- Returns:
Vectorized 6x7 Jacobian, column-major.
-
std::array<double, 42> zeroJacobian(Frame frame, const std::array<double, 7> &q, const std::array<double, 16> &F_T_EE, const std::array<double, 16> &EE_T_K) const
Gets the 6x7 Jacobian for the given joint relative to the base frame.
The Jacobian is represented as a 6x7 matrix in column-major format.
- Parameters:
frame – [in] The desired frame.
q – [in] Joint position.
F_T_EE – [in] End effector in flange frame.
EE_T_K – [in] Stiffness frame K in the end effector frame.
- Returns:
Vectorized 6x7 Jacobian, column-major.
-
std::array<double, 49> mass(const franka::RobotState &robot_state) const noexcept
Calculates the 7x7 mass matrix. Unit:
.- Parameters:
robot_state – [in] State from which the mass matrix should be calculated.
- Returns:
Vectorized 7x7 mass matrix, column-major.
-
std::array<double, 49> 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) const noexcept
Calculates the 7x7 mass matrix. Unit:
.- Parameters:
q – [in] Joint position.
I_total – [in] Inertia of the attached total load including end effector, relative to center of mass, given as vectorized 3x3 column-major matrix. Unit:
.m_total – [in] Weight of the attached total load including end effector. Unit:
.F_x_Ctotal – [in] Translation from flange to center of mass of the attached total load. Unit:
.
- Returns:
Vectorized 7x7 mass matrix, column-major.
-
std::array<double, 7> coriolis(const franka::RobotState &robot_state) const noexcept
Calculates the Coriolis force vector (state-space equation):
, in .- Parameters:
robot_state – [in] State from which the Coriolis force vector should be calculated.
- Returns:
Coriolis force vector.
-
std::array<double, 7> 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) const noexcept
Calculates the Coriolis force vector (state-space equation):
, in .- Parameters:
q – [in] Joint position.
dq – [in] Joint velocity.
I_total – [in] Inertia of the attached total load including end effector, relative to center of mass, given as vectorized 3x3 column-major matrix. Unit:
.m_total – [in] Weight of the attached total load including end effector. Unit:
.F_x_Ctotal – [in] Translation from flange to center of mass of the attached total load. Unit:
.
- Returns:
Coriolis force vector.
-
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. Unit:
.- Parameters:
q – [in] Joint position.
m_total – [in] Weight of the attached total load including end effector. Unit:
.F_x_Ctotal – [in] Translation from flange to center of mass of the attached total load. Unit:
.gravity_earth – [in] Earth’s gravity vector. Unit:
. Default to {0.0, 0.0, -9.81}.
- Returns:
Gravity vector.
-
std::array<double, 7> gravity(const franka::RobotState &robot_state, const std::array<double, 3> &gravity_earth) const noexcept
Calculates the gravity vector. Unit:
.- Parameters:
robot_state – [in] State from which the gravity vector should be calculated.
gravity_earth – [in] Earth’s gravity vector. Unit:
.
- Returns:
Gravity vector.
-
std::array<double, 7> gravity(const franka::RobotState &robot_state) const noexcept
Calculates the gravity vector using the robot state. Unit:
.- Parameters:
robot_state – [in] State from which the gravity vector should be calculated.
- Returns:
Gravity vector.
-
explicit Model(franka::Network &network, const std::string &urdf_model)