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