ボール回り込みの改善

crane_robot_skills/include/crane_robot_skills/kickoff_support.hpp

@@ -26,7 +26,7 @@ class KickoffSupport : public SkillBase<RobotCommandWrapperPosition>
   Status update([[maybe_unused]] const ConsaiVisualizerWrapper::SharedPtr & visualizer) override
   {
     Point target(getParameter<double>("target_x"), getParameter<double>("target_y"));
-    command.setDribblerTargetPosition(target).lookAtBallFrom(target);
+    command.lookAtBallFrom(target).setDribblerTargetPosition(target);
     return Status::RUNNING;
   }
 

crane_robot_skills/include/crane_robot_skills/receive.hpp

@@ -0,0 +1,82 @@
+// Copyright (c) 2023 ibis-ssl
+//
+// Use of this source code is governed by an MIT-style
+// license that can be found in the LICENSE file or at
+// https://opensource.org/licenses/MIT.
+
+#ifndef CRANE_ROBOT_SKILLS__RECEIVE_HPP_
+#define CRANE_ROBOT_SKILLS__RECEIVE_HPP_
+
+#include <crane_basics/eigen_adapter.hpp>
+#include <crane_robot_skills/skill_base.hpp>
+#include <memory>
+
+namespace crane::skills
+{
+class Receive : public SkillBase<RobotCommandWrapperPosition>
+{
+public:
+  explicit Receive(RobotCommandWrapperBase::SharedPtr & base) : SkillBase("Receive", base)
+  {
+    setParameter("dribble_power", 0.3);
+    setParameter("enable_software_bumper", true);
+    setParameter("software_bumper_start_time", 0.5);
+    // min_slack, max_slack, closest
+    setParameter("policy", std::string("closest"));
+  }
+
+  Status update([[maybe_unused]] const ConsaiVisualizerWrapper::SharedPtr & visualizer) override
+  {
+    auto offset = [&]() -> Point {
+      if (getParameter<bool>("enable_software_bumper")) {
+        // ボール到着まで残り<software_bumper_start_time>秒になったら、ボール速度方向に少し加速して衝撃を和らげる
+        double ball_speed = world_model()->ball.vel.norm();
+        if (
+          robot()->getDistance(world_model()->ball.pos) <
+          ball_speed * getParameter<double>("software_bumper_start_time")) {
+          // ボールから逃げ切らないようにするため、速度の0.5倍に制限
+          command.setMaxVelocity(ball_speed * 0.5);
+          // ボール速度方向に速度の0.5倍だけオフセット（1m/sで近づいていたら0.5m）
+          return world_model()->ball.vel.normalized() * (world_model()->ball.vel.norm() * 0.5);
+        } else {
+          return Point(0, 0);
+        }
+      } else {
+        return Point(0, 0);
+      }
+    }();
+    auto interception_point = getInterceptionPoint() + offset;
+    command.lookAtBallFrom(interception_point)
+      .setDribblerTargetPosition(interception_point)
+      .dribble(getParameter<double>("dribble_power"))
+      .disableBallAvoidance();
+
+    return Status::RUNNING;
+  }
+
+  void print(std::ostream & os) const override { os << "[Receive]"; }
+
+  Point getInterceptionPoint() const
+  {
+    std::string policy = getParameter<std::string>("policy");
+    if (policy.ends_with("slack")) {
+      auto [max_slack_point, max_slack] = world_model()->getMinMaxSlackInterceptPoint();
+      if (policy == "max_slack" && max_slack_point) {
+        return max_slack_point.value();
+      } else if (policy == "min_slack" && max_slack_point) {
+        return max_slack_point.value();
+      }
+      return world_model()->ball.pos;
+    } else if (policy == "closest") {
+      Segment ball_line(
+        world_model()->ball.pos,
+        (world_model()->ball.pos + world_model()->ball.vel.normalized() * 10.0));
+      auto result = getClosestPointAndDistance(robot()->pose.pos, ball_line);
+      return result.closest_point;
+    } else {
+      throw std::runtime_error("Invalid policy for Receive::getInterceptionPoint: " + policy);
+    }
+  }
+};
+}  // namespace crane::skills
+#endif  // CRANE_ROBOT_SKILLS__RECEIVE_HPP_

crane_robot_skills/include/crane_robot_skills/redirect.hpp

@@ -0,0 +1,78 @@
+// Copyright (c) 2023 ibis-ssl
+//
+// Use of this source code is governed by an MIT-style
+// license that can be found in the LICENSE file or at
+// https://opensource.org/licenses/MIT.
+
+#ifndef CRANE_ROBOT_SKILLS__REDIRECT_HPP_
+#define CRANE_ROBOT_SKILLS__REDIRECT_HPP_
+
+#include <crane_basics/eigen_adapter.hpp>
+#include <crane_robot_skills/skill_base.hpp>
+#include <memory>
+
+namespace crane::skills
+{
+class Redirect : public SkillBase<RobotCommandWrapperPosition>
+{
+public:
+  explicit Redirect(RobotCommandWrapperBase::SharedPtr & base) : SkillBase("Redirect", base)
+  {
+    setParameter("kick_power", 0.3);
+    setParameter("kick_with_chip", false);
+    setParameter("redirect_target", Point(0, 0));
+    // min_slack, max_slack, closest
+    setParameter("policy", std::string("closest"));
+  }
+
+  Status update([[maybe_unused]] const ConsaiVisualizerWrapper::SharedPtr & visualizer) override
+  {
+    auto interception_point = getInterceptionPoint();
+    auto target_angle = getRobotAngle(
+      world_model()->ball.pos, world_model()->ball.vel, interception_point,
+      getParameter<Point>("redirect_target"));
+    command.setDribblerTargetPosition(interception_point, target_angle);
+    command.liftUpDribbler().disableBallAvoidance();
+    if (getParameter<bool>("kick_with_chip")) {
+      command.kickWithChip(getParameter<double>("kick_power"));
+    } else {
+      command.kickStraight(getParameter<double>("kick_power"));
+    }
+    return Status::RUNNING;
+  }
+
+  void print(std::ostream & os) const override { os << "[Receive]"; }
+
+  Point getInterceptionPoint() const
+  {
+    std::string policy = getParameter<std::string>("policy");
+    if (policy.ends_with("slack")) {
+      auto [max_slack_point, max_slack] = world_model()->getMinMaxSlackInterceptPoint();
+      if (policy == "max_slack" && max_slack_point) {
+        return max_slack_point.value();
+      } else if (policy == "min_slack" && max_slack_point) {
+        return max_slack_point.value();
+      }
+      return world_model()->ball.pos;
+    } else if (policy == "closest") {
+      Segment ball_line(
+        world_model()->ball.pos,
+        (world_model()->ball.pos + world_model()->ball.vel.normalized() * 10.0));
+      auto result = getClosestPointAndDistance(robot()->pose.pos, ball_line);
+      return result.closest_point;
+    } else {
+      throw std::runtime_error("Invalid policy for Receive::getInterceptionPoint: " + policy);
+    }
+  }
+
+  double getRobotAngle(
+    Point ball_pos, Velocity ball_vel, Point interception_point, Point redirect_target) const
+  {
+    Vector2 to_ball = ball_pos - interception_point;
+    Vector2 to_target = redirect_target - interception_point;
+    // ボールとターゲットの角度の中間角を求める（暫定実装）
+    return getIntermediateAngle(getAngle(to_ball), getAngle(to_target));
+  }
+};
+}  // namespace crane::skills
+#endif  // CRANE_ROBOT_SKILLS__REDIRECT_HPP_

crane_robot_skills/include/crane_robot_skills/simple_attacker.hpp

@@ -37,8 +37,6 @@ class SimpleAttacker : public SkillBaseWithState<SimpleAttackerState, RobotComma
 
   bool isBallComingFromBack(double ball_vel_threshold = 0.5) const;
 
-  std::vector<std::pair<Point, double>> getBallSequence(double t_horizon, double t_step) const;
-
   Point & kick_target;
 
   Kick kick_skill;

crane_robot_skills/include/crane_robot_skills/skills.hpp

@@ -16,6 +16,8 @@
 #include <crane_robot_skills/kickoff_attack.hpp>
 #include <crane_robot_skills/kickoff_support.hpp>
 #include <crane_robot_skills/marker.hpp>
+#include <crane_robot_skills/receive.hpp>
+#include <crane_robot_skills/redirect.hpp>
 #include <crane_robot_skills/simple_kickoff.hpp>
 #include <crane_robot_skills/steal_ball.hpp>
 // #include <crane_robot_skills/move_to_geometry.hpp>

crane_robot_skills/src/get_ball_contact.cpp

@@ -25,8 +25,9 @@ Status GetBallContact::update(
     return Status::SUCCESS;
   } else {
     auto approach_vec = getApproachNormVec();
-    command.setDribblerTargetPosition(world_model()->ball.pos + approach_vec * 0.05);
-    command.setTargetTheta(getAngle(world_model()->ball.pos - robot()->pose.pos));
+    command.setDribblerTargetPosition(
+      world_model()->ball.pos + approach_vec * 0.05,
+      getAngle(world_model()->ball.pos - robot()->pose.pos));
     command.dribble(getParameter<double>("dribble_power"));
     command.disableBallAvoidance();
     return Status::RUNNING;

crane_robot_skills/src/move_with_ball.cpp

@@ -33,9 +33,8 @@ Status MoveWithBall::update([[maybe_unused]] const ConsaiVisualizerWrapper::Shar
 {
   command.setMaxVelocity(0.5);
   Point target_pos = parseTargetPoint();
-  command.setDribblerTargetPosition(target_pos);
   target_theta = getAngle(target_pos - robot()->pose.pos);
-  command.setTargetTheta(target_theta);
+  command.setDribblerTargetPosition(target_pos, target_theta);
   command.disableBallAvoidance();
   // 開始時にボールに接していることが前提にある
   if (not robot()->ball_contact.findPastContact(getParameter<double>("ball_lost_timeout"))) {

crane_robot_skills/src/robot_command_as_skill.cpp

@@ -128,6 +128,7 @@ CmdSetDribblerTargetPosition::CmdSetDribblerTargetPosition(
 {
   setParameter("x", 0.0);
   setParameter("y", 0.0);
+  setParameter("theta", 0.0);
   setParameter("reach_threshold", 0.1);
   setParameter("exit_immediately", false);
 }
@@ -136,7 +137,7 @@ Status CmdSetDribblerTargetPosition::update(
   [[maybe_unused]] const ConsaiVisualizerWrapper::SharedPtr & visualizer)
 {
   Point target{getParameter<double>("x"), getParameter<double>("y")};
-  command.setDribblerTargetPosition(target);
+  command.setDribblerTargetPosition(target, getParameter<double>("theta"));
   if (getParameter<bool>("exit_immediately")) {
     return Status::SUCCESS;
   } else {

crane_robot_skills/src/simple_attacker.cpp

@@ -44,6 +44,7 @@ SimpleAttacker::SimpleAttacker(RobotCommandWrapperBase::SharedPtr & base)
           }
           return target;
         } else {
+          // シュートの隙があるときはシュート
           return world_model()->ball.pos + getNormVec(best_angle) * 0.5;
         }
       }();
@@ -82,20 +83,28 @@ SimpleAttacker::SimpleAttacker(RobotCommandWrapperBase::SharedPtr & base)
     SimpleAttackerState::RECEIVE_APPROACH,
     [this]([[maybe_unused]] const ConsaiVisualizerWrapper::SharedPtr & visualizer) -> Status {
       std::cout << "RECEIVE_APPROACH" << std::endl;
+      auto [min_slack_point, max_slack_point] = world_model()->getMinMaxSlackInterceptPoint();
       auto ball_pos = world_model()->ball.pos;
       auto [closest_point, distance] = [&]() {
         Segment ball_line{ball_pos, ball_pos + world_model()->ball.vel * 10.0};
         auto result = getClosestPointAndDistance(robot()->pose.pos, ball_line);
         return std::make_pair(result.closest_point, result.distance);
       }();
       // 立ちふさがるように経由ポイント
-      Point target_point = ball_pos + world_model()->ball.vel.normalized() *
-                                        (distance / (robot()->vel.linear.norm() + 0.5) +
-                                         world_model()->ball.vel.norm() * 0.5 + 0.3);
-      command.setTargetPosition(target_point)
+      //      Point target_point = ball_pos + world_model->ball.vel.normalized() *
+      //                                        (distance / (robot()->vel.linear.norm() + 0.5) +
+      //                                         world_model->ball.vel.norm() * 0.5 + 0.3);
+      Point target = [&]() -> Point {
+        if (min_slack_point) {
+          return min_slack_point.value();
+        } else {
+          return (ball_pos + world_model()->ball.vel.normalized() * 0.5);
+        }
+      }();
+      command.setTargetPosition(target)
         .setTargetTheta([&]() {
-          auto to_target = (kick_target - target_point).normalized();
-          auto to_ball = (world_model()->ball.pos - target_point).normalized();
+          auto to_target = (kick_target - target).normalized();
+          auto to_ball = (world_model()->ball.pos - target).normalized();
           // 0.5m/sのときにボールとゴールの中間方向を向く
           // ボールが速いとよりボールの方向を向く
           return getAngle(to_target + 2.0 * world_model()->ball.vel.norm() * to_ball);
@@ -115,6 +124,7 @@ SimpleAttacker::SimpleAttacker(RobotCommandWrapperBase::SharedPtr & base)
     [this]([[maybe_unused]] const ConsaiVisualizerWrapper::SharedPtr & visualizer) -> Status {
       std::cout << "NORMAL_APPROACH" << std::endl;
       Point ball_pos = world_model()->ball.pos + world_model()->ball.vel * 0.0;
+      auto [min_slack_point, max_slack_point] = world_model()->getMinMaxSlackInterceptPoint();
       kick_skill.setParameter("target", kick_target);
       kick_skill.setParameter("kick_power", 0.8);
       kick_skill.run(visualizer);

crane_robot_skills/src/steal_ball.cpp

@@ -64,16 +64,18 @@ StealBall::StealBall(RobotCommandWrapperBase::SharedPtr & base)
       command.disableCollisionAvoidance();
       const auto method = getParameter<std::string>("steal_method");
       if (method == "front") {
-        command.setDribblerTargetPosition(world_model()->ball.pos);
+        command.setDribblerTargetPosition(
+          world_model()->ball.pos, getAngle(world_model()->ball.pos - robot()->pose.pos));
         command.dribble(0.5);
       } else if (method == "side") {
-        command.setDribblerTargetPosition(world_model()->ball.pos);
         if (robot()->getDistance(world_model()->ball.pos) < (0.085 + 0.000)) {
           // ロボット半径より近くに来れば急回転して刈り取れる
           command.setTargetTheta(getAngle(world_model()->ball.pos - robot()->pose.pos) + M_PI / 2);
         } else {
           command.setTargetTheta(getAngle(world_model()->ball.pos - robot()->pose.pos));
         }
+        // thetaに依存するので後置
+        command.setDribblerTargetPosition(world_model()->ball.pos);
       }
       return Status::RUNNING;
     });
@@ -173,10 +175,9 @@ StealBall::StealBall(RobotCommandWrapperBase::SharedPtr & base)
     auto to_goal = getNormVec(goal_angle);
     auto to_ball = (world_model()->ball.pos - across_point).normalized();
     double intermediate_angle = getAngle(2 * to_goal + to_ball);
-    command.setTargetTheta(intermediate_angle);
     command.liftUpDribbler();
     command.kickStraight(getParameter<double>("kicker_power"));
-    command.setDribblerTargetPosition(across_point);
+    command.setDribblerTargetPosition(across_point, intermediate_angle);
 
     return Status::RUNNING;
   });

crane_simple_ai/src/crane_commander.cpp

@@ -77,6 +77,8 @@ CraneCommander::CraneCommander(QWidget * parent) : QMainWindow(parent), ui(new U
   setUpSkillDictionary<skills::MoveWithBall>();
   //  setUpSkillDictionary<skills::TurnAroundPoint>();
   setUpSkillDictionary<skills::Sleep>();
+  setUpSkillDictionary<skills::Receive>();
+  setUpSkillDictionary<skills::Redirect>();
   setUpSkillDictionary<skills::GoOverBall>();
   setUpSkillDictionary<skills::SimpleAttacker>();
   setUpSkillDictionary<skills::SimpleKickOff>();

utility/crane_basics/include/crane_basics/ball_model.hpp

@@ -22,5 +22,30 @@ inline std::optional<Point> getFutureBallPosition(
     return ball_pos + ball_vel * t - 0.5 * t * t * deceleration * ball_vel.normalized();
   }
 }
+
+inline std::vector<double> generateSequence(double start, double end, double step)
+{
+  int size = (end - start) / step + 1;
+  std::vector<double> sequence(size);
+  double current = start;
+  std::generate_n(sequence.begin(), size, [&current, step]() mutable {
+    double temp = current;
+    current += step;
+    return temp;
+  });
+  return sequence;
+}
+
+inline std::vector<std::pair<Point, double>> getBallSequence(
+  double t_horizon, double t_step, Point ball_pos, Point ball_vel)
+{
+  std::vector<double> t_ball_sequence = generateSequence(0.0, t_horizon, t_step);
+  std::vector<std::pair<Point, double>> ball_sequence;
+  for (auto t_ball : t_ball_sequence) {
+    auto p_ball = getFutureBallPosition(ball_pos, ball_vel, t_ball);
+    ball_sequence.push_back({p_ball.value(), t_ball});
+  }
+  return ball_sequence;
+}
 }  // namespace crane
 #endif  // CRANE_BASICS__BALL_MODEL_HPP_

utility/crane_basics/include/crane_basics/robot_info.hpp

@@ -44,7 +44,12 @@ struct RobotInfo
 
   using SharedPtr = std::shared_ptr<RobotInfo>;
 
-  [[nodiscard]] Vector2 center_to_kicker() const { return getNormVec(pose.theta) * 0.090; }
+  [[nodiscard]] double getDribblerDistance() const { return 0.090; }
+
+  [[nodiscard]] Vector2 center_to_kicker() const
+  {
+    return getNormVec(pose.theta) * getDribblerDistance();
+  }
 
   [[nodiscard]] Point kicker_center() const { return pose.pos + center_to_kicker(); }
 

utility/crane_basics/include/crane_basics/travel_time.hpp

@@ -19,7 +19,7 @@ inline double getTravelTime(std::shared_ptr<RobotInfo> robot, Point target)
 }
 
 inline double getTravelTimeTrapezoidal(
-  std::shared_ptr<RobotInfo> robot, Point target, const double max_acceleration = 4.,
+  std::shared_ptr<RobotInfo> robot, Point target, const double max_acceleration = 2.,
   const double max_velocity = 4.)
 {
   double distance = (target - robot->pose.pos).norm();

utility/crane_msg_wrappers/include/crane_msg_wrappers/robot_command_wrapper.hpp

@@ -299,7 +299,14 @@ class RobotCommandWrapperPosition : public RobotCommandWrapperCommon<RobotComman
 
   RobotCommandWrapperPosition & setDribblerTargetPosition(Point position)
   {
-    return setTargetPosition(position - command->robot->center_to_kicker());
+    double theta = command->latest_msg.target_theta;
+    return setDribblerTargetPosition(position, theta);
+  }
+
+  RobotCommandWrapperPosition & setDribblerTargetPosition(Point position, double theta)
+  {
+    return setTargetPosition(
+      position + getNormVec(theta + M_PI) * getRobot()->getDribblerDistance(), theta);
   }
 
   RobotCommandWrapperPosition & setTargetPosition(Point position)