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;
173 D(0),nullspaceAccComp(0),constAccComp(0),biasAccComp(0),totalBias(0),u(0)
176 E_tilde.resize(6, nc);
188 std::vector<segment_info, Eigen::aligned_allocator<segment_info> >
results;
Matrix6Xd E_tilde
Definition: chainidsolver_vereshchagin.hpp:162
double totalBias
Definition: chainidsolver_vereshchagin.hpp:169
ArticulatedBodyInertia P
Definition: chainidsolver_vereshchagin.hpp:156
std::vector< Frame > Frames
Definition: chainidsolver_vereshchagin.hpp:43
Eigen::VectorXd G
Definition: chainidsolver_vereshchagin.hpp:164
Eigen::VectorXd tmpm
Definition: chainidsolver_vereshchagin.hpp:139
Wrench U
Definition: chainidsolver_vereshchagin.hpp:150
ArticulatedBodyInertia P_tilde
Definition: chainidsolver_vereshchagin.hpp:157
std::vector< Wrench > Wrenches
Definition: chainidsolver.hpp:33
Twist acc_root
Definition: chainidsolver_vereshchagin.hpp:129
Eigen::MatrixXd Um
Definition: chainidsolver_vereshchagin.hpp:133
double u
Definition: chainidsolver_vereshchagin.hpp:170
Wrench R
Definition: chainidsolver_vereshchagin.hpp:151
Definition: jacobian.hpp:36
Frame F
Definition: chainidsolver_vereshchagin.hpp:145
const Chain & chain
Definition: chainidsolver_vereshchagin.hpp:125
This class encapsulates a serial kinematic interconnection structure.
Definition: chain.hpp:35
Eigen::VectorXd EZ
Definition: chainidsolver_vereshchagin.hpp:165
double constAccComp
Definition: chainidsolver_vereshchagin.hpp:167
Eigen::Matrix< double, 6, 1 > Vector6d
Definition: chainidsolver_vereshchagin.hpp:44
Twist v
Definition: chainidsolver_vereshchagin.hpp:148
JntArray beta_N
Definition: chainidsolver_vereshchagin.hpp:135
This class represents an fixed size array containing joint values of a KDL::Chain.
Definition: jntarray.hpp:69
Solver interface supporting storage and description of the latest error.
Definition: solveri.hpp:84
virtual void updateInternalDataStructures()
Update the internal data structures.
Definition: chainidsolver_vereshchagin.cpp:46
Eigen::VectorXd Sm
Definition: chainidsolver_vereshchagin.hpp:138
Wrench PC
Definition: chainidsolver_vereshchagin.hpp:159
Wrench PZ
Definition: chainidsolver_vereshchagin.hpp:158
double D
Definition: chainidsolver_vereshchagin.hpp:160
ArticulatedBodyInertia H
Definition: chainidsolver_vereshchagin.hpp:155
Jacobian alfa_N2
Definition: chainidsolver_vereshchagin.hpp:131
Wrench R_tilde
Definition: chainidsolver_vereshchagin.hpp:152
unsigned int nj
Definition: chainidsolver_vereshchagin.hpp:126
represents both translational and rotational velocities.
Definition: frames.hpp:720
Twist acc
Definition: chainidsolver_vereshchagin.hpp:149
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:51
Definition: articulatedbodyinertia.cpp:28
6D Inertia of a articulated body
Definition: articulatedbodyinertia.hpp:40
~ChainIdSolver_Vereshchagin()
Definition: chainidsolver_vereshchagin.hpp:57
Twist Z
Definition: chainidsolver_vereshchagin.hpp:147
void constraint_calculation(const JntArray &beta)
This method calculates constraint force magnitudes.
Definition: chainidsolver_vereshchagin.cpp:249
Jacobian alfa_N
Definition: chainidsolver_vereshchagin.hpp:130
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:284
Wrench qdotdot_sum
Definition: chainidsolver_vereshchagin.hpp:140
Twist A
Definition: chainidsolver_vereshchagin.hpp:154
Frame F_base
Definition: chainidsolver_vereshchagin.hpp:146
double biasAccComp
Definition: chainidsolver_vereshchagin.hpp:168
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:31
Eigen::MatrixXd M
Definition: chainidsolver_vereshchagin.hpp:163
void downwards_sweep(const Jacobian &alfa, const JntArray &torques)
This method is a force balance sweep.
Definition: chainidsolver_vereshchagin.cpp:133
Eigen::Matrix< double, 6, Eigen::Dynamic > Matrix6Xd
Definition: chainidsolver_vereshchagin.hpp:46
unsigned int nc
Definition: chainidsolver_vereshchagin.hpp:128
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:72
represents a frame transformation in 3D space (rotation + translation)
Definition: frames.hpp:570
Twist C
Definition: chainidsolver_vereshchagin.hpp:153
represents both translational and rotational acceleration.
Definition: frames.hpp:878
double nullspaceAccComp
Definition: chainidsolver_vereshchagin.hpp:166
std::vector< segment_info, Eigen::aligned_allocator< segment_info > > results
Definition: chainidsolver_vereshchagin.hpp:188
Eigen::VectorXd nu_sum
Definition: chainidsolver_vereshchagin.hpp:137
Eigen::MatrixXd M_0_inverse
Definition: chainidsolver_vereshchagin.hpp:132
Definition: chainidsolver_vereshchagin.hpp:143
Eigen::VectorXd nu
Definition: chainidsolver_vereshchagin.hpp:136
Frame F_total
Definition: chainidsolver_vereshchagin.hpp:141
std::vector< Twist > Twists
Definition: chainidsolver_vereshchagin.hpp:42
unsigned int ns
Definition: chainidsolver_vereshchagin.hpp:127
Matrix6Xd E
Definition: chainidsolver_vereshchagin.hpp:161
Dynamics calculations by constraints based on Vereshchagin 1989.
Definition: chainidsolver_vereshchagin.hpp:40
Eigen::Matrix< double, 6, 6 > Matrix6d
Definition: chainidsolver_vereshchagin.hpp:45
segment_info(unsigned int nc)
Definition: chainidsolver_vereshchagin.hpp:172
Eigen::MatrixXd Vm
Definition: chainidsolver_vereshchagin.hpp:134