Is done planners

config/plays.txt

@@ -8,4 +8,5 @@ penalty_defense.PenaltyDefense()
 penalty_offense.PenaltyOffense()
 test_motion_planning.OneRobot()
 test_motion_planning.AllBots()
+test_motion_planning.KickBall()
 None

rj_common/include/rj_common/utils.hpp

@@ -21,6 +21,21 @@ static inline T fix_angle_radians(T a) {
     return remainder(a, 2 * M_PI);
 }
 
+/*
+ * @brief Approximate moving average with first-order low pass filter.
+ *
+ * @details For those without a digital signal processing background, think of
+ * it as giving small weights to new estimates compared to the existing belief.
+ * For example, to approximate the avg velocity, with a gain of 0.8, we can do:
+ *      new_avg_vel = (0.8 * old_avg_vel) + (0.2 * most_recent_vel)
+ * every time a new velocity comes in. This protects us from noisy velocity
+ * readings while also updating the average as it changes.
+ *
+ * @param old_value current value of moving average
+ * @param new_value new value to add to moving average
+ * @param gain resistance to noise (higher = more weight to old_value)
+ * @return new moving average estimate
+ */
 template <typename T>
 inline T apply_low_pass_filter(const T& old_value, const T& new_value, double gain) {
     return gain * new_value + (1 - gain) * old_value;

rj_gameplay/rj_gameplay/play/test_motion_planning.py

@@ -8,7 +8,7 @@
 import stp.role.cost
 from rj_msgs.msg import RobotIntent
 
-from rj_gameplay.skill import move
+from rj_gameplay.skill import move, pivot_kick
 
 
 class State(Enum):
@@ -159,3 +159,44 @@ def tick(
                 intents[i] = self._move_skills[i].tick(world_state)
 
         return intents
+
+
+class KickBall(stp.play.Play):
+    """
+    Make robot 0 capture, then kick the ball.
+
+    Directly overrides the STP architecture to send RI to gameplay.
+    """
+
+    def __init__(self):
+        super().__init__()
+        self._state = "capturing"
+        self.capture_skill = None
+        self.pivot_kick_skill = None
+
+        self.robot_id = 0
+        self.robot = None
+
+    def tick(
+        self,
+        world_state: stp.rc.WorldState,
+    ) -> List[RobotIntent]:
+
+        self.robot = world_state.our_robots[self.robot_id]
+        intents = [None for _ in range(16)]
+        my_robot_intent = None
+
+        # None on INIT and state changes
+        if self.pivot_kick_skill is None:
+            self.pivot_kick_skill = pivot_kick.PivotKick(
+                robot=self.robot,
+                target_point=np.array([0.0, 6.0]),
+                chip=True,
+                kick_speed=5.0,
+            )
+        else:
+            my_robot_intent = self.pivot_kick_skill.tick(world_state)
+
+        intents[self.robot_id] = my_robot_intent
+
+        return intents

rj_gameplay/rj_gameplay/role/seeker.py

@@ -32,7 +32,9 @@ def get_open_point(self, world_state, region: List, centroid) -> np.ndarray:
         # line-of-sight calculations
         # (LOS is maximized by making dot product of angle between unit vectors as close to 0 as possible)
         ball_dir = lambda pos: pos - world_state.ball.pos
-        ball_dir_norm = lambda pos: ball_dir(pos) / np.linalg.norm(ball_dir(pos))
+        ball_dir_norm = lambda pos: ball_dir(pos) / (
+            1e-9 + np.linalg.norm(ball_dir(pos))
+        )
         opps_to_ball = [
             ball_dir_norm(opp_robot.pose[0:2]) for opp_robot in world_state.their_robots
         ]

rj_gameplay/rj_gameplay/skill/capture.py

@@ -1,20 +1,14 @@
-from abc import ABC, abstractmethod
-
-import rj_gameplay.eval as eval
 import argparse
-import py_trees
 import sys
 import time
-from rj_msgs.msg import RobotIntent, CollectMotionCommand
+from typing import Optional
 
-import stp.skill as skill
-import stp.role as role
+import numpy as np
 import stp.action as action
 import stp.rc as rc
-from typing import Optional
-import numpy as np
-
-from stp.utils.constants import RobotConstants
+import stp.skill as skill
+from rj_msgs.msg import CollectMotionCommand, RobotIntent
+from stp.utils.constants import BallConstants, RobotConstants
 
 
 class Capture(skill.Skill):
@@ -46,8 +40,10 @@ def is_done(self, world_state) -> bool:
 
         dist_to_ball = np.linalg.norm(robot_pos - ball_pos)
 
-        ball_slow = ball_speed < 1.0
-        ball_close = dist_to_ball < RobotConstants.RADIUS * 1.3
+        ball_slow = ball_speed < 0.05
+        ball_close = (
+            dist_to_ball - (RobotConstants.RADIUS + BallConstants.RADIUS) < 0.03
+        )
 
         return ball_slow and ball_close
 

rj_gameplay/rj_gameplay/skill/line_kick.py

@@ -76,9 +76,9 @@ def is_done(self, world_state: rc.WorldState):
         KICK_DOT_THRESHOLD = 0.4
         KICK_BALL_SPEED_THRESHOLD = 0.9
 
-        # TODO: make pivot kick and line kick inherit from some common kick superclass to make this cleaner
         if self.robot is None:
             return False
+
         ball_vel_unit = world_state.ball.vel / np.linalg.norm(world_state.ball.vel)
         heading_angle = world_state.our_robots[self.robot.id].pose[2]
         heading_vect = np.array([np.cos(heading_angle), np.sin(heading_angle)])

rj_gameplay/rj_gameplay/skill/move.py

@@ -61,21 +61,22 @@ def tick(self, world_state: rc.WorldState) -> RobotIntent:
         return intent
 
     def is_done(self, world_state: rc.WorldState) -> bool:
-        threshold = 0.3
+        position_tolerance = 1e-2
+        velocity_tolerance = 1e-1
         if self.robot.id is None or world_state is None:
             return False
-        elif (
-            math.sqrt(
-                (world_state.our_robots[self.robot.id].pose[0] - self.target_point[0])
-                ** 2
-                + (world_state.our_robots[self.robot.id].pose[1] - self.target_point[1])
-                ** 2
-            )
-            < threshold
-        ):
-            return True
-        else:
-            return False
+
+        # only linear for now (so this is technically not pose, which
+        # includes angular position)
+        robot_pt = world_state.our_robots[self.robot.id].pose[:2]
+        goal_pt = self.target_point
+        robot_vel = world_state.our_robots[self.robot.id].twist[:2]
+        goal_vel = self.target_vel
+
+        return (
+            np.linalg.norm(robot_pt - goal_pt) < position_tolerance
+            and np.linalg.norm(robot_vel - goal_vel) < velocity_tolerance
+        )
 
     def __str__(self):
         ignore_ball_str = ", ignoring ball" if self.ignore_ball else ""

rj_gameplay/rj_gameplay/skill/pivot.py

@@ -16,7 +16,7 @@ def __init__(
         pivot_point: np.ndarray = None,
         target_point: np.ndarray = None,
         dribble_speed: float = 1.0,
-        threshold: float = 0.2,
+        threshold: float = 0.05,
         priority: int = 1,
     ):
         self.robot = robot
@@ -48,7 +48,6 @@ def is_done(self, world_state: rc.WorldState) -> bool:
         if self.robot is None:
             return False
         angle_threshold = self.threshold
-        angle_threshold = 0.05
         stopped_threshold = (
             5 * self.threshold
         )  # We don't _really_ care about this when we're kicking, if not for latency

rj_gameplay/rj_gameplay/skill/receive.py

@@ -1,19 +1,14 @@
-from abc import ABC, abstractmethod
-
-import rj_gameplay.eval as eval
 import argparse
-import py_trees
 import sys
 import time
-import numpy as np
 from typing import Optional
 
-import stp.skill as skill
-import stp.role as role
-from rj_gameplay.skill import settle, capture
-from rj_msgs.msg import RobotIntent, SettleMotionCommand
 import stp.rc as rc
+import stp.skill as skill
 from rj_msgs import msg
+from rj_msgs.msg import RobotIntent
+
+from rj_gameplay.skill import capture, settle
 
 
 class Receive(skill.Skill):
@@ -38,7 +33,6 @@ def tick(self, world_state: rc.WorldState) -> RobotIntent:
             return self.settle.tick(world_state)
 
     def is_done(self, world_state: rc.WorldState) -> bool:
-
         return self.capture.is_done(world_state)
 
     def __str__(self):

rj_geometry/include/rj_geometry/point.hpp

@@ -1,14 +1,16 @@
 #pragma once
 
-#include <rj_protos/Point.pb.h>
+#include <cmath>
+#include <sstream>
+#include <string>
 
 #include <Eigen/Dense>
 #include <QtCore/QPointF>
 #include <boost/functional/hash.hpp>
-#include <cmath>
+
+#include <rj_geometry/util.hpp>
 #include <rj_geometry_msgs/msg/point.hpp>
-#include <sstream>
-#include <string>
+#include <rj_protos/Point.pb.h>
 
 namespace rj_geometry {
 
@@ -375,7 +377,29 @@ class Point {
         return acos(std::max(std::min(angle, 1.0), -1.0));
     }
 
-    [[nodiscard]] bool nearly_equals(Point other) const;
+    /*
+     * @brief Return true if this point is close enough to another Point.
+     *
+     * @param other Point to compare with this point
+     * @tolerance maximum allowed difference in x/y coord to be considered equal
+     * @return true if x/y of this Point are both less than tolerance away from other
+     */
+    [[nodiscard]] bool nearly_equals(Point other, double tolerance = 1e-4) const {
+        return nearly_equal(static_cast<float>(x()), static_cast<float>(other.x()), tolerance) &&
+               nearly_equal(static_cast<float>(y()), static_cast<float>(other.y()), tolerance);
+    }
+
+    /*
+     * @brief Return true if points a and b are close enough to each other.
+     *
+     * @param a Point a
+     * @param b Point b
+     * @tolerance maximum allowed difference in x/y coord to be considered equal
+     * @return true if x/y of Point a are both less than tolerance away from Point b
+     */
+    static bool nearly_equals(const Point& a, const Point& b, double tolerance = 1e-4) {
+        return a.nearly_equals(b, tolerance);
+    }
 
     [[nodiscard]] std::string to_string() const {
         std::stringstream str;

rj_geometry/include/rj_geometry/util.hpp

@@ -34,8 +34,14 @@ static inline T sign(T f) {
 }
 
 // TODO(1485): Make this smaller once we figure out why test are failing in O3.
-// static const float FLOAT_EPSILON = 0.00001;
-static const float kFloatEpsilon = 1e-4;
-static bool nearly_equal(float a, float b) {
-    return std::fabs(a - b) < kFloatEpsilon;
+/*
+ * @brief Return true if doubles a and b are close enough to each other.
+ *
+ * @param a double a
+ * @param b double b
+ * @tolerance maximum allowed difference in x/y coord to be considered equal
+ * @return true if x/y of double a are both less than tolerance away from double b
+ */
+static bool nearly_equal(double a, double b, double tolerance = 1e-4) {
+    return std::fabs(a - b) < tolerance;
 }

rj_geometry/src/point.cpp

@@ -2,8 +2,6 @@
 #include <rj_geometry/util.hpp>
 
 namespace rj_geometry {
-bool Point::nearly_equals(Point other) const {
-    return nearly_equal(static_cast<float>(x()), static_cast<float>(other.x())) &&
-           nearly_equal(static_cast<float>(y()), static_cast<float>(other.y()));
-}
+// TODO(Kevin): delete this file
+// did not do it yet because build system make me sad
 }  // namespace rj_geometry

rj_geometry/testing/point_test.cpp

@@ -203,14 +203,17 @@ TEST(Point, normalized) {
 }
 
 TEST(Point, nearly_equals) {
-    EXPECT_TRUE(Point(0, 5).nearly_equals(Point(0, 5)));
-    EXPECT_TRUE(Point(0, 5).nearly_equals(Point(0 - kFloatEpsilon / 2, 5 + kFloatEpsilon / 2)));
-    EXPECT_TRUE(Point(0, 5).nearly_equals(Point(0 + kFloatEpsilon / 2, 5 - kFloatEpsilon / 2)));
-
-    EXPECT_FALSE(
-        Point(0, 5).nearly_equals(Point(0 - kFloatEpsilon * 1.1, 5 + kFloatEpsilon * 1.1)));
-    EXPECT_FALSE(
-        Point(0, 5).nearly_equals(Point(0 + kFloatEpsilon * 1.1, 5 - kFloatEpsilon * 1.1)));
+    double test_tolerance = 1e-4;
+    EXPECT_TRUE(Point(0, 5).nearly_equals(Point(0, 5), test_tolerance));
+    EXPECT_TRUE(Point(0, 5).nearly_equals(Point(0 - test_tolerance / 2, 5 + test_tolerance / 2),
+                                          test_tolerance));
+    EXPECT_TRUE(Point(0, 5).nearly_equals(Point(0 + test_tolerance / 2, 5 - test_tolerance / 2),
+                                          test_tolerance));
+
+    EXPECT_FALSE(Point(0, 5).nearly_equals(
+        Point(0 - test_tolerance * 1.1, 5 + test_tolerance * 1.1), test_tolerance));
+    EXPECT_FALSE(Point(0, 5).nearly_equals(
+        Point(0 + test_tolerance * 1.1, 5 - test_tolerance * 1.1), test_tolerance));
 }
 // TODO(ashaw596) Add tests for angle_to and angle_between once those changes are
 // merged

soccer/src/soccer/planning/instant.hpp

@@ -28,6 +28,22 @@ struct LinearMotionInstant {
     rj_geometry::Point position;
     rj_geometry::Point velocity;
 
+    /**
+     * @brief Return true if LinearMotionInstant a and b are nearly equal.
+     *
+     * @param a LinearMotionInstant a
+     * @param b LinearMotionInstant b
+     * @param position_tolerance tolerance on position component of LinearMotionInstant
+     * @param velocity_tolerance tolerance on velocity component of LinearMotionInstant
+     * @return True if a and b are within position_tolerance of each other AND velocity_tolerance of
+     * each other
+     */
+    static bool nearly_equals(const LinearMotionInstant& a, const LinearMotionInstant& b,
+                              double position_tolerance = 1e-4, double velocity_tolerance = 1e-4) {
+        return rj_geometry::Point::nearly_equals(a.position, b.position, position_tolerance) &&
+               rj_geometry::Point::nearly_equals(a.velocity, b.velocity, velocity_tolerance);
+    }
+
     friend std::ostream& operator<<(std::ostream& stream,
                                     const LinearMotionInstant& instant) {
         return stream << "LinearMotionInstant(position=" << instant.position