void rk4TimeStep(StateMatrix& s, const ControlMatrix& c, const
SimulationParameters& params, const Map & m)
{
// CHANGED(AL): Now implementing RK4
StateMatrix k1, k2, k3, k4;
uint n_states = s.cols();
uint n = s.rows();
k1 = StateMatrix(n, n_states);
k2 = StateMatrix(n, n_states);
k3 = StateMatrix(n, n_states);
k4 = StateMatrix(n, n_states);
float dt = params.timeStep;
derivatives(s, k1, c, params, m);
derivatives(s + 0.5*k1*dt, k2, c, params, m);
derivatives(s + 0.5*k2*dt, k3, c, params, m);
derivatives(s + k3*dt, k4, c, params, m);
s += (k1 + 2.*k2 + 2.*k3 + k4) * dt/6.;
}
// General outline: the model can be written:
// M(o)o_dotdot + Q(o,o_dot)o_dot + G(o) = tau
// When we linearize to the first order around position O, we get the approximation: o = O + e and
// M(O)e_dotdot + Q(O,O_dot)e_dot + G(O) + grad(G)(O)e = tau
// Or:
// (e)_dot = I e_dot + 0
// (e_dot)_dot = -M^-1 Q e_dot -M^-1 G e + M^-1 tau - G(O)
// We first compute the matrices M,Q,G and then linearize the gravity term.
bool SerialChainModel::getLinearization(const StateVector & x, StateMatrix & A, InputMatrix & B, InputVector & c)
{
// ROS_DEBUG("1");
assert(inputs_>0);
const int n=inputs_;
KDL::JntArray kdl_q(n);
KDL::JntArray kdl_q_dot(n);
// ROS_DEBUG("2");
//Fills the data
for(int i=0; i<n; ++i)
{
kdl_q(i) = x(i);
kdl_q_dot(i) = x(i+n);
}
// ROS_DEBUG("3");
//Compute M
Eigen::MatrixXd M(n,n);
NEWMAT::Matrix mass(n,n);
chain_->computeMassMatrix(kdl_q,kdl_torque_,mass);
for(int i=0;i<n;i++)
for(int j=0;j<n;j++)
M(i,j)=mass(i+1,j+1);
// ROS_DEBUG("4");
//Compute Q
Eigen::MatrixXd Q(n,n);
Q.setZero();
NEWMAT::Matrix christoffel(n*n,n);
chain_->computeChristoffelSymbols(kdl_q,kdl_torque_,christoffel);
// ROS_DEBUG("5-");
for(int i=0;i<n;i++)
for(int j=0;j<n;j++)
for(int k=0;k<n;k++)
Q(i,j)+=christoffel(i*n+j+1,k+1)*kdl_q_dot(j);
// ROS_DEBUG("5");
//Compute G
Eigen::VectorXd G(n,1);
chain_->computeGravityTerms(kdl_q,kdl_torque_);
for(int i=0;i<n;i++)
G(i)=kdl_torque_[i][2];
// ROS_DEBUG("6");
//Computing the gradient of G
Eigen::MatrixXd gG(n,n);
const double epsi=0.01;
for(int i=0;i<n;i++)
{
KDL::JntArray ja = kdl_q;
ja(i)=ja(i)+epsi;
chain_->computeGravityTerms(ja,kdl_torque_);
for(int j=0;j<n;j++)
gG(i,j)=(kdl_torque_[j][2]-G(j))/epsi;
}
// ROS_DEBUG("7");
// Final assembling
Eigen::MatrixXd Minvminus=-M.inverse(); //Computing M's inverse once
ROS_DEBUG_STREAM(A.rows());
assert(A.rows()==n*2);
assert(A.cols()==n*2);
A.block(0,0,n,n).setZero();
A.block(0,n,n,n)=Eigen::MatrixXd::Identity(n,n);
A.block(n,0,n,n)=Minvminus*gG;
A.block(n,n,n,n)=Minvminus*Q;
assert(B.rows()==n*2);
assert(B.cols()==n);
B.block(0,0,n,n).setZero();
B.block(n,0,n,n)=Minvminus;
// ROS_DEBUG("8");
c=-G;
return true;
}