Go to the documentation of this file.
16 #include <boost/test/unit_test.hpp>
17 #include <boost/utility/binary.hpp>
19 BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE)
23 using namespace Eigen;
30 VectorXd
q = VectorXd::Ones(
model.nq);
35 VectorXd
v = VectorXd::Ones(
model.nv);
36 VectorXd
tau = VectorXd::Zero(
model.nv);
38 const std::string RF =
"rleg6_joint";
39 const std::string LF =
"lleg6_joint";
48 Eigen::MatrixXd
J(12,
model.nv);
50 J.topRows<6>() = J_RF;
51 J.bottomRows<6>() = J_LF;
53 Eigen::VectorXd gamma(VectorXd::Ones(12));
55 Eigen::MatrixXd
H(
J.transpose());
62 data.M.triangularView<Eigen::StrictlyLower>() =
63 data.M.transpose().triangularView<Eigen::StrictlyLower>();
65 MatrixXd Minv(
data.M.inverse());
66 MatrixXd JMinvJt(
J * Minv *
J.transpose());
68 Eigen::MatrixXd G_ref(
J.transpose());
70 for (
int k = 0; k <
model.nv; ++k)
71 G_ref.row(k) /= sqrt(
data.D[k]);
72 Eigen::MatrixXd H_ref(G_ref.transpose() * G_ref);
73 BOOST_CHECK(H_ref.isApprox(JMinvJt, 1e-12));
75 VectorXd lambda_ref = -JMinvJt.inverse() * (
J * Minv * (
tau -
data.nle) + gamma);
76 BOOST_CHECK(
data.lambda_c.isApprox(lambda_ref, 1e-12));
78 VectorXd a_ref = Minv * (
tau -
data.nle +
J.transpose() * lambda_ref);
80 Eigen::VectorXd dynamics_residual_ref(
81 data.M * a_ref +
data.nle -
tau -
J.transpose() * lambda_ref);
83 dynamics_residual_ref.norm()
86 Eigen::VectorXd constraint_residual(
J *
data.ddq + gamma);
87 BOOST_CHECK(constraint_residual.norm() <= 1e-12);
89 Eigen::VectorXd dynamics_residual(
91 BOOST_CHECK(dynamics_residual.norm() <= 1e-12);
96 using namespace Eigen;
102 VectorXd
q = VectorXd::Ones(
model.nq);
107 const std::string RF =
"rleg6_joint";
108 const std::string LF =
"lleg6_joint";
117 Eigen::MatrixXd
J(12,
model.nv);
119 J.topRows<6>() = J_RF;
120 J.bottomRows<6>() = J_LF;
124 data_ref.
M.triangularView<Eigen::StrictlyLower>() =
125 data_ref.
M.transpose().triangularView<Eigen::StrictlyLower>();
127 Eigen::MatrixXd MJtJ(
model.nv + 12,
model.nv + 12);
128 MJtJ << data_ref.
M,
J.transpose(),
J, Eigen::MatrixXd::Zero(12, 12);
130 Eigen::MatrixXd KKTMatrix_inv(
model.nv + 12,
model.nv + 12);
133 BOOST_CHECK(KKTMatrix_inv.isApprox(MJtJ.inverse()));
138 using namespace Eigen;
144 VectorXd
q = VectorXd::Ones(
model.nq);
149 VectorXd
v = VectorXd::Ones(
model.nv);
150 VectorXd
tau = VectorXd::Zero(
model.nv);
152 const std::string RF =
"rleg6_joint";
153 const std::string LF =
"lleg6_joint";
162 Eigen::MatrixXd
J(12,
model.nv);
164 J.topRows<6>() = J_RF;
165 J.bottomRows<6>() = J_LF;
167 Eigen::VectorXd gamma(VectorXd::Ones(12));
169 Eigen::MatrixXd
H(
J.transpose());
177 data.M.triangularView<Eigen::StrictlyLower>() =
178 data.M.transpose().triangularView<Eigen::StrictlyLower>();
180 Eigen::MatrixXd MJtJ(
model.nv + 12,
model.nv + 12);
181 MJtJ <<
data.M,
J.transpose(),
J, Eigen::MatrixXd::Zero(12, 12);
183 Eigen::MatrixXd KKTMatrix_inv(
model.nv + 12,
model.nv + 12);
190 BOOST_CHECK(KKTMatrix_inv.isApprox(MJtJ.inverse()));
194 VectorXd v_before = VectorXd::Ones(
model.nv);
201 data.M.triangularView<Eigen::StrictlyLower>() =
202 data.M.transpose().triangularView<Eigen::StrictlyLower>();
203 MJtJ <<
data.M,
J.transpose(),
J, Eigen::MatrixXd::Zero(12, 12);
210 BOOST_CHECK(KKTMatrix_inv.isApprox(MJtJ.inverse()));
215 using namespace Eigen;
222 VectorXd
q = VectorXd::Ones(
model.nq);
227 VectorXd
v = VectorXd::Ones(
model.nv);
228 VectorXd
tau = VectorXd::Zero(
model.nv);
230 const std::string RF =
"rleg6_joint";
236 Eigen::MatrixXd
J(12,
model.nv);
238 J.topRows<6>() = J_RF;
239 J.bottomRows<6>() = J_RF;
241 Eigen::VectorXd gamma(VectorXd::Ones(12));
250 Eigen::MatrixXd
H(
J.transpose());
251 data.M.triangularView<Eigen::StrictlyLower>() =
252 data.M.transpose().triangularView<Eigen::StrictlyLower>();
254 MatrixXd Minv(
data.M.inverse());
255 MatrixXd JMinvJt(
J * Minv *
J.transpose());
258 Eigen::MatrixXd G_ref(
J.transpose());
260 for (
int k = 0; k <
model.nv; ++k)
261 G_ref.row(k) /= sqrt(
data.D[k]);
262 Eigen::MatrixXd H_ref(G_ref.transpose() * G_ref);
263 BOOST_CHECK(H_ref.isApprox(JMinvJt, 1e-12));
266 Eigen::VectorXd constraint_residual(
J *
data.ddq + gamma);
267 Eigen::VectorXd dynamics_residual(
269 BOOST_CHECK(constraint_residual.norm() <= 1e-9);
270 BOOST_CHECK(dynamics_residual.norm() <= 1e-12);
275 using namespace Eigen;
282 VectorXd
q = VectorXd::Ones(
model.nq);
287 VectorXd v_before = VectorXd::Ones(
model.nv);
289 const std::string RF =
"rleg6_joint";
290 const std::string LF =
"lleg6_joint";
299 Eigen::MatrixXd
J(12,
model.nv);
301 J.topRows<6>() = J_RF;
302 J.bottomRows<6>() = J_LF;
306 Eigen::MatrixXd
H(
J.transpose());
313 data.M.triangularView<Eigen::StrictlyLower>() =
314 data.M.transpose().triangularView<Eigen::StrictlyLower>();
316 MatrixXd Minv(
data.M.inverse());
317 MatrixXd JMinvJt(
J * Minv *
J.transpose());
319 Eigen::MatrixXd G_ref(
J.transpose());
321 for (
int k = 0; k <
model.nv; ++k)
322 G_ref.row(k) /= sqrt(
data.D[k]);
323 Eigen::MatrixXd H_ref(G_ref.transpose() * G_ref);
324 BOOST_CHECK(H_ref.isApprox(JMinvJt, 1e-12));
326 VectorXd lambda_ref = JMinvJt.inverse() * (-
r_coeff *
J * v_before -
J * v_before);
327 BOOST_CHECK(
data.impulse_c.isApprox(lambda_ref, 1e-12));
329 VectorXd v_after_ref = Minv * (
data.M * v_before +
J.transpose() * lambda_ref);
331 Eigen::VectorXd constraint_residual(
J *
data.dq_after +
r_coeff *
J * v_before);
332 BOOST_CHECK(constraint_residual.norm() <= 1e-12);
334 Eigen::VectorXd dynamics_residual(
336 BOOST_CHECK(dynamics_residual.norm() <= 1e-12);
341 using namespace Eigen;
349 #ifdef _INTENSE_TESTING_
350 const size_t NBT = 1000 * 1000;
352 const size_t NBT = 100;
356 const size_t NBT = 1;
357 std::cout <<
"(the time score in debug mode is not relevant) ";
358 #endif // ifndef NDEBUG
360 VectorXd
q = VectorXd::Ones(
model.nq);
365 VectorXd
v = VectorXd::Ones(
model.nv);
366 VectorXd
tau = VectorXd::Zero(
model.nv);
368 const std::string RF =
"rleg6_joint";
369 const std::string LF =
"lleg6_joint";
378 Eigen::MatrixXd
J(12,
model.nv);
379 J.topRows<6>() = J_RF;
380 J.bottomRows<6>() = J_LF;
382 Eigen::VectorXd gamma(VectorXd::Ones(12));
384 model.lowerPositionLimit.head<7>().
fill(-1.);
385 model.upperPositionLimit.head<7>().
fill(1.);
398 timer.
toc(std::cout, NBT);
401 BOOST_AUTO_TEST_SUITE_END()
MatrixXs M
The joint space inertia matrix (a square matrix of dim model.nv).
PINOCCHIO_DEPRECATED void getKKTContactDynamicMatrixInverse(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConstraintMatrixType > &J, const Eigen::MatrixBase< KKTMatrixType > &KKTMatrix_inv)
Computes the inverse of the KKT matrix for dynamics with contact constraints.
#define PINOCCHIO_COMPILER_DIAGNOSTIC_POP
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...
const DataTpl< Scalar, Options, JointCollectionTpl >::Matrix6x & computeJointJacobians(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q)
Computes the full model Jacobian, i.e. the stack of all motion subspace expressed in the world frame....
void computeKKTContactDynamicMatrixInverse(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q, const Eigen::MatrixBase< ConstraintMatrixType > &J, const Eigen::MatrixBase< KKTMatrixType > &KKTMatrix_inv, const Scalar &inv_damping=0.)
Computes the inverse of the KKT matrix for dynamics with contact constraints. It computes the followi...
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.
void getJointJacobian(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const DataTpl< Scalar, Options, JointCollectionTpl > &data, const JointIndex joint_id, const ReferenceFrame reference_frame, const Eigen::MatrixBase< Matrix6Like > &J)
Computes the Jacobian of a specific joint frame expressed in one of the pinocchio::ReferenceFrame opt...
const PINOCCHIO_DEPRECATED DataTpl< Scalar, Options, JointCollectionTpl >::TangentVectorType & forwardDynamics(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q, const Eigen::MatrixBase< TangentVectorType1 > &v, const Eigen::MatrixBase< TangentVectorType2 > &tau, const Eigen::MatrixBase< ConstraintMatrixType > &J, const Eigen::MatrixBase< DriftVectorType > &gamma, const Scalar inv_damping=0.)
Compute the forward dynamics with contact constraints. Internally, pinocchio::computeAllTerms is call...
#define PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
#define PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH
macros for pragma push/pop/ignore deprecated warnings
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.
Eigen::Matrix< Scalar, 6, Eigen::Dynamic, Options > Matrix6x
The 6d jacobian type (temporary)
const PINOCCHIO_DEPRECATED DataTpl< Scalar, Options, JointCollectionTpl >::TangentVectorType & impulseDynamics(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q, const Eigen::MatrixBase< TangentVectorType > &v_before, const Eigen::MatrixBase< ConstraintMatrixType > &J, const Scalar r_coeff=0., const Scalar inv_damping=0.)
Compute the impulse dynamics with contact constraints. Internally, pinocchio::crba is called.
void normalize(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ConfigVectorType > &qout)
Normalize a configuration vector.
Main pinocchio namespace.
pinocchio
Author(s):
autogenerated on Sun Dec 22 2024 03:41:08