Rotate Kick Development

docs/source/tutorial.rst

@@ -351,6 +351,7 @@ but here are some more hints.
 * The motion command for driving in a straight line is :cpp:`"path_target"`.
 * You will probably need to override some methods relating to passing, but you can leave their implementations empty. They don't need to do anything in your position, as your robot will not pass the ball
 * The simulator tells you the coordinates of your cursor—these are the same coordinates you can use in your motion commands.
+* You will need to add the new file name you create to ``soccer/src/soccer/CMakeLists.txt``. See how this is done for other positions.
 
 Testing
 ~~~~~~~
@@ -668,4 +669,4 @@ Here are all the external links from this document, copied again for your easy r
 
 * `C++ Inheritance`_ 
 
-.. _C++ Inheritance: https://www.learncpp.com/cpp-tutorial/basic-inheritance-in-c/
+.. _C++ Inheritance: https://www.learncpp.com/cpp-tutorial/basic-inheritance-in-c/

install/env.sh

@@ -0,0 +1,5 @@
+# Here are some useful environment variables for setting up a consistent ROS environment 
+# some information on this can be found here: https://docs.ros.org/en/humble/Tutorials/Beginner-CLI-Tools/Configuring-ROS2-Environment.html 
+export ROS_DOMAIN_ID=19 # this is a random number, it's just important to keep it consistent across shells 
+export ROS_LOCALHOST_ONLY=1 # for our stack specifically, this is quite helpful. 
+

install/setup.bash

@@ -34,7 +34,7 @@ _pythonpath_add() {
 # No direct replacement for the command exists, and in Ubuntu 22.04,
 # this script will produce a warning about this. We may need to find a
 # replacement library in the future.
-_PYTHON_LIB_PATH=$(python3 -c "from distutils.sysconfig import get_python_lib; print(get_python_lib(prefix='${_INSTALL_PATH}'))")
+_PYTHON_LIB_PATH=$(python3 -c "import sysconfig; print(sysconfig.get_path('purelib'))")
 _pythonpath_add "${_PYTHON_LIB_PATH}"
 unset _PYTHON_LIB_PATH
 

install/setup.zsh

@@ -30,7 +30,7 @@ _pythonpath_add() {
     fi
 }
 
-_PYTHON_LIB_PATH=$(python3 -c "from distutils.sysconfig import get_python_lib; print(get_python_lib(prefix='${_INSTALL_PATH}'))")
+_PYTHON_LIB_PATH=$(python3 -c "import sysconfig; print(sysconfig.get_path('purelib'))")
 _pythonpath_add "${_PYTHON_LIB_PATH}"
 unset _PYTHON_LIB_PATH
 

launch/framework.bash

@@ -10,7 +10,7 @@ if [ -z "$binary" ]; then
 fi
 
 # Run the binary in the background
-"$binary" &
+"$binary" -g 2020B --realism RC2021 &
 binary_pid=$!
 
 # Ensure that pressing Ctrl+C kills all subprocesses

makefile

@@ -72,11 +72,11 @@ run-sim:
 # run our stack with default flags
 # TODO: actually name our software stack something
 run-our-stack:
-	ROS_LOCALHOST_ONLY=1 ros2 launch rj_robocup soccer.launch.py run_sim:=True
+	ros2 launch rj_robocup soccer.launch.py run_sim:=True
 
 # run sim with external referee (SSL Game Controller)
 run-sim-external:
-	ROS_LOCALHOST_ONLY=1 ros2 launch rj_robocup soccer.launch.py run_sim:=True use_internal_ref:=False
+	ros2 launch rj_robocup soccer.launch.py run_sim:=True use_internal_ref:=False
 
 run-sim-ex: run-sim-external
 
@@ -95,7 +95,7 @@ run-manual:
 
 # same as run-real, with different server port
 run-alt-real:
-	ROS_DOMAIN_ID=2 ros2 launch rj_robocup soccer.launch.py run_sim:=False use_sim_radio:=False server_port:=25564 use_internal_ref:=False team_name:=AltRoboJackets team_flag:=-b
+	ros2 launch rj_robocup soccer.launch.py run_sim:=False use_sim_radio:=False server_port:=25564 use_internal_ref:=False team_name:=AltRoboJackets team_flag:=-b
 
 # run sim2play (requires external referee)
 run-sim2play:

soccer/src/soccer/CMakeLists.txt

@@ -40,6 +40,7 @@ set(ROBOCUP_LIB_SRC
     planning/planner/path_target_path_planner.cpp
     planning/planner/pivot_path_planner.cpp
     planning/planner/line_pivot_path_planner.cpp
+    planning/planner/rotate_path_planner.cpp
     planning/planner/plan_request.cpp
     planning/planner/settle_path_planner.cpp
     planning/planner/goalie_idle_path_planner.cpp

soccer/src/soccer/control/trapezoidal_motion.cpp

@@ -112,9 +112,9 @@ double Trapezoidal::get_time(double distance, double path_length, double max_spe
     }
 }
 
-bool trapezoidal_motion(double path_length, double max_speed, double max_acc, double time_into_lap,
-                        double start_speed, double final_speed, double& pos_out,
-                        double& speed_out) {
+bool Trapezoidal::trapezoidal_motion(double path_length, double max_speed, double max_acc,
+                                     double time_into_lap, double start_speed, double final_speed,
+                                     double& pos_out, double& speed_out) {
     // begin by assuming that there's enough time to get up to full speed
     // we do this by calculating the full ramp-up and ramp-down, then seeing
     // if the distance travelled is too great.  If it's gone too far, this is

soccer/src/soccer/control/trapezoidal_motion.hpp

@@ -1,23 +1,5 @@
 #pragma once
 
-/**
- * This function evaluates a control situation using "bang-bang". The robot
- * accelerates at maxAcceleration until it hits max speed, then decelerates at
- * max acc to the end point.
- *
- * @param[in] 	pathLength The total distance we are trying to travel
- * @param[in] 	timeIntoLap How long since we started moving along the trapezoid
- * @param[in] 	finalSpeed The speed we'd like to be going at the end of the
- * trapezoid
- * @param[out] 	distOut The distance to be at at the given time
- * @param[out] 	speedOut The speed to be at at the given time
- * @return		true if the trapezoid is valid at the given time, false
- * otherwise
- */
-bool trapezoidal_motion(double path_length, double max_speed, double max_acc,
-                       double time_into_lap, double start_speed, double final_speed,
-                       double& pos_out, double& speed_out);
-
 namespace Trapezoidal {
 /**
  * Estimates how long it would take to move to a certain distance down a path
@@ -37,4 +19,21 @@ namespace Trapezoidal {
 double get_time(double distance, double path_length, double max_speed,
                double max_acc, double start_speed, double final_speed);
 
+/**
+ * This function evaluates a control situation using "bang-bang". The robot
+ * accelerates at maxAcceleration until it hits max speed, then decelerates at
+ * max acc to the end point.
+ *
+ * @param[in] 	pathLength The total distance we are trying to travel
+ * @param[in] 	timeIntoLap How long since we started moving along the trapezoid
+ * @param[in] 	finalSpeed The speed we'd like to be going at the end of the
+ * trapezoid
+ * @param[out] 	distOut The distance to be at at the given time
+ * @param[out] 	speedOut The speed to be at at the given time
+ * @return		true if the trapezoid is valid at the given time, false
+ * otherwise
+ */
+bool trapezoidal_motion(double path_length, double max_speed, double max_acc, double time_into_lap,
+                        double start_speed, double final_speed, double& pos_out, double& speed_out);
+
 }  // namespace Trapezoidal

soccer/src/soccer/control/trapezoidal_motion_test.cpp

@@ -16,8 +16,8 @@ bool trapezoid1(double t, double& pos_out, double& speed_out) {
     double start_speed = 0;
     double final_speed = 0;
 
-    bool valid = trapezoidal_motion(path_length, max_speed, max_acc, t, start_speed, final_speed,
-                                    pos_out, speed_out);
+    bool valid = Trapezoidal::trapezoidal_motion(path_length, max_speed, max_acc, t, start_speed,
+                                                 final_speed, pos_out, speed_out);
 
     if (valid) {
         auto time = Trapezoidal::get_time(pos_out, path_length, max_speed, max_acc, start_speed,
@@ -34,8 +34,8 @@ bool trapezoid2(double t, double& pos_out, double& speed_out) {
     double start_speed = 1;
     double final_speed = 0;
 
-    bool valid = trapezoidal_motion(path_length, max_speed, max_acc, t, start_speed, final_speed,
-                                    pos_out, speed_out);
+    bool valid = Trapezoidal::trapezoidal_motion(path_length, max_speed, max_acc, t, start_speed,
+                                                 final_speed, pos_out, speed_out);
 
     if (valid) {
         double time = Trapezoidal::get_time(pos_out, path_length, max_speed, max_acc, start_speed,
@@ -55,8 +55,8 @@ bool triangle1(double t, double& pos_out, double& speed_out) {
     double start_speed = 0;
     double final_speed = 0;
 
-    bool valid = trapezoidal_motion(path_length, max_speed, max_acc, t, start_speed, final_speed,
-                                    pos_out, speed_out);
+    bool valid = Trapezoidal::trapezoidal_motion(path_length, max_speed, max_acc, t, start_speed,
+                                                 final_speed, pos_out, speed_out);
 
     if (valid) {
         double time = Trapezoidal::get_time(pos_out, path_length, max_speed, max_acc, start_speed,
@@ -76,8 +76,8 @@ bool triangle2(double t, double& pos_out, double& speed_out) {
     double start_speed = 0;
     double final_speed = 0;
 
-    bool valid = trapezoidal_motion(path_length, max_speed, max_acc, t, start_speed, final_speed,
-                                    pos_out, speed_out);
+    bool valid = Trapezoidal::trapezoidal_motion(path_length, max_speed, max_acc, t, start_speed,
+                                                 final_speed, pos_out, speed_out);
 
     if (valid) {
         double time = Trapezoidal::get_time(pos_out, path_length, max_speed, max_acc, start_speed,
@@ -97,8 +97,8 @@ bool triangle3(double t, double& pos_out, double& speed_out) {
     double start_speed = 1;
     double final_speed = 1;
 
-    bool valid = trapezoidal_motion(path_length, max_speed, max_acc, t, start_speed, final_speed,
-                                    pos_out, speed_out);
+    bool valid = Trapezoidal::trapezoidal_motion(path_length, max_speed, max_acc, t, start_speed,
+                                                 final_speed, pos_out, speed_out);
 
     if (valid) {
         double time = Trapezoidal::get_time(pos_out, path_length, max_speed, max_acc, start_speed,
@@ -118,8 +118,8 @@ bool triangle4(double t, double& pos_out, double& speed_out) {
     double start_speed = 1;
     double final_speed = 0;
 
-    bool valid = trapezoidal_motion(path_length, max_speed, max_acc, t, start_speed, final_speed,
-                                    pos_out, speed_out);
+    bool valid = Trapezoidal::trapezoidal_motion(path_length, max_speed, max_acc, t, start_speed,
+                                                 final_speed, pos_out, speed_out);
 
     if (valid) {
         double time = Trapezoidal::get_time(pos_out, path_length, max_speed, max_acc, start_speed,

soccer/src/soccer/planning/planner/line_pivot_path_planner.cpp

@@ -51,6 +51,7 @@ LinePivotPathPlanner::State LinePivotPathPlanner::next_state(const PlanRequest&
     double vel = plan_request.world_state->get_robot(true, static_cast<int>(plan_request.shell_id))
                      .velocity.linear()
                      .mag();
+
     if (current_state_ == LINE && (target_point.dist_to(current_point) < 0.3) && (vel < 0.3) &&
         (!plan_request.play_state.is_stop())) {
         return PIVOT;

soccer/src/soccer/planning/planner/path_target_path_planner.cpp

@@ -87,6 +87,10 @@ AngleFunction PathTargetPathPlanner::get_angle_function(const PlanRequest& reque
         return AngleFns::face_angle(std::get<FaceAngle>(face_option).target);
     }
 
+    if (std::holds_alternative<FaceTarget>(face_option)) {
+        return AngleFns::face_point(request.motion_command.target.position);
+    }
+
     // default to facing tangent to path
     // (rj_convert in motion_command.hpp also follows this default)
     return AngleFns::tangent;

soccer/src/soccer/planning/planner/rotate_path_planner.cpp

@@ -0,0 +1,80 @@
+#include "rotate_path_planner.hpp"
+
+#include <memory>
+#include <vector>
+
+#include <rj_constants/constants.hpp>
+#include <rj_geometry/pose.hpp>
+#include <rj_geometry/util.hpp>
+
+#include "planning/instant.hpp"
+#include "planning/planning_params.hpp"
+#include "planning/primitives/angle_planning.hpp"
+#include "planning/primitives/path_smoothing.hpp"
+#include "planning/primitives/trapezoidal_motion.hpp"
+#include "planning/primitives/velocity_profiling.hpp"
+#include "planning/trajectory.hpp"
+
+namespace planning {
+using namespace rj_geometry;
+
+Trajectory RotatePathPlanner::plan(const PlanRequest& request) {
+    return pivot(request);  // type is Trajectory
+}
+
+bool RotatePathPlanner::is_done() const {
+    if (!cached_angle_change_) {
+        return false;
+    }
+    return abs(cached_angle_change_.value()) <
+           degrees_to_radians(static_cast<float>(kIsDoneAngleChangeThresh));
+}
+
+Trajectory RotatePathPlanner::pivot(const PlanRequest& request) {
+    const RobotInstant& start_instant = request.start;
+    const auto& linear_constraints = request.constraints.mot;
+    const auto& rotation_constraints = request.constraints.rot;
+
+    rj_geometry::ShapeSet static_obstacles;
+    std::vector<DynamicObstacle> dynamic_obstacles;
+    fill_obstacles(request, &static_obstacles, &dynamic_obstacles, false);
+
+    const MotionCommand& command = request.motion_command;
+
+    auto pivot_point =
+        request.world_state->get_robot(true, static_cast<int>(request.shell_id)).pose.position();
+    auto pivot_target = command.target.position;
+
+    double start_angle =
+        request.world_state->get_robot(true, static_cast<int>(request.shell_id)).pose.heading();
+
+    double target_angle = pivot_point.angle_to(pivot_target);
+    double angle_change = fix_angle_radians(target_angle - start_angle);
+
+    cached_angle_change_ = angle_change;
+
+    Trajectory path{};
+
+    if (abs(*cached_target_angle_ - target_angle) < degrees_to_radians(kIsDoneAngleChangeThresh)) {
+        if (cached_path_) {
+            path = cached_path_.value();
+        } else {
+            plan_angles(&path, start_instant, AngleFns::face_point(pivot_target),
+                        request.constraints.rot);
+            path.stamp(RJ::now());
+            cached_path_ = path;
+        }
+    } else {
+        cached_path_.reset();
+        plan_angles(&path, start_instant, AngleFns::face_point(pivot_target),
+                    request.constraints.rot);
+        path.stamp(RJ::now());
+        cached_path_ = path;
+    }
+
+    cached_target_angle_ = target_angle;
+
+    return path;
+}
+
+}  // namespace planning

soccer/src/soccer/planning/planner/rotate_path_planner.hpp

@@ -0,0 +1,46 @@
+#pragma once
+
+#include "path_planner.hpp"
+#include "path_target_path_planner.hpp"
+
+namespace planning {
+/**
+ * Path planner that only rotates the robot about a point given.
+ *
+ * Params taken from MotionCommand:
+ *  target.position - robot will face this point when done
+ *  target.pivot_point - robot will pivot around this point
+ */
+class RotatePathPlanner : public PathPlanner {
+public:
+    RotatePathPlanner() : PathPlanner("rotate") {}
+    ~RotatePathPlanner() override = default;
+
+    RotatePathPlanner(RotatePathPlanner&&) noexcept = default;
+    RotatePathPlanner& operator=(RotatePathPlanner&&) noexcept = default;
+    RotatePathPlanner(const RotatePathPlanner&) = default;
+    RotatePathPlanner& operator=(const RotatePathPlanner&) = default;
+
+    Trajectory plan(const PlanRequest& request) override;
+
+    void reset() override {
+        cached_target_angle_ = std::nullopt;
+        cached_angle_change_ = std::nullopt;
+    }
+    [[nodiscard]] bool is_done() const override;
+
+private:
+    Trajectory previous_;
+
+    std::optional<double> cached_target_angle_;  // equivalent to previously recorded accorded
+    std::optional<double> cached_angle_change_;
+
+    std::optional<Trajectory> cached_path_;
+
+    PathTargetPathPlanner path_target_{};
+
+    Trajectory pivot(const PlanRequest& request);
+
+    static constexpr double kIsDoneAngleChangeThresh{1.0};
+};
+}  // namespace planning

soccer/src/soccer/planning/planner_for_robot.cpp

@@ -9,6 +9,7 @@
 #include "planning/planner/line_pivot_path_planner.hpp"
 #include "planning/planner/path_target_path_planner.hpp"
 #include "planning/planner/pivot_path_planner.hpp"
+#include "planning/planner/rotate_path_planner.hpp"
 #include "planning/planner/settle_path_planner.hpp"
 
 namespace planning {
@@ -32,6 +33,7 @@ PlannerForRobot::PlannerForRobot(int robot_id, rclcpp::Node* node,
     path_planners_[LineKickPathPlanner().name()] = std::make_unique<LineKickPathPlanner>();
     path_planners_[PivotPathPlanner().name()] = std::make_unique<PivotPathPlanner>();
     path_planners_[LinePivotPathPlanner().name()] = std::make_unique<LinePivotPathPlanner>();
+    path_planners_[RotatePathPlanner().name()] = std::make_unique<RotatePathPlanner>();
     path_planners_[EscapeObstaclesPathPlanner().name()] =
         std::make_unique<EscapeObstaclesPathPlanner>();