Calculates poses of joints and dynamic properties of the robot. More...
#include <model.h>
Public Member Functions | |
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. More... | |
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. More... | |
std::array< double, 7 > | coriolis (const franka::RobotState &robot_state) const noexcept |
Calculates the Coriolis force vector (state-space equation): , in . More... | |
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 . More... | |
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. More... | |
std::array< double, 7 > | gravity (const franka::RobotState &robot_state, const std::array< double, 3 > &gravity_earth={{0., 0.,-9.81}}) const noexcept |
Calculates the gravity vector. More... | |
std::array< double, 49 > | mass (const franka::RobotState &robot_state) const noexcept |
Calculates the 7x7 mass matrix. More... | |
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. More... | |
Model (franka::Network &network) | |
Creates a new Model instance. More... | |
Model (Model &&model) noexcept | |
Move-constructs a new Model instance. More... | |
Model & | operator= (Model &&model) noexcept |
Move-assigns this Model from another Model instance. More... | |
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. More... | |
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. More... | |
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. More... | |
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. More... | |
~Model () noexcept | |
Unloads the model library. More... | |
Private Attributes | |
std::unique_ptr< ModelLibrary > | library_ |
Calculates poses of joints and dynamic properties of the robot.
|
explicit |
Creates a new Model instance.
This constructor is for internal use only.
[in] | network | For internal use. |
ModelException | if the model library cannot be loaded. |
|
noexcept |
|
noexcept |
Unloads the model library.
std::array<double, 42> franka::Model::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.
[in] | frame | The desired frame. |
[in] | robot_state | State from which the pose should be calculated. |
std::array<double, 42> franka::Model::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.
[in] | frame | The desired frame. |
[in] | q | Joint position. |
[in] | F_T_EE | End effector in flange frame. |
[in] | EE_T_K | Stiffness frame K in the end effector frame. |
|
noexcept |
Calculates the Coriolis force vector (state-space equation): , in .
[in] | robot_state | State from which the Coriolis force vector should be calculated. |
|
noexcept |
Calculates the Coriolis force vector (state-space equation): , in .
[in] | q | Joint position. |
[in] | dq | Joint velocity. |
[in] | I_total | Inertia of the attached total load including end effector, relative to center of mass, given as vectorized 3x3 column-major matrix. Unit: . |
[in] | m_total | Weight of the attached total load including end effector. Unit: . |
[in] | F_x_Ctotal | Translation from flange to center of mass of the attached total load. Unit: . |
|
noexcept |
Calculates the gravity vector.
Unit: .
[in] | q | Joint position. |
[in] | m_total | Weight of the attached total load including end effector. Unit: . |
[in] | F_x_Ctotal | Translation from flange to center of mass of the attached total load. Unit: . |
[in] | gravity_earth | Earth's gravity vector. Unit: . Default to {0.0, 0.0, -9.81}. |
|
noexcept |
Calculates the gravity vector.
Unit: .
[in] | robot_state | State from which the gravity vector should be calculated. |
[in] | gravity_earth | Earth's gravity vector. Unit: . Default to {0.0, 0.0, -9.81}. |
|
noexcept |
Calculates the 7x7 mass matrix.
Unit: .
[in] | robot_state | State from which the pose should be calculated. |
|
noexcept |
Calculates the 7x7 mass matrix.
Unit: .
[in] | q | Joint position. |
[in] | I_total | Inertia of the attached total load including end effector, relative to center of mass, given as vectorized 3x3 column-major matrix. Unit: . |
[in] | m_total | Weight of the attached total load including end effector. Unit: . |
[in] | F_x_Ctotal | Translation from flange to center of mass of the attached total load. Unit: . |
std::array<double, 16> franka::Model::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.
[in] | frame | The desired frame. |
[in] | robot_state | State from which the pose should be calculated. |
std::array<double, 16> franka::Model::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.
[in] | frame | The desired frame. |
[in] | q | Joint position. |
[in] | F_T_EE | End effector in flange frame. |
[in] | EE_T_K | Stiffness frame K in the end effector frame. |
std::array<double, 42> franka::Model::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.
[in] | frame | The desired frame. |
[in] | robot_state | State from which the pose should be calculated. |
std::array<double, 42> franka::Model::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.
[in] | frame | The desired frame. |
[in] | q | Joint position. |
[in] | F_T_EE | End effector in flange frame. |
[in] | EE_T_K | Stiffness frame K in the end effector frame. |
|
private |