-
Notifications
You must be signed in to change notification settings - Fork 186
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
* Created a new motion planner called rotation planner. It is a work in progress. This planner should theoretically only be rotation around a point (in our case, we are developing it to rotate about itself). * tested some theory about the problem. With changing the values in points, we have pinpointed the likely problem. If the two rotation points are the current pivot point and the point (0, 0), it will rotate but also translate. If the only points in the rotation are the pivot point, it will try to plan but get stuck. This is because we try to move to the same point that we are already at, so we think it is done because planners check they are done by translation. However, we need to make this one check it is done by its orientation. * trapezoid angle planning * end 09/08 * updated files * Dribble in Sim (#2287) * dribbler is stupid * use realism * add realism to simoulator cli run --------- Co-authored-by: petergarud <[email protected]> * add ros env vars to source script (#2288) * Update tutorial.rst (#2286) * fixed rebase issues * Fix Code Style On rotate-kick (#2300) automated style fixes Co-authored-by: Squid5678 <[email protected]> * changes * Fix Code Style On rotate-kick (#2303) automated style fixes Co-authored-by: Squid5678 <[email protected]> * change to is angle done threshold * removing SPDLOG stuff * removed unnecessary line pivot changes * gitignore * remove log stuff * comments --------- Co-authored-by: Shourik Banerjee <[email protected]> Co-authored-by: rishiso <[email protected]> Co-authored-by: Sanat Dhanyamraju <[email protected]> Co-authored-by: petergarud <[email protected]> Co-authored-by: Sid Parikh <[email protected]> Co-authored-by: Jack <[email protected]> Co-authored-by: github-actions[bot] <41898282+github-actions[bot]@users.noreply.github.com> Co-authored-by: Squid5678 <[email protected]>
- Loading branch information
9 people
authored
Nov 6, 2024
1 parent
ba4465c
commit 68cbf04
Showing
14 changed files
with
253 additions
and
51 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
80 changes: 80 additions & 0 deletions
80
soccer/src/soccer/planning/planner/rotate_path_planner.cpp
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -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 |
46 changes: 46 additions & 0 deletions
46
soccer/src/soccer/planning/planner/rotate_path_planner.hpp
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -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 |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Oops, something went wrong.