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