22 #ifndef KDL_CHAINIDSOLVER_VERESHCHAGIN_HPP
23 #define KDL_CHAINIDSOLVER_VERESHCHAGIN_HPP
29 #include<Eigen/StdVector>
46 typedef Eigen::Matrix<double, 6, Eigen::Dynamic>
Matrix6Xd;
188 std::vector<segment_info, Eigen::aligned_allocator<segment_info> >
results;
6D Inertia of a articulated body
Definition: articulatedbodyinertia.hpp:40
Dynamics calculations by constraints based on Vereshchagin 1989.
Definition: chainidsolver_vereshchagin.hpp:41
Wrench qdotdot_sum
Definition: chainidsolver_vereshchagin.hpp:140
ChainIdSolver_Vereshchagin(const Chain &chain, Twist root_acc, unsigned int nc)
Constructor for the solver, it will allocate all the necessary memory.
Definition: chainidsolver_vereshchagin.cpp:33
Frame F_total
Definition: chainidsolver_vereshchagin.hpp:141
Jacobian alfa_N
Definition: chainidsolver_vereshchagin.hpp:130
Eigen::MatrixXd Vm
Definition: chainidsolver_vereshchagin.hpp:134
JntArray beta_N
Definition: chainidsolver_vereshchagin.hpp:135
std::vector< segment_info, Eigen::aligned_allocator< segment_info > > results
Definition: chainidsolver_vereshchagin.hpp:188
Eigen::MatrixXd M_0_inverse
Definition: chainidsolver_vereshchagin.hpp:132
void constraint_calculation(const JntArray &beta)
This method calculates constraint force magnitudes.
Definition: chainidsolver_vereshchagin.cpp:251
Jacobian alfa_N2
Definition: chainidsolver_vereshchagin.hpp:131
std::vector< Twist > Twists
Definition: chainidsolver_vereshchagin.hpp:42
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.
Definition: chainidsolver_vereshchagin.cpp:53
void downwards_sweep(const Jacobian &alfa, const JntArray &torques)
This method is a force balance sweep.
Definition: chainidsolver_vereshchagin.cpp:135
virtual void updateInternalDataStructures()
Update the internal data structures.
Definition: chainidsolver_vereshchagin.cpp:48
unsigned int nc
Definition: chainidsolver_vereshchagin.hpp:128
~ChainIdSolver_Vereshchagin()
Definition: chainidsolver_vereshchagin.hpp:57
unsigned int ns
Definition: chainidsolver_vereshchagin.hpp:127
Eigen::VectorXd nu
Definition: chainidsolver_vereshchagin.hpp:136
std::vector< Frame > Frames
Definition: chainidsolver_vereshchagin.hpp:43
Eigen::Matrix< double, 6, Eigen::Dynamic > Matrix6Xd
Definition: chainidsolver_vereshchagin.hpp:46
Twist acc_root
Definition: chainidsolver_vereshchagin.hpp:129
const Chain & chain
Definition: chainidsolver_vereshchagin.hpp:125
Eigen::VectorXd Sm
Definition: chainidsolver_vereshchagin.hpp:138
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.
Definition: chainidsolver_vereshchagin.cpp:74
Eigen::VectorXd nu_sum
Definition: chainidsolver_vereshchagin.hpp:137
void final_upwards_sweep(JntArray &q_dotdot, JntArray &torques)
This method puts all acceleration contributions (constraint, bias, nullspace and parent accelerations...
Definition: chainidsolver_vereshchagin.cpp:286
Eigen::MatrixXd Um
Definition: chainidsolver_vereshchagin.hpp:133
Eigen::Matrix< double, 6, 1 > Vector6d
Definition: chainidsolver_vereshchagin.hpp:44
unsigned int nj
Definition: chainidsolver_vereshchagin.hpp:126
Eigen::VectorXd tmpm
Definition: chainidsolver_vereshchagin.hpp:139
Eigen::Matrix< double, 6, 6 > Matrix6d
Definition: chainidsolver_vereshchagin.hpp:45
Definition: frames.hpp:570
Definition: jacobian.hpp:37
Definition: jntarray.hpp:70
Solver interface supporting storage and description of the latest error.
Definition: solveri.hpp:85
represents both translational and rotational velocities.
Definition: frames.hpp:720
represents both translational and rotational acceleration.
Definition: frames.hpp:879
Definition: articulatedbodyinertia.cpp:28
std::vector< Wrench > Wrenches
Definition: chainfdsolver.hpp:33
Definition: chainidsolver_vereshchagin.hpp:144
Wrench R
Definition: chainidsolver_vereshchagin.hpp:151
double biasAccComp
Definition: chainidsolver_vereshchagin.hpp:168
ArticulatedBodyInertia P_tilde
Definition: chainidsolver_vereshchagin.hpp:157
Wrench U
Definition: chainidsolver_vereshchagin.hpp:150
Eigen::MatrixXd M
Definition: chainidsolver_vereshchagin.hpp:163
Wrench PC
Definition: chainidsolver_vereshchagin.hpp:159
Wrench PZ
Definition: chainidsolver_vereshchagin.hpp:158
ArticulatedBodyInertia H
Definition: chainidsolver_vereshchagin.hpp:155
double D
Definition: chainidsolver_vereshchagin.hpp:160
Frame F_base
Definition: chainidsolver_vereshchagin.hpp:146
Eigen::VectorXd G
Definition: chainidsolver_vereshchagin.hpp:164
Matrix6Xd E_tilde
Definition: chainidsolver_vereshchagin.hpp:162
double u
Definition: chainidsolver_vereshchagin.hpp:170
Matrix6Xd E
Definition: chainidsolver_vereshchagin.hpp:161
Twist Z
Definition: chainidsolver_vereshchagin.hpp:147
Frame F
Definition: chainidsolver_vereshchagin.hpp:145
double constAccComp
Definition: chainidsolver_vereshchagin.hpp:167
Twist C
Definition: chainidsolver_vereshchagin.hpp:153
ArticulatedBodyInertia P
Definition: chainidsolver_vereshchagin.hpp:156
Wrench R_tilde
Definition: chainidsolver_vereshchagin.hpp:152
Eigen::VectorXd EZ
Definition: chainidsolver_vereshchagin.hpp:165
double nullspaceAccComp
Definition: chainidsolver_vereshchagin.hpp:166
segment_info(unsigned int nc)
Definition: chainidsolver_vereshchagin.hpp:172
Twist acc
Definition: chainidsolver_vereshchagin.hpp:149
double totalBias
Definition: chainidsolver_vereshchagin.hpp:169
Twist v
Definition: chainidsolver_vereshchagin.hpp:148
Twist A
Definition: chainidsolver_vereshchagin.hpp:154