Merge branch 'main' of https://github.com/citbrains/GankenKun_webots …

…into main

controllers/learning/learning.py

@@ -46,7 +46,7 @@
 children = supervisor.getRoot().getField('children')
 children.importMFNodeFromString(-1, f'RobocupSoccerField {{ size "kid" }}')
 children.importMFNodeFromString(-1, f'DEF BALL RobocupSoccerBall {{ translation 0 0 0.1 size 1 }}')
-children.importMFNodeFromString(-1, f'DEF PLAYER RoboCup_GankenKun {{translation -0.3 0 0.450 rotation 0 0 1 0 controller "play_motion"}}')
+children.importMFNodeFromString(-1, f'DEF PLAYER RoboCup_GankenKun {{translation -0.3 0 0.450 rotation 0 0 1 0 controller "play_motion" controllerArgs "./kick_motion0.csv"}}')
 player = supervisor.getFromDef('PLAYER')
 ball = supervisor.getFromDef('BALL')
 player_translation = supervisor.getFromDef('PLAYER').getField('translation')
@@ -60,12 +60,8 @@
         for y in np.arange(0, 0.25, 0.01):
             count = 0
             player.remove()
-            children.importMFNodeFromString(-1, f'DEF PLAYER RoboCup_GankenKun {{translation {x} {y} 0.450 rotation 0 0 1 0 controller "play_motion"}}')
+            children.importMFNodeFromString(-1, f'DEF PLAYER RoboCup_GankenKun {{translation {x} {y} 0.450 rotation 0 0 1 0 controller "play_motion" controllerArgs "./kick_motion0.csv"}}')
             player = supervisor.getFromDef('PLAYER')
-            #player.resetPhysics()
-            #player_translation.setSFVec3f([x, y, 0.450])
-            #player_rotation.setSFRotation([0, 0, 1, 0])
-            #player_controller.setSFString("play_motion")
             ball.resetPhysics()
             ball_translation.setSFVec3f([0, 0, 0.1])
             ball_rotation.setSFRotation([0, 0, 1, 0])
@@ -79,6 +75,5 @@
                     with open('result.csv', 'a', newline='') as f:
                         writer = csv.writer(f)
                         writer.writerow([x, y, pos[0], pos[1]])
-                    #player_controller.setSFString("void")
 except Exception:
     error(f"Unexpected exception in main referee loop: {traceback.format_exc()}", fatal=True)

controllers/learning_motion/field.py

@@ -0,0 +1,58 @@
+# Copyright 1996-2021 Cyberbotics Ltd.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+
+class Field:
+    def __init__(self, size):
+        self.size = size
+        self.size_y = 3 if size == 'kid' else 4.5
+        self.size_x = 4.5 if size == 'kid' else 7
+        self.penalty_mark_x = 3 if size == 'kid' else 4.9
+        self.goal_area_length = 1
+        self.goal_area_width = 3 if size == 'kid' else 4
+        self.goal_height = 1.2 if size == 'kid' else 1.8
+        self.penalty_area_length = 2 if size == 'kid' else 3
+        self.penalty_area_width = 5 if size == 'kid' else 6
+        self.circle_radius = 0.75 if size == 'kid' else 1.5
+        self.penalty_offset = 0.6 if size == 'kid' else 1
+        self.opponent_distance_to_ball = 0.75 if size == 'kid' else 1.5
+        self.ball_vincity = 0.75 if size == 'kid' else 1.5
+        self.robot_radius = 0.3 if size == 'kid' else 0.5
+        self.place_ball_safety_dist = 0.5 if size == 'kid' else 1.0
+        self.turf_depth = 0.01
+        self.border_strip_width = 1
+        self.line_width = 0.05
+        self.line_half_width = self.line_width / 2
+
+    def point_inside(self, point, include_turf=False, include_border_line=True):
+        if point[2] > self.turf_depth:  # in the air
+            return False
+        x = self.size_x + (self.border_strip_width if include_turf else 0)
+        y = self.size_y + (self.border_strip_width if include_turf else 0)
+        if not include_border_line:
+            x -= self.line_width
+            y -= self.line_width
+        if point[0] > x or point[0] < -x or point[1] > y or point[1] < -y:
+            return False
+        return True
+
+    def circle_fully_inside_goal_area(self, point, radius):
+        return (abs(point[0]) - radius > self.size_x - self.goal_area_length and
+                abs(point[0]) + radius < self.size_x and
+                abs(point[1]) + radius < self.goal_area_width / 2)
+
+    def circle_fully_inside_penalty_area(self, point, radius):
+        return (abs(point[0]) - radius > self.size_x - self.penalty_area_length and
+                abs(point[0]) + radius < self.size_x and
+                abs(point[1]) + radius < self.penalty_area_width / 2)

controllers/learning_motion/learning_motion.py

@@ -0,0 +1,95 @@
+# Copyright 1996-2021 Cyberbotics Ltd.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+#from gamestate import GameState
+from field import Field
+#from forceful_contact_matrix import ForcefulContactMatrix
+
+from controller import Supervisor, AnsiCodes, Node
+
+import copy
+import json
+import math
+import numpy as np
+import os
+import random
+import socket
+import subprocess
+import sys
+import time
+import traceback
+import transforms3d
+import random
+import numpy as np
+import csv
+from scipy.spatial import ConvexHull
+from types import SimpleNamespace
+from skopt import gp_minimize
+
+# start the webots supervisor
+supervisor = Supervisor()
+time_step = int(supervisor.getBasicTimeStep())
+
+field = Field("kid")
+children = supervisor.getRoot().getField('children')
+children.importMFNodeFromString(-1, f'RobocupSoccerField {{ size "kid" }}')
+children.importMFNodeFromString(-1, f'DEF BALL RobocupSoccerBall {{ translation 0 0 0.1 size 1 }}')
+children.importMFNodeFromString(-1, f'DEF PLAYER RoboCup_GankenKun {{translation -0.3 0 0.450 rotation 0 0 1 0 controller "play_motion" controllerArgs "motion.csv"}}')
+player = supervisor.getFromDef('PLAYER')
+ball = supervisor.getFromDef('BALL')
+player_translation = supervisor.getFromDef('PLAYER').getField('translation')
+player_rotation = supervisor.getFromDef('PLAYER').getField('rotation')
+player_controller = supervisor.getFromDef('PLAYER').getField('controller')
+ball_translation = supervisor.getFromDef('BALL').getField('translation')
+ball_rotation = supervisor.getFromDef('BALL').getField('rotation')
+
+def func(param):
+    global player, ball, children, ball_translation, ball_rotation, supervisor
+
+    with open("../play_motion/kick_motion.csv", "r") as f:
+        read_data = csv.reader(f, delimiter=",", lineterminator="\r\n")
+        data = [row for row in read_data]
+        data[6][0] = str(int(param[0]))
+        data[7][0] = str(int(param[1]))
+        data[7][3] = str(int(param[2]))
+        data[7][4] = str(int(param[3]))
+    with open("../play_motion/motion.csv", "w") as f:
+        writer = csv.writer(f)
+        writer.writerows(data)
+
+    count = 0
+    player.remove()
+    children.importMFNodeFromString(-1, f'DEF PLAYER RoboCup_GankenKun {{translation -0.2 0.1 0.450 rotation 0 0 1 0 controller "play_motion" controllerArgs "motion.csv"}}')
+    player = supervisor.getFromDef('PLAYER')
+    ball.resetPhysics()
+    ball_translation.setSFVec3f([0, 0, 0.1])
+    ball_rotation.setSFRotation([0, 0, 1, 0])
+    while supervisor.step(time_step) != -1:
+        count += 1
+        if count > 800:
+            break
+        if count > 800 - 1:
+            pos = ball_translation.getSFVec3f()
+            with open('result.csv', 'a', newline='') as f:
+                writer = csv.writer(f)
+                writer.writerow([param[0], param[1], param[2], param[3], pos[0], pos[1]])
+    return -pos[0]
+
+x0 = (1,50)
+x1 = (1,50)
+x2 = (0,80)
+x3 = (0,80)
+x = (x0, x1, x2, x3)
+result = gp_minimize(func, x, n_calls=100, noise=0.0, model_queue_size=1, verbose=True)
+

controllers/play_motion/kick_motion.csv

@@ -0,0 +1,13 @@
+100,-5,0,-16,21,1.5,0,-18,-8,40,5,0,-16,21,-1.5,0,-18,8,40,0,0,0,0,0,0,0,0,0,0,0,14,14,0x0
+15,-14,0,-22,22,14,2,-18,-18,0,-10,-1,-20,20,14,0,0,18,0,0,0,0,0,0,0,0,0,0,0,0,2,2,0x0
+10,-14,0,-12,10,15,2,-18,-18,0,-15,1,-5,3,15,0,0,18,0,0,0,0,0,0,0,0,0,0,0,0,3,3,0x0
+15,-18,-2,-50,36,27.5,2,-18,-18,0,-16,1,-15,13,17,0,0,18,0,0,0,0,0,0,0,0,0,0,0,0,4,4,0x0
+20,-15,-27,-61,48,27.6,15,50,-18,0,-16,17,7,40,16,-15,0,18,0,0,0,0,0,0,0,0,0,0,0,0,5,5,0x0
+10,-17,-36,-12,66,18,5,40,-18,0,-16,17,7,40,14,-5,0,18,0,0,0,0,0,0,0,0,0,0,0,0,6,6,0x0
+10,-17,10,30,72,36.2,-15,40,-18,0,-16,-40,-50,-20,14.5,10,0,18,0,0,0,0,0,0,0,0,0,0,0,0,7,7,0x0
+15,-18,1,30,39,25,0,-30,-11,0,-16,1,-15,13,14.5,0,-18,8,0,0,0,0,0,0,0,0,0,0,0,0,10,10,0x0
+10,-18,-5,-8.5,8,25,0,-18,-8,0,-9,1,-5,3,12.5,0,-18,8,0,0,0,0,0,0,0,0,0,0,0,0,11,11,0x0
+10,-5,0,-5,3,3,0,-18,-8,0,5,0,-5,3,-3,0,-18,8,0,0,0,0,0,0,0,0,0,0,0,0,12,12,0x0
+25,-5,0,-16,21,1.5,0,-18,-8,40,5,0,-16,21,-1.5,0,-18,8,40,0,0,0,0,0,0,0,0,0,0,0,13,13,0x0
+5,-5,0,-16,21,1.5,0,-18,-8,40,5,0,-16,21,-1.5,0,-18,8,40,0,0,0,0,0,0,0,0,0,0,0,14,14,0x0
+100,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,15,15,0x0

controllers/play_motion/kick_motion0.csv

@@ -0,0 +1,15 @@
+100,-5,0,-16,21,1.5,0,-18,-8,40,5,0,-16,21,-1.5,0,-18,8,40,0,0,0,0,0,0,0,0,0,0,0,14,14,0x0
+15,-14,0,-22,22,14,2,-18,-18,0,-10,-1,-20,20,14,0,0,18,0,0,0,0,0,0,0,0,0,0,0,0,2,2,0x0
+10,-14,0,-12,10,15,2,-18,-18,0,-15,1,-5,3,15,0,0,18,0,0,0,0,0,0,0,0,0,0,0,0,3,3,0x0
+15,-18,-2,-50,36,27.5,2,-18,-18,0,-16,1,-15,13,17,0,0,18,0,0,0,0,0,0,0,0,0,0,0,0,4,4,0x0
+20,-15,-27,-61,48,27.6,15,50,-18,0,-16,17,7,40,16,-15,0,18,0,0,0,0,0,0,0,0,0,0,0,0,5,5,0x0
+10,-17,-36,-12,66,18,5,40,-18,0,-16,17,7,40,14,-5,0,18,0,0,0,0,0,0,0,0,0,0,0,0,6,6,0x0
+10,-17,10,30,72,36.2,-15,40,-18,0,-16,-40,-50,-20,14.5,10,0,18,0,0,0,0,0,0,0,0,0,0,0,0,7,7,0x0
+4,-17,40,30,92,36.2,-15,-30,-18,0,-16,-35,-50,-20,14.5,20,0,18,0,0,0,0,0,0,0,0,0,0,0,0,8,8,0x0
+4,-17,40,30,92,36.2,-15,-30,-18,0,-16,-20,-50,-20,14.5,0,-18,8,0,0,0,0,0,0,0,0,0,0,0,0,9,9,0x0
+15,-18,1,30,39,25,0,-30,-11,0,-16,1,-15,13,14.5,0,-18,8,0,0,0,0,0,0,0,0,0,0,0,0,10,10,0x0
+10,-18,-5,-8.5,8,25,0,-18,-8,0,-9,1,-5,3,12.5,0,-18,8,0,0,0,0,0,0,0,0,0,0,0,0,11,11,0x0
+10,-5,0,-5,3,3,0,-18,-8,0,5,0,-5,3,-3,0,-18,8,0,0,0,0,0,0,0,0,0,0,0,0,12,12,0x0
+25,-5,0,-16,21,1.5,0,-18,-8,40,5,0,-16,21,-1.5,0,-18,8,40,0,0,0,0,0,0,0,0,0,0,0,13,13,0x0
+5,-5,0,-16,21,1.5,0,-18,-8,40,5,0,-16,21,-1.5,0,-18,8,40,0,0,0,0,0,0,0,0,0,0,0,14,14,0x0
+100,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,15,15,0x0

controllers/play_motion/play_motion.py

@@ -1,8 +1,12 @@
 from controller import Robot, Motor
 import csv
 import math
+import sys
 
-csv_file = open("./motion.csv", "r")
+file_name = "./motion.csv"
+if len(sys.argv) > 1:
+    file_name = sys.argv[1]
+csv_file = open(file_name, "r")
 f = csv.reader(csv_file, delimiter=",", lineterminator="\r\n")
 data = [row for row in f]
 

worlds/learning_motion.wbt

@@ -0,0 +1,49 @@
+#VRML_SIM R2021b utf8
+WorldInfo {
+  info [
+    "GANKENKUN robot."
+    "The GANKENKUN robot simulation model"
+  ]
+  title "GANKENKUN"
+  basicTimeStep 8
+  optimalThreadCount 1
+  physicsDisableTime 0.1
+  physicsDisableLinearThreshold 0.1
+  physicsDisableAngularThreshold 0.1
+  contactProperties [
+    ContactProperties {
+      material1 "grass"
+      coulombFriction [
+        0.5
+      ]
+      softCFM 0.03
+    }
+    ContactProperties {
+      material1 "grass"
+      material2 "robocup soccer ball"
+      coulombFriction [
+        0.5
+      ]
+      bounce 0.76
+      softCFM 0.05
+    }
+    ContactProperties {
+      material2 "robocup soccer ball"
+      bounce 0.76
+    }
+  ]
+}
+Viewpoint {
+  orientation 0.6719791180295076 -0.49317350564460494 -0.552470775935249 1.8510428047885563
+  position -2.5375047220358824 -0.9166711516828123 0.6010308394649483
+}
+TexturedBackground {
+  texture "stadium_dry"
+}
+TexturedBackgroundLight {
+  texture "stadium_dry"
+}
+Robot {
+  supervisor TRUE
+  controller "learning_motion"
+}