Todo List

Member mfcpp::PlanningNodelet::connect_lattices (const RobotModel::state_type &init_state, std::vector< Lattice > &lattices_in, std::vector< Lattice > &lattices_out)

Remove nodes of the first lattice that doesn't lead to the right tree depth.

Member mfcpp::RobotModel::get_lin_discr_matrices (const state_type &x_0, const input_type &u_0, MatrixT &Ad, MatrixT &Bd, float dt) const

Update how to get B_d