From 22d31b5a07dd7edc34a4d4754b5b532e5e74497a Mon Sep 17 00:00:00 2001 From: Mathew MacDougall Date: Fri, 21 Mar 2025 22:06:13 -0700 Subject: [PATCH] Implement the global velocity move command for robots. RobotMoveCommand::MoveGlobalVelocity commands from the Ssl Simulation Protocol are now handled and will move the robots as commanded instead of returning SimError::UNSUPPORTED_VELOCITY. --- src/amun/simulator/simrobot.cpp | 25 +++++++++++++++++++++---- src/simulator/simulator.cpp | 2 +- 2 files changed, 22 insertions(+), 5 deletions(-) diff --git a/src/amun/simulator/simrobot.cpp b/src/amun/simulator/simrobot.cpp index 5dedad58d..bc20d9dff 100644 --- a/src/amun/simulator/simrobot.cpp +++ b/src/amun/simulator/simrobot.cpp @@ -398,13 +398,30 @@ void SimRobot::begin(SimBall *ball, double time) m_shootTime = 0.0; } - if (m_inStandby || !m_sslCommand.has_move_command() || !m_sslCommand.move_command().has_local_velocity()) { + if (m_inStandby || !m_sslCommand.has_move_command()) { return; } - float output_v_f = m_sslCommand.move_command().local_velocity().forward(); - float output_v_s = -m_sslCommand.move_command().local_velocity().left(); - float output_omega = m_sslCommand.move_command().local_velocity().angular(); + float output_v_f = 0.0; + float output_v_s = 0.0; + float output_omega = 0.0; + + if(m_sslCommand.move_command().has_global_velocity()) { + const btQuaternion q = m_body->getWorldTransform().getRotation(); + const btVector3 dir = btMatrix3x3(q).getColumn(0); + const float body_global_rotation_rad = atan2(dir.y(), dir.x()); + const float global_to_local_rotation = -body_global_rotation_rad; + + output_v_f = m_sslCommand.move_command().global_velocity().x() * cos(global_to_local_rotation) - m_sslCommand.move_command().global_velocity().y() * sin(global_to_local_rotation); + output_v_s = -(m_sslCommand.move_command().global_velocity().x() * sin(global_to_local_rotation) + m_sslCommand.move_command().global_velocity().y() * cos(global_to_local_rotation)); + output_omega = m_sslCommand.move_command().global_velocity().angular(); + }else if (m_sslCommand.move_command().has_local_velocity()) { + output_v_f = m_sslCommand.move_command().local_velocity().forward(); + output_v_s = -m_sslCommand.move_command().local_velocity().left(); + output_omega = m_sslCommand.move_command().local_velocity().angular(); + }else { + return; + } btVector3 v_local(t.inverse() * m_body->getLinearVelocity()); btVector3 v_d_local(boundSpeed(output_v_s), boundSpeed(output_v_f), 0); diff --git a/src/simulator/simulator.cpp b/src/simulator/simulator.cpp index 09e651634..20c76777f 100644 --- a/src/simulator/simulator.cpp +++ b/src/simulator/simulator.cpp @@ -526,7 +526,7 @@ void RobotCommandAdaptor::handleDatagrams() for (const auto& command : control->robot_commands()) { if (command.has_move_command()) { const auto& moveCmd = command.move_command(); - if (moveCmd.has_wheel_velocity() || moveCmd.has_global_velocity()) { + if (moveCmd.has_wheel_velocity()) { sendRcr = true; const std::string robotStr = "(Robot :" + std::to_string(command.id()) + ")"; setError(rcr.add_errors(), SimError::UNSUPPORTED_VELOCITY, ERROR_SOURCE, robotStr);