[Core] update touch

Core/src/Algorithm/BallSpeedModel.cpp

@@ -17,7 +17,7 @@ CBallSpeedModel::CBallSpeedModel():_ballVel(0,0),_ballPos(-9999,-9999){
     bool IS_SIMULATION;
     ZSS::ZParamManager::instance()->loadParam(IS_SIMULATION,"Alert/IsSimulation",false);
     if (IS_SIMULATION)
-		ZSS::ZParamManager::instance()->loadParam(_DEC, "AlertParam/BallDec_Sim", 1200);
+		ZSS::ZParamManager::instance()->loadParam(_DEC, "AlertParam/BallDec_Sim", 1100);
 	else
 		ZSS::ZParamManager::instance()->loadParam(_DEC, "AlertParam/BallDec_Real", 400);
 }

Core/src/MotionControl/CMmotion.cpp

@@ -13,7 +13,7 @@ using namespace std;
 namespace {
 const double FRAME_PERIOD = 1.0 / PARAM::Vision::FRAME_RATE;
 
-bool DEBUG_NO_ZERO_VEL = false;
+bool DEBUG_NO_ZERO_VEL = ZSS::ZParamManager::instance()->value("Debug/NoZeroVel", QVariant(false)).toBool();
 const double DEC_FACTOR = 2.0;
 const double lowerBoundSpeedLimitRotate = 0;
 const double upperBoundSpeedLimitRotate = 300*10;
@@ -76,7 +76,7 @@ void compute_motion_1d(double x0, double v0, double v1,
     }
 
 
-    double decFactor = (pT == ROTATE ? 1.0 : DEC_FACTOR);
+    double decFactor = DEC_FACTOR;
     a_max /= a_factor;
     d_max /= (1.0 * a_factor);
 
@@ -231,27 +231,27 @@ void compute_motion_2d(CVector x0, CVector v0, CVector v1,
     double rotangle = 0;
     double traj_accel_x = 0;
     double traj_accel_y = 0;
-    if(v0 * x0 > 0) {           //如果发现正在反方向走，则不再零速到点，防止车冲出去
-//        GDebugEngine::Instance()->gui_debug_msg(CGeoPoint(1.0, 0.0), QString("Limit v1").toLatin1());
-        v1 = CVector(0.0, 0.0);
-    }
-    if (v1.mod() == 0 || mode == FAST) {
+//     if(v0 * x0 > 0) {           //如果发现正在反方向走，则不再零速到点，防止车冲出去
+// //        GDebugEngine::Instance()->gui_debug_msg(CGeoPoint(1.0, 0.0), QString("Limit v1").toLatin1());
+//         v1 = CVector(0.0, 0.0);
+//     }
+    // if (v1.mod() == 0 || mode == FAST) {
         rotangle = x0.dir();
-    }
-    else {
-        rotangle = v1.dir();
-    }
+    // }
+    // else {
+        // rotangle = v1.dir();
+    // }
     x0 = x0.rotate(-rotangle);
     v0 = v0.rotate(-rotangle);
     v1 = v1.rotate(-rotangle); //坐标系转换，转换到末速度方向为x轴的坐标系中
 
     double velFactorX = 1.0, velFactorY = 1.0;
-    velFactorX = (fabs(v1.x()) > 1e-8 ? 2.8 : 1.0);
-    velFactorY = (fabs(v1.y()) > 1e-8 ? 2.8 : 1.0);
-    if(v1.mod() > 0 && mode == FAST) {
-        v1.setVector(copysign(v1.mod(), v1.x()), 0);
-//        v_max = v1.mod();
-    }
+    // velFactorX = (fabs(v1.x()) > 1e-8 ? 2.8 : 1.0);
+    // velFactorY = (fabs(v1.y()) > 1e-8 ? 2.8 : 1.0);
+//     if(v1.mod() > 0 && mode == FAST) {
+//         v1.setVector(copysign(v1.mod(), v1.x()), 0);
+// //        v_max = v1.mod();
+//     }
 
     timeItor = 0;
     isX = 1;
@@ -262,27 +262,31 @@ void compute_motion_2d(CVector x0, CVector v0, CVector v1,
     compute_motion_1d(x0.y(), v0.y(), v1.y(), a_max, d_max, v_max, a_factor, velFactorY,
                       traj_accel_y, time_y, time_y_acc, time_y_dec, time_y_flat, MOVE_Y, mode);//两轴同样的最大速度、加速度独立考虑求两轴运动时间
 
-    if(v1.mod() > 1e-8 && mode == ACCURATE) {
-        if (time_x - time_y > FRAME_PERIOD) {
-            compute_motion_1d(x0.y(), v0.y(), 0, a_max, d_max, v_max, a_factor, velFactorX,
-                              traj_accel_y, time_y, time_y_acc, time_y_dec, time_y_flat, MOVE_X, mode);
-        } else if (time_y - time_x > FRAME_PERIOD) {
-            compute_motion_1d(x0.x(), v0.x(), 0, a_max, d_max, v_max, a_factor, velFactorY,
-                              traj_accel_x, time_x, time_x_acc, time_x_dec, time_x_flat, MOVE_X, mode);
-        }
-    }
+    // if(v1.mod() > 1e-8 && mode == ACCURATE) {
+    //     if (time_x - time_y > FRAME_PERIOD) {
+    //         compute_motion_1d(x0.y(), v0.y(), 0, a_max, d_max, v_max, a_factor, velFactorX,
+    //                           traj_accel_y, time_y, time_y_acc, time_y_dec, time_y_flat, MOVE_X, mode);
+    //     } else if (time_y - time_x > FRAME_PERIOD) {
+    //         compute_motion_1d(x0.x(), v0.x(), 0, a_max, d_max, v_max, a_factor, velFactorY,
+    //                           traj_accel_x, time_x, time_x_acc, time_x_dec, time_x_flat, MOVE_X, mode);
+    //     }
+    // }
     if(v1.mod() > 0 && DEBUG_NO_ZERO_VEL) {
-        GDebugEngine::Instance()->gui_debug_msg(CGeoPoint(0.0, 0.0), QString("xVel: %1").arg(v0.x()).toLatin1());
+        GDebugEngine::Instance()->gui_debug_msg(CGeoPoint(0.0, 0.0), QString("Vel: %1 %2").arg(v0.x()).arg(v0.y()).toLatin1());
         GDebugEngine::Instance()->gui_debug_msg(CGeoPoint(0.0, 20.0*10), QString("xVelFinal: %1").arg(v0.x() + traj_accel_x * FRAME_PERIOD).toLatin1());
-        GDebugEngine::Instance()->gui_debug_msg(CGeoPoint(0.0, 40.0*10), QString("targetVel:  %1").arg(v1.mod()).toLatin1());
+        GDebugEngine::Instance()->gui_debug_msg(CGeoPoint(0.0, 40.0*10), QString("targetVel:  %1 %2 %3").arg(v1.mod()).arg(v1.x()).arg(v1.y()).toLatin1());
         GDebugEngine::Instance()->gui_debug_msg(CGeoPoint(0.0, 60.0*10), QString("yVel: %1").arg(v0.y()).toLatin1());
         GDebugEngine::Instance()->gui_debug_msg(CGeoPoint(0.0, 80.0*10), QString("yVelFinal: %1").arg(v0.y() + traj_accel_y * FRAME_PERIOD).toLatin1());
-        GDebugEngine::Instance()->gui_debug_msg(CGeoPoint(0.0, 100.0*10), QString("v_max: %1").arg(v_max).toLatin1());
+        // GDebugEngine::Instance()->gui_debug_msg(CGeoPoint(0.0, 100.0*10), QString("v_max: %1").arg(v_max).toLatin1());
+        auto S = CGeoPoint(-1000,-1000);
+        GDebugEngine::Instance()->gui_debug_line(S,S+v0.rotate(rotangle),COLOR_BLUE);
+        GDebugEngine::Instance()->gui_debug_msg(S + v0.rotate(rotangle), "v0", COLOR_BLUE);
+        GDebugEngine::Instance()->gui_debug_line(S, S + v1.rotate(rotangle), COLOR_BLUE);
+        GDebugEngine::Instance()->gui_debug_msg(S + v1.rotate(rotangle), "v1", COLOR_BLUE);
     }
 
     traj_accel = CVector(traj_accel_x, traj_accel_y);
-    if (traj_accel.mod())
-        traj_accel = traj_accel.rotate(rotangle);
+    traj_accel = traj_accel.rotate(rotangle);
     if(time_x < 1e-5 || time_x > 50) time_x = 0;
     if(time_y < 1e-5 || time_y > 50) time_y = 0;
     if(time_x < time_y) {
@@ -362,6 +366,7 @@ void goto_point_omni( const PlayerPoseT& start,
     double time_a, time_a_acc, time_a_dec, time_a_flat, time;
     double time_acc, time_dec, time_flat;
     compute_motion_2d(x, v, target_vel, max_accel, max_decel, max_speed, accel_factor, a, time, time_acc, time_dec, time_flat, mode);
+    // a = Utils::Polar2Vector(std::min(1.0,a.mod()),a.dir());
     factor_a = 1;
 
     double rotateFactor;

Core/src/MotionControl/ControlModel.cpp

@@ -68,7 +68,7 @@ void CControlModel::makeCmTrajectory(const PlayerPoseT& start, const PlayerPoseT
 {
     _pathList.clear();
     double accel_factor = 1.5;
-    double angle_accel_factor = 6.5;
+    double angle_accel_factor = 5.5;
     if(IS_SIMULATION) {
         accel_factor = 1.0;
         angle_accel_factor = 1.0;

Core/src/Strategy/skill/SmartGotoPosition.cpp

@@ -193,7 +193,6 @@ void CSmartGotoPositionV2::plan(const CVisionModule* pVision)
 
     //处理无效目标点:在禁区内、在车身内、在场地外
     validateFinalTarget(finalTargetPos, myPos, avoidLength, isGoalie, avoidBallCircle, obs, onlyPlanInCircle, planCircleCenter, planCircleRadius);
-    CVector velNew;
 
     validateStartPoint(myPos, avoidLength, isGoalie, obs);
     GDebugEngine::Instance()->gui_debug_x(finalTargetPos, COLOR_YELLOW);
@@ -211,7 +210,6 @@ void CSmartGotoPositionV2::plan(const CVisionModule* pVision)
 
     TaskT newTask(task());
     newTask.player.pos = finalTargetPos;
-    newTask.player.vel = velNew;
 
     if(self.Pos().dist(finalTargetPos) < startToRotateToTargetDirDist && needBreakRotate)
         newTask.player.flag &= (!PlayerStatus::BREAK_THROUGH);

Core/tactics/play/TestGetBall.lua

@@ -0,0 +1,32 @@
+local testPos = {CGeoPoint:new_local(1000, 1000), CGeoPoint:new_local(-1000, 1000), CGeoPoint:new_local(-1000, -1000),
+                 CGeoPoint:new_local(1000, -1000)}
+local vel = CVector:new_local(0, 0)
+local maxvel = 0
+local time = 120
+local DSS_FLAG = bit:_or(flag.allow_dss, flag.dodge_ball)
+
+local DIR = function()
+    return (player.pos('Assister') - ball.pos()):dir()
+end
+
+return {
+    firstState = "run1",
+
+    ["run1"] = {
+        switch = function()
+            local dist = (ball.pos() - player.pos("Assister")):mod()
+            debugEngine:gui_debug_msg(ball.pos()+Utils.Polar2Vector(300,math.pi/2),"Dist : " .. dist)
+        end,
+        Assister = task.touchKick(CGeoPoint(6000,0),nil,1000,true),
+        -- Assister = task.stop(),
+        match = "[A]"
+    },
+
+    name = "TestGetBall",
+    applicable = {
+        exp = "a",
+        a = true
+    },
+    attribute = "attack",
+    timeout = 99999
+}

Core/tactics/play/TestMyRun.lua

@@ -1,8 +1,12 @@
-local testPos = {CGeoPoint:new_local(1000, 1000), CGeoPoint:new_local(-1000, 1000), CGeoPoint:new_local(-1000, -1000),
-                 CGeoPoint:new_local(1000, -1000)}
+local testPos = {
+    CGeoPoint:new_local(3000, 3100), 
+    CGeoPoint:new_local(-3000, 3100),
+    CGeoPoint:new_local(-3000,-3100),
+    CGeoPoint:new_local(3000,-3100)
+}
 local vel = CVector:new_local(0, 0)
 local maxvel = 0
-local time = 120
+local time = 1
 local DSS_FLAG = bit:_or(flag.allow_dss, flag.dodge_ball)
 
 local DIR = function()
@@ -11,12 +15,45 @@ end
 
 return {
     firstState = "run1",
-
     ["run1"] = {
         switch = function()
+            if bufcnt(player.toTargetDist("a")<5,time) then
+                return "run"..2
+            end
+        end,
+        a = task.goCmuRush(testPos[1],0, _, DSS_FLAG),
+        b = task.goCmuRush(testPos[1]+Utils.Polar2Vector(1000,-math.pi/2),0, _, DSS_FLAG),
+        match = "{ab}"
+    },
+    ["run2"] = {
+        switch = function()
+            if bufcnt(player.toTargetDist("a")<5,time) then
+                return "run"..3
+            end
+        end,
+        a = task.goCmuRush(testPos[2],0, _, DSS_FLAG),
+        b = task.goCmuRush(testPos[2]+Utils.Polar2Vector(1000,-math.pi/2),0, _, DSS_FLAG),
+        match = "{ab}"
+    },
+    ["run3"] = {
+        switch = function()
+            if bufcnt(player.toTargetDist("a")<5,time) then
+                return "run"..4
+            end
+        end,
+        a = task.goCmuRush(testPos[3],0, _, DSS_FLAG),
+        b = task.goCmuRush(testPos[3]+Utils.Polar2Vector(1000,-math.pi/2),0, _, DSS_FLAG),
+        match = "{ab}"
+    },
+    ["run4"] = {
+        switch = function()
+            if bufcnt(player.toTargetDist("a")<5,time) then
+                return "run"..1
+            end
         end,
-        Assister = task.goCmuRush(testPos[1], DIR, nil, DSS_FLAG),
-        match = "[A]"
+        a = task.goCmuRush(testPos[4],0, _, DSS_FLAG),
+        b = task.goCmuRush(testPos[4]+Utils.Polar2Vector(1000,-math.pi/2),0, _, DSS_FLAG),
+        match = "{ab}"
     },
 
     name = "TestMyRun",

Core/tactics/play/TestPassAndKick.lua

@@ -1,5 +1,5 @@
-local waitPos = ball.antiYPos(CGeoPoint:new_local(2600,1500))
-local waitPos2 = ball.antiYPos(CGeoPoint:new_local(1800,1500))
+local waitPos = ball.antiYPos(CGeoPoint:new_local(2800,2800))
+local waitPos2 = ball.antiYPos(CGeoPoint:new_local(2800,2800))
 local mode = true
 
 return {
@@ -20,9 +20,9 @@ firstState = "init",
             return "shoot"
         end
 	end,
-	Leader = task.touchKick(waitPos,false,3500,mode),
+	Leader = task.touchKick(waitPos,false,2500,mode),
     Assister = task.goCmuRush(waitPos2),
-	match = ""
+	match = "{LA}"
 },
 ["shoot"] = {
     switch = function()
@@ -32,7 +32,7 @@ firstState = "init",
     end,
     Leader = task.stop(),
     Assister = task.touchKick(pos.theirGoal(), false,6000,mode),
-    match = ""
+    match = "{LA}"
 },
 
 name = "TestPassAndKick",

Core/tactics/skill/Touch.cpp

@@ -1,8 +1,12 @@
+#include <fmt/core.h>
 #include "Touch.h"
 #include "parammanager.h"
 #include "WorldDefine.h"
 #include "VisionModule.h"
 #include "staticparams.h"
+#include "geomcalc.h"
+#include "BallSpeedModel.h"
+#include "CMmotion.h"
 namespace{
     const CGeoPoint THEIR_GOAL = CGeoPoint(PARAM::Field::PITCH_LENGTH/2,0);
     bool DEBUG_SWITCH;
@@ -19,41 +23,60 @@ void CTouch::plan(const CVisionModule* pVision){
     const int runner = task().executor;
     auto taskFlag = task().player.flag;
     const PlayerVisionT& me = pVision->ourPlayer(runner);
-    const auto mousePos = me.Pos() + Utils::Polar2Vector(PARAM::Vehicle::V2::PLAYER_CENTER_TO_BALL_CENTER,me.Dir());
+    const auto mouseVec = Utils::Polar2Vector(PARAM::Vehicle::V2::PLAYER_CENTER_TO_BALL_CENTER, me.Dir());
+    const auto mousePos = me.Pos() + mouseVec;
     const MobileVisionT& ball = pVision->ball();
     const double ballVelDir = ball.Vel().dir();
     const CGeoPoint& ballPos = ball.RawPos();
     const CGeoLine ballVelLine(ballPos, ballVelDir);
     const double ballVelMod = ball.Vel().mod();
-    const CGeoPoint projectionPos = ballVelLine.projection(mousePos);
-    const double toBallDist = (mousePos - ballPos).mod();
-    CGeoPoint targetPos;
-    double targetDir;
-    if(ballVelMod < 300){
-        targetDir = (target - ballPos).dir();
-        targetPos = ballPos + Utils::Polar2Vector(PARAM::Vehicle::V2::PLAYER_CENTER_TO_BALL_CENTER,targetDir + PARAM::Math::PI);
+    const CGeoPoint projectionMousePos = ballVelLine.projection(mousePos);
+    const CGeoPoint projectionRobotPos = projectionMousePos + mouseVec*-1;
+    const auto mouse2Ball = ballPos - mousePos;
+    const double toBallDist = mouse2Ball.mod();
+    // predict ball pos
+    const auto ballStopPose = BallSpeedModel::instance()->poseForTime(9999);
+    const auto getBallPos = BallSpeedModel::instance()->poseForTime(1.0).Pos(); // TODO : replace with GetBallPos after skillutils
+    const CGeoSegment ballRunningSeg(ballPos, ballStopPose.Pos());
+    const auto me2segTime = predictedTimeWithRawVel(me, projectionRobotPos);
+    const auto ball2segTime = BallSpeedModel::Instance()->timeForDist(ballPos.dist(projectionMousePos));
+    const bool canWaitForBall = ballRunningSeg.IsPointOnLineOnSegment(projectionMousePos) && me2segTime < ball2segTime;
+    const auto predictPos = ballVelMod > 100 ? getBallPos : ballPos;
 
-    }else{
-        targetDir = useInter ? Utils::Normalize(ballVelDir + PARAM::Math::PI) : (target - mousePos).dir();
-        targetPos = projectionPos + Utils::Polar2Vector(PARAM::Vehicle::V2::PLAYER_CENTER_TO_BALL_CENTER,targetDir + PARAM::Math::PI);
+    CGeoPoint targetRunPos;
+    double targetRunDir;
+    CVector targetRunVel;
+    if (!canWaitForBall){
+        targetRunDir = (target - predictPos).dir();
+        targetRunPos = predictPos + Utils::Polar2Vector(PARAM::Vehicle::V2::PLAYER_CENTER_TO_BALL_CENTER - 5, targetRunDir + PARAM::Math::PI);
+        targetRunVel = Utils::Polar2Vector(300, mouse2Ball.dir());
+    }
+    else{
+        targetRunDir = useInter ? Utils::Normalize(ballVelDir + PARAM::Math::PI) : (target - mousePos).dir();
+        targetRunPos = projectionMousePos + Utils::Polar2Vector(PARAM::Vehicle::V2::PLAYER_CENTER_TO_BALL_CENTER-5,targetRunDir + PARAM::Math::PI);
     }
 
+    const auto me2target = targetRunPos - me.Pos();
+    const auto runTarget2kickTarget = target - targetRunPos;
     // add avoid ball flag
-    if(toBallDist>200){
+    auto angle_diff = angleDiff(me2target.dir(), runTarget2kickTarget.dir());
+    GDebugEngine::Instance()->gui_debug_msg(CGeoPoint(-800,-800),fmt::format("anglediff  {:.2f}",angle_diff/PARAM::Math::PI*180.0));
+    if (toBallDist > 150 && std::abs(angle_diff) > (100.0) / 180.0 * PARAM::Math::PI)
+    {
         taskFlag |= PlayerStatus::DODGE_BALL;
     }
 
-
     TaskT newTask(task());
-    newTask.player.pos = targetPos;
-    newTask.player.angle = targetDir;
+    newTask.player.pos = targetRunPos;
+    newTask.player.angle = targetRunDir;
+    newTask.player.vel = targetRunVel;
     newTask.player.flag = taskFlag;
-    // setSubTask(TaskFactoryV2::Instance()->SmartGotoPosition(newTask));
     setSubTask("SmartGoto", newTask);
 
     if(DEBUG_SWITCH){
         auto endPos = ballPos + Utils::Polar2Vector(ballVelMod,ballVelDir);
         GDebugEngine::Instance()->gui_debug_line(ballPos,endPos,4);
+        GDebugEngine::Instance()->gui_debug_msg(targetRunPos, fmt::format("TVel:{},me2SegT:{:3.1f},b2SegT:{:3.1f}", targetRunVel.mod(), me2segTime, ball2segTime));
     }
 
     _lastCycle = pVision->getCycle();

Core/tactics/skill/Touch.lua

@@ -11,7 +11,6 @@ function Touch(task)
 		task_param.executor = runner
 		task_param.player.pos = mpos
 		task_param.is_specify_ctrl_method = useInter
-		-- return CTouch(runner, mpos:x(), mpos:y(), useInter)
 		return skillapi:run("Touch", task_param)
 	end