GCC Code Coverage Report

Directory: ./
File: unittest/casadi/explog.cpp
Date: 2025-02-12 21:03:38
Exec Total Coverage
Lines: 106 106 100.0%
Branches: 313 624 50.2%
Line Branch Exec Source
1 //
2 // Copyright (c) 2021 INRIA
3 //
4
5 #include "pinocchio/autodiff/casadi.hpp"
6 #include "pinocchio/autodiff/casadi-algo.hpp"
7
8 #include "pinocchio/algorithm/joint-configuration.hpp"
9
10 #include "pinocchio/multibody/sample-models.hpp"
11
12 #include <casadi/casadi.hpp>
13
14 #include <iostream>
15 #include <boost/test/unit_test.hpp>
16 #include <boost/utility/binary.hpp>
17
18 BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE)
19
20
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_squaredDistance)
21 {
22 typedef double Scalar;
23 typedef casadi::SX ADScalar;
24 typedef pinocchio::ModelTpl<Scalar> Model;
25 typedef pinocchio::ModelTpl<ADScalar> ADModel;
26 using casadi::SXVector;
27
28
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 Model model;
29
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 pinocchio::buildModels::humanoidRandom(model, true);
30
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.);
31
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.);
32
33 typedef Model::ConfigVectorType ConfigVector;
34 typedef Eigen::Map<ConfigVector> ConfigVectorMap;
35
36
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 ConfigVector q0(pinocchio::neutral(model));
37
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 ConfigVector q1(pinocchio::randomConfiguration(model));
38
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 ConfigVector q2(pinocchio::randomConfiguration(model));
39
40
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 ADModel ad_model = model.cast<ADScalar>();
41 typedef ADModel::ConfigVectorType cConfig_t;
42 typedef ADModel::TangentVectorType cTangent_t;
43
44
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 4 casadi::SX cs_q0 = casadi::SX::sym("q0", model.nq, 1);
45
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 4 casadi::SX cs_q1 = casadi::SX::sym("q1", model.nq, 1);
46
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 4 casadi::SX cv0 = casadi::SX::sym("v0", model.nv, 1);
47
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 4 casadi::SX cv1 = casadi::SX::sym("v1", model.nv, 1);
48
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 casadi::SX v_all = vertcat(cv0, cv1);
49
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 cConfig_t q0_ad(model.nq), q1_ad(model.nq);
50
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 cTangent_t v0_ad(model.nq), v1_ad(model.nv);
51
52
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 q0_ad = Eigen::Map<cConfig_t>(static_cast<SXVector>(cs_q0).data(), model.nq, 1);
53
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 q1_ad = Eigen::Map<cConfig_t>(static_cast<SXVector>(cs_q1).data(), model.nq, 1);
54
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 v0_ad = Eigen::Map<cTangent_t>(static_cast<SXVector>(cv0).data(), model.nv, 1);
55
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 v1_ad = Eigen::Map<cTangent_t>(static_cast<SXVector>(cv1).data(), model.nv, 1);
56
57
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 auto cq0_i = pinocchio::integrate(ad_model, q0_ad, v0_ad);
58
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 auto cq1_i = pinocchio::integrate(ad_model, q1_ad, v1_ad);
59
60
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 cTangent_t d = pinocchio::difference(ad_model, cq0_i, cq1_i);
61
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 casadi::SX d_vec(model.nv);
62
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 pinocchio::casadi::copy(d, d_vec);
63
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 ADScalar dist_expr = d.dot(d);
64
65 2 size_t nq = (size_t)model.nq;
66
67
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 casadi::SX zero_vec = casadi::SX::zeros(2 * model.nv);
68
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 4 casadi::SX Jdist_expr = substitute(gradient(dist_expr, v_all), v_all, zero_vec);
69
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 casadi::Function eval_Jdist("Jdistance", SXVector{cs_q0, cs_q1}, SXVector{Jdist_expr});
70
71
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 std::cout << "Jdistance func: " << eval_Jdist << '\n';
72
73
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 2 std::vector<Scalar> q0_vec(nq);
74
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 ConfigVectorMap(q0_vec.data(), model.nq, 1) = q0;
75
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 2 std::vector<Scalar> q1_vec(nq);
76
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 ConfigVectorMap(q1_vec.data(), model.nq, 1) = q1;
77
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 2 std::vector<Scalar> q2_vec(nq);
78
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 ConfigVectorMap(q2_vec.data(), model.nq, 1) = q2;
79
80
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 auto J0 = eval_Jdist(casadi::DMVector{q0_vec, q0_vec})[0];
81
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 auto J1 = eval_Jdist(casadi::DMVector{q0_vec, q1_vec})[0];
82
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 auto J2 = eval_Jdist(casadi::DMVector{q0_vec, q2_vec})[0];
83
84
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 ConfigVector q(model.nq);
85
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 2 std::vector<Scalar> q_vec(nq);
86
2/2
✓ Branch 0 taken 10 times.
✓ Branch 1 taken 1 times.
 22 for (size_t it = 0; it < 10; it++)
87 {
88
3/6
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 10 times.
✗ Branch 8 not taken.
 20 q.noalias() = pinocchio::randomConfiguration(model);
89
3/6
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 10 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 10 times.
✗ Branch 9 not taken.
 20 ConfigVectorMap(q_vec.data(), model.nq, 1) = q;
90
5/10
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 10 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 10 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 10 times.
✗ Branch 16 not taken.
 80 auto J_vec = static_cast<std::vector<Scalar>>(eval_Jdist(casadi::DMVector{q_vec, q_vec})[0]);
91
2/4
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 10 times.
✗ Branch 6 not taken.
 20 Eigen::Map<Eigen::MatrixXd> J_as_mat(J_vec.data(), 2 * model.nv, 1);
92
8/16
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 10 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 10 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 10 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 10 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 10 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 10 times.
✗ Branch 25 not taken.
✗ Branch 29 not taken.
✓ Branch 30 taken 10 times.
 20 BOOST_CHECK(J_as_mat.isApprox(Eigen::MatrixXd::Zero(2 * model.nv, 1)));
93 20 }
94 2 }
95
96 template<typename Derived>
97 4 casadi::DM SE3toCasadiDM(const pinocchio::SE3Base<Derived> & M)
98 {
99 typedef typename Derived::Scalar Scalar;
100
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 4 auto M_mat = M.toHomogeneousMatrix();
101
3/6
✓ Branch 2 taken 4 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 4 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 4 times.
✗ Branch 10 not taken.
 4 std::vector<Scalar> flat_M_vec(M_mat.data(), M_mat.data() + M_mat.size());
102
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 4 casadi::DM out{flat_M_vec};
103
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 8 return reshape(out, 4, 4);
104 4 }
105
106
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_Jlog6)
107 {
108 namespace pin = pinocchio;
109 using casadi::DMVector;
110 using casadi::SXVector;
111 using casadi::SXVectorVector;
112 typedef pin::SE3Tpl<double> SE3;
113 typedef casadi::SX ADScalar;
114 typedef pin::SE3Tpl<ADScalar> cSE3;
115 typedef pin::MotionTpl<ADScalar> cMotion;
116
117
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 4 casadi::SX csM0 = casadi::SX::sym("cM0", 4, 4);
118
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 4 casadi::SX csM1 = casadi::SX::sym("cM1", 4, 4);
119
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 4 casadi::SX csm0 = casadi::SX::sym("cm0", 6);
120
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 4 casadi::SX csm1 = casadi::SX::sym("cm1", 6);
121
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 casadi::SX csm_all = vertcat(csm0, csm1);
122
123
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 cMotion::Vector6 cm0_v, cm1_v;
124
125
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 cSE3 cM0_i, cM1_i;
126 {
127
128
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 cm0_v = Eigen::Map<cMotion::Vector6>(static_cast<SXVector>(csm0).data(), 6, 1);
129
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 cm1_v = Eigen::Map<cMotion::Vector6>(static_cast<SXVector>(csm1).data(), 6, 1);
130
131
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 cSE3::Matrix4 cM0_mat, cM1_mat;
132
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 cM0_mat = Eigen::Map<cSE3::Matrix4>(&static_cast<SXVector>(csM0)[0], 4, 4);
133
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 cM1_mat = Eigen::Map<cSE3::Matrix4>(&static_cast<SXVector>(csM1)[0], 4, 4);
134
135
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 auto rot0 = cM0_mat.template block<3, 3>(0, 0);
136
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 auto rot1 = cM1_mat.template block<3, 3>(0, 0);
137
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 auto trans0 = cM0_mat.template block<3, 1>(0, 3);
138
139
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 cSE3 cM0(rot0, trans0);
140
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 cSE3 cM1(rot1, cM1_mat.template block<3, 1>(0, 3));
141
142
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 cM0_i = cM0 * pin::exp6(cm0_v);
143
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 cM1_i = cM1 * pin::exp6(cm1_v);
144 2 }
145
146
4/8
✓ 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.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
 4 auto cdM(pin::log6(cM0_i.actInv(cM1_i)).toVector());
147
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 casadi::SX cdM_s(6);
148
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 pinocchio::casadi::copy(cdM, cdM_s);
149
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 casadi::SX zeros = casadi::SX::zeros(12);
150
151
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 auto dist = cdM.squaredNorm();
152
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 2 auto grad_cdM_expr = gradient(dist, csm_all);
153
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 2 auto hess_cdM_expr = jacobian(grad_cdM_expr, csm_all);
154
155 casadi::Function grad_cdM_eval(
156
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 "gdM", SXVector{csM0, csM1}, SXVector{substitute(grad_cdM_expr, csm_all, zeros)});
157 casadi::Function hess_cdM_eval(
158
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 "gdM", SXVector{csM0, csM1}, SXVector{substitute(hess_cdM_expr, csm_all, zeros)});
159
160
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 std::cout << "dM_eval: " << grad_cdM_eval << '\n';
161
162
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 SE3 M_neutral(SE3::Identity());
163
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 SE3 M0(SE3::Random()), M1(SE3::Random());
164
165
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 casadi::DM M_n_dm = SE3toCasadiDM(M_neutral);
166
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 casadi::DM M0_dm = SE3toCasadiDM(M0);
167
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 casadi::DM M1_dm = SE3toCasadiDM(M1);
168
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 casadi::DM M2_dm = SE3toCasadiDM(M1.actInv(M1));
169
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 std::cout << M0_dm << "\n\n";
170
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 std::cout << M2_dm << '\n';
171
172
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 auto J0 = grad_cdM_eval(DMVector{M0_dm, M1_dm})[0];
173
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 std::cout << J0 << '\n';
174
175
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 auto J1 = grad_cdM_eval(DMVector{M0_dm, M2_dm})[0];
176
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 std::cout << J1 << '\n';
177
178
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 auto J2 = grad_cdM_eval(DMVector{M2_dm, M2_dm})[0];
179
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 std::cout << J2 << '\n';
180
181
7/14
✓ 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.
✓ Branch 20 taken 2 times.
✓ Branch 21 taken 1 times.
✗ Branch 26 not taken.
✗ Branch 27 not taken.
 6 std::cout << hess_cdM_eval(DMVector{M_n_dm, M_n_dm})[0];
182
7/14
✓ 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.
✓ Branch 20 taken 2 times.
✓ Branch 21 taken 1 times.
✗ Branch 26 not taken.
✗ Branch 27 not taken.
 6 std::cout << hess_cdM_eval(DMVector{M0_dm, M0_dm})[0];
183
7/14
✓ 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.
✓ Branch 20 taken 2 times.
✓ Branch 21 taken 1 times.
✗ Branch 26 not taken.
✗ Branch 27 not taken.
 6 std::cout << hess_cdM_eval(DMVector{M0_dm, M1_dm})[0];
184
7/14
✓ 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.
✓ Branch 20 taken 2 times.
✓ Branch 21 taken 1 times.
✗ Branch 26 not taken.
✗ Branch 27 not taken.
 6 std::cout << hess_cdM_eval(DMVector{M2_dm, M2_dm})[0];
185 2 }
186
187 BOOST_AUTO_TEST_SUITE_END()
188