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);