Merge pull request #2 from ibis-ssl/feature/weak_dribbler

Feature/weak dribbler
ibis-ssl · Nov 12, 2023 · c0a48bc · c0a48bc
2 parents 76ec81e + 76d0619
commit c0a48bc
Show file tree

Hide file tree

Showing 3 changed files with 24 additions and 2 deletions.
diff --git a/include/robot.h b/include/robot.h
@@ -79,6 +79,7 @@ class Robot {
         int kickstate;
         dReal m_kickspeed,m_kicktime;
         bool holdingBall;
+        dJointFeedback *dribble_feedback = nullptr;
       public:
         Kicker(Robot* robot);
         void step();
@@ -89,6 +90,7 @@ class Robot {
         bool isTouchingBall();
         KickStatus isKicking();
         void holdBall();
+        bool checkDribbleFeedback();
         void unholdBall();
         dJointID joint;
         dJointID robot_to_ball;

diff --git a/src/robot.cpp b/src/robot.cpp
@@ -95,6 +95,8 @@ Robot::Kicker::Kicker(Robot* robot) : holdingBall(false)
 void Robot::Kicker::step()
 {
     if (!isTouchingBall() || rolling == 0) unholdBall();
+    bool pre_ball_hold = holdingBall;
+    bool dribble_failed = checkDribbleFeedback();
     if (kicking != NO_KICK)
     {
         box->setColor(1,0.3,0);
@@ -104,7 +106,7 @@ void Robot::Kicker::step()
     else if (rolling!=0)
     {
         box->setColor(1,0.7,0);
-        if (isTouchingBall())
+        if (isTouchingBall() && not (dribble_failed && pre_ball_hold))
         {
             holdBall();
         }
@@ -193,9 +195,27 @@ void Robot::Kicker::holdBall(){
     dBodySetLinearVel(rob->getBall()->body,0,0,0);
     robot_to_ball = dJointCreateHinge(rob->getWorld()->world,0);
     dJointAttach (robot_to_ball,box->body,rob->getBall()->body);
+    if(dribble_feedback == nullptr) {
+        dribble_feedback = new dJointFeedback();
+    }
+    if(dribble_feedback != nullptr){
+        dJointSetFeedback(joint, dribble_feedback);
+    }
     holdingBall = true;
 }
 
+bool Robot::Kicker::checkDribbleFeedback() {
+    if(dribble_feedback != nullptr){
+        // かかっている力の大きさを計算
+        double norm_force_f1 = sqrt(pow(dribble_feedback->f1[0], 2) + pow(dribble_feedback->f1[1], 2) + pow(dribble_feedback->f1[2], 2));
+        if(norm_force_f1 > 0.5) {
+            unholdBall();
+            return true;
+        }
+    }
+    return false;
+}
+
 void Robot::Kicker::unholdBall(){
     if(holdingBall) {
         dJointDestroy(robot_to_ball);

diff --git a/src/sslworld.cpp b/src/sslworld.cpp
@@ -1006,7 +1006,7 @@ SSL_WrapperPacket* SSLWorld::generatePacket(int cam_id) {
         field->set_boundary_width(CONVUNIT(cfg->Field_Margin()));
         field->set_goal_width(CONVUNIT(cfg->Goal_Width()));
         field->set_goal_depth(CONVUNIT(cfg->Goal_Depth()));
-	field->set_penalty_area_depth(CONVUNIT(cfg->Field_Penalty_Depth()));
+        field->set_penalty_area_depth(CONVUNIT(cfg->Field_Penalty_Depth()));
         field->set_penalty_area_width(CONVUNIT(cfg->Field_Penalty_Width()));
 
         // Field lines and arcs