Merge pull request #143 from Immortals-Robotics/dev/rc24-tmp

Dev/rc24 tmp

data/config.toml

@@ -40,17 +40,39 @@ ip = '127.0.0.1'
 port = 10302
 
 [soccer]
+chip_tune_coef = 2.5
 def_ball_distance_coef = 0.69999999999999996
 def_max_extension_area = 1100.0
 def_prediction_time = 0.5
 def_tight_start_angle = 30.0
 gk_tight_start_angle = 15.0
+kick_tune_coef = 1.0
+mark_distance = 250.0
 mark_in_stop = true
 one_touch_beta = 0.40000000000000002
 one_touch_gamma = 0.14000000000000001
 one_touch_shoot_k = 4000.0
-kick_tune_coef = 1.0
-chip_tune_coef = 2.5
+
+    [soccer.velocity_profile.aroom]
+    acceleration = 2160.0
+    deceleration = 2700.0
+    speed = 900.0
+
+    [soccer.velocity_profile.kharaki]
+    acceleration = 4010.0
+    deceleration = 4860.0
+    speed = 2200.0
+
+    [soccer.velocity_profile.mamooli]
+    acceleration = 3500.0
+    deceleration = 3850.0
+    speed = 1800.0
+
+    [soccer.velocity_profile.sooski]
+    acceleration = 1620.0
+    deceleration = 2700.0
+    speed = 450.0
+
     [[soccer.robot_physical_status]]
     has_chip_kick = true
     has_direct_kick = true
@@ -130,7 +152,7 @@ chip_tune_coef = 2.5
 
     [[soccer.robot_physical_status]]
     has_chip_kick = false
-    has_direct_kick = true
+    has_direct_kick = false
     has_dribbler = false
     id = 11
     is_3D_printed = true

source/sender/command.h

@@ -12,13 +12,13 @@ struct Command
     Common::Angle current_angle;
     Common::Angle target_angle;
 
-    int shoot    = 0;
-    int chip     = 0;
-    int dribbler = 0;
+    float shoot    = 0;
+    float chip     = 0;
+    float dribbler = 0;
 
     Command() = default;
 
-    Command(const Protos::Immortals::Command &t_command)
+    explicit Command(const Protos::Immortals::Command &t_command)
     {
         vision_id = t_command.vision_id();
 

source/sender/nrf.cpp

@@ -50,8 +50,6 @@ static float getCalibratedShootPow(float t_raw_shoot, const Common::Vec2 &t_coef
         return 0;
     }
 
-    t_raw_shoot = std::clamp(t_raw_shoot, 0.0f, 6500.f) / 1000.f;
-
     float calib_shoot = t_coeffs.x * t_raw_shoot + t_coeffs.y;
 
     calib_shoot = std::clamp(calib_shoot, 0.0f, 100.0f);
@@ -102,15 +100,19 @@ void Nrf::queueCommand(const Command &command)
         convert_float_to_2x_buff(data + 9, command.current_angle.deg());
         if (command.shoot > 0)
         {
+            const float raw_shoot        = std::clamp(command.shoot, 0.0f, 6500.f) / 1000.f;
+            const float calibrated_shoot = getCalibratedShootPow(raw_shoot, shoot_coeffs[command.vision_id]);
 
-            data[11] = static_cast<unsigned char> (getCalibratedShootPow(Common::config().soccer.kick_tune_coef * command.shoot,
-                                             shoot_coeffs[command.vision_id]));
+            data[11] = static_cast<unsigned char>(calibrated_shoot);
             data[12] = 0x00;
         }
         else if (command.chip > 0)
         {
+            const float raw_chip        = std::clamp(command.chip, 0.0f, 150.f);
+            const float calibrated_chip = getCalibratedShootPow(raw_chip, chip_coeffs[command.vision_id]);
+
             data[11] = 0x00;
-            data[12] = getCalibratedShootPow(Common::config().soccer.chip_tune_coef * command.chip, chip_coeffs[command.vision_id]);
+            data[12] = static_cast<unsigned char>(calibrated_chip);
         }
         else
         {

source/soccer/ai.h

@@ -188,7 +188,7 @@ class Ai
     void gkHi(int t_robot_num);
     void runningDef(int t_robot_num, Common::Vec2 t_target, Common::Vec2 *t_defend_target);
     void defenceWall(int t_robot_num, bool t_kick_off = false);
-    void attacker(int t_robot_num, Common::Angle t_angle, float t_kick, int t_chip, bool t_kiss, bool t_dribbler);
+    void attacker(int t_robot_num, Common::Angle t_angle, float t_kick, int t_chip, bool t_kiss, bool t_dribbler, bool precise = false);
     void waitForPass(int t_robot_num, bool t_chip = false, const Common::Vec2 *t_target = nullptr,
                      Common::Vec2 *t_stat_pos = nullptr);
     void waitForOmghi(int t_robot_num);

source/soccer/assignment/cost_static.cpp

@@ -14,7 +14,7 @@ int Ai::staticRoleCost(const int t_robot_idx, const Assignment &t_assignment) co
     if (t_assignment.needs_chip != physical_status.has_chip_kick)
         caps_cost += 5000;
 
-    int switch_cost = t_robot_idx == *t_assignment.role ? 0 : 500;
+    int switch_cost = t_robot_idx == *t_assignment.role ? 0 : 200;
 
     return dis_cost + caps_cost + switch_cost;
 }

source/soccer/assignment/create_mids.cpp

@@ -73,7 +73,11 @@ void Ai::createMidAssignments()
             const bool shoot = !m_is_defending;
             const bool chip  = !m_is_defending && role == &m_mid5;
 
-            createStaticAssignment(role, Assignment::Priority::Low, shoot, chip);
+            const bool important_dmf = role == &m_mid5 && (m_ref_state.ourBallPlacement() || m_ref_state.ourFreeKick());
+            const Assignment::Priority priority =
+                important_dmf ? Assignment::Priority::High : Assignment::Priority::Low;
+
+            createStaticAssignment(role, priority, shoot, chip);
         }
     }
 }

source/soccer/dss/dss.cpp

@@ -27,9 +27,9 @@ bool Dss::collisionWithOwn(const Common::RobotState &state_a, const Common::Vec2
                            const Common::RobotState &state_b, const Common::Vec2 &cmd_b) const
 {
     const Trajectory traj_a =
-        Trajectory::MakeTrajectory(state_a, cmd_a, m_profile.max_dec, 1.0f / Common::config().vision.vision_frame_rate);
+        Trajectory::MakeTrajectory(state_a, cmd_a, m_profile.deceleration, 1.0f / Common::config().vision.vision_frame_rate);
     const Trajectory traj_b =
-        Trajectory::MakeTrajectory(state_b, cmd_b, m_profile.max_dec, 1.0f / Common::config().vision.vision_frame_rate);
+        Trajectory::MakeTrajectory(state_b, cmd_b, m_profile.deceleration, 1.0f / Common::config().vision.vision_frame_rate);
 
     return Parabolic::HaveOverlap(traj_a.acc, traj_b.acc, Common::field().robot_radius * 2.f) ||
            Parabolic::HaveOverlap(traj_a.dec, traj_b.dec, Common::field().robot_radius * 2.f) ||
@@ -40,9 +40,9 @@ bool Dss::collisionWithOwn(const Common::RobotState &state_a, const Common::Vec2
 bool Dss::collisionWithOpp(const Common::RobotState &state_own, const Common::Vec2 &cmd_own,
                            const Common::RobotState &state_opp) const
 {
-    const Trajectory traj_own = Trajectory::MakeTrajectory(state_own, cmd_own, m_profile.max_dec,
+    const Trajectory traj_own = Trajectory::MakeTrajectory(state_own, cmd_own, m_profile.deceleration,
                                                            1.0f / Common::config().vision.vision_frame_rate);
-    const Trajectory traj_opp = Trajectory::MakeOpponentTrajectory(state_opp, m_profile.max_dec);
+    const Trajectory traj_opp = Trajectory::MakeOpponentTrajectory(state_opp, m_profile.deceleration);
 
     return Parabolic::HaveOverlap(traj_own.acc, traj_opp.dec, Common::field().robot_radius * 2.f) ||
            Parabolic::HaveOverlap(traj_own.dec, traj_opp.dec, Common::field().robot_radius * 2.f) ||
@@ -53,7 +53,7 @@ bool Dss::collisionWithOpp(const Common::RobotState &state_own, const Common::Ve
 bool Dss::RobotHasStaticCollision(const Common::RobotState &state, const Common::Vec2 &cmd) const
 {
     const Trajectory traj =
-        Trajectory::MakeTrajectory(state, cmd, m_profile.max_dec, 1.0f / Common::config().vision.vision_frame_rate);
+        Trajectory::MakeTrajectory(state, cmd, m_profile.deceleration, 1.0f / Common::config().vision.vision_frame_rate);
 
     return Parabolic::HasStaticOverlap(traj.acc) || Parabolic::HasStaticOverlap(traj.dec) ||
            Parabolic::HasStaticOverlap(traj.dec);
@@ -132,7 +132,7 @@ void Dss::Reset()
         else
         {
             const float dec =
-                std::min(m_profile.max_dec, state.velocity.length() * Common::config().vision.vision_frame_rate);
+                std::min(m_profile.deceleration, state.velocity.length() * Common::config().vision.vision_frame_rate);
             computed_motions[robot_idx] = state.velocity.normalized() * (-dec);
         }
     }
@@ -144,7 +144,7 @@ Common::Vec2 Dss::ComputeSafeMotion(const int robot_num, const Common::Vec2 &mot
     // TODO: in simulation setting a lower dec compared
     // to motion plan results in better avoidance.
     // Verify on the real field
-    m_profile.max_dec /= 2.0f;
+    m_profile.deceleration /= 2.0f;
 
     const Common::Vec2        target_a_cmd     = GetAccFromMotion(robot_num, motion);
     const float               target_a_cmd_mag = target_a_cmd.length();
@@ -162,7 +162,7 @@ Common::Vec2 Dss::ComputeSafeMotion(const int robot_num, const Common::Vec2 &mot
     else
     {
         const float dec =
-            std::min(m_profile.max_dec, state.velocity.length() * Common::config().vision.vision_frame_rate);
+            std::min(m_profile.deceleration, state.velocity.length() * Common::config().vision.vision_frame_rate);
         a_cmd       = state.velocity.normalized() * (-dec);
         float error = ComputeError(target_a_cmd, a_cmd);
 

source/soccer/errt/errt.cpp

@@ -15,7 +15,7 @@ void Planner::init(const Common::Vec2 init, const Common::Vec2 final, const floa
     m_step_size = step;
 
     m_tree.reset();
-    m_tree.AddNode(init_state, nullptr);
+    m_tree.addNode(init_state, nullptr);
 
     m_started_in_obs = g_obs_map.isInObstacle(init);
 }
@@ -62,14 +62,14 @@ Node *Planner::extend(Node *s, Common::Vec2 &target)
     if (g_obs_map.collisionDetect(new_state, s->state))
         return nullptr;
 
-    return m_tree.AddNode(new_state, s);
+    return m_tree.addNode(new_state, s);
 }
 
 void Planner::setWayPoints()
 {
     m_waypoints.clear();
 
-    Node *n = m_tree.NearestNeighbour(final_state);
+    Node *n = m_tree.nearestNeighbour(final_state);
     m_waypoints.push_back(n->state);
 
     while (n->parent)
@@ -85,28 +85,34 @@ void Planner::setWayPoints()
 
 Common::Vec2 Planner::plan()
 {
+    if (m_cached_waypoints.empty())
+    {
+        Common::logWarning("cached waypoints are empty");
+    }
+
     // return final_state;
     if (!g_obs_map.collisionDetect(init_state, final_state))
     {
-        m_tree.AddNode(final_state, m_tree.NearestNeighbour(final_state));
+        // TODO: slice the path so that the cache contains valid waypoints
+        m_tree.addNode(final_state, m_tree.nearestNeighbour(final_state));
     }
     else
     {
         for (int i = 0; ((i < m_max_nodes) && (!isReached())); i++)
         {
             Common::Vec2 r = chooseTarget();
-            extend(m_tree.NearestNeighbour(r), r);
+            extend(m_tree.nearestNeighbour(r), r);
         }
 
         if ((isReached()) && (!g_obs_map.isInObstacle(final_state)) &&
-            (final_state.distanceTo(m_tree.NearestNeighbour(final_state)->state) > 1))
+            (final_state.distanceTo(m_tree.nearestNeighbour(final_state)->state) > 1))
         {
-            m_tree.AddNode(final_state, m_tree.NearestNeighbour(final_state));
+            m_tree.addNode(final_state, m_tree.nearestNeighbour(final_state));
         }
     }
 
     setWayPoints();
-    optimize_tree();
+    optimizeTree();
 
     if (m_started_in_obs || m_waypoints.size() <= 1)
     {
@@ -118,7 +124,7 @@ Common::Vec2 Planner::plan()
     }
 }
 
-void Planner::optimize_tree()
+void Planner::optimizeTree()
 {
     for (size_t i = 0; i < m_waypoints.size() - 1; i++)
     {

source/soccer/errt/errt.h

@@ -8,7 +8,7 @@ namespace Tyr::Soccer
 class Planner
 {
 public:
-    Planner(int t_max_nodes = 1000);
+     Planner(int t_max_nodes = 2000);
     ~Planner() = default;
 
     void init(Common::Vec2 init, Common::Vec2 final, float step);
@@ -32,7 +32,7 @@ class Planner
 
     // TODO: move to setting
     float goal_target_prob     = 0.1f;
-    float waypoint_target_prob = 0.5f;
+    float waypoint_target_prob = 0.8f;
     float acceptable_dis       = 90.0f;
 
     Common::Random m_random;
@@ -41,8 +41,8 @@ class Planner
 
     Common::Vec2 randomState()
     {
-        return Common::Vec2((m_random.get(-1.0f, 1.0f) * (Common::field().width + 250.0f)),
-                            (m_random.get(-1.0f, 1.0f) * (Common::field().height + 250.0f)));
+        return Common::Vec2((m_random.get(-1.0f, 1.0f) * (Common::field().width + Common::field().boundary_width)),
+                            (m_random.get(-1.0f, 1.0f) * (Common::field().height + Common::field().boundary_width)));
     }
 
     Common::Vec2 nearestFree(Common::Vec2 state);
@@ -51,27 +51,27 @@ class Planner
     Node        *extend(Node *s, Common::Vec2 &target);
     void         setWayPoints();
 
-    Common::Vec2 getWayPoint(const unsigned int i)
+    Common::Vec2 getWayPoint(const unsigned int i) const
     {
         return m_waypoints[i];
     }
 
-    unsigned int getWayPointNum()
+    unsigned int getWayPointNum() const
     {
         return m_waypoints.size();
     }
 
     bool isReached()
     {
-        return final_state.distanceTo(m_tree.NearestNeighbour(final_state)->state) <= acceptable_dis;
+        return final_state.distanceTo(m_tree.nearestNeighbour(final_state)->state) <= acceptable_dis;
     }
 
-    void optimize_tree();
+    void optimizeTree();
 };
 
 inline Common::Vec2 Planner::chooseTarget()
 {
-    float r = m_random.get(0.0f, 1.0f);
+    const float r = m_random.get(0.0f, 1.0f);
 
     if (r <= goal_target_prob)
     {
@@ -82,7 +82,6 @@ inline Common::Vec2 Planner::chooseTarget()
         const int idx = m_random.get(0, static_cast<int>(m_cached_waypoints.size()) - 1);
         return m_cached_waypoints[idx];
     }
-
     else
     {
         return randomState();