void doStuff()
{
static float x =0; // Value to influence the work
static float x_inc =0.01f;
x += x_inc;
if ((x<=0.0) || (x>=1.0))
{
x_inc = -x_inc;
x += x_inc;
}
OSG::PerfMonitorGuard gG("doStuff");
{
OSG::PerfMonitorGuard g("Work1");
recursiveFunc();
randWork(unsigned(10.0*x), 1, "10*x");
randWork(unsigned(10.0*(1.0-x)), 1, "10*(1-x)");
}
{
OSG::PerfMonitorGuard g("Work2");
recursiveFunc();
randWork(unsigned(5.0*x), 1, "5x-1");
randWork(2, 10, "2-10");
}
{
OSG::PerfMonitorGuard g("Work3");
recursiveFunc();
randWork( 1, unsigned(5.0*x), "1-5x");
randWork(2, 0, "2-0");
}
{
OSG::PerfMonitorGuard g("Work4");
recursiveFunc(3);
}
{
OSG::PerfMonitorGuard g("Work5");
recursiveFunc(3);
}
{
OSG::PerfMonitorGuard g("Work6");
recursiveFunc(3);
}
{
OSG::PerfMonitorGuard g("Work7");
recursiveFunc(3);
}
}
// 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;
}
