Matrix Block::inverse(cflag tp)const{
try{
checkUni10TypeError(tp);
if(!(Rnum == Cnum)){
std::ostringstream err;
err<<"Cannot perform inversion on a non-square matrix.";
throw std::runtime_error(exception_msg(err.str()));
}
Matrix invM(*this);
assert(ongpu == invM.isOngpu());
matrixInv(invM.cm_elem, Rnum, invM.diag, invM.ongpu);
return invM;
}
catch(const std::exception& e){
propogate_exception(e, "In function Matrix::inverse(uni10::cflag ):");
return Matrix();
}
}
HmQuadToSqr<Real>::HmQuadToSqr (const Vector2<Real>& P00,
const Vector2<Real>& P10, const Vector2<Real>& P11,
const Vector2<Real>& P01)
{
// Translate to origin.
mT = P00;
Vector2<Real> Q10 = P10 - P00;
Vector2<Real> Q11 = P11 - P00;
Vector2<Real> Q01 = P01 - P00;
Matrix2<Real> invM(Q10.X(), Q01.X(), Q10.Y(), Q01.Y());
mM = invM.Inverse();
// Compute where p11 is mapped to.
Vector2<Real> corner = mM*Q11; // = (a,b)
// Compute homogeneous transform of quadrilateral
// {(0,0),(1,0),(a,b),(0,1)} to square {(0,0),(1,0),(1,1),(0,1)}.
mG.X() = (corner.Y() - (Real)1)/corner.X();
mG.Y() = (corner.X() - (Real)1)/corner.Y();
mD.X() = (Real)1 + mG.X();
mD.Y() = (Real)1 + mG.Y();
}
int
main(int argc, char** argv)
{
if (argc < 2)
{
std::cout << "Usage: rlDynamics2Demo MODELFILE Q1 ... Qn QD1 ... QDn QDD1 ... QDDn" << std::endl;
return 1;
}
try
{
rl::mdl::XmlFactory factory;
boost::shared_ptr< rl::mdl::Model > model(factory.create(argv[1]));
rl::mdl::Dynamic* dynamic = dynamic_cast< rl::mdl::Dynamic* >(model.get());
rl::math::Vector q(dynamic->getDof());
rl::math::Vector qd(dynamic->getDof());
rl::math::Vector qdd(dynamic->getDof());
rl::math::Vector tau(dynamic->getDof());
for (std::size_t i = 0; i < dynamic->getDof(); ++i)
{
q(i) = boost::lexical_cast< rl::math::Real >(argv[i + 2]);
qd(i) = boost::lexical_cast< rl::math::Real >(argv[i + 2 + dynamic->getDof()]);
qdd(i) = boost::lexical_cast< rl::math::Real >(argv[i + 2 + 2 * dynamic->getDof()]);
}
rl::math::Vector g(3);
dynamic->getWorldGravity(g);
rl::math::Vector tmp(dynamic->getDof());
rl::math::Vector tmp2(6 * dynamic->getOperationalDof());
std::cout << "===============================================================================" << std::endl;
// FP
dynamic->setPosition(q);
dynamic->forwardPosition();
const rl::math::Transform::ConstTranslationPart& position = dynamic->getOperationalPosition(0).translation();
rl::math::Vector3 orientation = dynamic->getOperationalPosition(0).rotation().eulerAngles(2, 1, 0).reverse();
std::cout << "x = " << position.x() << " m, y = " << position.y() << " m, z = " << position.z() << " m, a = " << orientation.x() * rl::math::RAD2DEG << " deg, b = " << orientation.y() * rl::math::RAD2DEG << " deg, c = " << orientation.z() * rl::math::RAD2DEG << " deg" << std::endl;
std::cout << "===============================================================================" << std::endl;
// FV
dynamic->setPosition(q);
dynamic->setVelocity(qd);
dynamic->forwardVelocity();
std::cout << "xd = " << dynamic->getOperationalVelocity(0).linear().transpose() << " " << dynamic->getOperationalVelocity(0).angular().transpose() << std::endl;
std::cout << "-------------------------------------------------------------------------------" << std::endl;
// J
rl::math::Matrix J(6 * dynamic->getOperationalDof(), dynamic->getDof());
dynamic->calculateJacobian(J);
std::cout << "J = " << std::endl << J << std::endl;
// J * qd
tmp2 = J * qd;
std::cout << "xd = " << tmp2.transpose() << std::endl;
// invJ
rl::math::Matrix invJ(dynamic->getDof(), 6 * dynamic->getOperationalDof());
dynamic->calculateJacobianInverse(J, invJ);
std::cout << "J^{-1} = " << std::endl << invJ << std::endl;
std::cout << "===============================================================================" << std::endl;
// FA
dynamic->setPosition(q);
dynamic->setVelocity(qd);
dynamic->setAcceleration(qdd);
dynamic->setWorldGravity(g);
dynamic->forwardVelocity();
dynamic->forwardAcceleration();
std::cout << "xdd = " << dynamic->getOperationalAcceleration(0).linear().transpose() << " " << dynamic->getOperationalAcceleration(0).angular().transpose() << std::endl;
std::cout << "-------------------------------------------------------------------------------" << std::endl;
// Jd * qd
rl::math::Vector Jdqd(6 * dynamic->getOperationalDof());
dynamic->calculateJacobianDerivative(Jdqd);
std::cout << "Jd*qd = " << Jdqd.transpose() << std::endl;
// J * qdd + Jd * qd
tmp2 = J * qdd + Jdqd;
std::cout << "xdd = " << tmp2.transpose() << std::endl;
std::cout << "===============================================================================" << std::endl;
// RNE
dynamic->setPosition(q);
dynamic->setVelocity(qd);
dynamic->setAcceleration(qdd);
dynamic->inverseDynamics();
dynamic->getTorque(tau);
std::cout << "tau = " << tau.transpose() << std::endl;
std::cout << "-------------------------------------------------------------------------------" << std::endl;
// M
rl::math::Matrix M(dynamic->getDof(), dynamic->getDof());
dynamic->setPosition(q);
dynamic->calculateMassMatrix(M);
std::cout << "M = " << std::endl << M << std::endl;
// V
rl::math::Vector V(dynamic->getDof());
dynamic->setPosition(q);
dynamic->setVelocity(qd);
dynamic->calculateCentrifugalCoriolis(V);
std::cout << "V = " << V.transpose() << std::endl;
// G
rl::math::Vector G(dynamic->getDof());
dynamic->setPosition(q);
dynamic->setWorldGravity(g);
dynamic->calculateGravity(G);
std::cout << "G = " << G.transpose() << std::endl;
// M * qdd + V + G
tmp = M * qdd + V + G;
std::cout << "tau = " << tmp.transpose() << std::endl;
std::cout << "===============================================================================" << std::endl;
// FD
dynamic->setPosition(q);
dynamic->setVelocity(qd);
dynamic->setTorque(tau);
dynamic->forwardDynamics();
dynamic->getAcceleration(tmp);
std::cout << "qdd = " << tmp.transpose() << std::endl;
std::cout << "-------------------------------------------------------------------------------" << std::endl;
// M^{-1}
rl::math::Matrix invM(dynamic->getDof(), dynamic->getDof());
dynamic->setPosition(q);
dynamic->calculateMassMatrixInverse(invM);
std::cout << "M^{-1} = " << std::endl << invM << std::endl;
// V
std::cout << "V = " << V.transpose() << std::endl;
// G
std::cout << "G = " << G.transpose() << std::endl;
// M^{-1} * ( tau - V - G )
tmp = invM * (tau - V - G);
std::cout << "qdd = " << tmp.transpose() << std::endl;
std::cout << "===============================================================================" << std::endl;
}
catch (const std::exception& e)
{
std::cout << e.what() << std::endl;
return 1;
}
return 0;
}
