From 72d20777b089249c139f3957a8b0e38d12ad1778 Mon Sep 17 00:00:00 2001 From: Matthew Barulic Date: Sun, 3 May 2026 23:03:57 -0400 Subject: [PATCH 1/2] Fixes handling velocity frames in a couple Kenobi spots. --- ateam_kenobi/src/core/defense_area_enforcement.cpp | 6 +++++- ateam_kenobi/src/core/motion/motion_executor.cpp | 3 +++ 2 files changed, 8 insertions(+), 1 deletion(-) diff --git a/ateam_kenobi/src/core/defense_area_enforcement.cpp b/ateam_kenobi/src/core/defense_area_enforcement.cpp index e1830acf5..9de610844 100644 --- a/ateam_kenobi/src/core/defense_area_enforcement.cpp +++ b/ateam_kenobi/src/core/defense_area_enforcement.cpp @@ -21,6 +21,7 @@ #include "defense_area_enforcement.hpp" #include #include +#include "core/motion/frame_conversions.hpp" namespace ateam_kenobi::defense_area_enforcement { @@ -72,8 +73,11 @@ bool WouldVelocityCauseCollision( const double delta_t = 0.01; // TODO(barulicm): This ignores the velocity frame - const ateam_geometry::Vector velocity{motion_command.twist.linear.x, + ateam_geometry::Vector velocity{motion_command.twist.linear.x, motion_command.twist.linear.y}; + if (motion_command.twist_frame == ateam_msgs::msg::RobotMotionCommand::FRAME_BODY) { + velocity = ateam_kenobi::motion::LocalToWorldFrame(velocity, world.our_robots[robot_id]); + } const ateam_geometry::Vector displacement = velocity * delta_t; diff --git a/ateam_kenobi/src/core/motion/motion_executor.cpp b/ateam_kenobi/src/core/motion/motion_executor.cpp index 79bfccd03..287920666 100644 --- a/ateam_kenobi/src/core/motion/motion_executor.cpp +++ b/ateam_kenobi/src/core/motion/motion_executor.cpp @@ -69,6 +69,9 @@ std::array, }, [&](const intents::linear::VelocityIntent & v) { body_velocity.linear = v.velocity; + if (v.frame == intents::linear::Frame::World) { + body_velocity.linear = WorldToLocalFrame(v.velocity, robot); + } use_controller_linvel = false; controller.reset_trajectory({robot.pos}); }, From cf2e994baed9cd82916811a500b7a6668b71f4f1 Mon Sep 17 00:00:00 2001 From: Matthew Barulic Date: Sun, 3 May 2026 23:33:42 -0400 Subject: [PATCH 2/2] Removes vestigial TODO comment --- ateam_kenobi/src/core/defense_area_enforcement.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/ateam_kenobi/src/core/defense_area_enforcement.cpp b/ateam_kenobi/src/core/defense_area_enforcement.cpp index 9de610844..6fc8f2e78 100644 --- a/ateam_kenobi/src/core/defense_area_enforcement.cpp +++ b/ateam_kenobi/src/core/defense_area_enforcement.cpp @@ -72,7 +72,6 @@ bool WouldVelocityCauseCollision( const double delta_t = 0.01; - // TODO(barulicm): This ignores the velocity frame ateam_geometry::Vector velocity{motion_command.twist.linear.x, motion_command.twist.linear.y}; if (motion_command.twist_frame == ateam_msgs::msg::RobotMotionCommand::FRAME_BODY) {