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