Skip to content

[DRAFT] Refactor AngularVelocity/Acceleration classes #3541

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Draft
nycrat wants to merge 7 commits into
base: master
Choose a base branch
from
Draft

[DRAFT] Refactor AngularVelocity/Acceleration classes #3541

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
7 commits
Select commit Hold shift + click to select a range
b9601d0
Fix misuses of angle vs angular velocity/acceleration
nycrat Dec 12, 2025
7f73125
Refactor into generic angle template class
nycrat Feb 8, 2026
2977873
Use concepts to only implement some methods for Angle
nycrat Feb 8, 2026
fb30878
Fix implementation of == operator depending on type
nycrat Feb 8, 2026
d7f1e83
Edit comments, resolve TODOs
nycrat Feb 8, 2026
6136224
Refactor GenericAngle into separate header
nycrat Feb 8, 2026
9a7b3e5
Fix use of minDiff for angular velocity
nycrat Feb 8, 2026
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
3 changes: 2 additions & 1 deletion src/proto/message_translation/er_force_world_test.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -2,6 +2,7 @@

#include <gtest/gtest.h>

#include "software/geom/angular_velocity.h"
#include "software/test_util/test_util.h"


Expand Down Expand Up @@ -47,7 +48,7 @@ TEST(ErForceWorldTest, test_create_robot)
const Point expected_pos(sim_robot->p_x(), sim_robot->p_y());
const Vector expected_vel(sim_robot->v_x(), sim_robot->v_y());
RobotState expected_state(expected_pos, expected_vel, Angle::zero(),
Angle::fromRadians(5.0));
AngularVelocity::fromRadians(5.0));
EXPECT_EQ(0, test_robot.id());
EXPECT_TRUE(TestUtil::equalWithinTolerance(test_robot.currentState(), expected_state,
1e-6, Angle::fromDegrees(0)));
Expand Down
2 changes: 1 addition & 1 deletion src/proto/message_translation/tbots_geometry_test.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -16,7 +16,7 @@ TEST(TbotsProtobufTest, point_msg_test)

TEST(TbotsProtobufTest, angular_velocity_msg_test)
{
auto angular_velocity = Angle::fromRadians(4.20);
auto angular_velocity = AngularVelocity::fromRadians(4.20);
auto angular_velocity_msg = createAngularVelocityProto(angular_velocity);

EXPECT_EQ(angular_velocity_msg->radians_per_second(), angular_velocity.toRadians());
Expand Down
5 changes: 3 additions & 2 deletions src/proto/message_translation/tbots_protobuf.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,6 +1,7 @@
#include "proto/message_translation/tbots_protobuf.h"

#include "software/ai/navigator/trajectory/bang_bang_trajectory_1d_angular.h"
#include "software/geom/angular_acceleration.h"
#include "software/logger/logger.h"


Expand Down Expand Up @@ -482,9 +483,9 @@ BangBangTrajectory1DAngular createAngularTrajectoryFromParams(
createAngle(params.start_angle()), createAngle(params.final_angle()),
initial_velocity,
AngularVelocity::fromRadians(robot_constants.robot_max_ang_speed_rad_per_s),
AngularVelocity::fromRadians(
AngularAcceleration::fromRadians(
robot_constants.robot_max_ang_acceleration_rad_per_s_2),
AngularVelocity::fromRadians(
AngularAcceleration::fromRadians(
robot_constants.robot_max_ang_acceleration_rad_per_s_2));
}

Expand Down
2 changes: 1 addition & 1 deletion src/proto/message_translation/tbots_protobuf_test.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -121,7 +121,7 @@ TEST(TbotsProtobufTest, robot_state_msg_test)
auto position = Point(4.20, 4.20);
auto velocity = Vector(4.20, 4.20);
auto orientation = Angle::fromRadians(4.20);
auto angular_velocity = Angle::fromRadians(4.20);
auto angular_velocity = AngularVelocity::fromRadians(4.20);

Robot robot(0, position, velocity, orientation, angular_velocity,
Timestamp::fromSeconds(0));
Expand Down
7 changes: 4 additions & 3 deletions src/software/ai/hl/stp/tactic/attacker/attacker_tactic_keep_away_test.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -196,7 +196,7 @@ INSTANTIATE_TEST_CASE_P(
// the state of the friendly robot
RobotStateWithId{
1, RobotState(Point(0.25, 0), Vector(0, 0), Angle::fromDegrees(180),
Angle::fromDegrees(0))},
AngularVelocity::fromDegrees(0))},
// the state of the ball
BallState(Point(0., 0.), Vector(0, 0)),
// the states of the enemy robots
Expand Down Expand Up @@ -227,8 +227,9 @@ INSTANTIATE_TEST_CASE_P(
Pass(Point(FIELD_TOP_LEFT.x() + 0.05, FIELD_TOP_LEFT.y() - 0.05), Point(0, 0),
5),
// the state of the friendly robot
RobotStateWithId{1, RobotState(FIELD_TOP_LEFT, Vector(0, 0),
Angle::fromDegrees(0), Angle::fromDegrees(0))},
RobotStateWithId{
1, RobotState(FIELD_TOP_LEFT, Vector(0, 0), Angle::fromDegrees(0),
AngularVelocity::fromDegrees(0))},
// the state of the ball
BallState(Point(FIELD_TOP_LEFT.x() + 0.05, FIELD_TOP_LEFT.y() - 0.2),
Vector(0, 0)),
Expand Down
43 changes: 22 additions & 21 deletions src/software/ai/hl/stp/tactic/attacker/attacker_tactic_passing_test.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -5,6 +5,7 @@
#include "software/ai/hl/stp/tactic/attacker/attacker_tactic.h"
#include "software/ai/hl/stp/tactic/move/move_tactic.h"
#include "software/geom/algorithms/contains.h"
#include "software/geom/angular_velocity.h"
#include "software/simulated_tests/simulated_er_force_sim_play_test_fixture.h"
#include "software/simulated_tests/terminating_validation_functions/ball_kicked_validation.h"
#include "software/simulated_tests/terminating_validation_functions/robot_state_validation.h"
Expand Down Expand Up @@ -73,23 +74,23 @@ INSTANTIATE_TEST_CASE_P(
// Stationary Ball Tests
// Attacker point != Balls location & Balls location != Robots Location
std::make_tuple(Pass(Point(0.0, 0.5), Point(0, 0), 5),
RobotStateWithId{
1, RobotState(Point(0, 0), Vector(0, 0),
Angle::fromDegrees(0), Angle::fromDegrees(0))},
RobotStateWithId{1, RobotState(Point(0, 0), Vector(0, 0),
Angle::fromDegrees(0),
AngularVelocity::fromDegrees(0))},
BallState(Point(0.5, 0.5), Vector(0, 0))),

// Attacker point == Balls location & Balls location != Robots Location
std::make_tuple(Pass(Point(-0.5, -0.5), Point(0, 0), 5),
RobotStateWithId{
1, RobotState(Point(0, 0), Vector(0, 0),
Angle::fromDegrees(0), Angle::fromDegrees(0))},
RobotStateWithId{1, RobotState(Point(0, 0), Vector(0, 0),
Angle::fromDegrees(0),
AngularVelocity::fromDegrees(0))},
BallState(Point(-0.5, -0.5), Vector(0, 0))),

// Attacker point != Balls location & Balls location == Robots Location
std::make_tuple(Pass(Point(0.5, 0.5), Point(0, 1), 5),
RobotStateWithId{
1, RobotState(Point(0.5, 0.5), Vector(0, 0),
Angle::fromDegrees(0), Angle::fromDegrees(0))},
RobotStateWithId{1, RobotState(Point(0.5, 0.5), Vector(0, 0),
Angle::fromDegrees(0),
AngularVelocity::fromDegrees(0))},
BallState(Point(-0.5, 0.5), Vector(0, 0))),

// TODO(#2909) : Enable test once the robot can turn faster and hits the ball with
Expand All @@ -103,24 +104,24 @@ INSTANTIATE_TEST_CASE_P(

// Attacker point far away (not a normal use case, but just to sanity check)
std::make_tuple(Pass(Point(0.0, 0.0), Point(0, 0), 5),
RobotStateWithId{
1, RobotState(Point(3.5, 2.5), Vector(0, 0),
Angle::fromDegrees(0), Angle::fromDegrees(0))},
RobotStateWithId{1, RobotState(Point(3.5, 2.5), Vector(0, 0),
Angle::fromDegrees(0),
AngularVelocity::fromDegrees(0))},
BallState(Point(0.0, 0.0), Vector(0, 0))),

// Attacker point != Balls location & Balls location != Robots Location
std::make_tuple(Pass(Point(0.0, 0.5), Point(0, 0), 5),
RobotStateWithId{
1, RobotState(Point(0, 0), Vector(0, 0),
Angle::fromDegrees(0), Angle::fromDegrees(0))},
RobotStateWithId{1, RobotState(Point(0, 0), Vector(0, 0),
Angle::fromDegrees(0),
AngularVelocity::fromDegrees(0))},
BallState(Point(0.5, 0.5), Vector(0, 0))),

// Moving Ball Tests
// Attacker point == Balls location & Balls location != Robots Location
std::make_tuple(Pass(Point(-0.5, -0.5), Point(0, 0), 5),
RobotStateWithId{
1, RobotState(Point(0, 0), Vector(0, 0),
Angle::fromDegrees(0), Angle::fromDegrees(0))},
RobotStateWithId{1, RobotState(Point(0, 0), Vector(0, 0),
Angle::fromDegrees(0),
AngularVelocity::fromDegrees(0))},
BallState(Point(-0.5, -0.5), Vector(1, 0))),

// TODO (#2859): Robot does not kick ball when dribbler is off since it is
Expand All @@ -135,7 +136,7 @@ INSTANTIATE_TEST_CASE_P(

// Attacker point == Balls location & Balls location == Robots Location
std::make_tuple(Pass(Point(0.0, 0.0), Point(0, 0), 5),
RobotStateWithId{
1, RobotState(Point(0, 0), Vector(0, 0),
Angle::fromDegrees(0), Angle::fromDegrees(0))},
RobotStateWithId{1, RobotState(Point(0, 0), Vector(0, 0),
Angle::fromDegrees(0),
AngularVelocity::fromDegrees(0))},
BallState(Point(0.0, 0.0), Vector(1, 0)))));
5 changes: 3 additions & 2 deletions src/software/ai/hl/stp/tactic/move_primitive.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -5,6 +5,7 @@
#include "proto/primitive/primitive_msg_factory.h"
#include "software/ai/navigator/trajectory/bang_bang_trajectory_1d_angular.h"
#include "software/geom/algorithms/end_in_obstacle_sample.h"
#include "software/geom/angular_acceleration.h"

MovePrimitive::MovePrimitive(
const Robot &robot, const Point &destination, const Angle &final_angle,
Expand Down Expand Up @@ -38,9 +39,9 @@ MovePrimitive::MovePrimitive(
robot.orientation(), final_angle, robot.angularVelocity(),
AngularVelocity::fromRadians(
robot.robotConstants().robot_max_ang_speed_rad_per_s),
AngularVelocity::fromRadians(
AngularAcceleration::fromRadians(
robot.robotConstants().robot_max_ang_acceleration_rad_per_s_2),
AngularVelocity::fromRadians(
AngularAcceleration::fromRadians(
robot.robotConstants().robot_max_ang_acceleration_rad_per_s_2));

estimated_cost =
Expand Down
34 changes: 18 additions & 16 deletions src/software/ai/hl/stp/tactic/penalty_kick/penalty_kick_tactic_test.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -4,6 +4,7 @@

#include <utility>

#include "software/geom/angular_velocity.h"
#include "software/simulated_tests/non_terminating_validation_functions/ball_in_play_or_scored_validation.h"
#include "software/simulated_tests/non_terminating_validation_functions/ball_never_moves_backward_validation.h"
#include "software/simulated_tests/non_terminating_validation_functions/robot_not_excessively_dribbling_validation.h"
Expand All @@ -21,8 +22,8 @@ class PenaltyKickTacticTest : public SimulatedErForceSimPlayTestFixture,
Field field = Field::createField(field_type);
BallState ball = BallState(field.friendlyPenaltyMark(), Vector(0, 0));
Point initial_position = field.friendlyPenaltyMark() + Vector(-0.1, 0);
RobotStateWithId shooter = {
0, RobotState(initial_position, Vector(0, 0), Angle::zero(), Angle::zero())};
RobotStateWithId shooter = {0, RobotState(initial_position, Vector(0, 0),
Angle::zero(), AngularVelocity::zero())};
};

// TODO (#2232): Improve dribbling control so the ball is not lost during this test
Expand Down Expand Up @@ -79,22 +80,23 @@ INSTANTIATE_TEST_CASE_P(
RobotLocations, PenaltyKickTacticTest,
::testing::Values(
// enemy robot stationary at centre of goal
RobotStateWithId{0, RobotState(Field::createSSLDivisionBField().enemyGoalCenter(),
Vector(0, 0), Angle::half(), Angle::zero())},
RobotStateWithId{
0, RobotState(Field::createSSLDivisionBField().enemyGoalCenter(),
Vector(0, 0), Angle::half(), AngularVelocity::zero())},

// enemy robot stationary left of net
RobotStateWithId{0,
RobotState(Field::createSSLDivisionBField().enemyGoalpostNeg(),
Vector(0, 0), Angle::half(), Angle::zero())},
RobotStateWithId{
0, RobotState(Field::createSSLDivisionBField().enemyGoalpostNeg(),
Vector(0, 0), Angle::half(), AngularVelocity::zero())},
// enemy robot stationary right of net
RobotStateWithId{0,
RobotState(Field::createSSLDivisionBField().enemyGoalpostPos(),
Vector(0, 0), Angle::half(), Angle::zero())},
RobotStateWithId{
0, RobotState(Field::createSSLDivisionBField().enemyGoalpostPos(),
Vector(0, 0), Angle::half(), AngularVelocity::zero())},
// enemy robot left of net but moving right
RobotStateWithId{0,
RobotState(Field::createSSLDivisionBField().enemyGoalpostNeg(),
Vector(0, 1.2), Angle::half(), Angle::zero())},
RobotStateWithId{
0, RobotState(Field::createSSLDivisionBField().enemyGoalpostNeg(),
Vector(0, 1.2), Angle::half(), AngularVelocity::zero())},
// enemy robot right of net but moving left
RobotStateWithId{0,
RobotState(Field::createSSLDivisionBField().enemyGoalpostPos(),
Vector(0, -1.2), Angle::half(), Angle::zero())}));
RobotStateWithId{
0, RobotState(Field::createSSLDivisionBField().enemyGoalpostPos(),
Vector(0, -1.2), Angle::half(), AngularVelocity::zero())}));
31 changes: 16 additions & 15 deletions src/software/ai/hl/stp/tactic/receiver/receiver_tactic_test.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -6,6 +6,7 @@

#include "software/ai/hl/stp/tactic/move/move_tactic.h"
#include "software/geom/algorithms/contains.h"
#include "software/geom/angular_velocity.h"
#include "software/simulated_tests/simulated_er_force_sim_play_test_fixture.h"
#include "software/simulated_tests/terminating_validation_functions/ball_kicked_validation.h"
#include "software/simulated_tests/terminating_validation_functions/friendly_scored_validation.h"
Expand Down Expand Up @@ -89,27 +90,27 @@ INSTANTIATE_TEST_CASE_P(

// Robot slighty off from receive point: test 2
std::make_tuple(Pass(Point(0.0, 0.4), Point(2, 2), 4),
RobotStateWithId{
1, RobotState(Point(2.5, 2.0), Vector(0, 0),
Angle::fromDegrees(0), Angle::fromDegrees(0))}),
RobotStateWithId{1, RobotState(Point(2.5, 2.0), Vector(0, 0),
Angle::fromDegrees(0),
AngularVelocity::fromDegrees(0))}),

// Robot facing away from pass
std::make_tuple(Pass(Point(0.0, 0.0), Point(-3, 0), 4),
RobotStateWithId{1, RobotState(Point(-3, 0), Vector(0, 0),
Angle::fromDegrees(180),
Angle::fromDegrees(0))}),
AngularVelocity::fromDegrees(0))}),

// Robot facing towards from pass
std::make_tuple(Pass(Point(0.0, 0.0), Point(-3, 0), 4),
RobotStateWithId{
1, RobotState(Point(-3, 0), Vector(0, 0),
Angle::fromDegrees(0), Angle::fromDegrees(0))}),
RobotStateWithId{1, RobotState(Point(-3, 0), Vector(0, 0),
Angle::fromDegrees(0),
AngularVelocity::fromDegrees(0))}),

// Robot facing towards pass speedy
std::make_tuple(Pass(Point(0.0, 0.0), Point(-3, 0), 5),
RobotStateWithId{
1, RobotState(Point(-3, 0), Vector(0, 0),
Angle::fromDegrees(0), Angle::fromDegrees(0))})
RobotStateWithId{1, RobotState(Point(-3, 0), Vector(0, 0),
Angle::fromDegrees(0),
AngularVelocity::fromDegrees(0))})

));

Expand Down Expand Up @@ -187,7 +188,7 @@ INSTANTIATE_TEST_CASE_P(
std::make_tuple(Pass(Point(4.0, 1.5), Point(4, -1), 5),
RobotStateWithId{1, RobotState(Point(4.0, -1), Vector(0, 0),
Angle::fromDegrees(180),
Angle::fromDegrees(0))}),
AngularVelocity::fromDegrees(0))}),

// TODO (#2570): re-enable when one-touch works for these tests
// std::make_tuple(Pass(Point(4.0, 1.5), Point(3.5, -1), 5),
Expand All @@ -205,7 +206,7 @@ INSTANTIATE_TEST_CASE_P(
std::make_tuple(Pass(Point(4.0, -1.5), Point(4, 1), 5),
RobotStateWithId{1, RobotState(Point(4.0, 1), Vector(0, 0),
Angle::fromDegrees(180),
Angle::fromDegrees(0))}),
AngularVelocity::fromDegrees(0))}),

// TODO (#2570): re-enable when one-touch works for these tests
// std::make_tuple(Pass(Point(4.0, -1.5), Point(3.5, 1), 5),
Expand All @@ -215,9 +216,9 @@ INSTANTIATE_TEST_CASE_P(
// Angle::fromDegrees(0))}),

std::make_tuple(Pass(Point(4.0, -1.5), Point(3.0, 1), 4.5),
RobotStateWithId{
1, RobotState(Point(3.0, 1), Vector(0, 0),
Angle::fromDegrees(0), Angle::fromDegrees(0))})
RobotStateWithId{1, RobotState(Point(3.0, 1), Vector(0, 0),
Angle::fromDegrees(0),
AngularVelocity::fromDegrees(0))})

// TODO (#2570): re-enable when one-touch works for these tests
// Direct one touch
Expand Down
12 changes: 9 additions & 3 deletions src/software/geom/BUILD
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,9 +1,15 @@
package(default_visibility = ["//visibility:public"])

cc_library(
name = "generic_angle",
hdrs = ["generic_angle.h"],
deps = [":geom_constants"],
)

cc_library(
name = "angle",
hdrs = ["angle.h"],
deps = [":geom_constants"],
deps = [":generic_angle"],
)

cc_library(
Expand Down Expand Up @@ -36,13 +42,13 @@ cc_library(
cc_library(
name = "angular_velocity",
hdrs = ["angular_velocity.h"],
deps = [":angle"],
deps = [":generic_angle"],
)

cc_library(
name = "angular_acceleration",
hdrs = ["angular_acceleration.h"],
deps = [":angle"],
deps = [":generic_angle"],
)

cc_library(
Expand Down
Loading
Loading