KDL
1.3.0
|
Dynamics calculations by constraints based on Vereshchagin 1989. More...
#include <src/chainidsolver_vereshchagin.hpp>
Classes | |
struct | segment_info |
Public Member Functions | |
ChainIdSolver_Vereshchagin (const Chain &chain, Twist root_acc, unsigned int nc) | |
Constructor for the solver, it will allocate all the necessary memory. More... | |
~ChainIdSolver_Vereshchagin () | |
int | 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. More... | |
Private Types | |
typedef std::vector< Twist > | Twists |
typedef std::vector< Frame > | Frames |
typedef Eigen::Matrix< double, 6, 1 > | Vector6d |
typedef Eigen::Matrix< double, 6, 6 > | Matrix6d |
typedef Eigen::Matrix< double, 6, Eigen::Dynamic > | Matrix6Xd |
Private Member Functions | |
void | initial_upwards_sweep (const JntArray &q, const JntArray &q_dot, const JntArray &q_dotdot, const Wrenches &f_ext) |
This method calculates all cartesian space poses, twists, bias accelerations. More... | |
void | downwards_sweep (const Jacobian &alfa, const JntArray &torques) |
This method is a force balance sweep. More... | |
void | constraint_calculation (const JntArray &beta) |
This method calculates constraint force magnitudes. More... | |
void | final_upwards_sweep (JntArray &q_dotdot, JntArray &torques) |
This method puts all acceleration contributions (constraint, bias, nullspace and parent accelerations) together. More... | |
Private Attributes | |
Chain | chain |
unsigned int | nj |
unsigned int | ns |
unsigned int | nc |
Twist | acc_root |
Jacobian | alfa_N |
Jacobian | alfa_N2 |
Eigen::MatrixXd | M_0_inverse |
Eigen::MatrixXd | Um |
Eigen::MatrixXd | Vm |
JntArray | beta_N |
Eigen::VectorXd | nu |
Eigen::VectorXd | nu_sum |
Eigen::VectorXd | Sm |
Eigen::VectorXd | tmpm |
Wrench | qdotdot_sum |
Frame | F_total |
std::vector< segment_info > | results |
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).
|
private |
|
private |
|
private |
|
private |
|
private |
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) |
References acc_root, M_0_inverse, nc, nu_sum, Sm, tmpm, Um, and Vm.
|
inline |
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 |
References KDL::Jacobian::columns(), constraint_calculation(), downwards_sweep(), final_upwards_sweep(), initial_upwards_sweep(), nc, nj, and ns.
|
private |
This method calculates constraint force magnitudes.
References acc_root, KDL::Vector::data, M_0_inverse, nc, nu, nu_sum, results, KDL::Twist::rot, Sm, tmpm, Um, KDL::Twist::vel, and Vm.
Referenced by CartToJnt().
|
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.
References KDL::ChainIdSolver_Vereshchagin::segment_info::C, chain, KDL::ChainIdSolver_Vereshchagin::segment_info::D, KDL::Vector::data, KDL::dot(), KDL::ChainIdSolver_Vereshchagin::segment_info::E, KDL::ChainIdSolver_Vereshchagin::segment_info::E_tilde, KDL::ChainIdSolver_Vereshchagin::segment_info::EZ, KDL::ChainIdSolver_Vereshchagin::segment_info::F, F_total, KDL::Wrench::force, KDL::ChainIdSolver_Vereshchagin::segment_info::G, KDL::Segment::getJoint(), KDL::Chain::getSegment(), KDL::Joint::getType(), KDL::ChainIdSolver_Vereshchagin::segment_info::H, KDL::Rotation::Inverse(), KDL::ChainIdSolver_Vereshchagin::segment_info::M, KDL::Frame::M, nc, nj, KDL::Joint::None, ns, KDL::ChainIdSolver_Vereshchagin::segment_info::P, KDL::ChainIdSolver_Vereshchagin::segment_info::P_tilde, KDL::ChainIdSolver_Vereshchagin::segment_info::PC, KDL::ChainIdSolver_Vereshchagin::segment_info::PZ, KDL::ChainIdSolver_Vereshchagin::segment_info::R, KDL::ChainIdSolver_Vereshchagin::segment_info::R_tilde, results, KDL::Twist::rot, KDL::Wrench::torque, KDL::ChainIdSolver_Vereshchagin::segment_info::totalBias, KDL::ChainIdSolver_Vereshchagin::segment_info::U, KDL::ChainIdSolver_Vereshchagin::segment_info::u, KDL::Twist::vel, and KDL::ChainIdSolver_Vereshchagin::segment_info::Z.
Referenced by CartToJnt().
|
private |
This method puts all acceleration contributions (constraint, bias, nullspace and parent accelerations) together.
References KDL::ChainIdSolver_Vereshchagin::segment_info::acc, acc_root, KDL::ChainIdSolver_Vereshchagin::segment_info::C, chain, KDL::ChainIdSolver_Vereshchagin::segment_info::constAccComp, KDL::ChainIdSolver_Vereshchagin::segment_info::D, KDL::dot(), KDL::ChainIdSolver_Vereshchagin::segment_info::E, KDL::ChainIdSolver_Vereshchagin::segment_info::F, KDL::Segment::getJoint(), KDL::Chain::getSegment(), KDL::Joint::getType(), KDL::Frame::Inverse(), KDL::Joint::None, ns, nu, KDL::ChainIdSolver_Vereshchagin::segment_info::nullspaceAccComp, KDL::ChainIdSolver_Vereshchagin::segment_info::P, results, KDL::ChainIdSolver_Vereshchagin::segment_info::u, and KDL::ChainIdSolver_Vereshchagin::segment_info::Z.
Referenced by CartToJnt().
|
private |
This method calculates all cartesian space poses, twists, bias accelerations.
External forces are also taken into account in this outward sweep.
References KDL::ChainIdSolver_Vereshchagin::segment_info::A, acc_root, KDL::ChainIdSolver_Vereshchagin::segment_info::C, chain, KDL::ChainIdSolver_Vereshchagin::segment_info::F, KDL::ChainIdSolver_Vereshchagin::segment_info::F_base, F_total, KDL::Segment::getInertia(), KDL::Segment::getJoint(), KDL::Chain::getSegment(), KDL::Joint::getType(), KDL::ChainIdSolver_Vereshchagin::segment_info::H, KDL::Frame::Identity(), KDL::Rotation::Inverse(), KDL::Frame::Inverse(), KDL::Frame::M, KDL::Joint::None, ns, KDL::Segment::pose(), results, KDL::Segment::twist(), KDL::ChainIdSolver_Vereshchagin::segment_info::U, KDL::ChainIdSolver_Vereshchagin::segment_info::v, and KDL::ChainIdSolver_Vereshchagin::segment_info::Z.
Referenced by CartToJnt().
|
private |
Referenced by ChainIdSolver_Vereshchagin(), constraint_calculation(), final_upwards_sweep(), and initial_upwards_sweep().
|
private |
|
private |
|
private |
|
private |
Referenced by downwards_sweep(), final_upwards_sweep(), and initial_upwards_sweep().
|
private |
Referenced by downwards_sweep(), and initial_upwards_sweep().
|
private |
Referenced by ChainIdSolver_Vereshchagin(), and constraint_calculation().
|
private |
Referenced by CartToJnt(), ChainIdSolver_Vereshchagin(), constraint_calculation(), and downwards_sweep().
|
private |
Referenced by CartToJnt(), and downwards_sweep().
|
private |
Referenced by CartToJnt(), downwards_sweep(), final_upwards_sweep(), and initial_upwards_sweep().
|
private |
Referenced by constraint_calculation(), and final_upwards_sweep().
|
private |
Referenced by ChainIdSolver_Vereshchagin(), and constraint_calculation().
|
private |
|
private |
Referenced by constraint_calculation(), downwards_sweep(), final_upwards_sweep(), and initial_upwards_sweep().
|
private |
Referenced by ChainIdSolver_Vereshchagin(), and constraint_calculation().
|
private |
Referenced by ChainIdSolver_Vereshchagin(), and constraint_calculation().
|
private |
Referenced by ChainIdSolver_Vereshchagin(), and constraint_calculation().
|
private |
Referenced by ChainIdSolver_Vereshchagin(), and constraint_calculation().