bool planning_models::KinematicModel::JointModel::isValueWithinVariableBounds(const std::string& variable, const double& value, bool& within_bounds) const { std::pair<double, double> bounds; if(!getVariableBounds(variable, bounds)) { return false; } if(value < bounds.first || value > bounds.second) { ROS_DEBUG_STREAM("Violates bounds: Value " << value << " lower " << bounds.first << " upper " << bounds.second); within_bounds = false; } else { ROS_DEBUG_STREAM("Satisfies bounds: Value " << value << " lower " << bounds.first << " upper " << bounds.second); within_bounds = true; } return true; }
void DirectCollocationInternal::evaluate(int nfdir, int nadir){ // get NLP variable bounds and initial guess getGuess(nlp_solver_.input(NLP_X_INIT).data()); getVariableBounds(nlp_solver_.input(NLP_LBX).data(),nlp_solver_.input(NLP_UBX).data()); // get NLP constraint bounds getConstraintBounds(nlp_solver_.input(NLP_LBG).data(), nlp_solver_.input(NLP_UBG).data()); //Solve the problem nlp_solver_.solve(); // Save the optimal solution setOptimalSolution(nlp_solver_.output(NLP_X_OPT).data()); // Save the optimal cost output(OCP_COST).set(nlp_solver_.output(NLP_COST)); }
void DirectMultipleShootingInternal::evaluate(){ // get NLP variable bounds and initial guess getGuess(nlp_solver_.input(NLP_SOLVER_X0).data()); getVariableBounds(nlp_solver_.input(NLP_SOLVER_LBX).data(),nlp_solver_.input(NLP_SOLVER_UBX).data()); // get NLP constraint bounds getConstraintBounds(nlp_solver_.input(NLP_SOLVER_LBG).data(), nlp_solver_.input(NLP_SOLVER_UBG).data()); //Solve the problem nlp_solver_.solve(); // Save the optimal solution setOptimalSolution(nlp_solver_.output(NLP_SOLVER_X).data()); // Save the optimal cost output(OCP_COST).set(nlp_solver_.output(NLP_SOLVER_F)); }