Go to the documentation of this file.
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;
segment_info(unsigned int nc)
Definition: chainidsolver_vereshchagin.hpp:172
Jacobian alfa_N2
Definition: chainidsolver_vereshchagin.hpp:131
double data[3]
Definition: frames.hpp:163
void downwards_sweep(const Jacobian &alfa, const JntArray &torques)
This method is a force balance sweep.
Definition: chainidsolver_vereshchagin.cpp:133
std::vector< Twist > Twists
Definition: chainidsolver_vereshchagin.hpp:42
ArticulatedBodyInertia P_tilde
Definition: chainidsolver_vereshchagin.hpp:157
unsigned int columns() const
Definition: jacobian.cpp:75
static Frame Identity()
Definition: frames.inl:695
double nullspaceAccComp
Definition: chainidsolver_vereshchagin.hpp:166
Vector rot
The rotational velocity of that point.
Definition: frames.hpp:723
Frame Inverse() const
Gives back inverse transformation of a Frame.
Definition: frames.inl:422
double u
Definition: chainidsolver_vereshchagin.hpp:170
double D
Definition: chainidsolver_vereshchagin.hpp:160
Definition: jntarray.hpp:70
Eigen::VectorXd EZ
Definition: chainidsolver_vereshchagin.hpp:165
Rotation Inverse() const
Gives back the inverse rotation matrix of *this.
Definition: frames.inl:632
Twist C
Definition: chainidsolver_vereshchagin.hpp:153
const Joint & getJoint() const
Request the joint of the segment.
Definition: segment.hpp:118
Eigen::VectorXd nu_sum
Definition: chainidsolver_vereshchagin.hpp:137
void constraint_calculation(const JntArray &beta)
This method calculates constraint force magnitudes.
Definition: chainidsolver_vereshchagin.cpp:249
Definition: chainidsolver_vereshchagin.hpp:144
Twist A
Definition: chainidsolver_vereshchagin.hpp:154
6D Inertia of a articulated body
Definition: articulatedbodyinertia.hpp:40
@ E_NOT_UP_TO_DATE
Chain size changed.
Definition: solveri.hpp:97
Matrix6Xd E_tilde
Definition: chainidsolver_vereshchagin.hpp:162
Frame F
Definition: chainidsolver_vereshchagin.hpp:145
ArticulatedBodyInertia P
Definition: chainidsolver_vereshchagin.hpp:156
double constAccComp
Definition: chainidsolver_vereshchagin.hpp:167
Twist v
Definition: chainidsolver_vereshchagin.hpp:148
Definition: articulatedbodyinertia.cpp:28
unsigned int getNrOfSegments() const
Request the total number of segments in the chain.
Definition: chain.hpp:76
Eigen::Matrix< double, 6, 6 > Matrix6d
Definition: chainidsolver_vereshchagin.hpp:45
A concrete implementation of a 3 dimensional vector class.
Definition: frames.hpp:161
Wrench qdotdot_sum
Definition: chainidsolver_vereshchagin.hpp:140
represents both translational and rotational velocities.
Definition: frames.hpp:720
Frame F_total
Definition: chainidsolver_vereshchagin.hpp:141
Twist acc
Definition: chainidsolver_vereshchagin.hpp:149
Wrench U
Definition: chainidsolver_vereshchagin.hpp:150
Wrench R_tilde
Definition: chainidsolver_vereshchagin.hpp:152
unsigned int ns
Definition: chainidsolver_vereshchagin.hpp:127
unsigned int rows() const
Returns the number of rows (size) of the array.
Definition: jntarray.cpp:72
Vector torque
Torque that is applied at the origin of the current ref frame.
Definition: frames.hpp:882
std::vector< segment_info, Eigen::aligned_allocator< segment_info > > results
Definition: chainidsolver_vereshchagin.hpp:188
doubleAcc dot(const VectorAcc &lhs, const VectorAcc &rhs)
Definition: frameacc.inl:137
const Segment & getSegment(unsigned int nr) const
Request the nr'd segment of the chain.
Definition: chain.cpp:68
@ E_SIZE_MISMATCH
Input size does not match internal state.
Definition: solveri.hpp:99
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_0_inverse
Definition: chainidsolver_vereshchagin.hpp:132
represents both translational and rotational acceleration.
Definition: frames.hpp:879
Dynamics calculations by constraints based on Vereshchagin 1989.
Definition: chainidsolver_vereshchagin.hpp:41
double biasAccComp
Definition: chainidsolver_vereshchagin.hpp:168
Eigen::VectorXd nu
Definition: chainidsolver_vereshchagin.hpp:136
Frame pose(const double &q) const
Request the pose of the segment, given the joint position q.
Definition: segment.cpp:57
Definition: frames.hpp:570
Twist twist(const double &q, const double &qdot) const
Request the 6D-velocity of the tip of the segment, given the joint position q and the joint velocity ...
Definition: segment.cpp:62
std::vector< Wrench > Wrenches
Definition: chainidsolver.hpp:33
Eigen::VectorXd data
Definition: jntarray.hpp:72
Eigen::VectorXd Sm
Definition: chainidsolver_vereshchagin.hpp:138
Wrench PC
Definition: chainidsolver_vereshchagin.hpp:159
Eigen::MatrixXd Vm
Definition: chainidsolver_vereshchagin.hpp:134
@ None
Definition: joint.hpp:47
Matrix6Xd E
Definition: chainidsolver_vereshchagin.hpp:161
double totalBias
Definition: chainidsolver_vereshchagin.hpp:169
Vector vel
The velocity of that point.
Definition: frames.hpp:722
Definition: segment.hpp:46
std::vector< Frame > Frames
Definition: chainidsolver_vereshchagin.hpp:43
Eigen::VectorXd G
Definition: chainidsolver_vereshchagin.hpp:164
const Chain & chain
Definition: chainidsolver_vereshchagin.hpp:125
Solver interface supporting storage and description of the latest error.
Definition: solveri.hpp:85
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
Twist acc_root
Definition: chainidsolver_vereshchagin.hpp:129
ArticulatedBodyInertia H
Definition: chainidsolver_vereshchagin.hpp:155
Rotation M
Orientation of the Frame.
Definition: frames.hpp:573
Wrench R
Definition: chainidsolver_vereshchagin.hpp:151
Eigen::VectorXd tmpm
Definition: chainidsolver_vereshchagin.hpp:139
Eigen::Matrix< double, 6, 1 > Vector6d
Definition: chainidsolver_vereshchagin.hpp:44
const JointType & getType() const
Request the type of the joint.
Definition: joint.hpp:159
Eigen::MatrixXd Um
Definition: chainidsolver_vereshchagin.hpp:133
unsigned int nj
Definition: chainidsolver_vereshchagin.hpp:126
Twist Z
Definition: chainidsolver_vereshchagin.hpp:147
@ E_NOERROR
No error.
Definition: solveri.hpp:91
unsigned int nc
Definition: chainidsolver_vereshchagin.hpp:128
Eigen::MatrixXd M
Definition: chainidsolver_vereshchagin.hpp:163
Vector force
Force that is applied at the origin of the current ref frame.
Definition: frames.hpp:881
virtual void updateInternalDataStructures()
Update the internal data structures.
Definition: chainidsolver_vereshchagin.cpp:46
const RigidBodyInertia & getInertia() const
Request the inertia of the segment.
Definition: segment.hpp:128
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
Jacobian alfa_N
Definition: chainidsolver_vereshchagin.hpp:130
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
JntArray beta_N
Definition: chainidsolver_vereshchagin.hpp:135
unsigned int getNrOfJoints() const
Request the total number of joints in the chain.
Definition: chain.hpp:71
Eigen::Matrix< double, 6, Eigen::Dynamic > Matrix6Xd
Definition: chainidsolver_vereshchagin.hpp:46
Frame F_base
Definition: chainidsolver_vereshchagin.hpp:146
~ChainIdSolver_Vereshchagin()
Definition: chainidsolver_vereshchagin.hpp:57
Wrench PZ
Definition: chainidsolver_vereshchagin.hpp:158
int error
Latest error, initialized to E_NOERROR in constructor.
Definition: solveri.hpp:149
represents rotations in 3 dimensional space.
Definition: frames.hpp:302
Definition: jacobian.hpp:37