24 #ifndef KDL_CHAINHDSOLVER_VERESHCHAGIN_HPP 25 #define KDL_CHAINHDSOLVER_VERESHCHAGIN_HPP 31 #include<Eigen/StdVector> 372 typedef Eigen::Matrix<double, 6, Eigen::Dynamic>
Matrix6Xd;
515 D(0),nullspaceAccComp(0),constAccComp(0),biasAccComp(0),totalBias(0),u(0)
518 E_tilde.resize(6, nc);
530 std::vector<segment_info, Eigen::aligned_allocator<segment_info> >
results;
535 #endif // KDL_CHAINHDSOLVER_VERESHCHAGIN_HPP
void final_upwards_sweep(JntArray &q_dotdot, JntArray &constraint_torques)
Eigen::Matrix< double, 6, Eigen::Dynamic > Matrix6Xd
Eigen::Matrix< double, 6, 6 > Matrix6d
std::vector< Twist > Twists
void getTotalTorque(JntArray &total_tau)
This class encapsulates a serial kinematic interconnection structure. It is built out of segments...
virtual void updateInternalDataStructures()
This class represents an fixed size array containing joint values of a KDL::Chain.
void initial_upwards_sweep(const JntArray &q, const JntArray &q_dot, const JntArray &q_dotdot, const Wrenches &f_ext)
represents both translational and rotational velocities.
std::vector< Frame > Frames
6D Inertia of a articulated body
void downwards_sweep(const Jacobian &alfa, const JntArray &ff_torques)
void getTransformedLinkAcceleration(Twists &x_dotdot)
void getContraintForceMagnitude(Eigen::VectorXd &nu_)
segment_info(unsigned int nc)
ArticulatedBodyInertia P_tilde
int CartToJnt(const JntArray &q, const JntArray &q_dot, JntArray &q_dotdot, const Jacobian &alfa, const JntArray &beta, const Wrenches &f_ext, const JntArray &ff_torques, JntArray &constraint_torques)
std::vector< Wrench > Wrenches
Eigen::MatrixXd M_0_inverse
Eigen::Matrix< double, 6, 1 > Vector6d
void constraint_calculation(const JntArray &beta)
ChainHdSolver_Vereshchagin(const Chain &chain, const Twist &root_acc, const unsigned int nc)
represents a frame transformation in 3D space (rotation + translation)
represents both translational and rotational acceleration.
Eigen::VectorXd total_torques
~ChainHdSolver_Vereshchagin()
Abstract: Acceleration constrained hybrid dynamics calculations for a chain, based on Vereshchagin 19...
std::vector< segment_info, Eigen::aligned_allocator< segment_info > > results