tinyrobotics
|
Contains functions for computing the dynamics of a tinyrobotics model. More...
#include <Eigen/Core>
#include <Eigen/Dense>
#include <Eigen/Geometry>
#include "kinematics.hpp"
#include "model.hpp"
Go to the source code of this file.
Functions | |
template<typename Scalar , int nq> | |
Eigen::Matrix< Scalar, nq, nq > | tinyrobotics::mass_matrix (Model< Scalar, nq > &m, const Eigen::Matrix< Scalar, nq, 1 > &q) |
Compute the mass matrix of the tinyrobotics model. | |
template<typename Scalar , int nq> | |
Scalar | tinyrobotics::kinetic_energy (Model< Scalar, nq > &m, const Eigen::Matrix< Scalar, nq, 1 > &q, const Eigen::Matrix< Scalar, nq, 1 > &dq) |
Compute the kinetic_energy of the tinyrobotics model. | |
template<typename Scalar , int nq> | |
Scalar | tinyrobotics::potential_energy (Model< Scalar, nq > &m, const Eigen::Matrix< Scalar, nq, 1 > &q) |
Compute the potential_energy of the tinyrobotics model. | |
template<typename Scalar , int nq> | |
Scalar | tinyrobotics::total_energy (Model< Scalar, nq > &m, const Eigen::Matrix< Scalar, nq, 1 > &q, const Eigen::Matrix< Scalar, nq, 1 > &dq) |
Compute the total energy of the tinyrobotics model. | |
template<typename Scalar , int nq> | |
std::vector< Eigen::Matrix< Scalar, 6, 1 > > | tinyrobotics::apply_external_forces (const Model< Scalar, nq > &m, const std::vector< Eigen::Matrix< Scalar, 6, 6 > > &Xup, const std::vector< Eigen::Matrix< Scalar, 6, 1 > > &f_in, const std::vector< Eigen::Matrix< Scalar, 6, 1 > > &f_ext) |
Apply external forces to the tinyrobotics model. | |
template<typename Scalar , int nq> | |
Eigen::Matrix< Scalar, nq, 1 > | tinyrobotics::forward_dynamics (Model< Scalar, nq > &m, const Eigen::Matrix< Scalar, nq, 1 > &q, const Eigen::Matrix< Scalar, nq, 1 > &dq, const Eigen::Matrix< Scalar, nq, 1 > &tau, const std::vector< Eigen::Matrix< Scalar, 6, 1 > > &f_ext={}) |
Compute the forward dynamics of the tinyrobotics model via Articulated-Body Algorithm. | |
template<typename Scalar , int nq> | |
Eigen::Matrix< Scalar, nq, 1 > | tinyrobotics::forward_dynamics_crb (Model< Scalar, nq > &m, const Eigen::Matrix< Scalar, nq, 1 > &q, const Eigen::Matrix< Scalar, nq, 1 > &dq, const Eigen::Matrix< Scalar, nq, 1 > &tau, const std::vector< Eigen::Matrix< Scalar, 6, 1 > > &f_ext={}) |
Compute the forward dynamics of the tinyrobotics model via Composite-Rigid-Body Algorithm. | |
template<typename Scalar , int nq> | |
Eigen::Matrix< Scalar, nq, 1 > | tinyrobotics::inverse_dynamics (Model< Scalar, nq > &m, const Eigen::Matrix< Scalar, nq, 1 > &q, const Eigen::Matrix< Scalar, nq, 1 > &dq, const Eigen::Matrix< Scalar, nq, 1 > &ddq, const std::vector< Eigen::Matrix< Scalar, 6, 1 > > &f_ext={}) |
Compute the inverse dynamics of a tinyrobotics model. | |
Contains functions for computing the dynamics of a tinyrobotics model.
std::vector< Eigen::Matrix< Scalar, 6, 1 > > tinyrobotics::apply_external_forces | ( | const Model< Scalar, nq > & | m, |
const std::vector< Eigen::Matrix< Scalar, 6, 6 > > & | Xup, | ||
const std::vector< Eigen::Matrix< Scalar, 6, 1 > > & | f_in, | ||
const std::vector< Eigen::Matrix< Scalar, 6, 1 > > & | f_ext | ||
) |
Apply external forces to the tinyrobotics model.
m | tinyrobotics model. |
Xup | The spatial transformation matrices between the ith link and its parent. |
f_in | The input force array of the tinyrobotics model. |
f_ext | The external force array to be added to the input force array. |
Eigen::Matrix< Scalar, nq, 1 > tinyrobotics::forward_dynamics | ( | Model< Scalar, nq > & | m, |
const Eigen::Matrix< Scalar, nq, 1 > & | q, | ||
const Eigen::Matrix< Scalar, nq, 1 > & | dq, | ||
const Eigen::Matrix< Scalar, nq, 1 > & | tau, | ||
const std::vector< Eigen::Matrix< Scalar, 6, 1 > > & | f_ext = {} |
||
) |
Compute the forward dynamics of the tinyrobotics model via Articulated-Body Algorithm.
m | tinyrobotics model. |
q | Joint configuration of the robot. |
dq | Joint velocity of the robot. |
tau | Joint torque of the robot. |
f_ext | External forces acting on the robot. |
Eigen::Matrix< Scalar, nq, 1 > tinyrobotics::forward_dynamics_crb | ( | Model< Scalar, nq > & | m, |
const Eigen::Matrix< Scalar, nq, 1 > & | q, | ||
const Eigen::Matrix< Scalar, nq, 1 > & | dq, | ||
const Eigen::Matrix< Scalar, nq, 1 > & | tau, | ||
const std::vector< Eigen::Matrix< Scalar, 6, 1 > > & | f_ext = {} |
||
) |
Compute the forward dynamics of the tinyrobotics model via Composite-Rigid-Body Algorithm.
m | tinyrobotics model. |
q | Joint configuration of the robot. |
dq | Joint velocity of the robot. |
tau | Joint torque of the robot. |
f_ext | External forces acting on the robot. |
Eigen::Matrix< Scalar, nq, 1 > tinyrobotics::inverse_dynamics | ( | Model< Scalar, nq > & | m, |
const Eigen::Matrix< Scalar, nq, 1 > & | q, | ||
const Eigen::Matrix< Scalar, nq, 1 > & | dq, | ||
const Eigen::Matrix< Scalar, nq, 1 > & | ddq, | ||
const std::vector< Eigen::Matrix< Scalar, 6, 1 > > & | f_ext = {} |
||
) |
Compute the inverse dynamics of a tinyrobotics model.
m | tinyrobotics model. |
q | Joint configuration of the robot. |
dq | Joint velocity of the robot. |
f_ext | External forces acting on the robot. |
Scalar tinyrobotics::kinetic_energy | ( | Model< Scalar, nq > & | m, |
const Eigen::Matrix< Scalar, nq, 1 > & | q, | ||
const Eigen::Matrix< Scalar, nq, 1 > & | dq | ||
) |
Compute the kinetic_energy of the tinyrobotics model.
m | tinyrobotics model. |
q | Joint configuration of the robot. |
dq | The joint velocity of the robot. |
Eigen::Matrix< Scalar, nq, nq > tinyrobotics::mass_matrix | ( | Model< Scalar, nq > & | m, |
const Eigen::Matrix< Scalar, nq, 1 > & | q | ||
) |
Compute the mass matrix of the tinyrobotics model.
m | tinyrobotics model. |
q | Joint configuration of the robot. |
Scalar | type of the tinyrobotics model. |
nq | Number of configuration coordinates (degrees of freedom). |
Scalar tinyrobotics::potential_energy | ( | Model< Scalar, nq > & | m, |
const Eigen::Matrix< Scalar, nq, 1 > & | q | ||
) |
Compute the potential_energy of the tinyrobotics model.
m | tinyrobotics model. |
q | Joint configuration of the robot. |
Scalar | type of the tinyrobotics model. |
nq | Number of configuration coordinates (degrees of freedom). |
Scalar tinyrobotics::total_energy | ( | Model< Scalar, nq > & | m, |
const Eigen::Matrix< Scalar, nq, 1 > & | q, | ||
const Eigen::Matrix< Scalar, nq, 1 > & | dq | ||
) |
Compute the total energy of the tinyrobotics model.
m | tinyrobotics model. |
q | Joint configuration of the robot. |
dq | Joint velocity of the robot. |
Scalar | type of the tinyrobotics model. |
nq | Number of configuration coordinates (degrees of freedom). |