| 23 | const std::size_t NUM_SIM_STEPS = 100; |
| 24 | |
| 25 | void addFloorToGeomModel(GeometryModel & geom_model) |
| 26 | { |
| 27 | using CollisionGeometryPtr = GeometryObject::CollisionGeometryPtr; |
| 28 | |
| 29 | CollisionGeometryPtr floor_collision_shape = CollisionGeometryPtr(new hpp::fcl::Halfspace(0.0, 0.0, 1.0, 0.0)); |
| 30 | |
| 31 | const SE3 M = SE3::Identity(); |
| 32 | GeometryObject floor("floor", 0, 0, M, floor_collision_shape); |
| 33 | geom_model.addGeometryObject(floor); |
| 34 | } |
| 35 | |
| 36 | void addSystemCollisionPairs(const Model & model, GeometryModel & geom_model, const Eigen::VectorXd & qref) |
| 37 | { |