Go to the documentation of this file.
23 #include <Eigen/Cholesky>
26 #include <boost/test/unit_test.hpp>
27 #include <boost/utility/binary.hpp>
29 BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE)
33 using namespace Eigen;
40 model.lowerPositionLimit.head<3>().
fill(-1.);
41 model.upperPositionLimit.head<3>().
fill(1.);
47 data.M.triangularView<Eigen::StrictlyLower>() =
48 data.M.triangularView<Eigen::StrictlyUpper>().transpose();
50 const Eigen::MatrixXd &
U =
data.U;
51 const Eigen::VectorXd &
D =
data.D;
52 const Eigen::MatrixXd &
M =
data.M;
55 std::cout <<
"M = [\n" <<
M <<
"];" << std::endl;
56 std::cout <<
"U = [\n" <<
U <<
"];" << std::endl;
57 std::cout <<
"D = [\n" <<
D.transpose() <<
"];" << std::endl;
60 BOOST_CHECK(
M.isApprox(
U *
D.asDiagonal() *
U.transpose(), 1e-12));
62 Eigen::VectorXd
v = Eigen::VectorXd::Random(
model.nv);
65 Eigen::VectorXd
Uv =
v;
67 BOOST_CHECK(
Uv.isApprox(
U *
v, 1e-12));
69 Eigen::VectorXd
Utv =
v;
71 BOOST_CHECK(
Utv.isApprox(
U.transpose() *
v, 1e-12));
73 Eigen::VectorXd
Uiv =
v;
75 BOOST_CHECK(
Uiv.isApprox(
U.inverse() *
v, 1e-12));
77 Eigen::VectorXd
Utiv =
v;
79 BOOST_CHECK(
Utiv.isApprox(
U.transpose().inverse() *
v, 1e-12));
81 Eigen::VectorXd Miv =
v;
83 BOOST_CHECK(Miv.isApprox(
M.inverse() *
v, 1e-12));
85 Eigen::VectorXd
Mv =
v;
87 BOOST_CHECK(
Mv.isApprox(
M *
v, 1e-12));
90 BOOST_CHECK(
Mv.isApprox(
M *
v, 1e-12));
103 using namespace Eigen;
110 model.lowerPositionLimit.head<3>().
fill(-1.);
111 model.upperPositionLimit.head<3>().
fill(1.);
116 long flag = BOOST_BINARY(1111111);
119 #ifdef _INTENSE_TESTING_
120 const size_t NBT = 1000 * 1000;
122 const size_t NBT = 10;
125 const size_t NBT = 1;
126 std::cout <<
"(the time score in debug mode is not relevant) ";
129 bool verbose = flag & (flag - 1);
131 std::cout <<
"--" << std::endl;
141 std::cout <<
"Decompose =\t";
142 timer.
toc(std::cout, NBT);
148 Eigen::VectorXd
v = Eigen::VectorXd::Random(
model.nv);
152 Eigen::LDLT<Eigen::MatrixXd> Mchol(
data.M);
153 res = Mchol.solve(
v);
156 std::cout <<
"Eigen::LDLt =\t";
157 timer.
toc(std::cout, NBT);
162 std::vector<Eigen::VectorXd> randvec(NBT);
163 for (
size_t i = 0;
i < NBT; ++
i)
164 randvec[
i] = Eigen::VectorXd::Random(
model.nv);
165 Eigen::VectorXd
zero = Eigen::VectorXd::Zero(
model.nv);
176 std::cout <<
"solve =\t\t";
177 timer.
toc(std::cout, NBT);
188 std::cout <<
"Uv =\t\t";
189 timer.
toc(std::cout, NBT);
200 std::cout <<
"Uiv =\t\t";
201 timer.
toc(std::cout, NBT);
212 std::cout <<
"Mv =\t\t";
213 timer.
toc(std::cout, NBT);
223 std::cout <<
"UDUtv =\t\t";
224 timer.
toc(std::cout, NBT);
231 using namespace Eigen;
238 model.lowerPositionLimit.head<3>().
fill(-1.);
239 model.upperPositionLimit.head<3>().
fill(1.);
242 data.M.triangularView<Eigen::StrictlyLower>() =
243 data.M.triangularView<Eigen::StrictlyUpper>().transpose();
244 MatrixXd Minv_ref(
data.M.inverse());
247 VectorXd v_unit(VectorXd::Unit(
model.nv, 0));
249 VectorXd Ui_v_unit(
model.nv);
250 VectorXd Ui_v_unit_ref(
model.nv);
252 for (
int k = 0; k <
model.nv; ++k)
254 v_unit = VectorXd::Unit(
model.nv, k);
256 cholesky::internal::Miunit(
model,
data, k, Ui_v_unit);
257 Ui_v_unit_ref = v_unit;
259 Ui_v_unit_ref.array() *=
data.Dinv.array();
261 BOOST_CHECK(Ui_v_unit.head(k + 1).isApprox(Ui_v_unit_ref.head(k + 1)));
263 Ui_v_unit_ref = v_unit;
265 BOOST_CHECK(Ui_v_unit.head(k + 1).isApprox(Ui_v_unit_ref.head(k + 1)));
272 BOOST_CHECK(Minv.isApprox(Minv_ref));
276 BOOST_CHECK(Minv.isApprox(Minv_ref));
283 BOOST_CHECK(data_bis.
Minv.isApprox(Minv_ref));
286 BOOST_AUTO_TEST_SUITE_END()
BOOST_AUTO_TEST_CASE(test_cholesky)
Mat & Uv(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< Mat > &v)
Perform the sparse multiplication using the Cholesky decomposition stored in data and acting in plac...
void humanoidRandom(ModelTpl< Scalar, Options, JointCollectionTpl > &model, bool usingFF=true)
Create a humanoid kinematic tree with 6-DOF limbs and random joint placements.
const DataTpl< Scalar, Options, JointCollectionTpl >::MatrixXs & crba(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q, const Convention convention=Convention::LOCAL)
Computes the upper triangular part of the joint space inertia matrix M by using the Composite Rigid B...
Mat & UDUtv(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< Mat > &m)
Performs the multiplication by using the Cholesky decomposition of M stored in data.
void randomConfiguration(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ConfigVectorIn1 > &lowerLimits, const Eigen::MatrixBase< ConfigVectorIn2 > &upperLimits, const Eigen::MatrixBase< ReturnType > &qout)
Generate a configuration vector uniformly sampled among provided limits.
Mat & solve(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< Mat > &y)
Return the solution of using the Cholesky decomposition stored in data given the entry ....
RowMatrixXs Minv
The inverse of the joint space inertia matrix (a square matrix of dim model.nv).
Mat & Uiv(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< Mat > &v)
Perform the pivot inversion using the Cholesky decomposition stored in data and acting in place.
const DataTpl< Scalar, Options, JointCollectionTpl >::MatrixXs & decompose(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data)
Compute the Cholesky decomposition of the joint space inertia matrix M contained in data.
Mat & computeMinv(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< Mat > &Minv)
Computes the inverse of the joint space inertia matrix M from its Cholesky factorization.
MatRes & Mv(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< Mat > &min, const Eigen::MatrixBase< MatRes > &mout)
Performs the multiplication by using the sparsity pattern of the M matrix.
Mat & Utv(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< Mat > &v)
Perform the sparse multiplication using the Cholesky decomposition stored in data and acting in plac...
Mat & Utiv(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< Mat > &v)
Perform the pivot inversion using the Cholesky decomposition stored in data and acting in place.
Main pinocchio namespace.
pinocchio
Author(s):
autogenerated on Sun Dec 22 2024 03:41:06