コード例 #1
0
kinematic_constraints::ConstraintEvaluationResult kinematic_constraints::OrientationConstraint::decide(const robot_state::RobotState &state, bool verbose) const
{
  if (!link_model_)
    return ConstraintEvaluationResult(true, 0.0);

  const robot_state::LinkState *link_state = state.getLinkState(link_model_->getName());

  if (!link_state)
  {
    logWarn("No link in state with name '%s'", link_model_->getName().c_str());
    return ConstraintEvaluationResult(false, 0.0);
  }

  Eigen::Vector3d xyz;
  if (mobile_frame_)
  {
    Eigen::Matrix3d tmp;
    tf_->transformRotationMatrix(state, desired_rotation_frame_id_, desired_rotation_matrix_, tmp);
    Eigen::Affine3d diff(tmp.inverse() * link_state->getGlobalLinkTransform().rotation());
    xyz = diff.rotation().eulerAngles(0, 1, 2);
    // 0,1,2 corresponds to ZXZ, the convention used in sampling constraints
  }
  else
  {
    Eigen::Affine3d diff(desired_rotation_matrix_inv_ * link_state->getGlobalLinkTransform().rotation());
    xyz = diff.rotation().eulerAngles(0, 1, 2); // 0,1,2 corresponds to ZXZ, the convention used in sampling constraints
  }

  xyz(0) = std::min(fabs(xyz(0)), boost::math::constants::pi<double>() - fabs(xyz(0)));
  xyz(1) = std::min(fabs(xyz(1)), boost::math::constants::pi<double>() - fabs(xyz(1)));
  xyz(2) = std::min(fabs(xyz(2)), boost::math::constants::pi<double>() - fabs(xyz(2)));
  bool result = xyz(2) < absolute_z_axis_tolerance_+std::numeric_limits<double>::epsilon()
    && xyz(1) < absolute_y_axis_tolerance_+std::numeric_limits<double>::epsilon()
    && xyz(0) < absolute_x_axis_tolerance_+std::numeric_limits<double>::epsilon();

  if (verbose)
  {
    Eigen::Quaterniond q_act(link_state->getGlobalLinkTransform().rotation());
    Eigen::Quaterniond q_des(desired_rotation_matrix_);
    logInform("Orientation constraint %s for link '%s'. Quaternion desired: %f %f %f %f, quaternion actual: %f %f %f %f, error: x=%f, y=%f, z=%f, tolerance: x=%f, y=%f, z=%f",
             result ? "satisfied" : "violated", link_model_->getName().c_str(),
             q_des.x(), q_des.y(), q_des.z(), q_des.w(),
             q_act.x(), q_act.y(), q_act.z(), q_act.w(), xyz(0), xyz(1), xyz(2),
             absolute_x_axis_tolerance_, absolute_y_axis_tolerance_, absolute_z_axis_tolerance_);
  }

  return ConstraintEvaluationResult(result, constraint_weight_ * (xyz(0) + xyz(1) + xyz(2)));
}
コード例 #2
0
kinematic_constraints::ConstraintEvaluationResult kinematic_constraints::PositionConstraint::decide(const robot_state::RobotState &state, bool verbose) const
{
  if (!link_model_ || constraint_region_.empty())
    return ConstraintEvaluationResult(true, 0.0);

  const robot_state::LinkState *link_state = state.getLinkState(link_model_->getName());

  if (!link_state)
  {
    logWarn("No link in state with name '%s'", link_model_->getName().c_str());
    return ConstraintEvaluationResult(false, 0.0);
  }

  Eigen::Vector3d pt = link_state->getGlobalLinkTransform() * offset_;
  if (mobile_frame_)
  {
    Eigen::Affine3d tmp;
    for (std::size_t i = 0 ; i < constraint_region_.size() ; ++i)
    {
      tf_->transformPose(state, constraint_frame_id_, constraint_region_pose_[i], tmp);
      bool result = constraint_region_[i]->cloneAt(tmp)->containsPoint(pt, verbose);
      if (result || (i + 1 == constraint_region_pose_.size()))
        return finishPositionConstraintDecision(pt, tmp.translation(), link_model_->getName(), constraint_weight_, result, verbose);
      else
        finishPositionConstraintDecision(pt, tmp.translation(), link_model_->getName(), constraint_weight_, result, verbose);
    }
  }
  else
  {
    for (std::size_t i = 0 ; i < constraint_region_.size() ; ++i)
    {
      bool result = constraint_region_[i]->containsPoint(pt, true);
      if (result || (i + 1 == constraint_region_.size()))
        return finishPositionConstraintDecision(pt, constraint_region_[i]->getPose().translation(), link_model_->getName(), constraint_weight_, result, verbose);
      else
        finishPositionConstraintDecision(pt, constraint_region_[i]->getPose().translation(), link_model_->getName(), constraint_weight_, result, verbose);
    }
  }
  return ConstraintEvaluationResult(false, 0.0);
}