| GCC Code Coverage Report | |||||||||||||||||||||
|
|||||||||||||||||||||
| Line | Branch | Exec | Source |
1 |
// |
||
2 |
// Copyright (c) 2017-2020 CNRS INRIA |
||
3 |
|||
4 |
#ifndef __pinocchio_algorithm_rnea_second_order_derivatives_hxx__ |
||
5 |
#define __pinocchio_algorithm_rnea_second_order_derivatives_hxx__ |
||
6 |
|||
7 |
#include "pinocchio/algorithm/check.hpp" |
||
8 |
#include "pinocchio/multibody/visitor.hpp" |
||
9 |
|||
10 |
namespace pinocchio { |
||
11 |
|||
12 |
template <typename Scalar, int Options, |
||
13 |
template <typename, int> class JointCollectionTpl, |
||
14 |
typename ConfigVectorType, typename TangentVectorType1, |
||
15 |
typename TangentVectorType2> |
||
16 |
struct ComputeRNEASecondOrderDerivativesForwardStep |
||
17 |
: public fusion::JointUnaryVisitorBase<ComputeRNEASecondOrderDerivativesForwardStep< |
||
18 |
Scalar, Options, JointCollectionTpl, ConfigVectorType, |
||
19 |
TangentVectorType1, TangentVectorType2>> { |
||
20 |
typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model; |
||
21 |
typedef DataTpl<Scalar, Options, JointCollectionTpl> Data; |
||
22 |
|||
23 |
typedef boost::fusion::vector<const Model &, Data &, const ConfigVectorType &, |
||
24 |
const TangentVectorType1 &, |
||
25 |
const TangentVectorType2 &> |
||
26 |
ArgsType; |
||
27 |
|||
28 |
template <typename JointModel> |
||
29 |
216 |
static void algo(const JointModelBase<JointModel> &jmodel, |
|
30 |
JointDataBase<typename JointModel::JointDataDerived> &jdata, |
||
31 |
const Model &model, Data &data, |
||
32 |
const Eigen::MatrixBase<ConfigVectorType> &q, |
||
33 |
const Eigen::MatrixBase<TangentVectorType1> &v, |
||
34 |
const Eigen::MatrixBase<TangentVectorType2> &a) { |
||
35 |
typedef typename Model::JointIndex JointIndex; |
||
36 |
typedef typename Data::Motion Motion; |
||
37 |
typedef typename Data::Inertia Inertia; |
||
38 |
|||
39 |
✓✗ | 216 |
const JointIndex i = jmodel.id(); |
40 |
216 |
const JointIndex parent = model.parents[i]; |
|
41 |
216 |
Motion &ov = data.ov[i]; |
|
42 |
216 |
Motion &oa = data.oa[i]; |
|
43 |
216 |
Motion &vJ = data.v[i]; |
|
44 |
|||
45 |
✓✗ | 216 |
jmodel.calc(jdata.derived(), q.derived(), v.derived()); |
46 |
|||
47 |
✓✗✓✓ ✗✓✗ |
216 |
data.liMi[i] = model.jointPlacements[i] * jdata.M(); |
48 |
✓✓ | 216 |
if (parent > 0) { |
49 |
✓✗ | 208 |
data.oMi[i] = data.oMi[parent] * data.liMi[i]; |
50 |
✓✗ | 208 |
ov = data.ov[parent]; |
51 |
✓✗ | 208 |
oa = data.oa[parent]; |
52 |
} else { |
||
53 |
✓✗ | 8 |
data.oMi[i] = data.liMi[i]; |
54 |
✓✗ | 8 |
ov.setZero(); |
55 |
✓✗ | 8 |
oa = -model.gravity; |
56 |
} |
||
57 |
|||
58 |
typedef typename SizeDepType<JointModel::NV>::template ColsReturn< |
||
59 |
typename Data::Matrix6x>::Type ColsBlock; |
||
60 |
216 |
ColsBlock J_cols = jmodel.jointCols( |
|
61 |
✓✗ | 216 |
data.J); // data.J has all the phi (in world frame) stacked in columns |
62 |
216 |
ColsBlock psid_cols = |
|
63 |
✓✗ | 216 |
jmodel.jointCols(data.psid); // psid_cols is the psi_dot in world frame |
64 |
216 |
ColsBlock psidd_cols = jmodel.jointCols( |
|
65 |
✓✗ | 216 |
data.psidd); // psidd_cols is the psi_dotdot in world frame |
66 |
216 |
ColsBlock dJ_cols = |
|
67 |
✓✗ | 216 |
jmodel.jointCols(data.dJ); // This here is phi_dot in world frame |
68 |
|||
69 |
✓✗✓✗ ✓✗✓✗ |
216 |
J_cols.noalias() = data.oMi[i].act( |
70 |
jdata.S()); // J_cols is just the phi in world frame for a joint |
||
71 |
✓✗✓✗ |
216 |
vJ = data.oMi[i].act(jdata.v()); |
72 |
✓✗ | 216 |
motionSet::motionAction( |
73 |
ov, J_cols, psid_cols); // This ov here is v(p(i)), psi_dot calcs |
||
74 |
✓✗ | 216 |
motionSet::motionAction( |
75 |
oa, J_cols, psidd_cols); // This oa here is a(p(i)) , psi_dotdot calcs |
||
76 |
✓✗ | 216 |
motionSet::motionAction<ADDTO>( |
77 |
ov, psid_cols, |
||
78 |
psidd_cols); // This ov here is v(p(i)) , psi_dotdot calcs |
||
79 |
✓✗ | 216 |
ov += vJ; |
80 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
216 |
oa += (ov ^ vJ) + |
81 |
✓✗✓✗ ✓✗✓✗ |
216 |
data.oMi[i].act(jdata.S() * jmodel.jointVelocitySelector(a) + |
82 |
jdata.c()); |
||
83 |
✓✗ | 216 |
motionSet::motionAction( |
84 |
ov, J_cols, dJ_cols); // This here is phi_dot, here ov used is v(p(i)) + |
||
85 |
// vJ Composite rigid body inertia |
||
86 |
216 |
Inertia &oY = data.oYcrb[i]; |
|
87 |
|||
88 |
✓✗✓✗ |
216 |
oY = data.oMi[i].act(model.inertias[i]); |
89 |
✓✗✓✗ |
216 |
data.oh[i] = oY * ov; |
90 |
|||
91 |
✓✗✓✗ ✓✗✓✗ |
216 |
data.of[i] = oY * oa + oY.vxiv(ov); // f_i in world frame |
92 |
|||
93 |
✓✗ | 216 |
data.doYcrb[i] = oY.variation(ov); |
94 |
✓✗ | 216 |
addForceCrossMatrix(data.oh[i], data.doYcrb[i]); // BC{i} |
95 |
} |
||
96 |
template <typename ForceDerived, typename M6> |
||
97 |
216 |
static void addForceCrossMatrix(const ForceDense<ForceDerived> &f, |
|
98 |
const Eigen::MatrixBase<M6> &mout) { |
||
99 |
216 |
M6 &mout_ = PINOCCHIO_EIGEN_CONST_CAST(M6, mout); |
|
100 |
✓✗✓✗ ✓✗ |
216 |
addSkew(-f.linear(), mout_.template block<3, 3>(ForceDerived::LINEAR, |
101 |
ForceDerived::ANGULAR)); |
||
102 |
✓✗✓✗ ✓✗ |
216 |
addSkew(-f.linear(), mout_.template block<3, 3>(ForceDerived::ANGULAR, |
103 |
ForceDerived::LINEAR)); |
||
104 |
✓✗✓✗ ✓✗ |
216 |
addSkew(-f.angular(), mout_.template block<3, 3>(ForceDerived::ANGULAR, |
105 |
ForceDerived::ANGULAR)); |
||
106 |
216 |
} |
|
107 |
}; |
||
108 |
|||
109 |
template <typename Scalar, int Options, |
||
110 |
template <typename, int> class JointCollectionTpl, |
||
111 |
typename Tensor1, typename Tensor2, typename Tensor3, |
||
112 |
typename Tensor4> |
||
113 |
struct ComputeRNEASecondOrderDerivativesBackwardStep |
||
114 |
: public fusion::JointUnaryVisitorBase<ComputeRNEASecondOrderDerivativesBackwardStep< |
||
115 |
Scalar, Options, JointCollectionTpl, Tensor1, Tensor2, |
||
116 |
Tensor3, Tensor4>> { |
||
117 |
typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model; |
||
118 |
typedef DataTpl<Scalar, Options, JointCollectionTpl> Data; |
||
119 |
|||
120 |
typedef boost::fusion::vector<const Model &, Data &, const Tensor1 &, |
||
121 |
const Tensor2 &, const Tensor3 &, |
||
122 |
const Tensor4 &> |
||
123 |
ArgsType; |
||
124 |
|||
125 |
template <typename JointModel> |
||
126 |
216 |
static void algo(const JointModelBase<JointModel> &jmodel, const Model &model, |
|
127 |
Data &data, const Tensor1 &d2tau_dqdq, |
||
128 |
const Tensor2 &d2tau_dvdv, const Tensor3 &dtau_dqdv, |
||
129 |
const Tensor3 &dtau_dadq) { |
||
130 |
typedef typename Data::Motion Motion; |
||
131 |
typedef typename Data::Force Force; |
||
132 |
typedef typename Data::Inertia Inertia; |
||
133 |
typedef typename Model::JointIndex JointIndex; |
||
134 |
typedef typename Motion::ActionMatrixType ActionMatrixType; |
||
135 |
typedef typename Data::Matrix6 Matrix6; |
||
136 |
typedef typename Data::Vector6r Vector6r; |
||
137 |
typedef typename Data::Vector6c Vector6c; |
||
138 |
|||
139 |
✓✗ | 216 |
const JointIndex i = jmodel.id(); |
140 |
216 |
const JointIndex parent = model.parents[i]; |
|
141 |
|||
142 |
216 |
const Inertia &oYcrb = data.oYcrb[i]; // IC{i} |
|
143 |
216 |
const Matrix6 &oBcrb = data.doYcrb[i]; // BC{i} |
|
144 |
|||
145 |
216 |
Tensor1 &d2tau_dqdq_ = const_cast<Tensor1 &>(d2tau_dqdq); |
|
146 |
216 |
Tensor2 &d2tau_dvdv_ = const_cast<Tensor2 &>(d2tau_dvdv); |
|
147 |
216 |
Tensor3 &dtau_dqdv_ = const_cast<Tensor3 &>(dtau_dqdv); |
|
148 |
216 |
Tensor4 &dtau_dadq_ = const_cast<Tensor4 &>(dtau_dadq); |
|
149 |
|||
150 |
✓✗ | 216 |
Vector6r u1; |
151 |
✓✗ | 216 |
Vector6r u2; |
152 |
✓✗ | 216 |
Vector6c u3; |
153 |
✓✗ | 216 |
Vector6c u4; |
154 |
✓✗ | 216 |
Vector6c u5; |
155 |
✓✗ | 216 |
Vector6c u6; |
156 |
✓✗ | 216 |
Vector6c u7; |
157 |
✓✗ | 216 |
Vector6c u8; |
158 |
✓✗ | 216 |
Vector6c u9; |
159 |
✓✗ | 216 |
Vector6c u10; |
160 |
✓✗ | 216 |
Vector6r u11; |
161 |
✓✗ | 216 |
Vector6r u12; |
162 |
✓✗ | 216 |
Vector6c u13; |
163 |
|||
164 |
✓✗ | 216 |
Matrix6 Bicphii; |
165 |
✓✗ | 216 |
Matrix6 oBicpsidot; |
166 |
|||
167 |
Scalar p1, p2, p3, p4, p5, p6; |
||
168 |
|||
169 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
216 |
Matrix6 r0, r1, r2, r3, r4, r5, r6, r7; |
170 |
|||
171 |
✓✓ | 472 |
for (int p = 0; p < model.nvs[i]; p++) { |
172 |
256 |
const Eigen::DenseIndex ip = model.idx_vs[i] + p; |
|
173 |
|||
174 |
✓✗✓✗ |
256 |
const MotionRef<typename Data::Matrix6x::ColXpr> S_i = data.J.col(ip); // S{i}(:,p) |
175 |
✓✗ | 256 |
const ActionMatrixType S_iA = S_i.toActionMatrix(); //(S{i}(:,p) )x matrix |
176 |
✓✗✓✗ |
256 |
const MotionRef<typename Data::Matrix6x::ColXpr> psid_dm = data.psid.col(ip); // psi_dot for p DOF |
177 |
✓✗✓✗ |
256 |
const MotionRef<typename Data::Matrix6x::ColXpr> psidd_dm = data.psidd.col(ip); // psi_ddot for p DOF |
178 |
✓✗✓✗ |
256 |
const MotionRef<typename Data::Matrix6x::ColXpr> phid_dm = data.dJ.col(ip); // phi_dot for p DOF |
179 |
|||
180 |
✓✗✓✗ ✓✗ |
256 |
r1 = Bicphii = oYcrb.variation(S_i); // S{i}(p)x*IC{i} - IC{i} S{i}(p)x |
181 |
✓✗✓✗ |
256 |
oBicpsidot = oYcrb.variation(psid_dm); // new Bicpsidot in world frame |
182 |
|||
183 |
✓✗ | 256 |
Force f_tmp = oYcrb * S_i; // IC{i}S{i}(:,p) |
184 |
✓✗ | 256 |
ForceCrossMatrix(f_tmp, r0); // cmf_bar(IC{i}S{i}(:,p)) |
185 |
✓✗ | 256 |
Bicphii += r0; |
186 |
|||
187 |
✓✗✓✗ |
256 |
f_tmp = oYcrb * psid_dm; // IC{i}S{i}(:,p) |
188 |
✓✗ | 256 |
addForceCrossMatrix(f_tmp, oBicpsidot); // cmf_bar(IC{i}S{i}(:,p)) |
189 |
|||
190 |
✓✗✓✗ ✓✗✓✗ |
256 |
r2.noalias() = 2 * r0 - Bicphii; |
191 |
|||
192 |
✓✗✓✗ ✓✗✓✗ |
256 |
r3.noalias() = |
193 |
✓✗✓✗ |
256 |
oBicpsidot - S_iA.transpose() * oBcrb - |
194 |
✓✗ | 256 |
oBcrb * S_iA; // Bicpsidot + S{i}(p)x*BC{i}- BC {i}S{i}(p)x |
195 |
|||
196 |
// r4 |
||
197 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
256 |
f_tmp.toVector().noalias() = oBcrb.transpose() * S_i.toVector(); |
198 |
✓✗ | 256 |
ForceCrossMatrix(f_tmp, r4); // cmf_bar(BC{i}.'S{i}(:,p)) |
199 |
// r5 |
||
200 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
256 |
f_tmp.toVector().noalias() = oBcrb * psid_dm.toVector(); |
201 |
✓✗✓✗ |
256 |
f_tmp += S_i.cross(data.of[i]); |
202 |
✓✗✓✗ ✓✗ |
256 |
motionSet::inertiaAction<ADDTO>(oYcrb, psidd_dm.toVector(), f_tmp.toVector()); |
203 |
✓✗ | 256 |
ForceCrossMatrix( |
204 |
f_tmp, |
||
205 |
r5); // cmf_bar(BC{i}psi_dot{i}(:,p)+IC{i}psi_ddot{i}(:,p)+S{i}(:,p)x*f{i}) |
||
206 |
|||
207 |
// S{i}(:,p)x* IC{i} + r0 |
||
208 |
✓✗✓✗ ✓✗✓✗ |
256 |
r6 = r0 + oYcrb.vxi(S_i); |
209 |
|||
210 |
// r7 |
||
211 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
256 |
f_tmp.toVector().noalias() = oBcrb * S_i.toVector(); |
212 |
✓✗✓✗ ✓✗ |
256 |
f_tmp += oYcrb * (psid_dm + phid_dm); |
213 |
✓✗ | 256 |
ForceCrossMatrix(f_tmp, r7); // cmf_bar(BC{i}S{i}(:,p) + |
214 |
// IC{i}(psi_dot{i}(:,p)+phi_dot{i}(:,p))) |
||
215 |
|||
216 |
256 |
JointIndex j = i; |
|
217 |
|||
218 |
✓✓ | 1400 |
while (j > 0) { |
219 |
|||
220 |
✓✓ | 3568 |
for (int q = 0; q < model.nvs[j]; q++) { |
221 |
2424 |
const Eigen::DenseIndex jq = model.idx_vs[j] + q; |
|
222 |
|||
223 |
✓✗✓✗ |
2424 |
const MotionRef<typename Data::Matrix6x::ColXpr> S_j = data.J.col(jq); |
224 |
✓✗✓✗ |
2424 |
const MotionRef<typename Data::Matrix6x::ColXpr> psid_dm = data.psid.col(jq); // psi_dot{j}(:,q) |
225 |
✓✗✓✗ |
2424 |
const MotionRef<typename Data::Matrix6x::ColXpr> psidd_dm = data.psidd.col(jq); // psi_ddot{j}(:,q) |
226 |
✓✗✓✗ |
2424 |
const MotionRef<typename Data::Matrix6x::ColXpr> phid_dm = data.dJ.col(jq); // phi_dot{j}(:,q) |
227 |
|||
228 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
2424 |
u1.noalias() = S_j.toVector().transpose() * r3; |
229 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
2424 |
u2.noalias() = S_j.toVector().transpose() * r1; |
230 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
2424 |
u3.noalias() = r3 * psid_dm.toVector() + r1 * psidd_dm.toVector() + r5 * S_j.toVector(); |
231 |
✓✗✓✗ ✓✗✓✗ |
2424 |
u4.noalias() = r6 * S_j.toVector(); |
232 |
✓✗✓✗ ✓✗✓✗ |
2424 |
u5.noalias() = r2 * psid_dm.toVector(); |
233 |
✓✗✓✗ ✓✗✓✗ |
2424 |
u6.noalias() = Bicphii * psid_dm.toVector(); |
234 |
✓✗✓✗ ✓✗✓✗ |
2424 |
u6.noalias() += r7 * S_j.toVector(); |
235 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
2424 |
u7.noalias() = r3 * S_j.toVector() + r1 * (psid_dm.toVector() + phid_dm.toVector()); |
236 |
✓✗✓✗ ✓✗✓✗ |
2424 |
u8.noalias() = r4 * S_j.toVector(); |
237 |
✓✗✓✗ ✓✗✓✗ |
2424 |
u9.noalias() = r0 * S_j.toVector(); |
238 |
✓✗✓✗ ✓✗✓✗ |
2424 |
u10.noalias() = Bicphii * S_j.toVector(); |
239 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
2424 |
u11.noalias() = S_j.toVector().transpose() * Bicphii; |
240 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
2424 |
u12.noalias() = psid_dm.toVector().transpose() * Bicphii; |
241 |
✓✗✓✗ ✓✗✓✗ |
2424 |
u13.noalias() = r1 * S_j.toVector(); |
242 |
|||
243 |
2424 |
JointIndex k = j; |
|
244 |
|||
245 |
✓✓ | 7632 |
while (k > 0) { |
246 |
|||
247 |
✓✓ | 22536 |
for (int r = 0; r < model.nvs[k]; r++) { |
248 |
17328 |
const Eigen::DenseIndex kr = model.idx_vs[k] + r; |
|
249 |
|||
250 |
✓✗✓✗ |
17328 |
const MotionRef<typename Data::Matrix6x::ColXpr> S_k(data.J.col(kr)); |
251 |
✓✗✓✗ |
17328 |
const MotionRef<typename Data::Matrix6x::ColXpr> psid_dm = data.psid.col(kr); // psi_dot{k}(:,r) |
252 |
✓✗✓✗ |
17328 |
const MotionRef<typename Data::Matrix6x::ColXpr> psidd_dm = data.psidd.col(kr); // psi_ddot{k}(:,r) |
253 |
✓✗✓✗ |
17328 |
const MotionRef<typename Data::Matrix6x::ColXpr> phid_dm = data.dJ.col(kr); // phi_dot{k}(:,r) |
254 |
|||
255 |
✓✗✓✗ ✓✗ |
17328 |
p1 = u11 * psid_dm.toVector(); |
256 |
✓✗✓✗ |
17328 |
p2 = u9.dot(psidd_dm.toVector()); |
257 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
17328 |
p2 += (-u12 + u8.transpose()) * psid_dm.toVector(); |
258 |
|||
259 |
✓✗ | 17328 |
d2tau_dqdq_(ip, jq, kr) = p2; |
260 |
✓✗ | 17328 |
dtau_dqdv_(ip, kr, jq) = -p1; |
261 |
|||
262 |
✓✓ | 17328 |
if (j != i) { |
263 |
✓✗✓✗ ✓✗✓✗ |
13464 |
p3 = -u11 * S_k.toVector(); |
264 |
✓✗✓✗ |
13464 |
p4 = S_k.toVector().dot(u13); |
265 |
✓✗✓✗ ✓✗✓✗ |
13464 |
d2tau_dqdq_(jq, kr, ip) = u1 * psid_dm.toVector(); |
266 |
✓✗✓✗ ✓✗✓✗ |
13464 |
d2tau_dqdq_(jq, kr, ip) += u2 * psidd_dm.toVector(); |
267 |
✓✗✓✗ |
13464 |
d2tau_dqdq_(jq, ip, kr) = d2tau_dqdq_(jq, kr, ip); |
268 |
✓✗ | 13464 |
dtau_dqdv_(jq, kr, ip) = p1; |
269 |
✓✗✓✗ ✓✗✓✗ |
13464 |
dtau_dqdv_(jq, ip, kr) = u1 * S_k.toVector(); |
270 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
13464 |
dtau_dqdv_(jq, ip, kr) += u2 * (psid_dm.toVector() + phid_dm.toVector()); |
271 |
✓✗ | 13464 |
d2tau_dvdv_(jq, kr, ip) = -p3; |
272 |
✓✗ | 13464 |
d2tau_dvdv_(jq, ip, kr) = -p3; |
273 |
✓✗ | 13464 |
dtau_dadq_(kr, jq, ip) = p4; |
274 |
✓✗ | 13464 |
dtau_dadq_(jq, kr, ip) = p4; |
275 |
} |
||
276 |
|||
277 |
✓✓ | 17328 |
if (k != j) { |
278 |
✓✗✓✗ ✓✗✓✗ |
7224 |
p3 = -u11 * S_k.toVector(); |
279 |
✓✗✓✗ |
7224 |
p5 = S_k.toVector().dot(u9); |
280 |
✓✗ | 7224 |
d2tau_dqdq_(ip, kr, jq) = p2; |
281 |
✓✗✓✗ ✓✗ |
7224 |
d2tau_dqdq_(kr, ip, jq) = S_k.toVector().dot(u3); |
282 |
✓✗ | 7224 |
d2tau_dvdv_(ip, jq, kr) = p3; |
283 |
✓✗ | 7224 |
d2tau_dvdv_(ip, kr, jq) = p3; |
284 |
✓✗✓✗ ✓✗✓✗ |
7224 |
dtau_dqdv_(ip, jq, kr) = S_k.toVector().dot(u5 + u8); |
285 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
7224 |
dtau_dqdv_(ip, jq, kr) += u9.dot(psid_dm.toVector() + phid_dm.toVector()); |
286 |
✓✗✓✗ ✓✗ |
7224 |
dtau_dqdv_(kr, jq, ip) = S_k.toVector().dot(u6); |
287 |
✓✗ | 7224 |
dtau_dadq_(kr, ip, jq) = p5; |
288 |
✓✗ | 7224 |
dtau_dadq_(ip, kr, jq) = p5; |
289 |
✓✓ | 7224 |
if (j != i) { |
290 |
✓✗✓✗ |
5296 |
p6 = S_k.toVector().dot(u10); |
291 |
✓✗✓✗ |
5296 |
d2tau_dqdq_(kr, jq, ip) = d2tau_dqdq_(kr, ip, jq); |
292 |
✓✗ | 5296 |
d2tau_dvdv_(kr, ip, jq) = p6; |
293 |
✓✗ | 5296 |
d2tau_dvdv_(kr, jq, ip) = p6; |
294 |
✓✗✓✗ ✓✗ |
5296 |
dtau_dqdv_(kr, ip, jq) = S_k.toVector().dot(u7); |
295 |
|||
296 |
} else { |
||
297 |
✓✗✓✗ ✓✗ |
1928 |
d2tau_dvdv_(kr, jq, ip) = S_k.toVector().dot(u4); |
298 |
} |
||
299 |
|||
300 |
} else { |
||
301 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
10104 |
d2tau_dvdv_(ip, jq, kr) = -u2 * S_k.toVector(); |
302 |
} |
||
303 |
} |
||
304 |
|||
305 |
5208 |
k = model.parents[k]; |
|
306 |
} |
||
307 |
} |
||
308 |
1144 |
j = model.parents[j]; |
|
309 |
} |
||
310 |
} |
||
311 |
|||
312 |
✓✓ | 216 |
if (parent > 0) { |
313 |
✓✗ | 208 |
data.oYcrb[parent] += data.oYcrb[i]; |
314 |
✓✗ | 208 |
data.doYcrb[parent] += data.doYcrb[i]; |
315 |
✓✗ | 208 |
data.of[parent] += data.of[i]; |
316 |
} |
||
317 |
} |
||
318 |
|||
319 |
// Function for cmf_bar operator |
||
320 |
template <typename ForceDerived, typename M6> |
||
321 |
512 |
static void ForceCrossMatrix(const ForceDense<ForceDerived> &f, |
|
322 |
const Eigen::MatrixBase<M6> &mout) { |
||
323 |
512 |
M6 &mout_ = PINOCCHIO_EIGEN_CONST_CAST(M6, mout); |
|
324 |
✓✗ | 512 |
mout_.template block<3, 3>(ForceDerived::LINEAR, ForceDerived::LINEAR) |
325 |
.setZero(); |
||
326 |
✓✗ | 512 |
mout_.template block<3, 3>(ForceDerived::LINEAR, ForceDerived::ANGULAR) = |
327 |
mout_.template block<3, 3>(ForceDerived::ANGULAR, |
||
328 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
512 |
ForceDerived::LINEAR) = skew(-f.linear()); |
329 |
✓✗✓✗ ✓✗✓✗ |
512 |
mout_.template block<3, 3>(ForceDerived::ANGULAR, ForceDerived::ANGULAR) = |
330 |
skew(-f.angular()); |
||
331 |
512 |
} |
|
332 |
template <typename ForceDerived, typename M6> |
||
333 |
|||
334 |
128 |
static void addForceCrossMatrix(const ForceDense<ForceDerived> &f, |
|
335 |
const Eigen::MatrixBase<M6> &mout) { |
||
336 |
128 |
M6 &mout_ = PINOCCHIO_EIGEN_CONST_CAST(M6, mout); |
|
337 |
✓✗✓✗ ✓✗ |
128 |
addSkew(-f.linear(), mout_.template block<3, 3>(ForceDerived::LINEAR, |
338 |
ForceDerived::ANGULAR)); |
||
339 |
✓✗✓✗ ✓✗ |
128 |
addSkew(-f.linear(), mout_.template block<3, 3>(ForceDerived::ANGULAR, |
340 |
ForceDerived::LINEAR)); |
||
341 |
✓✗✓✗ ✓✗ |
128 |
addSkew(-f.angular(), mout_.template block<3, 3>(ForceDerived::ANGULAR, |
342 |
ForceDerived::ANGULAR)); |
||
343 |
128 |
} |
|
344 |
}; |
||
345 |
|||
346 |
template <typename Scalar, int Options, |
||
347 |
template <typename, int> class JointCollectionTpl, |
||
348 |
typename ConfigVectorType, typename TangentVectorType1, |
||
349 |
typename TangentVectorType2, typename Tensor1, |
||
350 |
typename Tensor2, typename Tensor3, typename Tensor4> |
||
351 |
8 |
inline void ComputeRNEASecondOrderDerivatives( |
|
352 |
const ModelTpl<Scalar, Options, JointCollectionTpl> &model, |
||
353 |
DataTpl<Scalar, Options, JointCollectionTpl> &data, |
||
354 |
const Eigen::MatrixBase<ConfigVectorType> &q, |
||
355 |
const Eigen::MatrixBase<TangentVectorType1> &v, |
||
356 |
const Eigen::MatrixBase<TangentVectorType2> &a, const Tensor1 &d2tau_dqdq, |
||
357 |
const Tensor2 &d2tau_dvdv, const Tensor3 &dtau_dqdv, |
||
358 |
const Tensor4 &dtau_dadq) { |
||
359 |
// Extra safety here |
||
360 |
✗✓✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗ |
8 |
PINOCCHIO_CHECK_ARGUMENT_SIZE( |
361 |
q.size(), model.nq, |
||
362 |
"The joint configuration vector is not of right size"); |
||
363 |
✗✓✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗ |
8 |
PINOCCHIO_CHECK_ARGUMENT_SIZE( |
364 |
v.size(), model.nv, "The joint velocity vector is not of right size"); |
||
365 |
✗✓✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗ |
8 |
PINOCCHIO_CHECK_ARGUMENT_SIZE( |
366 |
a.size(), model.nv, "The joint acceleration vector is not of right size"); |
||
367 |
✗✓✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗ |
8 |
PINOCCHIO_CHECK_ARGUMENT_SIZE(d2tau_dqdq.dimension(0), model.nv); |
368 |
✗✓✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗ |
8 |
PINOCCHIO_CHECK_ARGUMENT_SIZE(d2tau_dqdq.dimension(1), model.nv); |
369 |
✗✓✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗ |
8 |
PINOCCHIO_CHECK_ARGUMENT_SIZE(d2tau_dqdq.dimension(2), model.nv); |
370 |
✗✓✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗ |
8 |
PINOCCHIO_CHECK_ARGUMENT_SIZE(d2tau_dvdv.dimension(0), model.nv); |
371 |
✗✓✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗ |
8 |
PINOCCHIO_CHECK_ARGUMENT_SIZE(d2tau_dvdv.dimension(1), model.nv); |
372 |
✗✓✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗ |
8 |
PINOCCHIO_CHECK_ARGUMENT_SIZE(d2tau_dvdv.dimension(2), model.nv); |
373 |
✗✓✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗ |
8 |
PINOCCHIO_CHECK_ARGUMENT_SIZE(dtau_dqdv.dimension(0), model.nv); |
374 |
✗✓✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗ |
8 |
PINOCCHIO_CHECK_ARGUMENT_SIZE(dtau_dqdv.dimension(1), model.nv); |
375 |
✗✓✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗ |
8 |
PINOCCHIO_CHECK_ARGUMENT_SIZE(dtau_dqdv.dimension(2), model.nv); |
376 |
✗✓✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗ |
8 |
PINOCCHIO_CHECK_ARGUMENT_SIZE(dtau_dadq.dimension(0), model.nv); |
377 |
✗✓✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗ |
8 |
PINOCCHIO_CHECK_ARGUMENT_SIZE(dtau_dadq.dimension(1), model.nv); |
378 |
✗✓✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗ |
8 |
PINOCCHIO_CHECK_ARGUMENT_SIZE(dtau_dadq.dimension(2), model.nv); |
379 |
✓✗ | 8 |
assert(model.check(data) && "data is not consistent with model."); |
380 |
|||
381 |
typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model; |
||
382 |
typedef typename Model::JointIndex JointIndex; |
||
383 |
|||
384 |
typedef ComputeRNEASecondOrderDerivativesForwardStep< |
||
385 |
Scalar, Options, JointCollectionTpl, ConfigVectorType, TangentVectorType1, |
||
386 |
TangentVectorType2> |
||
387 |
Pass1; |
||
388 |
✓✓ | 224 |
for (JointIndex i = 1; i < (JointIndex)model.njoints; ++i) { |
389 |
✓✗ | 216 |
Pass1::run(model.joints[i], data.joints[i], |
390 |
typename Pass1::ArgsType(model, data, q.derived(), v.derived(), |
||
391 |
a.derived())); |
||
392 |
} |
||
393 |
|||
394 |
typedef ComputeRNEASecondOrderDerivativesBackwardStep< |
||
395 |
Scalar, Options, JointCollectionTpl, Tensor1, Tensor2, |
||
396 |
Tensor3, Tensor4> |
||
397 |
Pass2; |
||
398 |
✓✓ | 224 |
for (JointIndex i = (JointIndex)(model.njoints - 1); i > 0; --i) { |
399 |
✓✗ | 216 |
Pass2::run(model.joints[i], |
400 |
typename Pass2::ArgsType(model, data, |
||
401 |
const_cast<Tensor1 &>(d2tau_dqdq), |
||
402 |
const_cast<Tensor2 &>(d2tau_dvdv), |
||
403 |
const_cast<Tensor3 &>(dtau_dqdv), |
||
404 |
const_cast<Tensor4 &>(dtau_dadq))); |
||
405 |
} |
||
406 |
8 |
} |
|
407 |
|||
408 |
} // namespace pinocchio |
||
409 |
|||
410 |
#endif // ifndef __pinocchio_algorithm_rnea_second_order_derivatives_hxx__ |
| Generated by: GCOVR (Version 4.2) |