Go to the documentation of this file.
8 #include <franka/model.h>
9 #include <franka/robot_state.h>
32 franka::RobotState& robot_state)
68 const std::array<double, 7>& q,
69 const std::array<double, 9>& total_inertia,
71 const std::array<double, 3>& F_x_Ctotal)
const {
72 return model_->
mass(q, total_inertia, total_mass, F_x_Ctotal);
104 const std::array<double, 7>& q,
105 const std::array<double, 7>& dq,
106 const std::array<double, 9>& total_inertia,
108 const std::array<double, 3>& F_x_Ctotal)
const {
109 return model_->
coriolis(q, dq, total_inertia, total_mass, F_x_Ctotal);
122 std::array<double, 7>
getGravity(
const std::array<double, 3>& gravity_earth = {
123 {0., 0., -9.81}})
const {
144 const std::array<double, 7>& q,
146 const std::array<double, 3>& F_x_Ctotal,
147 const std::array<double, 3>& gravity_earth = {{0., 0., -9.81}})
const {
148 return model_->
gravity(q, total_mass, F_x_Ctotal, gravity_earth);
163 std::array<double, 16>
getPose(
const franka::Frame& frame)
const {
183 const franka::Frame& frame,
184 const std::array<double, 7>& q,
185 const std::array<double, 16>& F_T_EE,
186 const std::array<double, 16>& EE_T_K)
188 return model_->
pose(frame, q, F_T_EE, EE_T_K);
223 const franka::Frame& frame,
224 const std::array<double, 7>& q,
225 const std::array<double, 16>& F_T_EE,
226 const std::array<double, 16>& EE_T_K)
263 const franka::Frame& frame,
264 const std::array<double, 7>& q,
265 const std::array<double, 16>& F_T_EE,
266 const std::array<double, 16>& EE_T_K)
std::array< double, 16 > pose(franka::Frame frame, const franka::RobotState &robot_state) const
Gets the 4x4 pose matrix for the given frame in base frame.
std::array< double, 42 > getBodyJacobian(const franka::Frame &frame) const
Gets the 6x7 Jacobian for the given frame, relative to that frame.
std::array< double, 16 > getPose(const franka::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, calculated from the given robot state.
std::array< double, 7 > coriolis(const franka::RobotState &robot_state) const
Calculates the Coriolis force vector (state-space equation): , in .
std::array< double, 7 > getCoriolis() const
Calculates the Coriolis force vector (state-space equation) from the current robot state: ,...
std::array< double, 7 > getGravity(const std::array< double, 7 > &q, double total_mass, const std::array< double, 3 > &F_x_Ctotal, const std::array< double, 3 > &gravity_earth={{0., 0., -9.81}}) const
Calculates the gravity vector from the given 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
Calculates the gravity vector.
std::array< double, 16 > getPose(const franka::Frame &frame) const
Gets the 4x4 pose matrix for the given frame in base frame, calculated from the current robot state.
FrankaModelHandle()=delete
const franka::RobotState * robot_state_
std::array< double, 49 > mass(const franka::RobotState &robot_state) const
Calculates the 7x7 mass matrix.
std::array< double, 42 > zeroJacobian(franka::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 > getCoriolis(const std::array< double, 7 > &q, const std::array< double, 7 > &dq, const std::array< double, 9 > &total_inertia, double total_mass, const std::array< double, 3 > &F_x_Ctotal) const
Calculates the Coriolis force vector (state-space equation) from the given robot state: ,...
Calculates poses of joints and dynamic properties of the robot.
std::array< double, 49 > getMass() const
Calculates the 7x7 mass matrix from the current robot state.
std::array< double, 42 > getBodyJacobian(const franka::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.
std::array< double, 42 > getZeroJacobian(const franka::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.
Hardware interface to perform calculations using the dynamic model of a robot.
std::string getName() const noexcept
Gets the name of the model handle.
std::array< double, 42 > getZeroJacobian(const franka::Frame &frame) const
Gets the 6x7 Jacobian for the given joint relative to the base frame.
std::array< double, 7 > getGravity(const std::array< double, 3 > &gravity_earth={ {0., 0., -9.81}}) const
Calculates the gravity vector from the current robot state.
const franka_hw::ModelBase * model_
std::array< double, 42 > bodyJacobian(franka::Frame frame, const franka::RobotState &robot_state) const
Gets the 6x7 Jacobian for the given frame, relative to that frame.
Handle to perform calculations using the dynamic model of a robot.
FrankaModelHandle(const std::string &name, franka_hw::ModelBase &model, franka::RobotState &robot_state)
Creates an instance of a FrankaModelHandle.
std::array< double, 49 > getMass(const std::array< double, 7 > &q, const std::array< double, 9 > &total_inertia, double total_mass, const std::array< double, 3 > &F_x_Ctotal) const
Calculates the 7x7 mass matrix from a given robot state.
franka_hw
Author(s): Franka Emika GmbH
autogenerated on Mon Sep 19 2022 02:33:21