GCC Code Coverage Report

Directory: ./
File: unittest/test_contacts.cpp
Date: 2025-01-16 08:47:40
Exec Total Coverage
Lines: 119 119 100.0%
Branches: 332 656 50.6%
Line Branch Exec Source
1 ///////////////////////////////////////////////////////////////////////////////
2 // BSD 3-Clause License
3 //
4 // Copyright (C) 2019-2023, University of Edinburgh, Heriot-Watt University
5 // Copyright note valid unless otherwise stated in individual files.
6 // All rights reserved.
7 ///////////////////////////////////////////////////////////////////////////////
8
9 #define BOOST_TEST_NO_MAIN
10 #define BOOST_TEST_ALTERNATIVE_INIT_API
11
12 #include <boost/make_shared.hpp>
13 #include <boost/shared_ptr.hpp>
14 #include <pinocchio/algorithm/frames.hpp>
15 #include <pinocchio/algorithm/kinematics-derivatives.hpp>
16
17 #include "crocoddyl/multibody/contacts/contact-1d.hpp"
18 #include "crocoddyl/multibody/contacts/contact-2d.hpp"
19 #include "crocoddyl/multibody/contacts/contact-3d.hpp"
20 #include "crocoddyl/multibody/contacts/contact-6d.hpp"
21 #include "factory/contact.hpp"
22 #include "unittest_common.hpp"
23
24 using namespace crocoddyl::unittest;
25 using namespace boost::unit_test;
26
27 //----------------------------------------------------------------------------//
28
29 50 void test_construct_data(ContactModelTypes::Type contact_type,
30 PinocchioModelTypes::Type model_type) {
31 // create the model
32
1/2
✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
 50 ContactModelFactory factory;
33 boost::shared_ptr<crocoddyl::ContactModelAbstract> model =
34
4/8
✓ Branch 3 taken 50 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 50 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 50 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 50 times.
✗ Branch 13 not taken.
 50 factory.create(contact_type, model_type, Eigen::Vector2d::Random());
35
36 // Run the print function
37
1/2
✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
 50 std::ostringstream tmp;
38
1/2
✓ Branch 2 taken 50 times.
✗ Branch 3 not taken.
 50 tmp << *model;
39
40 // create the corresponding data object
41 const boost::shared_ptr<pinocchio::Model>& pinocchio_model =
42 50 model->get_state()->get_pinocchio();
43
1/2
✓ Branch 2 taken 50 times.
✗ Branch 3 not taken.
 50 pinocchio::Data pinocchio_data(*pinocchio_model.get());
44 boost::shared_ptr<crocoddyl::ContactDataAbstract> data =
45
1/2
✓ Branch 2 taken 50 times.
✗ Branch 3 not taken.
 50 model->createData(&pinocchio_data);
46 50 }
47
48 50 void test_calc_fetch_jacobians(ContactModelTypes::Type contact_type,
49 PinocchioModelTypes::Type model_type) {
50 // create the model
51
1/2
✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
 50 ContactModelFactory factory;
52 boost::shared_ptr<crocoddyl::ContactModelAbstract> model =
53
4/8
✓ Branch 3 taken 50 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 50 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 50 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 50 times.
✗ Branch 13 not taken.
 50 factory.create(contact_type, model_type, Eigen::Vector2d::Random());
54
55 // create the corresponding data object
56 const boost::shared_ptr<pinocchio::Model>& pinocchio_model =
57 50 model->get_state()->get_pinocchio();
58
1/2
✓ Branch 2 taken 50 times.
✗ Branch 3 not taken.
 50 pinocchio::Data pinocchio_data(*pinocchio_model.get());
59 boost::shared_ptr<crocoddyl::ContactDataAbstract> data =
60
1/2
✓ Branch 2 taken 50 times.
✗ Branch 3 not taken.
 50 model->createData(&pinocchio_data);
61
62 // Compute the jacobian and check that the impulse model fetch it.
63
1/2
✓ Branch 4 taken 50 times.
✗ Branch 5 not taken.
 50 Eigen::VectorXd x = model->get_state()->rand();
64
2/4
✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 50 times.
✗ Branch 6 not taken.
 50 crocoddyl::unittest::updateAllPinocchio(pinocchio_model.get(),
65 &pinocchio_data, x);
66
67 // Getting the jacobian from the model
68
2/4
✓ Branch 2 taken 50 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 50 times.
✗ Branch 6 not taken.
 50 model->calc(data, x);
69
70 // Check that only the Jacobian has been filled
71
7/14
✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 50 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 50 times.
✗ Branch 12 not taken.
✓ Branch 16 taken 50 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 50 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 50 times.
✗ Branch 23 not taken.
✗ Branch 27 not taken.
✓ Branch 28 taken 50 times.
 50 BOOST_CHECK(!data->Jc.isZero());
72
2/2
✓ Branch 0 taken 40 times.
✓ Branch 1 taken 10 times.
 50 if (model_type !=
73 PinocchioModelTypes::Hector) { // this is due to Hector is a single rigid
74 // body system.
75
7/14
✓ Branch 1 taken 40 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 40 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 40 times.
✗ Branch 12 not taken.
✓ Branch 16 taken 40 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 40 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 40 times.
✗ Branch 23 not taken.
✗ Branch 27 not taken.
✓ Branch 28 taken 40 times.
 40 BOOST_CHECK(!data->a0.isZero());
76 }
77
7/14
✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 50 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 50 times.
✗ Branch 12 not taken.
✓ Branch 16 taken 50 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 50 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 50 times.
✗ Branch 23 not taken.
✗ Branch 27 not taken.
✓ Branch 28 taken 50 times.
 50 BOOST_CHECK(data->da0_dx.isZero());
78
8/16
✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 50 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 50 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 50 times.
✗ Branch 16 not taken.
✓ Branch 19 taken 50 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 50 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 50 times.
✗ Branch 26 not taken.
✗ Branch 30 not taken.
✓ Branch 31 taken 50 times.
 50 BOOST_CHECK(data->f.toVector().isZero());
79
7/14
✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 50 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 50 times.
✗ Branch 12 not taken.
✓ Branch 16 taken 50 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 50 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 50 times.
✗ Branch 23 not taken.
✗ Branch 27 not taken.
✓ Branch 28 taken 50 times.
 50 BOOST_CHECK(data->df_dx.isZero());
80
7/14
✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 50 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 50 times.
✗ Branch 12 not taken.
✓ Branch 16 taken 50 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 50 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 50 times.
✗ Branch 23 not taken.
✗ Branch 27 not taken.
✓ Branch 28 taken 50 times.
 50 BOOST_CHECK(data->df_du.isZero());
81 50 }
82
83 50 void test_calc_diff_fetch_derivatives(ContactModelTypes::Type contact_type,
84 PinocchioModelTypes::Type model_type) {
85 // create the model
86
1/2
✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
 50 ContactModelFactory factory;
87 boost::shared_ptr<crocoddyl::ContactModelAbstract> model =
88
4/8
✓ Branch 3 taken 50 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 50 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 50 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 50 times.
✗ Branch 13 not taken.
 50 factory.create(contact_type, model_type, Eigen::Vector2d::Random());
89
90 // create the corresponding data object
91 const boost::shared_ptr<pinocchio::Model>& pinocchio_model =
92 50 model->get_state()->get_pinocchio();
93
1/2
✓ Branch 2 taken 50 times.
✗ Branch 3 not taken.
 50 pinocchio::Data pinocchio_data(*pinocchio_model.get());
94 boost::shared_ptr<crocoddyl::ContactDataAbstract> data =
95
1/2
✓ Branch 2 taken 50 times.
✗ Branch 3 not taken.
 50 model->createData(&pinocchio_data);
96
97 // Compute the jacobian and check that the impulse model fetch it.
98
1/2
✓ Branch 4 taken 50 times.
✗ Branch 5 not taken.
 50 Eigen::VectorXd x = model->get_state()->rand();
99
2/4
✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 50 times.
✗ Branch 6 not taken.
 50 crocoddyl::unittest::updateAllPinocchio(pinocchio_model.get(),
100 &pinocchio_data, x);
101
102 // Getting the jacobian from the model
103
2/4
✓ Branch 2 taken 50 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 50 times.
✗ Branch 6 not taken.
 50 model->calc(data, x);
104
2/4
✓ Branch 2 taken 50 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 50 times.
✗ Branch 6 not taken.
 50 model->calcDiff(data, x);
105
106 // Check that nothing has been computed and that all value are initialized to
107 // 0
108
7/14
✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 50 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 50 times.
✗ Branch 12 not taken.
✓ Branch 16 taken 50 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 50 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 50 times.
✗ Branch 23 not taken.
✗ Branch 27 not taken.
✓ Branch 28 taken 50 times.
 50 BOOST_CHECK(!data->Jc.isZero());
109
2/2
✓ Branch 0 taken 40 times.
✓ Branch 1 taken 10 times.
 50 if (model_type !=
110 PinocchioModelTypes::Hector) { // this is due to Hector is a single rigid
111 // body system.
112
7/14
✓ Branch 1 taken 40 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 40 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 40 times.
✗ Branch 12 not taken.
✓ Branch 16 taken 40 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 40 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 40 times.
✗ Branch 23 not taken.
✗ Branch 27 not taken.
✓ Branch 28 taken 40 times.
 40 BOOST_CHECK(!data->a0.isZero());
113
7/14
✓ Branch 1 taken 40 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 40 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 40 times.
✗ Branch 12 not taken.
✓ Branch 16 taken 40 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 40 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 40 times.
✗ Branch 23 not taken.
✗ Branch 27 not taken.
✓ Branch 28 taken 40 times.
 40 BOOST_CHECK(!data->da0_dx.isZero());
114 }
115
8/16
✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 50 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 50 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 50 times.
✗ Branch 16 not taken.
✓ Branch 19 taken 50 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 50 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 50 times.
✗ Branch 26 not taken.
✗ Branch 30 not taken.
✓ Branch 31 taken 50 times.
 50 BOOST_CHECK(data->f.toVector().isZero());
116
7/14
✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 50 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 50 times.
✗ Branch 12 not taken.
✓ Branch 16 taken 50 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 50 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 50 times.
✗ Branch 23 not taken.
✗ Branch 27 not taken.
✓ Branch 28 taken 50 times.
 50 BOOST_CHECK(data->df_dx.isZero());
117
7/14
✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 50 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 50 times.
✗ Branch 12 not taken.
✓ Branch 16 taken 50 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 50 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 50 times.
✗ Branch 23 not taken.
✗ Branch 27 not taken.
✓ Branch 28 taken 50 times.
 50 BOOST_CHECK(data->df_du.isZero());
118 50 }
119
120 50 void test_update_force(ContactModelTypes::Type contact_type,
121 PinocchioModelTypes::Type model_type) {
122 // create the model
123
1/2
✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
 50 ContactModelFactory factory;
124 boost::shared_ptr<crocoddyl::ContactModelAbstract> model =
125
4/8
✓ Branch 3 taken 50 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 50 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 50 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 50 times.
✗ Branch 13 not taken.
 50 factory.create(contact_type, model_type, Eigen::Vector2d::Random());
126
127 // create the corresponding data object
128 const boost::shared_ptr<pinocchio::Model>& pinocchio_model =
129 50 model->get_state()->get_pinocchio();
130
1/2
✓ Branch 2 taken 50 times.
✗ Branch 3 not taken.
 50 pinocchio::Data pinocchio_data(*pinocchio_model.get());
131 boost::shared_ptr<crocoddyl::ContactDataAbstract> data =
132
1/2
✓ Branch 2 taken 50 times.
✗ Branch 3 not taken.
 50 model->createData(&pinocchio_data);
133
134 // Create a random force and update it
135
2/4
✓ Branch 3 taken 50 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 50 times.
✗ Branch 7 not taken.
 50 Eigen::VectorXd f = Eigen::VectorXd::Random(data->Jc.rows());
136
1/2
✓ Branch 2 taken 50 times.
✗ Branch 3 not taken.
 50 model->updateForce(data, f);
137 boost::shared_ptr<crocoddyl::ContactModel3D> m =
138 50 boost::static_pointer_cast<crocoddyl::ContactModel3D>(model);
139
140 // Check that nothing has been computed and that all value are initialized to
141 // 0
142
7/14
✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 50 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 50 times.
✗ Branch 12 not taken.
✓ Branch 16 taken 50 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 50 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 50 times.
✗ Branch 23 not taken.
✗ Branch 27 not taken.
✓ Branch 28 taken 50 times.
 50 BOOST_CHECK(data->Jc.isZero());
143
7/14
✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 50 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 50 times.
✗ Branch 12 not taken.
✓ Branch 16 taken 50 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 50 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 50 times.
✗ Branch 23 not taken.
✗ Branch 27 not taken.
✓ Branch 28 taken 50 times.
 50 BOOST_CHECK(data->a0.isZero());
144
7/14
✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 50 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 50 times.
✗ Branch 12 not taken.
✓ Branch 16 taken 50 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 50 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 50 times.
✗ Branch 23 not taken.
✗ Branch 27 not taken.
✓ Branch 28 taken 50 times.
 50 BOOST_CHECK(data->da0_dx.isZero());
145
8/16
✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 50 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 50 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 50 times.
✗ Branch 16 not taken.
✓ Branch 19 taken 50 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 50 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 50 times.
✗ Branch 26 not taken.
✗ Branch 30 not taken.
✓ Branch 31 taken 50 times.
 50 BOOST_CHECK(!data->f.toVector().isZero());
146
8/16
✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 50 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 50 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 50 times.
✗ Branch 16 not taken.
✓ Branch 19 taken 50 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 50 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 50 times.
✗ Branch 26 not taken.
✗ Branch 30 not taken.
✓ Branch 31 taken 50 times.
 50 BOOST_CHECK(!data->fext.toVector().isZero());
147
7/14
✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 50 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 50 times.
✗ Branch 12 not taken.
✓ Branch 16 taken 50 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 50 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 50 times.
✗ Branch 23 not taken.
✗ Branch 27 not taken.
✓ Branch 28 taken 50 times.
 50 BOOST_CHECK(data->df_dx.isZero());
148
7/14
✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 50 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 50 times.
✗ Branch 12 not taken.
✓ Branch 16 taken 50 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 50 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 50 times.
✗ Branch 23 not taken.
✗ Branch 27 not taken.
✓ Branch 28 taken 50 times.
 50 BOOST_CHECK(data->df_du.isZero());
149 50 }
150
151 50 void test_update_force_diff(ContactModelTypes::Type contact_type,
152 PinocchioModelTypes::Type model_type) {
153 // create the model
154
1/2
✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
 50 ContactModelFactory factory;
155 boost::shared_ptr<crocoddyl::ContactModelAbstract> model =
156
4/8
✓ Branch 3 taken 50 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 50 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 50 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 50 times.
✗ Branch 13 not taken.
 50 factory.create(contact_type, model_type, Eigen::Vector2d::Random());
157
158 // create the corresponding data object
159 const boost::shared_ptr<pinocchio::Model>& pinocchio_model =
160 50 model->get_state()->get_pinocchio();
161
1/2
✓ Branch 2 taken 50 times.
✗ Branch 3 not taken.
 50 pinocchio::Data pinocchio_data(*pinocchio_model.get());
162 boost::shared_ptr<crocoddyl::ContactDataAbstract> data =
163
1/2
✓ Branch 2 taken 50 times.
✗ Branch 3 not taken.
 50 model->createData(&pinocchio_data);
164
165 // Create a random force and update it
166 Eigen::MatrixXd df_dx =
167
2/4
✓ Branch 5 taken 50 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 50 times.
✗ Branch 9 not taken.
 50 Eigen::MatrixXd::Random(data->df_dx.rows(), data->df_dx.cols());
168 Eigen::MatrixXd df_du =
169
2/4
✓ Branch 5 taken 50 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 50 times.
✗ Branch 9 not taken.
 50 Eigen::MatrixXd::Random(data->df_du.rows(), data->df_du.cols());
170
1/2
✓ Branch 2 taken 50 times.
✗ Branch 3 not taken.
 50 model->updateForceDiff(data, df_dx, df_du);
171
172 // Check that nothing has been computed and that all value are initialized to
173 // 0
174
7/14
✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 50 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 50 times.
✗ Branch 12 not taken.
✓ Branch 16 taken 50 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 50 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 50 times.
✗ Branch 23 not taken.
✗ Branch 27 not taken.
✓ Branch 28 taken 50 times.
 50 BOOST_CHECK(data->Jc.isZero());
175
7/14
✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 50 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 50 times.
✗ Branch 12 not taken.
✓ Branch 16 taken 50 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 50 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 50 times.
✗ Branch 23 not taken.
✗ Branch 27 not taken.
✓ Branch 28 taken 50 times.
 50 BOOST_CHECK(data->a0.isZero());
176
7/14
✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 50 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 50 times.
✗ Branch 12 not taken.
✓ Branch 16 taken 50 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 50 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 50 times.
✗ Branch 23 not taken.
✗ Branch 27 not taken.
✓ Branch 28 taken 50 times.
 50 BOOST_CHECK(data->da0_dx.isZero());
177
8/16
✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 50 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 50 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 50 times.
✗ Branch 16 not taken.
✓ Branch 19 taken 50 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 50 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 50 times.
✗ Branch 26 not taken.
✗ Branch 30 not taken.
✓ Branch 31 taken 50 times.
 50 BOOST_CHECK(data->f.toVector().isZero());
178
8/16
✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 50 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 50 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 50 times.
✗ Branch 16 not taken.
✓ Branch 19 taken 50 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 50 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 50 times.
✗ Branch 26 not taken.
✗ Branch 30 not taken.
✓ Branch 31 taken 50 times.
 50 BOOST_CHECK(data->fext.toVector().isZero());
179
7/14
✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 50 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 50 times.
✗ Branch 12 not taken.
✓ Branch 16 taken 50 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 50 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 50 times.
✗ Branch 23 not taken.
✗ Branch 27 not taken.
✓ Branch 28 taken 50 times.
 50 BOOST_CHECK(!data->df_dx.isZero());
180
7/14
✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 50 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 50 times.
✗ Branch 12 not taken.
✓ Branch 16 taken 50 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 50 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 50 times.
✗ Branch 23 not taken.
✗ Branch 27 not taken.
✓ Branch 28 taken 50 times.
 50 BOOST_CHECK(!data->df_du.isZero());
181 50 }
182
183 50 void test_partial_derivatives_against_numdiff(
184 ContactModelTypes::Type contact_type,
185 PinocchioModelTypes::Type model_type) {
186 using namespace boost::placeholders;
187
188 // create the model
189
1/2
✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
 50 ContactModelFactory factory;
190 boost::shared_ptr<crocoddyl::ContactModelAbstract> model =
191
4/8
✓ Branch 3 taken 50 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 50 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 50 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 50 times.
✗ Branch 13 not taken.
 50 factory.create(contact_type, model_type, Eigen::Vector2d::Random());
192
193 // create the corresponding data object
194 pinocchio::Model& pinocchio_model =
195 50 *model->get_state()->get_pinocchio().get();
196
1/2
✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
 50 pinocchio::Data pinocchio_data(pinocchio_model);
197 boost::shared_ptr<crocoddyl::ContactDataAbstract> data =
198
1/2
✓ Branch 2 taken 50 times.
✗ Branch 3 not taken.
 50 model->createData(&pinocchio_data);
199
200 // Create the equivalent num diff model and data.
201
1/2
✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
 50 crocoddyl::ContactModelNumDiff model_num_diff(model);
202 const boost::shared_ptr<crocoddyl::ContactDataAbstract>& data_num_diff =
203
1/2
✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
 50 model_num_diff.createData(&pinocchio_data);
204
205 // Generating random values for the state
206
1/2
✓ Branch 4 taken 50 times.
✗ Branch 5 not taken.
 50 const Eigen::VectorXd x = model->get_state()->rand();
207
208 // Compute all the pinocchio function needed for the models.
209
2/4
✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 50 times.
✗ Branch 5 not taken.
 50 crocoddyl::unittest::updateAllPinocchio(&pinocchio_model, &pinocchio_data, x);
210
211 // set the function that needs to be called at every step of the numdiff
212 50 std::vector<crocoddyl::ContactModelNumDiff::ReevaluationFunction> reevals;
213
3/6
✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 50 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 50 times.
✗ Branch 8 not taken.
 50 reevals.push_back(boost::bind(&crocoddyl::unittest::updateAllPinocchio,
214 &pinocchio_model, &pinocchio_data, _1, _2));
215
1/2
✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
 50 model_num_diff.set_reevals(reevals);
216
217 // Computing the contact derivatives
218
2/4
✓ Branch 2 taken 50 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 50 times.
✗ Branch 6 not taken.
 50 model->calc(data, x);
219
2/4
✓ Branch 2 taken 50 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 50 times.
✗ Branch 6 not taken.
 50 model->calcDiff(data, x);
220
2/4
✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 50 times.
✗ Branch 5 not taken.
 50 model_num_diff.calc(data_num_diff, x);
221
2/4
✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 50 times.
✗ Branch 5 not taken.
 50 model_num_diff.calcDiff(data_num_diff, x);
222 // Tolerance defined as in
223 // http://www.it.uom.gr/teaching/linearalgebra/NumericalRecipiesInC/c5-7.pdf
224 50 double tol = std::pow(model_num_diff.get_disturbance(), 1. / 3.);
225
8/16
✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 50 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 50 times.
✗ Branch 12 not taken.
✓ Branch 16 taken 50 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 50 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 50 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 50 times.
✗ Branch 26 not taken.
✗ Branch 30 not taken.
✓ Branch 31 taken 50 times.
 50 BOOST_CHECK((data->da0_dx - data_num_diff->da0_dx).isZero(tol));
226 50 }
227
228 //----------------------------------------------------------------------------//
229
230 50 void register_contact_model_unit_tests(ContactModelTypes::Type contact_type,
231 PinocchioModelTypes::Type model_type) {
232
2/4
✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 50 times.
✗ Branch 5 not taken.
 50 boost::test_tools::output_test_stream test_name;
233
4/8
✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 50 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 50 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 50 times.
✗ Branch 11 not taken.
 50 test_name << "test_" << contact_type << "_" << model_type;
234
4/8
✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 50 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 50 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 50 times.
✗ Branch 11 not taken.
 50 std::cout << "Running " << test_name.str() << std::endl;
235
4/8
✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 50 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 50 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 50 times.
✗ Branch 12 not taken.
 50 test_suite* ts = BOOST_TEST_SUITE(test_name.str());
236
5/10
✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 50 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 50 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 50 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 50 times.
✗ Branch 15 not taken.
 50 ts->add(BOOST_TEST_CASE(
237 boost::bind(&test_construct_data, contact_type, model_type)));
238
5/10
✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 50 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 50 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 50 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 50 times.
✗ Branch 15 not taken.
 50 ts->add(BOOST_TEST_CASE(
239 boost::bind(&test_calc_fetch_jacobians, contact_type, model_type)));
240
5/10
✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 50 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 50 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 50 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 50 times.
✗ Branch 15 not taken.
 50 ts->add(BOOST_TEST_CASE(boost::bind(&test_calc_diff_fetch_derivatives,
241 contact_type, model_type)));
242
5/10
✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 50 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 50 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 50 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 50 times.
✗ Branch 15 not taken.
 50 ts->add(BOOST_TEST_CASE(
243 boost::bind(&test_update_force, contact_type, model_type)));
244
5/10
✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 50 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 50 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 50 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 50 times.
✗ Branch 15 not taken.
 50 ts->add(BOOST_TEST_CASE(
245 boost::bind(&test_update_force_diff, contact_type, model_type)));
246
5/10
✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 50 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 50 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 50 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 50 times.
✗ Branch 15 not taken.
 50 ts->add(BOOST_TEST_CASE(boost::bind(&test_partial_derivatives_against_numdiff,
247 contact_type, model_type)));
248
3/6
✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 50 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 50 times.
✗ Branch 7 not taken.
 50 framework::master_test_suite().add(ts);
249 50 }
250
251 1 bool init_function() {
252
2/2
✓ Branch 1 taken 10 times.
✓ Branch 2 taken 1 times.
 11 for (size_t contact_type = 0; contact_type < ContactModelTypes::all.size();
253 ++contact_type) {
254
2/2
✓ Branch 1 taken 50 times.
✓ Branch 2 taken 10 times.
 60 for (size_t model_type = 0; model_type < PinocchioModelTypes::all.size();
255 ++model_type) {
256 50 register_contact_model_unit_tests(ContactModelTypes::all[contact_type],
257 50 PinocchioModelTypes::all[model_type]);
258 }
259 }
260 1 return true;
261 }
262
263 1 int main(int argc, char** argv) {
264 1 return ::boost::unit_test::unit_test_main(&init_function, argc, argv);
265 }
266