diff --git a/src/proto/message_translation/tbots_protobuf.cpp b/src/proto/message_translation/tbots_protobuf.cpp index 7c146b4c45..f31737c17f 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_trajectory_max_ang_speed_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_trajectory_max_speed_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/robot_constants.h b/src/shared/robot_constants.h index 00a77c94da..5610133965 100644 --- a/src/shared/robot_constants.h +++ b/src/shared/robot_constants.h @@ -88,6 +88,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_trajectory_max_speed_m_per_s; + // The maximum acceleration achievable by our robots [m/s^2] float robot_max_acceleration_m_per_s_2; @@ -97,6 +101,11 @@ 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_trajectory_max_ang_speed_rad_per_s; + // The maximum angular acceleration achievable by our robots [rad/s^2] float robot_max_ang_acceleration_rad_per_s_2; @@ -142,13 +151,15 @@ constexpr RobotConstants createRobotConstants() .motor_max_acceleration_m_per_s_2 = 2.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_trajectory_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'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_trajectory_max_ang_speed_rad_per_s = 7.0f, + .robot_max_ang_acceleration_rad_per_s_2 = 30.0f, .wheel_radius_meters = 0.03f, @@ -173,13 +184,15 @@ 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_trajectory_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'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_trajectory_max_ang_speed_rad_per_s = 7.0f, + .robot_max_ang_acceleration_rad_per_s_2 = 30.0f, .wheel_radius_meters = 0.03f}; } diff --git a/src/software/ai/navigator/trajectory/BUILD b/src/software/ai/navigator/trajectory/BUILD index abc4f5c82f..5b2ef42fb2 100644 --- a/src/software/ai/navigator/trajectory/BUILD +++ b/src/software/ai/navigator/trajectory/BUILD @@ -21,6 +21,7 @@ cc_library( deps = [ ":trajectory", "//software/geom:point", + "//software/geom/algorithms", ], ) diff --git a/src/software/ai/navigator/trajectory/bang_bang_trajectory_1d.h b/src/software/ai/navigator/trajectory/bang_bang_trajectory_1d.h index d5a54bf7af..bb779c2996 100644 --- a/src/software/ai/navigator/trajectory/bang_bang_trajectory_1d.h +++ b/src/software/ai/navigator/trajectory/bang_bang_trajectory_1d.h @@ -1,6 +1,7 @@ #pragma once #include +#include #include #include "software/ai/navigator/trajectory/trajectory.hpp" @@ -103,6 +104,21 @@ class BangBangTrajectory1D : public Trajectory */ size_t getNumTrajectoryParts() const; + /** + * Checks if this 1D trajectory terminates at approximately the same destination + * position as another trajectory. + * + * @param other The other trajectory to compare against. + * @param threshold The maximum allowable distance between the two destinations. + * @return True if the distance between destinations is less than or equal to the + * threshold. False otherwise. + */ + bool hasSameDestination(const Trajectory& other, + double threshold) const override + { + return std::abs(getDestination() - other.getDestination()) <= threshold; + } + private: /** * Generate a trapezoidal trajectory and fill in `trajectory_parts`. diff --git a/src/software/ai/navigator/trajectory/bang_bang_trajectory_1d_angular.h b/src/software/ai/navigator/trajectory/bang_bang_trajectory_1d_angular.h index 2ffbff5ec2..dab2db5dc8 100644 --- a/src/software/ai/navigator/trajectory/bang_bang_trajectory_1d_angular.h +++ b/src/software/ai/navigator/trajectory/bang_bang_trajectory_1d_angular.h @@ -83,6 +83,23 @@ class BangBangTrajectory1DAngular */ double getTotalTime() const override; + /** + * Checks if this angular trajectory terminates at approximately the same destination + * orientation as another trajectory. + * + * @param other The other trajectory to compare against. + * @param threshold The maximum allowable distance in radians between the two + * destinations. + * @return True if the distance between destinations is less than or equal to the + * threshold. False otherwise. + */ + bool hasSameDestination( + const Trajectory& other, + double threshold) const override + { + return getDestination().minDiff(other.getDestination()).toDegrees() <= threshold; + } + private: BangBangTrajectory1D trajectory; }; diff --git a/src/software/ai/navigator/trajectory/trajectory.hpp b/src/software/ai/navigator/trajectory/trajectory.hpp index 4e7a7ee77e..c4765b129a 100644 --- a/src/software/ai/navigator/trajectory/trajectory.hpp +++ b/src/software/ai/navigator/trajectory/trajectory.hpp @@ -59,4 +59,16 @@ class Trajectory { return getPosition(getTotalTime()); } + + /** + * Checks if this trajectory terminates at approximately the same destination as + * another trajectory. + * + * @param other The other trajectory to compare against. + * @param threshold The maximum allowable distance between the two destinations. + * @return True if the distance between destinations is less than or equal to the + * threshold. False otherwise. + */ + virtual bool hasSameDestination(const Trajectory& other, + double threshold) const = 0; }; diff --git a/src/software/ai/navigator/trajectory/trajectory_2d.h b/src/software/ai/navigator/trajectory/trajectory_2d.h index 9699f07c17..8f2485587b 100644 --- a/src/software/ai/navigator/trajectory/trajectory_2d.h +++ b/src/software/ai/navigator/trajectory/trajectory_2d.h @@ -1,6 +1,7 @@ #pragma once #include "software/ai/navigator/trajectory/trajectory.hpp" +#include "software/geom/algorithms/distance.h" #include "software/geom/point.h" #include "software/geom/rectangle.h" @@ -14,4 +15,20 @@ class Trajectory2D : virtual public Trajectory * @return bounding boxes which this trajectory passes through */ virtual std::vector getBoundingBoxes() const = 0; + + /** + * Checks if this 2D trajectory terminates at approximately the same destination + * position as another trajectory. + * + * @param other The other trajectory to compare against. + * @param threshold The maximum allowable distance in metres between the two + * destinations. + * @return True if the distance between destinations is less than or equal to the + * threshold. False otherwise. + */ + bool hasSameDestination(const Trajectory& other, + double threshold) const override + { + return distance(getDestination(), other.getDestination()) <= threshold; + } }; diff --git a/src/software/embedded/BUILD b/src/software/embedded/BUILD index e5bba4bd99..44f22ed099 100644 --- a/src/software/embedded/BUILD +++ b/src/software/embedded/BUILD @@ -34,6 +34,8 @@ cc_library( "//proto/primitive:primitive_msg_factory", "//software/ai/navigator/trajectory:bang_bang_trajectory_1d_angular", "//software/ai/navigator/trajectory:trajectory_path", + "//software/embedded/motion_control:orientation_controller", + "//software/embedded/motion_control:position_controller", "//software/math:math_functions", "//software/physics:velocity_conversion_util", "//software/time:duration", diff --git a/src/software/embedded/motion_control/controller.h b/src/software/embedded/motion_control/controller.h index 6de0d30237..6882618082 100644 --- a/src/software/embedded/motion_control/controller.h +++ b/src/software/embedded/motion_control/controller.h @@ -1,3 +1,5 @@ +#pragma once + #include "software/time/duration.h" /** diff --git a/src/software/embedded/primitive_executor.cpp b/src/software/embedded/primitive_executor.cpp index 2859bc6c87..2b9a5dc189 100644 --- a/src/software/embedded/primitive_executor.cpp +++ b/src/software/embedded/primitive_executor.cpp @@ -22,66 +22,85 @@ void PrimitiveExecutor::updatePrimitive(const TbotsProto::Primitive& primitive_m if (current_primitive_.has_move()) { - trajectory_path_ = - createTrajectoryPathFromParams(current_primitive_.move().xy_traj_params(), - state_.velocity(), robot_constants_); - - angular_trajectory_ = - createAngularTrajectoryFromParams(current_primitive_.move().w_traj_params(), - state_.angularVelocity(), robot_constants_); + const auto new_trajectory_path = createTrajectoryPathFromParams( + current_primitive_.move().xy_traj_params(), state_.position(), + state_.velocity(), robot_constants_); + + const auto new_angular_trajectory = createAngularTrajectoryFromParams( + current_primitive_.move().w_traj_params(), state_.orientation(), + state_.angularVelocity(), robot_constants_); + + const bool is_linear_trajectory_new = + new_trajectory_path.has_value() != trajectory_path_.has_value() || + (trajectory_path_.has_value() && + !trajectory_path_->hasSameDestination(*new_trajectory_path, + LINEAR_DESTINATION_THRESHOLD_METERS)); + + const bool is_angular_trajectory_new = + !angular_trajectory_.has_value() || + !angular_trajectory_->hasSameDestination( + new_angular_trajectory, ANGULAR_DESTINATION_THRESHOLD_DEGREES); + + if (is_linear_trajectory_new) + { + trajectory_path_ = new_trajectory_path; + position_controller_.reset(); + time_since_linear_trajectory_creation_ = + Duration::fromSeconds(VISION_TO_ROBOT_DELAY_S); + } - time_since_trajectory_creation_ = Duration::fromSeconds(VISION_TO_ROBOT_DELAY_S); + if (is_angular_trajectory_new) + { + angular_trajectory_ = new_angular_trajectory; + orientation_controller_.reset(); + time_since_angular_trajectory_creation_ = + Duration::fromSeconds(VISION_TO_ROBOT_DELAY_S); + } } } void PrimitiveExecutor::updateState(const RobotState& state) { - Vector actual_global_velocity = - localToGlobalVelocity(state.localVelocity(), state_.orientation()); - state_.setVelocity(actual_global_velocity); - state_.setAngularVelocity(state.angularVelocity()); + state_ = state; } -Vector PrimitiveExecutor::getTargetLinearVelocity() +Vector PrimitiveExecutor::stepTargetLinearVelocity(Duration delta_time) { - Vector local_velocity = globalToLocalVelocity( - trajectory_path_->getVelocity(time_since_trajectory_creation_.toSeconds()), - state_.orientation()); - Point position = - trajectory_path_->getPosition(time_since_trajectory_creation_.toSeconds()); - double distance_to_destination = - distance(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) - { - local_velocity *= distance_to_destination / MAX_DAMPENING_VELOCITY_DISTANCE_M; - } - return local_velocity; + const auto target_v_global = + position_controller_.step(state_.position(), *trajectory_path_, + time_since_linear_trajectory_creation_, delta_time); + const auto target_v_local = + globalToLocalVelocity(target_v_global, state_.orientation()); + + return target_v_local.normalize( + std::min(target_v_local.length(), + static_cast(robot_constants_.robot_max_speed_m_per_s))); } -AngularVelocity PrimitiveExecutor::getTargetAngularVelocity() +AngularVelocity PrimitiveExecutor::stepTargetAngularVelocity(Duration delta_time) { - state_.setOrientation( - angular_trajectory_->getPosition(time_since_trajectory_creation_.toSeconds())); - - AngularVelocity angular_velocity = - angular_trajectory_->getVelocity(time_since_trajectory_creation_.toSeconds()); - Angle orientation_to_destination = - state_.orientation().minDiff(angular_trajectory_->getDestination()); - if (orientation_to_destination.toDegrees() < 5) + const auto target_w = + orientation_controller_.step(state_.orientation(), *angular_trajectory_, + time_since_angular_trajectory_creation_, delta_time); + if (target_w.toRadians() < -robot_constants_.robot_max_ang_speed_rad_per_s) { - angular_velocity *= orientation_to_destination.toDegrees() / 5; + return AngularVelocity::fromRadians( + -robot_constants_.robot_max_ang_speed_rad_per_s); } - - return angular_velocity; + if (target_w.toRadians() > robot_constants_.robot_max_ang_speed_rad_per_s) + { + return AngularVelocity::fromRadians( + robot_constants_.robot_max_ang_speed_rad_per_s); + } + return target_w; } std::unique_ptr PrimitiveExecutor::stepPrimitive( TbotsProto::PrimitiveExecutorStatus& status, Duration delta_time) { - time_since_trajectory_creation_ += delta_time; + time_since_linear_trajectory_creation_ += delta_time; + time_since_angular_trajectory_creation_ += delta_time; status.set_running_primitive(true); switch (current_primitive_.primitive_case()) @@ -113,8 +132,8 @@ std::unique_ptr PrimitiveExecutor::stepPrimi return output; } - Vector local_velocity = getTargetLinearVelocity(); - AngularVelocity angular_velocity = getTargetAngularVelocity(); + Vector local_velocity = stepTargetLinearVelocity(delta_time); + AngularVelocity angular_velocity = stepTargetAngularVelocity(delta_time); auto output = createDirectControlPrimitive( local_velocity, angular_velocity, diff --git a/src/software/embedded/primitive_executor.h b/src/software/embedded/primitive_executor.h index df081b321d..51a3228505 100644 --- a/src/software/embedded/primitive_executor.h +++ b/src/software/embedded/primitive_executor.h @@ -4,6 +4,8 @@ #include "proto/tbots_software_msgs.pb.h" #include "software/ai/navigator/trajectory/bang_bang_trajectory_1d_angular.h" #include "software/ai/navigator/trajectory/trajectory_path.h" +#include "software/embedded/motion_control/orientation_controller.h" +#include "software/embedded/motion_control/position_controller.h" #include "software/geom/vector.h" #include "software/time/duration.h" #include "software/world/robot_state.h" @@ -47,29 +49,43 @@ class PrimitiveExecutor private: /* - * Compute the next target linear _local_ velocity the robot should be at. + * Compute the next target linear _local_ velocity the robot should have. + * @param delta_time The elapsed time since last time step + * * @returns Vector The target linear _local_ velocity */ - Vector getTargetLinearVelocity(); + Vector stepTargetLinearVelocity(Duration delta_time); /* - * Returns the next target angular velocity the robot + * Compute the next target angular velocity the robot should have. + * @param delta_time The elapsed time since last time step * * @returns AngularVelocity The target angular velocity */ - AngularVelocity getTargetAngularVelocity(); + AngularVelocity stepTargetAngularVelocity(Duration delta_time); RobotState state_; TbotsProto::Primitive current_primitive_; - Duration time_since_trajectory_creation_; robot_constants::RobotConstants robot_constants_; + std::optional trajectory_path_; std::optional angular_trajectory_; + Duration time_since_linear_trajectory_creation_; + Duration time_since_angular_trajectory_creation_; + + PositionController position_controller_; + OrientationController orientation_controller_; + // Estimated delay between a vision frame to AI processing to robot executing static constexpr double VISION_TO_ROBOT_DELAY_S = 0.03; // 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; + + // 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; }; diff --git a/src/software/simulation/er_force_simulator.cpp b/src/software/simulation/er_force_simulator.cpp index ba150e3cb2..d45661af8f 100644 --- a/src/software/simulation/er_force_simulator.cpp +++ b/src/software/simulation/er_force_simulator.cpp @@ -299,7 +299,8 @@ void ErForceSimulator::setYellowRobotPrimitiveSet( { auto sim_state = getSimulatorState(); const auto& sim_robots = sim_state.yellow_robots(); - const auto robot_map = getRobotIdToRobotStateMap(sim_robots); + const auto robot_map = + getRobotIdToRobotStateMap(sim_robots, gameController::Team::YELLOW); yellow_team_world_msg = std::move(world_msg); const TbotsProto::World world_proto = *yellow_team_world_msg; @@ -320,7 +321,8 @@ void ErForceSimulator::setBlueRobotPrimitiveSet( { auto sim_state = getSimulatorState(); const auto& sim_robots = sim_state.blue_robots(); - const auto robot_map = getRobotIdToRobotStateMap(sim_robots); + const auto robot_map = + getRobotIdToRobotStateMap(sim_robots, gameController::Team::BLUE); blue_team_world_msg = std::move(world_msg); const TbotsProto::World world_proto = *blue_team_world_msg; @@ -370,7 +372,7 @@ SSLSimulationProto::RobotControl ErForceSimulator::updateSimulatorRobots( const auto& sim_robots = (side == gameController::Team::BLUE) ? sim_state.blue_robots() : sim_state.yellow_robots(); - const auto robot_map = getRobotIdToRobotStateMap(sim_robots); + const auto robot_map = getRobotIdToRobotStateMap(sim_robots, side); for (auto& [robot_id, primitive_executor] : robot_primitive_executor_map) { @@ -554,15 +556,25 @@ void ErForceSimulator::resetCurrentTime() } std::map ErForceSimulator::getRobotIdToRobotStateMap( - const google::protobuf::RepeatedPtrField& sim_robots) + const google::protobuf::RepeatedPtrField& sim_robots, + gameController::Team side) { std::map robot_map; for (const auto& sim_robot : sim_robots) { - const auto position = Point(sim_robot.p_x(), sim_robot.p_y()); - const auto velocity = Vector(sim_robot.v_x(), sim_robot.v_y()); - const auto orientation = Angle::fromRadians(sim_robot.angle()); - const auto angular_velocity = AngularVelocity::fromRadians(sim_robot.r_z()); + auto position = Point(sim_robot.p_x(), sim_robot.p_y()); + auto velocity = Vector(sim_robot.v_x(), sim_robot.v_y()); + auto orientation = Angle::fromRadians(sim_robot.angle()); + auto angular_velocity = AngularVelocity::fromRadians(sim_robot.r_z()); + + if (side == gameController::Team::YELLOW) + { + position = -position; + velocity = -velocity; + orientation += Angle::half(); + // angular_velocity is the same no matter which side + } + robot_map[sim_robot.id()] = RobotState(position, velocity, orientation, angular_velocity); } diff --git a/src/software/simulation/er_force_simulator.h b/src/software/simulation/er_force_simulator.h index 19a6f16fd6..79118fc25c 100644 --- a/src/software/simulation/er_force_simulator.h +++ b/src/software/simulation/er_force_simulator.h @@ -165,11 +165,14 @@ class ErForceSimulator * Gets a map from robot id to local and angular velocity from repeated sim robots * * @param sim_robots Repeated er force sim robot protos + * @param side Which team the robots belong to. If team is yellow, inverts position + * and linear velocity, and adds 180 degrees to orientation. * * @return a map from robot id to local velocity and angular velocity */ static std::map getRobotIdToRobotStateMap( - const google::protobuf::RepeatedPtrField& sim_robots); + const google::protobuf::RepeatedPtrField& sim_robots, + gameController::Team side); /** * Update Simulator Robot and get the latest robot control