diff --git a/rktl_autonomy/YAMLEditor/.gitignore b/rktl_autonomy/YAMLEditor/.gitignore
new file mode 100644
index 000000000..c4194cfa6
--- /dev/null
+++ b/rktl_autonomy/YAMLEditor/.gitignore
@@ -0,0 +1,7 @@
+# Java
+bin/
+.classpath
+.settings
+
+# macOS metadata file
+.DS_Store
diff --git a/rktl_autonomy/YAMLEditor/.project b/rktl_autonomy/YAMLEditor/.project
new file mode 100644
index 000000000..94ee56c1e
--- /dev/null
+++ b/rktl_autonomy/YAMLEditor/.project
@@ -0,0 +1,17 @@
+
+
+ YAMLEditor
+
+
+
+
+
+ org.eclipse.jdt.core.javabuilder
+
+
+
+
+
+ org.eclipse.jdt.core.javanature
+
+
diff --git a/rktl_autonomy/YAMLEditor/README.md b/rktl_autonomy/YAMLEditor/README.md
new file mode 100644
index 000000000..c97d0314c
--- /dev/null
+++ b/rktl_autonomy/YAMLEditor/README.md
@@ -0,0 +1,7 @@
+# YAMLEditor
+
+## Unpack into a table, edit, and repack two YAML arrays
+
+Intended for use with reward values
+
+Built jar in `jars/`
diff --git a/rktl_autonomy/YAMLEditor/jars/YAMLEditor.jar b/rktl_autonomy/YAMLEditor/jars/YAMLEditor.jar
new file mode 100644
index 000000000..bb3ed6c7d
Binary files /dev/null and b/rktl_autonomy/YAMLEditor/jars/YAMLEditor.jar differ
diff --git a/rktl_autonomy/YAMLEditor/src/com/purduearc/yamleditor/Main.java b/rktl_autonomy/YAMLEditor/src/com/purduearc/yamleditor/Main.java
new file mode 100644
index 000000000..67809ae9a
--- /dev/null
+++ b/rktl_autonomy/YAMLEditor/src/com/purduearc/yamleditor/Main.java
@@ -0,0 +1,177 @@
+package com.purduearc.yamleditor;
+
+import java.awt.Rectangle;
+import java.awt.event.ActionEvent;
+import java.awt.event.ActionListener;
+import java.util.Arrays;
+import java.util.regex.Pattern;
+
+import javax.swing.JButton;
+import javax.swing.JFrame;
+import javax.swing.JLabel;
+import javax.swing.JScrollPane;
+import javax.swing.JTable;
+import javax.swing.JTextField;
+import javax.swing.event.TableModelEvent;
+import javax.swing.event.TableModelListener;
+import javax.swing.table.DefaultTableModel;
+/**
+ * Unpack into a table, edit, and repack two YAML arrays
+ *
+ * @author Campbell McClendon
+ *
+ * @version 1.0
+ */
+public class Main {
+ private static final Rectangle scrollPaneBounds = new Rectangle(2, 44, 496, 426);
+ private static final String[] titles = new String[] {"ID", "WIN", "LOSE"};
+ private static String[][] data = null;
+ public static void main(String[] args) {
+ JFrame frame = new JFrame("YAML Editor");
+ frame.setResizable(false);
+ frame.setSize(500, 500);
+
+ JTextField winInputField = new JTextField("[1, 2, 3, 4, 5]", 50);
+ winInputField.setBounds(2, 2, 100, 20);
+
+ JTextField loseInputField = new JTextField("[6, 7, 8, 9, 10]");
+ loseInputField.setBounds(104, 2, 100, 20);
+
+ JLabel inputLabel = new JLabel("Input");
+ inputLabel.setBounds(106, 24, 100, 20);
+
+ data = new String[][] {{"0", "1", "2"}, {"1", "3", "4"}};
+ DefaultTableModel tableModel = new DefaultTableModel(data, titles);
+ JTable table = new JTable(tableModel);
+ table.setFillsViewportHeight(true);
+ tableModel.addTableModelListener(new TableModelListener() {
+ @Override
+ public void tableChanged(TableModelEvent e) {
+ if (table.isEditing()) {
+ data[table.getSelectedRow()][table.getSelectedColumn()] = (String) table.getValueAt(table.getSelectedRow(), table.getSelectedColumn());
+ }
+ }
+ });
+
+ JLabel errorLabel = new JLabel("");
+ errorLabel.setBounds(225, 2, 100, 20);
+
+ JScrollPane scrollPane = new JScrollPane(table);
+ scrollPane.setBounds(scrollPaneBounds);
+
+ JTextField winOutputField = new JTextField(50);
+ winOutputField.setBounds(296, 2, 100, 20);
+ winOutputField.setEditable(false);
+
+ JTextField loseOutputField = new JTextField(50);
+ loseOutputField.setBounds(398, 2, 100, 20);
+ loseOutputField.setEditable(false);
+
+ JLabel outputLabel = new JLabel("Output");
+ outputLabel.setBounds(298, 24, 100, 20);
+
+ JButton unpackButton = new JButton("Unpack Arrays");
+ unpackButton.setBounds(2, 24, 100, 20);
+ unpackButton.addActionListener(new ActionListener() {
+ @Override
+ public void actionPerformed(ActionEvent e) {
+ try {
+ String col1Text = clip(winInputField.getText().replaceAll("\\s+",""));
+ String[] col1ByID = col1Text.split(Pattern.quote(","));
+ if (col1ByID.length < 2) {
+ System.err.println("Malformed win input data");
+ errorLabel.setText("ERROR");
+ return;
+ }
+ String col2Text = clip(loseInputField.getText().replaceAll("\\s+",""));
+ String[] col2ByID = col2Text.split(Pattern.quote(","));
+ if (col2ByID.length < 2) {
+ System.err.println("Malformed lose input data");
+ errorLabel.setText("ERROR");
+ return;
+ }
+ if (col1ByID.length != col2ByID.length) {
+ System.err.println("Win and lose arrays must be of same length");
+ errorLabel.setText("ERROR");
+ return;
+ }
+ String[][] newData = new String[col1ByID.length][2];
+ for (int i = 0; i < col1ByID.length; i++) {
+ newData[i][0] = col1ByID[i];
+ newData[i][1] = col2ByID[i];
+ }
+ String[][] dataWithID = new String[newData.length][newData[0].length + 1];
+ for (int id = 0; id < dataWithID.length; id++) {
+ dataWithID[id][0] = "" + id;
+ for (int i = 1; i < dataWithID[id].length; i++) {
+ dataWithID[id][i] = newData[id][i - 1];
+ }
+ }
+ data = dataWithID;
+ tableModel.setDataVector(dataWithID, titles);
+ errorLabel.setText("");
+ } catch (Exception exception) {
+ errorLabel.setText("ERROR");
+ exception.printStackTrace();
+ }
+ }
+ });
+
+ JButton repackButton = new JButton("Repack Arrays");
+ repackButton.setBounds(398, 24, 100, 20);
+ repackButton.addActionListener(new ActionListener() {
+ @Override
+ public void actionPerformed(ActionEvent e) {
+ try {
+ String[][] dataWithoutID = new String[data.length][];
+ for (int x = 0; x < data.length; x++) {
+ String[] line = new String[data[x].length - 1];
+ for (int y = 1; y < data[x].length; y++) {
+ line[y - 1] = data[x][y];
+ }
+ dataWithoutID[x] = line;
+ }
+ String[] winValues = new String[dataWithoutID.length];
+ String[] loseValues = new String[dataWithoutID.length];
+ for (int i = 0; i < dataWithoutID.length; i++) {
+ winValues[i] = dataWithoutID[i][0];
+ loseValues[i] = dataWithoutID[i][1];
+ }
+ winOutputField.setText(Arrays.deepToString(winValues).replaceAll("\\s+",""));
+ loseOutputField.setText(Arrays.deepToString(loseValues).replaceAll("\\s+",""));
+ } catch (Exception exception) {
+ errorLabel.setText("ERROR");
+ exception.printStackTrace();
+ }
+ }
+ });
+
+ frame.add(winInputField);
+ frame.add(loseInputField);
+ frame.add(inputLabel);
+ frame.add(unpackButton);
+ frame.add(errorLabel);
+ frame.add(winOutputField);
+ frame.add(loseOutputField);
+ frame.add(outputLabel);
+ frame.add(repackButton);
+ frame.add(scrollPane);
+
+ frame.setLayout(null);
+ frame.setDefaultCloseOperation(JFrame.EXIT_ON_CLOSE);
+ frame.setVisible(true);
+ }
+ /**
+ * Clips the first and last characters off of a string
+ *
+ * @param str input string
+ * @return str without first and last characters
+ */
+ private static String clip(String str) {
+ if (str.charAt(0) == (char) '[') {
+ if (str.charAt(str.length() - 1) == (char) ']') return str.substring(1, str.length() - 1);
+ else return str.substring(1, str.length());
+ } else if (str.charAt(str.length() - 1) == (char) ']') return str.substring(0, str.length() - 1);
+ else return str;
+ }
+}
diff --git a/rktl_autonomy/config/rocket_league.yaml b/rktl_autonomy/config/rocket_league.yaml
index a040e580c..7f6cb85f9 100644
--- a/rktl_autonomy/config/rocket_league.yaml
+++ b/rktl_autonomy/config/rocket_league.yaml
@@ -8,9 +8,9 @@ reward:
# reward given when car changes velocity direction
direction_change: 0
# reward given when the episode ends with the car scoring the proper goal
- win: 100
+ win: [100, 200, 300]
# reward given when the episode ends with the car scoring on the wrong goal
- loss: 100
+ loss: [100, 100, 100]
# reward given each frame when the car is in reverse
# reverse: -25
# # reward given each frame when the car is near the walls
diff --git a/rktl_autonomy/scripts/modular_train.py b/rktl_autonomy/scripts/modular_train.py
new file mode 100755
index 000000000..9510d99bc
--- /dev/null
+++ b/rktl_autonomy/scripts/modular_train.py
@@ -0,0 +1,25 @@
+#!/usr/bin/env python3
+
+from train_rocket_league import train
+import yaml
+import sys
+import os
+from multiprocessing import Process
+
+if __name__ == '__main__':
+ numEnvsAllowed = 24
+
+ if len(sys.argv) == 2:
+ numEnvsAllowed = int(sys.argv[1])
+
+ configFile = os.path.join(os.pardir, 'config', 'rocket_league.yaml')
+ print(os.path.abspath(configFile))
+
+ file = yaml.load(open(configFile), Loader=yaml.FullLoader)
+ numGroups = len(file["reward"]["win"])
+
+ for i in range(numGroups):
+ args = (int(numEnvsAllowed / numGroups), 100, 240000000, i)
+ p = Process(target=train, args=args)
+ p.start()
+ print(f'Starting thread {i}/{numGroups}')
diff --git a/rktl_autonomy/scripts/train_rocket_league.py b/rktl_autonomy/scripts/train_rocket_league.py
index 7e0a30802..b8869c442 100755
--- a/rktl_autonomy/scripts/train_rocket_league.py
+++ b/rktl_autonomy/scripts/train_rocket_league.py
@@ -15,13 +15,13 @@
from os.path import expanduser
import uuid
-if __name__ == '__main__': # this is required due to forking processes
+
+def train(n_envs=24, n_saves=100, n_steps=240000000, env_number=0):
run_id = str(uuid.uuid4()) # ALL running environments must share this
print(f"RUN ID: {run_id}")
-
# to pass launch args, add to env_kwargs: 'launch_args': ['render:=false', 'plot_log:=true']
- env = make_vec_env(RocketLeagueInterface, env_kwargs={'run_id':run_id},
- n_envs=24, vec_env_cls=SubprocVecEnv)
+ env = make_vec_env(RocketLeagueInterface, env_kwargs={'run_id': run_id}, wrapper_kwargs = {'env_number': env_number, 'run_id': run_id},
+ n_envs=n_envs, vec_env_cls=SubprocVecEnv)
model = PPO("MlpPolicy", env)
@@ -31,23 +31,26 @@
model.set_parameters(previous_weights)
except:
print('Failed to load from previous weights')
-
+
# log training progress as CSV
log_dir = expanduser(f'~/catkin_ws/data/rocket_league/{run_id}')
logger = configure(log_dir, ["stdout", "csv", "log"])
model.set_logger(logger)
# log model weights
- freq = 20833 # save 20 times
- # freq = steps / (n_saves * n_envs)
+ freq = n_steps / (n_saves * n_envs)
callback = CheckpointCallback(save_freq=freq, save_path=log_dir)
# run training
- steps = 240000000 # 240M (10M sequential)
+ steps = n_steps
print(f"training on {steps} steps")
model.learn(total_timesteps=steps, callback=callback)
# save final weights
print("done training")
model.save(log_dir + "/final_weights")
- env.close() # this must be done to clean up other processes
+ env.close() # this must be done to clean up other processes
+
+
+if __name__ == '__main__':
+ train()
diff --git a/rktl_autonomy/src/rktl_autonomy/__init__.py b/rktl_autonomy/src/rktl_autonomy/__init__.py
index 601ecf6ee..e91dad629 100644
--- a/rktl_autonomy/src/rktl_autonomy/__init__.py
+++ b/rktl_autonomy/src/rktl_autonomy/__init__.py
@@ -19,10 +19,12 @@
from .cartpole_direct_interface import CartpoleDirectInterface
from .snake_interface import SnakeInterface
from .rocket_league_interface import RocketLeagueInterface
+from .env_counter import EnvCounter
__all__ = [
"ROSInterface",
"CartpoleInterface",
"CartpoleDirectInterface",
"SnakeInterface",
- "RocketLeagueInterface"]
+ "RocketLeagueInterface",
+ "EnvCounter"]
\ No newline at end of file
diff --git a/rktl_autonomy/src/rktl_autonomy/env_counter.py b/rktl_autonomy/src/rktl_autonomy/env_counter.py
new file mode 100644
index 000000000..c77dba7d7
--- /dev/null
+++ b/rktl_autonomy/src/rktl_autonomy/env_counter.py
@@ -0,0 +1,9 @@
+class EnvCounter:
+ def __init__(self):
+ self.counter = 0
+
+ def count_env(self):
+ self.counter += 1
+
+ def get_current_counter(self):
+ return self.counter
diff --git a/rktl_autonomy/src/rktl_autonomy/rocket_league_interface.py b/rktl_autonomy/src/rktl_autonomy/rocket_league_interface.py
index 3b50518b5..e8b343570 100755
--- a/rktl_autonomy/src/rktl_autonomy/rocket_league_interface.py
+++ b/rktl_autonomy/src/rktl_autonomy/rocket_league_interface.py
@@ -40,10 +40,12 @@ class CarActions(IntEnum):
class RocketLeagueInterface(ROSInterface):
"""ROS interface for the Rocket League."""
- def __init__(self, eval=False, launch_file=['rktl_autonomy', 'rocket_league_train.launch'], launch_args=[], run_id=None):
- super().__init__(node_name='rocket_league_agent', eval=eval, launch_file=launch_file, launch_args=launch_args, run_id=run_id)
-
- ## Constants
+ def __init__(self, eval=False, launch_file=('rktl_autonomy', 'rocket_league_train.launch'), launch_args=[],
+ run_id=None, env_number=0):
+ super().__init__(node_name='rocket_league_agent', eval=eval, launch_file=launch_file, launch_args=launch_args,
+ run_id=run_id)
+ # Constants
+ self.env_number = env_number
# Actions
self._MIN_VELOCITY = -rospy.get_param('/cars/throttle/max_speed')
self._MAX_VELOCITY = rospy.get_param('/cars/throttle/max_speed')
@@ -81,8 +83,20 @@ def __init__(self, eval=False, launch_file=['rktl_autonomy', 'rocket_league_trai
self._BALL_DISTANCE_REWARD = rospy.get_param('~reward/ball_dist_sq', 0.0)
self._GOAL_DISTANCE_REWARD = rospy.get_param('~reward/goal_dist_sq', 0.0)
self._DIRECTION_CHANGE_REWARD = rospy.get_param('~reward/direction_change', 0.0)
- self._WIN_REWARD = rospy.get_param('~reward/win', 100.0)
- self._LOSS_REWARD = rospy.get_param('~reward/loss', 0.0)
+ if isinstance(rospy.get_param('~reward/win', [100.0]), int):
+ self._WIN_REWARD = rospy.get_param('~reward/win', [100.0])
+ else:
+ if len(rospy.get_param('~reward/win', [100.0])) >= self.env_number:
+ self._WIN_REWARD = rospy.get_param('~reward/win', [100.0])[0]
+ else:
+ self._WIN_REWARD = rospy.get_param('~reward/win', [100.0])[self.env_number]
+ if isinstance(rospy.get_param('~reward/loss', [100.0]), int):
+ self._LOSS_REWARD = rospy.get_param('~reward/loss', [100.0])
+ else:
+ if len(rospy.get_param('~reward/loss', [100.0])) >= self.env_number:
+ self._LOSS_REWARD = rospy.get_param('~reward/loss', [100.0])[0]
+ else:
+ self._LOSS_REWARD = rospy.get_param('~reward/loss', [100.0])[self.env_number]
self._REVERSE_REWARD = rospy.get_param('~reward/reverse', 0.0)
self._WALL_REWARD = rospy.get_param('~reward/walls/value', 0.0)
self._WALL_THRESHOLD = rospy.get_param('~reward/walls/threshold', 0.0)
diff --git a/rktl_launch/config/global_params.yaml b/rktl_launch/config/global_params.yaml
index a2b12ffeb..2053ccd0c 100644
--- a/rktl_launch/config/global_params.yaml
+++ b/rktl_launch/config/global_params.yaml
@@ -3,7 +3,11 @@
# field dimensions
field:
width: 3
+ # max: 3.5
+ # min: 2.5
length: 4.25
+ # max: 5
+ # min: 3.5
wall_thickness: 0.25
goal:
width: 1
@@ -15,8 +19,12 @@ ball:
# car dimensions and physical constants
cars:
length: 0.12 # front to rear wheel center to center, meters
+ # min: 0.10
+ # max: 0.30
steering:
max_throw: 0.1826 # center to side, radians
+ # min: 0.16
+ # max: 0.20
rate: 0.9128 # rad/s
throttle:
max_speed: 2.3 # m/s
diff --git a/rktl_launch/launch/rocket_league_sim.launch b/rktl_launch/launch/rocket_league_sim.launch
index 00a975c97..83614a149 100644
--- a/rktl_launch/launch/rocket_league_sim.launch
+++ b/rktl_launch/launch/rocket_league_sim.launch
@@ -24,6 +24,10 @@
+
+
+
+
diff --git a/rktl_sim/config/simulation.yaml b/rktl_sim/config/simulation.yaml
index 0ac21cd77..60be00b58 100644
--- a/rktl_sim/config/simulation.yaml
+++ b/rktl_sim/config/simulation.yaml
@@ -18,3 +18,86 @@ ball:
pos: [0.01, 0.01, 0.0]
orient: [0.0, 0.0, 0.0]
dropout: 0.1
+
+#
+# Simulation settings
+#
+
+rate: 10
+spawn_height: 0.06
+
+# Directly export to setPhysicsEngineParameters()
+engine:
+ fixedTimeStep: 0.001
+ restitutionVelocityThreshold: 0.0001
+
+# Directly exported to changeDynamics()
+dynamics:
+ ball:
+ mass: 0.2
+ lateralFriction: 0.4
+ restitution: 0.7
+ rollingFriction: 0.0001
+ spinningFriction: 0.001
+ contactDamping: 0.0
+ contactStiffness: 0.0
+ collisionMargin: 0.0
+ car:
+ mass: 0.0
+ lateralFriction: 0.0
+ restitution: 0.0
+ rollingFriction: 0.0
+ spinningFriction: 0.0
+ contactDamping: 0.0
+ contactStiffness: 0.0
+ collisionMargin: 0.0
+ walls:
+ mass: 0.0
+ lateralFriction: 0.0
+ restitution: 0.0
+ rollingFriction: 0.0
+ spinningFriction: 0.0
+ contactDamping: 0.0
+ contactStiffness: 0.0
+ collisionMargin: 0.0
+ floor:
+ mass: 0.0
+ lateralFriction: 0.0
+ restitution: 0.0
+ rollingFriction: 0.0
+ spinningFriction: 0.0
+ contactDamping: 0.0
+ contactStiffness: 0.0
+ collisionMargin: 0.0
+
+#
+# Object instances
+#
+
+sensor_noise:
+ car:
+ pos: [0.01, 0.01, 0.00]
+ orient: [0.0, 0.0, 0.09]
+ dropout: 0.0
+ ball:
+ pos: [0.01, 0.01, 0.0]
+ orient: [0.0, 0.0, 0.0]
+ dropout: 0.0
+
+cars:
+ - name: "car0"
+ # init_pose:
+ # pos: [0.5, 0.0, 0.06]
+
+ # - name: "car1"
+ # # init_pose:
+ # # pos: [0.5, 0.0, 0.06]
+ # sensor_noise:
+ # pos: [0.01, 0.01, 0.00]
+ # orient: [0.0, 0.0, 0.09]
+
+ball:
+ init_speed: 0.0
+ # init_pose:
+ # pos: [0.0, 0.5, 0.06]
+ # orient: [0.0, 0.0, 0.0]
\ No newline at end of file
diff --git a/rktl_sim/nodes/simulator b/rktl_sim/nodes/simulator
index 0bec54e09..3a206a363 100755
--- a/rktl_sim/nodes/simulator
+++ b/rktl_sim/nodes/simulator
@@ -4,13 +4,10 @@ License:
BSD 3-Clause License
Copyright (c) 2023, Autonomous Robotics Club of Purdue (Purdue ARC)
All rights reserved.
-
-TODO:
-- Scale to support multiple vehicles
-- Add offset for walls
"""
# 3rd party modules
+import random
from geometry_msgs.msg import PoseWithCovarianceStamped
from nav_msgs.msg import Odometry
import os
@@ -19,21 +16,47 @@ from std_srvs.srv import Empty, EmptyResponse
from threading import Lock
from enum import Enum
-# Local library
+# local libraries
import simulator
from rktl_msgs.msg import MatchStatus, ControlCommand, ControlEffort
+
class SimulatorMode(Enum):
- IDEAL = 1
- REALISTIC = 2
+ IDEAL = 1 # no sensor noise, publish car and ball odom & pose early
+ REALISTIC = 2 # sensor noise for pose & orient of ball and car, publish with a delay
+
+
+class Simulator(object):
+ """Serves as an interface between params and services to the Sim class.
+ Handles simulator configuration parameters:
+ sim rate(default=30), frame id(default=map), timeout(default=10)
+ sensor noise, urdf paths, car list, car properties
+
+ Two types of parameters: configuration-based and instance-based.
+ Configuration-based parameters determine the kinematics and dynamics of the sim.
+ A few examples: car size, sim timestep, friction, etc.
+ Instance-based parameters determine the initial state of a new run.
+ A few examples: car pose, ball pose, etc.
+ On startup, we load both configuration and instance parameters and create an initial setup.
-class SimulatorROS(object):
- """ROS wrapper for the simulator."""
+ Simulator controls Configuration-based parameters.
+ Sim.py handles instance-based parameters.
+ """
def __init__(self):
+ """Initialize the simulator environment, field and objects properties, and car and ball objects."""
+ self.props = None
+ self.spawn_bounds = None
+ self.sensor_noise = None
+ self.ball_noise = None
+ self.car_noise = None
+ self.ball_init_pose = None
+ self.ball_init_speed = None
+ self.car_properties = None
+
+ # setting config parameters (stay constant for the whole simulator run)
rospy.init_node('simulator')
-
- mode = rospy.get_param('~mode')
+ mode = self.get_sim_param('~mode')
if mode == 'ideal':
self.mode = SimulatorMode.IDEAL
elif mode == 'realistic':
@@ -41,100 +64,71 @@ class SimulatorROS(object):
else:
rospy.signal_shutdown('unknown sim mode set "{}"'.format(mode))
- render_enabled = rospy.get_param('~render', False)
- rate = rospy.Rate(rospy.get_param('~rate', 30))
- self.frame_id = rospy.get_param('~frame_id', 'map')
- self.timeout = rospy.get_param('~timeout', 10)
-
- # Setting up field
- fw = rospy.get_param('/field/width')
- fl = rospy.get_param('/field/length')
- wt = rospy.get_param('/field/wall_thickness')
- gw = rospy.get_param('/field/goal/width')
- spawn_height = rospy.get_param('~spawn_height', 0.06)
-
- # Setting up field
- field_setup = {}
- field_setup["goalA"] = [(fl/2.) + (wt/2.), 0, spawn_height]
- field_setup["goalB"] = [(-fl/2.) - (wt/2.), 0, spawn_height]
- field_setup["rsidewall"] = [0, (fw/2) + (wt/2.), spawn_height]
- field_setup["lsidewall"] = [0, (-fw/2) - (wt/2.), spawn_height]
-
- bww = (fw-gw)/2
- offset = (gw+bww)/2
- field_setup["flbackwall"] = [(fl/2) + (wt/2.), -offset, spawn_height]
- field_setup["frbackwall"] = [(fl/2) + (wt/2.), offset, spawn_height]
- field_setup["blbackwall"] = [(-fl/2) - (wt/2.), -offset, spawn_height]
- field_setup["brbackwall"] = [(-fl/2) - (wt/2.), offset, spawn_height]
+ self.cmd_lock = Lock()
+ self.reset_lock = Lock()
+ self.car_cmd = (0.0, 0.0)
- # Setup bounds for spawning car and ball
- spawn_bounds = [[-(fl/2) + (2 * wt), (fl/2) - (2 * wt)],
- [-(fw/2) + (2 * wt), (fw/2) - (2 * wt)],
- [spawn_height, spawn_height]]
+ render_enabled = self.get_sim_param('~render', secondParam=False)
+ rate = rospy.Rate(self.get_sim_param('~rate', secondParam=30))
+ self.frame_id = self.get_sim_param('~frame_id', secondParam='map')
+ self.timeout = self.get_sim_param('~timeout', secondParam=10)
- urdf_paths = rospy.get_param('~urdf')
- for path in urdf_paths.values():
+ # setup urdf file paths: a universal way to describe kinematics and dynamics of robots
+ self.urdf_paths = self.get_sim_param('~urdf')
+ for path in self.urdf_paths.values():
self.check_urdf(path)
- # Creating physics simulator
- self.sim = simulator.Sim(urdf_paths, field_setup, spawn_bounds, render_enabled)
+ self.props = {
+ 'engine': self.get_sim_param('~engine', secondParam=None),
+ 'dynamics': self.get_sim_param('~dynamics', secondParam=None),
+ }
- # Creating the ball
- ball_init_pose = rospy.get_param('~ball/init_pose', None)
- ball_init_speed = rospy.get_param('~ball/init_speed', None)
- ball_noise = rospy.get_param('~ball/sensor_noise', None)
- if self.mode == SimulatorMode.IDEAL:
- ball_noise = None
+ # prep the simulator for a new run, setting all instance parameters for the sim
- self.sim.create_ball('ball', init_pose=ball_init_pose,
- init_speed=ball_init_speed, noise=ball_noise)
-
- if self.mode == SimulatorMode.REALISTIC:
- self.ball_pose_pub = rospy.Publisher('/ball/pose_sync_early',
- PoseWithCovarianceStamped, queue_size=1)
- self.ball_odom_pub = rospy.Publisher('/ball/odom_truth', Odometry, queue_size=1)
- elif self.mode == SimulatorMode.IDEAL:
- self.ball_odom_pub = rospy.Publisher('/ball/odom', Odometry, queue_size=1)
-
- # Creating a single car
- car_properties = {}
- car_properties['length'] = rospy.get_param('/cars/length')
- car_properties['max_speed'] = rospy.get_param('/cars/throttle/max_speed')
- car_properties['throttle_tau'] = rospy.get_param('/cars/throttle/tau')
- car_properties['steering_throw'] = rospy.get_param('/cars/steering/max_throw')
- car_properties['steering_rate'] = rospy.get_param('/cars/steering/rate')
- car_properties['simulate_effort'] = self.mode == SimulatorMode.REALISTIC
-
- noise = rospy.get_param('~car/sensor_noise', None)
- if self.mode == SimulatorMode.IDEAL:
- noise = None
+ self.spawn_bounds, self.field_setup = self.configure_field()
+ self.car_ids = {}
+ self.car_pose_pubs = {}
+ self.car_odom_pubs = {}
+ self.car_effort_subs = {}
+ self.car_cmd_subs = {}
+ # TODO: find a better way to not have duplicate code segment
+ self.car_properties = {'length': self.get_sim_param('/cars/length'),
+ 'max_speed': self.get_sim_param("/cars/throttle/max_speed"),
+ 'steering_throw': self.get_sim_param("/cars/steering/max_throw"),
+ 'throttle_tau': self.get_sim_param("/cars/throttle/tau"),
+ 'steering_rate': self.get_sim_param("/cars/steering/rate"),
+ 'simulate_effort': (self.mode == SimulatorMode.REALISTIC)}
- self.sim.create_car('car',
- init_pose=rospy.get_param('~car/init_pose', None),
- noise=noise,
- props=car_properties)
+ self.sim = simulator.Sim(
+ self.props, self.urdf_paths, self.spawn_bounds, self.field_setup, render_enabled)
- if self.mode == SimulatorMode.REALISTIC:
- self.car_pose_pub = rospy.Publisher('/cars/car0/pose_sync_early',
- PoseWithCovarianceStamped, queue_size=1)
- rospy.Subscriber('/cars/car0/effort', ControlEffort, self.effort_cb)
- self.car_odom_pub = rospy.Publisher('/cars/car0/odom_truth', Odometry, queue_size=1)
- elif self.mode == SimulatorMode.IDEAL:
- self.car_odom_pub = rospy.Publisher('/cars/car0/odom', Odometry, queue_size=1)
- rospy.Subscriber('/cars/car0/command', ControlCommand, self.cmd_cb)
+ self.sim.create_ball('ball', init_pose=self.ball_init_pose,
+ init_speed=self.ball_init_speed, noise=self.ball_noise)
+
+ self.update_all_cars()
- # Node data
self.cmd_lock = Lock()
self.reset_lock = Lock()
- self.car_cmd = (0.0, 0.0)
self.last_time = None
+ self.reset_cb(None) # janky reset call with mandatory none parameter
+
# Publishers
self.status_pub = rospy.Publisher(
'match_status', MatchStatus, queue_size=1)
+ self.ball_pose_pub, self.ball_odom_pub = None, None
+ if self.mode == SimulatorMode.REALISTIC:
+ self.ball_pose_pub = rospy.Publisher('/ball/pose_sync_early',
+ PoseWithCovarianceStamped, queue_size=1)
+ self.ball_odom_pub = rospy.Publisher('/ball/odom_truth',
+ Odometry, queue_size=1)
+ elif self.mode == SimulatorMode.IDEAL:
+ self.ball_odom_pub = rospy.Publisher('/ball/odom',
+ Odometry, queue_size=1)
# Services
- rospy.Service('sim_reset', Empty, self.reset_cb)
+ rospy.Service('sim_reset_car_and_ball', Empty, self.reset_cb)
+ rospy.Service('sim_reset_ball', Empty, self.reset_ball_cb)
while not rospy.is_shutdown():
self.loop_once()
@@ -143,42 +137,88 @@ class SimulatorROS(object):
except rospy.ROSInterruptException:
pass
- def check_urdf(self, param):
- """Validates that URDF exists, then returns path."""
- if param is None:
- rospy.signal_shutdown('no urdf path set for "{}"'.format(param))
+ def check_urdf(self, urdf_path):
+ """Validates that a URDF exists, then returns its file."""
- if not os.path.isfile(param):
+ if urdf_path is None:
rospy.signal_shutdown(
- 'no urdf file exists at path {}'.format(param))
+ 'no urdf path set for "{}"'.format(urdf_path))
+
+ i = 0
+ while not os.path.isfile(urdf_path) and i < 5:
+ rospy.sleep(0.1) # wait for xacro build
+ i += 1
- def effort_cb(self, effort_msg):
+ if not os.path.isfile(urdf_path):
+ rospy.signal_shutdown(
+ 'no urdf file exists at path {}'.format(urdf_path))
+
+ def effort_cb(self, effort_msg, car_id):
+ """Sets a car's effort with throttle and steering (overwrites commands)."""
self.cmd_lock.acquire()
- self.car_cmd = (effort_msg.throttle, effort_msg.steering)
+
+ self.sim.set_car_command(car_id,
+ (effort_msg.throttle, effort_msg.steering))
self.cmd_lock.release()
- def cmd_cb(self, cmd_msg):
+ def cmd_cb(self, cmd_msg, car_id):
+ """Sets a car's command with the velocity and curvature. (overwrites effort)."""
self.cmd_lock.acquire()
- self.car_cmd = (cmd_msg.velocity, cmd_msg.curvature)
+ self.sim.set_car_command(car_id,
+ (cmd_msg.velocity, cmd_msg.curvature))
self.cmd_lock.release()
def reset_cb(self, _):
- """Resets simulator."""
+
self.reset_lock.acquire()
- self.sim.reset()
- self.car_cmd = (0.0, 0.0)
+
+ # setting sim parameters (can be modified by the user)
+ self.spawn_bounds, self.field_setup = self.configure_field()
+
+ self.sensor_noise = self.get_sim_param(
+ '~sensor_noise', secondParam=None)
+
+ self.car_noise = None
+ if self.sensor_noise is not None and self.mode == SimulatorMode.REALISTIC:
+ self.car_noise = self.sensor_noise.get('car', None)
+
+ self.reset_ball_cb(None)
+
+ self.car_properties = {'length': self.get_sim_param('/cars/length'),
+ 'max_speed': self.get_sim_param("/cars/throttle/max_speed"),
+ 'steering_throw': self.get_sim_param("/cars/steering/max_throw"),
+ 'throttle_tau': self.get_sim_param("/cars/throttle/tau"),
+ 'steering_rate': self.get_sim_param("/cars/steering/rate"),
+ 'simulate_effort': (self.mode == SimulatorMode.REALISTIC)}
+ self.sim.reset(self.spawn_bounds, self.car_properties,
+ self.ball_init_pose, self.ball_init_speed)
+
self.last_time = None
self.reset_lock.release()
return EmptyResponse()
+ def reset_ball_cb(self, _):
+ """Resets the ball sensor noise and pose WITHOUT resetting the whole sim."""
+ self.ball_noise = None
+ if self.sensor_noise is not None and self.mode == SimulatorMode.REALISTIC:
+ self.ball_noise = self.sensor_noise.get('ball', None)
+
+ self.ball_init_pose = self.get_sim_param('~ball/init_pose')
+ self.ball_init_speed = self.get_sim_param('/ball/init_speed')
+
+ self.sim.reset_ball()
+ return EmptyResponse()
+
def loop_once(self):
- """Main loop."""
+ """Step the simulation once step, updating match status, moving and publishing new car and ball positions."""
self.reset_lock.acquire()
+
now = rospy.Time.now()
if self.last_time is not None and self.last_time != now:
- # Iterate sim one step
delta_t = (now - self.last_time).to_sec()
+ self.sim.step(self.car_cmd, delta_t)
+ # publish game status
status = MatchStatus()
if self.sim.scored:
if self.sim.winner == "A":
@@ -189,30 +229,28 @@ class SimulatorROS(object):
status.status = MatchStatus.ONGOING
self.status_pub.publish(status)
- self.sim.step(self.car_cmd, delta_t)
-
+ # publish pose and odom data
if self.mode == SimulatorMode.REALISTIC:
- # Publish ball sync pose
- ball_pos, ball_quat = self.sim.getBallPose(add_noise=True)
- if ball_pos is not None:
- ball_msg = PoseWithCovarianceStamped()
- ball_msg.header.stamp = now
- ball_msg.header.frame_id = self.frame_id
- ball_msg.pose.pose.position.x = ball_pos[0]
- ball_msg.pose.pose.position.y = ball_pos[1]
- ball_msg.pose.pose.position.z = ball_pos[2]
- ball_msg.pose.pose.orientation.x = ball_quat[0]
- ball_msg.pose.pose.orientation.y = ball_quat[1]
- ball_msg.pose.pose.orientation.z = ball_quat[2]
- ball_msg.pose.pose.orientation.w = ball_quat[3]
- self.ball_pose_pub.publish(ball_msg)
-
- # Publish car sync pose
- car_pos, car_quat = self.sim.getCarPose(add_noise=True)
- if car_pos is not None:
+ ball_msg = PoseWithCovarianceStamped()
+ ball_msg.header.stamp = now
+ ball_msg.header.frame_id = self.frame_id
+ ball_pos, ball_quat = self.sim.get_ball_pose(add_noise=True)
+ ball_msg.pose.pose.position.x = ball_pos[0]
+ ball_msg.pose.pose.position.y = ball_pos[1]
+ ball_msg.pose.pose.position.z = ball_pos[2]
+ ball_msg.pose.pose.orientation.x = ball_quat[0]
+ ball_msg.pose.pose.orientation.y = ball_quat[1]
+ ball_msg.pose.pose.orientation.z = ball_quat[2]
+ ball_msg.pose.pose.orientation.w = ball_quat[3]
+ self.ball_pose_pub.publish(ball_msg)
+
+ for car_name in self.car_ids:
+
car_msg = PoseWithCovarianceStamped()
car_msg.header.stamp = now
car_msg.header.frame_id = self.frame_id
+ car_pos, car_quat = self.sim.get_car_pose(
+ self.car_ids[car_name], add_noise=True)
car_msg.pose.pose.position.x = car_pos[0]
car_msg.pose.pose.position.y = car_pos[1]
car_msg.pose.pose.position.z = car_pos[2]
@@ -220,13 +258,11 @@ class SimulatorROS(object):
car_msg.pose.pose.orientation.y = car_quat[1]
car_msg.pose.pose.orientation.z = car_quat[2]
car_msg.pose.pose.orientation.w = car_quat[3]
- self.car_pose_pub.publish(car_msg)
-
- # Publish ball odometry
+ self.car_pose_pubs[car_name].publish(car_msg)
ball_msg = Odometry()
ball_msg.header.stamp = now
ball_msg.header.frame_id = self.frame_id
- ball_pos, ball_quat = self.sim.getBallPose()
+ ball_pos, ball_quat = self.sim.get_ball_pose()
ball_msg.pose.pose.position.x = ball_pos[0]
ball_msg.pose.pose.position.y = ball_pos[1]
ball_msg.pose.pose.position.z = ball_pos[2]
@@ -234,7 +270,7 @@ class SimulatorROS(object):
ball_msg.pose.pose.orientation.y = ball_quat[1]
ball_msg.pose.pose.orientation.z = ball_quat[2]
ball_msg.pose.pose.orientation.w = ball_quat[3]
- ball_linear, ball_angular = self.sim.getBallVelocity()
+ ball_linear, ball_angular = self.sim.get_ball_velocity()
ball_msg.twist.twist.linear.x = ball_linear[0]
ball_msg.twist.twist.linear.y = ball_linear[1]
ball_msg.twist.twist.linear.z = ball_linear[2]
@@ -243,29 +279,174 @@ class SimulatorROS(object):
ball_msg.twist.twist.angular.z = ball_angular[2]
self.ball_odom_pub.publish(ball_msg)
- # Publish bot odometry
- car_msg = Odometry()
- car_msg.header.stamp = now
- car_msg.header.frame_id = self.frame_id
- car_pos, car_quat = self.sim.getCarPose()
- car_msg.pose.pose.position.x = car_pos[0]
- car_msg.pose.pose.position.y = car_pos[1]
- car_msg.pose.pose.position.z = car_pos[2]
- car_msg.pose.pose.orientation.x = car_quat[0]
- car_msg.pose.pose.orientation.y = car_quat[1]
- car_msg.pose.pose.orientation.z = car_quat[2]
- car_msg.pose.pose.orientation.w = car_quat[3]
- car_linear, car_angular = self.sim.getCarVelocity()
- car_msg.twist.twist.linear.x = car_linear[0]
- car_msg.twist.twist.linear.y = car_linear[1]
- car_msg.twist.twist.linear.z = car_linear[2]
- car_msg.twist.twist.angular.x = car_angular[0]
- car_msg.twist.twist.angular.y = car_angular[1]
- car_msg.twist.twist.angular.z = car_angular[2]
- self.car_odom_pub.publish(car_msg)
+ for car_name in self.car_ids:
+ car_msg = Odometry()
+ car_msg.header.stamp = now
+ car_msg.header.frame_id = self.frame_id
+ car_pos, car_quat = self.sim.get_car_pose(
+ self.car_ids[car_name])
+ car_msg.pose.pose.position.x = car_pos[0]
+ car_msg.pose.pose.position.y = car_pos[1]
+ car_msg.pose.pose.position.z = car_pos[2]
+ car_msg.pose.pose.orientation.x = car_quat[0]
+ car_msg.pose.pose.orientation.y = car_quat[1]
+ car_msg.pose.pose.orientation.z = car_quat[2]
+ car_msg.pose.pose.orientation.w = car_quat[3]
+ car_linear, car_angular = self.sim.get_car_velocity(
+ self.car_ids[car_name])
+ car_msg.twist.twist.linear.x = car_linear[0]
+ car_msg.twist.twist.linear.y = car_linear[1]
+ car_msg.twist.twist.linear.z = car_linear[2]
+ car_msg.twist.twist.angular.x = car_angular[0]
+ car_msg.twist.twist.angular.y = car_angular[1]
+ car_msg.twist.twist.angular.z = car_angular[2]
+ self.car_odom_pubs[car_name].publish(car_msg)
self.last_time = now
self.reset_lock.release()
+ # def get_sim_dict(self, path, returnValue=False, secondParam=None):
+ # rospy_param = rospy.get_param(path, secondParam)
+ # if not rospy_param:
+ # if returnValue:
+ # rospy.logfatal(f'invalid file path: {path}')
+ # return None
+ # else:
+ # for param in rospy_param:
+
+
+ def get_sim_param(self, path, returnValue=False, secondParam=None):
+ """
+ @param secondParam: Specify if you want to pass in a second parameter to rospy.
+ @param path: A direct path to the variable.
+ @param returnValue:
+ True: None is returned if variable does not exist.
+ False: An error is thrown if variable does not exist.
+ @return: None or Exception.
+ """
+ rospy_param = rospy.get_param(path, secondParam)
+ if not rospy_param:
+ if returnValue:
+ rospy.logfatal(f'invalid file path: {path}')
+ return None
+ else:
+ if '~' in path:
+ if secondParam is not None:
+
+ return rospy.get_param(f'{path}', secondParam)
+ else:
+
+ return rospy.get_param(f'{path}')
+
+ type_rospy = type(rospy_param)
+
+ if type_rospy == dict:
+ if secondParam is None:
+
+ min_param = (float)(rospy.get_param(f'{path}/min'))
+ max_param = (float)(rospy.get_param(f'{path}/max'))
+ else:
+ min_param = (float)(rospy.get_param(
+ f'{path}/min', secondParam))
+ max_param = (float)(rospy.get_param(
+ f'{path}/max', secondParam))
+
+ if not max_param:
+ if returnValue:
+ rospy.logfatal(f'invalid file path: {path}/max')
+ return None
+ if not min_param:
+ if returnValue:
+ rospy.logfatal(f'invalid file path: {path}/min')
+ return None
+ # accounting for bugs in yaml file
+ if min_param > max_param:
+ param_val =(float)(random.uniform(max_param, min_param))
+ rospy.set_param(f'{path}',param_val)
+
+ return param_val
+ else:
+ param_val =(float)(random.uniform(min_param, max_param))
+ rospy.set_param(f'{path}',param_val)
+ return param_val
+
+ elif type_rospy == float or type_rospy == int:
+ if secondParam is not None:
+ return rospy.get_param(path, secondParam)
+ else:
+ return rospy.get_param(path)
+ if returnValue:
+ rospy.logfatal(f'invalid file path: {path}')
+ return None
+
+ def configure_field(self):
+ """Configures the field boundries and goals to be used in the simulator."""
+ fw = self.get_sim_param('/field/width')
+ fl = self.get_sim_param('/field/length')
+ wt = self.get_sim_param('/field/wall_thickness')
+ gw = self.get_sim_param('/field/goal/width')
+ spawn_height = self.get_sim_param('~spawn_height', 0.06)
+
+ spawn_bounds = [[-(fl / 2) + (2 * wt), (fl / 2) - (2 * wt)],
+ [-(fw / 2) + (2 * wt), (fw / 2) - (2 * wt)],
+ [spawn_height, spawn_height]]
+ # Setting up field
+ field_setup = {}
+ field_setup["goalA"] = [(fl/2.) + (wt/2.), 0, spawn_height]
+ field_setup["goalB"] = [(-fl/2.) - (wt/2.), 0, spawn_height]
+ field_setup["rsidewall"] = [0, (fw/2) + (wt/2.), spawn_height]
+ field_setup["lsidewall"] = [0, (-fw/2) - (wt/2.), spawn_height]
+
+ bww = (fw-gw)/2
+ offset = (gw+bww)/2
+ field_setup["flbackwall"] = [(fl/2) + (wt/2.), -offset, spawn_height]
+ field_setup["frbackwall"] = [(fl/2) + (wt/2.), offset, spawn_height]
+ field_setup["blbackwall"] = [(-fl/2) - (wt/2.), -offset, spawn_height]
+ field_setup["brbackwall"] = [(-fl/2) - (wt/2.), offset, spawn_height]
+
+ return spawn_bounds, field_setup
+
+ def update_all_cars(self):
+ """Generates instance-parameters, Subscribers, Publishers for each car."""
+ car_configs = self.get_sim_param('~cars', secondParam=[])
+
+
+ for car_config in car_configs:
+ init_pose = self.get_sim_param('~cars/init_pose')
+
+ if 'name' not in car_config:
+ rospy.signal_shutdown('no "name" set for car config in sim')
+ car_name = car_config['name']
+ if 'randomize_pose' in car_config:
+ init_pose = None
+
+ # means the car is already there and we only need to reset it
+ if car_name not in self.car_ids:
+ # the car does not exist so we will create it
+ # otherwise, we are only reseting the car's parameters which will happen in the sim.reset call
+
+ self.car_ids[car_name] = self.sim.create_car(
+ 'car', init_pose=init_pose, noise=self.car_noise, car_props=self.car_properties)
+ car_id = self.car_ids[car_name]
+ # create the car's Subscribers
+ self.car_effort_subs[car_name] = rospy.Subscriber(
+ f'/cars/{car_name}/effort', ControlEffort,
+ self.effort_cb, callback_args=car_id)
+ self.car_cmd_subs[car_name] = rospy.Subscriber(
+ f'/cars/{car_name}/command', ControlCommand,
+ self.cmd_cb, callback_args=car_id)
+ # create the car's Publishers
+ if self.mode == SimulatorMode.REALISTIC:
+ self.car_pose_pubs[car_name] = rospy.Publisher(
+ f'/cars/{car_name}/pose_sync_early',
+ PoseWithCovarianceStamped, queue_size=1)
+ self.car_odom_pubs[car_name] = rospy.Publisher(
+ f'/cars/{car_name}/odom_truth', Odometry, queue_size=1)
+ elif self.mode == SimulatorMode.IDEAL:
+ self.car_odom_pubs[car_name] = rospy.Publisher(
+ f'/cars/{car_name}/odom', Odometry, queue_size=1)
+
+ return True
+
if __name__ == "__main__":
- SimulatorROS()
+ Simulator()
diff --git a/rktl_sim/src/simulator/car.py b/rktl_sim/src/simulator/car.py
index 6b3d01f58..f4d0582bb 100644
--- a/rktl_sim/src/simulator/car.py
+++ b/rktl_sim/src/simulator/car.py
@@ -10,12 +10,35 @@
import math
import numpy as np
+# locations used for accessing car position and orientation
+JOINT_IDS = (1, 0, 2) # X, Y, W
+BASE_QUATERNION = [0., 0., 0.]
+
+
class Car(object):
- def __init__(self, carID, pos, orient, car_properties):
- self.id = carID
+ def __init__(self, car_id, pos, orient, car_properties):
+ """Sets instance-based properties for a car and generates instance-based properties for a sim run."""
+ self._MAX_CURVATURE = None
+ self._STEERING_RATE = None
+ self._THROTTLE_TAU = None
+ self._STEERING_THROW = None
+ self._LENGTH = None
+ self._MAX_SPEED = None
+ self._psi = None
+ self.cmd = None
+ self.joint_ids = None
+ self._v_rear = None
+ self.id = car_id
+ self.init_pos = None
+ self.orient = None
self.simulate_effort = car_properties['simulate_effort']
+ self.set_properties(car_properties)
+
+ self.body_link_id = 1 # urdf configuration
- # physical constants
+ self.reset(pos, orient)
+
+ def set_properties(self, car_properties):
self._LENGTH = car_properties['length']
self._MAX_SPEED = car_properties['max_speed']
self._THROTTLE_TAU = car_properties['throttle_tau']
@@ -23,34 +46,25 @@ def __init__(self, carID, pos, orient, car_properties):
self._STEERING_RATE = car_properties['steering_rate']
self._MAX_CURVATURE = math.tan(self._STEERING_THROW) / self._LENGTH
- # urdf configuration
- self.body_link_id = 1
-
- # system state
- self._v_rear = 0.0
- self._psi = 0.0
-
- # model configuration
- p.resetBasePositionAndOrientation(
- self.id, [0., 0., pos[2]], p.getQuaternionFromEuler([0., 0., 0.]))
-
- self.joint_ids = (1, 0, 2) # X, Y, W
- p.resetJointState(self.id, self.joint_ids[0], targetValue=pos[0])
- p.resetJointState(self.id, self.joint_ids[1], targetValue=pos[1])
- p.resetJointState(self.id, self.joint_ids[2], targetValue=orient[2])
+ def setCmd(self, cmd):
+ self.cmd = cmd
- def step(self, cmd, dt):
+ def step(self, dt):
# get current yaw angle
- _, orient = self.getPose()
+ _, orient = self.get_pose()
theta = p.getEulerFromQuaternion(orient)[2]
+ if self.cmd is None:
+ return
+
if self.simulate_effort:
# transfrom control input to reference angles and velocities
- v_rear_ref = cmd[0] * self._MAX_SPEED
- psi_ref = cmd[1] * self._STEERING_THROW
+ v_rear_ref = self.cmd[0] * self._MAX_SPEED
+ psi_ref = self.cmd[1] * self._STEERING_THROW
# update rear wheel velocity using 1st order model
- self._v_rear = (self._v_rear - v_rear_ref) * math.exp(-dt/self._THROTTLE_TAU) + v_rear_ref
+ self._v_rear = (self._v_rear - v_rear_ref) * \
+ math.exp(-dt/self._THROTTLE_TAU) + v_rear_ref
# update steering angle using massless acceleration to a fixed rate
if abs(psi_ref - self._psi) < self._STEERING_RATE * dt:
@@ -68,24 +82,29 @@ def step(self, cmd, dt):
math.sqrt(math.pow(math.tan(self._psi), 2.0) / 4.0 + 1.0)
omega = self._v_rear * math.tan(self._psi) / self._LENGTH
else:
- body_vel = cmd[0]
+ body_vel = self.cmd[0]
if abs(body_vel) > self._MAX_SPEED:
body_vel = math.copysign(self._MAX_SPEED, body_vel)
- body_curv = cmd[1]
+ body_curv = self.cmd[1]
if abs(body_curv) > self._MAX_CURVATURE:
- body_curv = math.copysign(self._MAX_CURVATURE, body_curv)
-
+ body_curv = math.copysign(self._MAX_CURVATURE, body_curv)
+
x_dot = body_vel * math.cos(theta)
y_dot = body_vel * math.sin(theta)
omega = body_vel * body_curv
p.setJointMotorControlArray(self.id, self.joint_ids,
- targetVelocities=(x_dot, y_dot, omega),
- controlMode=p.VELOCITY_CONTROL,
- forces=(5000, 5000, 5000))
-
- def getPose(self, noise=None):
+ targetVelocities=(x_dot, y_dot, omega),
+ controlMode=p.VELOCITY_CONTROL,
+ forces=(5000, 5000, 5000))
+
+ def get_pose(self, noise=None):
+ """
+ Randomizes and sets a new position for the car.
+ @param noise: The sensor noise and if it is present (None=no noise).
+ @return: The position and orientation of the car.
+ """
pos = p.getLinkState(self.id, self.body_link_id)[0]
heading = p.getJointState(self.id, self.joint_ids[2])[0]
orient = (0.0, 0.0, heading)
@@ -96,10 +115,13 @@ def getPose(self, noise=None):
pos = np.random.normal(pos, noise['pos'])
orient = np.random.normal(orient, noise['orient'])
- return (pos, p.getQuaternionFromEuler(orient))
+ return pos, p.getQuaternionFromEuler(orient)
+
+ def get_velocity(self):
+ """Returns the linear and angular velocity of the car."""
- def getVelocity(self):
- link_state = p.getLinkState(self.id, self.body_link_id, computeLinkVelocity=1)
+ link_state = p.getLinkState(
+ self.id, self.body_link_id, computeLinkVelocity=1)
orient = link_state[1]
linear, angular = link_state[6:8]
heading = p.getEulerFromQuaternion(orient)[2]
@@ -107,12 +129,35 @@ def getVelocity(self):
[math.sin(heading), math.cos(heading), 0.],
[0., 0., 1.]], dtype=np.float32)
linear = r_inv @ linear
- return (linear, angular)
+
+ return linear, angular
def reset(self, pos, orient):
+ """Resets the car state with the new pose and orient."""
+ self.init_pos = pos
+ self.orient = orient
+
self._v_rear = 0.0
self._psi = 0.0
+ p.resetBasePositionAndOrientation(
+ self.id, [0., 0., pos[2]], p.getQuaternionFromEuler(BASE_QUATERNION))
+
+ self.joint_ids = JOINT_IDS # X, Y, W
p.resetJointState(self.id, self.joint_ids[0], targetValue=pos[0])
p.resetJointState(self.id, self.joint_ids[1], targetValue=pos[1])
p.resetJointState(self.id, self.joint_ids[2], targetValue=orient[2])
+
+ self.cmd = None
+
+ def check_overlap(self, pos):
+ """
+ Returns whether the position will overlap with the current car.
+ @param pos: The position of the other object.
+ @return: Boolean if the positions overlap (true = overlap).
+ """
+
+ val = ((pos[0] - self.init_pos[0]) * (pos[0] - self.init_pos[0])) + \
+ ((pos[1] - self.init_pos[1]) * (pos[1] - self.init_pos[1]))
+ dist = math.sqrt(val)
+ return dist < self._LENGTH
diff --git a/rktl_sim/src/simulator/sim.py b/rktl_sim/src/simulator/sim.py
index 4907eaaa0..164eb5cd1 100644
--- a/rktl_sim/src/simulator/sim.py
+++ b/rktl_sim/src/simulator/sim.py
@@ -7,6 +7,7 @@
# 3rd party modules
import math
+import re
import pybullet as p
import random
import numpy as np
@@ -14,35 +15,64 @@
# Local modules
from simulator.car import Car
-class Sim(object):
- """Oversees components of the simulator"""
- def __init__(self, urdf_paths, field_setup, spawn_bounds, render_enabled):
+class Sim(object):
+ """
+ Oversees instance-based parameters and objects of the simulator.
+ Cars, ball objects, goal position, etc.
+ """
+
+ class NoURDFError(Exception):
+ pass
+
+ def __init__(self, props, urdf_paths, spawn_bounds, field_setup, render_enabled):
+ """
+ Initializes the playing field, field properties, and field elements.
+ @param props: Connect the pybullet object based on the gui and direct.
+ @param urdf_paths: Configure: filed type, walls, floor, goal a and b.
+ @param spawn_bounds: Initialize cars list and other data related to them.
+ @param render_enabled: Use the loadURDF via p.loadURDF (loads the specific instruction).
+ """
+ self.touchedLast = None
+ self.ball_noise = None
+ self._speed_bound = None
+ self.init_ball_pos = None
+ # determine if you want to display the simulation
if render_enabled:
self._client = p.connect(p.GUI)
else:
self._client = p.connect(p.DIRECT)
+ self.props = props
+
+ # urdf is used to encode kinetics and location of the object
+ # use it for setting the field type, walls, floor and the different goals
self.urdf_paths = urdf_paths
self.spawn_bounds = spawn_bounds
-
- zeroOrient = p.getQuaternionFromEuler([0.0, 0.0, 0.0])
- self._planeID = None
+ # set the floor for the simulation
+ zero_pos = [0.0, 0.0, 0.0]
+ zero_orient = p.getQuaternionFromEuler([0.0, 0.0, 0.0])
+ self._plane_id = None
+
+ # set the goals for the simulation
+ self._goal_a_id = None
+ self._goal_b_id = None
if "plane" in urdf_paths:
self._planeID = p.loadURDF(
- urdf_paths["plane"], [0, 0, 0], zeroOrient, useFixedBase=1
+ urdf_paths["plane"], [0, 0, 0], zero_orient, useFixedBase=1
)
- p.changeDynamics(bodyUniqueId=self._planeID, linkIndex=-1, restitution=1.0)
+ p.changeDynamics(bodyUniqueId=self._planeID,
+ linkIndex=-1, restitution=1.0)
self._goalAID = None
self._goalBID = None
if "goal" in urdf_paths:
self._goalAID = p.loadURDF(
- urdf_paths["goal"], field_setup["goalA"], zeroOrient, useFixedBase=1
+ urdf_paths["goal"], field_setup["goalA"], zero_orient, useFixedBase=1
)
self._goalBID = p.loadURDF(
- urdf_paths["goal"], field_setup["goalB"], zeroOrient, useFixedBase=1
+ urdf_paths["goal"], field_setup["goalB"], zero_orient, useFixedBase=1
)
self._walls = {}
@@ -50,19 +80,21 @@ def __init__(self, urdf_paths, field_setup, spawn_bounds, render_enabled):
lSidewallID = p.loadURDF(
urdf_paths["sidewall"],
field_setup["lsidewall"],
- zeroOrient,
+ zero_orient,
useFixedBase=1,
)
- p.changeDynamics(bodyUniqueId=lSidewallID, linkIndex=-1, restitution=1.0)
+ p.changeDynamics(bodyUniqueId=lSidewallID,
+ linkIndex=-1, restitution=1.0)
self._walls[lSidewallID] = True
rSidewallId = p.loadURDF(
urdf_paths["sidewall"],
field_setup["rsidewall"],
- zeroOrient,
+ zero_orient,
useFixedBase=1,
)
- p.changeDynamics(bodyUniqueId=rSidewallId, linkIndex=-1, restitution=1.0)
+ p.changeDynamics(bodyUniqueId=rSidewallId,
+ linkIndex=-1, restitution=1.0)
self._walls[rSidewallId] = True
if "backwall" in urdf_paths:
@@ -70,170 +102,280 @@ def __init__(self, urdf_paths, field_setup, spawn_bounds, render_enabled):
flBackwallID = p.loadURDF(
urdf_paths["backwall"],
field_setup["flbackwall"],
- zeroOrient,
+ zero_orient,
useFixedBase=1,
)
- p.changeDynamics(bodyUniqueId=flBackwallID, linkIndex=-1, restitution=1.0)
+ p.changeDynamics(bodyUniqueId=flBackwallID,
+ linkIndex=-1, restitution=1.0)
self._walls[flBackwallID] = True
frBackwallID = p.loadURDF(
urdf_paths["backwall"],
field_setup["frbackwall"],
- zeroOrient,
+ zero_orient,
useFixedBase=1,
)
- p.changeDynamics(bodyUniqueId=frBackwallID, linkIndex=-1, restitution=1.0)
+ p.changeDynamics(bodyUniqueId=frBackwallID,
+ linkIndex=-1, restitution=1.0)
self._walls[frBackwallID] = True
blBackwallID = p.loadURDF(
urdf_paths["backwall"],
field_setup["blbackwall"],
- zeroOrient,
+ zero_orient,
useFixedBase=1,
)
- p.changeDynamics(bodyUniqueId=blBackwallID, linkIndex=-1, restitution=1.0)
+ p.changeDynamics(bodyUniqueId=blBackwallID,
+ linkIndex=-1, restitution=1.0)
self._walls[blBackwallID] = True
brBackwallID = p.loadURDF(
urdf_paths["backwall"],
field_setup["brbackwall"],
- zeroOrient,
+ zero_orient,
useFixedBase=1,
)
- p.changeDynamics(bodyUniqueId=brBackwallID, linkIndex=-1, restitution=1.0)
+ p.changeDynamics(bodyUniqueId=brBackwallID,
+ linkIndex=-1, restitution=1.0)
self._walls[brBackwallID] = True
self._cars = {}
- self._ballID = None
+ self._car_data = {}
+ self._ball_id = None
self.touched_last = None
self.scored = False
self.winner = None
- p.setPhysicsEngineParameter(useSplitImpulse=1, restitutionVelocityThreshold=0.0001)
+ if 'engine' in self.props and self.props['engine'] is not None:
+ p.setPhysicsEngineParameter(**self.props['engine'])
p.setGravity(0, 0, -10)
- def create_ball(self, urdf_name, init_pose=None, init_speed=None, noise=None):
+ def configure_dynamics(self, body_id, body_type):
+ """
+ Set the car's curvature and general car behavior.
+ @param body_id: The id of the object to be configured.
+ @param body_type: The specific type of object (ie ball,car,goal,etc).
+ @return: Error if not initialized.
+ """
+ if 'dynamics' not in self.props or \
+ self.props['dynamics'] is None or \
+ body_type not in self.props['dynamics']:
+ return
+
+ num_links = p.getNumJoints(body_id)
+ for i in range(num_links):
+ p.changeDynamics(body_id, i, **self.props['dynamics'][body_type])
+
+ def create_ball(self, urdf_name, init_pose=None, init_speed=None,
+ noise=None, init_vel=None):
+ """
+ @param urdf_name: The id for the specific pybullet object.
+ @param init_pose: The initial position of the ball (override randomization).
+ @param init_speed: The max speed of the ball (override known speed parameter).
+ @param noise: The noise and if it should be present in the location of the object.
+ @param init_vel: The initial velocity of the ball (override randomization).
+ @return: The ball id if the creation was successful.
+ """
if urdf_name in self.urdf_paths:
- zeroOrient = p.getQuaternionFromEuler([0.0, 0.0, 0.0])
+ zero_orient = p.getQuaternionFromEuler([0.0, 0.0, 0.0])
if init_pose:
- ballPos = init_pose["pos"]
- self.initBallPos = ballPos
+ ball_pos = init_pose["pos"]
+ self.init_ball_pos = ball_pos
else:
- ballPos = [
- random.uniform(self.spawn_bounds[0][0], self.spawn_bounds[0][1]),
- random.uniform(self.spawn_bounds[1][0], self.spawn_bounds[1][1]),
- random.uniform(self.spawn_bounds[2][0], self.spawn_bounds[2][1]),
+ ball_pos = [
+ random.uniform(
+ self.spawn_bounds[0][0], self.spawn_bounds[0][1]),
+ random.uniform(
+ self.spawn_bounds[1][0], self.spawn_bounds[1][1]),
+ random.uniform(
+ self.spawn_bounds[2][0], self.spawn_bounds[2][1]),
+ ]
+ self.init_ball_pos = None
+ self._ball_id = p.loadURDF(
+ self.urdf_paths[urdf_name], ball_pos, zero_orient)
+ self.configure_dynamics(self._ball_id, "ball")
+
+ # initialize the ball with some speed
+ if init_vel:
+ ball_vel = init_vel
+ else:
+ if init_speed:
+ self._speed_bound = math.sqrt(2.0) * init_speed
+ else:
+ self._speed_bound = 0.0
+ ball_vel = [
+ random.uniform(-self._speed_bound, self._speed_bound),
+ random.uniform(-self._speed_bound, self._speed_bound),
+ 0.0,
]
- self.initBallPos = None
- self._ballID = p.loadURDF(
- self.urdf_paths[urdf_name], ballPos, zeroOrient)
- p.changeDynamics(
- bodyUniqueId=self._ballID,
- linkIndex=-1,
- restitution=0.7,
- linearDamping=0,
- angularDamping=0,
- rollingFriction=0.0001,
- spinningFriction=0.001,
- )
- # initize ball with some speed
- self._speed_bound = math.sqrt(2.0) * init_speed
- ballVel = [
- random.uniform(-self._speed_bound, self._speed_bound),
- random.uniform(-self._speed_bound, self._speed_bound),
- 0.0,
- ]
- p.resetBaseVelocity(self._ballID, ballVel, zeroOrient)
+ p.resetBaseVelocity(self._ball_id, ball_vel, zero_orient)
self.ball_noise = noise
- return self._ballID
+ return self._ball_id
else:
return None
- def create_car(self, urdf_name, init_pose=None, noise=None, props=None):
+ def create_car(self, urdf_name, init_pose=None, noise=None, car_props=None):
+ """
+ Creates instance based car properties(pose,vel,orient) and configures car dynamics.
+ @param urdf_name: The id for the specific pybullet object.
+ @param init_pose: The initial position of the ball (override randomization).
+ @param noise: The noise and if it should be present in the location of the object.
+ @param car_props: Configuration based car properties.
+ @return: The car id if the creation was successful.
+ """
+
+ # set the spawn location for the car
if urdf_name in self.urdf_paths:
- zeroPos = [0.0, 0.0, 0.0]
- zeroOrient = p.getQuaternionFromEuler([0.0, 0.0, 0.0])
- self._carID = p.loadURDF(self.urdf_paths[urdf_name], zeroPos, zeroOrient)
+ zero_pos = [0.0, 0.0, 0.0]
+ zero_orient = [0.0, 0.0, 0.0]
+ car_id = p.loadURDF(self.urdf_paths[urdf_name], zero_pos,
+ p.getQuaternionFromEuler(zero_orient))
if init_pose:
if "pos" in init_pose:
- carPos = init_pose["pos"]
+ car_pos = init_pose["pos"]
else:
- carPos = zeroPos
+ car_pos = zero_pos
if "orient" in init_pose:
- carOrient = init_pose["orient"]
+ car_orient = init_pose["orient"]
else:
- carOrient = zeroOrient
+ car_orient = zero_orient
- self.initCarPos = carPos
- self.initCarOrient = carOrient
+ init_car_pos = car_pos
+ init_car_orient = car_orient
else:
- carPos = [
- random.uniform(self.spawn_bounds[0][0], self.spawn_bounds[0][1]),
- random.uniform(self.spawn_bounds[1][0], self.spawn_bounds[1][1]),
- random.uniform(self.spawn_bounds[2][0], self.spawn_bounds[2][1]),
+ car_pos = [
+ random.uniform(
+ self.spawn_bounds[0][0], self.spawn_bounds[0][1]),
+ random.uniform(
+ self.spawn_bounds[1][0], self.spawn_bounds[1][1]),
+ random.uniform(
+ self.spawn_bounds[2][0], self.spawn_bounds[2][1]),
]
- carOrient = [0.0, 0.0, random.uniform(0, 2 * math.pi)]
- self.initCarPos = None
- self.initCarOrient = None
- self._cars[self._carID] = Car(
- self._carID,
- carPos,
- carOrient,
- props
+ car_orient = [0.0, 0.0, random.uniform(0, 2 * math.pi)]
+ init_car_pos = None
+ init_car_orient = None
+
+ self._cars[car_id] = Car(
+ car_id,
+ car_pos,
+ car_orient,
+ car_props
)
- self.car_noise = noise
- return self._carID
+
+ # configures dynamics of the car
+ self.configure_dynamics(car_id, "car")
+
+ self._car_data[car_id] = {
+ "init_pos": init_car_pos,
+ "init_orient": init_car_orient,
+ "noise": noise,
+ }
+ return car_id
else:
return None
+ def delete_car(self, car_id):
+ """
+ Removes a car from being tracked in the _cars and _car_data lists.
+ @param car_id: The id of the car in the simulator class.
+ @return: Whether the deletion was successful.
+ """
+ if car_id not in self._cars:
+ return False
+
+ p.removeBody(car_id)
+ del self._cars[car_id]
+ del self._car_data[car_id]
+ return True
+
+ def set_car_command(car_id, msg):
+ """
+ Set the command for the car
+ """
+ cars[car_id].setCmd(msg)
+
def step(self, car_cmd, dt):
- """Advance one time-step in the sim."""
- if self._ballID is not None:
- ballContacts = p.getContactPoints(bodyA=self._ballID)
- for contact in ballContacts:
+ """
+ Moves the sim forward one timestep, checking if a goal is score to end the sim round.
+ @param dt: The change in time (delta-t) for this sim step.
+ """
+ # PyBullet steps at 240hz
+ p_dt = 1.0 / 240.0
+ if self._ball_id is not None:
+ ball_contacts = p.getContactPoints(bodyA=self._ball_id)
+ # TODO: need to decrease ball velocity
+ linear, angular = self.get_ball_velocity()
+ new_linear,new_angular = self.get_decreased_velocity(linear,angular,p_dt)
+ p.resetBaseVelocity(self._ball_id,new_linear,new_angular)
+
+ for contact in ball_contacts:
if contact[2] in self._cars:
self.touchedLast = contact[2]
- elif contact[2] == self._goalAID:
+ elif contact[2] == self._goal_a_id:
self.scored = True
self.winner = "A"
- elif contact[2] == self._goalBID:
+ elif contact[2] == self._goal_b_id:
self.scored = True
self.winner = "B"
+
- # PyBullet steps at 240hz
- p_dt = 1.0 / 240.0
+
for _ in range(round(dt / p_dt)):
- # Step kinematic objects independently, at max possible rate
+ # step kinematic objects independently, at max possible rate
for car in self._cars.values():
- car.step(car_cmd, p_dt)
+ car.step(p_dt)
p.stepSimulation()
-
- def getCarPose(self, add_noise=False):
- cars = list(self._cars.values())
- if len(cars) == 0:
+ def get_decreased_velocity(self,linear,angular,dt):
+ x_vel,y_vel,z_vel=linear[0],linear[1],linear[2]
+
+ current_speed = math.sqrt(x_vel*x_vel +y_vel*y_vel)
+ # r*cos(theta), r*sin(theta)
+
+
+ if current_speed < 0.001: # a very small number
+ return (0,0,z_vel),angular
+ # for each time step, new_vel = old_vel - myu_r*dt
+ angle = math.atan(y_vel/x_vel)
+ current_speed = current_speed - 0.17 * dt
+
+ new_x_vel = current_speed*math.cos(angle) # v*cos(theta)
+ new_y_vel = current_speed*math.sin(angle) # v*sin(theta)
+
+
+ return (new_x_vel,new_y_vel,z_vel),angular
+
+ def get_car_pose(self, id, add_noise=False):
+
+ if id not in self._cars:
return None
- # TODO: Provide translation from ARC IDs to Sim IDs
+ noise = self._car_data[id]['noise']
if add_noise:
- return cars[0].getPose(noise=self.car_noise)
+ return self._cars[id].get_pose(noise=noise)
else:
- return cars[0].getPose(noise=None)
+ return self._cars[id].get_pose(noise=None)
+
+ def get_car_velocity(self, id):
+ """Returns a tuple of linear and angular velocity for the car."""
+ if id not in self._cars:
+ return None
+
+ return self._cars[id].get_velocity()
- def getCarVelocity(self):
- cars = list(self._cars.values())
- if len(cars) == 0:
+ def set_car_command(self, id, cmd):
+ if id not in self._cars:
return None
- # TODO: Provide translation from ARC IDs to Sim IDs
- return cars[0].getVelocity()
+ return self._cars[id].setCmd(cmd)
- def getBallPose(self, add_noise=False):
- if self._ballID is None:
+ def get_ball_pose(self, add_noise=False):
+ """@param add_noise: State whether you want noise to get the ball position (default=False)."""
+ if self._ball_id is None:
return None
- pos, _ = p.getBasePositionAndOrientation(self._ballID)
+ pos, _ = p.getBasePositionAndOrientation(self._ball_id)
if add_noise and self.ball_noise:
if np.random.uniform() < self.ball_noise['dropout']:
@@ -243,46 +385,90 @@ def getBallPose(self, add_noise=False):
return pos, p.getQuaternionFromEuler([0, 0, 0])
- def getBallVelocity(self):
- if self._ballID is None:
+ def get_ball_velocity(self):
+ if self._ball_id is None:
return None
- return p.getBaseVelocity(self._ballID)
- def reset(self):
+ return p.getBaseVelocity(self._ball_id)
+
+ def reset(self, spawn_bounds, car_properties, ball_init_pose, ball_init_speed):
+ """
+ Resets the ball, score, winner, spawn bounds, cars and ball.
+ @param spawn_bounds: The new spawn bounds.
+ @param car_properties: The new car properties.
+ """
self.scored = False
self.winner = None
self.touched_last = None
+ if ball_init_pose is not None:
+ self.init_ball_pos = ball_init_pose
+ if ball_init_speed is not None:
+ self._speed_bound = ball_init_speed
- if self._ballID is not None:
- ballPos = self.initBallPos
- if ballPos is None:
- ballPos = [
- random.uniform(self.spawn_bounds[0][0], self.spawn_bounds[0][1]),
- random.uniform(self.spawn_bounds[1][0], self.spawn_bounds[1][1]),
- random.uniform(self.spawn_bounds[2][0], self.spawn_bounds[2][1]),
+ self.spawn_bounds = spawn_bounds
+ self.reset_ball()
+ for car in self._cars.values():
+ self.reset_car(car, car_properties)
+
+ def reset_car(self, car, car_properties):
+ """
+ Loops over the cars and generates new initial positions (if they were not specified).
+ @param car_properties: The new car config properties.
+ """
+ # reset the car properties in advance
+ car.set_properties(car_properties)
+ car_pos = self._car_data[car.id]["init_pos"]
+ car_orient = self._car_data[car.id]["init_orient"]
+
+ if car_pos is None:
+ car_pos = self.generate_new_car_pos()
+
+ while self.check_if_pos_overlap(car_pos):
+ car_pos = self.generate_new_car_pos()
+
+ if car_orient is None:
+ car_orient = [0, 0, random.uniform(0, 2 * math.pi)]
+ car.reset(car_pos, car_orient)
+
+ def check_if_pos_overlap(self, car_pos):
+ """
+ Checks if two cars spawn bounds overlap with each other.
+ @param car_pos: The position of the car.
+ @return: Whether overlap happens (true = need to generate new bounds).
+ """
+ for car in self._cars.values():
+ overlap = car.check_overlap(car_pos)
+ if overlap:
+ return True
+
+ return False
+
+ def generate_new_car_pos(self):
+ car_pos = [random.uniform(self.spawn_bounds[0][0], self.spawn_bounds[0][1]),
+ random.uniform(
+ self.spawn_bounds[1][0], self.spawn_bounds[1][1]),
+ random.uniform(self.spawn_bounds[2][0], self.spawn_bounds[2][1])]
+ return car_pos
+
+ def reset_ball(self):
+
+ if self._ball_id is not None:
+ ball_pos = self.init_ball_pos
+ if ball_pos is None:
+ ball_pos = [
+ random.uniform(
+ self.spawn_bounds[0][0], self.spawn_bounds[0][1]),
+ random.uniform(
+ self.spawn_bounds[1][0], self.spawn_bounds[1][1]),
+ random.uniform(
+ self.spawn_bounds[2][0], self.spawn_bounds[2][1]),
]
p.resetBasePositionAndOrientation(
- self._ballID, ballPos, p.getQuaternionFromEuler([0, 0, 0])
+ self._ball_id, ball_pos, p.getQuaternionFromEuler([0, 0, 0])
)
-
- ballVel = [
+ ball_vel = [
random.uniform(-self._speed_bound, self._speed_bound),
random.uniform(-self._speed_bound, self._speed_bound),
0.0,
]
- p.resetBaseVelocity(self._ballID, ballVel, [0, 0, 0])
-
- for car in self._cars.values():
- carPos = self.initCarPos
- carOrient = self.initCarOrient
-
- if carPos is None:
- carPos = [random.uniform(self.spawn_bounds[0][0], self.spawn_bounds[0][1]),
- random.uniform(
- self.spawn_bounds[1][0], self.spawn_bounds[1][1]),
- random.uniform(self.spawn_bounds[2][0], self.spawn_bounds[2][1])]
-
- if carOrient is None:
- carOrient = [0, 0, random.uniform(0, 2 * math.pi)]
-
- car.reset(carPos, carOrient)
+ p.resetBaseVelocity(self._ball_id, ball_vel, [0, 0, 0])
diff --git a/rktl_sim/urdf/walls.urdf b/rktl_sim/urdf/walls.urdf
new file mode 100644
index 000000000..77271a6bb
--- /dev/null
+++ b/rktl_sim/urdf/walls.urdf
@@ -0,0 +1,126 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+