From 9ea9d672398e67e0738f1423eaf2d8c11ce7da62 Mon Sep 17 00:00:00 2001 From: dmkwis Date: Mon, 18 Apr 2022 00:14:18 +0200 Subject: [PATCH 01/10] initial commit --- example_csv.csv | 2 ++ neural_net_example.py | 27 ++++++++++++++++ neural_networks/__init__.py | 0 neural_networks/models.py | 31 ++++++++++++++++++ neural_networks/net_utils.py | 62 ++++++++++++++++++++++++++++++++++++ 5 files changed, 122 insertions(+) create mode 100644 example_csv.csv create mode 100644 neural_net_example.py create mode 100644 neural_networks/__init__.py create mode 100644 neural_networks/models.py create mode 100644 neural_networks/net_utils.py diff --git a/example_csv.csv b/example_csv.csv new file mode 100644 index 0000000..da813b6 --- /dev/null +++ b/example_csv.csv @@ -0,0 +1,2 @@ +1,2,3 +4,5,6 diff --git a/neural_net_example.py b/neural_net_example.py new file mode 100644 index 0000000..fd48481 --- /dev/null +++ b/neural_net_example.py @@ -0,0 +1,27 @@ +import neural_networks.models as models +import neural_networks.net_utils as utils +import torch + +net = models.MLP("example_MLP_network", [2, 10, 1]) + +dataset = utils.CSVDataset("example_csv.csv", [0,1], [2]) +dataloader = utils.create_dataloader(dataset, 2) + +print(dataset.data, dataloader) +for input, labels in dataloader: + print("input: ", input) + print("labels: ", labels) + print("predicted:", net(input)) + +optimizer = torch.optim.SGD(net.parameters(), lr=1, momentum=0.9) + +#using same dataloader on train and eval just for example +utils.train(net, dataloader, dataloader, optimizer, torch.nn.L1Loss(), 5, 2) + +for input, labels in dataloader: + print("input: ", input) + print("labels: ", labels) + print("predicted:", net(input)) + +#saving net +utils.pickle_net(net, ".", suffix="example_suffix") \ No newline at end of file diff --git a/neural_networks/__init__.py b/neural_networks/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/neural_networks/models.py b/neural_networks/models.py new file mode 100644 index 0000000..f7e23fa --- /dev/null +++ b/neural_networks/models.py @@ -0,0 +1,31 @@ +import torch.nn as nn +from typing import List +from abc import ABC + +class AbstractNet(ABC): + def __init__(self, *args, **kwargs): + if 'name' not in kwargs: + raise "Net's constructor needs 'name' argument" + self.name = kwargs['name'] + + def get_name(self) -> str: + return self.name + + +class MLP(nn.Module, AbstractNet): + def __init__(self, name: str, sizes: List[int]): + AbstractNet.__init__(self, name=name) + nn.Module.__init__(self) + self.mod_list = nn.ModuleList() + for i in range(len(sizes) - 1): + self.mod_list.append(nn.Linear(sizes[i], sizes[i + 1])) + if i + 1 != len(sizes): + self.mod_list.append(nn.ReLU()) + + def forward(self, x): + for module in self.mod_list: + x = module(x) + return x + + + diff --git a/neural_networks/net_utils.py b/neural_networks/net_utils.py new file mode 100644 index 0000000..075601a --- /dev/null +++ b/neural_networks/net_utils.py @@ -0,0 +1,62 @@ +from neural_networks.models import AbstractNet +import torch.nn as nn +import torch +import pickle +import time +from pathlib import Path +from torch.utils.data import Dataset, DataLoader +import numpy as np +from typing import List + + +class CSVDataset(Dataset): + #TODO: ustalic delimiter + def __init__(self, csv_path: Path, x_indicies: List[int], y_indicies: List[int], delimiter: str = ',') -> None: + super().__init__() + self.data = np.genfromtxt(csv_path, delimiter=delimiter) + self.x = torch.from_numpy(self.data[:, x_indicies]).float() + self.y = torch.from_numpy(self.data[:, y_indicies]).float() + self.size = self.data.shape[0] + + def __getitem__(self, idx): + return self.x[idx], self.y[idx] + + def __len__(self): + return self.size + + +def create_dataloader(dataset: Dataset, batch_size: int) -> DataLoader: + #TODO: shuffle=True? num_workes=>1? other args + return DataLoader(dataset, batch_size=batch_size) + +def train(net: nn.Module, dataloader_train, dataloader_test, optim, loss_function, epochs, eval_gap): + for epoch in range(1, epochs + 1): + if epoch % eval_gap == 0: + net.eval() + total_test_loss = 0 + for input, labels in dataloader_test: + out = net(input) + loss = loss_function(out, labels) + total_test_loss += torch.sum(loss).item() + print("AVG LOSS ON TEST SET:", total_test_loss/len(dataloader_test.dataset)) + + net.train() + total_train_loss = 0 + for input, labels in dataloader_train: + optim.zero_grad() + + out = net(input) + loss = loss_function(out, labels) + total_train_loss += torch.sum(loss).item() + loss.backward() + optim.step() + print("AVG LOSS ON TRAIN SET:", total_train_loss/len(dataloader_train.dataset)) + + + + +def pickle_net(net: AbstractNet, path: Path, suffix: str = ""): + path = Path(path) + filehandler = open(path / (net.get_name() + suffix + str(time.time())), "wb") + pickle.dump(net, filehandler) + filehandler.close() \ No newline at end of file From 81a8b0d2dddcf209da541adda7ebc7dc7b8c55b8 Mon Sep 17 00:00:00 2001 From: dmkwis Date: Mon, 18 Apr 2022 00:40:44 +0200 Subject: [PATCH 02/10] test commit --- example_csv.csv | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/example_csv.csv b/example_csv.csv index da813b6..8dc346a 100644 --- a/example_csv.csv +++ b/example_csv.csv @@ -1,2 +1,6 @@ 1,2,3 4,5,6 +1,1,1 +2,2,2 +3,3,3 +10,10,11 From ed2e2994d1def88ecae98117e0a2c2ce5233d5c5 Mon Sep 17 00:00:00 2001 From: matiutd888 <34310650+matiutd888@users.noreply.github.com> Date: Sat, 23 Apr 2022 16:19:33 +0200 Subject: [PATCH 03/10] Refactor benchmark (#52) --- benchmarkmain.py | 41 +++++++++++++++++++++++++------- position_prediction/benchmark.py | 6 ++--- 2 files changed, 34 insertions(+), 13 deletions(-) diff --git a/benchmarkmain.py b/benchmarkmain.py index c188467..cea9e0e 100644 --- a/benchmarkmain.py +++ b/benchmarkmain.py @@ -1,3 +1,6 @@ +import argparse +import os + from position_prediction.benchmark import Benchmark import pybullet as p @@ -6,23 +9,43 @@ from trackers.concurrent_ball_tracker import ConcurrentPredictingBallTracker from utils.environment import init_env_and_load_assets -ball_controller, ball, paddle, wind_controllers = init_env_and_load_assets(p) +parser = argparse.ArgumentParser() +DEFAULT_FILE_NAME = "polynomial_prediction_benchmark" +DEFAULT_N_PREDICT = 20 +DEFAULT_FETCH_TIME = 1 / 10 # 10 is the maximum number of camera outputs per second. + +parser.add_argument("file_name", default=DEFAULT_FILE_NAME, type=str) +parser.add_argument("-d", type=int, help="N_DELAYED parameter") +parser.add_argument( + "-p", type=int, help="N_PREDICT parameter", default=DEFAULT_N_PREDICT +) +parser.add_argument( + "-f", type=float, help="FETCH_TIME parameter", default=DEFAULT_FETCH_TIME +) +parser.add_argument("--delete", action="store_true") +args = parser.parse_args() + +N_DELAYED = args.d +N_PREDICT = args.p +ball_controller, ball, paddle, wind_controllers = init_env_and_load_assets(p) paddle.create_joint_controllers() -N_DELAYED = 0 -N_PREDICT = 1 predicter = PolynomialPredicter() tracker = ConcurrentPredictingBallTracker( - DelayedPybulletBall(ball, N_DELAYED), paddle, N_PREDICT, 0, predicter, 0.0001 + DelayedPybulletBall(ball, N_DELAYED), paddle, N_PREDICT, 0, predicter, 0.1 ) -FILE_NAME = "polynomial_prediction_benchmark" -file_name = FILE_NAME + ".csv" -plot_name = FILE_NAME + ".png" +file_name = args.file_name +csv_name = file_name + ".csv" +plot_name = file_name + ".png" -benchmark = Benchmark(tracker, ball, file_name, [(1.5, 1.5), (-1.5, -1.5)]) +benchmark = Benchmark(tracker, ball, csv_name, [(0.5, 0.5), (-0.5, -0.5), (-1, -1)]) benchmark.run_benchmark(p, paddle, 6) -benchmark.plot_linear(file_name, plot_name) +benchmark.plot_linear(csv_name, plot_name) + +if args.delete: + os.remove(csv_name) + os.remove(plot_name) diff --git a/position_prediction/benchmark.py b/position_prediction/benchmark.py index bf5a047..71a4589 100644 --- a/position_prediction/benchmark.py +++ b/position_prediction/benchmark.py @@ -1,15 +1,13 @@ import csv -import math import time from datetime import date import pandas import pybullet -from paddle.abc_paddle import ABCPaddle +from ball.pybullet_ball import PyBulletBall from paddle.paddle import Paddle from trackers.abstract_tracker import AbstractBallTracker -from ball.pybullet_ball import PyBulletBall from utils.environment import set_wind from utils.pid_performer import PidPerformer @@ -123,7 +121,7 @@ def plot_linear(csv_file_name: str, plot_file_name): data = pandas.read_csv(csv_file_name) fig, axs = plt.subplots(3) - fig.suptitle("Vertically stacked subplots") + fig.suptitle("Prediction errors") fig.tight_layout() axs[0].set_title("Error x") From 123084f9027a5deca3c794c7c8339076c6247707 Mon Sep 17 00:00:00 2001 From: matiutd888 <34310650+matiutd888@users.noreply.github.com> Date: Sun, 24 Apr 2022 01:41:57 +0200 Subject: [PATCH 04/10] Refactor predicter interface (#53) * Refactor predicter interface --- benchmarkmain.py | 4 +-- main_with_delayed_ball.py | 4 +-- position_prediction/abc_predicter.py | 8 +++-- .../polynomial_interpolation.py | 31 +++++++++++-------- trackers/concurrent_ball_tracker.py | 10 ++---- 5 files changed, 29 insertions(+), 28 deletions(-) diff --git a/benchmarkmain.py b/benchmarkmain.py index cea9e0e..b8f1a8c 100644 --- a/benchmarkmain.py +++ b/benchmarkmain.py @@ -32,9 +32,9 @@ paddle.create_joint_controllers() -predicter = PolynomialPredicter() +predicter = PolynomialPredicter(N_PREDICT) tracker = ConcurrentPredictingBallTracker( - DelayedPybulletBall(ball, N_DELAYED), paddle, N_PREDICT, 0, predicter, 0.1 + DelayedPybulletBall(ball, N_DELAYED), paddle, predicter, 0.1 ) file_name = args.file_name diff --git a/main_with_delayed_ball.py b/main_with_delayed_ball.py index 66d1600..b06d9d7 100644 --- a/main_with_delayed_ball.py +++ b/main_with_delayed_ball.py @@ -16,8 +16,8 @@ N_DELAYED = 10 N_PREDICT = 15 -predicter = PolynomialPredicter() -tracker = ConcurrentPredictingBallTracker(ball, paddle, N_PREDICT, 0, predicter, 0.05) +predicter = PolynomialPredicter(N_PREDICT) +tracker = ConcurrentPredictingBallTracker(ball, paddle, predicter, 0.05) pid_performer = PidPerformer(p, tracker, paddle) while True: diff --git a/position_prediction/abc_predicter.py b/position_prediction/abc_predicter.py index af3008b..027d265 100644 --- a/position_prediction/abc_predicter.py +++ b/position_prediction/abc_predicter.py @@ -4,7 +4,9 @@ class ABCPredicter(ABC): @abstractmethod - def predict_x_y( - self, positions: List[Tuple[float, float, float]], position_index: int - ) -> Tuple[float, float, float]: + def add_position(self, position: List[float]): + pass + + @abstractmethod + def next_position(self) -> List[float]: pass diff --git a/position_prediction/polynomial_interpolation.py b/position_prediction/polynomial_interpolation.py index e571da9..358dd66 100644 --- a/position_prediction/polynomial_interpolation.py +++ b/position_prediction/polynomial_interpolation.py @@ -1,3 +1,4 @@ +from collections import deque from typing import Tuple, List from scipy import interpolate @@ -7,22 +8,26 @@ class PolynomialPredicter(ABCPredicter): - def predict(self, positions: List[float], position_index: int) -> float: + def __init__(self, n_predict): + self.confirmed_positions = deque(maxlen=n_predict) + + def add_position(self, position: List[float]): + self.confirmed_positions.append(position) + + def next_position(self) -> List[float]: + a = list(zip(*self.confirmed_positions)) + x_positions, y_positions, z_positions = a[0], a[1], a[2] + return [ + self.predict(list(x_positions)), + self.predict(list(y_positions)), + self.predict(list(z_positions)), + ] + + def predict(self, positions: List[float]) -> float: n = len(positions) if n == 1: return positions[0] time_series = np.arange(n) f = interpolate.interp1d(time_series, positions, fill_value="extrapolate") - return f(n + position_index) - - def predict_x_y( - self, positions: List[Tuple[float, float, float]], position_index: int - ) -> Tuple[float, float, float]: - a = list(zip(*positions)) - x_positions, y_positions, z_positions = a[0], a[1], a[2] - return ( - self.predict(x_positions, position_index), - self.predict(y_positions, position_index), - self.predict(z_positions, position_index), - ) + return f(n) diff --git a/trackers/concurrent_ball_tracker.py b/trackers/concurrent_ball_tracker.py index 7d46dd7..aa9bb56 100644 --- a/trackers/concurrent_ball_tracker.py +++ b/trackers/concurrent_ball_tracker.py @@ -15,16 +15,12 @@ def __init__( self, ball: ABCBall, paddle: Paddle, - n_predict: int, - prediction_index: int, predicter: ABCPredicter, fetch_time: float, ): self.last_position = NonBlockingPutQueue(maxsize=1) self.ball = ball self.paddle = paddle - self.prediction_index = prediction_index - self.m_queue = deque(maxlen=n_predict) self.predicter = predicter self.fetch_time = fetch_time self.get_position_in_loop() @@ -61,11 +57,9 @@ def get_error_vector(self) -> List[float]: ball_pos: List[float] try: ball_pos = self.last_position.get_nowait() - self.m_queue.append(ball_pos) + self.predicter.add_position(ball_pos) except queue.Empty: - self.__last_predicted_pos = list( - self.predicter.predict_x_y(list(self.m_queue), self.prediction_index) - ) + self.__last_predicted_pos = self.predicter.next_position() ball_pos = self.__last_predicted_pos return self.__get_error_from_position(ball_pos) From b3093beab3e70fd1ae36377b7f7bc16cdc06dc1a Mon Sep 17 00:00:00 2001 From: matiutd888 <34310650+matiutd888@users.noreply.github.com> Date: Sun, 24 Apr 2022 16:15:08 +0200 Subject: [PATCH 05/10] Implement force controllers (#56) --- benchmarkmain.py | 8 +++++++- main_simulation.py | 15 +++++++++++++-- main_with_delayed_ball.py | 15 +++++++++++++-- main_with_pid.py | 15 +++++++++++++-- testing/main_with_robot.py | 15 +++++++++++++-- utils/environment.py | 36 ++++++++++++++++++++++++++++++++---- 6 files changed, 91 insertions(+), 13 deletions(-) diff --git a/benchmarkmain.py b/benchmarkmain.py index b8f1a8c..a448ad5 100644 --- a/benchmarkmain.py +++ b/benchmarkmain.py @@ -28,7 +28,13 @@ N_DELAYED = args.d N_PREDICT = args.p -ball_controller, ball, paddle, wind_controllers = init_env_and_load_assets(p) +( + ball_controller, + ball, + paddle, + wind_controllers, + force_controllers, +) = init_env_and_load_assets(p) paddle.create_joint_controllers() diff --git a/main_simulation.py b/main_simulation.py index 347443e..1aff199 100644 --- a/main_simulation.py +++ b/main_simulation.py @@ -5,7 +5,11 @@ import pybullet as p from trackers.ball_tracker import BallTracker -from utils.environment import init_env_and_load_assets, update_wind_controllers +from utils.environment import ( + init_env_and_load_assets, + update_wind_controllers, + update_force_controllers, +) from utils.pid_performer import PidPerformer @@ -22,7 +26,13 @@ def get_mode(): mode, pid_flag = get_mode() keyboard_mode = mode == "keyboard" -ball_controller, ball, paddle, wind_controllers = init_env_and_load_assets(p) +( + ball_controller, + ball, + paddle, + wind_controllers, + force_controllers, +) = init_env_and_load_assets(p) if keyboard_mode: # add rotation speed controller @@ -50,6 +60,7 @@ def get_mode(): ball_controller.throw_ball(paddle.get_center_position()) update_wind_controllers(p, *wind_controllers) + update_force_controllers(p, ball, *force_controllers) p.stepSimulation() diff --git a/main_with_delayed_ball.py b/main_with_delayed_ball.py index b06d9d7..fbe6ce6 100644 --- a/main_with_delayed_ball.py +++ b/main_with_delayed_ball.py @@ -6,10 +6,20 @@ from position_prediction.polynomial_interpolation import PolynomialPredicter from trackers.concurrent_ball_tracker import ConcurrentPredictingBallTracker from trackers.predicting_ball_tracker import PredictingBallTracker -from utils.environment import init_env_and_load_assets, update_wind_controllers +from utils.environment import ( + init_env_and_load_assets, + update_wind_controllers, + update_force_controllers, +) from utils.pid_performer import PidPerformer -ball_controller, ball, paddle, wind_controllers = init_env_and_load_assets(p) +( + ball_controller, + ball, + paddle, + wind_controllers, + force_controllers, +) = init_env_and_load_assets(p) paddle.create_joint_controllers() @@ -32,6 +42,7 @@ ball_controller.throw_ball(paddle.get_center_position()) update_wind_controllers(p, *wind_controllers) + update_force_controllers(p, ball, *force_controllers) p.stepSimulation() diff --git a/main_with_pid.py b/main_with_pid.py index 9a63069..d30c04e 100644 --- a/main_with_pid.py +++ b/main_with_pid.py @@ -3,10 +3,20 @@ import pybullet as p from trackers.ball_tracker import BallTracker -from utils.environment import init_env_and_load_assets, update_wind_controllers +from utils.environment import ( + init_env_and_load_assets, + update_wind_controllers, + update_force_controllers, +) from utils.pid_performer import PidPerformer -ball_controller, ball, paddle, wind_controllers = init_env_and_load_assets(p) +( + ball_controller, + ball, + paddle, + wind_controllers, + force_controllers, +) = init_env_and_load_assets(p) paddle.create_joint_controllers() pid_performer = PidPerformer(p, BallTracker(ball, paddle), paddle) @@ -23,6 +33,7 @@ ball_controller.throw_ball(paddle.get_center_position()) update_wind_controllers(p, *wind_controllers) + update_force_controllers(p, ball, *force_controllers) p.stepSimulation() diff --git a/testing/main_with_robot.py b/testing/main_with_robot.py index 38954f1..47e4b18 100644 --- a/testing/main_with_robot.py +++ b/testing/main_with_robot.py @@ -4,7 +4,11 @@ import pybullet as p -from utils.environment import init_env_and_load_assets, update_wind_controllers +from utils.environment import ( + init_env_and_load_assets, + update_wind_controllers, + update_force_controllers, +) from utils.pid_performer import PidPerformer from robot_interactions.robot_paddle import RobotPaddle from paddle.bi_paddle import BiPaddle @@ -23,7 +27,13 @@ def get_mode(): mode, pid_flag = get_mode() keyboard_mode = mode == "keyboard" -ball_controller, ball, paddle, wind_controllers = init_env_and_load_assets(p) +( + ball_controller, + ball, + paddle, + wind_controllers, + force_controllers, +) = init_env_and_load_assets(p) if keyboard_mode: # add rotation speed controller @@ -57,6 +67,7 @@ def get_mode(): ball_controller.throw_ball(paddle.get_center_position()) update_wind_controllers(p, *wind_controllers) + update_force_controllers(p, ball, *force_controllers) p.stepSimulation() diff --git a/utils/environment.py b/utils/environment.py index dba8240..e4aa46f 100644 --- a/utils/environment.py +++ b/utils/environment.py @@ -25,12 +25,25 @@ def init_environment(p): def init_wind_controllers(p): - wind_x_controller = p.addUserDebugParameter("Adjust the wind (x - axis)", -G, G, 0) - wind_y_controller = p.addUserDebugParameter("Adjust the wind (y - axis)", -G, G, 0) + wind_x_controller = p.addUserDebugParameter("Adjust the wind (x axis)", -G, G, 0) + wind_y_controller = p.addUserDebugParameter("Adjust the wind (y axis)", -G, G, 0) return wind_x_controller, wind_y_controller +def init_force_controllers(p): + force_x_controller = p.addUserDebugParameter("force (x axis)", -1, 1, 0) + force_y_controller = p.addUserDebugParameter("force (y acis)", -1, 1, 0) + b = Button(p.addUserDebugParameter("Apply the force", 1, 0, 0)) + return b, force_x_controller, force_y_controller + + +def apply_force(p: pybullet, ball, force_x_controller, force_y_controller): + force_x = p.readUserDebugParameter(force_x_controller) + force_y = p.readUserDebugParameter(force_y_controller) + p.applyExternalForce(ball, -1, [force_x, force_y, 0], [0, 0, 0], p.WORLD_FRAME) + + def set_wind(p, wind_x_value, wind_y_value): p.setGravity(wind_x_value, wind_y_value, -G) @@ -42,6 +55,13 @@ def update_wind_controllers(p, wind_x_controller, wind_y_controller): set_wind(p, wind_x, wind_y) +def update_force_controllers( + p, ball: PyBulletBall, button: Button, force_x_controller, force_y_controller +): + if button.was_clicked(): + apply_force(p, ball.id, force_x_controller, force_y_controller) + + def load_plane(p): p.setAdditionalSearchPath(pybullet_data.getDataPath()) @@ -97,11 +117,19 @@ def init_standard_pid_tools( def init_env_and_load_assets( p, -) -> Tuple[PyBulletBallController, PyBulletBall, Paddle, Tuple[int, int]]: +) -> Tuple[ + PyBulletBallController, + PyBulletBall, + Paddle, + Tuple[int, int], + Tuple[Button, int, int], +]: init_environment(p) wind_controllers = init_wind_controllers(p) + force_controllers = init_force_controllers(p) + load_plane(p) ball = PyBulletBall(p) ball_controller = PyBulletBallController(ball) paddle = load_paddle(p) - return ball_controller, ball, paddle, wind_controllers + return ball_controller, ball, paddle, wind_controllers, force_controllers From 13b1bef11b87dba4375d76a682ae7b03e5d70fb3 Mon Sep 17 00:00:00 2001 From: dmkwis Date: Mon, 2 May 2022 20:21:54 +0200 Subject: [PATCH 06/10] data_gatherer_v1 --- ball/pybullet_ball.py | 18 +++++++- data_gatherer.py | 97 +++++++++++++++++++++++++++++++++++++++++++ paddle/paddle.py | 4 ++ utils/environment.py | 11 +++++ 4 files changed, 129 insertions(+), 1 deletion(-) create mode 100644 data_gatherer.py diff --git a/ball/pybullet_ball.py b/ball/pybullet_ball.py index 4bf586d..f57376d 100644 --- a/ball/pybullet_ball.py +++ b/ball/pybullet_ball.py @@ -1,4 +1,4 @@ -from typing import List +from typing import List, Tuple from ball.abc_ball import ABCBall @@ -35,6 +35,15 @@ def set_ball_angular_velocity(self, angular_velocity): self.pybullet_client.resetBaseVelocity( self.id, angularVelocity=angular_velocity ) + + def set_ball_velocity(self, linear_velocity, angular_velocity): + self.pybullet_client.resetBaseVelocity( + self.id, linearVelocity=linear_velocity, + angularVelocity=angular_velocity + ) + + def stabilize_ball(self): + self.set_ball_velocity([0, 0, 0], [0, 0, 0]) def set_position(self, position, orientation): self.pybullet_client.resetBasePositionAndOrientation( @@ -43,3 +52,10 @@ def set_position(self, position, orientation): def get_position(self) -> List[float]: return self.pybullet_client.getBasePositionAndOrientation(self.id)[0] + + def get_orientation(self) -> List[float]: + return self.pybullet_client.getBasePositionAndOrientation(self.id)[1] + + def get_velocity(self) -> Tuple[List[float], List[float]]: + linear_velocity, angular_velocity = self.pybullet_client.getBaseVelocity(self.id) + return linear_velocity, angular_velocity \ No newline at end of file diff --git a/data_gatherer.py b/data_gatherer.py new file mode 100644 index 0000000..691aab3 --- /dev/null +++ b/data_gatherer.py @@ -0,0 +1,97 @@ +#!/usr/bin/env python3 +import argparse +import time + + +import pybullet as p +import pandas as pd +import numpy as np + + +from trackers.ball_tracker import BallTracker +from utils.environment import init_minimalistic_env_and_load_assets +from utils.pid_performer import PidPerformer + +INITIAL_WAIT_TIME = 3.0 +DEFAULT_FETCH_TIME = 0.1 +RANDOM_EVENT_TIME = 0.2 + +parser = argparse.ArgumentParser(description="Mode manager") +parser.add_argument("--pid", action="store_true") +parser.add_argument("--filename", action="store", type=str, default="gathered_data.csv") +parser.add_argument("--fetch_time", action="store", type=float, default=DEFAULT_FETCH_TIME) +parser.add_argument("--initial_wait_time", action="store", type=float, default=INITIAL_WAIT_TIME) +parser.add_argument("--random_event_time", action="store", type=float, default=RANDOM_EVENT_TIME) + +args = parser.parse_args() +filename = args.filename +pid_flag = args.pid +fetch_time = args.fetch_time +initial_wait_time = args.initial_wait_time +random_event_time = args.random_event_time + +( + ball, + paddle +) = init_minimalistic_env_and_load_assets(p) + + +paddle.create_joint_controllers() + +if pid_flag: + pid_performer = PidPerformer(p, BallTracker(ball, paddle), paddle) + +initial_wait_timer = time.time() +fetch_timer = time.time() +random_event_timer = time.time() + +def distance_from_paddle_center(pos): + return np.linalg.norm(np.asarray(pos) - np.asarray([0.0, 0.0, 0.5]), ord=2) + +def vector_norm(vec): + return np.linalg.norm(np.asarray(vec), ord=2) + +trial_id = 1 +df = pd.DataFrame(columns=["trial_id", + "pos_x", "pos_y", "pos_z", + "lin_x", "lin_y", "lin_z", + "ang_x", "ang_y", "ang_z", + "y_roll", "x_roll" + ]) + +while True: + paddle.read_and_update_joint_position() + if time.time() - initial_wait_timer >= initial_wait_time: + if time.time() - fetch_timer >= fetch_time: + linear_velocity, angular_velocity = ball.get_velocity() + position = ball.get_position() + _, y_joint_state, x_joint_state = paddle.get_joint_rolls() + #rolls are at index 0 in those tuples - see pybullet documentation for further details + y_roll, x_roll = y_joint_state[0], x_joint_state[0] + + df.loc[len(df)] = [trial_id, *position, *linear_velocity, *angular_velocity, y_roll, x_roll] + fetch_timer = time.time() + + if pid_flag: + pid_performer.perform_pid_step() + + if time.time() - random_event_timer >= random_event_time: + linear_velocity, angular_velocity = ball.get_velocity() + if vector_norm(linear_velocity) + vector_norm(angular_velocity) < 0.5 and distance_from_paddle_center(ball.get_position()) < 0.2: + random_angular_velocity = np.random.uniform(low=-0.3, high=0.3, size=3).tolist() + random_linear_velocity = np.random.uniform(low=-0.7, high=0.7, size=3).tolist() + ball.set_ball_velocity(random_linear_velocity, random_angular_velocity) + else: + print("NIE") + random_event_timer = time.time() + + if distance_from_paddle_center(ball.get_position()) > 0.4: + ball.stabilize_ball() + ball.set_position([0, 0, 0.5 + 0.1], ball.get_orientation()) + #setting new trial id + trial_id += 1 + #delaying random event occurence + random_event_timer = time.time() + p.stepSimulation() + + time.sleep(0.01) # sometimes pybullet crashes, this line helps a lot diff --git a/paddle/paddle.py b/paddle/paddle.py index ccf7a34..352498a 100644 --- a/paddle/paddle.py +++ b/paddle/paddle.py @@ -52,6 +52,10 @@ def create_joint_controllers(self): self.pybullet_client.addUserDebugParameter("x_roll", -3.14, 3.14, 0) ) + def get_joint_rolls(self): + #(jointPosition, jointVelocity, jointReactionForces, appliedJointMotorTorque) + return tuple(self.pybullet_client.getJointState(self.robot_id, i) for i in [3, 4, 5]) + def read_and_update_joint_position(self): for i in range(len(self.joint_controllers)): self.pybullet_client.setJointMotorControl2( diff --git a/utils/environment.py b/utils/environment.py index e4aa46f..1b57fcf 100644 --- a/utils/environment.py +++ b/utils/environment.py @@ -133,3 +133,14 @@ def init_env_and_load_assets( ball_controller = PyBulletBallController(ball) paddle = load_paddle(p) return ball_controller, ball, paddle, wind_controllers, force_controllers + +def init_minimalistic_env_and_load_assets(p) -> Tuple[ + PyBulletBall, + Paddle, +]: + init_environment(p) + load_plane(p) + ball = PyBulletBall(p) + paddle = load_paddle(p) + + return ball, paddle From adee2ee5812b057c960c01ca26943be1d810b278 Mon Sep 17 00:00:00 2001 From: dmkwis Date: Mon, 2 May 2022 20:28:57 +0200 Subject: [PATCH 07/10] fix data_gatherer.py --- data_gatherer.py | 15 +++++++++++---- 1 file changed, 11 insertions(+), 4 deletions(-) diff --git a/data_gatherer.py b/data_gatherer.py index 691aab3..a035c9e 100644 --- a/data_gatherer.py +++ b/data_gatherer.py @@ -15,6 +15,8 @@ INITIAL_WAIT_TIME = 3.0 DEFAULT_FETCH_TIME = 0.1 RANDOM_EVENT_TIME = 0.2 +DEFAULT_TRIAL_NUM = 5 + parser = argparse.ArgumentParser(description="Mode manager") parser.add_argument("--pid", action="store_true") @@ -22,13 +24,14 @@ parser.add_argument("--fetch_time", action="store", type=float, default=DEFAULT_FETCH_TIME) parser.add_argument("--initial_wait_time", action="store", type=float, default=INITIAL_WAIT_TIME) parser.add_argument("--random_event_time", action="store", type=float, default=RANDOM_EVENT_TIME) - +parser.add_argument("--number_of_trials", action="store", type=int, default=DEFAULT_TRIAL_NUM) args = parser.parse_args() filename = args.filename pid_flag = args.pid fetch_time = args.fetch_time initial_wait_time = args.initial_wait_time random_event_time = args.random_event_time +trial_num = args.number_of_trials ( ball, @@ -51,7 +54,7 @@ def distance_from_paddle_center(pos): def vector_norm(vec): return np.linalg.norm(np.asarray(vec), ord=2) -trial_id = 1 +trial_id = 0 df = pd.DataFrame(columns=["trial_id", "pos_x", "pos_y", "pos_z", "lin_x", "lin_y", "lin_z", @@ -81,8 +84,7 @@ def vector_norm(vec): random_angular_velocity = np.random.uniform(low=-0.3, high=0.3, size=3).tolist() random_linear_velocity = np.random.uniform(low=-0.7, high=0.7, size=3).tolist() ball.set_ball_velocity(random_linear_velocity, random_angular_velocity) - else: - print("NIE") + random_event_timer = time.time() if distance_from_paddle_center(ball.get_position()) > 0.4: @@ -90,8 +92,13 @@ def vector_norm(vec): ball.set_position([0, 0, 0.5 + 0.1], ball.get_orientation()) #setting new trial id trial_id += 1 + if trial_id == trial_num: + break #delaying random event occurence random_event_timer = time.time() p.stepSimulation() time.sleep(0.01) # sometimes pybullet crashes, this line helps a lot + +df.to_csv(filename, sep=',') +print("DATA GATHERED TO:", filename) \ No newline at end of file From a5bb641e400b0bd0d607ed7f39c6632c2d0fd1a3 Mon Sep 17 00:00:00 2001 From: dmkwis Date: Mon, 2 May 2022 20:30:36 +0200 Subject: [PATCH 08/10] linear regression prediction --- position_prediction/linear_regression.py | 35 ++++++++++++++++++++++++ 1 file changed, 35 insertions(+) create mode 100644 position_prediction/linear_regression.py diff --git a/position_prediction/linear_regression.py b/position_prediction/linear_regression.py new file mode 100644 index 0000000..3850ede --- /dev/null +++ b/position_prediction/linear_regression.py @@ -0,0 +1,35 @@ +from collections import deque +from turtle import pos +from typing import Tuple, List + +from scipy import stats +import numpy as np + +from position_prediction.abc_predicter import ABCPredicter + + +class LinearRegressionPredicter(ABCPredicter): + def __init__(self, n_predict): + self.confirmed_positions = deque(maxlen=n_predict) + + def add_position(self, position: List[float]): + self.confirmed_positions.append(position) + + def next_position(self) -> List[float]: + a = list(zip(*self.confirmed_positions)) + x_positions, y_positions, z_positions = a[0], a[1], a[2] + #print("ZWRACAM", self.predict(list(x_positions))) + return [ + self.predict(list(x_positions)), + self.predict(list(y_positions)), + self.predict(list(z_positions)), + ] + + def predict(self, positions: List[float]) -> float: + n = len(positions) + if n == 1: + return positions[0] + + time_series = np.arange(n) + res = stats.linregress(time_series, positions) + return res.slope * n + res.intercept From 4e14529e5218c076e92d75aaa2647a27f860bd19 Mon Sep 17 00:00:00 2001 From: dmkwis Date: Mon, 2 May 2022 20:44:58 +0200 Subject: [PATCH 09/10] linter --- ball/pybullet_ball.py | 15 ++--- data_gatherer.py | 80 +++++++++++++++++------- neural_net_example.py | 8 +-- neural_networks/models.py | 12 ++-- neural_networks/net_utils.py | 37 ++++++++--- paddle/paddle.py | 6 +- position_prediction/linear_regression.py | 2 +- utils/environment.py | 10 +-- 8 files changed, 110 insertions(+), 60 deletions(-) diff --git a/ball/pybullet_ball.py b/ball/pybullet_ball.py index f57376d..08bb211 100644 --- a/ball/pybullet_ball.py +++ b/ball/pybullet_ball.py @@ -35,13 +35,12 @@ def set_ball_angular_velocity(self, angular_velocity): self.pybullet_client.resetBaseVelocity( self.id, angularVelocity=angular_velocity ) - + def set_ball_velocity(self, linear_velocity, angular_velocity): self.pybullet_client.resetBaseVelocity( - self.id, linearVelocity=linear_velocity, - angularVelocity=angular_velocity + self.id, linearVelocity=linear_velocity, angularVelocity=angular_velocity ) - + def stabilize_ball(self): self.set_ball_velocity([0, 0, 0], [0, 0, 0]) @@ -55,7 +54,9 @@ def get_position(self) -> List[float]: def get_orientation(self) -> List[float]: return self.pybullet_client.getBasePositionAndOrientation(self.id)[1] - + def get_velocity(self) -> Tuple[List[float], List[float]]: - linear_velocity, angular_velocity = self.pybullet_client.getBaseVelocity(self.id) - return linear_velocity, angular_velocity \ No newline at end of file + linear_velocity, angular_velocity = self.pybullet_client.getBaseVelocity( + self.id + ) + return linear_velocity, angular_velocity diff --git a/data_gatherer.py b/data_gatherer.py index a035c9e..27023ab 100644 --- a/data_gatherer.py +++ b/data_gatherer.py @@ -21,10 +21,18 @@ parser = argparse.ArgumentParser(description="Mode manager") parser.add_argument("--pid", action="store_true") parser.add_argument("--filename", action="store", type=str, default="gathered_data.csv") -parser.add_argument("--fetch_time", action="store", type=float, default=DEFAULT_FETCH_TIME) -parser.add_argument("--initial_wait_time", action="store", type=float, default=INITIAL_WAIT_TIME) -parser.add_argument("--random_event_time", action="store", type=float, default=RANDOM_EVENT_TIME) -parser.add_argument("--number_of_trials", action="store", type=int, default=DEFAULT_TRIAL_NUM) +parser.add_argument( + "--fetch_time", action="store", type=float, default=DEFAULT_FETCH_TIME +) +parser.add_argument( + "--initial_wait_time", action="store", type=float, default=INITIAL_WAIT_TIME +) +parser.add_argument( + "--random_event_time", action="store", type=float, default=RANDOM_EVENT_TIME +) +parser.add_argument( + "--number_of_trials", action="store", type=int, default=DEFAULT_TRIAL_NUM +) args = parser.parse_args() filename = args.filename pid_flag = args.pid @@ -33,10 +41,7 @@ random_event_time = args.random_event_time trial_num = args.number_of_trials -( - ball, - paddle -) = init_minimalistic_env_and_load_assets(p) +(ball, paddle) = init_minimalistic_env_and_load_assets(p) paddle.create_joint_controllers() @@ -48,19 +53,32 @@ fetch_timer = time.time() random_event_timer = time.time() + def distance_from_paddle_center(pos): return np.linalg.norm(np.asarray(pos) - np.asarray([0.0, 0.0, 0.5]), ord=2) + def vector_norm(vec): return np.linalg.norm(np.asarray(vec), ord=2) + trial_id = 0 -df = pd.DataFrame(columns=["trial_id", - "pos_x", "pos_y", "pos_z", - "lin_x", "lin_y", "lin_z", - "ang_x", "ang_y", "ang_z", - "y_roll", "x_roll" - ]) +df = pd.DataFrame( + columns=[ + "trial_id", + "pos_x", + "pos_y", + "pos_z", + "lin_x", + "lin_y", + "lin_z", + "ang_x", + "ang_y", + "ang_z", + "y_roll", + "x_roll", + ] +) while True: paddle.read_and_update_joint_position() @@ -69,10 +87,17 @@ def vector_norm(vec): linear_velocity, angular_velocity = ball.get_velocity() position = ball.get_position() _, y_joint_state, x_joint_state = paddle.get_joint_rolls() - #rolls are at index 0 in those tuples - see pybullet documentation for further details + # rolls are at index 0 in those tuples - see pybullet documentation for further details y_roll, x_roll = y_joint_state[0], x_joint_state[0] - df.loc[len(df)] = [trial_id, *position, *linear_velocity, *angular_velocity, y_roll, x_roll] + df.loc[len(df)] = [ + trial_id, + *position, + *linear_velocity, + *angular_velocity, + y_roll, + x_roll, + ] fetch_timer = time.time() if pid_flag: @@ -80,25 +105,32 @@ def vector_norm(vec): if time.time() - random_event_timer >= random_event_time: linear_velocity, angular_velocity = ball.get_velocity() - if vector_norm(linear_velocity) + vector_norm(angular_velocity) < 0.5 and distance_from_paddle_center(ball.get_position()) < 0.2: - random_angular_velocity = np.random.uniform(low=-0.3, high=0.3, size=3).tolist() - random_linear_velocity = np.random.uniform(low=-0.7, high=0.7, size=3).tolist() + if ( + vector_norm(linear_velocity) + vector_norm(angular_velocity) < 0.5 + and distance_from_paddle_center(ball.get_position()) < 0.2 + ): + random_angular_velocity = np.random.uniform( + low=-0.3, high=0.3, size=3 + ).tolist() + random_linear_velocity = np.random.uniform( + low=-0.7, high=0.7, size=3 + ).tolist() ball.set_ball_velocity(random_linear_velocity, random_angular_velocity) random_event_timer = time.time() - + if distance_from_paddle_center(ball.get_position()) > 0.4: ball.stabilize_ball() ball.set_position([0, 0, 0.5 + 0.1], ball.get_orientation()) - #setting new trial id + # setting new trial id trial_id += 1 if trial_id == trial_num: break - #delaying random event occurence + # delaying random event occurence random_event_timer = time.time() p.stepSimulation() time.sleep(0.01) # sometimes pybullet crashes, this line helps a lot -df.to_csv(filename, sep=',') -print("DATA GATHERED TO:", filename) \ No newline at end of file +df.to_csv(filename, sep=",") +print("DATA GATHERED TO:", filename) diff --git a/neural_net_example.py b/neural_net_example.py index fd48481..164d67e 100644 --- a/neural_net_example.py +++ b/neural_net_example.py @@ -4,7 +4,7 @@ net = models.MLP("example_MLP_network", [2, 10, 1]) -dataset = utils.CSVDataset("example_csv.csv", [0,1], [2]) +dataset = utils.CSVDataset("example_csv.csv", [0, 1], [2]) dataloader = utils.create_dataloader(dataset, 2) print(dataset.data, dataloader) @@ -15,7 +15,7 @@ optimizer = torch.optim.SGD(net.parameters(), lr=1, momentum=0.9) -#using same dataloader on train and eval just for example +# using same dataloader on train and eval just for example utils.train(net, dataloader, dataloader, optimizer, torch.nn.L1Loss(), 5, 2) for input, labels in dataloader: @@ -23,5 +23,5 @@ print("labels: ", labels) print("predicted:", net(input)) -#saving net -utils.pickle_net(net, ".", suffix="example_suffix") \ No newline at end of file +# saving net +utils.pickle_net(net, ".", suffix="example_suffix") diff --git a/neural_networks/models.py b/neural_networks/models.py index f7e23fa..cab6659 100644 --- a/neural_networks/models.py +++ b/neural_networks/models.py @@ -2,17 +2,18 @@ from typing import List from abc import ABC + class AbstractNet(ABC): def __init__(self, *args, **kwargs): - if 'name' not in kwargs: + if "name" not in kwargs: raise "Net's constructor needs 'name' argument" - self.name = kwargs['name'] + self.name = kwargs["name"] def get_name(self) -> str: return self.name -class MLP(nn.Module, AbstractNet): +class MLP(nn.Module, AbstractNet): def __init__(self, name: str, sizes: List[int]): AbstractNet.__init__(self, name=name) nn.Module.__init__(self) @@ -21,11 +22,8 @@ def __init__(self, name: str, sizes: List[int]): self.mod_list.append(nn.Linear(sizes[i], sizes[i + 1])) if i + 1 != len(sizes): self.mod_list.append(nn.ReLU()) - + def forward(self, x): for module in self.mod_list: x = module(x) return x - - - diff --git a/neural_networks/net_utils.py b/neural_networks/net_utils.py index 075601a..6175107 100644 --- a/neural_networks/net_utils.py +++ b/neural_networks/net_utils.py @@ -10,14 +10,20 @@ class CSVDataset(Dataset): - #TODO: ustalic delimiter - def __init__(self, csv_path: Path, x_indicies: List[int], y_indicies: List[int], delimiter: str = ',') -> None: + # TODO: ustalic delimiter + def __init__( + self, + csv_path: Path, + x_indicies: List[int], + y_indicies: List[int], + delimiter: str = ",", + ) -> None: super().__init__() self.data = np.genfromtxt(csv_path, delimiter=delimiter) self.x = torch.from_numpy(self.data[:, x_indicies]).float() self.y = torch.from_numpy(self.data[:, y_indicies]).float() self.size = self.data.shape[0] - + def __getitem__(self, idx): return self.x[idx], self.y[idx] @@ -26,10 +32,19 @@ def __len__(self): def create_dataloader(dataset: Dataset, batch_size: int) -> DataLoader: - #TODO: shuffle=True? num_workes=>1? other args + # TODO: shuffle=True? num_workes=>1? other args return DataLoader(dataset, batch_size=batch_size) -def train(net: nn.Module, dataloader_train, dataloader_test, optim, loss_function, epochs, eval_gap): + +def train( + net: nn.Module, + dataloader_train, + dataloader_test, + optim, + loss_function, + epochs, + eval_gap, +): for epoch in range(1, epochs + 1): if epoch % eval_gap == 0: net.eval() @@ -38,7 +53,9 @@ def train(net: nn.Module, dataloader_train, dataloader_test, optim, loss_functio out = net(input) loss = loss_function(out, labels) total_test_loss += torch.sum(loss).item() - print("AVG LOSS ON TEST SET:", total_test_loss/len(dataloader_test.dataset)) + print( + "AVG LOSS ON TEST SET:", total_test_loss / len(dataloader_test.dataset) + ) net.train() total_train_loss = 0 @@ -50,13 +67,13 @@ def train(net: nn.Module, dataloader_train, dataloader_test, optim, loss_functio total_train_loss += torch.sum(loss).item() loss.backward() optim.step() - print("AVG LOSS ON TRAIN SET:", total_train_loss/len(dataloader_train.dataset)) - - + print( + "AVG LOSS ON TRAIN SET:", total_train_loss / len(dataloader_train.dataset) + ) def pickle_net(net: AbstractNet, path: Path, suffix: str = ""): path = Path(path) filehandler = open(path / (net.get_name() + suffix + str(time.time())), "wb") pickle.dump(net, filehandler) - filehandler.close() \ No newline at end of file + filehandler.close() diff --git a/paddle/paddle.py b/paddle/paddle.py index 352498a..e0e080e 100644 --- a/paddle/paddle.py +++ b/paddle/paddle.py @@ -53,8 +53,10 @@ def create_joint_controllers(self): ) def get_joint_rolls(self): - #(jointPosition, jointVelocity, jointReactionForces, appliedJointMotorTorque) - return tuple(self.pybullet_client.getJointState(self.robot_id, i) for i in [3, 4, 5]) + # (jointPosition, jointVelocity, jointReactionForces, appliedJointMotorTorque) + return tuple( + self.pybullet_client.getJointState(self.robot_id, i) for i in [3, 4, 5] + ) def read_and_update_joint_position(self): for i in range(len(self.joint_controllers)): diff --git a/position_prediction/linear_regression.py b/position_prediction/linear_regression.py index 3850ede..739f471 100644 --- a/position_prediction/linear_regression.py +++ b/position_prediction/linear_regression.py @@ -18,7 +18,7 @@ def add_position(self, position: List[float]): def next_position(self) -> List[float]: a = list(zip(*self.confirmed_positions)) x_positions, y_positions, z_positions = a[0], a[1], a[2] - #print("ZWRACAM", self.predict(list(x_positions))) + # print("ZWRACAM", self.predict(list(x_positions))) return [ self.predict(list(x_positions)), self.predict(list(y_positions)), diff --git a/utils/environment.py b/utils/environment.py index 1b57fcf..d2f493b 100644 --- a/utils/environment.py +++ b/utils/environment.py @@ -134,13 +134,13 @@ def init_env_and_load_assets( paddle = load_paddle(p) return ball_controller, ball, paddle, wind_controllers, force_controllers -def init_minimalistic_env_and_load_assets(p) -> Tuple[ - PyBulletBall, - Paddle, -]: + +def init_minimalistic_env_and_load_assets( + p, +) -> Tuple[PyBulletBall, Paddle,]: init_environment(p) load_plane(p) ball = PyBulletBall(p) paddle = load_paddle(p) - + return ball, paddle From 000fb75e6b5fb0fda53f653c524305902f391d46 Mon Sep 17 00:00:00 2001 From: dmkwis Date: Mon, 2 May 2022 22:16:09 +0200 Subject: [PATCH 10/10] merge conflicts --- benchmarkmain.py | 1 - utils/environment.py | 1 + 2 files changed, 1 insertion(+), 1 deletion(-) diff --git a/benchmarkmain.py b/benchmarkmain.py index 4fbe343..2700fc7 100644 --- a/benchmarkmain.py +++ b/benchmarkmain.py @@ -46,7 +46,6 @@ predicter = PolynomialPredicter(N_PREDICT) tracker = ConcurrentPredictingBallTracker( DelayedPybulletBall(ball, N_DELAYED), paddle, predicter, FETCH_TIME - ) file_name = args.file_name diff --git a/utils/environment.py b/utils/environment.py index 5041595..d2f493b 100644 --- a/utils/environment.py +++ b/utils/environment.py @@ -134,6 +134,7 @@ def init_env_and_load_assets( paddle = load_paddle(p) return ball_controller, ball, paddle, wind_controllers, force_controllers + def init_minimalistic_env_and_load_assets( p, ) -> Tuple[PyBulletBall, Paddle,]: