kinematic_constraints::ConstraintEvaluationResult kinematic_constraints::JointConstraint::decide(const robot_state::RobotState &state, bool verbose) const
{
if (!joint_model_)
return ConstraintEvaluationResult(true, 0.0);
const robot_state::JointState *joint = state.getJointState(joint_model_->getName());
if (!joint)
{
logWarn("No joint in state with name '%s'", joint_model_->getName().c_str());
return ConstraintEvaluationResult(true, 0.0);
}
double current_joint_position = joint->getVariableValues()[0];
if (!local_variable_name_.empty())
{
const std::map<std::string, unsigned int> &index_map = joint->getVariableIndexMap();
std::map<std::string, unsigned int>::const_iterator it = index_map.find(joint_variable_name_);
if (it == index_map.end())
{
logWarn("Local name '%s' is not known to joint state with name '%s'", local_variable_name_.c_str(), joint_model_->getName().c_str());
return ConstraintEvaluationResult(true, 0.0);
}
else
current_joint_position = joint->getVariableValues()[it->second];
}
double dif = 0.0;
// compute signed shortest distance for continuous joints
if (joint_is_continuous_)
{
dif = normalizeAngle(current_joint_position) - joint_position_;
if (dif > boost::math::constants::pi<double>())
dif = 2.0*boost::math::constants::pi<double>() - dif;
else
if (dif < -boost::math::constants::pi<double>())
dif += 2.0*boost::math::constants::pi<double>(); // we include a sign change to have dif > 0
}
else
dif = current_joint_position - joint_position_;
// check bounds
bool result = dif <= (joint_tolerance_above_+2*std::numeric_limits<double>::epsilon()) && dif >= (-joint_tolerance_below_-2*std::numeric_limits<double>::epsilon());
if (verbose)
logInform("Constraint %s:: Joint name: '%s', actual value: %f, desired value: %f, tolerance_above: %f, tolerance_below: %f",
result ? "satisfied" : "violated", joint_variable_name_.c_str(),
current_joint_position, joint_position_, joint_tolerance_above_, joint_tolerance_below_);
return ConstraintEvaluationResult(result, constraint_weight_ * fabs(dif));
}
bool userCallback(const robot_state::RobotState &kinematic_state, bool verbose)
{
// get the joint value for the right shoulder pan of the PR2 robot
const std::vector<double>& joint_state_values = kinematic_state.getJointState("shoulder_pan_joint")->getVariableValues();
return (joint_state_values.front() > 0.0);
}