tinyrobotics
Loading...
Searching...
No Matches
dynamics.hpp File Reference

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.
 

Detailed Description

Contains functions for computing the dynamics of a tinyrobotics model.

Function Documentation

◆ apply_external_forces()

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.

Parameters
mtinyrobotics model.
XupThe spatial transformation matrices between the ith link and its parent.
f_inThe input force array of the tinyrobotics model.
f_extThe external force array to be added to the input force array.
Returns
f_out The output force array with the external forces incorporated.

◆ forward_dynamics()

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.

Parameters
mtinyrobotics model.
qJoint configuration of the robot.
dqJoint velocity of the robot.
tauJoint torque of the robot.
f_extExternal forces acting on the robot.
Returns
Joint accelerations of the model.

◆ forward_dynamics_crb()

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.

Parameters
mtinyrobotics model.
qJoint configuration of the robot.
dqJoint velocity of the robot.
tauJoint torque of the robot.
f_extExternal forces acting on the robot.
Returns
Joint accelerations of the model.

◆ inverse_dynamics()

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.

Parameters
mtinyrobotics model.
qJoint configuration of the robot.
dqJoint velocity of the robot.
f_extExternal forces acting on the robot.
Returns
tau

◆ kinetic_energy()

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.

Parameters
mtinyrobotics model.
qJoint configuration of the robot.
dqThe joint velocity of the robot.

◆ mass_matrix()

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.

Parameters
mtinyrobotics model.
qJoint configuration of the robot.
Template Parameters
Scalartype of the tinyrobotics model.
nqNumber of configuration coordinates (degrees of freedom).

◆ potential_energy()

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.

Parameters
mtinyrobotics model.
qJoint configuration of the robot.
Template Parameters
Scalartype of the tinyrobotics model.
nqNumber of configuration coordinates (degrees of freedom).

◆ total_energy()

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.

Parameters
mtinyrobotics model.
qJoint configuration of the robot.
dqJoint velocity of the robot.
Template Parameters
Scalartype of the tinyrobotics model.
nqNumber of configuration coordinates (degrees of freedom).