GCC Code Coverage Report

Directory: ./
File: unittest/casadi/joints.cpp
Date: 2025-04-30 16:14:33
Exec Total Coverage
Lines: 193 193 100.0%
Branches: 508 984 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 30 static JointModel_ run()
153 {
154 30 JointModel_ jmodel;
155 30 jmodel.setIndexes(0, 0, 0);
156 30 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 Scalar, int Options, template<typename, int> class JointCollection>
268 struct init<pinocchio::JointModelMimicTpl<Scalar, Options, JointCollection>>
269 {
270 typedef pinocchio::JointModelMimicTpl<Scalar, Options, JointCollection> JointModel;
271
272 1 static JointModel run()
273 {
274 typedef pinocchio::JointModelRevoluteTpl<Scalar, Options, 0> JointModelRX;
275
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 JointModelRX jmodel_ref = init<JointModelRX>::run();
276
277
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 JointModel jmodel(jmodel_ref, 1., 0.);
278
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 jmodel.setIndexes(1, 0, 0, 0);
279 2 return jmodel;
280 }
281
282 static std::string name()
283 {
284 return JointModel::classname();
285 }
286 };
287
288 struct TestADOnJoints
289 {
290 template<typename JointModel_>
291 30 void operator()(const pinocchio::JointModelBase<JointModel_> &) const
292 {
293
1/2
✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
 30 JointModel_ jmodel = init<JointModel_>::run();
294
1/2
✓ Branch 1 taken 14 times.
✗ Branch 2 not taken.
 30 test(jmodel);
295 30 }
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 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 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 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 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 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 Scalar, int Options, template<typename, int> class JointCollection>
388 static void
389 1 test(const pinocchio::JointModelMimicTpl<Scalar, Options, JointCollection> & /*jmodel*/)
390 { /* do nothing */
391 1 }
392
393 template<typename JointModel>
394 48 static void test(const pinocchio::JointModelBase<JointModel> & jmodel)
395 {
396
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;
397
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;
398
399 typedef casadi::SX AD_double;
400
401 typedef pinocchio::SE3Tpl<AD_double> SE3AD;
402 typedef pinocchio::MotionTpl<AD_double> MotionAD;
403 typedef pinocchio::SE3Tpl<double> SE3;
404 typedef pinocchio::MotionTpl<double> Motion;
405 typedef pinocchio::JointMotionSubspaceTpl<Eigen::Dynamic, double> JointMotionSubspaceXd;
406
407 typedef Eigen::Matrix<AD_double, Eigen::Dynamic, 1> VectorXAD;
408 typedef Eigen::Matrix<AD_double, 6, 1> Vector6AD;
409
410 typedef typename pinocchio::CastType<AD_double, JointModel>::type JointModelAD;
411 typedef typename JointModelAD::JointDataDerived JointDataAD;
412
413 typedef typename JointModelAD::ConfigVector_t ConfigVectorAD;
414
415 typedef typename JointModel::JointDataDerived JointData;
416 typedef typename JointModel::ConfigVector_t ConfigVector;
417 typedef typename JointModel::TangentVector_t TangentVector;
418
419
1/2
✓ Branch 1 taken 24 times.
✗ Branch 2 not taken.
 48 JointData jdata(jmodel.createData());
420 48 pinocchio::JointDataBase<JointData> & jdata_base = jdata;
421
422
1/2
✓ Branch 1 taken 24 times.
✗ Branch 2 not taken.
 48 JointModelAD jmodel_ad = jmodel.template cast<AD_double>();
423
1/2
✓ Branch 1 taken 24 times.
✗ Branch 2 not taken.
 48 JointDataAD jdata_ad(jmodel_ad.createData());
424 48 pinocchio::JointDataBase<JointDataAD> & jdata_ad_base = jdata_ad;
425
426
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());
427
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 lb(ConfigVector::Constant(jmodel.nq(), -1.));
429
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.));
430
431 typedef pinocchio::RandomConfigurationStep<
432 pinocchio::LieGroupMap, ConfigVector, ConfigVector, ConfigVector>
433 RandomConfigAlgo;
434
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));
435
436
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());
437
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());
438
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)
439 {
440
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);
441 }
442
443 // Zero order
444
1/2
✓ Branch 1 taken 24 times.
✗ Branch 2 not taken.
 48 jmodel_ad.calc(jdata_ad, q_ad);
445
1/2
✓ Branch 1 taken 24 times.
✗ Branch 2 not taken.
 48 jmodel.calc(jdata, q);
446
447
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());
448
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());
449
450
1/2
✓ Branch 1 taken 24 times.
✗ Branch 2 not taken.
 48 casadi::SX cs_trans(3, 1);
451
2/2
✓ Branch 0 taken 72 times.
✓ Branch 1 taken 24 times.
 192 for (Eigen::DenseIndex k = 0; k < 3; ++k)
452 {
453
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];
454 }
455
1/2
✓ Branch 1 taken 24 times.
✗ Branch 2 not taken.
 48 casadi::SX cs_rot(3, 3);
456
2/2
✓ Branch 0 taken 72 times.
✓ Branch 1 taken 24 times.
 192 for (Eigen::DenseIndex i = 0; i < 3; ++i)
457 {
458
2/2
✓ Branch 0 taken 216 times.
✓ Branch 1 taken 72 times.
 576 for (Eigen::DenseIndex j = 0; j < 3; ++j)
459 {
460
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);
461 }
462 }
463
464
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(
465
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});
466
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;
467
468
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());
469
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;
470
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});
471
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;
472
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.translation().isApprox(Eigen::Map<SE3::Vector3>(res[0]->data())));
474
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())));
475
476 // First order
477
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());
478
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()));
479
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());
480
481
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());
482
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;
483
484
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)
485 {
486
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);
487 }
488
489
1/2
✓ Branch 1 taken 24 times.
✗ Branch 2 not taken.
 48 jmodel.calc(jdata, q, v);
490
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());
491
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());
492
493
1/2
✓ Branch 1 taken 24 times.
✗ Branch 2 not taken.
 48 jmodel_ad.calc(jdata_ad, q_ad, v_ad);
494
1/2
✓ Branch 1 taken 24 times.
✗ Branch 2 not taken.
 48 Vector6AD Y;
495
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());
496
497
1/2
✓ Branch 1 taken 24 times.
✗ Branch 2 not taken.
 48 casadi::SX cs_vel(6, 1);
498
2/2
✓ Branch 0 taken 144 times.
✓ Branch 1 taken 24 times.
 336 for (Eigen::DenseIndex k = 0; k < 6; ++k)
499 {
500
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];
501 }
502
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(
503
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});
504
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;
505
506
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});
507
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;
508
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.linear().isApprox(Eigen::Map<Motion::Vector3>(res_vel[0]->data())));
510
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)));
511
512
1/2
✓ Branch 2 taken 24 times.
✗ Branch 3 not taken.
 48 casadi::SX dvel_dv = jacobian(cs_vel, cs_v);
513
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});
514
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;
515
516
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});
517
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;
518
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();
519
520
2/2
✓ Branch 0 taken 106 times.
✓ Branch 1 taken 24 times.
 260 for (Eigen::DenseIndex i = 0; i < 6; ++i)
521 {
522
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)
523
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(
524 std::fabs(Sref_mat(i, j) - (double)res_S[0](i, j))
525 <= Eigen::NumTraits<double>::dummy_precision());
526 }
527
528
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;
529 48 }
530 };
531
532
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)
533 {
534 typedef pinocchio::JointCollectionDefault::JointModelVariant JointModelVariant;
535 2 boost::mpl::for_each<JointModelVariant::types>(TestADOnJoints());
536
537
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 TestADOnJoints()(pinocchio::JointModel());
538 2 }
539
540 BOOST_AUTO_TEST_SUITE_END()
541