GCC Code Coverage Report | |||||||||||||||||||||
|
|||||||||||||||||||||
Line | Branch | Exec | Source |
1 |
// |
||
2 |
// Copyright (c) 2019-2020 INRIA |
||
3 |
// |
||
4 |
|||
5 |
#include "pinocchio/algorithm/jacobian.hpp" |
||
6 |
#include "pinocchio/algorithm/rnea.hpp" |
||
7 |
#include "pinocchio/algorithm/rnea-derivatives.hpp" |
||
8 |
#include "pinocchio/algorithm/kinematics-derivatives.hpp" |
||
9 |
#include "pinocchio/algorithm/contact-dynamics.hpp" |
||
10 |
#include "pinocchio/algorithm/joint-configuration.hpp" |
||
11 |
#include "pinocchio/parsers/sample-models.hpp" |
||
12 |
|||
13 |
#include <iostream> |
||
14 |
|||
15 |
#include <boost/test/unit_test.hpp> |
||
16 |
#include <boost/utility/binary.hpp> |
||
17 |
|||
18 |
BOOST_AUTO_TEST_SUITE ( BOOST_TEST_MODULE ) |
||
19 |
|||
20 |
using namespace Eigen; |
||
21 |
using namespace pinocchio; |
||
22 |
|||
23 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE ( test_FD_with_contact_cst_gamma ) |
24 |
{ |
||
25 |
✓✗ | 4 |
pinocchio::Model model; |
26 |
✓✗ | 2 |
pinocchio::buildModels::humanoidRandom(model,true); |
27 |
✓✗✓✗ |
4 |
pinocchio::Data data(model), data_check(model); |
28 |
|||
29 |
✓✗✓✗ |
4 |
VectorXd q = VectorXd::Ones(model.nq); |
30 |
✓✗ | 2 |
normalize(model,q); |
31 |
|||
32 |
✓✗ | 2 |
computeJointJacobians(model, data, q); |
33 |
|||
34 |
✓✗✓✗ |
4 |
VectorXd v = VectorXd::Ones(model.nv); |
35 |
✓✗✓✗ |
4 |
VectorXd tau = VectorXd::Random(model.nv); |
36 |
|||
37 |
✓✗ | 4 |
const std::string RF = "rleg6_joint"; |
38 |
✓✗ | 2 |
const Model::JointIndex RF_id = model.getJointId(RF); |
39 |
// const std::string LF = "lleg6_joint"; |
||
40 |
// const Model::JointIndex LF_id = model.getJointId(LF); |
||
41 |
|||
42 |
✓✗✓✗ |
4 |
Data::Matrix6x J_RF(6,model.nv); J_RF.setZero(); |
43 |
✓✗ | 2 |
getJointJacobian(model, data, RF_id, LOCAL, J_RF); |
44 |
✓✗✓✗ |
2 |
Motion::Vector6 gamma_RF; gamma_RF.setZero(); |
45 |
✓✗✓✗ |
2 |
forwardKinematics(model,data,q,v,VectorXd::Zero(model.nv)); |
46 |
✓✗✓✗ |
2 |
gamma_RF += data.a[RF_id].toVector(); // Jdot * qdot |
47 |
|||
48 |
✓✗ | 2 |
forwardDynamics(model, data, q, v, tau, J_RF, gamma_RF); |
49 |
✓✗ | 4 |
VectorXd ddq_ref = data.ddq; |
50 |
✓✗ | 2 |
Force::Vector6 contact_force_ref = data.lambda_c; |
51 |
|||
52 |
✓✗✓✗ |
4 |
PINOCCHIO_ALIGNED_STD_VECTOR(Force) fext((size_t)model.njoints,Force::Zero()); |
53 |
✓✗✓✗ |
2 |
fext[RF_id] = ForceRef<Force::Vector6>(contact_force_ref); |
54 |
|||
55 |
// check call to RNEA |
||
56 |
✓✗ | 2 |
rnea(model,data_check,q,v,ddq_ref,fext); |
57 |
|||
58 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(data_check.tau.isApprox(tau)); |
59 |
✓✗✓✗ |
2 |
forwardKinematics(model,data_check,q,VectorXd::Zero(model.nv),ddq_ref); |
60 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(data_check.a[RF_id].toVector().isApprox(-gamma_RF)); |
61 |
|||
62 |
✓✗ | 4 |
Data data_fd(model); |
63 |
✓✗ | 4 |
VectorXd q_plus(model.nq); |
64 |
✓✗✓✗ |
4 |
VectorXd v_eps(model.nv); v_eps.setZero(); |
65 |
✓✗ | 4 |
VectorXd v_plus(v); |
66 |
✓✗ | 4 |
VectorXd tau_plus(tau); |
67 |
2 |
const double eps = 1e-8; |
|
68 |
|||
69 |
// check: dddq_dtau and dlambda_dtau |
||
70 |
✓✗ | 4 |
MatrixXd dddq_dtau(model.nv,model.nv); |
71 |
✓✗ | 4 |
Data::Matrix6x dlambda_dtau(6,model.nv); |
72 |
|||
73 |
✓✓ | 66 |
for(int k = 0; k < model.nv; ++k) |
74 |
{ |
||
75 |
✓✗ | 64 |
tau_plus[k] += eps; |
76 |
✓✗ | 64 |
forwardDynamics(model, data_fd, q, v, tau_plus, J_RF, gamma_RF); |
77 |
|||
78 |
64 |
const Data::TangentVectorType & ddq_plus = data_fd.ddq; |
|
79 |
✓✗ | 64 |
Force::Vector6 contact_force_plus = data_fd.lambda_c; |
80 |
|||
81 |
✓✗✓✗ ✓✗✓✗ |
64 |
dddq_dtau.col(k) = (ddq_plus - ddq_ref)/eps; |
82 |
✓✗✓✗ ✓✗✓✗ |
64 |
dlambda_dtau.col(k) = (contact_force_plus - contact_force_ref)/eps; |
83 |
|||
84 |
✓✗ | 64 |
tau_plus[k] -= eps; |
85 |
} |
||
86 |
|||
87 |
✓✗ | 4 |
MatrixXd A(model.nv+6,model.nv+6); |
88 |
✓✗✓✗ ✓✗✓✗ |
2 |
data.M.transpose().triangularView<Eigen::Upper>() = data.M.triangularView<Eigen::Upper>(); |
89 |
✓✗✓✗ |
2 |
A.topLeftCorner(model.nv,model.nv) = data.M; |
90 |
✓✗✓✗ |
2 |
A.bottomLeftCorner(6, model.nv) = J_RF; |
91 |
✓✗✓✗ ✓✗ |
2 |
A.topRightCorner(model.nv, 6) = J_RF.transpose(); |
92 |
✓✗✓✗ |
2 |
A.bottomRightCorner(6,6).setZero(); |
93 |
|||
94 |
✓✗✓✗ |
4 |
MatrixXd Ainv = A.inverse(); |
95 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(Ainv.topRows(model.nv).leftCols(model.nv).isApprox(dddq_dtau,std::sqrt(eps))); |
96 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK(Ainv.bottomRows(6).leftCols(model.nv).isApprox(-dlambda_dtau,std::sqrt(eps))); |
97 |
|||
98 |
// check: dddq_dv and dlambda_dv |
||
99 |
✓✗ | 4 |
MatrixXd dddq_dv(model.nv,model.nv); |
100 |
✓✗ | 4 |
Data::Matrix6x dlambda_dv(6,model.nv); |
101 |
|||
102 |
✓✓ | 66 |
for(int k = 0; k < model.nv; ++k) |
103 |
{ |
||
104 |
✓✗ | 64 |
v_plus[k] += eps; |
105 |
✓✗ | 64 |
forwardDynamics(model, data_fd, q, v_plus, tau, J_RF, gamma_RF); |
106 |
|||
107 |
64 |
const Data::TangentVectorType & ddq_plus = data_fd.ddq; |
|
108 |
✓✗ | 64 |
Force::Vector6 contact_force_plus = data_fd.lambda_c; |
109 |
|||
110 |
✓✗✓✗ ✓✗✓✗ |
64 |
dddq_dv.col(k) = (ddq_plus - ddq_ref)/eps; |
111 |
✓✗✓✗ ✓✗✓✗ |
64 |
dlambda_dv.col(k) = (contact_force_plus - contact_force_ref)/eps; |
112 |
|||
113 |
✓✗ | 64 |
v_plus[k] -= eps; |
114 |
} |
||
115 |
|||
116 |
✓✗✓✗ |
2 |
computeRNEADerivatives(model,data_check,q,v,VectorXd::Zero(model.nv)); |
117 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
MatrixXd dddq_dv_anal = -Ainv.topRows(model.nv).leftCols(model.nv) * data_check.dtau_dv; |
118 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
MatrixXd dlambda_dv_anal = -Ainv.bottomRows(6).leftCols(model.nv) * data_check.dtau_dv; |
119 |
|||
120 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(dddq_dv_anal.isApprox(dddq_dv,std::sqrt(eps))); |
121 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK(dlambda_dv_anal.isApprox(-dlambda_dv,std::sqrt(eps))); |
122 |
|||
123 |
✓✗ | 4 |
MatrixXd dddq_dq(model.nv,model.nv); |
124 |
✓✗ | 4 |
Data::Matrix6x dlambda_dq(6,model.nv); |
125 |
|||
126 |
✓✓ | 66 |
for(int k = 0; k < model.nv; ++k) |
127 |
{ |
||
128 |
✓✗ | 64 |
v_eps[k] = eps; |
129 |
✓✗ | 64 |
q_plus = integrate(model,q,v_eps); |
130 |
✓✗ | 64 |
computeJointJacobians(model, data_fd, q_plus); |
131 |
✓✗ | 64 |
getJointJacobian(model, data_fd, RF_id, LOCAL, J_RF); |
132 |
✓✗ | 64 |
forwardDynamics(model, data_fd, q_plus, v, tau, J_RF, gamma_RF); |
133 |
|||
134 |
64 |
const Data::TangentVectorType & ddq_plus = data_fd.ddq; |
|
135 |
✓✗ | 64 |
Force::Vector6 contact_force_plus = data_fd.lambda_c; |
136 |
|||
137 |
✓✗✓✗ ✓✗✓✗ |
64 |
dddq_dq.col(k) = (ddq_plus - ddq_ref)/eps; |
138 |
✓✗✓✗ ✓✗✓✗ |
64 |
dlambda_dq.col(k) = (contact_force_plus - contact_force_ref)/eps; |
139 |
|||
140 |
✓✗ | 64 |
v_eps[k] = 0.; |
141 |
} |
||
142 |
|||
143 |
✓✗ | 2 |
computeRNEADerivatives(model,data_check,q,v,ddq_ref,fext); |
144 |
✓✗✓✗ ✓✗✓✗ |
4 |
Data::Matrix6x v_partial_dq(6,model.nv), a_partial_dq(6,model.nv), a_partial_dv(6,model.nv), a_partial_da(6,model.nv); |
145 |
✓✗✓✗ ✓✗✓✗ |
2 |
v_partial_dq.setZero(); a_partial_dq.setZero(); a_partial_dv.setZero(); a_partial_da.setZero(); |
146 |
✓✗ | 4 |
Data data_kin(model); |
147 |
✓✗✓✗ |
2 |
computeForwardKinematicsDerivatives(model,data_kin,q,VectorXd::Zero(model.nv),ddq_ref); |
148 |
✓✗ | 2 |
getJointAccelerationDerivatives(model,data_kin,RF_id,LOCAL,v_partial_dq,a_partial_dq,a_partial_dv,a_partial_da); |
149 |
|||
150 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
MatrixXd dddq_dq_anal = -Ainv.topRows(model.nv).leftCols(model.nv) * data_check.dtau_dq; |
151 |
✓✗✓✗ ✓✗✓✗ |
2 |
dddq_dq_anal -= Ainv.topRows(model.nv).rightCols(6) * a_partial_dq; |
152 |
|||
153 |
✓✗✓✗ ✓✗✓✗ |
4 |
MatrixXd dlambda_dq_anal = Ainv.bottomRows(6).leftCols(model.nv) * data_check.dtau_dq; |
154 |
✓✗✓✗ ✓✗✓✗ |
2 |
dlambda_dq_anal += Ainv.bottomRows(6).rightCols(6) * a_partial_dq; |
155 |
|||
156 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(dddq_dq_anal.isApprox(dddq_dq,std::sqrt(eps))); |
157 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(dlambda_dq_anal.isApprox(dlambda_dq,std::sqrt(eps))); |
158 |
|||
159 |
2 |
} |
|
160 |
|||
161 |
template<typename ConfigVectorType, typename TangentVectorType1, typename TangentVectorType2> |
||
162 |
97 |
VectorXd contactDynamics(const Model & model, Data & data, |
|
163 |
const Eigen::MatrixBase<ConfigVectorType> & q, |
||
164 |
const Eigen::MatrixBase<TangentVectorType1> & v, |
||
165 |
const Eigen::MatrixBase<TangentVectorType2> & tau, |
||
166 |
const Model::JointIndex id) |
||
167 |
{ |
||
168 |
✓✗ | 97 |
computeJointJacobians(model, data, q); |
169 |
✓✗✓✗ |
194 |
Data::Matrix6x J(6,model.nv); J.setZero(); |
170 |
|||
171 |
✓✗ | 97 |
getJointJacobian(model, data, id, LOCAL, J); |
172 |
✓✗ | 97 |
Motion::Vector6 gamma; |
173 |
✓✗✓✗ |
97 |
forwardKinematics(model, data, q, v, VectorXd::Zero(model.nv)); |
174 |
✓✗✓✗ |
97 |
gamma = data.a[id].toVector(); |
175 |
|||
176 |
✓✗ | 97 |
forwardDynamics(model, data, q, v, tau, J, gamma); |
177 |
✓✗✓✗ |
97 |
VectorXd res(VectorXd::Zero(model.nv+6)); |
178 |
|||
179 |
✓✗✓✗ |
97 |
res.head(model.nv) = data.ddq; |
180 |
✓✗✓✗ |
97 |
res.tail(6) = data.lambda_c; |
181 |
|||
182 |
194 |
return res; |
|
183 |
} |
||
184 |
|||
185 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE ( test_FD_with_contact_varying_gamma ) |
186 |
{ |
||
187 |
✓✗ | 4 |
pinocchio::Model model; |
188 |
✓✗ | 2 |
pinocchio::buildModels::humanoidRandom(model,true); |
189 |
✓✗✓✗ |
4 |
pinocchio::Data data(model), data_check(model); |
190 |
|||
191 |
✓✗✓✗ |
4 |
VectorXd q = VectorXd::Ones(model.nq); |
192 |
✓✗ | 2 |
normalize(model,q); |
193 |
|||
194 |
✓✗✓✗ |
4 |
VectorXd v = VectorXd::Random(model.nv); |
195 |
✓✗✓✗ |
4 |
VectorXd tau = VectorXd::Random(model.nv); |
196 |
|||
197 |
✓✗ | 4 |
const std::string RF = "rleg6_joint"; |
198 |
✓✗ | 2 |
const Model::JointIndex RF_id = model.getJointId(RF); |
199 |
|||
200 |
✓✗✓✗ |
4 |
Data::Matrix6x J_RF(6,model.nv); J_RF.setZero(); |
201 |
✓✗ | 2 |
computeJointJacobians(model, data, q); |
202 |
✓✗ | 2 |
getJointJacobian(model, data, RF_id, LOCAL, J_RF); |
203 |
✓✗✓✗ |
2 |
Motion::Vector6 gamma_RF; gamma_RF.setZero(); |
204 |
|||
205 |
✓✗ | 4 |
VectorXd x_ref = contactDynamics(model,data,q,v,tau,RF_id); |
206 |
✓✗✓✗ |
4 |
VectorXd ddq_ref = x_ref.head(model.nv); |
207 |
✓✗✓✗ |
2 |
Force::Vector6 contact_force_ref = x_ref.tail(6); |
208 |
|||
209 |
✓✗✓✗ |
4 |
PINOCCHIO_ALIGNED_STD_VECTOR(Force) fext((size_t)model.njoints,Force::Zero()); |
210 |
✓✗✓✗ |
2 |
fext[RF_id] = ForceRef<Force::Vector6>(contact_force_ref); |
211 |
|||
212 |
// check call to RNEA |
||
213 |
✓✗ | 2 |
rnea(model,data_check,q,v,ddq_ref,fext); |
214 |
|||
215 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(data_check.tau.isApprox(tau)); |
216 |
✓✗ | 2 |
forwardKinematics(model,data_check,q,v,ddq_ref); |
217 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK(data_check.a[RF_id].toVector().isZero()); |
218 |
|||
219 |
✓✗ | 4 |
Data data_fd(model); |
220 |
✓✗ | 4 |
VectorXd q_plus(model.nq); |
221 |
✓✗✓✗ |
4 |
VectorXd v_eps(model.nv); v_eps.setZero(); |
222 |
✓✗ | 4 |
VectorXd v_plus(v); |
223 |
✓✗ | 4 |
VectorXd tau_plus(tau); |
224 |
✓✗ | 4 |
VectorXd x_plus(model.nv + 6); |
225 |
2 |
const double eps = 1e-8; |
|
226 |
|||
227 |
// check: dddq_dtau and dlambda_dtau |
||
228 |
✓✗ | 4 |
MatrixXd dddq_dtau(model.nv,model.nv); |
229 |
✓✗ | 4 |
Data::Matrix6x dlambda_dtau(6,model.nv); |
230 |
|||
231 |
✓✓ | 66 |
for(int k = 0; k < model.nv; ++k) |
232 |
{ |
||
233 |
✓✗ | 64 |
tau_plus[k] += eps; |
234 |
✓✗ | 64 |
x_plus = contactDynamics(model,data,q,v,tau_plus,RF_id); |
235 |
|||
236 |
✓✗✓✗ |
64 |
const Data::TangentVectorType ddq_plus = x_plus.head(model.nv); |
237 |
✓✗✓✗ |
64 |
Force::Vector6 contact_force_plus = x_plus.tail(6); |
238 |
|||
239 |
✓✗✓✗ ✓✗✓✗ |
64 |
dddq_dtau.col(k) = (ddq_plus - ddq_ref)/eps; |
240 |
✓✗✓✗ ✓✗✓✗ |
64 |
dlambda_dtau.col(k) = (contact_force_plus - contact_force_ref)/eps; |
241 |
|||
242 |
✓✗ | 64 |
tau_plus[k] -= eps; |
243 |
} |
||
244 |
|||
245 |
✓✗ | 4 |
MatrixXd A(model.nv+6,model.nv+6); |
246 |
✓✗✓✗ ✓✗✓✗ |
2 |
data.M.transpose().triangularView<Eigen::Upper>() = data.M.triangularView<Eigen::Upper>(); |
247 |
✓✗✓✗ |
2 |
A.topLeftCorner(model.nv,model.nv) = data.M; |
248 |
✓✗✓✗ |
2 |
A.bottomLeftCorner(6, model.nv) = J_RF; |
249 |
✓✗✓✗ ✓✗ |
2 |
A.topRightCorner(model.nv, 6) = J_RF.transpose(); |
250 |
✓✗✓✗ |
2 |
A.bottomRightCorner(6,6).setZero(); |
251 |
|||
252 |
✓✗✓✗ |
4 |
MatrixXd Ainv = A.inverse(); |
253 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(Ainv.topRows(model.nv).leftCols(model.nv).isApprox(dddq_dtau,std::sqrt(eps))); |
254 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK(Ainv.bottomRows(6).leftCols(model.nv).isApprox(-dlambda_dtau,std::sqrt(eps))); |
255 |
|||
256 |
// check: dddq_dv and dlambda_dv |
||
257 |
✓✗ | 4 |
MatrixXd dddq_dv(model.nv,model.nv); |
258 |
✓✗ | 4 |
Data::Matrix6x dlambda_dv(6,model.nv); |
259 |
|||
260 |
✓✓ | 66 |
for(int k = 0; k < model.nv; ++k) |
261 |
{ |
||
262 |
✓✗ | 64 |
v_plus[k] += eps; |
263 |
✓✗ | 64 |
x_plus = contactDynamics(model,data,q,v_plus,tau,RF_id); |
264 |
|||
265 |
✓✗✓✗ |
64 |
const Data::TangentVectorType ddq_plus = x_plus.head(model.nv); |
266 |
✓✗✓✗ |
64 |
Force::Vector6 contact_force_plus = x_plus.tail(6); |
267 |
|||
268 |
✓✗✓✗ ✓✗✓✗ |
64 |
dddq_dv.col(k) = (ddq_plus - ddq_ref)/eps; |
269 |
✓✗✓✗ ✓✗✓✗ |
64 |
dlambda_dv.col(k) = (contact_force_plus - contact_force_ref)/eps; |
270 |
|||
271 |
✓✗ | 64 |
v_plus[k] -= eps; |
272 |
} |
||
273 |
|||
274 |
|||
275 |
✓✗✓✗ |
2 |
computeRNEADerivatives(model,data_check,q,v,VectorXd::Zero(model.nv)); |
276 |
✓✗✓✗ ✓✗✓✗ |
4 |
Data::Matrix6x v_partial_dq(6,model.nv), a_partial_dq(6,model.nv), a_partial_dv(6,model.nv), a_partial_da(6,model.nv); |
277 |
✓✗✓✗ ✓✗✓✗ |
2 |
v_partial_dq.setZero(); a_partial_dq.setZero(); a_partial_dv.setZero(); a_partial_da.setZero(); |
278 |
✓✗ | 4 |
Data data_kin(model); |
279 |
✓✗✓✗ |
2 |
computeForwardKinematicsDerivatives(model,data_kin,q,v,VectorXd::Zero(model.nv)); |
280 |
✓✗ | 2 |
getJointAccelerationDerivatives(model,data_kin,RF_id,LOCAL,v_partial_dq,a_partial_dq,a_partial_dv,a_partial_da); |
281 |
|||
282 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
MatrixXd dddq_dv_anal = -Ainv.topRows(model.nv).leftCols(model.nv) * data_check.dtau_dv; |
283 |
✓✗✓✗ ✓✗✓✗ |
2 |
dddq_dv_anal -= Ainv.topRows(model.nv).rightCols(6) * a_partial_dv; |
284 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
MatrixXd dlambda_dv_anal = -Ainv.bottomRows(6).leftCols(model.nv) * data_check.dtau_dv; |
285 |
✓✗✓✗ ✓✗✓✗ |
2 |
dlambda_dv_anal -= Ainv.bottomRows(6).rightCols(6) * a_partial_dv; |
286 |
|||
287 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(dddq_dv_anal.isApprox(dddq_dv,std::sqrt(eps))); |
288 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK(dlambda_dv_anal.isApprox(-dlambda_dv,std::sqrt(eps))); |
289 |
|||
290 |
|||
291 |
✓✗ | 4 |
MatrixXd dddq_dq(model.nv,model.nv); |
292 |
✓✗ | 4 |
Data::Matrix6x dlambda_dq(6,model.nv); |
293 |
|||
294 |
✓✓ | 66 |
for(int k = 0; k < model.nv; ++k) |
295 |
{ |
||
296 |
✓✗ | 64 |
v_eps[k] = eps; |
297 |
✓✗ | 64 |
q_plus = integrate(model,q,v_eps); |
298 |
|||
299 |
✓✗ | 64 |
x_plus = contactDynamics(model,data,q_plus,v,tau,RF_id); |
300 |
|||
301 |
✓✗✓✗ |
64 |
const Data::TangentVectorType ddq_plus = x_plus.head(model.nv); |
302 |
✓✗✓✗ |
64 |
Force::Vector6 contact_force_plus = x_plus.tail(6); |
303 |
|||
304 |
✓✗✓✗ ✓✗✓✗ |
64 |
dddq_dq.col(k) = (ddq_plus - ddq_ref)/eps; |
305 |
✓✗✓✗ ✓✗✓✗ |
64 |
dlambda_dq.col(k) = (contact_force_plus - contact_force_ref)/eps; |
306 |
|||
307 |
✓✗ | 64 |
v_eps[k] = 0.; |
308 |
} |
||
309 |
|||
310 |
✓✗ | 2 |
computeRNEADerivatives(model,data_check,q,v,ddq_ref,fext); |
311 |
✓✗✓✗ ✓✗✓✗ |
2 |
v_partial_dq.setZero(); a_partial_dq.setZero(); a_partial_dv.setZero(); a_partial_da.setZero(); |
312 |
✓✗ | 2 |
computeForwardKinematicsDerivatives(model,data_kin,q,v,ddq_ref); |
313 |
✓✗ | 2 |
getJointAccelerationDerivatives(model,data_kin,RF_id,LOCAL,v_partial_dq,a_partial_dq,a_partial_dv,a_partial_da); |
314 |
|||
315 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
MatrixXd dddq_dq_anal = -Ainv.topRows(model.nv).leftCols(model.nv) * data_check.dtau_dq; |
316 |
✓✗✓✗ ✓✗✓✗ |
2 |
dddq_dq_anal -= Ainv.topRows(model.nv).rightCols(6) * a_partial_dq; |
317 |
|||
318 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(dddq_dq_anal.isApprox(dddq_dq,std::sqrt(eps))); |
319 |
|||
320 |
✓✗✓✗ ✓✗✓✗ |
4 |
MatrixXd dlambda_dq_anal = Ainv.bottomRows(6).leftCols(model.nv) * data_check.dtau_dq; |
321 |
✓✗✓✗ ✓✗✓✗ |
2 |
dlambda_dq_anal += Ainv.bottomRows(6).rightCols(6) * a_partial_dq; |
322 |
|||
323 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(dlambda_dq_anal.isApprox(dlambda_dq,std::sqrt(eps))); |
324 |
|||
325 |
2 |
} |
|
326 |
|||
327 |
BOOST_AUTO_TEST_SUITE_END () |
||
328 |
Generated by: GCOVR (Version 4.2) |