예제 #1
0
double KinematicsMetrics::getJointLimitsPenalty(const robot_state::RobotState &state, const robot_model::JointModelGroup *joint_model_group) const
{
  if (fabs(penalty_multiplier_) <= boost::math::tools::epsilon<double>())
     return 1.0;
  double joint_limits_multiplier(1.0);
  const std::vector<const robot_model::JointModel*> &joint_model_vector = joint_model_group->getJointModels();
  for (std::size_t i = 0; i < joint_model_vector.size(); ++i)
  {
    if (joint_model_vector[i]->getType() == robot_model::JointModel::REVOLUTE)
    {
      const robot_model::RevoluteJointModel* revolute_model = static_cast<const robot_model::RevoluteJointModel*>(joint_model_vector[i]);
      if (revolute_model->isContinuous())
        continue;
    }
    if (joint_model_vector[i]->getType() == robot_model::JointModel::PLANAR)
    {
      const robot_model::JointModel::Bounds &bounds = joint_model_vector[i]->getVariableBounds();
      if (bounds[0].min_position_ == -std::numeric_limits<double>::max() || bounds[0].max_position_ == std::numeric_limits<double>::max() ||
          bounds[1].min_position_ == -std::numeric_limits<double>::max() || bounds[1].max_position_ == std::numeric_limits<double>::max() ||
          bounds[2].min_position_ == -boost::math::constants::pi<double>() || bounds[2].max_position_ == boost::math::constants::pi<double>())
        continue;
    }
    if (joint_model_vector[i]->getType() == robot_model::JointModel::FLOATING)
    {
      //Joint limits are not well-defined for floating joints
      continue;
    }
    const double *joint_values = state.getJointPositions(joint_model_vector[i]);
    const robot_model::JointModel::Bounds &bounds = joint_model_vector[i]->getVariableBounds();
    std::vector<double> lower_bounds, upper_bounds;
    for (std::size_t j = 0; j < bounds.size(); ++j)
    {
      lower_bounds.push_back(bounds[j].min_position_);
      upper_bounds.push_back(bounds[j].max_position_);
    }
    double lower_bound_distance = joint_model_vector[i]->distance(joint_values, &lower_bounds[0]);
    double upper_bound_distance = joint_model_vector[i]->distance(joint_values, &upper_bounds[0]);
    double range = lower_bound_distance + upper_bound_distance;
    if (range <= boost::math::tools::epsilon<double>())
      continue;
    joint_limits_multiplier *= (lower_bound_distance * upper_bound_distance/(range*range));
  }
  return (1.0 - exp(-penalty_multiplier_*joint_limits_multiplier));
}
// BEGIN_SUB_TUTORIAL userCallback
//
// User defined constraints can also be specified to the PlanningScene
// class. This is done by specifying a callback using the
// setStateFeasibilityPredicate function. Here's a simple example of a
// user-defined callback that checks whether the "r_shoulder_pan" of
// the PR2 robot is at a positive or negative angle:
bool userCallback(const robot_state::RobotState &kinematic_state, bool verbose)
{
  const double* joint_values = kinematic_state.getJointPositions("shoulder_pan_joint");
  return (joint_values[0] > 0.0);
}