GCC Code Coverage Report

Directory: ./
File: unittest/casadi/integrate-derivatives.cpp
Date: 2025-02-12 21:03:38
Exec Total Coverage
Lines: 33 33 100.0%
Branches: 101 202 50.0%
Line Branch Exec Source
1 //
2 // Copyright (c) 2019-2020 INRIA
3 //
4
5 #include "pinocchio/autodiff/casadi.hpp"
6 #include "pinocchio/autodiff/casadi-algo.hpp"
7
8 #include "pinocchio/algorithm/rnea.hpp"
9 #include "pinocchio/algorithm/rnea-derivatives.hpp"
10 #include "pinocchio/algorithm/aba.hpp"
11 #include "pinocchio/algorithm/aba-derivatives.hpp"
12 #include "pinocchio/algorithm/joint-configuration.hpp"
13
14 #include "pinocchio/multibody/sample-models.hpp"
15
16 #include <casadi/casadi.hpp>
17
18 #include <iostream>
19 #include <boost/test/unit_test.hpp>
20 #include <boost/utility/binary.hpp>
21
22 BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE)
23
24
33/66
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
✓ Branch 23 taken 1 times.
✗ Branch 24 not taken.
✓ Branch 27 taken 1 times.
✗ Branch 28 not taken.
✓ Branch 30 taken 1 times.
✗ Branch 31 not taken.
✓ Branch 33 taken 1 times.
✗ Branch 34 not taken.
✓ Branch 36 taken 1 times.
✗ Branch 37 not taken.
✓ Branch 41 taken 1 times.
✗ Branch 42 not taken.
✓ Branch 45 taken 1 times.
✗ Branch 46 not taken.
✓ Branch 48 taken 1 times.
✗ Branch 49 not taken.
✓ Branch 52 taken 1 times.
✗ Branch 53 not taken.
✓ Branch 55 taken 1 times.
✗ Branch 56 not taken.
✓ Branch 58 taken 1 times.
✗ Branch 59 not taken.
✓ Branch 61 taken 1 times.
✗ Branch 62 not taken.
✓ Branch 66 taken 1 times.
✗ Branch 67 not taken.
✓ Branch 70 taken 1 times.
✗ Branch 71 not taken.
✓ Branch 73 taken 1 times.
✗ Branch 74 not taken.
✓ Branch 77 taken 1 times.
✗ Branch 78 not taken.
✓ Branch 80 taken 1 times.
✗ Branch 81 not taken.
✓ Branch 83 taken 1 times.
✗ Branch 84 not taken.
✓ Branch 86 taken 1 times.
✗ Branch 87 not taken.
✓ Branch 91 taken 1 times.
✗ Branch 92 not taken.
✓ Branch 95 taken 1 times.
✗ Branch 96 not taken.
✓ Branch 98 taken 1 times.
✗ Branch 99 not taken.
✓ Branch 102 taken 1 times.
✗ Branch 103 not taken.
✓ Branch 105 taken 1 times.
✗ Branch 106 not taken.
✓ Branch 108 taken 1 times.
✗ Branch 109 not taken.
✓ Branch 111 taken 1 times.
✗ Branch 112 not taken.
✓ Branch 116 taken 1 times.
✗ Branch 117 not taken.
 4 BOOST_AUTO_TEST_CASE(test_integrate)
25 {
26 typedef double Scalar;
27 typedef casadi::SX ADScalar;
28
29 typedef pinocchio::ModelTpl<Scalar> Model;
30 typedef Model::Data Data;
31
32 typedef pinocchio::ModelTpl<ADScalar> ADModel;
33 typedef ADModel::Data ADData;
34
35
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 Model model;
36
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 pinocchio::buildModels::humanoidRandom(model);
37
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 model.lowerPositionLimit.head<3>().fill(-1.);
38
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 model.upperPositionLimit.head<3>().fill(1.);
39
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 Data data(model);
40
41 typedef Model::ConfigVectorType ConfigVector;
42 typedef Model::TangentVectorType TangentVector;
43
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 ConfigVector q(model.nq);
44
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 q = pinocchio::randomConfiguration(model);
45
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 TangentVector v(TangentVector::Random(model.nv));
46
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 TangentVector a(TangentVector::Random(model.nv));
47
48 typedef ADModel::ConfigVectorType ConfigVectorAD;
49
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 ADModel ad_model = model.cast<ADScalar>();
50
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 ADData ad_data(ad_model);
51
52
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 pinocchio::rnea(model, data, q, v, a);
53
54
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 4 casadi::SX cs_q = casadi::SX::sym("q", model.nq);
55
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 4 casadi::SX cs_v_int = casadi::SX::sym("v_inc", model.nv);
56
57
3/6
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
 2 ConfigVectorAD q_ad(model.nq), v_int_ad(model.nv), q_int_ad(model.nq);
58
4/8
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
 2 q_ad = Eigen::Map<ConfigVectorAD>(static_cast<std::vector<ADScalar>>(cs_q).data(), model.nq, 1);
59 v_int_ad =
60
4/8
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
 2 Eigen::Map<ConfigVectorAD>(static_cast<std::vector<ADScalar>>(cs_v_int).data(), model.nv, 1);
61
62
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 pinocchio::integrate(ad_model, q_ad, v_int_ad, q_int_ad);
63
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 casadi::SX cs_q_int(model.nq, 1);
64
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 pinocchio::casadi::copy(q_int_ad, cs_q_int);
65
66 casadi::Function eval_integrate(
67
7/14
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 1 times.
✗ Branch 13 not taken.
✓ Branch 16 taken 1 times.
✗ Branch 17 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 1 times.
✗ Branch 24 not taken.
 14 "eval_integrate", casadi::SXVector{cs_q, cs_v_int}, casadi::SXVector{cs_q_int});
68
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 2 std::vector<double> q_vec((size_t)model.nq);
69
3/6
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
 2 Eigen::Map<ConfigVector>(q_vec.data(), model.nq, 1) = q;
70
71
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 2 std::vector<double> v_int_vec((size_t)model.nv);
72
3/6
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
 2 Eigen::Map<TangentVector>(v_int_vec.data(), model.nv, 1).setZero();
73
5/10
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
 8 casadi::DM q_int_res = eval_integrate(casadi::DMVector{q_vec, v_int_vec})[0];
74
75
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 6 Data::ConfigVectorType q_int_vec = Eigen::Map<Data::TangentVectorType>(
76
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 4 static_cast<std::vector<double>>(q_int_res).data(), model.nq, 1);
77
78
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 ConfigVector q_plus(model.nq);
79
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 pinocchio::integrate(model, q, TangentVector::Zero(model.nv), q_plus);
80
81
7/14
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✗ Branch 26 not taken.
✓ Branch 27 taken 1 times.
 2 BOOST_CHECK(q_plus.isApprox(q_int_vec));
82 2 }
83
84 BOOST_AUTO_TEST_SUITE_END()
85