GCC Code Coverage Report

Directory: ./
File: unittest/casadi/joints.cpp
Date: 2025-02-12 21:03:38
Exec Total Coverage
Lines: 193 193 100.0%
Branches: 513 994 51.6%
Line Branch Exec Source
1 //
2 // Copyright (c) 2019-2020 INRIA
3 //
4
5 #include "pinocchio/autodiff/casadi.hpp"
6
7 #include "pinocchio/multibody/joint/joint-generic.hpp"
8 #include "pinocchio/multibody/liegroup/liegroup.hpp"
9 #include "pinocchio/multibody/liegroup/liegroup-algo.hpp"
10
11 #include <iostream>
12 #include <boost/test/unit_test.hpp>
13 #include <boost/utility/binary.hpp>
14
15 BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE)
16
17
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_jointRX_motion_space)
18 {
19 typedef casadi::SX AD_double;
20 typedef pinocchio::JointCollectionDefaultTpl<AD_double> JointCollectionAD;
21 typedef pinocchio::JointCollectionDefaultTpl<double> JointCollection;
22
23 typedef pinocchio::SE3Tpl<AD_double> SE3AD;
24 typedef pinocchio::MotionTpl<AD_double> MotionAD;
25 typedef pinocchio::SE3Tpl<double> SE3;
26 typedef pinocchio::MotionTpl<double> Motion;
27 typedef pinocchio::JointMotionSubspaceTpl<Eigen::Dynamic, double> JointMotionSubspaceXd;
28
29 typedef Eigen::Matrix<AD_double, Eigen::Dynamic, 1> VectorXAD;
30 typedef Eigen::Matrix<AD_double, 6, 1> Vector6AD;
31
32 typedef JointCollectionAD::JointModelRX JointModelRXAD;
33 typedef JointModelRXAD::ConfigVector_t ConfigVectorAD;
34 // typedef JointModelRXAD::TangentVector_t TangentVectorAD;
35 typedef JointCollectionAD::JointDataRX JointDataRXAD;
36
37 typedef JointCollection::JointModelRX JointModelRX;
38 typedef JointModelRX::ConfigVector_t ConfigVector;
39 typedef JointModelRX::TangentVector_t TangentVector;
40 typedef JointCollection::JointDataRX JointDataRX;
41
42
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 JointModelRX jmodel;
43
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 jmodel.setIndexes(0, 0, 0);
44
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 JointDataRX jdata(jmodel.createData());
45
46
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 JointModelRXAD jmodel_ad = jmodel.cast<AD_double>();
47
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 JointDataRXAD jdata_ad(jmodel_ad.createData());
48
49 typedef pinocchio::LieGroup<JointModelRX>::type JointOperation;
50
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 ConfigVector q(jmodel.nq());
51
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 JointOperation().random(q);
52
53
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.
 4 casadi::SX cs_q = casadi::SX::sym("q", jmodel.nq());
54
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 ConfigVectorAD q_ad(jmodel.nq());
55
3/4
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 1 times.
✓ Branch 4 taken 1 times.
 4 for (Eigen::DenseIndex k = 0; k < jmodel.nq(); ++k)
56 {
57
3/6
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
 2 q_ad[k] = cs_q(k);
58 }
59
60 // Zero order
61
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 jmodel_ad.calc(jdata_ad, q_ad);
62
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 jmodel.calc(jdata, q);
63
64
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 SE3 M1(jdata.M);
65
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 SE3AD M2(jdata_ad.M);
66
67
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 casadi::SX cs_trans(3, 1);
68
2/2
✓ Branch 0 taken 3 times.
✓ Branch 1 taken 1 times.
 8 for (Eigen::DenseIndex k = 0; k < 3; ++k)
69 {
70
4/8
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 3 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 3 times.
✗ Branch 11 not taken.
 6 cs_trans(k) = M2.translation()[k];
71 }
72
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 casadi::SX cs_rot(3, 3);
73
2/2
✓ Branch 0 taken 3 times.
✓ Branch 1 taken 1 times.
 8 for (Eigen::DenseIndex i = 0; i < 3; ++i)
74 {
75
2/2
✓ Branch 0 taken 9 times.
✓ Branch 1 taken 3 times.
 24 for (Eigen::DenseIndex j = 0; j < 3; ++j)
76 {
77
4/8
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 9 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 9 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 9 times.
✗ Branch 11 not taken.
 18 cs_rot(i, j) = M2.rotation()(i, j);
78 }
79 }
80
81 casadi::Function eval_placement(
82
7/14
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 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_placement", casadi::SXVector{cs_q}, casadi::SXVector{cs_trans, cs_rot});
83
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 << "Joint Placement = " << eval_placement << std::endl;
84
85
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 2 std::vector<double> q_vec((size_t)jmodel.nq());
86
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<ConfigVector>(q_vec.data(), jmodel.nq(), 1) = q;
87
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.
 6 casadi::DMVector res = eval_placement(casadi::DMVector{q_vec});
88
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 << "M(q)=" << res << std::endl;
89
90
11/22
✓ 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 26 taken 1 times.
✗ Branch 27 not taken.
✓ Branch 29 taken 1 times.
✗ Branch 30 not taken.
✓ Branch 32 taken 1 times.
✗ Branch 33 not taken.
✓ Branch 35 taken 1 times.
✗ Branch 36 not taken.
✗ Branch 40 not taken.
✓ Branch 41 taken 1 times.
 2 BOOST_CHECK(M1.translation().isApprox(Eigen::Map<SE3::Vector3>(res[0]->data())));
91
11/22
✓ 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 26 taken 1 times.
✗ Branch 27 not taken.
✓ Branch 29 taken 1 times.
✗ Branch 30 not taken.
✓ Branch 32 taken 1 times.
✗ Branch 33 not taken.
✓ Branch 35 taken 1 times.
✗ Branch 36 not taken.
✗ Branch 40 not taken.
✓ Branch 41 taken 1 times.
 2 BOOST_CHECK(M1.rotation().isApprox(Eigen::Map<SE3::Matrix3>(res[1]->data())));
92
93 // First order
94
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.
 4 casadi::SX cs_v = casadi::SX::sym("v", jmodel.nv());
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 TangentVector v(TangentVector::Random(jmodel.nv()));
96
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 VectorXAD v_ad(jmodel_ad.nv());
97
98
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 2 std::vector<double> v_vec((size_t)jmodel.nv());
99
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<TangentVector>(v_vec.data(), jmodel.nv(), 1) = v;
100
101
3/4
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 1 times.
✓ Branch 4 taken 1 times.
 4 for (Eigen::DenseIndex k = 0; k < jmodel.nv(); ++k)
102 {
103
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 v_ad[k] = cs_v(k);
104 }
105
106
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 jmodel.calc(jdata, q, v);
107
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 Motion m(jdata.v);
108
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 JointMotionSubspaceXd Sref(jdata.S.matrix());
109
110
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 jmodel_ad.calc(jdata_ad, q_ad, v_ad);
111
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 Vector6AD Y;
112
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 MotionAD m_ad(jdata_ad.v);
113
114
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 casadi::SX cs_vel(6, 1);
115
2/2
✓ Branch 0 taken 6 times.
✓ Branch 1 taken 1 times.
 14 for (Eigen::DenseIndex k = 0; k < 6; ++k)
116 {
117
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_vel(k) = m_ad.toVector()[k];
118 }
119 casadi::Function eval_velocity(
120
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", casadi::SXVector{cs_q, cs_v}, casadi::SXVector{cs_vel});
121
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 << "Joint Velocity = " << eval_velocity << std::endl;
122
123
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 res_vel = eval_velocity(casadi::DMVector{q_vec, v_vec});
124
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(q,v)=" << res_vel << std::endl;
125
126
11/22
✓ 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 26 taken 1 times.
✗ Branch 27 not taken.
✓ Branch 29 taken 1 times.
✗ Branch 30 not taken.
✓ Branch 32 taken 1 times.
✗ Branch 33 not taken.
✓ Branch 35 taken 1 times.
✗ Branch 36 not taken.
✗ Branch 40 not taken.
✓ Branch 41 taken 1 times.
 2 BOOST_CHECK(m.linear().isApprox(Eigen::Map<Motion::Vector3>(res_vel[0]->data())));
127
11/22
✓ 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 26 taken 1 times.
✗ Branch 27 not taken.
✓ Branch 29 taken 1 times.
✗ Branch 30 not taken.
✓ Branch 32 taken 1 times.
✗ Branch 33 not taken.
✓ Branch 35 taken 1 times.
✗ Branch 36 not taken.
✗ Branch 40 not taken.
✓ Branch 41 taken 1 times.
 2 BOOST_CHECK(m.angular().isApprox(Eigen::Map<Motion::Vector3>(res_vel[0]->data() + 3)));
128
129
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 2 casadi::SX dvel_dv = jacobian(cs_vel, cs_v);
130
7/14
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 1 times.
✗ Branch 13 not taken.
✓ Branch 16 taken 1 times.
✗ Branch 17 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 1 times.
✗ Branch 24 not taken.
 14 casadi::Function eval_S("eval_S", casadi::SXVector{cs_q, cs_v}, casadi::SXVector{dvel_dv});
131
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 << "S = " << eval_S << std::endl;
132
133
4/8
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
 8 casadi::DMVector res_S = eval_S(casadi::DMVector{q_vec, v_vec});
134
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 << "res_S:" << res_S << std::endl;
135
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 JointMotionSubspaceXd::DenseBase Sref_mat = Sref.matrix();
136
137
2/2
✓ Branch 0 taken 5 times.
✓ Branch 1 taken 1 times.
 12 for (Eigen::DenseIndex i = 0; i < 6; ++i)
138 {
139
3/4
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 1 times.
✓ Branch 4 taken 5 times.
 12 for (Eigen::DenseIndex j = 0; i < Sref.nv(); ++i)
140
9/18
✓ 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 21 taken 1 times.
✗ Branch 22 not taken.
✓ Branch 25 taken 1 times.
✗ Branch 26 not taken.
✓ Branch 28 taken 1 times.
✗ Branch 29 not taken.
✗ Branch 34 not taken.
✓ Branch 35 taken 1 times.
 2 BOOST_CHECK(
141 std::fabs(Sref_mat(i, j) - (double)res_S[0](i, j))
142 <= Eigen::NumTraits<double>::dummy_precision());
143 }
144 2 }
145
146 template<typename JointModel_>
147 struct init;
148
149 template<typename JointModel_>
150 struct init
151 {
152 34 static JointModel_ run()
153 {
154 34 JointModel_ jmodel;
155 34 jmodel.setIndexes(0, 0, 0);
156 34 return jmodel;
157 }
158
159 static std::string name()
160 {
161 return "default " + JointModel_::classname();
162 }
163 };
164
165 template<typename Scalar, int Options>
166 struct init<pinocchio::JointModelRevoluteUnalignedTpl<Scalar, Options>>
167 {
168 typedef pinocchio::JointModelRevoluteUnalignedTpl<Scalar, Options> JointModel;
169
170 static JointModel run()
171 {
172 typedef typename JointModel::Vector3 Vector3;
173 JointModel jmodel(Vector3::Random().normalized());
174
175 jmodel.setIndexes(0, 0, 0);
176 return jmodel;
177 }
178
179 static std::string name()
180 {
181 return JointModel::classname();
182 }
183 };
184
185 template<typename Scalar, int Options>
186 struct init<pinocchio::JointModelRevoluteUnboundedUnalignedTpl<Scalar, Options>>
187 {
188 typedef pinocchio::JointModelRevoluteUnboundedUnalignedTpl<Scalar, Options> JointModel;
189
190 static JointModel run()
191 {
192 typedef typename JointModel::Vector3 Vector3;
193 JointModel jmodel(Vector3::Random().normalized());
194
195 jmodel.setIndexes(0, 0, 0);
196 return jmodel;
197 }
198
199 static std::string name()
200 {
201 return JointModel::classname();
202 }
203 };
204
205 template<typename Scalar, int Options>
206 struct init<pinocchio::JointModelPrismaticUnalignedTpl<Scalar, Options>>
207 {
208 typedef pinocchio::JointModelPrismaticUnalignedTpl<Scalar, Options> JointModel;
209
210 static JointModel run()
211 {
212 typedef typename JointModel::Vector3 Vector3;
213 JointModel jmodel(Vector3::Random().normalized());
214
215 jmodel.setIndexes(0, 0, 0);
216 return jmodel;
217 }
218
219 static std::string name()
220 {
221 return JointModel::classname();
222 }
223 };
224
225 template<typename Scalar, int Options, template<typename, int> class JointCollection>
226 struct init<pinocchio::JointModelTpl<Scalar, Options, JointCollection>>
227 {
228 typedef pinocchio::JointModelTpl<Scalar, Options, JointCollection> JointModel;
229
230 static JointModel run()
231 {
232 typedef pinocchio::JointModelRevoluteTpl<Scalar, Options, 0> JointModelRX;
233 JointModel jmodel((JointModelRX()));
234
235 jmodel.setIndexes(0, 0, 0);
236 return jmodel;
237 }
238
239 static std::string name()
240 {
241 return JointModel::classname();
242 }
243 };
244
245 template<typename Scalar, int Options, template<typename, int> class JointCollection>
246 struct init<pinocchio::JointModelCompositeTpl<Scalar, Options, JointCollection>>
247 {
248 typedef pinocchio::JointModelCompositeTpl<Scalar, Options, JointCollection> JointModel;
249
250 static JointModel run()
251 {
252 typedef pinocchio::JointModelRevoluteTpl<Scalar, Options, 0> JointModelRX;
253 typedef pinocchio::JointModelRevoluteTpl<Scalar, Options, 1> JointModelRY;
254 JointModel jmodel((JointModelRX()));
255 jmodel.addJoint(JointModelRY());
256
257 jmodel.setIndexes(0, 0, 0);
258 return jmodel;
259 }
260
261 static std::string name()
262 {
263 return JointModel::classname();
264 }
265 };
266
267 template<typename JointModel_>
268 struct init<pinocchio::JointModelMimic<JointModel_>>
269 {
270 typedef pinocchio::JointModelMimic<JointModel_> JointModel;
271
272 6 static JointModel run()
273 {
274
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 6 JointModel_ jmodel_ref = init<JointModel_>::run();
275
276
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 6 JointModel jmodel(jmodel_ref, 1., 0.);
277
278 12 return jmodel;
279 }
280
281 static std::string name()
282 {
283 return JointModel::classname();
284 }
285 };
286
287 struct TestADOnJoints
288 {
289 template<typename JointModel_>
290 34 void operator()(const pinocchio::JointModelBase<JointModel_> &) const
291 {
292
1/2
✓ Branch 1 taken 17 times.
✗ Branch 2 not taken.
 34 JointModel_ jmodel = init<JointModel_>::run();
293
1/2
✓ Branch 1 taken 17 times.
✗ Branch 2 not taken.
 34 jmodel.setIndexes(0, 0, 0);
294
1/2
✓ Branch 1 taken 14 times.
✗ Branch 2 not taken.
 34 test(jmodel);
295 34 }
296
297 template<typename Scalar, int Options, int axis>
298 6 void operator()(const pinocchio::JointModelHelicalTpl<Scalar, Options, axis> &) const
299 {
300 typedef pinocchio::JointModelHelicalTpl<Scalar, Options, axis> JointModel;
301
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 6 JointModel jmodel(Scalar(0.4));
302
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 6 jmodel.setIndexes(0, 0, 0);
303
304
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 6 test(jmodel);
305 6 }
306
307 template<typename Scalar, int Options>
308 1 void operator()(const pinocchio::JointModelUniversalTpl<Scalar, Options> &) const
309 {
310 typedef pinocchio::JointModelUniversalTpl<Scalar, Options> JointModel;
311 typedef typename JointModel::Vector3 Vector3;
312
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.
 1 JointModel jmodel(Vector3::UnitX(), Vector3::UnitY());
313
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 jmodel.setIndexes(0, 0, 0);
314
315
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 test(jmodel);
316 1 }
317
318 template<typename Scalar, int Options>
319 1 void operator()(const pinocchio::JointModelHelicalUnalignedTpl<Scalar, Options> &) const
320 {
321 typedef pinocchio::JointModelHelicalUnalignedTpl<Scalar, Options> JointModel;
322 typedef typename JointModel::Vector3 Vector3;
323
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.
 1 JointModel jmodel(Vector3::Random().normalized());
324
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 jmodel.setIndexes(0, 0, 0);
325
326
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 test(jmodel);
327 1 }
328
329 template<typename Scalar, int Options>
330 1 void operator()(const pinocchio::JointModelRevoluteUnalignedTpl<Scalar, Options> &) const
331 {
332 typedef pinocchio::JointModelRevoluteUnalignedTpl<Scalar, Options> JointModel;
333 typedef typename JointModel::Vector3 Vector3;
334
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.
 1 JointModel jmodel(Vector3::Random().normalized());
335
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 jmodel.setIndexes(0, 0, 0);
336
337
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 test(jmodel);
338 1 }
339
340 template<typename Scalar, int Options>
341 1 void operator()(const pinocchio::JointModelRevoluteUnboundedUnalignedTpl<Scalar, Options> &) const
342 {
343 typedef pinocchio::JointModelRevoluteUnboundedUnalignedTpl<Scalar, Options> JointModel;
344 typedef typename JointModel::Vector3 Vector3;
345
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.
 1 JointModel jmodel(Vector3::Random().normalized());
346
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 jmodel.setIndexes(0, 0, 0);
347
348
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 test(jmodel);
349 1 }
350
351 template<typename Scalar, int Options>
352 1 void operator()(const pinocchio::JointModelPrismaticUnalignedTpl<Scalar, Options> &) const
353 {
354 typedef pinocchio::JointModelPrismaticUnalignedTpl<Scalar, Options> JointModel;
355 typedef typename JointModel::Vector3 Vector3;
356
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.
 1 JointModel jmodel(Vector3::Random().normalized());
357
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 jmodel.setIndexes(0, 0, 0);
358
359
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 test(jmodel);
360 1 }
361
362 template<typename Scalar, int Options, template<typename, int> class JointCollection>
363 1 void operator()(const pinocchio::JointModelTpl<Scalar, Options, JointCollection> &) const
364 {
365 typedef pinocchio::JointModelRevoluteTpl<Scalar, Options, 0> JointModelRX;
366 typedef pinocchio::JointModelTpl<Scalar, Options, JointCollection> JointModel;
367
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 JointModel jmodel((JointModelRX()));
368
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 jmodel.setIndexes(0, 0, 0);
369
370
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 test(jmodel);
371 1 }
372
373 template<typename Scalar, int Options, template<typename, int> class JointCollection>
374 1 void operator()(const pinocchio::JointModelCompositeTpl<Scalar, Options, JointCollection> &) const
375 {
376 typedef pinocchio::JointModelRevoluteTpl<Scalar, Options, 0> JointModelRX;
377 typedef pinocchio::JointModelRevoluteTpl<Scalar, Options, 1> JointModelRY;
378 typedef pinocchio::JointModelCompositeTpl<Scalar, Options, JointCollection> JointModel;
379
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.
 1 JointModel jmodel((JointModelRX()));
380
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.
 1 jmodel.addJoint(JointModelRY());
381
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 jmodel.setIndexes(0, 0, 0);
382
383
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 test(jmodel);
384 1 }
385
386 // TODO: get the nq and nv quantity from LieGroups
387 template<typename JointModel_>
388 6 static void test(const pinocchio::JointModelMimic<JointModel_> & /*jmodel*/)
389 { /* do nothing */
390 6 }
391
392 template<typename JointModel>
393 48 static void test(const pinocchio::JointModelBase<JointModel> & jmodel)
394 {
395
2/4
✓ Branch 1 taken 24 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 24 times.
✗ Branch 5 not taken.
 48 std::cout << "--" << std::endl;
396
4/8
✓ Branch 1 taken 24 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 24 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 24 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 24 times.
✗ Branch 11 not taken.
 48 std::cout << "jmodel: " << jmodel.shortname() << std::endl;
397
398 typedef casadi::SX AD_double;
399
400 typedef pinocchio::SE3Tpl<AD_double> SE3AD;
401 typedef pinocchio::MotionTpl<AD_double> MotionAD;
402 typedef pinocchio::SE3Tpl<double> SE3;
403 typedef pinocchio::MotionTpl<double> Motion;
404 typedef pinocchio::JointMotionSubspaceTpl<Eigen::Dynamic, double> JointMotionSubspaceXd;
405
406 typedef Eigen::Matrix<AD_double, Eigen::Dynamic, 1> VectorXAD;
407 typedef Eigen::Matrix<AD_double, 6, 1> Vector6AD;
408
409 typedef typename pinocchio::CastType<AD_double, JointModel>::type JointModelAD;
410 typedef typename JointModelAD::JointDataDerived JointDataAD;
411
412 typedef typename JointModelAD::ConfigVector_t ConfigVectorAD;
413
414 typedef typename JointModel::JointDataDerived JointData;
415 typedef typename JointModel::ConfigVector_t ConfigVector;
416 typedef typename JointModel::TangentVector_t TangentVector;
417
418
1/2
✓ Branch 1 taken 24 times.
✗ Branch 2 not taken.
 48 JointData jdata(jmodel.createData());
419 48 pinocchio::JointDataBase<JointData> & jdata_base = jdata;
420
421
1/2
✓ Branch 1 taken 24 times.
✗ Branch 2 not taken.
 48 JointModelAD jmodel_ad = jmodel.template cast<AD_double>();
422
1/2
✓ Branch 1 taken 24 times.
✗ Branch 2 not taken.
 48 JointDataAD jdata_ad(jmodel_ad.createData());
423 48 pinocchio::JointDataBase<JointDataAD> & jdata_ad_base = jdata_ad;
424
425
2/4
✓ Branch 1 taken 24 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 24 times.
✗ Branch 5 not taken.
 48 ConfigVector q(jmodel.nq());
426
427
3/6
✓ Branch 1 taken 24 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 24 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 24 times.
✗ Branch 8 not taken.
 48 ConfigVector lb(ConfigVector::Constant(jmodel.nq(), -1.));
428
3/6
✓ Branch 1 taken 24 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 24 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 24 times.
✗ Branch 8 not taken.
 48 ConfigVector ub(ConfigVector::Constant(jmodel.nq(), 1.));
429
430 typedef pinocchio::RandomConfigurationStep<
431 pinocchio::LieGroupMap, ConfigVector, ConfigVector, ConfigVector>
432 RandomConfigAlgo;
433
2/4
✓ Branch 1 taken 24 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 24 times.
✗ Branch 6 not taken.
 48 RandomConfigAlgo::run(jmodel.derived(), typename RandomConfigAlgo::ArgsType(q, lb, ub));
434
435
3/6
✓ Branch 1 taken 24 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 24 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 24 times.
✗ Branch 9 not taken.
 96 casadi::SX cs_q = casadi::SX::sym("q", jmodel.nq());
436
2/4
✓ Branch 1 taken 24 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 24 times.
✗ Branch 5 not taken.
 48 ConfigVectorAD q_ad(jmodel.nq());
437
3/4
✓ Branch 1 taken 70 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 46 times.
✓ Branch 4 taken 24 times.
 140 for (Eigen::DenseIndex k = 0; k < jmodel.nq(); ++k)
438 {
439
3/6
✓ Branch 1 taken 46 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 46 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 46 times.
✗ Branch 8 not taken.
 92 q_ad[k] = cs_q(k);
440 }
441
442 // Zero order
443
1/2
✓ Branch 1 taken 24 times.
✗ Branch 2 not taken.
 48 jmodel_ad.calc(jdata_ad, q_ad);
444
1/2
✓ Branch 1 taken 24 times.
✗ Branch 2 not taken.
 48 jmodel.calc(jdata, q);
445
446
2/4
✓ Branch 1 taken 24 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 23 times.
✗ Branch 5 not taken.
 48 SE3 M1(jdata_base.M());
447
2/4
✓ Branch 1 taken 24 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 23 times.
✗ Branch 5 not taken.
 48 SE3AD M2(jdata_ad_base.M());
448
449
1/2
✓ Branch 1 taken 24 times.
✗ Branch 2 not taken.
 48 casadi::SX cs_trans(3, 1);
450
2/2
✓ Branch 0 taken 72 times.
✓ Branch 1 taken 24 times.
 192 for (Eigen::DenseIndex k = 0; k < 3; ++k)
451 {
452
4/8
✓ 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.
✓ Branch 10 taken 72 times.
✗ Branch 11 not taken.
 144 cs_trans(k) = M2.translation()[k];
453 }
454
1/2
✓ Branch 1 taken 24 times.
✗ Branch 2 not taken.
 48 casadi::SX cs_rot(3, 3);
455
2/2
✓ Branch 0 taken 72 times.
✓ Branch 1 taken 24 times.
 192 for (Eigen::DenseIndex i = 0; i < 3; ++i)
456 {
457
2/2
✓ Branch 0 taken 216 times.
✓ Branch 1 taken 72 times.
 576 for (Eigen::DenseIndex j = 0; j < 3; ++j)
458 {
459
4/8
✓ Branch 1 taken 216 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 216 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 216 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 216 times.
✗ Branch 11 not taken.
 432 cs_rot(i, j) = M2.rotation()(i, j);
460 }
461 }
462
463
2/4
✓ Branch 3 taken 24 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 24 times.
✗ Branch 7 not taken.
 144 casadi::Function eval_placement(
464
5/10
✓ Branch 1 taken 24 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 24 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 24 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 24 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 24 times.
✗ Branch 16 not taken.
 336 "eval_placement", casadi::SXVector{cs_q}, casadi::SXVector{cs_trans, cs_rot});
465
3/6
✓ Branch 1 taken 24 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 24 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 24 times.
✗ Branch 8 not taken.
 48 std::cout << "Joint Placement = " << eval_placement << std::endl;
466
467
2/4
✓ Branch 2 taken 24 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 24 times.
✗ Branch 6 not taken.
 48 std::vector<double> q_vec((size_t)jmodel.nq());
468
4/8
✓ Branch 1 taken 24 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 24 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 24 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 24 times.
✗ Branch 12 not taken.
 48 Eigen::Map<ConfigVector>(q_vec.data(), jmodel.nq(), 1) = q;
469
3/6
✓ Branch 1 taken 24 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 24 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 24 times.
✗ Branch 9 not taken.
 144 casadi::DMVector res = eval_placement(casadi::DMVector{q_vec});
470
3/6
✓ Branch 1 taken 24 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 24 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 24 times.
✗ Branch 8 not taken.
 48 std::cout << "M(q)=" << res << std::endl;
471
472
11/22
✓ Branch 1 taken 24 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 24 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 24 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 24 times.
✗ Branch 15 not taken.
✓ Branch 18 taken 24 times.
✗ Branch 19 not taken.
✓ Branch 22 taken 24 times.
✗ Branch 23 not taken.
✓ Branch 26 taken 24 times.
✗ Branch 27 not taken.
✓ Branch 29 taken 24 times.
✗ Branch 30 not taken.
✓ Branch 32 taken 24 times.
✗ Branch 33 not taken.
✓ Branch 35 taken 24 times.
✗ Branch 36 not taken.
✗ Branch 40 not taken.
✓ Branch 41 taken 24 times.
 48 BOOST_CHECK(M1.translation().isApprox(Eigen::Map<SE3::Vector3>(res[0]->data())));
473
11/22
✓ Branch 1 taken 24 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 24 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 24 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 24 times.
✗ Branch 15 not taken.
✓ Branch 18 taken 24 times.
✗ Branch 19 not taken.
✓ Branch 22 taken 24 times.
✗ Branch 23 not taken.
✓ Branch 26 taken 24 times.
✗ Branch 27 not taken.
✓ Branch 29 taken 24 times.
✗ Branch 30 not taken.
✓ Branch 32 taken 24 times.
✗ Branch 33 not taken.
✓ Branch 35 taken 24 times.
✗ Branch 36 not taken.
✗ Branch 40 not taken.
✓ Branch 41 taken 24 times.
 48 BOOST_CHECK(M1.rotation().isApprox(Eigen::Map<SE3::Matrix3>(res[1]->data())));
474
475 // First order
476
3/6
✓ Branch 1 taken 24 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 24 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 24 times.
✗ Branch 9 not taken.
 96 casadi::SX cs_v = casadi::SX::sym("v", jmodel.nv());
477
3/6
✓ Branch 1 taken 24 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 24 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 24 times.
✗ Branch 8 not taken.
 48 TangentVector v(TangentVector::Random(jmodel.nv()));
478
2/4
✓ Branch 1 taken 24 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 24 times.
✗ Branch 5 not taken.
 48 VectorXAD v_ad(jmodel_ad.nv());
479
480
2/4
✓ Branch 2 taken 24 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 24 times.
✗ Branch 6 not taken.
 48 std::vector<double> v_vec((size_t)jmodel.nv());
481
4/8
✓ Branch 1 taken 24 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 24 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 24 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 24 times.
✗ Branch 12 not taken.
 48 Eigen::Map<TangentVector>(v_vec.data(), jmodel.nv(), 1) = v;
482
483
3/4
✓ Branch 1 taken 63 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 39 times.
✓ Branch 4 taken 24 times.
 126 for (Eigen::DenseIndex k = 0; k < jmodel.nv(); ++k)
484 {
485
3/6
✓ Branch 1 taken 39 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 39 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 39 times.
✗ Branch 8 not taken.
 78 v_ad[k] = cs_v(k);
486 }
487
488
1/2
✓ Branch 1 taken 24 times.
✗ Branch 2 not taken.
 48 jmodel.calc(jdata, q, v);
489
2/4
✓ Branch 1 taken 24 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 23 times.
✗ Branch 5 not taken.
 48 Motion m(jdata_base.v());
490
3/6
✓ Branch 1 taken 24 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 24 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 24 times.
✗ Branch 8 not taken.
 48 JointMotionSubspaceXd Sref(jdata_base.S().matrix());
491
492
1/2
✓ Branch 1 taken 24 times.
✗ Branch 2 not taken.
 48 jmodel_ad.calc(jdata_ad, q_ad, v_ad);
493
1/2
✓ Branch 1 taken 24 times.
✗ Branch 2 not taken.
 48 Vector6AD Y;
494
2/4
✓ Branch 1 taken 24 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 23 times.
✗ Branch 5 not taken.
 48 MotionAD m_ad(jdata_ad_base.v());
495
496
1/2
✓ Branch 1 taken 24 times.
✗ Branch 2 not taken.
 48 casadi::SX cs_vel(6, 1);
497
2/2
✓ Branch 0 taken 144 times.
✓ Branch 1 taken 24 times.
 336 for (Eigen::DenseIndex k = 0; k < 6; ++k)
498 {
499
4/8
✓ Branch 1 taken 144 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 144 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 144 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 144 times.
✗ Branch 11 not taken.
 288 cs_vel(k) = m_ad.toVector()[k];
500 }
501
2/4
✓ Branch 3 taken 24 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 24 times.
✗ Branch 7 not taken.
 144 casadi::Function eval_velocity(
502
5/10
✓ Branch 1 taken 24 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 24 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 24 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 24 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 24 times.
✗ Branch 16 not taken.
 336 "eval_velocity", casadi::SXVector{cs_q, cs_v}, casadi::SXVector{cs_vel});
503
3/6
✓ Branch 1 taken 24 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 24 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 24 times.
✗ Branch 8 not taken.
 48 std::cout << "Joint Velocity = " << eval_velocity << std::endl;
504
505
4/8
✓ Branch 1 taken 24 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 24 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 24 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 24 times.
✗ Branch 12 not taken.
 192 casadi::DMVector res_vel = eval_velocity(casadi::DMVector{q_vec, v_vec});
506
3/6
✓ Branch 1 taken 24 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 24 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 24 times.
✗ Branch 8 not taken.
 48 std::cout << "v(q,v)=" << res_vel << std::endl;
507
508
11/22
✓ Branch 1 taken 24 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 24 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 24 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 24 times.
✗ Branch 15 not taken.
✓ Branch 18 taken 24 times.
✗ Branch 19 not taken.
✓ Branch 22 taken 24 times.
✗ Branch 23 not taken.
✓ Branch 26 taken 24 times.
✗ Branch 27 not taken.
✓ Branch 29 taken 24 times.
✗ Branch 30 not taken.
✓ Branch 32 taken 24 times.
✗ Branch 33 not taken.
✓ Branch 35 taken 24 times.
✗ Branch 36 not taken.
✗ Branch 40 not taken.
✓ Branch 41 taken 24 times.
 48 BOOST_CHECK(m.linear().isApprox(Eigen::Map<Motion::Vector3>(res_vel[0]->data())));
509
11/22
✓ Branch 1 taken 24 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 24 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 24 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 24 times.
✗ Branch 15 not taken.
✓ Branch 18 taken 24 times.
✗ Branch 19 not taken.
✓ Branch 22 taken 24 times.
✗ Branch 23 not taken.
✓ Branch 26 taken 24 times.
✗ Branch 27 not taken.
✓ Branch 29 taken 24 times.
✗ Branch 30 not taken.
✓ Branch 32 taken 24 times.
✗ Branch 33 not taken.
✓ Branch 35 taken 24 times.
✗ Branch 36 not taken.
✗ Branch 40 not taken.
✓ Branch 41 taken 24 times.
 48 BOOST_CHECK(m.angular().isApprox(Eigen::Map<Motion::Vector3>(res_vel[0]->data() + 3)));
510
511
1/2
✓ Branch 2 taken 24 times.
✗ Branch 3 not taken.
 48 casadi::SX dvel_dv = jacobian(cs_vel, cs_v);
512
7/14
✓ Branch 2 taken 24 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 24 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 24 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 24 times.
✗ Branch 13 not taken.
✓ Branch 16 taken 24 times.
✗ Branch 17 not taken.
✓ Branch 20 taken 24 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 24 times.
✗ Branch 24 not taken.
 336 casadi::Function eval_S("eval_S", casadi::SXVector{cs_q, cs_v}, casadi::SXVector{dvel_dv});
513
3/6
✓ Branch 1 taken 24 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 24 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 24 times.
✗ Branch 8 not taken.
 48 std::cout << "S = " << eval_S << std::endl;
514
515
4/8
✓ Branch 1 taken 24 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 24 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 24 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 24 times.
✗ Branch 12 not taken.
 192 casadi::DMVector res_S = eval_S(casadi::DMVector{q_vec, v_vec});
516
3/6
✓ Branch 1 taken 24 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 24 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 24 times.
✗ Branch 8 not taken.
 48 std::cout << "res_S:" << res_S << std::endl;
517
2/4
✓ Branch 1 taken 24 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 24 times.
✗ Branch 5 not taken.
 48 JointMotionSubspaceXd::DenseBase Sref_mat = Sref.matrix();
518
519
2/2
✓ Branch 0 taken 106 times.
✓ Branch 1 taken 24 times.
 260 for (Eigen::DenseIndex i = 0; i < 6; ++i)
520 {
521
3/4
✓ Branch 1 taken 145 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 39 times.
✓ Branch 4 taken 106 times.
 290 for (Eigen::DenseIndex j = 0; i < Sref.nv(); ++i)
522
9/18
✓ Branch 1 taken 39 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 39 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 39 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 39 times.
✗ Branch 15 not taken.
✓ Branch 18 taken 39 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 39 times.
✗ Branch 22 not taken.
✓ Branch 25 taken 39 times.
✗ Branch 26 not taken.
✓ Branch 28 taken 39 times.
✗ Branch 29 not taken.
✗ Branch 34 not taken.
✓ Branch 35 taken 39 times.
 78 BOOST_CHECK(
523 std::fabs(Sref_mat(i, j) - (double)res_S[0](i, j))
524 <= Eigen::NumTraits<double>::dummy_precision());
525 }
526
527
3/6
✓ Branch 1 taken 24 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 24 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 24 times.
✗ Branch 8 not taken.
 48 std::cout << "--" << std::endl << std::endl;
528 48 }
529 };
530
531
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_all_joints)
532 {
533 typedef pinocchio::JointCollectionDefault::JointModelVariant JointModelVariant;
534 2 boost::mpl::for_each<JointModelVariant::types>(TestADOnJoints());
535
536
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 TestADOnJoints()(pinocchio::JointModel());
537 2 }
538
539 BOOST_AUTO_TEST_SUITE_END()
540