GCC Code Coverage Report

Directory:	./
File:	unittest/casadi/algorithms.cpp
Date:	2025-02-12 21:03:38

	Exec	Total	Coverage
Lines:	398	398	100.0%
Branches:	1155	2296	50.3%

Line	Branch	Exec	Source
1			//
2			// Copyright (c) 2019-2021 INRIA
3			//
4
5			#include "pinocchio/autodiff/casadi.hpp"
6
7			#include "pinocchio/multibody/model.hpp"
8			#include "pinocchio/multibody/data.hpp"
9
10			#include "pinocchio/algorithm/kinematics.hpp"
11			#include "pinocchio/algorithm/frames.hpp"
12			#include "pinocchio/algorithm/jacobian.hpp"
13			#include "pinocchio/algorithm/crba.hpp"
14			#include "pinocchio/algorithm/rnea.hpp"
15			#include "pinocchio/algorithm/aba.hpp"
16			#include "pinocchio/algorithm/joint-configuration.hpp"
17			#include "pinocchio/algorithm/energy.hpp"
18
19			#include "pinocchio/multibody/sample-models.hpp"
20
21			#include <boost/test/unit_test.hpp>
22			#include <boost/utility/binary.hpp>
23
24			#include "casadi-utils.hpp"
25
26			BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE)
27
28	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_jacobian)
29			{
30			typedef double Scalar;
31			typedef casadi::SX ADScalar;
32
33			typedef pinocchio::ModelTpl<Scalar> Model;
34			typedef Model::Data Data;
35
36			typedef pinocchio::ModelTpl<ADScalar> ADModel;
37			typedef ADModel::Data ADData;
38
39	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	Model model;
40	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	pinocchio::buildModels::humanoidRandom(model);
41	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.);
42	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.);
43	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	Data data(model);
44
45			typedef Model::ConfigVectorType ConfigVector;
46			typedef Model::TangentVectorType TangentVector;
47	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	ConfigVector q(model.nq);
48	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	q = pinocchio::randomConfiguration(model);
49	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	TangentVector v(TangentVector::Random(model.nv));
50
51			typedef ADModel::ConfigVectorType ConfigVectorAD;
52			typedef ADModel::TangentVectorType TangentVectorAD;
53	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	ADModel ad_model = model.cast<ADScalar>();
54	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	ADData ad_data(ad_model);
55
56			Model::Index joint_id =
57	5/18 ✓ Branch 2 taken 1 times. ✗ Branch 3 not taken. ✓ Branch 5 taken 1 times. ✗ Branch 6 not taken. ✗ Branch 7 not taken. ✓ Branch 8 taken 1 times. ✗ Branch 11 not taken. ✗ Branch 12 not taken. ✗ Branch 14 not taken. ✗ Branch 15 not taken. ✗ Branch 16 not taken. ✓ Branch 17 taken 1 times. ✗ Branch 19 not taken. ✓ Branch 20 taken 1 times. ✗ Branch 24 not taken. ✗ Branch 25 not taken. ✗ Branch 27 not taken. ✗ Branch 28 not taken.	2	model.existJointName("rarm2") ? model.getJointId("rarm2") : (Model::Index)(model.njoints - 1);
58	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	Data::Matrix6x jacobian_local(6, model.nv), jacobian_world(6, model.nv);
59	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	jacobian_local.setZero();
60	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	jacobian_world.setZero();
61
62	10/20 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 5 taken 1 times. ✗ Branch 6 not taken. ✓ Branch 11 taken 1 times. ✗ Branch 12 not taken. ✓ Branch 15 taken 1 times. ✗ Branch 16 not taken. ✓ Branch 17 taken 1 times. ✗ Branch 18 not taken. ✓ Branch 21 taken 1 times. ✗ Branch 22 not taken. ✓ Branch 23 taken 1 times. ✗ Branch 24 not taken. ✓ Branch 26 taken 1 times. ✗ Branch 27 not taken. ✓ Branch 29 taken 1 times. ✗ Branch 30 not taken. ✗ Branch 34 not taken. ✓ Branch 35 taken 1 times.	2	BOOST_CHECK(jacobian_local.isZero() && jacobian_world.isZero());
63
64	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	pinocchio::computeJointJacobians(model, data, q);
65	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	pinocchio::getJointJacobian(model, data, joint_id, pinocchio::WORLD, jacobian_world);
66	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	pinocchio::getJointJacobian(model, data, joint_id, pinocchio::LOCAL, jacobian_local);
67
68	2/4 ✓ Branch 2 taken 1 times. ✗ Branch 3 not taken. ✓ Branch 5 taken 1 times. ✗ Branch 6 not taken.	4	casadi::SX cs_q = casadi::SX::sym("q", model.nq);
69	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	ConfigVectorAD q_ad(model.nq);
70	2/2 ✓ Branch 0 taken 33 times. ✓ Branch 1 taken 1 times.	68	for (Eigen::DenseIndex k = 0; k < model.nq; ++k)
71			{
72	3/6 ✓ Branch 1 taken 33 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 33 times. ✗ Branch 5 not taken. ✓ Branch 7 taken 33 times. ✗ Branch 8 not taken.	66	q_ad[k] = cs_q(k);
73			}
74	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 << "q =\n " << q_ad << std::endl;
75
76	2/4 ✓ Branch 2 taken 1 times. ✗ Branch 3 not taken. ✓ Branch 5 taken 1 times. ✗ Branch 6 not taken.	4	casadi::SX cs_v = casadi::SX::sym("v", model.nv);
77	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	TangentVectorAD v_ad(model.nv);
78	2/2 ✓ Branch 0 taken 32 times. ✓ Branch 1 taken 1 times.	66	for (Eigen::DenseIndex k = 0; k < model.nv; ++k)
79			{
80	3/6 ✓ Branch 1 taken 32 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 32 times. ✗ Branch 5 not taken. ✓ Branch 7 taken 32 times. ✗ Branch 8 not taken.	64	v_ad[k] = cs_v(k);
81			}
82	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 << "v =\n " << v_ad << std::endl;
83
84	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	pinocchio::forwardKinematics(ad_model, ad_data, q_ad, v_ad);
85			typedef pinocchio::MotionTpl<ADScalar> MotionAD;
86		2	MotionAD & v_local = ad_data.v[(size_t)joint_id];
87	1/2 ✓ Branch 2 taken 1 times. ✗ Branch 3 not taken.	2	MotionAD v_world = ad_data.oMi[(size_t)joint_id].act(v_local);
88
89	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	casadi::SX cs_v_local(6, 1), cs_v_world(6, 1);
90	2/2 ✓ Branch 0 taken 6 times. ✓ Branch 1 taken 1 times.	14	for (Eigen::DenseIndex k = 0; k < 6; ++k)
91			{
92	4/8 ✓ Branch 1 taken 6 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 6 times. ✗ Branch 5 not taken. ✓ Branch 7 taken 6 times. ✗ Branch 8 not taken. ✓ Branch 10 taken 6 times. ✗ Branch 11 not taken.	12	cs_v_local(k) = v_local.toVector()[k];
93	4/8 ✓ Branch 1 taken 6 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 6 times. ✗ Branch 5 not taken. ✓ Branch 7 taken 6 times. ✗ Branch 8 not taken. ✓ Branch 10 taken 6 times. ✗ Branch 11 not taken.	12	cs_v_world(k) = v_world.toVector()[k];
94			}
95	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 << "v_local = " << cs_v_local << std::endl;
96	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 << "v_world = " << cs_v_world << std::endl;
97
98			casadi::Function eval_velocity_local(
99	7/14 ✓ Branch 2 taken 1 times. ✗ Branch 3 not taken. ✓ Branch 6 taken 1 times. ✗ Branch 7 not taken. ✓ Branch 9 taken 1 times. ✗ Branch 10 not taken. ✓ Branch 12 taken 1 times. ✗ Branch 13 not taken. ✓ Branch 16 taken 1 times. ✗ Branch 17 not taken. ✓ Branch 20 taken 1 times. ✗ Branch 21 not taken. ✓ Branch 23 taken 1 times. ✗ Branch 24 not taken.	14	"eval_velocity_local", casadi::SXVector{cs_q, cs_v}, casadi::SXVector{cs_v_local});
100	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 << "eval_velocity_local = " << eval_velocity_local << std::endl;
101
102			casadi::Function eval_velocity_world(
103	7/14 ✓ Branch 2 taken 1 times. ✗ Branch 3 not taken. ✓ Branch 6 taken 1 times. ✗ Branch 7 not taken. ✓ Branch 9 taken 1 times. ✗ Branch 10 not taken. ✓ Branch 12 taken 1 times. ✗ Branch 13 not taken. ✓ Branch 16 taken 1 times. ✗ Branch 17 not taken. ✓ Branch 20 taken 1 times. ✗ Branch 21 not taken. ✓ Branch 23 taken 1 times. ✗ Branch 24 not taken.	14	"eval_velocity_world", casadi::SXVector{cs_q, cs_v}, casadi::SXVector{cs_v_world});
104	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 << "eval_velocity_world = " << eval_velocity_world << std::endl;
105
106	1/2 ✓ Branch 2 taken 1 times. ✗ Branch 3 not taken.	2	casadi::SX dv_dv_local = jacobian(cs_v_local, cs_v);
107			casadi::Function eval_jacobian_local(
108	7/14 ✓ Branch 2 taken 1 times. ✗ Branch 3 not taken. ✓ Branch 6 taken 1 times. ✗ Branch 7 not taken. ✓ Branch 9 taken 1 times. ✗ Branch 10 not taken. ✓ Branch 12 taken 1 times. ✗ Branch 13 not taken. ✓ Branch 16 taken 1 times. ✗ Branch 17 not taken. ✓ Branch 20 taken 1 times. ✗ Branch 21 not taken. ✓ Branch 23 taken 1 times. ✗ Branch 24 not taken.	14	"eval_jacobian_local", casadi::SXVector{cs_q, cs_v}, casadi::SXVector{dv_dv_local});
109	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 << "eval_jacobian_local = " << eval_jacobian_local << std::endl;
110
111	1/2 ✓ Branch 2 taken 1 times. ✗ Branch 3 not taken.	2	casadi::SX dv_dv_world = jacobian(cs_v_world, cs_v);
112			casadi::Function eval_jacobian_world(
113	7/14 ✓ Branch 2 taken 1 times. ✗ Branch 3 not taken. ✓ Branch 6 taken 1 times. ✗ Branch 7 not taken. ✓ Branch 9 taken 1 times. ✗ Branch 10 not taken. ✓ Branch 12 taken 1 times. ✗ Branch 13 not taken. ✓ Branch 16 taken 1 times. ✗ Branch 17 not taken. ✓ Branch 20 taken 1 times. ✗ Branch 21 not taken. ✓ Branch 23 taken 1 times. ✗ Branch 24 not taken.	14	"eval_jacobian_world", casadi::SXVector{cs_q, cs_v}, casadi::SXVector{dv_dv_world});
114
115	1/2 ✓ Branch 2 taken 1 times. ✗ Branch 3 not taken.	2	std::vector<double> q_vec((size_t)model.nq);
116	3/6 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 5 taken 1 times. ✗ Branch 6 not taken. ✓ Branch 8 taken 1 times. ✗ Branch 9 not taken.	2	Eigen::Map<ConfigVector>(q_vec.data(), model.nq, 1) = q;
117
118	1/2 ✓ Branch 2 taken 1 times. ✗ Branch 3 not taken.	2	std::vector<double> v_vec((size_t)model.nv);
119	3/6 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 5 taken 1 times. ✗ Branch 6 not taken. ✓ Branch 8 taken 1 times. ✗ Branch 9 not taken.	2	Eigen::Map<TangentVector>(v_vec.data(), model.nv, 1) = v;
120
121	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.	8	casadi::DMVector v_local_res = eval_velocity_local(casadi::DMVector{q_vec, v_vec});
122	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.	8	casadi::DMVector J_local_res = eval_jacobian_local(casadi::DMVector{q_vec, v_vec});
123	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 << "J_local_res:" << J_local_res << std::endl;
124
125	1/2 ✓ Branch 2 taken 1 times. ✗ Branch 3 not taken.	2	std::vector<double> v_local_vec(static_cast<std::vector<double>>(v_local_res[0]));
126	10/20 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 5 taken 1 times. ✗ Branch 6 not taken. ✓ Branch 11 taken 1 times. ✗ Branch 12 not taken. ✓ Branch 14 taken 1 times. ✗ Branch 15 not taken. ✓ Branch 18 taken 1 times. ✗ Branch 19 not taken. ✓ Branch 22 taken 1 times. ✗ Branch 23 not taken. ✓ Branch 25 taken 1 times. ✗ Branch 26 not taken. ✓ Branch 28 taken 1 times. ✗ Branch 29 not taken. ✓ Branch 31 taken 1 times. ✗ Branch 32 not taken. ✗ Branch 36 not taken. ✓ Branch 37 taken 1 times.	2	BOOST_CHECK(
127			(jacobian_local * v).isApprox(Eigen::Map<pinocchio::Motion::Vector6>(v_local_vec.data())));
128
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.	8	casadi::DMVector v_world_res = eval_velocity_world(casadi::DMVector{q_vec, v_vec});
130	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.	8	casadi::DMVector J_world_res = eval_jacobian_world(casadi::DMVector{q_vec, v_vec});
131
132	1/2 ✓ Branch 2 taken 1 times. ✗ Branch 3 not taken.	2	std::vector<double> v_world_vec(static_cast<std::vector<double>>(v_world_res[0]));
133	10/20 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 5 taken 1 times. ✗ Branch 6 not taken. ✓ Branch 11 taken 1 times. ✗ Branch 12 not taken. ✓ Branch 14 taken 1 times. ✗ Branch 15 not taken. ✓ Branch 18 taken 1 times. ✗ Branch 19 not taken. ✓ Branch 22 taken 1 times. ✗ Branch 23 not taken. ✓ Branch 25 taken 1 times. ✗ Branch 26 not taken. ✓ Branch 28 taken 1 times. ✗ Branch 29 not taken. ✓ Branch 31 taken 1 times. ✗ Branch 32 not taken. ✗ Branch 36 not taken. ✓ Branch 37 taken 1 times.	2	BOOST_CHECK(
134			(jacobian_world * v).isApprox(Eigen::Map<pinocchio::Motion::Vector6>(v_world_vec.data())));
135
136	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	Data::Matrix6x J_local_mat(6, model.nv), J_world_mat(6, model.nv);
137
138	1/2 ✓ Branch 2 taken 1 times. ✗ Branch 3 not taken.	2	std::vector<double> J_local_vec(static_cast<std::vector<double>>(J_local_res[0]));
139	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	J_local_mat = Eigen::Map<Data::Matrix6x>(J_local_vec.data(), 6, model.nv);
140	7/14 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 5 taken 1 times. ✗ Branch 6 not taken. ✓ Branch 11 taken 1 times. ✗ Branch 12 not taken. ✓ Branch 15 taken 1 times. ✗ Branch 16 not taken. ✓ Branch 18 taken 1 times. ✗ Branch 19 not taken. ✓ Branch 21 taken 1 times. ✗ Branch 22 not taken. ✗ Branch 26 not taken. ✓ Branch 27 taken 1 times.	2	BOOST_CHECK(jacobian_local.isApprox(J_local_mat));
141
142	1/2 ✓ Branch 2 taken 1 times. ✗ Branch 3 not taken.	2	std::vector<double> J_world_vec(static_cast<std::vector<double>>(J_world_res[0]));
143	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	J_world_mat = Eigen::Map<Data::Matrix6x>(J_world_vec.data(), 6, model.nv);
144	7/14 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 5 taken 1 times. ✗ Branch 6 not taken. ✓ Branch 11 taken 1 times. ✗ Branch 12 not taken. ✓ Branch 15 taken 1 times. ✗ Branch 16 not taken. ✓ Branch 18 taken 1 times. ✗ Branch 19 not taken. ✓ Branch 21 taken 1 times. ✗ Branch 22 not taken. ✗ Branch 26 not taken. ✓ Branch 27 taken 1 times.	2	BOOST_CHECK(jacobian_world.isApprox(J_world_mat));
145		2	}
146
147	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_fk)
148			{
149			typedef double Scalar;
150			typedef casadi::SX ADScalar;
151
152			typedef pinocchio::ModelTpl<Scalar> Model;
153			typedef Model::Data Data;
154
155			typedef pinocchio::ModelTpl<ADScalar> ADModel;
156			typedef ADModel::Data ADData;
157
158	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	Model model;
159	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	pinocchio::buildModels::humanoidRandom(model);
160	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.);
161	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.);
162	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	Data data(model);
163
164			typedef Model::ConfigVectorType ConfigVector;
165			typedef Model::TangentVectorType TangentVector;
166	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	ConfigVector q(model.nq);
167	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	q = pinocchio::randomConfiguration(model);
168	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	TangentVector v(TangentVector::Random(model.nv));
169	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	TangentVector a(TangentVector::Random(model.nv));
170
171	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	pinocchio::forwardKinematics(model, data, q);
172
173			typedef ADModel::ConfigVectorType ConfigVectorAD;
174			typedef ADModel::TangentVectorType TangentVectorAD;
175	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	ADModel ad_model = model.cast<ADScalar>();
176	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	ADData ad_data(ad_model);
177
178	2/4 ✓ Branch 2 taken 1 times. ✗ Branch 3 not taken. ✓ Branch 5 taken 1 times. ✗ Branch 6 not taken.	4	casadi::SX cs_q = casadi::SX::sym("q", model.nq);
179	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	ConfigVectorAD q_ad(model.nq);
180	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	pinocchio::casadi::copy(cs_q, q_ad);
181
182	2/4 ✓ Branch 2 taken 1 times. ✗ Branch 3 not taken. ✓ Branch 5 taken 1 times. ✗ Branch 6 not taken.	4	casadi::SX cs_v = casadi::SX::sym("v", model.nv);
183	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	TangentVectorAD v_ad(model.nv);
184	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	pinocchio::casadi::copy(cs_v, v_ad);
185
186	2/4 ✓ Branch 2 taken 1 times. ✗ Branch 3 not taken. ✓ Branch 5 taken 1 times. ✗ Branch 6 not taken.	4	casadi::SX cs_a = casadi::SX::sym("a", model.nv);
187	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	TangentVectorAD a_ad(model.nv);
188	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	pinocchio::casadi::copy(cs_a, a_ad);
189
190	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	pinocchio::forwardKinematics(ad_model, ad_data, q_ad, v_ad, a_ad);
191	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	pinocchio::updateGlobalPlacements(ad_model, ad_data);
192	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	pinocchio::updateFramePlacements(ad_model, ad_data);
193			// typedef pinocchio::MotionTpl<ADScalar> MotionAD;
194		2	}
195
196	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_rnea)
197			{
198			typedef double Scalar;
199			typedef casadi::SX ADScalar;
200
201			typedef pinocchio::ModelTpl<Scalar> Model;
202			typedef Model::Data Data;
203
204			typedef pinocchio::ModelTpl<ADScalar> ADModel;
205			typedef ADModel::Data ADData;
206
207	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	Model model;
208	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	pinocchio::buildModels::humanoidRandom(model);
209	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.);
210	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.);
211	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	Data data(model);
212
213			typedef Model::ConfigVectorType ConfigVector;
214			typedef Model::TangentVectorType TangentVector;
215	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	ConfigVector q(model.nq);
216	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	q = pinocchio::randomConfiguration(model);
217	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	TangentVector v(TangentVector::Random(model.nv));
218	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	TangentVector a(TangentVector::Random(model.nv));
219
220			typedef ADModel::ConfigVectorType ConfigVectorAD;
221			typedef ADModel::TangentVectorType TangentVectorAD;
222	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	ADModel ad_model = model.cast<ADScalar>();
223	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	ADData ad_data(ad_model);
224
225	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	pinocchio::rnea(model, data, q, v, a);
226
227	2/4 ✓ Branch 2 taken 1 times. ✗ Branch 3 not taken. ✓ Branch 5 taken 1 times. ✗ Branch 6 not taken.	4	casadi::SX cs_q = casadi::SX::sym("q", model.nq);
228	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	ConfigVectorAD q_ad(model.nq);
229	4/8 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken. ✓ Branch 8 taken 1 times. ✗ Branch 9 not taken. ✓ Branch 11 taken 1 times. ✗ Branch 12 not taken.	2	q_ad = Eigen::Map<ConfigVectorAD>(static_cast<std::vector<ADScalar>>(cs_q).data(), model.nq, 1);
230
231	2/4 ✓ Branch 2 taken 1 times. ✗ Branch 3 not taken. ✓ Branch 5 taken 1 times. ✗ Branch 6 not taken.	4	casadi::SX cs_v = casadi::SX::sym("v", model.nv);
232	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	TangentVectorAD v_ad(model.nv);
233	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	v_ad = Eigen::Map<TangentVectorAD>(static_cast<std::vector<ADScalar>>(cs_v).data(), model.nv, 1);
234
235	2/4 ✓ Branch 2 taken 1 times. ✗ Branch 3 not taken. ✓ Branch 5 taken 1 times. ✗ Branch 6 not taken.	4	casadi::SX cs_a = casadi::SX::sym("a", model.nv);
236	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	TangentVectorAD a_ad(model.nv);
237	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	a_ad = Eigen::Map<TangentVectorAD>(static_cast<std::vector<ADScalar>>(cs_a).data(), model.nv, 1);
238
239	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	rnea(ad_model, ad_data, q_ad, v_ad, a_ad);
240	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	casadi::SX tau_ad(model.nv, 1);
241			// Eigen::Map<TangentVectorAD>(tau_ad->data(),model.nv,1)
242			// = ad_data.tau;
243	2/2 ✓ Branch 0 taken 32 times. ✓ Branch 1 taken 1 times.	66	for (Eigen::DenseIndex k = 0; k < model.nv; ++k)
244	3/6 ✓ Branch 1 taken 32 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 32 times. ✗ Branch 5 not taken. ✓ Branch 7 taken 32 times. ✗ Branch 8 not taken.	64	tau_ad(k) = ad_data.tau[k];
245			casadi::Function eval_rnea(
246	8/16 ✓ 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 15 taken 1 times. ✗ Branch 16 not taken. ✓ Branch 19 taken 1 times. ✗ Branch 20 not taken. ✓ Branch 23 taken 1 times. ✗ Branch 24 not taken. ✓ Branch 26 taken 1 times. ✗ Branch 27 not taken.	16	"eval_rnea", casadi::SXVector{cs_q, cs_v, cs_a}, casadi::SXVector{tau_ad});
247
248	1/2 ✓ Branch 2 taken 1 times. ✗ Branch 3 not taken.	2	std::vector<double> q_vec((size_t)model.nq);
249	3/6 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 5 taken 1 times. ✗ Branch 6 not taken. ✓ Branch 8 taken 1 times. ✗ Branch 9 not taken.	2	Eigen::Map<ConfigVector>(q_vec.data(), model.nq, 1) = q;
250
251	1/2 ✓ Branch 2 taken 1 times. ✗ Branch 3 not taken.	2	std::vector<double> v_vec((size_t)model.nv);
252	3/6 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 5 taken 1 times. ✗ Branch 6 not taken. ✓ Branch 8 taken 1 times. ✗ Branch 9 not taken.	2	Eigen::Map<TangentVector>(v_vec.data(), model.nv, 1) = v;
253
254	1/2 ✓ Branch 2 taken 1 times. ✗ Branch 3 not taken.	2	std::vector<double> a_vec((size_t)model.nv);
255	3/6 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 5 taken 1 times. ✗ Branch 6 not taken. ✓ Branch 8 taken 1 times. ✗ Branch 9 not taken.	2	Eigen::Map<TangentVector>(a_vec.data(), model.nv, 1) = a;
256	6/12 ✓ 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 11 taken 1 times. ✗ Branch 12 not taken. ✓ Branch 14 taken 1 times. ✗ Branch 15 not taken. ✓ Branch 18 taken 1 times. ✗ Branch 19 not taken.	10	casadi::DM tau_res = eval_rnea(casadi::DMVector{q_vec, v_vec, a_vec})[0];
257	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 << "tau_res = " << tau_res << std::endl;
258	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 5 taken 1 times. ✗ Branch 6 not taken.	6	Data::TangentVectorType tau_vec = Eigen::Map<Data::TangentVectorType>(
259	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	6	static_cast<std::vector<double>>(tau_res).data(), model.nv, 1);
260
261	7/14 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 5 taken 1 times. ✗ Branch 6 not taken. ✓ Branch 11 taken 1 times. ✗ Branch 12 not taken. ✓ Branch 15 taken 1 times. ✗ Branch 16 not taken. ✓ Branch 18 taken 1 times. ✗ Branch 19 not taken. ✓ Branch 21 taken 1 times. ✗ Branch 22 not taken. ✗ Branch 26 not taken. ✓ Branch 27 taken 1 times.	2	BOOST_CHECK(data.tau.isApprox(tau_vec));
262		2	}
263
264	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_crba)
265			{
266			typedef double Scalar;
267			typedef casadi::SX ADScalar;
268
269			typedef pinocchio::ModelTpl<Scalar> Model;
270			typedef Model::Data Data;
271
272			typedef pinocchio::ModelTpl<ADScalar> ADModel;
273			typedef ADModel::Data ADData;
274
275	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	Model model;
276	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	pinocchio::buildModels::humanoidRandom(model);
277	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.);
278	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.);
279	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	Data data(model);
280
281			typedef Model::ConfigVectorType ConfigVector;
282			typedef Model::TangentVectorType TangentVector;
283	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	ConfigVector q(model.nq);
284	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	q = pinocchio::randomConfiguration(model);
285	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	TangentVector v(TangentVector::Random(model.nv));
286	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	TangentVector a(TangentVector::Random(model.nv));
287
288			typedef ADModel::ConfigVectorType ConfigVectorAD;
289			typedef ADModel::TangentVectorType TangentVectorAD;
290	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	ADModel ad_model = model.cast<ADScalar>();
291	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	ADData ad_data(ad_model);
292
293	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	pinocchio::crba(model, data, q, pinocchio::Convention::WORLD);
294	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	data.M.triangularView<Eigen::StrictlyLower>() =
295	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.	4	data.M.transpose().triangularView<Eigen::StrictlyLower>();
296	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	pinocchio::rnea(model, data, q, v, a);
297
298	2/4 ✓ Branch 2 taken 1 times. ✗ Branch 3 not taken. ✓ Branch 5 taken 1 times. ✗ Branch 6 not taken.	4	casadi::SX cs_q = casadi::SX::sym("q", model.nq);
299	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	ConfigVectorAD q_ad(model.nq);
300	4/8 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken. ✓ Branch 8 taken 1 times. ✗ Branch 9 not taken. ✓ Branch 11 taken 1 times. ✗ Branch 12 not taken.	2	q_ad = Eigen::Map<ConfigVectorAD>(static_cast<std::vector<ADScalar>>(cs_q).data(), model.nq, 1);
301
302	2/4 ✓ Branch 2 taken 1 times. ✗ Branch 3 not taken. ✓ Branch 5 taken 1 times. ✗ Branch 6 not taken.	4	casadi::SX cs_v = casadi::SX::sym("v", model.nv);
303	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	TangentVectorAD v_ad(model.nv);
304	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	v_ad = Eigen::Map<TangentVectorAD>(static_cast<std::vector<ADScalar>>(cs_v).data(), model.nv, 1);
305
306	2/4 ✓ Branch 2 taken 1 times. ✗ Branch 3 not taken. ✓ Branch 5 taken 1 times. ✗ Branch 6 not taken.	4	casadi::SX cs_a = casadi::SX::sym("a", model.nv);
307	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	TangentVectorAD a_ad(model.nv);
308	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	a_ad = Eigen::Map<TangentVectorAD>(static_cast<std::vector<ADScalar>>(cs_a).data(), model.nv, 1);
309
310			// RNEA
311	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	rnea(ad_model, ad_data, q_ad, v_ad, a_ad);
312	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	casadi::SX cs_tau(model.nv, 1);
313			// Eigen::Map<TangentVectorAD>(tau_ad->data(),model.nv,1)
314			// = ad_data.tau;
315	2/2 ✓ Branch 0 taken 32 times. ✓ Branch 1 taken 1 times.	66	for (Eigen::DenseIndex k = 0; k < model.nv; ++k)
316	3/6 ✓ Branch 1 taken 32 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 32 times. ✗ Branch 5 not taken. ✓ Branch 7 taken 32 times. ✗ Branch 8 not taken.	64	cs_tau(k) = ad_data.tau[k];
317			casadi::Function eval_rnea(
318	8/16 ✓ 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 15 taken 1 times. ✗ Branch 16 not taken. ✓ Branch 19 taken 1 times. ✗ Branch 20 not taken. ✓ Branch 23 taken 1 times. ✗ Branch 24 not taken. ✓ Branch 26 taken 1 times. ✗ Branch 27 not taken.	16	"eval_rnea", casadi::SXVector{cs_q, cs_v, cs_a}, casadi::SXVector{cs_tau});
319			// CRBA
320	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	crba(ad_model, ad_data, q_ad, pinocchio::Convention::WORLD);
321	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	ad_data.M.triangularView<Eigen::StrictlyLower>() =
322	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.	4	ad_data.M.transpose().triangularView<Eigen::StrictlyLower>();
323	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	casadi::SX M_ad(model.nv, model.nv);
324	2/2 ✓ Branch 0 taken 32 times. ✓ Branch 1 taken 1 times.	66	for (Eigen::DenseIndex j = 0; j < model.nv; ++j)
325			{
326	2/2 ✓ Branch 0 taken 1024 times. ✓ Branch 1 taken 32 times.	2112	for (Eigen::DenseIndex i = 0; i < model.nv; ++i)
327			{
328	3/6 ✓ Branch 1 taken 1024 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1024 times. ✗ Branch 5 not taken. ✓ Branch 7 taken 1024 times. ✗ Branch 8 not taken.	2048	M_ad(i, j) = ad_data.M(i, j);
329			}
330			}
331
332	1/2 ✓ Branch 2 taken 1 times. ✗ Branch 3 not taken.	2	std::vector<double> q_vec((size_t)model.nq);
333	3/6 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 5 taken 1 times. ✗ Branch 6 not taken. ✓ Branch 8 taken 1 times. ✗ Branch 9 not taken.	2	Eigen::Map<ConfigVector>(q_vec.data(), model.nq, 1) = q;
334
335	1/2 ✓ Branch 2 taken 1 times. ✗ Branch 3 not taken.	2	std::vector<double> v_vec((size_t)model.nv);
336	3/6 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 5 taken 1 times. ✗ Branch 6 not taken. ✓ Branch 8 taken 1 times. ✗ Branch 9 not taken.	2	Eigen::Map<TangentVector>(v_vec.data(), model.nv, 1) = v;
337
338	1/2 ✓ Branch 2 taken 1 times. ✗ Branch 3 not taken.	2	std::vector<double> a_vec((size_t)model.nv);
339	3/6 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 5 taken 1 times. ✗ Branch 6 not taken. ✓ Branch 8 taken 1 times. ✗ Branch 9 not taken.	2	Eigen::Map<TangentVector>(a_vec.data(), model.nv, 1) = a;
340
341	6/12 ✓ 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 13 taken 1 times. ✗ Branch 14 not taken. ✓ Branch 17 taken 1 times. ✗ Branch 18 not taken. ✓ Branch 20 taken 1 times. ✗ Branch 21 not taken.	12	casadi::Function eval_crba("eval_crba", casadi::SXVector{cs_q}, casadi::SXVector{M_ad});
342	4/8 ✓ 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 12 taken 1 times. ✗ Branch 13 not taken.	6	casadi::DM M_res = eval_crba(casadi::DMVector{q_vec})[0];
343			Data::MatrixXs M_mat =
344	4/8 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken. ✓ Branch 8 taken 1 times. ✗ Branch 9 not taken. ✓ Branch 11 taken 1 times. ✗ Branch 12 not taken.	2	Eigen::Map<Data::MatrixXs>(static_cast<std::vector<double>>(M_res).data(), model.nv, model.nv);
345
346	7/14 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 5 taken 1 times. ✗ Branch 6 not taken. ✓ Branch 11 taken 1 times. ✗ Branch 12 not taken. ✓ Branch 15 taken 1 times. ✗ Branch 16 not taken. ✓ Branch 18 taken 1 times. ✗ Branch 19 not taken. ✓ Branch 21 taken 1 times. ✗ Branch 22 not taken. ✗ Branch 26 not taken. ✓ Branch 27 taken 1 times.	2	BOOST_CHECK(data.M.isApprox(M_mat));
347
348	1/2 ✓ Branch 2 taken 1 times. ✗ Branch 3 not taken.	2	casadi::SX dtau_da = jacobian(cs_tau, cs_a);
349			casadi::Function eval_dtau_da(
350	8/16 ✓ 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 15 taken 1 times. ✗ Branch 16 not taken. ✓ Branch 19 taken 1 times. ✗ Branch 20 not taken. ✓ Branch 23 taken 1 times. ✗ Branch 24 not taken. ✓ Branch 26 taken 1 times. ✗ Branch 27 not taken.	16	"eval_dtau_da", casadi::SXVector{cs_q, cs_v, cs_a}, casadi::SXVector{dtau_da});
351	6/12 ✓ 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 11 taken 1 times. ✗ Branch 12 not taken. ✓ Branch 14 taken 1 times. ✗ Branch 15 not taken. ✓ Branch 18 taken 1 times. ✗ Branch 19 not taken.	10	casadi::DM dtau_da_res = eval_dtau_da(casadi::DMVector{q_vec, v_vec, a_vec})[0];
352	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 5 taken 1 times. ✗ Branch 6 not taken.	6	Data::MatrixXs dtau_da_mat = Eigen::Map<Data::MatrixXs>(
353	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	6	static_cast<std::vector<double>>(dtau_da_res).data(), model.nv, model.nv);
354	7/14 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 5 taken 1 times. ✗ Branch 6 not taken. ✓ Branch 11 taken 1 times. ✗ Branch 12 not taken. ✓ Branch 15 taken 1 times. ✗ Branch 16 not taken. ✓ Branch 18 taken 1 times. ✗ Branch 19 not taken. ✓ Branch 21 taken 1 times. ✗ Branch 22 not taken. ✗ Branch 26 not taken. ✓ Branch 27 taken 1 times.	2	BOOST_CHECK(data.M.isApprox(dtau_da_mat));
355		2	}
356
357	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_aba)
358			{
359			typedef double Scalar;
360			typedef casadi::SX ADScalar;
361
362			typedef pinocchio::ModelTpl<Scalar> Model;
363			typedef Model::Data Data;
364
365			typedef pinocchio::ModelTpl<ADScalar> ADModel;
366			typedef ADModel::Data ADData;
367
368	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	Model model;
369	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	pinocchio::buildModels::humanoidRandom(model);
370	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.);
371	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.);
372	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	Data data(model);
373
374			typedef Model::ConfigVectorType ConfigVector;
375			typedef Model::TangentVectorType TangentVector;
376	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	ConfigVector q(model.nq);
377	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	q = pinocchio::randomConfiguration(model);
378	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	TangentVector v(TangentVector::Random(model.nv));
379	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	TangentVector tau(TangentVector::Random(model.nv));
380
381			typedef ADModel::ConfigVectorType ConfigVectorAD;
382			typedef ADModel::TangentVectorType TangentVectorAD;
383	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	ADModel ad_model = model.cast<ADScalar>();
384	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	ADData ad_data(ad_model);
385
386	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	pinocchio::aba(model, data, q, v, tau, pinocchio::Convention::WORLD);
387
388	2/4 ✓ Branch 2 taken 1 times. ✗ Branch 3 not taken. ✓ Branch 5 taken 1 times. ✗ Branch 6 not taken.	4	casadi::SX cs_q = casadi::SX::sym("q", model.nq);
389	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	ConfigVectorAD q_ad(model.nq);
390	4/8 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken. ✓ Branch 8 taken 1 times. ✗ Branch 9 not taken. ✓ Branch 11 taken 1 times. ✗ Branch 12 not taken.	2	q_ad = Eigen::Map<ConfigVectorAD>(static_cast<std::vector<ADScalar>>(cs_q).data(), model.nq, 1);
391
392	2/4 ✓ Branch 2 taken 1 times. ✗ Branch 3 not taken. ✓ Branch 5 taken 1 times. ✗ Branch 6 not taken.	4	casadi::SX cs_v = casadi::SX::sym("v", model.nv);
393	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	TangentVectorAD v_ad(model.nv);
394	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	v_ad = Eigen::Map<TangentVectorAD>(static_cast<std::vector<ADScalar>>(cs_v).data(), model.nv, 1);
395
396	2/4 ✓ Branch 2 taken 1 times. ✗ Branch 3 not taken. ✓ Branch 5 taken 1 times. ✗ Branch 6 not taken.	4	casadi::SX cs_tau = casadi::SX::sym("tau", model.nv);
397	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	TangentVectorAD tau_ad(model.nv);
398			tau_ad =
399	4/8 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken. ✓ Branch 8 taken 1 times. ✗ Branch 9 not taken. ✓ Branch 11 taken 1 times. ✗ Branch 12 not taken.	2	Eigen::Map<TangentVectorAD>(static_cast<std::vector<ADScalar>>(cs_tau).data(), model.nv, 1);
400
401			// ABA
402	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	pinocchio::aba(ad_model, ad_data, q_ad, v_ad, tau_ad, pinocchio::Convention::WORLD);
403	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	casadi::SX cs_ddq(model.nv, 1);
404	2/2 ✓ Branch 0 taken 32 times. ✓ Branch 1 taken 1 times.	66	for (Eigen::DenseIndex k = 0; k < model.nv; ++k)
405	3/6 ✓ Branch 1 taken 32 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 32 times. ✗ Branch 5 not taken. ✓ Branch 7 taken 32 times. ✗ Branch 8 not taken.	64	cs_ddq(k) = ad_data.ddq[k];
406			casadi::Function eval_aba(
407	8/16 ✓ 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 15 taken 1 times. ✗ Branch 16 not taken. ✓ Branch 19 taken 1 times. ✗ Branch 20 not taken. ✓ Branch 23 taken 1 times. ✗ Branch 24 not taken. ✓ Branch 26 taken 1 times. ✗ Branch 27 not taken.	16	"eval_aba", casadi::SXVector{cs_q, cs_v, cs_tau}, casadi::SXVector{cs_ddq});
408
409	1/2 ✓ Branch 2 taken 1 times. ✗ Branch 3 not taken.	2	std::vector<double> q_vec((size_t)model.nq);
410	3/6 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 5 taken 1 times. ✗ Branch 6 not taken. ✓ Branch 8 taken 1 times. ✗ Branch 9 not taken.	2	Eigen::Map<ConfigVector>(q_vec.data(), model.nq, 1) = q;
411
412	1/2 ✓ Branch 2 taken 1 times. ✗ Branch 3 not taken.	2	std::vector<double> v_vec((size_t)model.nv);
413	3/6 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 5 taken 1 times. ✗ Branch 6 not taken. ✓ Branch 8 taken 1 times. ✗ Branch 9 not taken.	2	Eigen::Map<TangentVector>(v_vec.data(), model.nv, 1) = v;
414
415	1/2 ✓ Branch 2 taken 1 times. ✗ Branch 3 not taken.	2	std::vector<double> tau_vec((size_t)model.nv);
416	3/6 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 5 taken 1 times. ✗ Branch 6 not taken. ✓ Branch 8 taken 1 times. ✗ Branch 9 not taken.	2	Eigen::Map<TangentVector>(tau_vec.data(), model.nv, 1) = tau;
417
418	6/12 ✓ 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 11 taken 1 times. ✗ Branch 12 not taken. ✓ Branch 14 taken 1 times. ✗ Branch 15 not taken. ✓ Branch 18 taken 1 times. ✗ Branch 19 not taken.	10	casadi::DM ddq_res = eval_aba(casadi::DMVector{q_vec, v_vec, tau_vec})[0];
419	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 5 taken 1 times. ✗ Branch 6 not taken.	6	Data::TangentVectorType ddq_mat = Eigen::Map<Data::TangentVectorType>(
420	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	6	static_cast<std::vector<double>>(ddq_res).data(), model.nv, 1);
421
422	7/14 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 5 taken 1 times. ✗ Branch 6 not taken. ✓ Branch 11 taken 1 times. ✗ Branch 12 not taken. ✓ Branch 15 taken 1 times. ✗ Branch 16 not taken. ✓ Branch 18 taken 1 times. ✗ Branch 19 not taken. ✓ Branch 21 taken 1 times. ✗ Branch 22 not taken. ✗ Branch 26 not taken. ✓ Branch 27 taken 1 times.	2	BOOST_CHECK(ddq_mat.isApprox(data.ddq));
423		2	}
424
425		2	void test_interp_for_model(const pinocchio::ModelTpl<double> & model)
426			{
427			using casadi::DMVector;
428			using casadi::SX;
429			using casadi::SXVector;
430			typedef SX ADScalar;
431			typedef pinocchio::ModelTpl<double> Model;
432			typedef pinocchio::ModelTpl<ADScalar> ADModel;
433	1/2 ✓ Branch 1 taken 2 times. ✗ Branch 2 not taken.	2	std::cout << model;
434
435	1/2 ✓ Branch 1 taken 2 times. ✗ Branch 2 not taken.	2	ADModel ad_model = model.cast<ADScalar>();
436		2	int nq = model.nq;
437		2	int nv = model.nv;
438
439	2/4 ✓ Branch 2 taken 2 times. ✗ Branch 3 not taken. ✓ Branch 5 taken 2 times. ✗ Branch 6 not taken.	4	SX cs_q0 = SX::sym("q0", nq);
440	2/4 ✓ Branch 2 taken 2 times. ✗ Branch 3 not taken. ✓ Branch 5 taken 2 times. ✗ Branch 6 not taken.	4	SX cs_q1 = SX::sym("q1", nq);
441	2/4 ✓ Branch 2 taken 2 times. ✗ Branch 3 not taken. ✓ Branch 5 taken 2 times. ✗ Branch 6 not taken.	4	SX cs_dq0 = SX::sym("dq0", nv);
442	2/4 ✓ Branch 2 taken 2 times. ✗ Branch 3 not taken. ✓ Branch 5 taken 2 times. ✗ Branch 6 not taken.	4	SX cs_dq1 = SX::sym("dq0", nv);
443	1/2 ✓ Branch 1 taken 2 times. ✗ Branch 2 not taken.	2	SX zero_vec(SX::zeros(2 * nv));
444
445			typedef ADModel::ConfigVectorType cConfig_t;
446			typedef ADModel::TangentVectorType cTangent_t;
447
448	2/4 ✓ Branch 1 taken 2 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 2 times. ✗ Branch 5 not taken.	2	cConfig_t cq0(nq), cq1(nq);
449	2/4 ✓ Branch 1 taken 2 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 2 times. ✗ Branch 5 not taken.	2	cTangent_t cdq0(nv), cdq1(nv);
450	1/2 ✓ Branch 1 taken 2 times. ✗ Branch 2 not taken.	2	pinocchio::casadi::copy(cs_q0, cq0);
451	1/2 ✓ Branch 1 taken 2 times. ✗ Branch 2 not taken.	2	pinocchio::casadi::copy(cs_q1, cq1);
452	1/2 ✓ Branch 1 taken 2 times. ✗ Branch 2 not taken.	2	pinocchio::casadi::copy(cs_dq0, cdq0);
453	1/2 ✓ Branch 1 taken 2 times. ✗ Branch 2 not taken.	2	pinocchio::casadi::copy(cs_dq1, cdq1);
454	1/2 ✓ Branch 1 taken 2 times. ✗ Branch 2 not taken.	2	ADScalar cs_dqall = vertcat(cs_dq0, cs_dq1);
455
456	1/2 ✓ Branch 1 taken 2 times. ✗ Branch 2 not taken.	2	auto cq0_i = pinocchio::integrate(ad_model, cq0, cdq0);
457	1/2 ✓ Branch 1 taken 2 times. ✗ Branch 2 not taken.	2	auto cq1_i = pinocchio::integrate(ad_model, cq1, cdq1);
458
459	1/2 ✓ Branch 1 taken 2 times. ✗ Branch 2 not taken.	2	ADScalar alpha(0.5);
460
461	1/2 ✓ Branch 1 taken 2 times. ✗ Branch 2 not taken.	2	cConfig_t qinterp = pinocchio::interpolate(ad_model, cq0_i, cq1_i, alpha);
462	1/2 ✓ Branch 1 taken 2 times. ✗ Branch 2 not taken.	2	cConfig_t qneutral = pinocchio::neutral(ad_model);
463	1/2 ✓ Branch 1 taken 2 times. ✗ Branch 2 not taken.	2	cTangent_t log_interp = pinocchio::difference(ad_model, qinterp, qneutral);
464
465	1/2 ✓ Branch 1 taken 2 times. ✗ Branch 2 not taken.	2	auto norm_interp = log_interp.dot(log_interp);
466	1/2 ✓ Branch 2 taken 2 times. ✗ Branch 3 not taken.	2	auto Jnorm_interp = jacobian(norm_interp, cs_dqall);
467
468			casadi::Function Jnorm_eval(
469	7/14 ✓ Branch 2 taken 2 times. ✗ Branch 3 not taken. ✓ Branch 6 taken 2 times. ✗ Branch 7 not taken. ✓ Branch 9 taken 2 times. ✗ Branch 10 not taken. ✓ Branch 12 taken 2 times. ✗ Branch 13 not taken. ✓ Branch 16 taken 2 times. ✗ Branch 17 not taken. ✓ Branch 20 taken 2 times. ✗ Branch 21 not taken. ✓ Branch 23 taken 2 times. ✗ Branch 24 not taken.	14	"Jnorm", SXVector{cs_q0, cs_q1}, SXVector{substitute(Jnorm_interp, cs_dqall, zero_vec)});
470	2/4 ✓ Branch 1 taken 2 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 2 times. ✗ Branch 5 not taken.	2	std::cout << Jnorm_eval << '\n';
471
472	1/2 ✓ Branch 1 taken 2 times. ✗ Branch 2 not taken.	2	const auto q0 = pinocchio::neutral(model);
473	1/2 ✓ Branch 1 taken 2 times. ✗ Branch 2 not taken.	2	const auto q1 = pinocchio::randomConfiguration(model);
474	1/2 ✓ Branch 1 taken 2 times. ✗ Branch 2 not taken.	2	const auto q2 = pinocchio::randomConfiguration(model);
475
476			typedef Eigen::Map<Model::ConfigVectorType> ConfigMap_t;
477	1/2 ✓ Branch 2 taken 2 times. ✗ Branch 3 not taken.	2	std::vector<double> q0_vec((size_t)nq);
478	1/2 ✓ Branch 2 taken 2 times. ✗ Branch 3 not taken.	2	std::vector<double> q1_vec((size_t)nq);
479	1/2 ✓ Branch 2 taken 2 times. ✗ Branch 3 not taken.	2	std::vector<double> q2_vec((size_t)nq);
480	3/6 ✓ Branch 1 taken 2 times. ✗ Branch 2 not taken. ✓ Branch 5 taken 2 times. ✗ Branch 6 not taken. ✓ Branch 8 taken 2 times. ✗ Branch 9 not taken.	2	ConfigMap_t(q0_vec.data(), nq, 1) = q0;
481	3/6 ✓ Branch 1 taken 2 times. ✗ Branch 2 not taken. ✓ Branch 5 taken 2 times. ✗ Branch 6 not taken. ✓ Branch 8 taken 2 times. ✗ Branch 9 not taken.	2	ConfigMap_t(q1_vec.data(), nq, 1) = q1;
482	3/6 ✓ Branch 1 taken 2 times. ✗ Branch 2 not taken. ✓ Branch 5 taken 2 times. ✗ Branch 6 not taken. ✓ Branch 8 taken 2 times. ✗ Branch 9 not taken.	2	ConfigMap_t(q2_vec.data(), nq, 1) = q2;
483
484	8/16 ✓ Branch 1 taken 2 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 2 times. ✗ Branch 5 not taken. ✓ Branch 8 taken 2 times. ✗ Branch 9 not taken. ✓ Branch 11 taken 2 times. ✗ Branch 12 not taken. ✓ Branch 15 taken 2 times. ✗ Branch 16 not taken. ✓ Branch 18 taken 2 times. ✗ Branch 19 not taken. ✓ Branch 23 taken 4 times. ✓ Branch 24 taken 2 times. ✗ Branch 29 not taken. ✗ Branch 30 not taken.	6	std::cout << Jnorm_eval(DMVector{q0_vec, q0_vec})[0] << '\n';
485	8/16 ✓ Branch 1 taken 2 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 2 times. ✗ Branch 5 not taken. ✓ Branch 8 taken 2 times. ✗ Branch 9 not taken. ✓ Branch 11 taken 2 times. ✗ Branch 12 not taken. ✓ Branch 15 taken 2 times. ✗ Branch 16 not taken. ✓ Branch 18 taken 2 times. ✗ Branch 19 not taken. ✓ Branch 23 taken 4 times. ✓ Branch 24 taken 2 times. ✗ Branch 29 not taken. ✗ Branch 30 not taken.	6	std::cout << Jnorm_eval(DMVector{q0_vec, q1_vec})[0] << '\n';
486	8/16 ✓ Branch 1 taken 2 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 2 times. ✗ Branch 5 not taken. ✓ Branch 8 taken 2 times. ✗ Branch 9 not taken. ✓ Branch 11 taken 2 times. ✗ Branch 12 not taken. ✓ Branch 15 taken 2 times. ✗ Branch 16 not taken. ✓ Branch 18 taken 2 times. ✗ Branch 19 not taken. ✓ Branch 23 taken 4 times. ✓ Branch 24 taken 2 times. ✗ Branch 29 not taken. ✗ Branch 30 not taken.	6	std::cout << Jnorm_eval(DMVector{q1_vec, q1_vec})[0] << '\n';
487	8/16 ✓ Branch 1 taken 2 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 2 times. ✗ Branch 5 not taken. ✓ Branch 8 taken 2 times. ✗ Branch 9 not taken. ✓ Branch 11 taken 2 times. ✗ Branch 12 not taken. ✓ Branch 15 taken 2 times. ✗ Branch 16 not taken. ✓ Branch 18 taken 2 times. ✗ Branch 19 not taken. ✓ Branch 23 taken 4 times. ✓ Branch 24 taken 2 times. ✗ Branch 29 not taken. ✗ Branch 30 not taken.	6	std::cout << Jnorm_eval(DMVector{q2_vec, q2_vec})[0] << '\n';
488		2	}
489
490	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_interp)
491			{
492			typedef pinocchio::ModelTpl<double> Model;
493	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	Model model;
494	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	pinocchio::buildModels::humanoidRandom(model, true);
495	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.);
496	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.);
497
498	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	test_interp_for_model(model);
499
500	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	Model model2;
501			typedef pinocchio::SE3Tpl<double> SE3;
502	5/10 ✓ Branch 2 taken 1 times. ✗ Branch 3 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.	2	size_t baseId = model2.addJoint(0, pinocchio::JointModelSpherical(), SE3::Identity(), "base");
503	5/10 ✓ Branch 2 taken 1 times. ✗ Branch 3 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.	2	model2.addJoint(baseId, pinocchio::JointModelRX(), SE3::Random(), "pole");
504	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	model2.lowerPositionLimit.tail<1>().fill(-4.);
505	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	model2.upperPositionLimit.tail<1>().fill(4.);
506
507	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	test_interp_for_model(model2);
508		2	}
509
510	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_kinetic_energy)
511			{
512			using casadi::DMVector;
513			using casadi::SX;
514			using casadi::SXVector;
515			typedef SX ADScalar;
516			typedef pinocchio::ModelTpl<double> Model;
517			typedef pinocchio::ModelTpl<ADScalar> ADModel;
518			typedef ADModel::Data ADData;
519
520	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	Model model;
521			// pinocchio::buildModels::humanoidRandom(model, true);
522	5/10 ✓ Branch 2 taken 1 times. ✗ Branch 3 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.	2	model.addJoint(0, pinocchio::JointModelSpherical(), pinocchio::SE3::Identity(), "base");
523	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	model.appendBodyToJoint(1, pinocchio::Inertia::Identity());
524	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	std::cout << model;
525	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.);
526	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.);
527
528	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	ADModel ad_model = model.cast<ADScalar>();
529	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	ADData ad_data(ad_model);
530		2	int nq = model.nq;
531		2	int nv = model.nv;
532
533	2/4 ✓ Branch 2 taken 1 times. ✗ Branch 3 not taken. ✓ Branch 5 taken 1 times. ✗ Branch 6 not taken.	4	SX cs_q0 = SX::sym("q0", nq);
534	2/4 ✓ Branch 2 taken 1 times. ✗ Branch 3 not taken. ✓ Branch 5 taken 1 times. ✗ Branch 6 not taken.	4	SX cs_q1 = SX::sym("q1", nq);
535	2/4 ✓ Branch 2 taken 1 times. ✗ Branch 3 not taken. ✓ Branch 5 taken 1 times. ✗ Branch 6 not taken.	4	SX cs_dq0 = SX::sym("dq0", nv);
536	2/4 ✓ Branch 2 taken 1 times. ✗ Branch 3 not taken. ✓ Branch 5 taken 1 times. ✗ Branch 6 not taken.	4	SX cs_dq1 = SX::sym("dq0", nv);
537	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	SX zero_vec(SX::zeros(2 * nv));
538
539			typedef ADModel::ConfigVectorType cConfig_t;
540			typedef ADModel::TangentVectorType cTangent_t;
541
542	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	cConfig_t cq0(nq), cq1(nq);
543	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	cTangent_t cdq0(nv), cdq1(nv);
544	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	pinocchio::casadi::copy(cs_q0, cq0);
545	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	pinocchio::casadi::copy(cs_q1, cq1);
546	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	pinocchio::casadi::copy(cs_dq0, cdq0);
547	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	pinocchio::casadi::copy(cs_dq1, cdq1);
548	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	ADScalar cs_dqall = vertcat(cs_dq0, cs_dq1);
549
550	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	cConfig_t cq0_i = pinocchio::integrate(ad_model, cq0, cdq0);
551	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	cConfig_t cq1_i = pinocchio::integrate(ad_model, cq1, cdq1);
552
553		2	const double dt(0.05);
554
555	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.	4	cTangent_t cv = pinocchio::difference(ad_model, cq0_i, cq1_i) / dt;
556	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	pinocchio::computeKineticEnergy(ad_model, ad_data, cq0_i, cv);
557	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	const auto KE_expr = ad_data.kinetic_energy;
558	1/2 ✓ Branch 2 taken 1 times. ✗ Branch 3 not taken.	2	const auto gKE_expr = jacobian(KE_expr, cs_dqall);
559	1/2 ✓ Branch 2 taken 1 times. ✗ Branch 3 not taken.	2	const auto HKE_expr = jacobian(gKE_expr, cs_dqall);
560
561			casadi::Function KE_eval(
562	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	"KE", SXVector{cs_q0, cs_q1}, SXVector{substitute(KE_expr, cs_dqall, zero_vec)});
563			casadi::Function gKE_eval(
564	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	"gKE", SXVector{cs_q0, cs_q1}, SXVector{substitute(gKE_expr, cs_dqall, zero_vec)});
565			casadi::Function HKE_eval(
566	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	"HKE", SXVector{cs_q0, cs_q1}, SXVector{substitute(HKE_expr, cs_dqall, zero_vec)});
567	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	std::cout << HKE_eval << std::endl;
568
569	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	auto q0 = pinocchio::neutral(model);
570	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	auto q1 = pinocchio::randomConfiguration(model);
571	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	auto q2 = pinocchio::randomConfiguration(model);
572	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	casadi::DM q0d(nq);
573	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	pinocchio::casadi::copy(q0, q0d);
574	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	casadi::DM q1d(nq);
575	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	pinocchio::casadi::copy(q1, q1d);
576	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	casadi::DM q2d(nq);
577	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	pinocchio::casadi::copy(q2, q2d);
578
579			const static double eps = 1e-8;
580
581			auto fd_grad_lambda =
582		56	[&model, &KE_eval](const Model::ConfigVectorType q0_, const Model::ConfigVectorType & q1_) {
583		4	auto nv = model.nv;
584			// finite differencing
585	2/4 ✓ Branch 1 taken 4 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 4 times. ✗ Branch 5 not taken.	4	Model::TangentVectorType dq0(nv), dq1(nv);
586	1/2 ✓ Branch 1 taken 4 times. ✗ Branch 2 not taken.	4	dq0.setZero();
587	1/2 ✓ Branch 1 taken 4 times. ✗ Branch 2 not taken.	4	dq1.setZero();
588	1/2 ✓ Branch 1 taken 4 times. ✗ Branch 2 not taken.	4	Eigen::VectorXd jac_fd(2 * nv);
589
590	5/10 ✓ Branch 1 taken 4 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 4 times. ✗ Branch 5 not taken. ✓ Branch 8 taken 4 times. ✗ Branch 9 not taken. ✓ Branch 11 taken 4 times. ✗ Branch 12 not taken. ✓ Branch 15 taken 4 times. ✗ Branch 16 not taken.	16	const casadi::DM dm = KE_eval(DMVector{eigenToDM(q0_), eigenToDM(q1_)})[0];
591	2/2 ✓ Branch 0 taken 12 times. ✓ Branch 1 taken 4 times.	16	for (int i = 0; i < nv; i++)
592			{
593	1/2 ✓ Branch 1 taken 12 times. ✗ Branch 2 not taken.	12	dq0[i] = eps;
594	1/2 ✓ Branch 1 taken 12 times. ✗ Branch 2 not taken.	12	dq1[i] = eps;
595
596	1/2 ✓ Branch 1 taken 12 times. ✗ Branch 2 not taken.	12	Model::ConfigVectorType q0_i = pinocchio::integrate(model, q0_, dq0);
597			// std::cout << "\nq0_i: " << q0_i.transpose() << std::endl;
598	5/10 ✓ Branch 1 taken 12 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 12 times. ✗ Branch 5 not taken. ✓ Branch 8 taken 12 times. ✗ Branch 9 not taken. ✓ Branch 11 taken 12 times. ✗ Branch 12 not taken. ✓ Branch 15 taken 12 times. ✗ Branch 16 not taken.	48	casadi::DM dp1 = KE_eval(DMVector{eigenToDM(q0_i), eigenToDM(q1_)})[0];
599			// std::cout << "dp1: " << dp1 << std::endl;
600	3/6 ✓ Branch 1 taken 12 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 12 times. ✗ Branch 5 not taken. ✓ Branch 7 taken 12 times. ✗ Branch 8 not taken.	24	casadi::DM diff1 = (dp1 - dm) / eps;
601
602	1/2 ✓ Branch 1 taken 12 times. ✗ Branch 2 not taken.	12	Model::ConfigVectorType q1_i = pinocchio::integrate(model, q1_, dq1);
603	5/10 ✓ Branch 1 taken 12 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 12 times. ✗ Branch 5 not taken. ✓ Branch 8 taken 12 times. ✗ Branch 9 not taken. ✓ Branch 11 taken 12 times. ✗ Branch 12 not taken. ✓ Branch 15 taken 12 times. ✗ Branch 16 not taken.	48	casadi::DM dp2 = KE_eval(DMVector{eigenToDM(q0_), eigenToDM(q1_i)})[0];
604			// std::cout << "dp2: " << dp2 << std::endl;
605	3/6 ✓ Branch 1 taken 12 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 12 times. ✗ Branch 5 not taken. ✓ Branch 7 taken 12 times. ✗ Branch 8 not taken.	24	casadi::DM diff2 = (dp2 - dm) / eps;
606
607	2/4 ✓ Branch 1 taken 12 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 12 times. ✗ Branch 5 not taken.	12	jac_fd[i] = static_cast<double>(diff1);
608	2/4 ✓ Branch 1 taken 12 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 12 times. ✗ Branch 5 not taken.	12	jac_fd[i + nv] = static_cast<double>(diff2);
609
610	1/2 ✓ Branch 1 taken 12 times. ✗ Branch 2 not taken.	12	dq0[i] = 0.;
611	1/2 ✓ Branch 1 taken 12 times. ✗ Branch 2 not taken.	12	dq1[i] = 0.;
612			}
613		8	return jac_fd;
614		6	};
615
616	9/18 ✓ 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 11 taken 1 times. ✗ Branch 12 not taken. ✓ Branch 14 taken 1 times. ✗ Branch 15 not taken. ✓ Branch 17 taken 1 times. ✗ Branch 18 not taken. ✓ Branch 20 taken 1 times. ✗ Branch 21 not taken. ✓ Branch 25 taken 2 times. ✓ Branch 26 taken 1 times. ✗ Branch 31 not taken. ✗ Branch 32 not taken.	6	std::cout << "eval: {q0d,q0d}: " << KE_eval(DMVector{q0d, q0d}) << std::endl;
617	9/18 ✓ 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 11 taken 1 times. ✗ Branch 12 not taken. ✓ Branch 14 taken 1 times. ✗ Branch 15 not taken. ✓ Branch 17 taken 1 times. ✗ Branch 18 not taken. ✓ Branch 20 taken 1 times. ✗ Branch 21 not taken. ✓ Branch 25 taken 2 times. ✓ Branch 26 taken 1 times. ✗ Branch 31 not taken. ✗ Branch 32 not taken.	6	std::cout << "grad: {q0d,q0d}: " << gKE_eval(DMVector{q0d, q0d}) << std::endl;
618	6/12 ✓ 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. ✓ Branch 13 taken 1 times. ✗ Branch 14 not taken. ✓ Branch 16 taken 1 times. ✗ Branch 17 not taken.	2	std::cout << "FD grad: {q0d,q0d}:" << fd_grad_lambda(q0, q0).transpose() << std::endl;
619	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	std::cout << "---" << std::endl;
620
621	9/18 ✓ 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 11 taken 1 times. ✗ Branch 12 not taken. ✓ Branch 14 taken 1 times. ✗ Branch 15 not taken. ✓ Branch 17 taken 1 times. ✗ Branch 18 not taken. ✓ Branch 20 taken 1 times. ✗ Branch 21 not taken. ✓ Branch 25 taken 2 times. ✓ Branch 26 taken 1 times. ✗ Branch 31 not taken. ✗ Branch 32 not taken.	6	std::cout << "eval: {q1d,q1d}: " << KE_eval(DMVector{q1d, q1d}) << std::endl;
622	9/18 ✓ 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 11 taken 1 times. ✗ Branch 12 not taken. ✓ Branch 14 taken 1 times. ✗ Branch 15 not taken. ✓ Branch 17 taken 1 times. ✗ Branch 18 not taken. ✓ Branch 20 taken 1 times. ✗ Branch 21 not taken. ✓ Branch 25 taken 2 times. ✓ Branch 26 taken 1 times. ✗ Branch 31 not taken. ✗ Branch 32 not taken.	6	std::cout << "grad: {q1d,q1d}: " << gKE_eval(DMVector{q1d, q1d}) << std::endl;
623	6/12 ✓ 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. ✓ Branch 13 taken 1 times. ✗ Branch 14 not taken. ✓ Branch 16 taken 1 times. ✗ Branch 17 not taken.	2	std::cout << "FD grad: {q1d,q1d}:" << fd_grad_lambda(q1, q1).transpose() << std::endl;
624	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	std::cout << "---" << std::endl;
625
626	9/18 ✓ 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 11 taken 1 times. ✗ Branch 12 not taken. ✓ Branch 14 taken 1 times. ✗ Branch 15 not taken. ✓ Branch 17 taken 1 times. ✗ Branch 18 not taken. ✓ Branch 20 taken 1 times. ✗ Branch 21 not taken. ✓ Branch 25 taken 2 times. ✓ Branch 26 taken 1 times. ✗ Branch 31 not taken. ✗ Branch 32 not taken.	6	std::cout << "eval: {q1d,q2d}: " << KE_eval(DMVector{q1d, q2d}) << std::endl;
627	9/18 ✓ 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 11 taken 1 times. ✗ Branch 12 not taken. ✓ Branch 14 taken 1 times. ✗ Branch 15 not taken. ✓ Branch 17 taken 1 times. ✗ Branch 18 not taken. ✓ Branch 20 taken 1 times. ✗ Branch 21 not taken. ✓ Branch 25 taken 2 times. ✓ Branch 26 taken 1 times. ✗ Branch 31 not taken. ✗ Branch 32 not taken.	6	std::cout << "grad: {q1d,q2d}: " << gKE_eval(DMVector{q1d, q2d}) << std::endl;
628	6/12 ✓ 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. ✓ Branch 13 taken 1 times. ✗ Branch 14 not taken. ✓ Branch 16 taken 1 times. ✗ Branch 17 not taken.	2	std::cout << "FD grad: {q1d,q2d}:" << fd_grad_lambda(q1, q2).transpose() << std::endl;
629	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	std::cout << "---" << std::endl;
630
631	9/18 ✓ 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 11 taken 1 times. ✗ Branch 12 not taken. ✓ Branch 14 taken 1 times. ✗ Branch 15 not taken. ✓ Branch 17 taken 1 times. ✗ Branch 18 not taken. ✓ Branch 20 taken 1 times. ✗ Branch 21 not taken. ✓ Branch 25 taken 2 times. ✓ Branch 26 taken 1 times. ✗ Branch 31 not taken. ✗ Branch 32 not taken.	6	std::cout << "eval: {q2d,q2d}: " << KE_eval(DMVector{q2d, q2d}) << std::endl;
632	9/18 ✓ 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 11 taken 1 times. ✗ Branch 12 not taken. ✓ Branch 14 taken 1 times. ✗ Branch 15 not taken. ✓ Branch 17 taken 1 times. ✗ Branch 18 not taken. ✓ Branch 20 taken 1 times. ✗ Branch 21 not taken. ✓ Branch 25 taken 2 times. ✓ Branch 26 taken 1 times. ✗ Branch 31 not taken. ✗ Branch 32 not taken.	6	std::cout << "grad: {q2d,q2d}: " << gKE_eval(DMVector{q2d, q2d}) << std::endl;
633	6/12 ✓ 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. ✓ Branch 13 taken 1 times. ✗ Branch 14 not taken. ✓ Branch 16 taken 1 times. ✗ Branch 17 not taken.	2	std::cout << "FD grad: {q2d,q2d}:" << fd_grad_lambda(q2, q2).transpose() << std::endl;
634	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	std::cout << "---" << std::endl;
635	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	std::cout << '\n';
636
637			auto fd_hess_ambda =
638		64	[&model, &gKE_eval](const Model::ConfigVectorType & q0_, const Model::ConfigVectorType & q1_) {
639		4	auto nv = model.nv;
640			// finite differencing
641	2/4 ✓ Branch 1 taken 4 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 4 times. ✗ Branch 5 not taken.	4	Model::TangentVectorType dq0(nv), dq1(nv);
642	1/2 ✓ Branch 1 taken 4 times. ✗ Branch 2 not taken.	4	dq0.setZero();
643	1/2 ✓ Branch 1 taken 4 times. ✗ Branch 2 not taken.	4	dq1.setZero();
644	1/2 ✓ Branch 1 taken 4 times. ✗ Branch 2 not taken.	4	Eigen::MatrixXd jac_fd(2 * nv, 2 * nv);
645	2/2 ✓ Branch 0 taken 12 times. ✓ Branch 1 taken 4 times.	16	for (int i = 0; i < nv; i++)
646			{
647	1/2 ✓ Branch 1 taken 12 times. ✗ Branch 2 not taken.	12	dq0(i, 0) = eps;
648	1/2 ✓ Branch 1 taken 12 times. ✗ Branch 2 not taken.	12	dq1(i, 0) = eps;
649
650	1/2 ✓ Branch 1 taken 12 times. ✗ Branch 2 not taken.	12	Model::ConfigVectorType q0_i = pinocchio::integrate(model, q0_, dq0);
651	4/8 ✓ Branch 1 taken 12 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 12 times. ✗ Branch 5 not taken. ✓ Branch 8 taken 12 times. ✗ Branch 9 not taken. ✓ Branch 11 taken 12 times. ✗ Branch 12 not taken.	48	auto dp = gKE_eval(DMVector{eigenToDM(q0_i), eigenToDM(q1_)});
652	4/8 ✓ Branch 1 taken 12 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 12 times. ✗ Branch 5 not taken. ✓ Branch 8 taken 12 times. ✗ Branch 9 not taken. ✓ Branch 11 taken 12 times. ✗ Branch 12 not taken.	48	auto dm = gKE_eval(DMVector{eigenToDM(q0_), eigenToDM(q1_)});
653	3/6 ✓ Branch 1 taken 12 times. ✗ Branch 2 not taken. ✓ Branch 6 taken 12 times. ✗ Branch 7 not taken. ✓ Branch 9 taken 12 times. ✗ Branch 10 not taken.	24	auto diff1 = (dp[0] - dm[0]) / eps;
654
655	1/2 ✓ Branch 1 taken 12 times. ✗ Branch 2 not taken.	12	Model::ConfigVectorType q1_i = pinocchio::integrate(model, q1_, dq1);
656	6/12 ✓ Branch 1 taken 12 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 12 times. ✗ Branch 5 not taken. ✓ Branch 8 taken 12 times. ✗ Branch 9 not taken. ✓ Branch 11 taken 12 times. ✗ Branch 12 not taken. ✓ Branch 17 taken 24 times. ✓ Branch 18 taken 12 times. ✗ Branch 22 not taken. ✗ Branch 23 not taken.	36	dp = gKE_eval(DMVector{eigenToDM(q0_), eigenToDM(q1_i)});
657	3/6 ✓ Branch 1 taken 12 times. ✗ Branch 2 not taken. ✓ Branch 6 taken 12 times. ✗ Branch 7 not taken. ✓ Branch 9 taken 12 times. ✗ Branch 10 not taken.	24	auto diff2 = (dp[0] - dm[0]) / eps;
658
659	2/2 ✓ Branch 1 taken 72 times. ✓ Branch 2 taken 12 times.	84	for (int j = 0; j < jac_fd.rows(); j++)
660			{
661	3/6 ✓ Branch 1 taken 72 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 72 times. ✗ Branch 5 not taken. ✓ Branch 7 taken 72 times. ✗ Branch 8 not taken.	72	jac_fd(j, i) = static_cast<double>(diff1(j));
662	3/6 ✓ Branch 1 taken 72 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 72 times. ✗ Branch 5 not taken. ✓ Branch 7 taken 72 times. ✗ Branch 8 not taken.	72	jac_fd(j, i + nv) = static_cast<double>(diff2(j));
663			}
664
665	1/2 ✓ Branch 1 taken 12 times. ✗ Branch 2 not taken.	12	dq0(i, 0) = 0.;
666	1/2 ✓ Branch 1 taken 12 times. ✗ Branch 2 not taken.	12	dq1(i, 0) = 0.;
667			}
668		8	return jac_fd;
669		6	};
670
671	8/16 ✓ 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 18 taken 1 times. ✗ Branch 19 not taken. ✓ Branch 23 taken 2 times. ✓ Branch 24 taken 1 times. ✗ Branch 29 not taken. ✗ Branch 30 not taken.	6	std::cout << HKE_eval(DMVector{q0d, q0d})[0] << '\n';
672	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	auto jac_fd = fd_hess_ambda(q0, q0);
673	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	std::cout << jac_fd << '\n';
674
675	8/16 ✓ 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 18 taken 1 times. ✗ Branch 19 not taken. ✓ Branch 23 taken 2 times. ✓ Branch 24 taken 1 times. ✗ Branch 29 not taken. ✗ Branch 30 not taken.	6	std::cout << HKE_eval(DMVector{q1d, q2d})[0] << '\n';
676	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	jac_fd = fd_hess_ambda(q1, q2);
677	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	std::cout << jac_fd << '\n';
678
679	8/16 ✓ 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 18 taken 1 times. ✗ Branch 19 not taken. ✓ Branch 23 taken 2 times. ✓ Branch 24 taken 1 times. ✗ Branch 29 not taken. ✗ Branch 30 not taken.	6	std::cout << HKE_eval(DMVector{q0d, q2d})[0] << '\n';
680	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	jac_fd = fd_hess_ambda(q0, q2);
681	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	std::cout << jac_fd << '\n';
682
683	8/16 ✓ 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 18 taken 1 times. ✗ Branch 19 not taken. ✓ Branch 23 taken 2 times. ✓ Branch 24 taken 1 times. ✗ Branch 29 not taken. ✗ Branch 30 not taken.	6	std::cout << HKE_eval(DMVector{q2d, q2d})[0] << '\n';
684	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	jac_fd = fd_hess_ambda(q2, q2);
685	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	std::cout << jac_fd << '\n';
686		2	}
687
688			BOOST_AUTO_TEST_SUITE_END()
689