GCC Code Coverage Report | |||||||||||||||||||||
|
|||||||||||||||||||||
Line | Branch | Exec | Source |
1 |
// |
||
2 |
// Copyright (c) 2016-2019 CNRS, INRIA |
||
3 |
// |
||
4 |
|||
5 |
#include "pinocchio/multibody/model.hpp" |
||
6 |
#include "pinocchio/multibody/data.hpp" |
||
7 |
#include "pinocchio/parsers/sample-models.hpp" |
||
8 |
#include "pinocchio/algorithm/joint-configuration.hpp" |
||
9 |
#include "pinocchio/algorithm/kinematics.hpp" |
||
10 |
#include "pinocchio/algorithm/jacobian.hpp" |
||
11 |
|||
12 |
#include <boost/test/unit_test.hpp> |
||
13 |
#include <boost/utility/binary.hpp> |
||
14 |
|||
15 |
using namespace pinocchio; |
||
16 |
using namespace Eigen; |
||
17 |
|||
18 |
template<bool local> |
||
19 |
4 |
Data::Matrix6x finiteDiffJacobian(const Model & model, Data & data, const Eigen::VectorXd & q, const Model::JointIndex joint_id) |
|
20 |
{ |
||
21 |
✓✗✓✗ |
4 |
Data::Matrix6x res(6,model.nv); res.setZero(); |
22 |
✓✗ | 8 |
VectorXd q_integrate (model.nq); |
23 |
✓✗✓✗ |
8 |
VectorXd v_integrate (model.nv); v_integrate.setZero(); |
24 |
|||
25 |
✓✗ | 4 |
forwardKinematics(model,data,q); |
26 |
✓✗ | 4 |
const SE3 oMi_ref = data.oMi[joint_id]; |
27 |
|||
28 |
4 |
double eps = 1e-8; |
|
29 |
✓✓ | 132 |
for(int k=0; k<model.nv; ++k) |
30 |
{ |
||
31 |
// Integrate along kth direction |
||
32 |
✓✗ | 128 |
v_integrate[k] = eps; |
33 |
✓✗ | 128 |
q_integrate = integrate(model,q,v_integrate); |
34 |
|||
35 |
✓✗ | 128 |
forwardKinematics(model,data,q_integrate); |
36 |
128 |
const SE3 & oMi = data.oMi[joint_id]; |
|
37 |
|||
38 |
if (local) |
||
39 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
64 |
res.col(k) = log6(oMi_ref.inverse()*oMi).toVector(); |
40 |
else |
||
41 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
64 |
res.col(k) = oMi_ref.act(log6(oMi_ref.inverse()*oMi)).toVector(); |
42 |
|||
43 |
✓✗✓✗ |
128 |
res.col(k) /= eps; |
44 |
|||
45 |
✓✗ | 128 |
v_integrate[k] = 0.; |
46 |
} |
||
47 |
|||
48 |
8 |
return res; |
|
49 |
} |
||
50 |
|||
51 |
template<typename Matrix> |
||
52 |
void filterValue(MatrixBase<Matrix> & mat, typename Matrix::Scalar value) |
||
53 |
{ |
||
54 |
for(int k = 0; k < mat.size(); ++k) |
||
55 |
mat.derived().data()[k] = math::fabs(mat.derived().data()[k]) <= value?0:mat.derived().data()[k]; |
||
56 |
} |
||
57 |
|||
58 |
template<typename JointModel_> struct init; |
||
59 |
|||
60 |
template<typename JointModel_> |
||
61 |
struct init |
||
62 |
{ |
||
63 |
34 |
static JointModel_ run() |
|
64 |
{ |
||
65 |
34 |
JointModel_ jmodel; |
|
66 |
34 |
jmodel.setIndexes(0,0,0); |
|
67 |
34 |
return jmodel; |
|
68 |
} |
||
69 |
}; |
||
70 |
|||
71 |
template<typename Scalar, int Options> |
||
72 |
struct init<pinocchio::JointModelRevoluteUnalignedTpl<Scalar,Options> > |
||
73 |
{ |
||
74 |
typedef pinocchio::JointModelRevoluteUnalignedTpl<Scalar,Options> JointModel; |
||
75 |
|||
76 |
1 |
static JointModel run() |
|
77 |
{ |
||
78 |
typedef typename JointModel::Vector3 Vector3; |
||
79 |
✓✗✓✗ |
1 |
JointModel jmodel(Vector3::Random().normalized()); |
80 |
|||
81 |
1 |
jmodel.setIndexes(0,0,0); |
|
82 |
1 |
return jmodel; |
|
83 |
} |
||
84 |
}; |
||
85 |
|||
86 |
template<typename Scalar, int Options> |
||
87 |
struct init<pinocchio::JointModelRevoluteUnboundedUnalignedTpl<Scalar,Options> > |
||
88 |
{ |
||
89 |
typedef pinocchio::JointModelRevoluteUnboundedUnalignedTpl<Scalar,Options> JointModel; |
||
90 |
|||
91 |
1 |
static JointModel run() |
|
92 |
{ |
||
93 |
typedef typename JointModel::Vector3 Vector3; |
||
94 |
✓✗✓✗ |
1 |
JointModel jmodel(Vector3::Random().normalized()); |
95 |
|||
96 |
1 |
jmodel.setIndexes(0,0,0); |
|
97 |
1 |
return jmodel; |
|
98 |
} |
||
99 |
}; |
||
100 |
|||
101 |
template<typename Scalar, int Options> |
||
102 |
struct init<pinocchio::JointModelPrismaticUnalignedTpl<Scalar,Options> > |
||
103 |
{ |
||
104 |
typedef pinocchio::JointModelPrismaticUnalignedTpl<Scalar,Options> JointModel; |
||
105 |
|||
106 |
1 |
static JointModel run() |
|
107 |
{ |
||
108 |
typedef typename JointModel::Vector3 Vector3; |
||
109 |
✓✗✓✗ |
1 |
JointModel jmodel(Vector3::Random().normalized()); |
110 |
|||
111 |
1 |
jmodel.setIndexes(0,0,0); |
|
112 |
1 |
return jmodel; |
|
113 |
} |
||
114 |
}; |
||
115 |
|||
116 |
template<typename Scalar, int Options, template<typename,int> class JointCollection> |
||
117 |
struct init<pinocchio::JointModelTpl<Scalar,Options,JointCollection> > |
||
118 |
{ |
||
119 |
typedef pinocchio::JointModelTpl<Scalar,Options,JointCollection> JointModel; |
||
120 |
|||
121 |
static JointModel run() |
||
122 |
{ |
||
123 |
typedef pinocchio::JointModelRevoluteTpl<Scalar,Options,0> JointModelRX; |
||
124 |
JointModel jmodel((JointModelRX())); |
||
125 |
|||
126 |
jmodel.setIndexes(0,0,0); |
||
127 |
return jmodel; |
||
128 |
} |
||
129 |
}; |
||
130 |
|||
131 |
template<typename Scalar, int Options, template<typename,int> class JointCollection> |
||
132 |
struct init<pinocchio::JointModelCompositeTpl<Scalar,Options,JointCollection> > |
||
133 |
{ |
||
134 |
typedef pinocchio::JointModelCompositeTpl<Scalar,Options,JointCollection> JointModel; |
||
135 |
|||
136 |
static JointModel run() |
||
137 |
{ |
||
138 |
typedef pinocchio::JointModelRevoluteTpl<Scalar,Options,0> JointModelRX; |
||
139 |
typedef pinocchio::JointModelRevoluteTpl<Scalar,Options,1> JointModelRY; |
||
140 |
JointModel jmodel((JointModelRX())); |
||
141 |
jmodel.addJoint(JointModelRY()); |
||
142 |
|||
143 |
jmodel.setIndexes(0,0,0); |
||
144 |
return jmodel; |
||
145 |
} |
||
146 |
}; |
||
147 |
|||
148 |
template<typename JointModel_> |
||
149 |
struct init<pinocchio::JointModelMimic<JointModel_> > |
||
150 |
{ |
||
151 |
typedef pinocchio::JointModelMimic<JointModel_> JointModel; |
||
152 |
|||
153 |
6 |
static JointModel run() |
|
154 |
{ |
||
155 |
✓✗ | 6 |
JointModel_ jmodel_ref = init<JointModel_>::run(); |
156 |
|||
157 |
✓✗ | 6 |
JointModel jmodel(jmodel_ref,1.,0.); |
158 |
✓✗ | 6 |
jmodel.setIndexes(0,0,0); |
159 |
|||
160 |
12 |
return jmodel; |
|
161 |
} |
||
162 |
}; |
||
163 |
|||
164 |
struct FiniteDiffJoint |
||
165 |
{ |
||
166 |
1 |
void operator()(JointModelComposite & /*jmodel*/) const |
|
167 |
1 |
{} |
|
168 |
|||
169 |
template<typename JointModel> |
||
170 |
40 |
void operator()(JointModelBase<JointModel> & /*jmodel*/) const |
|
171 |
{ |
||
172 |
typedef typename JointModel::ConfigVector_t CV; |
||
173 |
typedef typename JointModel::TangentVector_t TV; |
||
174 |
typedef typename LieGroup<JointModel>::type LieGroupType; |
||
175 |
|||
176 |
✓✗ | 40 |
JointModel jmodel = init<JointModel>::run(); |
177 |
✓✗✓✗ ✓✗✓✗ |
40 |
std::cout << "name: " << jmodel.classname() << std::endl; |
178 |
|||
179 |
✓✗ | 40 |
typename JointModel::JointDataDerived jdata_ = jmodel.createData(); |
180 |
typedef JointDataBase<typename JointModel::JointDataDerived> DataBaseType; |
||
181 |
40 |
DataBaseType & jdata = static_cast<DataBaseType &>(jdata_); |
|
182 |
|||
183 |
✓✗✓✗ |
40 |
CV q = LieGroupType().random(); |
184 |
✓✗ | 40 |
jmodel.calc(jdata.derived(),q); |
185 |
✓✗✓✗ |
40 |
SE3 M_ref(jdata.M()); |
186 |
|||
187 |
✓✗ | 40 |
CV q_int(q); |
188 |
✓✗✓✗ |
40 |
const Eigen::DenseIndex nv = jdata.S().nv(); |
189 |
✓✗✓✗ |
40 |
TV v(nv); v.setZero(); |
190 |
40 |
double eps = 1e-8; |
|
191 |
|||
192 |
✓✗✓✗ ✓✗✓✗ |
40 |
Eigen::Matrix<double,6,JointModel::NV> S(6,nv), S_ref(jdata.S().matrix()); |
193 |
|||
194 |
✓✓ | 106 |
for(int k=0;k<nv;++k) |
195 |
{ |
||
196 |
✓✗ | 66 |
v[k] = eps; |
197 |
✓✗✓✗ |
66 |
q_int = LieGroupType().integrate(q,v); |
198 |
✓✗ | 66 |
jmodel.calc(jdata.derived(),q_int); |
199 |
✓✗✓✗ |
66 |
SE3 M_int = jdata.M(); |
200 |
|||
201 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
66 |
S.col(k) = log6(M_ref.inverse()*M_int).toVector(); |
202 |
✓✗✓✗ |
66 |
S.col(k) /= eps; |
203 |
|||
204 |
✓✗ | 66 |
v[k] = 0.; |
205 |
} |
||
206 |
|||
207 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
40 |
BOOST_CHECK(S.isApprox(S_ref,eps*1e1)); |
208 |
✓✗✓✗ ✓✗ |
40 |
std::cout << "S_ref:\n" << S_ref << std::endl; |
209 |
✓✗✓✗ ✓✗ |
40 |
std::cout << "S:\n" << S << std::endl; |
210 |
40 |
} |
|
211 |
}; |
||
212 |
|||
213 |
BOOST_AUTO_TEST_SUITE ( BOOST_TEST_MODULE ) |
||
214 |
|||
215 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE (test_S_finit_diff) |
216 |
{ |
||
217 |
✓✗ | 2 |
boost::mpl::for_each<JointModelVariant::types>(FiniteDiffJoint()); |
218 |
2 |
} |
|
219 |
|||
220 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE (test_jacobian_vs_finit_diff) |
221 |
{ |
||
222 |
✓✗ | 4 |
pinocchio::Model model; |
223 |
✓✗ | 2 |
pinocchio::buildModels::humanoidRandom(model); |
224 |
✓✗ | 4 |
pinocchio::Data data(model); |
225 |
|||
226 |
✓✗✓✗ |
4 |
VectorXd q = VectorXd::Ones(model.nq); |
227 |
✓✗✓✗ |
2 |
q.segment<4>(3).normalize(); |
228 |
✓✗ | 2 |
computeJointJacobians(model,data,q); |
229 |
|||
230 |
✓✗✓✗ ✗✓✗✗ ✗✗✗✓ ✗✓✗✗ ✗✗ |
2 |
Model::Index idx = model.existJointName("rarm2")?model.getJointId("rarm2"):(Model::Index)(model.njoints-1); |
231 |
✓✗✓✗ |
4 |
Data::Matrix6x Jrh(6,model.nv); Jrh.fill(0); |
232 |
|||
233 |
✓✗ | 2 |
getJointJacobian(model,data,idx,WORLD,Jrh); |
234 |
✓✗ | 4 |
Data::Matrix6x Jrh_finite_diff = finiteDiffJacobian<false>(model,data,q,idx); |
235 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(Jrh_finite_diff.isApprox(Jrh,1e-8*1e1)); |
236 |
|||
237 |
✓✗ | 2 |
getJointJacobian(model,data,idx,LOCAL,Jrh); |
238 |
✓✗ | 2 |
Jrh_finite_diff = finiteDiffJacobian<true>(model,data,q,idx); |
239 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(Jrh_finite_diff.isApprox(Jrh,1e-8*1e1)); |
240 |
2 |
} |
|
241 |
|||
242 |
BOOST_AUTO_TEST_SUITE_END() |
Generated by: GCOVR (Version 4.2) |