GCC Code Coverage Report

Directory: ./
File: unittest/test_states.cpp
Date: 2025-01-16 08:47:40
Exec Total Coverage
Lines: 225 225 100.0%
Branches: 605 1208 50.1%
Line Branch Exec Source
1 ///////////////////////////////////////////////////////////////////////////////
2 // BSD 3-Clause License
3 //
4 // Copyright (C) 2019-2023, LAAS-CNRS, New York University, Max Planck
5 // Gesellschaft,
6 // University of Edinburgh, INRIA
7 // Copyright note valid unless otherwise stated in individual files.
8 // All rights reserved.
9 ///////////////////////////////////////////////////////////////////////////////
10
11 #define BOOST_TEST_NO_MAIN
12 #define BOOST_TEST_ALTERNATIVE_INIT_API
13
14 #include "factory/state.hpp"
15 #include "unittest_common.hpp"
16
17 using namespace boost::unit_test;
18 using namespace crocoddyl::unittest;
19
20 //----------------------------------------------------------------------------//
21
22 7 void test_state_dimension(StateModelTypes::Type state_type) {
23
1/2
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
 7 StateModelFactory factory;
24 const boost::shared_ptr<crocoddyl::StateAbstract>& state =
25
1/2
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
 7 factory.create(state_type);
26 // Checking the dimension of zero and random states
27
7/14
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 7 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 7 times.
✗ Branch 16 not taken.
✓ Branch 21 taken 7 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 7 times.
✗ Branch 25 not taken.
✗ Branch 30 not taken.
✓ Branch 31 taken 7 times.
 7 BOOST_CHECK(static_cast<std::size_t>(state->zero().size()) ==
28 state->get_nx());
29
7/14
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 7 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 7 times.
✗ Branch 16 not taken.
✓ Branch 21 taken 7 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 7 times.
✗ Branch 25 not taken.
✗ Branch 30 not taken.
✓ Branch 31 taken 7 times.
 7 BOOST_CHECK(static_cast<std::size_t>(state->rand().size()) ==
30 state->get_nx());
31
6/12
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 7 times.
✗ Branch 12 not taken.
✓ Branch 20 taken 7 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 7 times.
✗ Branch 24 not taken.
✗ Branch 28 not taken.
✓ Branch 29 taken 7 times.
 7 BOOST_CHECK(state->get_nx() == (state->get_nq() + state->get_nv()));
32
6/12
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 7 times.
✗ Branch 12 not taken.
✓ Branch 18 taken 7 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 7 times.
✗ Branch 22 not taken.
✗ Branch 26 not taken.
✓ Branch 27 taken 7 times.
 7 BOOST_CHECK(state->get_ndx() == (2 * state->get_nv()));
33
6/12
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 7 times.
✗ Branch 12 not taken.
✓ Branch 19 taken 7 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 7 times.
✗ Branch 23 not taken.
✗ Branch 27 not taken.
✓ Branch 28 taken 7 times.
 7 BOOST_CHECK(static_cast<std::size_t>(state->get_lb().size()) ==
34 state->get_nx());
35
6/12
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 7 times.
✗ Branch 12 not taken.
✓ Branch 19 taken 7 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 7 times.
✗ Branch 23 not taken.
✗ Branch 27 not taken.
✓ Branch 28 taken 7 times.
 7 BOOST_CHECK(static_cast<std::size_t>(state->get_ub().size()) ==
36 state->get_nx());
37 7 }
38
39 7 void test_integrate_against_difference(StateModelTypes::Type state_type) {
40
1/2
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
 7 StateModelFactory factory;
41 const boost::shared_ptr<crocoddyl::StateAbstract>& state =
42
1/2
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
 7 factory.create(state_type);
43 // Generating random states
44
1/2
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
 7 const Eigen::VectorXd x1 = state->rand();
45
1/2
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
 7 const Eigen::VectorXd x2 = state->rand();
46
47 // Computing x2 by integrating its difference
48
1/2
✓ Branch 3 taken 7 times.
✗ Branch 4 not taken.
 7 Eigen::VectorXd dx(state->get_ndx());
49
4/8
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 7 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 7 times.
✗ Branch 12 not taken.
 7 state->diff(x1, x2, dx);
50
1/2
✓ Branch 3 taken 7 times.
✗ Branch 4 not taken.
 7 Eigen::VectorXd x2i(state->get_nx());
51
4/8
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 7 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 7 times.
✗ Branch 12 not taken.
 7 state->integrate(x1, dx, x2i);
52
53
1/2
✓ Branch 3 taken 7 times.
✗ Branch 4 not taken.
 7 Eigen::VectorXd dxi(state->get_ndx());
54
4/8
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 7 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 7 times.
✗ Branch 12 not taken.
 7 state->diff(x2i, x2, dxi);
55
56 // Checking that both states agree
57
7/14
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 7 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 7 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 7 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 7 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 7 times.
 7 BOOST_CHECK(dxi.isZero(1e-9));
58 7 }
59
60 7 void test_difference_against_integrate(StateModelTypes::Type state_type) {
61
1/2
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
 7 StateModelFactory factory;
62 const boost::shared_ptr<crocoddyl::StateAbstract>& state =
63
1/2
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
 7 factory.create(state_type);
64 // Generating random states
65
1/2
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
 7 const Eigen::VectorXd x = state->rand();
66
2/4
✓ Branch 3 taken 7 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 7 times.
✗ Branch 7 not taken.
 7 const Eigen::VectorXd dx = Eigen::VectorXd::Random(state->get_ndx());
67
68 // Computing dx by differentiation of its integrate
69
1/2
✓ Branch 3 taken 7 times.
✗ Branch 4 not taken.
 7 Eigen::VectorXd xidx(state->get_nx());
70
4/8
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 7 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 7 times.
✗ Branch 12 not taken.
 7 state->integrate(x, dx, xidx);
71
1/2
✓ Branch 3 taken 7 times.
✗ Branch 4 not taken.
 7 Eigen::VectorXd dxd(state->get_ndx());
72
4/8
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 7 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 7 times.
✗ Branch 12 not taken.
 7 state->diff(x, xidx, dxd);
73
74 // Checking that both states agree
75
8/16
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 7 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 7 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 7 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 7 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 7 times.
✗ Branch 24 not taken.
✗ Branch 28 not taken.
✓ Branch 29 taken 7 times.
 7 BOOST_CHECK((dxd - dx).isZero(1e-9));
76 7 }
77
78 7 void test_Jdiff_firstsecond(StateModelTypes::Type state_type) {
79
1/2
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
 7 StateModelFactory factory;
80 const boost::shared_ptr<crocoddyl::StateAbstract>& state =
81
1/2
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
 7 factory.create(state_type);
82 // Generating random values for the initial and terminal states
83
1/2
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
 7 const Eigen::VectorXd x1 = state->rand();
84
1/2
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
 7 const Eigen::VectorXd x2 = state->rand();
85
86 // Computing the partial derivatives of the difference function separately
87 Eigen::MatrixXd Jdiff_tmp(
88
2/4
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 7 times.
✗ Branch 9 not taken.
 7 Eigen::MatrixXd::Zero(state->get_ndx(), state->get_ndx()));
89 Eigen::MatrixXd Jdiff_first(
90
2/4
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 7 times.
✗ Branch 9 not taken.
 7 Eigen::MatrixXd::Zero(state->get_ndx(), state->get_ndx()));
91 Eigen::MatrixXd Jdiff_second(
92
2/4
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 7 times.
✗ Branch 9 not taken.
 7 Eigen::MatrixXd::Zero(state->get_ndx(), state->get_ndx()));
93
5/10
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 7 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 7 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 7 times.
✗ Branch 15 not taken.
 7 state->Jdiff(x1, x2, Jdiff_first, Jdiff_tmp, crocoddyl::first);
94
5/10
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 7 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 7 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 7 times.
✗ Branch 15 not taken.
 7 state->Jdiff(x1, x2, Jdiff_tmp, Jdiff_second, crocoddyl::second);
95
96 // Computing the partial derivatives of the difference function separately
97 Eigen::MatrixXd Jdiff_both_first(
98
2/4
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 7 times.
✗ Branch 9 not taken.
 7 Eigen::MatrixXd::Zero(state->get_ndx(), state->get_ndx()));
99 Eigen::MatrixXd Jdiff_both_second(
100
2/4
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 7 times.
✗ Branch 9 not taken.
 7 Eigen::MatrixXd::Zero(state->get_ndx(), state->get_ndx()));
101
5/10
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 7 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 7 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 7 times.
✗ Branch 15 not taken.
 7 state->Jdiff(x1, x2, Jdiff_both_first, Jdiff_both_second);
102
103
8/16
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 7 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 7 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 7 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 7 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 7 times.
✗ Branch 24 not taken.
✗ Branch 28 not taken.
✓ Branch 29 taken 7 times.
 7 BOOST_CHECK((Jdiff_first - Jdiff_both_first).isZero(1e-9));
104
8/16
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 7 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 7 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 7 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 7 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 7 times.
✗ Branch 24 not taken.
✗ Branch 28 not taken.
✓ Branch 29 taken 7 times.
 7 BOOST_CHECK((Jdiff_second - Jdiff_both_second).isZero(1e-9));
105 7 }
106
107 7 void test_Jint_firstsecond(StateModelTypes::Type state_type) {
108
1/2
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
 7 StateModelFactory factory;
109 const boost::shared_ptr<crocoddyl::StateAbstract>& state =
110
1/2
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
 7 factory.create(state_type);
111 // Generating random values for the initial and terminal states
112
1/2
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
 7 const Eigen::VectorXd x = state->rand();
113
2/4
✓ Branch 3 taken 7 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 7 times.
✗ Branch 7 not taken.
 7 const Eigen::VectorXd dx = Eigen::VectorXd::Random(state->get_ndx());
114
115 // Computing the partial derivatives of the difference function separately
116 Eigen::MatrixXd Jint_tmp(
117
2/4
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 7 times.
✗ Branch 9 not taken.
 7 Eigen::MatrixXd::Zero(state->get_ndx(), state->get_ndx()));
118 Eigen::MatrixXd Jint_first(
119
2/4
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 7 times.
✗ Branch 9 not taken.
 7 Eigen::MatrixXd::Zero(state->get_ndx(), state->get_ndx()));
120 Eigen::MatrixXd Jint_second(
121
2/4
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 7 times.
✗ Branch 9 not taken.
 7 Eigen::MatrixXd::Zero(state->get_ndx(), state->get_ndx()));
122
5/10
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 7 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 7 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 7 times.
✗ Branch 15 not taken.
 7 state->Jintegrate(x, dx, Jint_first, Jint_tmp, crocoddyl::first);
123
5/10
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 7 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 7 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 7 times.
✗ Branch 15 not taken.
 7 state->Jintegrate(x, dx, Jint_tmp, Jint_second, crocoddyl::second);
124
125 // Computing the partial derivatives of the integrate function separately
126 Eigen::MatrixXd Jint_both_first(
127
2/4
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 7 times.
✗ Branch 9 not taken.
 7 Eigen::MatrixXd::Zero(state->get_ndx(), state->get_ndx()));
128 Eigen::MatrixXd Jint_both_second(
129
2/4
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 7 times.
✗ Branch 9 not taken.
 7 Eigen::MatrixXd::Zero(state->get_ndx(), state->get_ndx()));
130
5/10
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 7 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 7 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 7 times.
✗ Branch 15 not taken.
 7 state->Jintegrate(x, dx, Jint_both_first, Jint_both_second);
131
132
8/16
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 7 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 7 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 7 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 7 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 7 times.
✗ Branch 24 not taken.
✗ Branch 28 not taken.
✓ Branch 29 taken 7 times.
 7 BOOST_CHECK((Jint_first - Jint_both_first).isZero(1e-9));
133
8/16
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 7 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 7 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 7 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 7 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 7 times.
✗ Branch 24 not taken.
✗ Branch 28 not taken.
✓ Branch 29 taken 7 times.
 7 BOOST_CHECK((Jint_second - Jint_both_second).isZero(1e-9));
134 7 }
135
136 7 void test_Jdiff_num_diff_firstsecond(StateModelTypes::Type state_type) {
137
1/2
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
 7 StateModelFactory factory;
138 const boost::shared_ptr<crocoddyl::StateAbstract>& state =
139
1/2
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
 7 factory.create(state_type);
140 // Generating random values for the initial and terminal states
141
1/2
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
 7 const Eigen::VectorXd x1 = state->rand();
142
1/2
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
 7 const Eigen::VectorXd x2 = state->rand();
143
144 // Get the num diff state
145
1/2
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
 7 crocoddyl::StateNumDiff state_num_diff(state);
146
147 // Computing the partial derivatives of the difference function separately
148 Eigen::MatrixXd Jdiff_num_diff_tmp(
149
2/4
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 7 times.
✗ Branch 9 not taken.
 7 Eigen::MatrixXd::Zero(state->get_ndx(), state->get_ndx()));
150 Eigen::MatrixXd Jdiff_num_diff_first(
151
2/4
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 7 times.
✗ Branch 9 not taken.
 7 Eigen::MatrixXd::Zero(state->get_ndx(), state->get_ndx()));
152 Eigen::MatrixXd Jdiff_num_diff_second(
153
2/4
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 7 times.
✗ Branch 9 not taken.
 7 Eigen::MatrixXd::Zero(state->get_ndx(), state->get_ndx()));
154
5/10
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 7 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 7 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 7 times.
✗ Branch 14 not taken.
 7 state_num_diff.Jdiff(x1, x2, Jdiff_num_diff_first, Jdiff_num_diff_tmp,
155 crocoddyl::first);
156
5/10
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 7 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 7 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 7 times.
✗ Branch 14 not taken.
 7 state_num_diff.Jdiff(x1, x2, Jdiff_num_diff_tmp, Jdiff_num_diff_second,
157 crocoddyl::second);
158
159 // Computing the partial derivatives of the difference function separately
160 Eigen::MatrixXd Jdiff_num_diff_both_first(
161
2/4
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 7 times.
✗ Branch 9 not taken.
 7 Eigen::MatrixXd::Zero(state->get_ndx(), state->get_ndx()));
162 Eigen::MatrixXd Jdiff_num_diff_both_second(
163
2/4
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 7 times.
✗ Branch 9 not taken.
 7 Eigen::MatrixXd::Zero(state->get_ndx(), state->get_ndx()));
164
5/10
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 7 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 7 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 7 times.
✗ Branch 14 not taken.
 7 state_num_diff.Jdiff(x1, x2, Jdiff_num_diff_both_first,
165 Jdiff_num_diff_both_second);
166
167
8/16
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 7 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 7 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 7 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 7 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 7 times.
✗ Branch 24 not taken.
✗ Branch 28 not taken.
✓ Branch 29 taken 7 times.
 7 BOOST_CHECK((Jdiff_num_diff_first - Jdiff_num_diff_both_first).isZero(1e-9));
168
8/16
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 7 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 7 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 7 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 7 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 7 times.
✗ Branch 24 not taken.
✗ Branch 28 not taken.
✓ Branch 29 taken 7 times.
 7 BOOST_CHECK(
169 (Jdiff_num_diff_second - Jdiff_num_diff_both_second).isZero(1e-9));
170 7 }
171
172 7 void test_Jint_num_diff_firstsecond(StateModelTypes::Type state_type) {
173
1/2
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
 7 StateModelFactory factory;
174 const boost::shared_ptr<crocoddyl::StateAbstract>& state =
175
1/2
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
 7 factory.create(state_type);
176 // Generating random values for the initial and terminal states
177
1/2
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
 7 const Eigen::VectorXd x = state->rand();
178
2/4
✓ Branch 3 taken 7 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 7 times.
✗ Branch 7 not taken.
 7 const Eigen::VectorXd dx = Eigen::VectorXd::Random(state->get_ndx());
179
180 // Get the num diff state
181
1/2
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
 7 crocoddyl::StateNumDiff state_num_diff(state);
182
183 // Computing the partial derivatives of the difference function separately
184 Eigen::MatrixXd Jint_num_diff_tmp(
185
2/4
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 7 times.
✗ Branch 9 not taken.
 7 Eigen::MatrixXd::Zero(state->get_ndx(), state->get_ndx()));
186 Eigen::MatrixXd Jint_num_diff_first(
187
2/4
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 7 times.
✗ Branch 9 not taken.
 7 Eigen::MatrixXd::Zero(state->get_ndx(), state->get_ndx()));
188 Eigen::MatrixXd Jint_num_diff_second(
189
2/4
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 7 times.
✗ Branch 9 not taken.
 7 Eigen::MatrixXd::Zero(state->get_ndx(), state->get_ndx()));
190
5/10
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 7 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 7 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 7 times.
✗ Branch 14 not taken.
 7 state_num_diff.Jintegrate(x, dx, Jint_num_diff_first, Jint_num_diff_tmp,
191 crocoddyl::first);
192
5/10
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 7 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 7 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 7 times.
✗ Branch 14 not taken.
 7 state_num_diff.Jintegrate(x, dx, Jint_num_diff_tmp, Jint_num_diff_second,
193 crocoddyl::second);
194
195 // Computing the partial derivatives of the given function separately
196 Eigen::MatrixXd Jint_num_diff_both_first(
197
2/4
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 7 times.
✗ Branch 9 not taken.
 7 Eigen::MatrixXd::Zero(state->get_ndx(), state->get_ndx()));
198 Eigen::MatrixXd Jint_num_diff_both_second(
199
2/4
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 7 times.
✗ Branch 9 not taken.
 7 Eigen::MatrixXd::Zero(state->get_ndx(), state->get_ndx()));
200
5/10
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 7 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 7 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 7 times.
✗ Branch 14 not taken.
 7 state_num_diff.Jintegrate(x, dx, Jint_num_diff_both_first,
201 Jint_num_diff_both_second);
202
203
8/16
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 7 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 7 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 7 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 7 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 7 times.
✗ Branch 24 not taken.
✗ Branch 28 not taken.
✓ Branch 29 taken 7 times.
 7 BOOST_CHECK((Jint_num_diff_first - Jint_num_diff_both_first).isZero(1e-9));
204
8/16
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 7 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 7 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 7 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 7 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 7 times.
✗ Branch 24 not taken.
✗ Branch 28 not taken.
✓ Branch 29 taken 7 times.
 7 BOOST_CHECK((Jint_num_diff_second - Jint_num_diff_both_second).isZero(1e-9));
205 7 }
206
207 7 void test_Jdiff_against_numdiff(StateModelTypes::Type state_type) {
208
1/2
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
 7 StateModelFactory factory;
209 const boost::shared_ptr<crocoddyl::StateAbstract>& state =
210
1/2
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
 7 factory.create(state_type);
211 // Generating random values for the initial and terminal states
212
1/2
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
 7 const Eigen::VectorXd x1 = state->rand();
213
1/2
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
 7 const Eigen::VectorXd x2 = state->rand();
214
215 // Computing the partial derivatives of the difference function analytically
216 Eigen::MatrixXd Jdiff_1(
217
2/4
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 7 times.
✗ Branch 9 not taken.
 7 Eigen::MatrixXd::Zero(state->get_ndx(), state->get_ndx()));
218 Eigen::MatrixXd Jdiff_2(
219
2/4
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 7 times.
✗ Branch 9 not taken.
 7 Eigen::MatrixXd::Zero(state->get_ndx(), state->get_ndx()));
220
5/10
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 7 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 7 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 7 times.
✗ Branch 15 not taken.
 7 state->Jdiff(x1, x2, Jdiff_1, Jdiff_2, crocoddyl::first);
221
5/10
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 7 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 7 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 7 times.
✗ Branch 15 not taken.
 7 state->Jdiff(x1, x2, Jdiff_1, Jdiff_2, crocoddyl::second);
222
223 // Computing the partial derivatives of the difference function numerically
224
1/2
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
 7 crocoddyl::StateNumDiff state_num_diff(state);
225 Eigen::MatrixXd Jdiff_num_1(
226
2/4
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 7 times.
✗ Branch 9 not taken.
 7 Eigen::MatrixXd::Zero(state->get_ndx(), state->get_ndx()));
227 Eigen::MatrixXd Jdiff_num_2(
228
2/4
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 7 times.
✗ Branch 9 not taken.
 7 Eigen::MatrixXd::Zero(state->get_ndx(), state->get_ndx()));
229
5/10
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 7 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 7 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 7 times.
✗ Branch 14 not taken.
 7 state_num_diff.Jdiff(x1, x2, Jdiff_num_1, Jdiff_num_2);
230
231 // Checking the partial derivatives against numerical differentiation
232 // Tolerance defined as in
233 // http://www.it.uom.gr/teaching/linearalgebra/NumericalRecipiesInC/c5-7.pdf
234 7 double tol = std::pow(std::sqrt(2.0 * std::numeric_limits<double>::epsilon()),
235 1. / 3.);
236
8/16
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 7 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 7 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 7 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 7 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 7 times.
✗ Branch 24 not taken.
✗ Branch 28 not taken.
✓ Branch 29 taken 7 times.
 7 BOOST_CHECK((Jdiff_1 - Jdiff_num_1).isZero(tol));
237
8/16
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 7 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 7 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 7 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 7 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 7 times.
✗ Branch 24 not taken.
✗ Branch 28 not taken.
✓ Branch 29 taken 7 times.
 7 BOOST_CHECK((Jdiff_2 - Jdiff_num_2).isZero(tol));
238 7 }
239
240 7 void test_Jintegrate_against_numdiff(StateModelTypes::Type state_type) {
241
1/2
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
 7 StateModelFactory factory;
242 const boost::shared_ptr<crocoddyl::StateAbstract>& state =
243
1/2
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
 7 factory.create(state_type);
244 // Generating random values for the initial state and its rate of change
245
1/2
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
 7 const Eigen::VectorXd x = state->rand();
246
2/4
✓ Branch 3 taken 7 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 7 times.
✗ Branch 7 not taken.
 7 const Eigen::VectorXd dx = Eigen::VectorXd::Random(state->get_ndx());
247
248 // Computing the partial derivatives of the difference function analytically
249 Eigen::MatrixXd Jint_1(
250
2/4
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 7 times.
✗ Branch 9 not taken.
 7 Eigen::MatrixXd::Zero(state->get_ndx(), state->get_ndx()));
251 Eigen::MatrixXd Jint_2(
252
2/4
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 7 times.
✗ Branch 9 not taken.
 7 Eigen::MatrixXd::Zero(state->get_ndx(), state->get_ndx()));
253
5/10
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 7 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 7 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 7 times.
✗ Branch 15 not taken.
 7 state->Jintegrate(x, dx, Jint_1, Jint_2);
254
255 // Computing the partial derivatives of the difference function numerically
256
1/2
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
 7 crocoddyl::StateNumDiff state_num_diff(state);
257 Eigen::MatrixXd Jint_num_1(
258
2/4
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 7 times.
✗ Branch 9 not taken.
 7 Eigen::MatrixXd::Zero(state->get_ndx(), state->get_ndx()));
259 Eigen::MatrixXd Jint_num_2(
260
2/4
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 7 times.
✗ Branch 9 not taken.
 7 Eigen::MatrixXd::Zero(state->get_ndx(), state->get_ndx()));
261
5/10
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 7 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 7 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 7 times.
✗ Branch 14 not taken.
 7 state_num_diff.Jintegrate(x, dx, Jint_num_1, Jint_num_2);
262
263 // Checking the partial derivatives against numerical differentiation
264 // Tolerance defined as in
265 // http://www.it.uom.gr/teaching/linearalgebra/NumericalRecipiesInC/c5-7.pdf
266 7 double tol = std::pow(std::sqrt(2.0 * std::numeric_limits<double>::epsilon()),
267 1. / 3.);
268
8/16
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 7 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 7 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 7 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 7 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 7 times.
✗ Branch 24 not taken.
✗ Branch 28 not taken.
✓ Branch 29 taken 7 times.
 7 BOOST_CHECK((Jint_1 - Jint_num_1).isZero(tol));
269
8/16
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 7 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 7 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 7 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 7 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 7 times.
✗ Branch 24 not taken.
✗ Branch 28 not taken.
✓ Branch 29 taken 7 times.
 7 BOOST_CHECK((Jint_2 - Jint_num_2).isZero(tol));
270 7 }
271
272 7 void test_JintegrateTransport(StateModelTypes::Type state_type) {
273
1/2
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
 7 StateModelFactory factory;
274 const boost::shared_ptr<crocoddyl::StateAbstract>& state =
275
1/2
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
 7 factory.create(state_type);
276 // Generating random values for the initial state and its rate of change
277
1/2
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
 7 const Eigen::VectorXd x = state->rand();
278
2/4
✓ Branch 3 taken 7 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 7 times.
✗ Branch 7 not taken.
 7 const Eigen::VectorXd dx = Eigen::VectorXd::Random(state->get_ndx());
279
280 // Computing the partial derivatives of the difference function analytically
281 Eigen::MatrixXd Jint_1(
282
2/4
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 7 times.
✗ Branch 9 not taken.
 7 Eigen::MatrixXd::Zero(state->get_ndx(), state->get_ndx()));
283 Eigen::MatrixXd Jint_2(
284
2/4
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 7 times.
✗ Branch 9 not taken.
 7 Eigen::MatrixXd::Zero(state->get_ndx(), state->get_ndx()));
285
5/10
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 7 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 7 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 7 times.
✗ Branch 15 not taken.
 7 state->Jintegrate(x, dx, Jint_1, Jint_2);
286
287 Eigen::MatrixXd Jref(
288
2/4
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 7 times.
✗ Branch 9 not taken.
 7 Eigen::MatrixXd::Random(state->get_ndx(), 2 * state->get_ndx()));
289
1/2
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
 7 const Eigen::MatrixXd Jtest(Jref);
290
291
4/8
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 7 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 7 times.
✗ Branch 12 not taken.
 7 state->JintegrateTransport(x, dx, Jref, crocoddyl::first);
292
9/18
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 7 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 7 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 7 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 7 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 7 times.
✗ Branch 24 not taken.
✓ Branch 26 taken 7 times.
✗ Branch 27 not taken.
✗ Branch 31 not taken.
✓ Branch 32 taken 7 times.
 7 BOOST_CHECK((Jref - Jint_1 * Jtest).isZero(1e-10));
293
294
1/2
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
 7 Jref = Jtest;
295
4/8
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 7 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 7 times.
✗ Branch 12 not taken.
 7 state->JintegrateTransport(x, dx, Jref, crocoddyl::second);
296
9/18
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 7 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 7 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 7 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 7 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 7 times.
✗ Branch 24 not taken.
✓ Branch 26 taken 7 times.
✗ Branch 27 not taken.
✗ Branch 31 not taken.
✓ Branch 32 taken 7 times.
 7 BOOST_CHECK((Jref - Jint_2 * Jtest).isZero(1e-10));
297 7 }
298
299 7 void test_Jdiff_and_Jintegrate_are_inverses(StateModelTypes::Type state_type) {
300
1/2
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
 7 StateModelFactory factory;
301 const boost::shared_ptr<crocoddyl::StateAbstract>& state =
302
1/2
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
 7 factory.create(state_type);
303 // Generating random states
304
1/2
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
 7 const Eigen::VectorXd x1 = state->rand();
305
2/4
✓ Branch 3 taken 7 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 7 times.
✗ Branch 7 not taken.
 7 const Eigen::VectorXd dx = Eigen::VectorXd::Random(state->get_ndx());
306
1/2
✓ Branch 3 taken 7 times.
✗ Branch 4 not taken.
 7 Eigen::VectorXd x2(state->get_nx());
307
4/8
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 7 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 7 times.
✗ Branch 12 not taken.
 7 state->integrate(x1, dx, x2);
308
309 // Computing the partial derivatives of the integrate and difference function
310
2/4
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 7 times.
✗ Branch 9 not taken.
 7 Eigen::MatrixXd Jx(Eigen::MatrixXd::Zero(state->get_ndx(), state->get_ndx()));
311 Eigen::MatrixXd Jdx(
312
2/4
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 7 times.
✗ Branch 9 not taken.
 7 Eigen::MatrixXd::Zero(state->get_ndx(), state->get_ndx()));
313
5/10
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 7 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 7 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 7 times.
✗ Branch 15 not taken.
 7 state->Jintegrate(x1, dx, Jx, Jdx);
314
2/4
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 7 times.
✗ Branch 9 not taken.
 7 Eigen::MatrixXd J1(Eigen::MatrixXd::Zero(state->get_ndx(), state->get_ndx()));
315
2/4
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 7 times.
✗ Branch 9 not taken.
 7 Eigen::MatrixXd J2(Eigen::MatrixXd::Zero(state->get_ndx(), state->get_ndx()));
316
5/10
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 7 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 7 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 7 times.
✗ Branch 15 not taken.
 7 state->Jdiff(x1, x2, J1, J2);
317
318 // Checking that Jdiff and Jintegrate are inverses
319
1/2
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
 7 Eigen::MatrixXd dX_dDX = Jdx;
320
1/2
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
 7 Eigen::MatrixXd dDX_dX = J2;
321
9/18
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 7 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 7 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 7 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 7 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 7 times.
✗ Branch 24 not taken.
✓ Branch 26 taken 7 times.
✗ Branch 27 not taken.
✗ Branch 31 not taken.
✓ Branch 32 taken 7 times.
 7 BOOST_CHECK((dX_dDX - dDX_dX.inverse()).isZero(1e-9));
322 7 }
323
324 7 void test_velocity_from_Jintegrate_Jdiff(StateModelTypes::Type state_type) {
325
1/2
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
 7 StateModelFactory factory;
326 const boost::shared_ptr<crocoddyl::StateAbstract>& state =
327
1/2
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
 7 factory.create(state_type);
328 // Generating random states
329
1/2
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
 7 const Eigen::VectorXd x1 = state->rand();
330
2/4
✓ Branch 3 taken 7 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 7 times.
✗ Branch 7 not taken.
 7 Eigen::VectorXd dx = Eigen::VectorXd::Random(state->get_ndx());
331
1/2
✓ Branch 3 taken 7 times.
✗ Branch 4 not taken.
 7 Eigen::VectorXd x2(state->get_nx());
332
4/8
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 7 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 7 times.
✗ Branch 12 not taken.
 7 state->integrate(x1, dx, x2);
333
2/4
✓ Branch 3 taken 7 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 7 times.
✗ Branch 7 not taken.
 7 Eigen::VectorXd eps = Eigen::VectorXd::Random(state->get_ndx());
334 7 double h = 1e-8;
335
336 // Computing the partial derivatives of the integrate and difference function
337
2/4
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 7 times.
✗ Branch 9 not taken.
 7 Eigen::MatrixXd Jx(Eigen::MatrixXd::Zero(state->get_ndx(), state->get_ndx()));
338 Eigen::MatrixXd Jdx(
339
2/4
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 7 times.
✗ Branch 9 not taken.
 7 Eigen::MatrixXd::Zero(state->get_ndx(), state->get_ndx()));
340
5/10
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 7 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 7 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 7 times.
✗ Branch 15 not taken.
 7 state->Jintegrate(x1, dx, Jx, Jdx);
341
2/4
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 7 times.
✗ Branch 9 not taken.
 7 Eigen::MatrixXd J1(Eigen::MatrixXd::Zero(state->get_ndx(), state->get_ndx()));
342
2/4
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 7 times.
✗ Branch 9 not taken.
 7 Eigen::MatrixXd J2(Eigen::MatrixXd::Zero(state->get_ndx(), state->get_ndx()));
343
5/10
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 7 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 7 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 7 times.
✗ Branch 15 not taken.
 7 state->Jdiff(x1, x2, J1, J2);
344
345 // Checking that computed velocity from Jintegrate
346 7 const Eigen::MatrixXd& dX_dDX = Jdx;
347
1/2
✓ Branch 3 taken 7 times.
✗ Branch 4 not taken.
 7 Eigen::VectorXd x2eps(state->get_nx());
348
6/12
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 7 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 7 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 7 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 7 times.
✗ Branch 18 not taken.
 7 state->integrate(x1, dx + eps * h, x2eps);
349
1/2
✓ Branch 3 taken 7 times.
✗ Branch 4 not taken.
 7 Eigen::VectorXd x2_eps(state->get_ndx());
350
4/8
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 7 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 7 times.
✗ Branch 12 not taken.
 7 state->diff(x2, x2eps, x2_eps);
351
10/20
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 7 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 7 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 7 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 7 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 7 times.
✗ Branch 24 not taken.
✓ Branch 26 taken 7 times.
✗ Branch 27 not taken.
✓ Branch 29 taken 7 times.
✗ Branch 30 not taken.
✗ Branch 34 not taken.
✓ Branch 35 taken 7 times.
 7 BOOST_CHECK((dX_dDX * eps - x2_eps / h).isZero(1e-3));
352
353 // Checking the velocity computed from Jdiff
354
1/2
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
 7 const Eigen::VectorXd x = state->rand();
355
1/2
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
 7 dx.setZero();
356
4/8
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 7 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 7 times.
✗ Branch 12 not taken.
 7 state->diff(x1, x, dx);
357
1/2
✓ Branch 3 taken 7 times.
✗ Branch 4 not taken.
 7 Eigen::VectorXd x2i(state->get_nx());
358
5/10
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 7 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 7 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 7 times.
✗ Branch 15 not taken.
 7 state->integrate(x, eps * h, x2i);
359
1/2
✓ Branch 3 taken 7 times.
✗ Branch 4 not taken.
 7 Eigen::VectorXd dxi(state->get_ndx());
360
4/8
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 7 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 7 times.
✗ Branch 12 not taken.
 7 state->diff(x1, x2i, dxi);
361
1/2
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
 7 J1.setZero();
362
1/2
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
 7 J2.setZero();
363
5/10
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 7 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 7 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 7 times.
✗ Branch 15 not taken.
 7 state->Jdiff(x1, x, J1, J2);
364
12/24
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 7 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 7 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 7 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 7 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 7 times.
✗ Branch 24 not taken.
✓ Branch 26 taken 7 times.
✗ Branch 27 not taken.
✓ Branch 29 taken 7 times.
✗ Branch 30 not taken.
✓ Branch 32 taken 7 times.
✗ Branch 33 not taken.
✓ Branch 35 taken 7 times.
✗ Branch 36 not taken.
✗ Branch 40 not taken.
✓ Branch 41 taken 7 times.
 7 BOOST_CHECK((J2 * eps - (-dx + dxi) / h).isZero(1e-3));
365 7 }
366
367 //----------------------------------------------------------------------------//
368
369 7 void register_state_unit_tests(StateModelTypes::Type state_type) {
370
2/4
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7 times.
✗ Branch 5 not taken.
 7 boost::test_tools::output_test_stream test_name;
371
2/4
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7 times.
✗ Branch 5 not taken.
 7 test_name << "test_" << state_type;
372
4/8
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 7 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 7 times.
✗ Branch 11 not taken.
 7 std::cout << "Running " << test_name.str() << std::endl;
373
4/8
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 7 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 7 times.
✗ Branch 12 not taken.
 7 test_suite* ts = BOOST_TEST_SUITE(test_name.str());
374
5/10
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 7 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 7 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 7 times.
✗ Branch 15 not taken.
 7 ts->add(BOOST_TEST_CASE(boost::bind(&test_state_dimension, state_type)));
375
5/10
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 7 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 7 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 7 times.
✗ Branch 15 not taken.
 7 ts->add(BOOST_TEST_CASE(
376 boost::bind(&test_integrate_against_difference, state_type)));
377
5/10
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 7 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 7 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 7 times.
✗ Branch 15 not taken.
 7 ts->add(BOOST_TEST_CASE(
378 boost::bind(&test_difference_against_integrate, state_type)));
379
5/10
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 7 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 7 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 7 times.
✗ Branch 15 not taken.
 7 ts->add(BOOST_TEST_CASE(boost::bind(&test_Jdiff_firstsecond, state_type)));
380
5/10
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 7 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 7 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 7 times.
✗ Branch 15 not taken.
 7 ts->add(BOOST_TEST_CASE(boost::bind(&test_Jint_firstsecond, state_type)));
381
5/10
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 7 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 7 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 7 times.
✗ Branch 15 not taken.
 7 ts->add(BOOST_TEST_CASE(
382 boost::bind(&test_Jdiff_num_diff_firstsecond, state_type)));
383
5/10
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 7 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 7 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 7 times.
✗ Branch 15 not taken.
 7 ts->add(BOOST_TEST_CASE(
384 boost::bind(&test_Jint_num_diff_firstsecond, state_type)));
385 14 ts->add(
386
5/10
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 7 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 7 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 7 times.
✗ Branch 15 not taken.
 7 BOOST_TEST_CASE(boost::bind(&test_Jdiff_against_numdiff, state_type)));
387
5/10
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 7 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 7 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 7 times.
✗ Branch 15 not taken.
 7 ts->add(BOOST_TEST_CASE(
388 boost::bind(&test_Jintegrate_against_numdiff, state_type)));
389
5/10
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 7 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 7 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 7 times.
✗ Branch 15 not taken.
 7 ts->add(BOOST_TEST_CASE(boost::bind(&test_JintegrateTransport, state_type)));
390
5/10
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 7 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 7 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 7 times.
✗ Branch 15 not taken.
 7 ts->add(BOOST_TEST_CASE(
391 boost::bind(&test_Jdiff_and_Jintegrate_are_inverses, state_type)));
392
5/10
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 7 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 7 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 7 times.
✗ Branch 15 not taken.
 7 ts->add(BOOST_TEST_CASE(
393 boost::bind(&test_velocity_from_Jintegrate_Jdiff, state_type)));
394
3/6
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 7 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 7 times.
✗ Branch 7 not taken.
 7 framework::master_test_suite().add(ts);
395 7 }
396
397 1 bool init_function() {
398
2/2
✓ Branch 1 taken 7 times.
✓ Branch 2 taken 1 times.
 8 for (size_t i = 0; i < StateModelTypes::all.size(); ++i) {
399 7 register_state_unit_tests(StateModelTypes::all[i]);
400 }
401 1 return true;
402 }
403
404 1 int main(int argc, char** argv) {
405 1 return ::boost::unit_test::unit_test_main(&init_function, argc, argv);
406 }
407