31 DynSolver(chain, _grav),
32 IdSolver(chain, _grav),
33 nj(chain.getNrOfJoints()),
34 ns(chain.getNrOfSegments()),
73 for(
unsigned int i=0;i<
nj;i++){
81 for(
unsigned int i=0;i<
nj;i++){
83 for(
unsigned int j=0;j<
nj;j++){
88 for(
unsigned int i=0;i<
nj;i++){
100 fdsolver.
CartToJnt(q, q_dot, torques, f_ext, q_dotdot);
101 for (
unsigned int i=0; i<
nj; ++i)
103 q_temp(i) = q(i) + q_dot(i)*dt/2.0;
104 q_dot_temp(i) = q_dot(i) + q_dotdot(i)*dt/2.0;
106 dq_dot(i) = q_dotdot(i);
108 fdsolver.
CartToJnt(q_temp, q_dot_temp, torques, f_ext, q_dotdot);
109 for (
unsigned int i=0; i<
nj; ++i)
111 q_temp(i) = q(i) + q_dot_temp(i)*dt/2.0;
112 q_dot_temp(i) = q_dot(i) + q_dotdot(i)*dt/2.0;
113 dq(i) += 2.0*q_dot_temp(i);
114 dq_dot(i) += 2.0*q_dotdot(i);
116 fdsolver.
CartToJnt(q_temp, q_dot_temp, torques, f_ext, q_dotdot);
117 for (
unsigned int i=0; i<
nj; ++i)
119 q_temp(i) = q(i) + q_dot_temp(i)*dt;
120 q_dot_temp(i) = q_dot(i) + q_dotdot(i)*dt;
121 dq(i) += 2.0*q_dot_temp(i);
122 dq_dot(i) += 2.0*q_dotdot(i);
124 fdsolver.
CartToJnt(q_temp, q_dot_temp, torques, f_ext, q_dotdot);
125 for (
unsigned int i=0; i<
nj; ++i)
127 dq(i) = (dq(i)+q_dot_temp(i))*dt/6.0;
128 dq_dot(i) = (dq_dot(i)+q_dotdot(i))*dt/6.0;
130 for (
unsigned int i=0; i<
nj; ++i)
133 q_dot(i) += dq_dot(i);
void RK4Integrator(unsigned int &nj, const double &t, double &dt, KDL::JntArray &q, KDL::JntArray &q_dot, KDL::JntArray &torques, KDL::Wrenches &f_ext, KDL::ChainFdSolver_RNE &fdsolver, KDL::JntArray &q_dotdot, KDL::JntArray &dq, KDL::JntArray &dq_dot, KDL::JntArray &q_temp, KDL::JntArray &q_dot_temp)
unsigned int rows() const
This class encapsulates a serial kinematic interconnection structure. It is built out of segments...
unsigned int getNrOfSegments() const
This class represents an fixed size array containing joint values of a KDL::Chain.
int ldl_solver_eigen(const Eigen::MatrixXd &A, const Eigen::VectorXd &v, Eigen::MatrixXd &L, Eigen::VectorXd &D, Eigen::VectorXd &vtmp, Eigen::VectorXd &q)
Solves the system of equations Aq = v for q via LDL decomposition, where A is a square positive defin...
Input size does not match internal state.
Eigen::VectorXd Tzeroacc_eig
Recursive newton euler forward dynamics solver.
int CartToJnt(const JntArray &q, const JntArray &q_dot, const JntArray &torques, const Wrenches &f_ext, JntArray &q_dotdot)
ChainFdSolver_RNE(const Chain &chain, Vector grav)
A concrete implementation of a 3 dimensional vector class.
int CartToJnt(const JntArray &q, const JntArray &q_dot, const JntArray &q_dotdot, const Wrenches &f_ext, JntArray &torques)
unsigned int getNrOfJoints() const
std::vector< Wrench > Wrenches
ChainIdSolver_RNE IdSolver
virtual void updateInternalDataStructures()
virtual int JntToMass(const JntArray &q, JntSpaceInertiaMatrix &H)
int error
Latest error, initialized to E_NOERROR in constructor.