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