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) } } }