Merge pull request #911 from RoboJackets/albert/fixLineKick

Fix line kick and forward pass.

soccer/SystemState.cpp

@@ -16,9 +16,6 @@ using Planning::MotionInstant;
 class BallPath : public Planning::Path {
 public:
     BallPath(const Ball& ball) : ball(ball){};
-    virtual boost::optional<RobotInstant> evaluate(RJ::Seconds t) const {
-        return RobotInstant(ball.predict(startTime() + t));
-    }
 
     virtual bool hit(const Geometry2d::ShapeSet& obstacles,
                      RJ::Seconds startTimeIntoPath,
@@ -49,6 +46,11 @@ class BallPath : public Planning::Path {
         return std::make_unique<BallPath>(*this);
     }
 
+protected:
+    virtual boost::optional<RobotInstant> eval(RJ::Seconds t) const {
+        return RobotInstant(ball.predict(startTime() + t));
+    }
+
 private:
     const Ball& ball;
 };
@@ -70,12 +72,30 @@ Planning::MotionInstant Ball::predict(RJ::Time estimateTime) const {
 
     const auto s0 = vel.mag();
 
+    const auto decayConstant = 0.1795;
+
+    double speed = 0;
+    double distance = 0;
+    if (s0 != 0) {
+        auto maxTime = s0 / decayConstant;
+        if (t.count() >= maxTime) {
+            speed = 0;
+            distance = s0 * maxTime - pow(maxTime, 2) / 2.0 * decayConstant;
+        } else {
+            speed = s0 - (t.count() * decayConstant);
+            distance = s0 * t.count() - pow(t.count(), 2) / 2.0 * decayConstant;
+        }
+    } else {
+        speed = 0;
+        distance = 0;
+    }
+
     // Based on sim ball
     // v = v0 * e^-0.2913t
     // d = v0 * -3.43289 (-1 + e^(-0.2913 t))
-    auto part = std::exp(-0.2913f * t.count());
-    auto speed = s0 * part;
-    auto distance = s0 * -3.43289f * (part - 1.0f);
+    // auto part = std::exp(-0.2913f * t.count());
+    // auto speed = s0 * part;
+    // auto distance = s0 * -3.43289f * (part - 1.0f);
 
     return MotionInstant(pos + vel.normalized(distance), vel.normalized(speed));
 }
@@ -88,10 +108,22 @@ RJ::Time Ball::estimateTimeTo(const Geometry2d::Point& point,
         *nearPointOut = nearPoint;
     }
     auto dist = nearPoint.distTo(pos);
-    // d = v0 * -3.43289 (-1 + e^(-0.2913 t))
-    // (d + v0 * -3.43289) / (v0 * -3.43289)= e^(-0.2913 t))
-    auto part = vel.mag() * -3.43289;
-    return time + RJ::Seconds(std::log((dist + part) / part) / -0.2913);
+    // d = v0t - 1/2*t^2*Constant
+    // d = v0t - 1/2*t^2*Constant
+    // t = (v - sqrt(-2 C d + v^2))/C
+
+    const auto decayConstant = 0.1795;
+
+    auto v = vel.mag();
+    auto part = pow(v, 2) - 2 * decayConstant * dist;
+    if (part > 0) {
+        auto t = (v - sqrt(part)) / decayConstant;
+        return time + RJ::Seconds(t);
+    } else {
+        return RJ::Time::max();
+    }
+
+    // auto part = vel.mag() * -3.43289;
 }
 
 SystemState::SystemState() {

soccer/config/realGermany.xml

@@ -186,9 +186,10 @@
  </ChipCalibration>
  <MotionControl>
   <Max_Acceleration value="2"/>
-  <Max_Velocity value="2"/>
+  <Max_Velocity value="3"/>
   <PathJitterCompensationFactor value="2"/>
   <PathChangeBoost value="50"/>
+  <X_Multiplier value="1.3"/>
  </MotionControl>
  <PathPlanner>
   <Max_Acceleration value="1"/>
@@ -206,24 +207,28 @@
    <EnableDebugDrawing value="false"/>
    <StepSize value="0.15"/>
    <GoalBias value="0.3"/>
+   <WayPointBias value="0.5"/>
   </RRT>
+  <goalPosChangeThreshold value="0.025"/>
+  <goalVelChangeThreshold value="0.025"/>
  </PathPlanner>
  <MotionConstraints>
   <Max_Acceleration value="1"/>
   <Max_Velocity value="1"/>
   <Max_Angle_Speed value="0.436"/>
   <Replan_Threshold value="0.3"/>
   <Replan_Lead_Time value="0"/>
+  <Max_Centripetal_Acceleration value="0.5"/>
  </MotionConstraints>
  <Rev2015>
   <translation>
-   <p value="3"/>
-   <i value="0"/>
-   <i_windup value="10"/>
-   <accelMultiplier value="0"/>
-   <d value="0.4"/>
+   <p value="5"/>
+   <i value="0.05"/>
+   <i_windup value="20"/>
+   <accelMultiplier value="2"/>
+   <d value="0.7"/>
    <velMultiplier value="1.2"/>
-   <minEffectiveVelocity value="0.1"/>
+   <minEffectiveVelocity value="0.2"/>
   </translation>
   <rotation>
    <p value="5"/>
@@ -288,4 +293,10 @@
   <robot_std_dev value="0.3"/>
   <start_x_offset value="0.1"/>
  </KickEvaluator>
+ <RobotFilter>
+  <Velocity_Alpha value="0.2"/>
+ </RobotFilter>
+ <RRTPlanner>
+  <partialReplanLeadTime value="0.2"/>
+ </RRTPlanner>
 </config>

soccer/gameplay/plays/testing/repeated_line_up.py

@@ -5,10 +5,15 @@
 import robocup
 import constants
 import time
+import enum
 
 
 ## Robots repeatedly line up on opposite sides of the field
 class RepeatedLineUp(play.Play):
+    class State(enum.Enum):
+        left = 0
+        right = 1
+        pause = 2
 
     PAUSE = 2.0
     BUFFER = .7
@@ -24,18 +29,18 @@ def __init__(self):
             tactics.line_up.LineUp(self.generate_line(RepeatedLineUp.BUFFER)),
             tools.sleep.SleepBehavior(RepeatedLineUp.PAUSE),
         ]
-        b = behavior_sequence.BehaviorSequence(continuous=True,
-                                               repeat=True,
-                                               behaviors=behaviors)
+        b = behavior_sequence.BehaviorSequence(
+            continuous=True, repeat=True, behaviors=behaviors)
         self.add_subbehavior(b, 'line up behavior')
 
     # x_multiplier is a 1 or -1 to indicate which side of the field to be on
     # 1 is right, -1 is left
     def generate_line(self, x_multiplier):
-        x = (constants.Field.Width / 2 - constants.Robot.Radius *
-             2) * x_multiplier
+        x = (constants.Field.Width / 2 - constants.Robot.Radius * 2
+             ) * x_multiplier
         y_start = 1.0
         line = robocup.Segment(
-            robocup.Point(x, constants.Robot.Radius + y_start), robocup.Point(
-                x, (constants.Robot.Radius * 2.3 + 0.1) * 6 + y_start))
+            robocup.Point(x, constants.Robot.Radius + y_start),
+            robocup.Point(x,
+                          (constants.Robot.Radius * 2.3 + 0.1) * 6 + y_start))
         return line

soccer/gameplay/skills/line_kick.py

@@ -30,13 +30,18 @@ def __init__(self):
         self.add_transition(behavior.Behavior.State.start,
                             LineKick.State.waiting, lambda: True,
                             'immediately')
-        self.add_transition(LineKick.State.waiting, LineKick.State.kick,
-                            lambda: self.enable_kick, 'kicker is enabled')
+        self.add_transition(LineKick.State.waiting,
+                            LineKick.State.kick, lambda: self.enable_kick,
+                            'kicker is enabled')
 
         self.add_transition(
-            LineKick.State.kick, behavior.Behavior.State.completed,
-            lambda: self.robot is not None and self._got_close and self.robot.just_kicked(),
-            "robot kicked")
+            LineKick.State.kick,
+            behavior.Behavior.State.completed, lambda: self.robot is not None
+            and self._got_close and self.robot.just_kicked(), "robot kicked")
+        self.shell_id = None
+
+        self.max_speed = None
+        self.max_accel = None
 
     def on_enter_running(self):
         super().recalculate_aim_target_point()
@@ -46,6 +51,9 @@ def execute_kick(self):
         self.robot.disable_avoid_ball()
         self.robot.line_kick(self.aim_target_point)
 
+        if self.max_speed is not None:
+            self.robot.set_max_speed(self.max_speed)
+
         if main.ball().pos.dist_to(
                 self.robot.pos) < LineKick.ClosenessThreshold:
             self._got_close = True
@@ -64,4 +72,7 @@ def role_requirements(self):
         reqs.require_kicking = True
         if self.use_chipper:
             reqs.chipper_preference_weight = role_assignment.PreferChipper
+
+        if self.shell_id:
+            reqs.required_shell_id = self.shell_id
         return reqs

soccer/gameplay/skills/line_kick_receive.py

@@ -15,7 +15,7 @@
 # the ball is moving and attempt to catch it.
 # It will move to the 'completed' state if it catches the ball, otherwise it will go to 'failed'.
 class LineKickReceive(
-    single_robot_composite_behavior.SingleRobotCompositeBehavior):
+        single_robot_composite_behavior.SingleRobotCompositeBehavior):
 
     ## how much we're allowed to be off in the direction of the pass line
     PositionErrorThreshold = 0.1
@@ -45,24 +45,28 @@ def __init__(self, captureFunction=(lambda: skills.capture.Capture())):
                             LineKickReceive.State.aligning, lambda: True,
                             'immediately')
 
-        self.add_transition(
-            LineKickReceive.State.aligning, LineKickReceive.State.aligned,
-            lambda: self.errors_below_thresholds() and not self.ball_kicked,
-            'steady and in position to receive')
+        self.add_transition(LineKickReceive.State.aligning,
+                            LineKickReceive.State.aligned, lambda: self.
+                            errors_below_thresholds() and not self.ball_kicked,
+                            'steady and in position to receive')
 
-        self.add_transition(
-            LineKickReceive.State.aligned, LineKickReceive.State.aligning,
-            lambda: not self.errors_below_thresholds() and not self.ball_kicked,
-            'not in receive position')
+        self.add_transition(LineKickReceive.State.aligned,
+                            LineKickReceive.State.aligning, lambda: not self.
+                            errors_below_thresholds() and not self.ball_kicked,
+                            'not in receive position')
 
-        for state in [LineKickReceive.State.aligning,
-                      LineKickReceive.State.aligned]:
-            self.add_transition(state, LineKickReceive.State.receiving,
-                                lambda: self.ball_kicked, 'ball kicked')
+        for state in [
+                LineKickReceive.State.aligning, LineKickReceive.State.aligned
+        ]:
+            self.add_transition(
+                state,
+                LineKickReceive.State.receiving, lambda: self.ball_kicked,
+                'ball kicked')
 
-        self.add_transition(LineKickReceive.State.receiving,
-                            behavior.Behavior.State.completed,
-                            lambda: self.robot.has_ball(), 'ball received!')
+        self.add_transition(
+            LineKickReceive.State.receiving,
+            behavior.Behavior.State.completed, lambda: self.robot.has_ball(),
+            'ball received!')
 
         # TODO add a failed state for this Receiver (possibly a timeout...)
         # self.add_transition(
@@ -109,6 +113,7 @@ def execute_aligning(self):
 
     def on_enter_receiving(self):
         kick = skills.line_kick.LineKick()
+        kick.shell_id = self.robot.shell_id()
         kick.target = self.target
         self.add_subbehavior(kick, 'kick', required=True)
         self.kicked_time = time.time()

soccer/gameplay/tactics/coordinated_pass.py

@@ -55,9 +55,8 @@ def __init__(self,
             skillreceiver = skills.pass_receive.PassReceive()
 
         if skillkicker == None:
-            skillkicker = (
-                skills.pivot_kick.PivotKick(),
-                lambda x: x == skills.pivot_kick.PivotKick.State.aimed)
+            skillkicker = (skills.pivot_kick.PivotKick(), lambda x: x ==
+                           skills.pivot_kick.PivotKick.State.aimed)
 
         self.receive_point = receive_point
         self.skillreceiver = skillreceiver
@@ -81,8 +80,9 @@ def __init__(self,
 
         self.add_transition(
             CoordinatedPass.State.preparing, CoordinatedPass.State.kicking,
-            lambda: (skillkicker[1](self.subbehavior_with_name('kicker').state) and self.subbehavior_with_name('receiver').state == self.skillreceiver.State.aligned),
-            'kicker and receiver ready')
+            lambda: (skillkicker[1](self.subbehavior_with_name('kicker').state)
+                     and self.subbehavior_with_name('receiver').state == self.
+                     skillreceiver.State.aligned), 'kicker and receiver ready')
 
         self.add_transition(
             CoordinatedPass.State.preparing, CoordinatedPass.State.timeout,
@@ -96,19 +96,20 @@ def __init__(self,
                             CoordinatedPass.State.timeout,
                             self.prekick_timeout_exceeded, 'Timed out on kick')
 
-        self.add_transition(
-            CoordinatedPass.State.kicking, CoordinatedPass.State.receiving,
-            lambda: self.subbehavior_with_name('kicker').state == behavior.Behavior.State.completed,
-            'kicker kicked')
+        self.add_transition(CoordinatedPass.State.kicking,
+                            CoordinatedPass.State.receiving, lambda: self.
+                            subbehavior_with_name('kicker').state == behavior.
+                            Behavior.State.completed, 'kicker kicked')
 
         self.add_transition(
             CoordinatedPass.State.receiving, behavior.Behavior.State.completed,
-            lambda: self.subbehavior_with_name('receiver').state == behavior.Behavior.State.completed,
-            'pass received!')
+            lambda: self.subbehavior_with_name('receiver').state == behavior.
+            Behavior.State.completed, 'pass received!')
 
         self.add_transition(
-            CoordinatedPass.State.receiving, behavior.Behavior.State.failed,
-            lambda: self.subbehavior_with_name('receiver').state == behavior.Behavior.State.failed,
+            CoordinatedPass.State.receiving,
+            behavior.Behavior.State.failed, lambda: self.subbehavior_with_name(
+                'receiver').state == behavior.Behavior.State.failed,
             'pass failed :(')
 
     ## Handles restarting this behaivor.
@@ -138,9 +139,8 @@ def receive_point(self, value):
     def on_enter_running(self):
         receiver = self.skillreceiver
         receiver.receive_point = self.receive_point
-        self.add_subbehavior(receiver,
-                             'receiver',
-                             required=self.receiver_required)
+        self.add_subbehavior(
+            receiver, 'receiver', required=self.receiver_required)
 
     def on_exit_running(self):
         self.remove_subbehavior('receiver')
@@ -152,11 +152,9 @@ def on_enter_preparing(self):
         kicker = self.skillkicker[0]
         kicker.target = self.receive_point
         kickpower = (main.ball().pos - self.receive_point).mag() / 8
-        if (kickpower < 0.2):
-            kickpower = 0.2
 
-        if (kickpower > 1.0):
-            kickpower = 1.0
+        kickpower = max(0.4, min(kickpower, 1.0))
+
         kicker.kick_power = kickpower
         kicker.enable_kick = False  # we'll re-enable kick once both bots are ready
 

soccer/modeling/BallFilter.cpp

@@ -24,23 +24,12 @@ void BallFilter::updateEstimate(const BallObservation& obs) {
 
 Ball BallFilter::predict(RJ::Time time, float* velocityUncertainty) const {
     Ball prediction{};
-
-    RJ::Seconds t = time - _currentEstimate.time;
-
-    const auto& vel = _currentEstimate.vel;
-    const auto& pos = _currentEstimate.pos;
-    const auto s0 = vel.mag();
-    auto part = std::exp(-0.2913f * t.count());
-    auto speed = s0 * part;
-    auto distance = s0 * -3.43289f * (part - 1.0f);
+    const auto& estimate = _currentEstimate.predict(time);
 
     prediction.time = time;
     prediction.valid = true;
-    prediction.pos = pos + vel.normalized(distance);
-    prediction.vel = vel.normalized(speed);
+    prediction.pos = estimate.pos;
+    prediction.vel = estimate.vel;
 
-    if (velocityUncertainty) {
-        *velocityUncertainty = 2.0f + _currentEstimate.vel.mag() * 0.5f;
-    }
     return prediction;
 }