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