Dynamics calculations by constraints based on Vereshchagin 1989. for a chain. This class creates instance of hybrid dynamics solver. The solver calculates total joint space accelerations in a chain when a constraint force(s) is applied to the chain's end-effector (task space/cartesian space). More...
#include <chainidsolver_vereshchagin.hpp>
Classes | |
struct | segment_info |
Public Member Functions | |
int | CartToJnt (const JntArray &q, const JntArray &q_dot, JntArray &q_dotdot, const Jacobian &alfa, const JntArray &beta, const Wrenches &f_ext, JntArray &torques) |
ChainIdSolver_Vereshchagin (const Chain &chain, Twist root_acc, unsigned int nc) | |
~ChainIdSolver_Vereshchagin () | |
Private Types | |
typedef std::vector< Frame > | Frames |
typedef Eigen::Matrix< double, 6, 6 > | Matrix6d |
typedef Eigen::Matrix< double, 6, Eigen::Dynamic > | Matrix6Xd |
typedef std::vector< Twist > | Twists |
typedef Eigen::Matrix< double, 6, 1 > | Vector6d |
Private Member Functions | |
void | constraint_calculation (const JntArray &beta) |
void | downwards_sweep (const Jacobian &alfa, const JntArray &torques) |
void | final_upwards_sweep (JntArray &q_dotdot, JntArray &torques) |
void | initial_upwards_sweep (const JntArray &q, const JntArray &q_dot, const JntArray &q_dotdot, const Wrenches &f_ext) |
Private Attributes | |
Twist | acc_root |
Jacobian | alfa_N |
Jacobian | alfa_N2 |
JntArray | beta_N |
Chain | chain |
Frame | F_total |
Eigen::MatrixXd | M_0_inverse |
unsigned int | nc |
unsigned int | nj |
unsigned int | ns |
Eigen::VectorXd | nu |
Eigen::VectorXd | nu_sum |
Wrench | qdotdot_sum |
std::vector< segment_info > | results |
Eigen::VectorXd | Sm |
Eigen::VectorXd | tmpm |
Eigen::MatrixXd | Um |
Eigen::MatrixXd | Vm |
Dynamics calculations by constraints based on Vereshchagin 1989. for a chain. This class creates instance of hybrid dynamics solver. The solver calculates total joint space accelerations in a chain when a constraint force(s) is applied to the chain's end-effector (task space/cartesian space).
Definition at line 38 of file chainidsolver_vereshchagin.hpp.
typedef std::vector<Frame> KDL::ChainIdSolver_Vereshchagin::Frames [private] |
Definition at line 41 of file chainidsolver_vereshchagin.hpp.
typedef Eigen::Matrix<double, 6, 6 > KDL::ChainIdSolver_Vereshchagin::Matrix6d [private] |
Definition at line 43 of file chainidsolver_vereshchagin.hpp.
typedef Eigen::Matrix<double, 6, Eigen::Dynamic> KDL::ChainIdSolver_Vereshchagin::Matrix6Xd [private] |
Definition at line 44 of file chainidsolver_vereshchagin.hpp.
typedef std::vector<Twist> KDL::ChainIdSolver_Vereshchagin::Twists [private] |
Definition at line 40 of file chainidsolver_vereshchagin.hpp.
typedef Eigen::Matrix<double, 6, 1 > KDL::ChainIdSolver_Vereshchagin::Vector6d [private] |
Definition at line 42 of file chainidsolver_vereshchagin.hpp.
KDL::ChainIdSolver_Vereshchagin::ChainIdSolver_Vereshchagin | ( | const Chain & | chain, |
Twist | root_acc, | ||
unsigned int | nc | ||
) |
Constructor for the solver, it will allocate all the necessary memory
chain | The kinematic chain to calculate the inverse dynamics for, an internal copy will be made. |
root_acc | The acceleration vector of the root to use during the calculation.(most likely contains gravity) |
Definition at line 31 of file chainidsolver_vereshchagin.cpp.
Definition at line 55 of file chainidsolver_vereshchagin.hpp.
int KDL::ChainIdSolver_Vereshchagin::CartToJnt | ( | const JntArray & | q, |
const JntArray & | q_dot, | ||
JntArray & | q_dotdot, | ||
const Jacobian & | alfa, | ||
const JntArray & | beta, | ||
const Wrenches & | f_ext, | ||
JntArray & | torques | ||
) |
This method calculates joint space constraint torques and total joint space acceleration. It returns 0 when it succeeds, otherwise -1 or -2 for unmatching matrix and array sizes. Input parameters;
q | The current joint positions |
q_dot | The current joint velocities |
f_ext | The external forces (no gravity, it is given in root acceleration) on the segments. Output parameters: |
q_dotdot | The joint accelerations |
torques | the resulting constraint torques for the joints |
Definition at line 46 of file chainidsolver_vereshchagin.cpp.
void KDL::ChainIdSolver_Vereshchagin::constraint_calculation | ( | const JntArray & | beta | ) | [private] |
This method calculates constraint force magnitudes.
Definition at line 241 of file chainidsolver_vereshchagin.cpp.
void KDL::ChainIdSolver_Vereshchagin::downwards_sweep | ( | const Jacobian & | alfa, |
const JntArray & | torques | ||
) | [private] |
This method is a force balance sweep. It calculates articulated body inertias and bias forces. Additionally, acceleration energies generated by bias forces and unit forces are calculated here.
Definition at line 125 of file chainidsolver_vereshchagin.cpp.
void KDL::ChainIdSolver_Vereshchagin::final_upwards_sweep | ( | JntArray & | q_dotdot, |
JntArray & | torques | ||
) | [private] |
This method puts all acceleration contributions (constraint, bias, nullspace and parent accelerations) together.
Definition at line 276 of file chainidsolver_vereshchagin.cpp.
void KDL::ChainIdSolver_Vereshchagin::initial_upwards_sweep | ( | const JntArray & | q, |
const JntArray & | q_dot, | ||
const JntArray & | q_dotdot, | ||
const Wrenches & | f_ext | ||
) | [private] |
This method calculates all cartesian space poses, twists, bias accelerations. External forces are also taken into account in this outward sweep.
Definition at line 64 of file chainidsolver_vereshchagin.cpp.
Definition at line 123 of file chainidsolver_vereshchagin.hpp.
Definition at line 124 of file chainidsolver_vereshchagin.hpp.
Definition at line 125 of file chainidsolver_vereshchagin.hpp.
Definition at line 129 of file chainidsolver_vereshchagin.hpp.
Chain KDL::ChainIdSolver_Vereshchagin::chain [private] |
Definition at line 119 of file chainidsolver_vereshchagin.hpp.
Definition at line 135 of file chainidsolver_vereshchagin.hpp.
Eigen::MatrixXd KDL::ChainIdSolver_Vereshchagin::M_0_inverse [private] |
Definition at line 126 of file chainidsolver_vereshchagin.hpp.
unsigned int KDL::ChainIdSolver_Vereshchagin::nc [private] |
Definition at line 122 of file chainidsolver_vereshchagin.hpp.
unsigned int KDL::ChainIdSolver_Vereshchagin::nj [private] |
Definition at line 120 of file chainidsolver_vereshchagin.hpp.
unsigned int KDL::ChainIdSolver_Vereshchagin::ns [private] |
Definition at line 121 of file chainidsolver_vereshchagin.hpp.
Eigen::VectorXd KDL::ChainIdSolver_Vereshchagin::nu [private] |
Definition at line 130 of file chainidsolver_vereshchagin.hpp.
Eigen::VectorXd KDL::ChainIdSolver_Vereshchagin::nu_sum [private] |
Definition at line 131 of file chainidsolver_vereshchagin.hpp.
Definition at line 134 of file chainidsolver_vereshchagin.hpp.
std::vector<segment_info> KDL::ChainIdSolver_Vereshchagin::results [private] |
Definition at line 182 of file chainidsolver_vereshchagin.hpp.
Eigen::VectorXd KDL::ChainIdSolver_Vereshchagin::Sm [private] |
Definition at line 132 of file chainidsolver_vereshchagin.hpp.
Eigen::VectorXd KDL::ChainIdSolver_Vereshchagin::tmpm [private] |
Definition at line 133 of file chainidsolver_vereshchagin.hpp.
Eigen::MatrixXd KDL::ChainIdSolver_Vereshchagin::Um [private] |
Definition at line 127 of file chainidsolver_vereshchagin.hpp.
Eigen::MatrixXd KDL::ChainIdSolver_Vereshchagin::Vm [private] |
Definition at line 128 of file chainidsolver_vereshchagin.hpp.