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