Matrix<Scalar, Dynamic, 1> dynamicsBiasTermTemp(const RigidBodyTree &model, KinematicsCache<Scalar> &cache, const MatrixBase<DerivedF> &f_ext_value) {
// temporary solution.
eigen_aligned_unordered_map<const RigidBody *, Matrix<Scalar, 6, 1> > f_ext;
if (f_ext_value.size() > 0) {
assert(f_ext_value.cols() == model.bodies.size());
for (DenseIndex i = 0; i < f_ext_value.cols(); i++) {
f_ext.insert({model.bodies[i].get(), f_ext_value.col(i)});
}
}
return model.dynamicsBiasTerm(cache, f_ext);
};
void scenario2(
const RigidBodyTree& model, KinematicsCache<Scalar>& cache,
const vector<pair<Matrix<Scalar, Dynamic, 1>, Matrix<Scalar, Dynamic, 1>>>&
states) {
const eigen_aligned_unordered_map<RigidBody const*,
Matrix<Scalar, TWIST_SIZE, 1>>
f_ext;
for (const auto& state : states) {
cache.initialize(state.first, state.second);
model.doKinematics(cache, true);
auto H = model.massMatrix(cache);
auto C = model.dynamicsBiasTerm(cache, f_ext);
if (uniform(generator) <
1e-15) { // print with some probability to avoid optimizing away
printMatrix<decltype(H)::RowsAtCompileTime,
decltype(H)::ColsAtCompileTime>(
H); // MSVC 2013 can't infer rows and cols (ICE)
printMatrix<decltype(C)::RowsAtCompileTime,
decltype(C)::ColsAtCompileTime>(
C); // MSVC 2013 can't infer rows and cols (ICE)
}
}
}
