diff --git a/src/proto/message_translation/tbots_protobuf.cpp b/src/proto/message_translation/tbots_protobuf.cpp index 7c146b4c45..23e7d0c51c 100644 --- a/src/proto/message_translation/tbots_protobuf.cpp +++ b/src/proto/message_translation/tbots_protobuf.cpp @@ -425,7 +425,8 @@ std::unique_ptr createCostVisualization( } std::optional createTrajectoryPathFromParams( - const TbotsProto::TrajectoryPathParams2D& params, const Vector& initial_velocity, + const TbotsProto::TrajectoryPathParams2D& params, const Point& start_position, + const Vector& initial_velocity, const robot_constants::RobotConstants& robot_constants) { double max_speed = convertMaxAllowedSpeedModeToMaxAllowedSpeed( @@ -449,8 +450,7 @@ std::optional createTrajectoryPathFromParams( } auto trajectory = std::make_shared( - createPoint(params.start_position()), initial_destination, initial_velocity, - constraints); + start_position, initial_destination, initial_velocity, constraints); TrajectoryPath trajectory_path(trajectory, BangBangTrajectory2D::generator); @@ -475,14 +475,14 @@ std::optional createTrajectoryPathFromParams( } BangBangTrajectory1DAngular createAngularTrajectoryFromParams( - const TbotsProto::TrajectoryParamsAngular1D& params, + const TbotsProto::TrajectoryParamsAngular1D& params, const Angle& start_angle, const AngularVelocity& initial_velocity, const robot_constants::RobotConstants& robot_constants) { return BangBangTrajectory1DAngular( - createAngle(params.start_angle()), createAngle(params.final_angle()), - initial_velocity, - AngularVelocity::fromRadians(robot_constants.robot_max_ang_speed_rad_per_s), + start_angle, createAngle(params.final_angle()), initial_velocity, + AngularVelocity::fromRadians( + robot_constants.robot_max_ang_speed_trajectory_rad_per_s), AngularVelocity::fromRadians( robot_constants.robot_max_ang_acceleration_rad_per_s_2), AngularVelocity::fromRadians( @@ -513,7 +513,7 @@ double convertMaxAllowedSpeedModeToMaxAllowedSpeed( switch (max_allowed_speed_mode) { case TbotsProto::MaxAllowedSpeedMode::PHYSICAL_LIMIT: - return robot_constants.robot_max_speed_m_per_s; + return robot_constants.robot_max_speed_trajectory_m_per_s; case TbotsProto::MaxAllowedSpeedMode::STOP_COMMAND: return STOP_COMMAND_ROBOT_MAX_SPEED_METERS_PER_SECOND - STOP_COMMAND_SPEED_SAFETY_MARGIN_METERS_PER_SECOND; diff --git a/src/proto/message_translation/tbots_protobuf.h b/src/proto/message_translation/tbots_protobuf.h index 5976da5fcc..2831857e02 100644 --- a/src/proto/message_translation/tbots_protobuf.h +++ b/src/proto/message_translation/tbots_protobuf.h @@ -238,25 +238,28 @@ std::unique_ptr createCostVisualization( * Generate a 2D Trajectory Path given 2D trajectory parameters * * @param params 2D Trajectory Path + * @param start_position Initial position to use for the trajectory * @param initial_velocity Initial velocity to use for the trajectory * @param robot_constants Constants to use for the trajectory * @return TrajectoryPath, or std::nullopt if the trajectory path could not be created * from the given parameters */ std::optional createTrajectoryPathFromParams( - const TbotsProto::TrajectoryPathParams2D& params, const Vector& initial_velocity, + const TbotsProto::TrajectoryPathParams2D& params, const Point& start_position, + const Vector& initial_velocity, const robot_constants::RobotConstants& robot_constants); /** - * Generate an angular trajectory Path given angular trajectory proto parameters + * Generate an angular trajectory path given angular trajectory proto parameters * - * @param params angular Trajectory Path + * @param params Angular trajectory path + * @param start_angle Initial angle to use for the trajectory * @param initial_velocity Initial velocity to use for the trajectory * @param robot_constants Constants to use for the trajectory * @return Generate angular trajectory */ BangBangTrajectory1DAngular createAngularTrajectoryFromParams( - const TbotsProto::TrajectoryParamsAngular1D& params, + const TbotsProto::TrajectoryParamsAngular1D& params, const Angle& start_angle, const AngularVelocity& initial_velocity, const robot_constants::RobotConstants& robot_constants); diff --git a/src/proto/message_translation/tbots_protobuf_test.cpp b/src/proto/message_translation/tbots_protobuf_test.cpp index 0f82de69d6..2735d57a3b 100644 --- a/src/proto/message_translation/tbots_protobuf_test.cpp +++ b/src/proto/message_translation/tbots_protobuf_test.cpp @@ -209,8 +209,8 @@ TEST_P(TrajectoryParamConversionTest, trajectory_params_msg_test) *(params.add_sub_destinations()) = sub_destination_proto; } - auto converted_trajectory_path_opt = - createTrajectoryPathFromParams(params, initial_velocity, robot_constants); + auto converted_trajectory_path_opt = createTrajectoryPathFromParams( + params, createPoint(params.start_position()), initial_velocity, robot_constants); ASSERT_TRUE(converted_trajectory_path_opt.has_value()); TrajectoryPath converted_trajectory_path = converted_trajectory_path_opt.value(); diff --git a/src/shared/constants.h b/src/shared/constants.h index ed94c0cde9..b0f3dbaa95 100644 --- a/src/shared/constants.h +++ b/src/shared/constants.h @@ -224,6 +224,8 @@ static const unsigned THUNDERLOOP_HZ = 300u; static const unsigned NUM_GENEVA_ANGLES = 5; +static constexpr double RTT_S = 0.03; + // Robot diagnostics constants constexpr double AUTO_CHIP_DISTANCE_DEFAULT_M = 1.5; constexpr double AUTO_KICK_SPEED_DEFAULT_M_PER_S = 1.5; diff --git a/src/shared/robot_constants.h b/src/shared/robot_constants.h index 93ceb390cd..ef302965e0 100644 --- a/src/shared/robot_constants.h +++ b/src/shared/robot_constants.h @@ -60,6 +60,10 @@ struct RobotConstants // The maximum speed achievable by our robots, in metres per second [m/s] float robot_max_speed_m_per_s; + // The maximum speed that the trajectory planner is allowed to command the robot to + // move at, while still leaving headroom for the PID to apply correction on lag. [m/s] + float robot_max_speed_trajectory_m_per_s; + // The maximum acceleration achievable by our robots [m/s^2] float robot_max_acceleration_m_per_s_2; @@ -69,11 +73,23 @@ struct RobotConstants // The maximum angular speed achievable by our robots [rad/s] float robot_max_ang_speed_rad_per_s; + // The maximum speed that the trajectory planner is allowed to command the robot to + // move at, while still leaving headroom for the PID to apply correction on lag. + // [rad/s] + float robot_max_ang_speed_trajectory_rad_per_s; + // The maximum angular acceleration achievable by our robots [rad/s^2] float robot_max_ang_acceleration_rad_per_s_2; // The radius of the wheel, in meters float wheel_radius_meters; + + // Various variances for the robot localizer + float kalman_process_noise_variance_rad_per_s_4; + + float kalman_vision_noise_variance_rad_2; + + float kalman_motor_sensor_noise_variance_rad_per_s_2; }; /** @@ -87,7 +103,7 @@ constexpr RobotConstants createRobotConstants() return { .robot_radius_m = static_cast(ROBOT_MAX_RADIUS_METERS), .front_wheel_angle_deg = 32.0f, - .back_wheel_angle_deg = 44.0f, + .back_wheel_angle_deg = 46.0f, .front_of_robot_width_meters = 0.11f, .dribbler_width_meters = 0.07825f, @@ -97,18 +113,25 @@ constexpr RobotConstants createRobotConstants() .max_force_dribbler_speed_rpm = -12000, // Motor constant - .motor_max_acceleration_m_per_s_2 = 2.0f, + .motor_max_acceleration_m_per_s_2 = 4.0f, // Robot's linear movement constants - .robot_max_speed_m_per_s = 3.0f, - .robot_max_acceleration_m_per_s_2 = 3.0f, - .robot_max_deceleration_m_per_s_2 = 3.0f, + .robot_max_speed_m_per_s = 3.0f, + .robot_max_speed_trajectory_m_per_s = 2.5f, + .robot_max_acceleration_m_per_s_2 = 2.5f, + .robot_max_deceleration_m_per_s_2 = 2.0f, // Robot's angular movement constants - .robot_max_ang_speed_rad_per_s = 10.0f, - .robot_max_ang_acceleration_rad_per_s_2 = 30.0f, + .robot_max_ang_speed_rad_per_s = 6.0f, + .robot_max_ang_speed_trajectory_rad_per_s = 5.0f, + .robot_max_ang_acceleration_rad_per_s_2 = 10.0f, + + .wheel_radius_meters = 0.03f, - .wheel_radius_meters = 0.03f}; + // Kalman filter variances for robot localizer + .kalman_process_noise_variance_rad_per_s_4 = 1.0f, + .kalman_vision_noise_variance_rad_2 = 0.01f, + .kalman_motor_sensor_noise_variance_rad_per_s_2 = 0.5}; } #elif CHECK_VERSION(2021) constexpr RobotConstants createRobotConstants() @@ -129,15 +152,22 @@ constexpr RobotConstants createRobotConstants() .motor_max_acceleration_m_per_s_2 = 4.5f, // Robot's linear movement constants - .robot_max_speed_m_per_s = 3.000f, - .robot_max_acceleration_m_per_s_2 = 3.0f, - .robot_max_deceleration_m_per_s_2 = 3.0f, + .robot_max_speed_m_per_s = 3.000f, + .robot_max_speed_trajectory_m_per_s = 3.000f, + .robot_max_acceleration_m_per_s_2 = 3.0f, + .robot_max_deceleration_m_per_s_2 = 3.0f, // Robot's angular movement constants - .robot_max_ang_speed_rad_per_s = 10.0f, - .robot_max_ang_acceleration_rad_per_s_2 = 30.0f, + .robot_max_ang_speed_rad_per_s = 10.0f, + .robot_max_ang_speed_trajectory_rad_per_s = 7.0f, + .robot_max_ang_acceleration_rad_per_s_2 = 30.0f, + + .wheel_radius_meters = 0.03f, - .wheel_radius_meters = 0.03f}; + // Kalman filter variances for robot localizer + .kalman_process_noise_variance_rad_per_s_4 = 0.5f, + .kalman_vision_noise_variance_rad_2 = 0.01f * 0.01f, + .kalman_motor_sensor_noise_variance_rad_per_s_2 = 0.5f * 0.5f}; } #endif diff --git a/src/software/ai/evaluation/intercept.cpp b/src/software/ai/evaluation/intercept.cpp index 457ffb0639..2c729dd516 100644 --- a/src/software/ai/evaluation/intercept.cpp +++ b/src/software/ai/evaluation/intercept.cpp @@ -97,7 +97,7 @@ Point findOvershootInterceptPosition(const Robot& robot, const Point intercept_p Point best_position = intercept_position; double final_speed = step_speed; bool finished = false; - double max_speed = robot.robotConstants().robot_max_speed_m_per_s; + double max_speed = robot.robotConstants().robot_max_speed_trajectory_m_per_s; double max_acc = robot.robotConstants().robot_max_acceleration_m_per_s_2; while (!finished) diff --git a/src/software/ai/hl/stp/tactic/move_primitive.cpp b/src/software/ai/hl/stp/tactic/move_primitive.cpp index 7e748c2068..325da493a3 100644 --- a/src/software/ai/hl/stp/tactic/move_primitive.cpp +++ b/src/software/ai/hl/stp/tactic/move_primitive.cpp @@ -40,7 +40,7 @@ MovePrimitive::MovePrimitive( angular_trajectory.generate( robot.orientation(), final_angle, robot.angularVelocity(), AngularVelocity::fromRadians( - robot.robotConstants().robot_max_ang_speed_rad_per_s), + robot.robotConstants().robot_max_ang_speed_trajectory_rad_per_s), AngularVelocity::fromRadians( robot.robotConstants().robot_max_ang_acceleration_rad_per_s_2), AngularVelocity::fromRadians( diff --git a/src/software/embedded/BUILD b/src/software/embedded/BUILD index 5ee03cb54c..d58f3ae945 100644 --- a/src/software/embedded/BUILD +++ b/src/software/embedded/BUILD @@ -37,6 +37,7 @@ cc_library( "//software/math:math_functions", "//software/physics:velocity_conversion_util", "//software/time:duration", + "//software/util/pid", "//software/world:robot_state", "//software/world:team_colour", ], @@ -58,11 +59,14 @@ cc_library( deps = [ ":primitive_executor", "//proto:tbots_cc_proto", + "//software/embedded:robot_localizer", + "//software/embedded/services:imu", "//software/embedded/services:motor", "//software/embedded/services:power", "//software/embedded/services/network", "//software/embedded/toml_config", "//software/logger:network_logger", + "//software/physics:velocity_conversion_util", "//software/tracy:tracy_constants", "//software/util/scoped_timespec_timer", "@tracy", @@ -100,3 +104,21 @@ cc_test( "//shared/test_util:tbots_gtest_main", ], ) + +cc_library( + name = "robot_localizer", + srcs = ["robot_localizer.cpp"], + hdrs = ["robot_localizer.h"], + deps = [ + "//proto:tbots_cc_proto", + "//software:constants", + "//software/embedded/services:imu", + "//software/geom:angle", + "//software/geom:angular_velocity", + "//software/geom:point", + "//software/geom:vector", + "//software/sensor_fusion/filter:kalman_filter", + "//software/util/scoped_timespec_timer", + "@eigen", + ], +) diff --git a/src/software/embedded/motor_controller/stspin_motor_controller.h b/src/software/embedded/motor_controller/stspin_motor_controller.h index 8402db8080..2a86e09884 100644 --- a/src/software/embedded/motor_controller/stspin_motor_controller.h +++ b/src/software/embedded/motor_controller/stspin_motor_controller.h @@ -57,11 +57,11 @@ class StSpinMotorController : public MotorController static constexpr int RESET_GPIO_PIN = 12; - static constexpr int SPEED_PID_PROPORTIONAL_GAIN = 700; - static constexpr int SPEED_PID_INTEGRAL_GAIN = 30; + static constexpr int SPEED_PID_PROPORTIONAL_GAIN = 1300; + static constexpr int SPEED_PID_INTEGRAL_GAIN = 40; - static constexpr int MAX_SPEED_FEED_FORWARD_STATIC_GAIN = 750; - static constexpr int MIN_SPEED_FEED_FORWARD_STATIC_GAIN = 300; + static constexpr int MAX_SPEED_FEED_FORWARD_STATIC_GAIN = 800; + static constexpr int MIN_SPEED_FEED_FORWARD_STATIC_GAIN = 250; robot_constants::RobotConstants robot_constants_; diff --git a/src/software/embedded/primitive_executor.cpp b/src/software/embedded/primitive_executor.cpp index d1f56161ee..5732281874 100644 --- a/src/software/embedded/primitive_executor.cpp +++ b/src/software/embedded/primitive_executor.cpp @@ -11,76 +11,165 @@ #include "software/physics/velocity_conversion_util.h" PrimitiveExecutor::PrimitiveExecutor( - const Duration time_step, const robot_constants::RobotConstants& robot_constants, - const TeamColour friendly_team_colour, const RobotId robot_id) - : current_primitive_(), - friendly_team_colour_(friendly_team_colour), - robot_constants_(robot_constants), - time_step_(time_step), - robot_id_(robot_id) + const robot_constants::RobotConstants& robot_constants) + : robot_constants_(robot_constants) { } +bool PrimitiveExecutor::isLinearTrajectoryNew( + const std::optional& new_trajectory) const +{ + if (new_trajectory.has_value() != trajectory_path_.has_value()) + { + // Either we don't have a trajectory right now, and this one does, or we have one + // and this trajectory doesn't. + return true; + } + + if (!new_trajectory.has_value()) + { + return false; + } + + return distance(new_trajectory->getDestination(), + trajectory_path_->getDestination()) > + LINEAR_DESTINATION_THRESHOLD_METERS; +} + +bool PrimitiveExecutor::isAngularTrajectoryNew( + const BangBangTrajectory1DAngular& new_trajectory) const +{ + if (!angular_trajectory_.has_value()) + { + return true; + } + + return new_trajectory.getDestination() + .minDiff(angular_trajectory_->getDestination()) + .toDegrees() > ANGULAR_DESTINATION_THRESHOLD_DEGREES; +} + void PrimitiveExecutor::updatePrimitive(const TbotsProto::Primitive& primitive_msg) { current_primitive_ = primitive_msg; if (current_primitive_.has_move()) { - trajectory_path_ = createTrajectoryPathFromParams( - current_primitive_.move().xy_traj_params(), velocity_, robot_constants_); + const std::optional new_trajectory_path = + createTrajectoryPathFromParams(current_primitive_.move().xy_traj_params(), + position_, velocity_, robot_constants_); - angular_trajectory_ = + if (isLinearTrajectoryNew(new_trajectory_path)) + { + trajectory_path_ = new_trajectory_path; + time_since_linear_trajectory_creation_ = + std::chrono::duration::zero(); + x_pid.reset(); + y_pid.reset(); + } + + const BangBangTrajectory1DAngular new_angular_trajectory = createAngularTrajectoryFromParams(current_primitive_.move().w_traj_params(), - angular_velocity_, robot_constants_); + orientation_, angular_velocity_, + robot_constants_); - time_since_trajectory_creation_ = Duration::fromSeconds(VISION_TO_ROBOT_DELAY_S); + if (isAngularTrajectoryNew(new_angular_trajectory)) + { + angular_trajectory_ = new_angular_trajectory; + time_since_angular_trajectory_creation_ = + std::chrono::duration::zero(); + w_pid.reset(); + } } } -void PrimitiveExecutor::setStopPrimitive() +void PrimitiveExecutor::updateState(const Point& position, const Vector& velocity, + const Angle& orientation, + const AngularVelocity& angular_velocity) { - current_primitive_ = *createStopPrimitiveProto(); -} + position_ = position; + velocity_ = velocity; + orientation_ = orientation; + angular_velocity_ = angular_velocity; -void PrimitiveExecutor::updateVelocity(const Vector& local_velocity, - const AngularVelocity& angular_velocity) -{ - Vector actual_global_velocity = localToGlobalVelocity(local_velocity, orientation_); - velocity_ = actual_global_velocity; - angular_velocity_ = angular_velocity; + // If we are lagging behind trajectory too much, we have stalled! We need to + // regenerate trajectory. + // TODO: Add a timeout to following error + if (trajectory_path_.has_value()) + { + const double linear_following_error = + (position_ - trajectory_path_->getPosition( + time_since_linear_trajectory_creation_.count())) + .length(); + + if (linear_following_error > LINEAR_STALL_ERROR_MAX_METERS) + { + // regenerate linear trajectory + trajectory_path_ = + createTrajectoryPathFromParams(current_primitive_.move().xy_traj_params(), + position_, velocity_, robot_constants_); + + time_since_linear_trajectory_creation_ = + std::chrono::duration::zero(); + } + } + + if (angular_trajectory_.has_value()) + { + const double angular_following_error = + angular_trajectory_ + ->getPosition(time_since_angular_trajectory_creation_.count()) + .minDiff(orientation_) + .toDegrees(); + + if (angular_following_error > ANGULAR_STALL_ERROR_MAX_DEGREES) + { + // regenerate angular trajectory + angular_trajectory_ = createAngularTrajectoryFromParams( + current_primitive_.move().w_traj_params(), orientation_, + angular_velocity_, robot_constants_); + + time_since_angular_trajectory_creation_ = + std::chrono::duration::zero(); + } + } } -Vector PrimitiveExecutor::getTargetLinearVelocity() +Vector PrimitiveExecutor::getTargetLinearVelocity() const { - Vector local_velocity = globalToLocalVelocity( - trajectory_path_->getVelocity(time_since_trajectory_creation_.toSeconds()), - orientation_); - Point position = - trajectory_path_->getPosition(time_since_trajectory_creation_.toSeconds()); - double distance_to_destination = - distance(position, trajectory_path_->getDestination()); + Vector target_velocity = + trajectory_path_->getVelocity(time_since_linear_trajectory_creation_.count()); + + const Point expected_position = + trajectory_path_->getPosition(time_since_linear_trajectory_creation_.count()); + const double distance_to_destination = + distance(expected_position, trajectory_path_->getDestination()); // Dampen velocity as we get closer to the destination to reduce jittering - if (distance_to_destination < MAX_DAMPENING_VELOCITY_DISTANCE_M) + if (distance_to_destination < MAX_DAMPENING_LINEAR_VELOCITY_DISTANCE_M) { - local_velocity *= distance_to_destination / MAX_DAMPENING_VELOCITY_DISTANCE_M; + target_velocity *= + distance_to_destination / MAX_DAMPENING_LINEAR_VELOCITY_DISTANCE_M; } - return local_velocity; + + return target_velocity; } -AngularVelocity PrimitiveExecutor::getTargetAngularVelocity() +AngularVelocity PrimitiveExecutor::getTargetAngularVelocity() const { - orientation_ = - angular_trajectory_->getPosition(time_since_trajectory_creation_.toSeconds()); - AngularVelocity angular_velocity = - angular_trajectory_->getVelocity(time_since_trajectory_creation_.toSeconds()); - Angle orientation_to_destination = - orientation_.minDiff(angular_trajectory_->getDestination()); - if (orientation_to_destination.toDegrees() < 5) + angular_trajectory_->getVelocity(time_since_angular_trajectory_creation_.count()); + + const Angle expected_orientation = + angular_trajectory_->getPosition(time_since_angular_trajectory_creation_.count()); + const double distance_to_destination = + expected_orientation.minDiff(angular_trajectory_->getDestination()).toDegrees(); + + // Dampen velocity as we get closer to the destination to reduce jittering + if (distance_to_destination < MAX_DAMPENING_ANGULAR_VELOCITY_DISTANCE_DEGREES) { - angular_velocity *= orientation_to_destination.toDegrees() / 5; + angular_velocity *= + distance_to_destination / MAX_DAMPENING_ANGULAR_VELOCITY_DISTANCE_DEGREES; } return angular_velocity; @@ -90,19 +179,22 @@ AngularVelocity PrimitiveExecutor::getTargetAngularVelocity() std::unique_ptr PrimitiveExecutor::stepPrimitive( TbotsProto::PrimitiveExecutorStatus& status) { - time_since_trajectory_creation_ += time_step_; + const auto current_time = std::chrono::steady_clock::now(); + time_since_linear_trajectory_creation_ += current_time - last_step_time_; + time_since_angular_trajectory_creation_ += current_time - last_step_time_; + last_step_time_ = current_time; + status.set_running_primitive(true); switch (current_primitive_.primitive_case()) { case TbotsProto::Primitive::kStop: { - auto prim = createDirectControlPrimitive(Vector(), AngularVelocity(), 0.0, - TbotsProto::AutoChipOrKick()); - auto output = std::make_unique( - prim->direct_control()); + const auto prim = createDirectControlPrimitive( + Vector(), AngularVelocity(), 0.0, TbotsProto::AutoChipOrKick()); status.set_running_primitive(false); - return output; + return std::make_unique( + prim->direct_control()); } case TbotsProto::Primitive::kDirectControl: { @@ -113,26 +205,72 @@ std::unique_ptr PrimitiveExecutor::stepPrimi { if (!trajectory_path_.has_value() || !angular_trajectory_.has_value()) { - auto prim = createDirectControlPrimitive(Vector(), AngularVelocity(), 0.0, - TbotsProto::AutoChipOrKick()); - auto output = std::make_unique( + const auto prim = createDirectControlPrimitive( + Vector(), AngularVelocity(), 0.0, TbotsProto::AutoChipOrKick()); + LOG(INFO) << "Not moving because trajectory_path_ has value " + << trajectory_path_.has_value() << " angular has " + << angular_trajectory_.has_value(); + return std::make_unique( prim->direct_control()); - LOG(INFO) - << "Not moving because trajectory_path_ or angular_trajectory_ is not set"; - return output; } - Vector local_velocity = getTargetLinearVelocity(); - AngularVelocity angular_velocity = getTargetAngularVelocity(); + const Point target_position = trajectory_path_->getDestination(); + const Angle target_orientation = angular_trajectory_->getDestination(); + Vector error_linear = target_position - position_; + Angle error_angular = (target_orientation - orientation_).clamp(); - auto output = createDirectControlPrimitive( - local_velocity, angular_velocity, + Vector target_linear_velocity; + AngularVelocity target_angular_velocity; + + // if close enough, use special PID to destination + if (error_linear.lengthSquared() < LINEAR_PURE_PID_THRESHOLD_METERS) + { + target_linear_velocity = + globalToLocalVelocity({x_pid_close.step(error_linear.x()), + y_pid_close.step(error_linear.y())}, + orientation_); + } + else + { + // FEEDFORWARD velocities from the trajectory + const Vector trajectory_linear_velocity = getTargetLinearVelocity(); + error_linear = trajectory_path_->getPosition( + time_since_linear_trajectory_creation_.count()) - + position_; + const Vector pid_effort_linear(x_pid.step(error_linear.x()), + y_pid.step(error_linear.y())); + target_linear_velocity = globalToLocalVelocity( + trajectory_linear_velocity + pid_effort_linear, orientation_); + } + + if (error_angular.abs().toDegrees() < ANGULAR_PURE_PID_THRESHOLD_DEGREES) + { + target_angular_velocity = AngularVelocity::fromRadians( + w_pid_close.step(error_angular.toRadians())); + } + else + { + // FEEDFORWARD velocities from the trajectory + error_angular = (angular_trajectory_->getPosition( + time_since_angular_trajectory_creation_.count()) - + orientation_) + .clamp(); + const AngularVelocity trajectory_angular_velocity = + getTargetAngularVelocity(); + const AngularVelocity pid_effort_angular = + AngularVelocity::fromRadians(w_pid.step(error_angular.toRadians())); + target_angular_velocity = + trajectory_angular_velocity + pid_effort_angular; + } + + const auto prim = createDirectControlPrimitive( + target_linear_velocity, target_angular_velocity, convertDribblerModeToDribblerSpeed( current_primitive_.move().dribbler_mode(), robot_constants_), current_primitive_.move().auto_chip_or_kick()); return std::make_unique( - output->direct_control()); + prim->direct_control()); } case TbotsProto::Primitive::PRIMITIVE_NOT_SET: { @@ -145,8 +283,3 @@ std::unique_ptr PrimitiveExecutor::stepPrimi } return std::make_unique(); } - -void PrimitiveExecutor::setRobotId(const RobotId robot_id) -{ - robot_id_ = robot_id; -} diff --git a/src/software/embedded/primitive_executor.h b/src/software/embedded/primitive_executor.h index 145874f3c2..01ad540a12 100644 --- a/src/software/embedded/primitive_executor.h +++ b/src/software/embedded/primitive_executor.h @@ -1,4 +1,5 @@ #pragma once + #include "proto/primitive.pb.h" #include "proto/robot_status_msg.pb.h" #include "proto/tbots_software_msgs.pb.h" @@ -6,6 +7,7 @@ #include "software/ai/navigator/trajectory/trajectory_path.h" #include "software/geom/vector.h" #include "software/time/duration.h" +#include "software/util/pid/pid_controller.hpp" #include "software/world/robot_state.h" #include "software/world/team_types.h" @@ -14,42 +16,28 @@ class PrimitiveExecutor public: /** * Constructor - * @param time_step Time step which this primitive executor operates in - * @param robot_constants The robot constants for the robot which uses this primitive - * executor - * @param friendly_team_colour The colour of the friendly team - * @param robot_id The id of the robot which uses this primitive executor + * + * @param robot_constants The robot constants for the robot */ - explicit PrimitiveExecutor(const Duration time_step, - const robot_constants::RobotConstants& robot_constants, - const TeamColour friendly_team_colour, - const RobotId robot_id); + explicit PrimitiveExecutor(const robot_constants::RobotConstants& robot_constants); /** * Update primitive executor with a new Primitive + * * @param primitive_msg The primitive to start */ void updatePrimitive(const TbotsProto::Primitive& primitive_msg); /** - * Set the current primitive to the stop primitive - */ - void setStopPrimitive(); - - /** - * Update primitive executor with the current velocity of the robot + * Update primitive executor with the current velocity and orientation of the robot * - * @param local_velocity The current _local_ velocity + * @param position The current position + * @param velocity The current velocity + * @param orientation The current orientation of the robot * @param angular_velocity The current angular velocity */ - void updateVelocity(const Vector& local_velocity, - const AngularVelocity& angular_velocity); - - /** - * Set the robot id - * @param robot_id The id of the robot which uses this primitive executor - */ - void setRobotId(RobotId robot_id); + void updateState(const Point& position, const Vector& velocity, + const Angle& orientation, const AngularVelocity& angular_velocity); /** * Steps the current primitive and returns a direct control primitive with the @@ -63,38 +51,85 @@ class PrimitiveExecutor TbotsProto::PrimitiveExecutorStatus& status); private: - /* + /** + * Check if the given trajectory is "new". That is, if its destination differs + * meaningfully from our current trajectory. + * + * @param new_trajectory The new trajectory requested by the AI. + * @return True if the new trajectory requested is meaningfully different from the + * current trajectory. That is, if the destinations are new. + */ + bool isLinearTrajectoryNew(const std::optional& new_trajectory) const; + + /** + * Check if the given trajectory is "new". That is, if its destination differs + * meaningfully from our current trajectory. + * + * @param new_trajectory The new trajectory requested by the AI. + * @return True if the new trajectory requested is meaningfully different from the + * current trajectory. That is, if the destinations are new. + */ + bool isAngularTrajectoryNew(const BangBangTrajectory1DAngular& new_trajectory) const; + + /** * Compute the next target linear _local_ velocity the robot should be at. + * * @returns Vector The target linear _local_ velocity */ - Vector getTargetLinearVelocity(); + Vector getTargetLinearVelocity() const; /* - * Returns the next target angular velocity the robot + * Returns the next target angular velocity the robot should be at. * * @returns AngularVelocity The target angular velocity */ - AngularVelocity getTargetAngularVelocity(); + AngularVelocity getTargetAngularVelocity() const; TbotsProto::Primitive current_primitive_; - Duration time_since_trajectory_creation_; + + std::optional trajectory_path_; + std::optional angular_trajectory_; + + std::chrono::time_point last_step_time_; + std::chrono::duration time_since_linear_trajectory_creation_; + std::chrono::duration time_since_angular_trajectory_creation_; + + Point position_; Vector velocity_; AngularVelocity angular_velocity_; Angle orientation_; - TeamColour friendly_team_colour_; + + Point last_position_; + Angle last_orientation_; + robot_constants::RobotConstants robot_constants_; - std::optional trajectory_path_; - std::optional angular_trajectory_; - // TODO (#2855): Add dynamic time_step to `stepPrimitive` and remove this constant - // time step to be used, in Seconds - Duration time_step_; - RobotId robot_id_; + controls::PIDController x_pid = {0, 0, 0, 0}; + controls::PIDController y_pid = {0, 0, 0, 0}; + controls::PIDController w_pid = {0.5, 0, 0, 0}; - // Estimated delay between a vision frame to AI processing to robot executing - static constexpr double VISION_TO_ROBOT_DELAY_S = 0.03; + // When close to target position, ignore trajectory velocity and use pure PID control. + // These PIDs should be used in that case. + controls::PIDController x_pid_close = {2.0, 0.01, 0, 1}; + controls::PIDController y_pid_close = {3.0, 0.01, 0, 1}; + controls::PIDController w_pid_close = {0.3, 0.03, 2, 3}; + + // If distance between current linear trajectory destination and new one is larger + // than this, we change trajectories. + static constexpr double LINEAR_DESTINATION_THRESHOLD_METERS = 0.03; + static constexpr double ANGULAR_DESTINATION_THRESHOLD_DEGREES = 4; + + static constexpr double LINEAR_STALL_ERROR_MAX_METERS = 0.25; + static constexpr double ANGULAR_STALL_ERROR_MAX_DEGREES = 45; + + static constexpr double LINEAR_PURE_PID_THRESHOLD_METERS = 0.25; + static constexpr double ANGULAR_PURE_PID_THRESHOLD_DEGREES = 45; // The distance away from the destination at which we start dampening the velocity // to avoid jittering around the destination. - static constexpr double MAX_DAMPENING_VELOCITY_DISTANCE_M = 0.05; + static constexpr double MAX_DAMPENING_LINEAR_VELOCITY_DISTANCE_M = 0.05; + + // The angular distance away from the destination at which we start dampening + // the angular velocity to avoid jittering around the destination. + static constexpr double MAX_DAMPENING_ANGULAR_VELOCITY_DISTANCE_DEGREES = 5; }; diff --git a/src/software/embedded/robot_localizer.cpp b/src/software/embedded/robot_localizer.cpp new file mode 100644 index 0000000000..e2ab7e219d --- /dev/null +++ b/src/software/embedded/robot_localizer.cpp @@ -0,0 +1,309 @@ +#include "robot_localizer.h" + +#include + +RobotLocalizer::RobotLocalizer(const double process_noise_variance, + const double vision_noise_variance, + const double motor_sensor_noise_variance) + : process_linear_acceleration_noise_variance_(process_noise_variance), + process_angular_acceleration_noise_variance_(process_noise_variance) +{ + filter_.state_covariance = + Eigen::Vector(1, 1, 1, 1, 1, 1).asDiagonal(); + + filter_.measurement_covariance = + Eigen::Vector( + vision_noise_variance, vision_noise_variance, vision_noise_variance, + motor_sensor_noise_variance, motor_sensor_noise_variance, + motor_sensor_noise_variance, ImuService::IMU_VARIANCE) + .asDiagonal(); + + last_step_time_ = std::chrono::steady_clock::now(); +} + +void RobotLocalizer::step(const Vector& linear_acceleration) +{ + FilterStep step{ + .prediction = FilterStep::Predict{}, + .update = std::nullopt, + .state_estimate = filter_.state_estimate, + .state_covariance = filter_.state_covariance, + .time = std::chrono::steady_clock::now(), + }; + + const std::chrono::duration delta_time = step.time - last_step_time_; + const double delta_time_seconds = delta_time.count(); + last_step_time_ = step.time; + + // clang-format off + step.prediction->process_model << + 1, 0, 0, delta_time_seconds, 0, 0, + 0, 1, 0, 0, delta_time_seconds, 0, + 0, 0, 1, 0, 0, delta_time_seconds, + 0, 0, 0, 1, 0, 0, + 0, 0, 0, 0, 1, 0, + 0, 0, 0, 0, 0, 1; + // clang-format on + + const double delta_time_squared = delta_time_seconds * delta_time_seconds; + const double delta_time_cubed = delta_time_squared * delta_time_seconds; + const double delta_time_fourth = delta_time_cubed * delta_time_seconds; + + auto& process_covariance = step.prediction->process_covariance; + process_covariance.setZero(); + + process_covariance(static_cast(StateIndex::X_POSITION), + static_cast(StateIndex::X_POSITION)) = + delta_time_fourth / 4 * process_linear_acceleration_noise_variance_; + + process_covariance(static_cast(StateIndex::X_POSITION), + static_cast(StateIndex::X_VELOCITY)) = + delta_time_cubed / 2 * process_linear_acceleration_noise_variance_; + + process_covariance(static_cast(StateIndex::X_VELOCITY), + static_cast(StateIndex::X_POSITION)) = + delta_time_cubed / 2 * process_linear_acceleration_noise_variance_; + + process_covariance(static_cast(StateIndex::X_VELOCITY), + static_cast(StateIndex::X_VELOCITY)) = + delta_time_squared * process_linear_acceleration_noise_variance_; + + process_covariance(static_cast(StateIndex::Y_POSITION), + static_cast(StateIndex::Y_POSITION)) = + delta_time_fourth / 4 * process_linear_acceleration_noise_variance_; + + process_covariance(static_cast(StateIndex::Y_POSITION), + static_cast(StateIndex::Y_VELOCITY)) = + delta_time_cubed / 2 * process_linear_acceleration_noise_variance_; + + process_covariance(static_cast(StateIndex::Y_VELOCITY), + static_cast(StateIndex::Y_POSITION)) = + delta_time_cubed / 2 * process_linear_acceleration_noise_variance_; + + process_covariance(static_cast(StateIndex::Y_VELOCITY), + static_cast(StateIndex::Y_VELOCITY)) = + delta_time_squared * process_linear_acceleration_noise_variance_; + + process_covariance(static_cast(StateIndex::ORIENTATION), + static_cast(StateIndex::ORIENTATION)) = + delta_time_fourth / 4 * process_angular_acceleration_noise_variance_; + + process_covariance(static_cast(StateIndex::ORIENTATION), + static_cast(StateIndex::ANGULAR_VELOCITY)) = + delta_time_cubed / 2 * process_angular_acceleration_noise_variance_; + + process_covariance(static_cast(StateIndex::ANGULAR_VELOCITY), + static_cast(StateIndex::ORIENTATION)) = + delta_time_cubed / 2 * process_angular_acceleration_noise_variance_; + + process_covariance(static_cast(StateIndex::ANGULAR_VELOCITY), + static_cast(StateIndex::ANGULAR_VELOCITY)) = + delta_time_squared * process_angular_acceleration_noise_variance_; + + auto& control_model = step.prediction->control_model; + control_model.setZero(); + + control_model(static_cast(StateIndex::X_POSITION), + static_cast(ControlIndex::X_ACCELERATION)) = + delta_time_squared / 2; + + control_model(static_cast(StateIndex::Y_POSITION), + static_cast(ControlIndex::Y_ACCELERATION)) = + delta_time_squared / 2; + + control_model(static_cast(StateIndex::X_VELOCITY), + static_cast(ControlIndex::X_ACCELERATION)) = + delta_time_seconds; + + control_model(static_cast(StateIndex::Y_VELOCITY), + static_cast(ControlIndex::Y_ACCELERATION)) = + delta_time_seconds; + + step.prediction->control_input << linear_acceleration.x(), linear_acceleration.y(); + + filter_.process_model = step.prediction->process_model; + filter_.process_covariance = step.prediction->process_covariance; + filter_.control_model = step.prediction->control_model; + + history.push_front(step); + filter_.predict(step.prediction->control_input); +} + +void RobotLocalizer::updateVision(const Point& position, const Angle& orientation, + const double age_seconds) +{ + if (history.empty()) + { + updateFilterWithVision(position, orientation); + return; + } + + const auto current_time = std::chrono::steady_clock::now(); + const auto sample_age = std::chrono::duration(age_seconds); + + const auto rollback_point = std::find_if( + history.begin(), history.end(), + [&](const FilterStep& step) { return step.time - current_time >= sample_age; }); + + if (rollback_point == history.begin()) + { + // All history predates the sample, no need to rollback + updateFilterWithVision(position, orientation); + return; + } + + auto replay_iter = std::make_reverse_iterator(rollback_point); + + // Truncate history after the rollback point + history.erase(rollback_point, history.end()); + + filter_.state_estimate = replay_iter->state_estimate; + filter_.state_covariance = replay_iter->state_covariance; + + updateFilterWithVision(position, orientation); + + // Replay from the rollback point back to the current estimate + for (; replay_iter != history.rbegin(); --replay_iter) + { + if (replay_iter->prediction.has_value()) + { + const auto& prediction = replay_iter->prediction.value(); + filter_.process_model = prediction.process_model; + filter_.process_covariance = prediction.process_covariance; + filter_.control_model = prediction.control_model; + filter_.predict(prediction.control_input); + } + + if (replay_iter->update.has_value()) + { + const auto& update = replay_iter->update.value(); + filter_.measurement_model = update.measurement_model; + filter_.update(update.measurement); + } + } +} + +void RobotLocalizer::updateFilterWithVision(const Point& position, + const Angle& orientation) +{ + const double orientation_estimate = + filter_.state_estimate(static_cast(StateIndex::ORIENTATION)); + + Eigen::Vector measurement; + measurement.setZero(); + + measurement(static_cast(MeasurementIndex::VISION_X_POSITION)) = + position.x(); + measurement(static_cast(MeasurementIndex::VISION_Y_POSITION)) = + position.y(); + + // Coterminal angle that is closest to current estimate + measurement(static_cast(MeasurementIndex::VISION_ORIENTATION)) = + orientation_estimate + + (orientation - Angle::fromRadians(orientation_estimate)).clamp().toRadians(); + + filter_.measurement_model.setZero(); + filter_.measurement_model( + static_cast(MeasurementIndex::VISION_X_POSITION), + static_cast(StateIndex::X_POSITION)) = 1; + filter_.measurement_model( + static_cast(MeasurementIndex::VISION_Y_POSITION), + static_cast(StateIndex::Y_POSITION)) = 1; + filter_.measurement_model( + static_cast(MeasurementIndex::VISION_ORIENTATION), + static_cast(StateIndex::ORIENTATION)) = 1; + + filter_.update(measurement); +} + +void RobotLocalizer::updateMotorSensors(const Vector& velocity, + const AngularVelocity& angular_velocity) +{ + filter_.measurement_model.setZero(); + filter_.measurement_model( + static_cast(MeasurementIndex::MOTOR_X_VELOCITY), + static_cast(StateIndex::X_VELOCITY)) = 1; + filter_.measurement_model( + static_cast(MeasurementIndex::MOTOR_Y_VELOCITY), + static_cast(StateIndex::Y_VELOCITY)) = 1; + filter_.measurement_model( + static_cast(MeasurementIndex::MOTOR_ANGULAR_VELOCITY), + static_cast(StateIndex::ANGULAR_VELOCITY)) = 1; + + FilterStep::Update update{ + .measurement_model = filter_.measurement_model, + .measurement = Eigen::Vector::Zero(), + }; + + update.measurement(static_cast(MeasurementIndex::MOTOR_X_VELOCITY)) = + velocity.x(); + update.measurement(static_cast(MeasurementIndex::MOTOR_Y_VELOCITY)) = + velocity.y(); + update.measurement(static_cast( + MeasurementIndex::MOTOR_ANGULAR_VELOCITY)) = angular_velocity.toRadians(); + + const FilterStep step{ + .prediction = std::nullopt, + .update = update, + .state_estimate = filter_.state_estimate, + .state_covariance = filter_.state_covariance, + .time = std::chrono::steady_clock::now(), + }; + + history.push_front(step); + filter_.update(step.update->measurement); +} + +void RobotLocalizer::updateImu(const AngularVelocity& angular_velocity) +{ + filter_.measurement_model.setZero(); + filter_.measurement_model( + static_cast(MeasurementIndex::IMU_ANGULAR_VELOCITY), + static_cast(StateIndex::ANGULAR_VELOCITY)) = 1; + + FilterStep::Update update{ + .measurement_model = filter_.measurement_model, + .measurement = Eigen::Vector::Zero(), + }; + + update.measurement(static_cast( + MeasurementIndex::IMU_ANGULAR_VELOCITY)) = angular_velocity.toRadians(); + + const FilterStep step{ + .prediction = std::nullopt, + .update = update, + .state_estimate = filter_.state_estimate, + .state_covariance = filter_.state_covariance, + .time = std::chrono::steady_clock::now(), + }; + + history.push_front(step); + filter_.update(step.update->measurement); +} + +Point RobotLocalizer::getPosition() const +{ + return Point( + filter_.state_estimate(static_cast(StateIndex::X_POSITION)), + filter_.state_estimate(static_cast(StateIndex::Y_POSITION))); +} + +Vector RobotLocalizer::getVelocity() const +{ + return Vector( + filter_.state_estimate(static_cast(StateIndex::X_VELOCITY)), + filter_.state_estimate(static_cast(StateIndex::Y_VELOCITY))); +} + +Angle RobotLocalizer::getOrientation() const +{ + return Angle::fromRadians( + filter_.state_estimate(static_cast(StateIndex::ORIENTATION))) + .clamp(); +} + +AngularVelocity RobotLocalizer::getAngularVelocity() const +{ + return AngularVelocity::fromRadians( + filter_.state_estimate(static_cast(StateIndex::ANGULAR_VELOCITY))); +} diff --git a/src/software/embedded/robot_localizer.h b/src/software/embedded/robot_localizer.h new file mode 100644 index 0000000000..dc8ef6f9ed --- /dev/null +++ b/src/software/embedded/robot_localizer.h @@ -0,0 +1,161 @@ +#pragma once + +#include +#include +#include + +#include "proto/world.pb.h" +#include "software/embedded/services/imu.h" +#include "software/geom/angle.h" +#include "software/geom/point.h" +#include "software/geom/vector.h" +#include "software/sensor_fusion/filter/kalman_filter.hpp" +#include "software/util/make_enum/make_enum.hpp" + +MAKE_ENUM(StateIndex, X_POSITION, Y_POSITION, ORIENTATION, X_VELOCITY, Y_VELOCITY, + ANGULAR_VELOCITY); + +MAKE_ENUM(MeasurementIndex, VISION_X_POSITION, VISION_Y_POSITION, VISION_ORIENTATION, + MOTOR_X_VELOCITY, MOTOR_Y_VELOCITY, MOTOR_ANGULAR_VELOCITY, + IMU_ANGULAR_VELOCITY); + +MAKE_ENUM(ControlIndex, X_ACCELERATION, Y_ACCELERATION); + +/** + * Estimates robot orientation, angular velocity, and angular acceleration + * using a Kalman filter. + * + * The filter keeps a history of recent predict/update operations. When delayed + * vision data arrives, the localizer rewinds to the matching historical state, + * applies the delayed measurement, then replays newer steps to recover the + * current estimate. + */ +class RobotLocalizer +{ + public: + /** + * Creates a new robot localizer. + * + * The variances determine how strongly each source influences the estimate. + * + * @param process_noise_variance Variance applied to the process noise model. + * @param vision_noise_variance Variance of camera heading measurements. + * @param motor_sensor_noise_variance Variance of motor sensor angular velocity. + */ + explicit RobotLocalizer(double process_noise_variance, double vision_noise_variance, + double motor_sensor_noise_variance); + + /** + * Runs one prediction step using elapsed time since the previous call. + * + * @param linear_acceleration The current linear acceleration of the robot + */ + void step(const Vector& linear_acceleration); + + /** + * Update the robot's position and orientation from data reported by vision. + * + * @param position Vision reading of the robot's position in world space + * @param orientation Vision reading of the robot's orientation in world space + * @param age_seconds Age in seconds of the vision snapshot (time since it was taken) + */ + void updateVision(const Point& position, const Angle& orientation, + double age_seconds); + + /** + * Update the robot's velocity from data reported by motor sensors + * (i.e. encoders or Hall sensors). + * + * @param velocity Motor sensor reading of the robot's velocity in local space + * @param angular_velocity Motor sensor reading of the robot's angular velocity + */ + void updateMotorSensors(const Vector& velocity, + const AngularVelocity& angular_velocity); + + /** + * Update the angular velocity from IMU. + * + * @param angular_velocity angular velocity of the robot + */ + void updateImu(const AngularVelocity& angular_velocity); + + /** + * Gets the estimated position of the robot in world space. + * + * @return the estimated position of the robot in world space + */ + Point getPosition() const; + + /** + * Gets the estimated velocity of the robot in world space. + * + * @return the estimated velocity of the robot in world space + */ + Vector getVelocity() const; + + /** + * Gets the estimated orientation of the robot in world space. + * + * @return estimated orientation of the robot in world space + */ + Angle getOrientation() const; + + /** + * Gets the estimated angular velocity of the robot. + * + * @return estimated angular velocity of the robot + */ + AngularVelocity getAngularVelocity() const; + + private: + /** + * Update the Kalman filter with the robot's position and orientation from vision. + * + * @param position Vision reading of the robot's position in world space + * @param orientation Vision reading of the robot's orientation in world space + */ + void updateFilterWithVision(const Point& position, const Angle& orientation); + + static constexpr size_t STATE_SIZE = reflective_enum::size(); + static constexpr size_t MEASUREMENT_SIZE = reflective_enum::size(); + static constexpr size_t CONTROL_SIZE = reflective_enum::size(); + + /** + * Snapshot of a Kalman filter predict/update step needed for rollback/replay. + */ + struct FilterStep + { + struct Predict + { + Eigen::Matrix process_model; + Eigen::Matrix process_covariance; + Eigen::Matrix control_model; + Eigen::Vector control_input; + }; + + struct Update + { + Eigen::Matrix measurement_model; + Eigen::Vector measurement; + }; + + std::optional prediction; + std::optional update; + + Eigen::Vector state_estimate; + Eigen::Matrix state_covariance; + + std::chrono::time_point time; + }; + + KalmanFilter filter_; + + // Process noise variance used in prediction + double process_linear_acceleration_noise_variance_; + double process_angular_acceleration_noise_variance_; + + std::chrono::time_point last_step_time_; + + // History is ordered newest-first (front is the most recent step) + std::deque history; +}; diff --git a/src/software/embedded/services/BUILD b/src/software/embedded/services/BUILD index 528b1eedc9..8d0a6729c0 100644 --- a/src/software/embedded/services/BUILD +++ b/src/software/embedded/services/BUILD @@ -36,3 +36,36 @@ cc_library( "@boost//:filesystem", ], ) + +cc_library( + name = "imu", + srcs = ["imu.cpp"], + hdrs = ["imu.h"], + deps = [ + "//proto:tbots_cc_proto", + "//shared:constants", + "//software/geom:angular_acceleration", + "//software/geom:angular_velocity", + "//software/logger", + "@eigen", + ], +) + +cc_binary( + name = "robot_auto_test", + srcs = ["robot_auto_test.cpp"], + linkopts = [ + "-lpthread", + "-lrt", + ], + deps = [ + ":motor", + ":power", + "//proto/message_translation:tbots_geometry", + "//proto/primitive:primitive_msg_factory", + "//shared:robot_constants", + "//software/embedded:primitive_executor", + "//software/logger", + "@trinamic", + ], +) diff --git a/src/software/embedded/services/imu.cpp b/src/software/embedded/services/imu.cpp new file mode 100644 index 0000000000..ef3678459f --- /dev/null +++ b/src/software/embedded/services/imu.cpp @@ -0,0 +1,316 @@ +#include "imu.h" + +#include +#include +#include +#include + +#include // For SHRT_MAX + +#include "shared/constants.h" +#include "software/logger/logger.h" + +// these functions taken from +// https://git.kernel.org/pub/scm/utils/i2c-tools/i2c-tools.git/tree/lib/smbus.c +static __s32 i2c_smbus_access(int file, char read_write, __u8 command, int size, + union i2c_smbus_data* data) +{ + struct i2c_smbus_ioctl_data args; + __s32 err; + + args.read_write = read_write; + args.command = command; + args.size = size; + args.data = data; + + err = ioctl(file, I2C_SMBUS, &args); + if (err == -1) + err = -errno; + return err; +} + +static __s32 i2c_smbus_read_byte_data(int file, __u8 command) +{ + union i2c_smbus_data data; + int err; + + err = i2c_smbus_access(file, I2C_SMBUS_READ, command, I2C_SMBUS_BYTE_DATA, &data); + if (err < 0) + return err; + + return 0x0FF & data.byte; +} + +static __s32 i2c_smbus_write_byte_data(int file, __u8 command, __u8 value) +{ + union i2c_smbus_data data; + data.byte = value; + return i2c_smbus_access(file, I2C_SMBUS_WRITE, command, I2C_SMBUS_BYTE_DATA, &data); +} + + +ImuService::ImuService() : initialized_(false) +{ + // Establish connection to the IMU and verify that the who am I pin is correct. + file_descriptor_ = open(IMU_DEVICE.c_str(), O_RDWR); + int ret = ioctl(file_descriptor_, I2C_SLAVE_FORCE, 0x6b); + if (ret < 0) + { + LOG(WARNING) + << "Failed to initialize the IMU: failed to establish i2c connection."; + return; + } + if (i2c_smbus_read_byte_data(file_descriptor_, WHOAMI_REG) != 106) + { + LOG(WARNING) << "Failed to initialize the IMU: WHOAMI register " + << static_cast(WHOAMI_REG) << " read incorrectly."; + return; + } + // Attempt to enable gyro and accelerometer, checking that writes are successful + // See lsm6dsl datasheet for how to set these registers. + i2c_smbus_write_byte_data(file_descriptor_, ACCEL_CONTROL_REG, 0b01000000); + if (i2c_smbus_read_byte_data(file_descriptor_, ACCEL_CONTROL_REG) != 0b01000000) + { // write unsuccessful + LOG(WARNING) + << "Failed to initialize the IMU: writing to accelerometer config register " + << static_cast(ACCEL_CONTROL_REG) << " unsuccessful"; + return; + } + // Set Gyroscope output data rate to 208 Hz, setting FS to 1000 dps (pg 61 of + // datasheet for lsm6dsl, pg 21) + i2c_smbus_write_byte_data(file_descriptor_, GYRO_CONTROL_REG, 0b01011000); + if (i2c_smbus_read_byte_data(file_descriptor_, GYRO_CONTROL_REG) != 0b01011000) + { // write unsuccessful + LOG(WARNING) + << "Failed to initialize the IMU: writing to gyroscope config register " + << "unsuccessful"; + return; + } + + // Enable Gyro digital LPF1 and set bandwidth to ~65Hz (FTYPE=000) + // CTRL4_C: bit 1 (LPF1_SEL_G) = 1 + i2c_smbus_write_byte_data(file_descriptor_, CTRL4_C, 0b00000010); + // CTRL6_C: bits 2:0 (FTYPE) = 000 + i2c_smbus_write_byte_data(file_descriptor_, CTRL6_C, 0b00000000); + + // Enable Accelerometer digital LPF2 + // CTRL8_XL: bit 7 (LPF2_XL_EN) = 1 + i2c_smbus_write_byte_data(file_descriptor_, CTRL8_XL, 0b10000000); + + initialized_ = true; + LOG(INFO) << "Initialized IMU! Calibrating..."; + degrees_error_ = 0; + // get 100 sample average of stationary reading, so all future readings can be + // corrected + double sum = 0; + int valid_samples = 0; + for (int i = 0; i < 100; i++) + { + // Fixed: Call the actual implemented function name + auto poll = pollAngularVelocity(); + if (poll.has_value()) + { + sum += poll->toDegrees(); + valid_samples++; + } + usleep(50000); + } + + // TODO (3421): More robust calibration + if (valid_samples > 0) + { + degrees_error_ = sum / valid_samples; + LOG(INFO) << "IMU Calibration complete. Offset: " << degrees_error_ << " dps"; + } + else + { + LOG(WARNING) << "IMU Calibration failed: no valid samples received. Angular " + "stability will be poor."; + initialized_ = false; + } + + // Temporary: enable when need to calibrate + // Eigen::Vector2d deviation = calibrate_imu(); + // LOG(INFO) << "error: " << deviation.x() << deviation.y() << "."; +} + +ImuData ImuService::poll() +{ + std::optional angular_velocity = pollAngularVelocity(); + std::optional angular_acceleration = + pollAngularAcceleration(angular_velocity); + std::optional imu_linear_acceleration = pollLinearAcceleration(); + + std::optional linear_acceleration; + if (angular_velocity && angular_acceleration && imu_linear_acceleration) + { + linear_acceleration = transformLinearAcceleration( + *angular_velocity, *angular_acceleration, *imu_linear_acceleration); + } + + + return ImuData{angular_velocity, angular_acceleration, linear_acceleration}; +} +std::optional ImuService::readAndCombineByteData(uint8_t ls_reg, uint8_t ms_reg) +{ + int least_significant = i2c_smbus_read_byte_data(file_descriptor_, ls_reg); + int most_significant = i2c_smbus_read_byte_data(file_descriptor_, ms_reg); + + if (least_significant < 0 || most_significant < 0) + { + return std::nullopt; + } + + uint16_t combined = (static_cast(most_significant) << 8) | + static_cast(least_significant); + + return static_cast(combined); +} + +std::optional ImuService::pollAngularVelocity() +{ + if (!initialized_) + { + return std::nullopt; + } + + std::optional opt_full_word = + readAndCombineByteData(GYRO_LEAST_SIG_REG, GYRO_MOST_SIG_REG); + + if (!opt_full_word.has_value()) + { + return std::nullopt; + } + + int16_t full_word = opt_full_word.value(); + + double degrees_per_sec = static_cast(full_word) / + static_cast(SHRT_MAX) * IMU_FULL_SCALE_DPS; + + return AngularVelocity::fromRadians((degrees_per_sec - degrees_error_) * M_PI / 180); +} + + +std::optional ImuService::pollAngularAcceleration( + std::optional curr_angular_velocity) +{ + if (!initialized_) + { + return std::nullopt; + } + + if (!prev_angular_velocity_.has_value()) + { + prev_angular_velocity_ = pollAngularVelocity(); + prev_time_ = std::chrono::steady_clock::now(); + return std::nullopt; + } + + auto curr_time = std::chrono::steady_clock::now(); + + double dt = std::chrono::duration(curr_time - prev_time_).count(); + if (dt <= 0 || !curr_angular_velocity.has_value()) + return std::nullopt; + + double alpha = + (curr_angular_velocity->toRadians() - prev_angular_velocity_->toRadians()) / dt; + + prev_angular_velocity_ = curr_angular_velocity; + prev_time_ = curr_time; + + return AngularAcceleration::fromRadians(alpha); +} + + + +std::optional ImuService::pollLinearAcceleration() +{ + if (!initialized_) + { + return std::nullopt; + } + + std::optional opt_x = + readAndCombineByteData(ACCEL_X_LEAST_SIG_REG, ACCEL_X_MOST_SIG_REG); + std::optional opt_y = + readAndCombineByteData(ACCEL_Y_LEAST_SIG_REG, ACCEL_Y_MOST_SIG_REG); + + if (!opt_x.has_value() || !opt_y.has_value()) + { + return std::nullopt; + } + + int16_t raw_x = opt_x.value(); + int16_t raw_y = opt_y.value(); + + double a_x = (static_cast(raw_x) / SHRT_MAX) * ACCELEROMETER_FULL_SCALE_G * + ACCELERATION_DUE_TO_GRAVITY_METERS_PER_SECOND_SQUARED; + + double a_y = (static_cast(raw_y) / SHRT_MAX) * ACCELEROMETER_FULL_SCALE_G * + ACCELERATION_DUE_TO_GRAVITY_METERS_PER_SECOND_SQUARED; + + return Eigen::Vector2d(a_x, a_y); +} + +Eigen::Vector2d ImuService::transformLinearAcceleration(AngularVelocity omega, + AngularAcceleration alpha, + Eigen::Vector2d imu_acceleration) +{ + Eigen::Vector2d r(IMU_OFFSET_X, IMU_OFFSET_Y); + + double w = omega.toRadians(); + double a = alpha.toRadians(); + + // tangential: alpha x r → (-alpha*ry, alpha*rx) + Eigen::Vector2d tangential(-a * r.y(), a * r.x()); + + // centripetal: omega^2 * r + Eigen::Vector2d centripetal = (w * w) * r; + + return imu_acceleration + tangential - centripetal; +} + +Eigen::Vector2d ImuService::calibrate_imu() +{ + LOG(INFO) << "Start IMU x,y calibration" << std::endl; + Eigen::MatrixXd A(2 * 100, 2); + Eigen::VectorXd b(2 * 100); + int valid = 0; + + for (int i = 0; i < 100; i++) + { + // Fixed: Call the actual implemented function name + auto omega_opt = pollAngularVelocity(); + if (!omega_opt.has_value()) + { + continue; + } + auto alpha_opt = pollAngularAcceleration(omega_opt); + auto acc = pollLinearAcceleration(); + if (omega_opt.has_value() && alpha_opt.has_value() && acc.has_value()) + { + double omega = omega_opt->toRadians(); + double alpha = alpha_opt->toRadians(); + + A(2 * valid, 0) = -omega * omega; + A(2 * valid, 1) = -alpha; + A(2 * valid + 1, 0) = alpha; + A(2 * valid + 1, 1) = -omega * omega; + + b(2 * valid) = acc->x(); + b(2 * valid + 1) = acc->y(); + valid++; + } + usleep(100000); + } + if (valid < 10) + { + LOG(WARNING) << "Calibration failed: insufficient valid samples"; + return Eigen::Vector2d(0.0, 0.0); + } + Eigen::MatrixXd A_valid = A.topRows(2 * valid); + Eigen::VectorXd b_valid = b.topRows(2 * valid); + Eigen::Vector2d X = + A_valid.bdcSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(b_valid); + return X; +} diff --git a/src/software/embedded/services/imu.h b/src/software/embedded/services/imu.h new file mode 100644 index 0000000000..aa40cab509 --- /dev/null +++ b/src/software/embedded/services/imu.h @@ -0,0 +1,122 @@ +#pragma once + +#include +#include +#include +#include + +#include "proto/tbots_software_msgs.pb.h" +#include "software/geom/angular_acceleration.h" +#include "software/geom/angular_velocity.h" + +/** + * Handles low level IMU I2C communication, and some minor offset filtering. + */ +struct ImuData +{ + std::optional angular_velocity; + std::optional angular_acceleration; + std::optional linear_acceleration; +}; + +class ImuService +{ + public: + /** + * Constructs and initializes a new IMU service object. + * + * If successfully initialized, will try to do a simple calibration of the IMU. + */ + ImuService(); + + ImuData poll(); + + // Variance from datasheet (in rad^2/s^2) + static constexpr double IMU_VARIANCE = + (4.0 * 14.4222 / 1000.0 * M_PI / 180.0) * (4.0 * 14.4222 / 1000.0 * M_PI / 180.0); + + private: + /** + * Polls the latest IMU reading of the angular velocity of the robot on the z axis + * @return the current angular velocty of the robot on the z axis + */ + std::optional pollAngularVelocity(); + /** + * Computes angular acceleration from successive angular velocity readings + * @return the current angular acceleration of the robot on the z axis + */ + std::optional pollAngularAcceleration( + std::optional curr_angular_velocity); + /** + * Polls the latest IMU reading of the linear acceleration of the robot on the z plane + * @return the current linear acceleration of the robot on the z plane + */ + std::optional pollLinearAcceleration(); + /** + * Reads byte data from two registers, and combine them into a single value + * @parama ls_reg register of the least significant register + * @parama ms_reg register of the most significant register + * @return the combined integer value of the two registers + */ + std::optional readAndCombineByteData(uint8_t ls_reg, uint8_t ms_reg); + /** + * Transform linear acceleration to robot center of mass + * @parama omega angular veocity of the robot + * @param alpha angular acceleration of the robot + * @parama imu_acceleration lianer acceleration of imu + * @return the linear acceleration of the robot center of mass + */ + Eigen::Vector2d transformLinearAcceleration(AngularVelocity omega, + AngularAcceleration alpha, + Eigen::Vector2d imu_acceleration); + /** + * Finds the deviation of the imu from the robot center of mass by taking 100 data + * points, applying a least square regression on the relative acceleration formula, + * and solving for the distance + * @return the deviation of the imu from the robot center of mass + */ + Eigen::Vector2d calibrate_imu(); + + + bool initialized_ = false; + + int file_descriptor_ = 0; + + double degrees_error_; + + // Maps the maximum raw reading from 16-bit integer to be 2 times gravity + static constexpr double ACCELEROMETER_FULL_SCALE_G = 2.0; + // Same for gyroscope, 1000 degrees per second + static constexpr double IMU_FULL_SCALE_DPS = 1000.0; + + /// Various i2c registers. + static const uint8_t WHOAMI_REG = 0xf; + static const uint8_t ACCEL_CONTROL_REG = 0x10; + static const uint8_t GYRO_CONTROL_REG = 0x11; + static const uint8_t CTRL4_C = 0x13; + static const uint8_t CTRL6_C = 0x15; + static const uint8_t CTRL8_XL = 0x17; + + // Device path for the IMU + inline static const std::string IMU_DEVICE = "/dev/i2c-1"; + + // Gyroscope Z-axis (Yaw) Output Data Registers + static constexpr uint8_t GYRO_LEAST_SIG_REG = 0x26; // OUTZ_L_G + static constexpr uint8_t GYRO_MOST_SIG_REG = 0x27; // OUTZ_H_G + + // Accelerometer X-axis Output Data Registers + static constexpr uint8_t ACCEL_X_LEAST_SIG_REG = 0x28; // OUTX_L_XL + static constexpr uint8_t ACCEL_X_MOST_SIG_REG = 0x29; // OUTX_H_XL + + // Accelerometer Y-axis Output Data Registers + static constexpr uint8_t ACCEL_Y_LEAST_SIG_REG = 0x2A; // OUTY_L_XL + static constexpr uint8_t ACCEL_Y_MOST_SIG_REG = 0x2B; // OUTY_H_XL + + // IMU offset for acceleration calculation + static constexpr double IMU_OFFSET_X = 0.0; + static constexpr double IMU_OFFSET_Y = 0.0; + + // prev time and angular acceleration + std::optional prev_angular_velocity_; + std::chrono::steady_clock::time_point prev_time_; +}; diff --git a/src/software/embedded/services/motor.cpp b/src/software/embedded/services/motor.cpp index 0c6515f5cf..132e94e681 100644 --- a/src/software/embedded/services/motor.cpp +++ b/src/software/embedded/services/motor.cpp @@ -138,35 +138,6 @@ TbotsProto::MotorStatus MotorService::poll(const TbotsProto::MotorControl& motor TbotsProto::MotorStatus motor_status = createMotorStatus(current_wheel_velocities, dribbler_rpm); - if (std::abs(current_wheel_velocities[FRONT_RIGHT_WHEEL_SPACE_INDEX] - - prev_wheel_velocities_[FRONT_RIGHT_WHEEL_SPACE_INDEX]) > - RUNAWAY_PROTECTION_THRESHOLD_MPS) - { - motor_controller_->immediatelyDisable(); - LOG(FATAL) << "Front right motor runaway"; - } - else if (std::abs(current_wheel_velocities[FRONT_LEFT_WHEEL_SPACE_INDEX] - - prev_wheel_velocities_[FRONT_LEFT_WHEEL_SPACE_INDEX]) > - RUNAWAY_PROTECTION_THRESHOLD_MPS) - { - motor_controller_->immediatelyDisable(); - LOG(FATAL) << "Front left motor runaway"; - } - else if (std::abs(current_wheel_velocities[BACK_LEFT_WHEEL_SPACE_INDEX] - - prev_wheel_velocities_[BACK_LEFT_WHEEL_SPACE_INDEX]) > - RUNAWAY_PROTECTION_THRESHOLD_MPS) - { - motor_controller_->immediatelyDisable(); - LOG(FATAL) << "Back left motor runaway"; - } - else if (std::abs(current_wheel_velocities[BACK_RIGHT_WHEEL_SPACE_INDEX] - - prev_wheel_velocities_[BACK_RIGHT_WHEEL_SPACE_INDEX]) > - RUNAWAY_PROTECTION_THRESHOLD_MPS) - { - motor_controller_->immediatelyDisable(); - LOG(FATAL) << "Back right motor runaway"; - } - // Convert to Euclidean velocity_delta const EuclideanSpace_t current_euclidean_velocity = euclidean_to_four_wheel_.getEuclideanVelocity(current_wheel_velocities); diff --git a/src/software/embedded/services/motor.h b/src/software/embedded/services/motor.h index 445fb03ff5..7a882be8e1 100644 --- a/src/software/embedded/services/motor.h +++ b/src/software/embedded/services/motor.h @@ -102,6 +102,5 @@ class MotorService static constexpr int MOTOR_RESET_TIME_THRESHOLD_S = 60; static constexpr int MOTOR_RESET_THRESHOLD_COUNT = 3; - static constexpr double RUNAWAY_PROTECTION_THRESHOLD_MPS = 2.00; static constexpr int DRIBBLER_ACCELERATION_THRESHOLD_RPM_PER_S_2 = 10000; }; diff --git a/src/software/embedded/thunderloop.cpp b/src/software/embedded/thunderloop.cpp index b7b300a393..5b3294fd1b 100644 --- a/src/software/embedded/thunderloop.cpp +++ b/src/software/embedded/thunderloop.cpp @@ -4,16 +4,19 @@ #include #include "proto/message_translation/tbots_protobuf.h" +#include "proto/primitive/primitive_msg_factory.h" #include "proto/robot_crash_msg.pb.h" #include "proto/robot_status_msg.pb.h" #include "proto/tbots_software_msgs.pb.h" #include "shared/constants.h" -#include "shared/robot_constants.h" +#include "software/constants.h" #include "software/embedded/primitive_executor.h" +#include "software/embedded/services/imu.h" #include "software/embedded/services/motor.h" #include "software/logger/logger.h" #include "software/logger/network_logger.h" #include "software/networking/tbots_network_exception.h" +#include "software/physics/velocity_conversion_util.h" #include "software/tracy/tracy_constants.h" #include "software/util/scoped_timespec_timer/scoped_timespec_timer.h" @@ -82,13 +85,16 @@ Thunderloop::Thunderloop(const robot_constants::RobotConstants& robot_constants, kick_constant_(std::stoi(toml_config_client_->get(ROBOT_KICK_CONSTANT_CONFIG_KEY))), chip_pulse_width_( std::stoi(toml_config_client_->get(ROBOT_CHIP_PULSE_WIDTH_CONFIG_KEY))), - primitive_executor_(Duration::fromSeconds(1.0 / loop_hz), robot_constants, - TeamColour::YELLOW, robot_id_) + primitive_executor_(robot_constants), + robot_localizer_(robot_constants.kalman_process_noise_variance_rad_per_s_4, + robot_constants.kalman_vision_noise_variance_rad_2, + robot_constants.kalman_motor_sensor_noise_variance_rad_per_s_2) { waitForNetworkUp(); g3::overrideSetupSignals({}); - NetworkLoggerSingleton::initializeLogger(robot_id_, enable_log_merging); + NetworkLoggerSingleton::initializeLogger(robot_id_, enable_log_merging, + network_interface_); // catch all catch-able signals std::signal(SIGSEGV, tbotsExit); @@ -121,7 +127,11 @@ Thunderloop::Thunderloop(const robot_constants::RobotConstants& robot_constants, motor_service_ = std::make_unique(robot_constants); g_motor_service = motor_service_.get(); motor_service_->setup(); - LOG(INFO) << "THUNDERLOOP: Motor Service initialized!"; + + LOG(INFO) << "THUNDERLOOP: Motor Service initialized! Next initializing IMU Service"; + + imu_service_ = std::make_unique(); + LOG(INFO) << "THUNDERLOOP: IMU Service initialized!"; LOG(INFO) << "THUNDERLOOP: finished initialization with ROBOT ID: " << robot_id_ << ", CHANNEL ID: " << channel_id_ @@ -229,6 +239,17 @@ void Thunderloop::runLoop() // Save new primitive primitive_ = new_primitive; + if (primitive_.has_move()) + { + const Point position = + createPoint(primitive_.move().xy_traj_params().start_position()); + + const Angle orientation = + createAngle(primitive_.move().w_traj_params().start_angle()); + + robot_localizer_.updateVision(position, orientation, RTT_S / 2); + } + // Update primitive executor's primitive set { clock_gettime(CLOCK_MONOTONIC, &last_primitive_received_time); @@ -244,12 +265,39 @@ void Thunderloop::runLoop() } } + const ImuData imu_poll = imu_service_->poll(); + + if (imu_poll.angular_velocity.has_value()) + { + robot_localizer_.updateImu(imu_poll.angular_velocity.value()); + } + if (motor_status_.has_value()) { auto status = motor_status_.value(); - primitive_executor_.updateVelocity( - createVector(status.local_velocity()), + + robot_localizer_.updateMotorSensors( + localToGlobalVelocity(createVector(status.local_velocity()), + robot_localizer_.getOrientation()), createAngularVelocity(status.angular_velocity())); + + Vector linear_acceleration; + if (imu_poll.linear_acceleration.has_value()) + { + const auto accel = imu_poll.linear_acceleration.value(); + linear_acceleration = Vector(accel[1], accel[0]); + LOG(PLOTJUGGLER) << *createPlotJugglerValue({ + {"linear_acceleration_x", linear_acceleration.x()}, + {"linear_acceleration_y", linear_acceleration.y()}, + }); + } + + robot_localizer_.step(linear_acceleration); + + primitive_executor_.updateState(robot_localizer_.getPosition(), + robot_localizer_.getVelocity(), + robot_localizer_.getOrientation(), + robot_localizer_.getAngularVelocity()); } // Timeout Overrides for Primitives @@ -268,7 +316,7 @@ void Thunderloop::runLoop() if (nanoseconds_elapsed_since_last_primitive > PACKET_TIMEOUT_NS) { - primitive_executor_.setStopPrimitive(); + primitive_executor_.updatePrimitive(*createStopPrimitiveProto()); } direct_control_ = diff --git a/src/software/embedded/thunderloop.h b/src/software/embedded/thunderloop.h index 6d12cd4c0d..fa5cfe1d27 100644 --- a/src/software/embedded/thunderloop.h +++ b/src/software/embedded/thunderloop.h @@ -10,6 +10,8 @@ #include "shared/constants.h" #include "shared/robot_constants.h" #include "software/embedded/primitive_executor.h" +#include "software/embedded/robot_localizer.h" +#include "software/embedded/services/imu.h" #include "software/embedded/services/motor.h" #include "software/embedded/services/network/network.h" #include "software/embedded/services/power.h" @@ -60,6 +62,7 @@ class Thunderloop std::unique_ptr motor_service_; std::unique_ptr network_service_; std::unique_ptr power_service_; + std::unique_ptr imu_service_; // TOML config client std::unique_ptr toml_config_client_; @@ -118,6 +121,7 @@ class Thunderloop */ TbotsProto::PowerStatus pollPowerService(struct timespec& poll_time); + /** * Wait for networking communication to be established. This function is blocking. */ @@ -155,6 +159,9 @@ class Thunderloop // Primitive Executor PrimitiveExecutor primitive_executor_; + // Robot localization model + RobotLocalizer robot_localizer_; + // 500 millisecond timeout on receiving primitives before we stop the robots const double PACKET_TIMEOUT_NS = 500.0 * NANOSECONDS_PER_MILLISECOND; diff --git a/src/software/logger/network_logger.cpp b/src/software/logger/network_logger.cpp index 4620e14393..833d103ef3 100644 --- a/src/software/logger/network_logger.cpp +++ b/src/software/logger/network_logger.cpp @@ -5,7 +5,8 @@ std::shared_ptr NetworkLoggerSingleton::instance; -NetworkLoggerSingleton::NetworkLoggerSingleton(RobotId robot_id, bool enable_log_merging) +NetworkLoggerSingleton::NetworkLoggerSingleton(RobotId robot_id, bool enable_log_merging, + const std::string& network_interface) { logWorker = g3::LogWorker::createLogWorker(); @@ -18,8 +19,10 @@ NetworkLoggerSingleton::NetworkLoggerSingleton(RobotId robot_id, bool enable_log logWorker->addSink(std::make_unique(CSV_PATH), &CSVSink::appendToFile); - logWorker->addSink(std::make_unique("tbotswifi5"), - &PlotJugglerSink::sendToPlotJuggler); + // Sink for PlotJuggler plotting + auto plotjuggler_handle = + logWorker->addSink(std::make_unique(network_interface), + &PlotJugglerSink::sendToPlotJuggler); g3::only_change_at_initialization::addLogLevel(CSV); g3::only_change_at_initialization::addLogLevel(PLOTJUGGLER); @@ -27,12 +30,13 @@ NetworkLoggerSingleton::NetworkLoggerSingleton(RobotId robot_id, bool enable_log g3::initializeLogging(logWorker.get()); } -void NetworkLoggerSingleton::initializeLogger(RobotId robot_id, bool enable_log_merging) +void NetworkLoggerSingleton::initializeLogger(RobotId robot_id, bool enable_log_merging, + const std::string& network_interface) { if (!instance) { instance = std::shared_ptr( - new NetworkLoggerSingleton(robot_id, enable_log_merging)); + new NetworkLoggerSingleton(robot_id, enable_log_merging, network_interface)); } } diff --git a/src/software/logger/network_logger.h b/src/software/logger/network_logger.h index eec4a4b302..7fe021b845 100644 --- a/src/software/logger/network_logger.h +++ b/src/software/logger/network_logger.h @@ -21,7 +21,8 @@ class NetworkLoggerSingleton * Initializes a g3log logger for the calling program. This should only be * called once at the start of a program. */ - static void initializeLogger(RobotId robot_id, bool enable_log_merging); + static void initializeLogger(RobotId robot_id, bool enable_log_merging, + const std::string& network_interface = "tbotswifi5"); /** * Updates the underlying UDP sender associated with this network sink. Useful when a @@ -31,7 +32,8 @@ class NetworkLoggerSingleton std::shared_ptr> new_sender); private: - NetworkLoggerSingleton(RobotId robot_id, bool enable_log_merging); + NetworkLoggerSingleton(RobotId robot_id, bool enable_log_merging, + const std::string& network_interface); std::unique_ptr logWorker; std::unique_ptr> network_sink_handle; diff --git a/src/software/physics/euclidean_to_wheel.cpp b/src/software/physics/euclidean_to_wheel.cpp index 1b9cde8dac..7bacff40f4 100644 --- a/src/software/physics/euclidean_to_wheel.cpp +++ b/src/software/physics/euclidean_to_wheel.cpp @@ -8,6 +8,50 @@ #include "software/geom/angular_velocity.h" #include "software/geom/vector.h" +#ifdef DEBUG_WHEEL +EuclideanToWheel::EuclideanToWheel(const robot_constants::RobotConstants& robot_constants) + : robot_constants_(robot_constants) +{ + // Angles [rads] + auto p = robot_constants_.front_wheel_angle_deg * M_PI / 180.; + auto t = robot_constants_.back_wheel_angle_deg * M_PI / 180.; + + auto cos_p = std::cos(p); + auto sin_p = std::sin(p); + auto cos_t = std::cos(t); + auto sin_t = std::sin(t); + + // 1. Physical wheel coordinates in meters + // Mapped to the robot frame: +X = Right, +Y = Forward + double fr_x = 0.06632, fr_y = 0.03485; + double br_x = 0.05592, br_y = -0.04985; + + // 2. Unit drive vectors (extracted directly from the matrix's linear columns) + double fr_dx = -sin_p, fr_dy = cos_p; + double br_dx = sin_t, br_dy = cos_t; + + // 3. Dynamically calculate the rotational lever arms using the 2D cross product + // Lever Arm = | X * Dy - Y * Dx | + double rot_f = std::abs((fr_x * fr_dy) - (fr_y * fr_dx)); + double rot_b = std::abs((br_x * br_dy) - (br_y * br_dx)); + + // clang-format off + euclidean_to_wheel_velocity_D_ << + -sin_p, cos_p, rot_f, + -sin_p, -cos_p, rot_f, + sin_t, -cos_t, rot_b, + sin_t, cos_t, rot_b; + // clang-format on + + // Calculate Pseudo-inverse dynamically + wheel_to_euclidean_velocity_D_inverse_ = + (euclidean_to_wheel_velocity_D_.transpose() * euclidean_to_wheel_velocity_D_) + .inverse() * + euclidean_to_wheel_velocity_D_.transpose(); +} + +#else + EuclideanToWheel::EuclideanToWheel(const robot_constants::RobotConstants& robot_constants) : robot_constants_(robot_constants) { @@ -56,6 +100,26 @@ EuclideanToWheel::EuclideanToWheel(const robot_constants::RobotConstants& robot_ // clang-format on } +#endif + +#ifdef DEBUG_WHEEL +WheelSpace_t EuclideanToWheel::getWheelVelocity(EuclideanSpace_t euclidean_velocity) const +{ + // The generalized D matrix natively handles the w -> tangential velocity + // conversion because its third column already represents the lever arm in meters. + return euclidean_to_wheel_velocity_D_ * euclidean_velocity; +} + + +EuclideanSpace_t EuclideanToWheel::getEuclideanVelocity( + const WheelSpace_t& wheel_velocity) const +{ + // The D inverse matrix natively outputs w in rad/s. + return wheel_to_euclidean_velocity_D_inverse_ * wheel_velocity; +} + +#else + WheelSpace_t EuclideanToWheel::getWheelVelocity(EuclideanSpace_t euclidean_velocity) const { // need to multiply the angular velocity by the robot radius to @@ -80,6 +144,7 @@ EuclideanSpace_t EuclideanToWheel::getEuclideanVelocity( return euclidean_velocity; } +#endif WheelSpace_t EuclideanToWheel::rampWheelVelocity( const WheelSpace_t& current_wheel_velocity, const WheelSpace_t& target_wheel_velocity, diff --git a/src/software/physics/euclidean_to_wheel.h b/src/software/physics/euclidean_to_wheel.h index b1fe981abe..a20f723159 100644 --- a/src/software/physics/euclidean_to_wheel.h +++ b/src/software/physics/euclidean_to_wheel.h @@ -7,6 +7,8 @@ #include "software/geom/angular_velocity.h" #include "software/geom/vector.h" +#define DEBUG_WHEEL + /** * Vector representation of 2D Euclidean space. * diff --git a/src/software/physics/euclidean_to_wheel_test.cpp b/src/software/physics/euclidean_to_wheel_test.cpp index b546c9f01f..1cd3024560 100644 --- a/src/software/physics/euclidean_to_wheel_test.cpp +++ b/src/software/physics/euclidean_to_wheel_test.cpp @@ -106,6 +106,51 @@ TEST_F(EuclideanToWheelTest, test_target_wheel_speeds_negative_y) -calculated_wheel_speeds[BACK_RIGHT_WHEEL_SPACE_INDEX]); } +#ifdef DEBUG_WHEEL +TEST_F(EuclideanToWheelTest, test_target_wheel_speeds_positive_w) +{ + // Test +w/counter-clockwise + target_euclidean_velocity = {0, 0, 1}; + calculated_wheel_speeds = + euclidean_to_four_wheel.getWheelVelocity(target_euclidean_velocity); + + // Because the wheels are offset, their speed at 1 rad/s equals their + // exact physical lever arm (in meters), NOT the nominal robot_radius. + // Based on the physical offsets, the lever arm evaluates to 0.0746 meters. + double expected_lever_arm = 0.0746; + + EXPECT_NEAR(calculated_wheel_speeds[FRONT_RIGHT_WHEEL_SPACE_INDEX], + expected_lever_arm, 0.001); + EXPECT_NEAR(calculated_wheel_speeds[FRONT_LEFT_WHEEL_SPACE_INDEX], expected_lever_arm, + 0.001); + EXPECT_NEAR(calculated_wheel_speeds[BACK_LEFT_WHEEL_SPACE_INDEX], expected_lever_arm, + 0.001); + EXPECT_NEAR(calculated_wheel_speeds[BACK_RIGHT_WHEEL_SPACE_INDEX], expected_lever_arm, + 0.001); +} + +TEST_F(EuclideanToWheelTest, test_target_wheel_speeds_negative_w) +{ + // Test -w/clockwise + target_euclidean_velocity = {0, 0, -1}; + calculated_wheel_speeds = + euclidean_to_four_wheel.getWheelVelocity(target_euclidean_velocity); + + // Because the wheels are offset, their speed at -1 rad/s equals their + // exact physical lever arm (in meters) multiplied by the negative velocity. + double expected_lever_arm = -0.0746; + + EXPECT_NEAR(calculated_wheel_speeds[FRONT_RIGHT_WHEEL_SPACE_INDEX], + expected_lever_arm, 0.001); + EXPECT_NEAR(calculated_wheel_speeds[FRONT_LEFT_WHEEL_SPACE_INDEX], expected_lever_arm, + 0.001); + EXPECT_NEAR(calculated_wheel_speeds[BACK_LEFT_WHEEL_SPACE_INDEX], expected_lever_arm, + 0.001); + EXPECT_NEAR(calculated_wheel_speeds[BACK_RIGHT_WHEEL_SPACE_INDEX], expected_lever_arm, + 0.001); +} +#else + TEST_F(EuclideanToWheelTest, test_target_wheel_speeds_positive_w) { // Test +w/counter-clockwise @@ -123,6 +168,7 @@ TEST_F(EuclideanToWheelTest, test_target_wheel_speeds_positive_w) EXPECT_DOUBLE_EQ(calculated_wheel_speeds[BACK_RIGHT_WHEEL_SPACE_INDEX], robot_radius); } + TEST_F(EuclideanToWheelTest, test_target_wheel_speeds_negative_w) { // Test -w/clockwise @@ -143,6 +189,8 @@ TEST_F(EuclideanToWheelTest, test_target_wheel_speeds_negative_w) -robot_radius); } +#endif + TEST_F(EuclideanToWheelTest, test_conversion_is_linear) { target_euclidean_velocity = {3, 1, 5}; diff --git a/src/software/simulation/er_force_simulator.cpp b/src/software/simulation/er_force_simulator.cpp index 7a645b85e1..7593b87ce1 100644 --- a/src/software/simulation/er_force_simulator.cpp +++ b/src/software/simulation/er_force_simulator.cpp @@ -280,16 +280,14 @@ void ErForceSimulator::setRobots( { if (side == gameController::Team::BLUE) { - auto robot_primitive_executor = std::make_shared( - Duration::fromSeconds(primitive_executor_time_step_s), robot_constants, - TeamColour::BLUE, id); + auto robot_primitive_executor = + std::make_shared(robot_constants); blue_primitive_executor_map.insert({id, robot_primitive_executor}); } else { - auto robot_primitive_executor = std::make_shared( - Duration::fromSeconds(primitive_executor_time_step_s), robot_constants, - TeamColour::YELLOW, id); + auto robot_primitive_executor = + std::make_shared(robot_constants); yellow_primitive_executor_map.insert({id, robot_primitive_executor}); } } @@ -299,19 +297,22 @@ void ErForceSimulator::setYellowRobotPrimitiveSet( const TbotsProto::PrimitiveSet& primitive_set_msg, std::unique_ptr world_msg) { - auto sim_state = getSimulatorState(); - const auto& sim_robots = sim_state.yellow_robots(); - const auto robot_to_vel_pair_map = getRobotIdToLocalVelocityMap(sim_robots); + auto sim_state = getSimulatorState(); + const auto& sim_robots = sim_state.yellow_robots(); + const auto robot_to_state = getRobotIdToStateMap(sim_robots); yellow_team_world_msg = std::move(world_msg); const TbotsProto::World world_proto = *yellow_team_world_msg; + for (auto& [robot_id, primitive] : primitive_set_msg.robot_primitives()) { - if (robot_to_vel_pair_map.contains(robot_id)) + if (robot_to_state.contains(robot_id)) { - auto& [local_vel, angular_vel] = robot_to_vel_pair_map.at(robot_id); + const auto& [position, orientation, velocity, angular_velocity] = + robot_to_state.at(robot_id); setRobotPrimitive(robot_id, primitive_set_msg, yellow_primitive_executor_map, - world_proto, local_vel, angular_vel); + world_proto, position, orientation, velocity, + angular_velocity); } } } @@ -320,20 +321,22 @@ void ErForceSimulator::setBlueRobotPrimitiveSet( const TbotsProto::PrimitiveSet& primitive_set_msg, std::unique_ptr world_msg) { - auto sim_state = getSimulatorState(); - const auto& sim_robots = sim_state.blue_robots(); - const auto robot_to_vel_pair_map = getRobotIdToLocalVelocityMap(sim_robots); + auto sim_state = getSimulatorState(); + const auto& sim_robots = sim_state.blue_robots(); + const auto robot_to_state = getRobotIdToStateMap(sim_robots); blue_team_world_msg = std::move(world_msg); const TbotsProto::World world_proto = *blue_team_world_msg; for (auto& [robot_id, primitive] : primitive_set_msg.robot_primitives()) { - if (robot_to_vel_pair_map.contains(robot_id)) + if (robot_to_state.contains(robot_id)) { - auto& [local_vel, angular_vel] = robot_to_vel_pair_map.at(robot_id); + const auto& [position, orientation, velocity, angular_velocity] = + robot_to_state.at(robot_id); setRobotPrimitive(robot_id, primitive_set_msg, blue_primitive_executor_map, - world_proto, local_vel, angular_vel); + world_proto, position, orientation, velocity, + angular_velocity); } } } @@ -342,8 +345,8 @@ void ErForceSimulator::setRobotPrimitive( RobotId id, const TbotsProto::PrimitiveSet& primitive_set_msg, std::unordered_map>& robot_primitive_executor_map, - const TbotsProto::World& world_msg, const Vector& local_velocity, - const AngularVelocity angular_velocity) + const TbotsProto::World& world_msg, const Point& position, const Angle& orientation, + const Vector& velocity, const AngularVelocity& angular_velocity) { // Set to NEG_X because the world msg in this simulator is normalized // correctly @@ -352,8 +355,9 @@ void ErForceSimulator::setRobotPrimitive( if (robot_primitive_executor_iter != robot_primitive_executor_map.end()) { auto primitive_executor = robot_primitive_executor_iter->second; + primitive_executor->updateState(position, velocity, orientation, + angular_velocity); primitive_executor->updatePrimitive(primitive_set_msg.robot_primitives().at(id)); - primitive_executor->updateVelocity(local_velocity, angular_velocity); } else { @@ -370,16 +374,16 @@ SSLSimulationProto::RobotControl ErForceSimulator::updateSimulatorRobots( SSLSimulationProto::RobotControl robot_control; auto sim_state = getSimulatorState(); - std::map> current_velocity_map; + std::map> current_state; if (side == gameController::Team::BLUE) { const auto& sim_robots = sim_state.blue_robots(); - current_velocity_map = getRobotIdToLocalVelocityMap(sim_robots); + current_state = getRobotIdToStateMap(sim_robots); } else { const auto& sim_robots = sim_state.yellow_robots(); - current_velocity_map = getRobotIdToLocalVelocityMap(sim_robots); + current_state = getRobotIdToStateMap(sim_robots); } for (auto& primitive_executor_with_id : robot_primitive_executor_map) @@ -393,8 +397,9 @@ SSLSimulationProto::RobotControl ErForceSimulator::updateSimulatorRobots( { auto direct_control_no_ramp = primitive_executor->stepPrimitive(status); direct_control = getRampedVelocityPrimitive( - current_velocity_map.at(robot_id).first, - current_velocity_map.at(robot_id).second, *direct_control_no_ramp, + globalToLocalVelocity(std::get<2>(current_state.at(robot_id)), + std::get<1>(current_state.at(robot_id))), + std::get<3>(current_state.at(robot_id)), *direct_control_no_ramp, primitive_executor_time_step_s); } else @@ -564,18 +569,18 @@ void ErForceSimulator::resetCurrentTime() current_time = Timestamp::fromSeconds(0); } -std::map> -ErForceSimulator::getRobotIdToLocalVelocityMap( +std::map> +ErForceSimulator::getRobotIdToStateMap( const google::protobuf::RepeatedPtrField& sim_robots) { - std::map> robot_to_local_velocity; + std::map> map; for (const auto& sim_robot : sim_robots) { - const Vector local_vel = - globalToLocalVelocity(Vector(sim_robot.v_x(), sim_robot.v_y()), - Angle::fromRadians(sim_robot.angle())); - const AngularVelocity angular_vel = Angle::fromRadians(sim_robot.r_z()); - robot_to_local_velocity[sim_robot.id()] = {local_vel, angular_vel}; + const Point position = {sim_robot.p_x(), sim_robot.p_y()}; + const Angle orientation = Angle::fromRadians(sim_robot.angle()); + const Vector velocity = {sim_robot.v_x(), sim_robot.v_y()}; + const AngularVelocity angular_vel = Angle::fromRadians(sim_robot.r_z()); + map[sim_robot.id()] = {position, orientation, velocity, angular_vel}; } - return robot_to_local_velocity; + return map; } diff --git a/src/software/simulation/er_force_simulator.h b/src/software/simulation/er_force_simulator.h index 80459c4df4..9b65c81449 100644 --- a/src/software/simulation/er_force_simulator.h +++ b/src/software/simulation/er_force_simulator.h @@ -158,18 +158,19 @@ class ErForceSimulator RobotId id, const TbotsProto::PrimitiveSet& primitive_set_msg, std::unordered_map>& robot_primitive_executor_map, - const TbotsProto::World& world_msg, const Vector& local_velocity, - const AngularVelocity angular_velocity); + const TbotsProto::World& world_msg, const Point& position, + const Angle& orientation, const Vector& velocity, + const AngularVelocity& angular_velocity); /** - * Gets a map from robot id to local and angular velocity from repeated sim robots + * Gets a map from robot id to position and velocity from repeated sim robots * * @param sim_robots Repeated er force sim robot protos * - * @return a map from robot id to local velocity and angular velocity + * @return a map from robot id to position and velocity */ - static std::map> - getRobotIdToLocalVelocityMap( + static std::map> + getRobotIdToStateMap( const google::protobuf::RepeatedPtrField& sim_robots); /** diff --git a/src/software/util/pid/BUILD b/src/software/util/pid/BUILD new file mode 100644 index 0000000000..156e74085e --- /dev/null +++ b/src/software/util/pid/BUILD @@ -0,0 +1,8 @@ +package(default_visibility = ["//visibility:public"]) + +cc_library( + name = "pid", + hdrs = [ + "pid_controller.hpp", + ], +) diff --git a/src/software/util/pid/pid_controller.hpp b/src/software/util/pid/pid_controller.hpp new file mode 100644 index 0000000000..86ebd0d23f --- /dev/null +++ b/src/software/util/pid/pid_controller.hpp @@ -0,0 +1,66 @@ +#pragma once + +#include + +namespace controls +{ +template +class PIDController +{ + public: + /** + * Constructs a new PID controller. + **/ + PIDController(T k_p, T k_i, T k_d, T max_integral); + + /** + * Given an error, returns the control effort to minimize it. + * + * @param deltaTime The time passed since last step, for calculating integrator and + *derivative. If this function is calling in invariant intervals, deltaTime can be set + *to 1 and any effects it would have can be handled by tuning kD. + **/ + T step(T error, T delta_time = 1.0f); + + /** + * Resets the integrator and clears the last error used for derivative calculation. + **/ + void reset(); + + T k_p; + T k_i; + T k_d; + T max_integral; + + protected: + T integral; + std::optional last_error = std::nullopt; +}; +} // namespace controls + +template +controls::PIDController::PIDController(T k_p, T k_i, T k_d, T max_integral) + : k_p(k_p), k_i(k_i), k_d(k_d), max_integral(max_integral){}; + +template +T controls::PIDController::step(T error, T delta_time) +{ + // If sign of error swaps, reset integrator + if (last_error.value_or(0) * error < 0) + { + integral = 0; + } + + integral += std::max(std::min(error * delta_time, max_integral), -max_integral); + // set derivative, if no last_error, just set to 0 + const T derivative = (error - last_error.value_or(error)) / delta_time; + last_error = error; + return error * k_p + integral * k_i + derivative * k_d; +} + +template +void controls::PIDController::reset() +{ + integral = 0; + last_error = std::nullopt; +} diff --git a/src/software/world/robot.cpp b/src/software/world/robot.cpp index 66e2244d76..5a13d519a6 100644 --- a/src/software/world/robot.cpp +++ b/src/software/world/robot.cpp @@ -218,7 +218,8 @@ Duration Robot::getTimeToPosition(const Point& destination, double initial_velocity_1d = velocity().dot(dist_vector.normalize()); double final_velocity_1d = final_velocity.dot(dist_vector.normalize()); - return getTimeToTravelDistance(dist, robot_constants_.robot_max_speed_m_per_s, + return getTimeToTravelDistance(dist, + robot_constants_.robot_max_speed_trajectory_m_per_s, robot_constants_.robot_max_acceleration_m_per_s_2, initial_velocity_1d, final_velocity_1d); }