cppadcg/joint-configurations.cpp
Go to the documentation of this file.
1 //
2 // Copyright (c) 2020 CNRS INRIA
3 //
4 
9 
10 #include "../utils/model-generator.hpp"
11 
12 #include <boost/test/unit_test.hpp>
13 #include <boost/utility/binary.hpp>
14 
15 using namespace pinocchio;
16 
18 
19 BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE)
20 
21 BOOST_AUTO_TEST_CASE(test_joint_configuration_code_generation)
22 {
23  typedef double Scalar;
24 
26 
27  Model model;
29  Eigen::VectorXd q1 = Eigen::VectorXd::Random(model.nq);
30  Eigen::VectorXd q2 = Eigen::VectorXd::Random(model.nq);
31  normalize(model, q1);
32  normalize(model, q2);
33 
34  Eigen::VectorXd v = Eigen::VectorXd::Random(model.nv);
35  std::vector<Eigen::VectorXd> results_q(2, Eigen::VectorXd::Zero(model.nq));
36  std::vector<Eigen::VectorXd> results_v(2, Eigen::VectorXd::Zero(model.nv));
37 
38  // Integrate
39  CodeGenIntegrate<double> cg_integrate(model);
40  cg_integrate.initLib();
41  cg_integrate.compileAndLoadLib(PINOCCHIO_CXX_COMPILER);
42 
43  cg_integrate.evalFunction(q1, v, results_q[0]);
44  pinocchio::integrate(model, q1, v, results_q[1]);
45  BOOST_CHECK(results_q[1].isApprox(results_q[0]));
46 
47  cg_integrate.evalFunction(q1, 0 * v, results_q[0]);
48  BOOST_CHECK(results_q[0].isApprox(q1));
49 
50  // Difference
51  CodeGenDifference<double> cg_difference(model);
52  cg_difference.initLib();
53  cg_difference.compileAndLoadLib(PINOCCHIO_CXX_COMPILER);
54 
55  cg_difference.evalFunction(q1, q2, results_v[0]);
56  pinocchio::difference(model, q1, q2, results_v[1]);
57  BOOST_CHECK(results_v[1].isApprox(results_v[0]));
58 
59  cg_difference.evalFunction(q1, q1, results_v[0]);
60  BOOST_CHECK(results_v[0].isZero());
61 
62  // dDifference
63  CodeGenDDifference<double> cg_dDifference(model);
64  cg_dDifference.initLib();
65  cg_dDifference.compileAndLoadLib(PINOCCHIO_CXX_COMPILER);
66 
67  // ARG0
68  std::vector<Eigen::MatrixXd> results_J(2, Eigen::MatrixXd::Zero(model.nv, model.nv));
69  cg_dDifference.evalFunction(q1, q2, results_J[0], pinocchio::ARG0);
71  BOOST_CHECK(results_J[1].isApprox(results_J[0]));
72 
73  // ARG1
74  cg_dDifference.evalFunction(q1, q2, results_J[0], pinocchio::ARG1);
76  BOOST_CHECK(results_J[1].isApprox(results_J[0]));
77 
78  // ARG0
79  cg_dDifference.evalFunction(q1, q1, results_J[0], pinocchio::ARG0);
80  BOOST_CHECK((-results_J[0]).isIdentity());
81 
82  // ARG1
83  cg_dDifference.evalFunction(q1, q1, results_J[0], pinocchio::ARG1);
84  BOOST_CHECK(results_J[0].isIdentity());
85 
87  {
88  std::remove("cg_integrate_eval_.dylib");
89  std::remove("cg_difference_eval_.dylib");
90  std::remove("cg_dDifference_eval_.dylib");
91  }
92 }
93 
94 BOOST_AUTO_TEST_SUITE_END()
pinocchio::CodeGenBase::initLib
void initLib()
Definition: code-generator-base.hpp:78
pinocchio::CodeGenDifference
Definition: code-generator-algo.hpp:1135
cppadcg.hpp
pinocchio::CodeGenDifference::evalFunction
void evalFunction(const Eigen::MatrixBase< ConfigVectorType1 > &q0, const Eigen::MatrixBase< ConfigVectorType2 > &q1, const Eigen::MatrixBase< TangentVector > &v)
Definition: code-generator-algo.hpp:1176
pinocchio::isZero
bool isZero(const Eigen::MatrixBase< MatrixLike > &m, const typename MatrixLike::RealScalar &prec=Eigen::NumTraits< typename MatrixLike::Scalar >::dummy_precision())
Definition: math/matrix.hpp:59
code-generator-algo.hpp
model.hpp
q2
q2
pinocchio::python::Scalar
context::Scalar Scalar
Definition: admm-solver.cpp:29
pinocchio::CodeGenDDifference
Definition: code-generator-algo.hpp:1206
pinocchio::difference
void difference(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ConfigVectorIn1 > &q0, const Eigen::MatrixBase< ConfigVectorIn2 > &q1, const Eigen::MatrixBase< ReturnType > &dvout)
Compute the tangent vector that must be integrated during one unit time to go from q0 to q1.
Definition: joint-configuration.hpp:193
pinocchio::CodeGenDDifference::evalFunction
void evalFunction(const Eigen::MatrixBase< ConfigVectorType1 > &q0, const Eigen::MatrixBase< ConfigVectorType2 > &q1, const Eigen::MatrixBase< JacobianMatrix > &J, const ArgumentPosition arg)
Definition: code-generator-algo.hpp:1253
pinocchio::ARG0
@ ARG0
Definition: fwd.hpp:123
BOOST_AUTO_TEST_CASE
BOOST_AUTO_TEST_CASE(test_joint_configuration_code_generation)
Definition: cppadcg/joint-configurations.cpp:21
joint-configuration.hpp
pinocchio::integrate
void integrate(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ConfigVectorType > &q, const Eigen::MatrixBase< TangentVectorType > &v, const Eigen::MatrixBase< ReturnType > &qout)
Integrate a configuration vector for the specified model for a tangent vector during one unit time.
Definition: joint-configuration.hpp:70
pinocchio::ARG1
@ ARG1
Definition: fwd.hpp:124
pinocchio::buildAllJointsModel
void buildAllJointsModel(Model &model)
Definition: model-generator.hpp:31
pinocchio::v
JointCollectionTpl const Eigen::MatrixBase< ConfigVectorType > const Eigen::MatrixBase< TangentVectorType > & v
Definition: joint-configuration.hpp:1084
pinocchio::CodeGenBase::compileAndLoadLib
void compileAndLoadLib(const std::string &gcc_path)
Definition: code-generator-base.hpp:117
DELETE_CODEGEN_LIBS_AFTER_TEST
bool DELETE_CODEGEN_LIBS_AFTER_TEST
Definition: cppadcg/joint-configurations.cpp:17
pinocchio::q1
JointCollectionTpl const Eigen::MatrixBase< ConfigVectorIn1 > const Eigen::MatrixBase< ConfigVectorIn2 > & q1
Definition: joint-configuration.hpp:1138
pinocchio::dDifference
void dDifference(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ConfigVector1 > &q0, const Eigen::MatrixBase< ConfigVector2 > &q1, const Eigen::MatrixBase< JacobianMatrix > &J, const ArgumentPosition arg)
Computes the Jacobian of a small variation of the configuration vector into the tangent space at iden...
Definition: joint-configuration.hpp:742
pinocchio::CodeGenIntegrate::evalFunction
void evalFunction(const Eigen::MatrixBase< ConfigVectorType1 > &q, const Eigen::MatrixBase< TangentVector > &v, const Eigen::MatrixBase< ConfigVectorType2 > &qout)
Definition: code-generator-algo.hpp:1105
pinocchio::ModelTpl
Definition: context/generic.hpp:20
pinocchio::Model
ModelTpl< context::Scalar, context::Options > Model
Definition: multibody/fwd.hpp:33
pinocchio::normalize
void normalize(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ConfigVectorType > &qout)
Normalize a configuration vector.
Definition: joint-configuration.hpp:887
pinocchio::model
JointCollectionTpl & model
Definition: joint-configuration.hpp:1082
pinocchio::CodeGenIntegrate
Definition: code-generator-algo.hpp:1064
pinocchio
Main pinocchio namespace.
Definition: timings.cpp:27
isApprox
bool isApprox(const Box &s1, const Box &s2, const FCL_REAL tol)


pinocchio
Author(s):
autogenerated on Sun Dec 22 2024 03:41:10