/// Multiplies a vector of spatial axes by a vector
SVelocityd mult(const vector<SVelocityd>& t, const VectorNd& v)
{
const unsigned SPATIAL_DIM = 6;
if (t.size() != v.size())
throw MissizeException();
// verify that the vector is not empty - we lose frame info!
if (t.empty())
throw std::runtime_error("loss of frame information");
// setup the result
SVelocityd result = SVelocityd::zero();
// verify that all twists are in the same pose
result.pose = t.front().pose;
for (unsigned i=1; i< t.size(); i++)
if (t[i].pose != result.pose)
throw FrameException();
// finally, do the computation
const double* vdata = v.data();
for (unsigned j=0; j< SPATIAL_DIM; j++)
for (unsigned i=0; i< t.size(); i++)
result[j] += t[i][j]*vdata[i];
return result;
}
int main(int argc, char const *argv[])
{
// Small test case for model wrapper
cout << "Leo Model Test Case" << endl;
// Initialize model from lua file
cout << "Initializing model ..." << endl;
LeoModel leo;
leo.loadModelFromFile (model_path.c_str());
cout << "... successful!" << endl << endl;
// Load contact points
cout << "Loading points ..." << endl;
leo.loadPointsFromFile (model_path.c_str(), true);
cout << "... successful!" << endl << endl;
// Load constraint sets
cout << "Loading constraint sets ..." << endl;
leo.loadConstraintSetsFromFile (model_path.c_str(), true);
cout << "... successful!" << endl;
VectorNd q = VectorNd::Zero(leo.nDof);
VectorNd qdot = VectorNd::Zero(leo.nDof);
VectorNd qddot = VectorNd::Zero(leo.nDof);
VectorNd tau = VectorNd::Zero(leo.nDof);
VectorNd rhs = VectorNd::Zero(2*leo.nDof);
// provide initial values
q <<
0.05,
-0.1,
0.1,
0.0;
// 0.1,
// -0.1,
// 0.05;
// assign them to model
leo.q = q;
leo.qdot = qdot;
leo.qddot = qddot;
leo.activeConstraintSet = "";
cout << "Computing inverse dynamics ... " << endl;
cout << " q: " << leo.q.transpose() << endl;
cout << " qdot: " << leo.qdot.transpose() << endl;
cout << "for desired accelerations:" << endl;
cout << " qddot: " << leo.qddot.transpose() << endl;
leo.updateInverseDynamics();
cout << "needed torques:" << endl;
cout << " taus: " << leo.tau.transpose() << endl;
cout << "... successful!" << endl;
cout << "Checking forward dynamics ... " << endl;
cout << " q: " << leo.q.transpose() << endl;
cout << " qdot: " << leo.qdot.transpose() << endl;
cout << "for given torques:" << endl;
cout << " taus: " << leo.tau.transpose() << endl;
leo.updateForwardDynamics();
cout << "resulting accelerations:" << endl;
cout << " qddot: " << leo.qddot.transpose() << endl;
leo.calcForwardDynamicsRhs(rhs.data());
cout << "resulting accelerations:" << endl;
cout << " rhs: " << rhs.transpose() << endl;
cout << "... successful!" << endl;
cout << endl;
// provide initial values
leo.tau <<
0.1,
0.1,
0.1,
0.1;
// 0.1,
// -0.1,
// 0.05;
cout << "Checking forward dynamics ... " << endl;
cout << " q: " << leo.q.transpose() << endl;
cout << " qdot: " << leo.qdot.transpose() << endl;
cout << "for given torques:" << endl;
cout << " taus: " << leo.tau.transpose() << endl;
leo.updateForwardDynamics();
cout << "resulting accelerations:" << endl;
cout << " qddot: " << leo.qddot.transpose() << endl;
cout << endl;
leo.calcForwardDynamicsRhs(rhs.data());
cout << "resulting accelerations:" << endl;
cout << " rhs: " << rhs.transpose() << endl;
cout << "... successful!" << endl;
cout << endl;
return 0;
// // calculate mass matrix
// MatrixNd H = leo.calcMassMatrix();
// cout << "Computing mass matrix ... " << endl;
// cout << H << endl;
// cout << "... successful!" << endl;
// // calculate constraint Jacobian for given constraint set
// MatrixNd G = leo.calcContactJacobian();
// cout << "Computing contact Jacobian ... " << endl;
// cout << G << endl;
// cout << G.transpose() << endl;
// cout << "... successful!" << endl;
// return 0;
}
