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