From b50c3361406d19abc9123a69325fe4a8c293780a Mon Sep 17 00:00:00 2001 From: Haddy Alchaer Date: Sun, 25 Sep 2022 18:42:03 +0000 Subject: [PATCH 01/43] number of envs, saves, and steps are now external variables --- rktl_autonomy/scripts/train_rocket_league.py | 11 +++++++---- 1 file changed, 7 insertions(+), 4 deletions(-) diff --git a/rktl_autonomy/scripts/train_rocket_league.py b/rktl_autonomy/scripts/train_rocket_league.py index dcb356fc8..40f52a8b5 100755 --- a/rktl_autonomy/scripts/train_rocket_league.py +++ b/rktl_autonomy/scripts/train_rocket_league.py @@ -15,13 +15,17 @@ from os.path import expanduser import uuid +n_envs = 24 +n_saves = 100 +n_steps = 240000000 + if __name__ == '__main__': # this is required due to forking processes 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) + n_envs=n_envs, vec_env_cls=SubprocVecEnv) model = PPO("MlpPolicy", env) @@ -31,12 +35,11 @@ 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 # 240M (10M sequential) print(f"training on {steps} steps") model.learn(total_timesteps=steps, callback=callback) From 5aeb6b643cbd087465f26b73425de11a9dbc13e6 Mon Sep 17 00:00:00 2001 From: Haddy Alchaer Date: Sun, 25 Sep 2022 15:06:57 -0400 Subject: [PATCH 02/43] Training file is not a modular function --- rktl_autonomy/scripts/train_rocket_league.py | 11 +++++------ 1 file changed, 5 insertions(+), 6 deletions(-) diff --git a/rktl_autonomy/scripts/train_rocket_league.py b/rktl_autonomy/scripts/train_rocket_league.py index 40f52a8b5..5bf8974ac 100755 --- a/rktl_autonomy/scripts/train_rocket_league.py +++ b/rktl_autonomy/scripts/train_rocket_league.py @@ -15,11 +15,7 @@ from os.path import expanduser import uuid -n_envs = 24 -n_saves = 100 -n_steps = 240000000 - -if __name__ == '__main__': # this is required due to forking processes +def train(n_envs = 24, n_saves = 100, n_steps = 240000000) : run_id = str(uuid.uuid4()) # ALL running environments must share this print(f"RUN ID: {run_id}") @@ -39,7 +35,7 @@ callback = CheckpointCallback(save_freq=freq, save_path=log_dir) # run training - steps = n_steps # 240M (10M sequential) + steps = n_steps print(f"training on {steps} steps") model.learn(total_timesteps=steps, callback=callback) @@ -47,3 +43,6 @@ print("done training") model.save(log_dir + "/final_weights") env.close() # this must be done to clean up other processes + +if __name__ == '__main__': + train() \ No newline at end of file From 9e28e129729f0fc258ed31f64f80d5ab94d35e66 Mon Sep 17 00:00:00 2001 From: Haddy Alchaer Date: Sun, 25 Sep 2022 15:23:08 -0400 Subject: [PATCH 03/43] modular_train.py created, can run the training script as a modular function --- rktl_autonomy/scripts/modular_train.py | 4 ++++ 1 file changed, 4 insertions(+) create mode 100755 rktl_autonomy/scripts/modular_train.py diff --git a/rktl_autonomy/scripts/modular_train.py b/rktl_autonomy/scripts/modular_train.py new file mode 100755 index 000000000..300a52d94 --- /dev/null +++ b/rktl_autonomy/scripts/modular_train.py @@ -0,0 +1,4 @@ +from train_rocket_league import * + +if __name__ == '__main__' : + train_rocket_league.train() \ No newline at end of file From aeea180e775af116032b56885b5ed35f5b3f9c4f Mon Sep 17 00:00:00 2001 From: Haddy Alchaer Date: Sun, 25 Sep 2022 19:50:42 +0000 Subject: [PATCH 04/43] Template for idea --- rktl_autonomy/scripts/modular_train.py | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/rktl_autonomy/scripts/modular_train.py b/rktl_autonomy/scripts/modular_train.py index 300a52d94..4a72293b5 100755 --- a/rktl_autonomy/scripts/modular_train.py +++ b/rktl_autonomy/scripts/modular_train.py @@ -1,4 +1,9 @@ -from train_rocket_league import * +from train_rocket_league import train if __name__ == '__main__' : - train_rocket_league.train() \ No newline at end of file + + chnage_params(ydgfydgsyufgf) + + train() + + From c4e0411bfbdb9eeae323108813b08233c0b877e2 Mon Sep 17 00:00:00 2001 From: jcrm1 <52137472+jcrm1@users.noreply.github.com> Date: Fri, 30 Sep 2022 10:22:34 -0400 Subject: [PATCH 05/43] Add YAML Editor and update .gitignore --- .../src/com/jcrm1/arc/yamleditor/Main.java | 127 ++++++++++++++++++ 1 file changed, 127 insertions(+) create mode 100644 rktl_autonomy/YAMLEditor/src/com/jcrm1/arc/yamleditor/Main.java diff --git a/rktl_autonomy/YAMLEditor/src/com/jcrm1/arc/yamleditor/Main.java b/rktl_autonomy/YAMLEditor/src/com/jcrm1/arc/yamleditor/Main.java new file mode 100644 index 000000000..398333d47 --- /dev/null +++ b/rktl_autonomy/YAMLEditor/src/com/jcrm1/arc/yamleditor/Main.java @@ -0,0 +1,127 @@ +package com.jcrm1.arc.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.JScrollPane; +import javax.swing.JTable; +import javax.swing.JTextField; +import javax.swing.table.DefaultTableModel; +/** + * Unpack into a table, edit, and repack a YAML-styled 2D array + * + * @author Campbell McClendon + * + * @version 0.1 + */ +public class Main { + private static JFrame frame; + private static JTextField inputField; + private static JButton updateButton; + private static final Rectangle scrollPaneBounds = new Rectangle(2, 44, 496, 454); + private static final String[] titles = new String[] {"ID", "WIN", "LOSE"}; + private static String[][] data = null; + public static void main(String[] args) { + frame = new JFrame("YAML Editor"); + frame.setSize(500, 500); + + inputField = new JTextField("[[5,6],[7,8]]", 50); + + inputField.setBounds(2, 2, 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); + + JScrollPane scrollPane = new JScrollPane(table); + scrollPane.setBounds(scrollPaneBounds); + + JTextField outputField = new JTextField(50); + outputField.setBounds(104, 2, 100, 20); + + updateButton = new JButton("Unpack Array"); + updateButton.setBounds(2, 22, 100, 20); + updateButton.addActionListener(new ActionListener() { + @Override + public void actionPerformed(ActionEvent e) { + String text = clip(inputField.getText()); + String[] byID = text.split(Pattern.quote("],[")); + if (byID.length < 2) { + System.err.println("Malformed input data"); + System.exit(1); + } + /* + * tableModel.getDataVector().toArray(new Vector[tableModel.getDataVector().size()]) + */ + String[][] newData = new String[byID.length][]; + int prevValuesLength = -1; + for (int id = 0; id < byID.length; id++) { + String[] values = clip(byID[id]).split(","); + if (prevValuesLength != -1) if (values.length != prevValuesLength) { + System.err.println("Malformed input data"); + System.exit(1); + } + newData[id] = values; + } + 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); + } + }); + + JButton repackButton = new JButton("Repack Array"); + repackButton.setBounds(104, 22, 100, 20); + repackButton.addActionListener(new ActionListener() { + + @SuppressWarnings("unchecked") + @Override + public void actionPerformed(ActionEvent e) { + String[][] outputData = 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]; + } + outputData[x] = line; + } + outputField.setText(Arrays.deepToString(outputData).replaceAll("\\s+","")); + } + + }); + + frame.add(inputField); + frame.add(updateButton); + frame.add(repackButton); + frame.add(scrollPane); + frame.add(outputField); + + 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; + } +} From ab67a12a22b9a724880f8b5e3f6405f0799a84e4 Mon Sep 17 00:00:00 2001 From: jcrm1 <52137472+jcrm1@users.noreply.github.com> Date: Fri, 30 Sep 2022 15:16:17 -0400 Subject: [PATCH 06/43] Update .gitignore Forgot to stage it on the last commit --- .gitignore | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/.gitignore b/.gitignore index bf5d198a2..35ddc54f2 100644 --- a/.gitignore +++ b/.gitignore @@ -1,7 +1,13 @@ # Byte-compiled / optimized / DLL files __pycache__/ *.py[cod] -*$py.class +*.class + +# Java +rktl_autonomy/YAMLEditor/bin +rktl_autonomy/YAMLEditor/.classpath +rktl_autonomy/YAMLEditor/.project +rktl_autonomy/YAMLEditor/.settings # C extensions *.so From a5816dd83abaeddf68535fb7a3d96fab7a46480d Mon Sep 17 00:00:00 2001 From: jcrm1 <52137472+jcrm1@users.noreply.github.com> Date: Fri, 30 Sep 2022 15:30:08 -0400 Subject: [PATCH 07/43] Allow table editing, fix repacking not acknowledging edits --- .../src/com/jcrm1/arc/yamleditor/Main.java | 13 ++++++++++--- 1 file changed, 10 insertions(+), 3 deletions(-) diff --git a/rktl_autonomy/YAMLEditor/src/com/jcrm1/arc/yamleditor/Main.java b/rktl_autonomy/YAMLEditor/src/com/jcrm1/arc/yamleditor/Main.java index 398333d47..409489732 100644 --- a/rktl_autonomy/YAMLEditor/src/com/jcrm1/arc/yamleditor/Main.java +++ b/rktl_autonomy/YAMLEditor/src/com/jcrm1/arc/yamleditor/Main.java @@ -11,6 +11,8 @@ 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 a YAML-styled 2D array @@ -38,6 +40,14 @@ public static void main(String[] args) { 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()); + } + } + }); JScrollPane scrollPane = new JScrollPane(table); scrollPane.setBounds(scrollPaneBounds); @@ -56,9 +66,6 @@ public void actionPerformed(ActionEvent e) { System.err.println("Malformed input data"); System.exit(1); } - /* - * tableModel.getDataVector().toArray(new Vector[tableModel.getDataVector().size()]) - */ String[][] newData = new String[byID.length][]; int prevValuesLength = -1; for (int id = 0; id < byID.length; id++) { From 45f12b36ebfab335c94aeddfe3dd9e91b3425320 Mon Sep 17 00:00:00 2001 From: jcrm1 <52137472+jcrm1@users.noreply.github.com> Date: Fri, 30 Sep 2022 15:40:02 -0400 Subject: [PATCH 08/43] Last cleanup before switch to two 1D arrays --- .../src/com/jcrm1/arc/yamleditor/Main.java | 16 +++++----------- 1 file changed, 5 insertions(+), 11 deletions(-) diff --git a/rktl_autonomy/YAMLEditor/src/com/jcrm1/arc/yamleditor/Main.java b/rktl_autonomy/YAMLEditor/src/com/jcrm1/arc/yamleditor/Main.java index 409489732..cbcac07f3 100644 --- a/rktl_autonomy/YAMLEditor/src/com/jcrm1/arc/yamleditor/Main.java +++ b/rktl_autonomy/YAMLEditor/src/com/jcrm1/arc/yamleditor/Main.java @@ -22,18 +22,15 @@ * @version 0.1 */ public class Main { - private static JFrame frame; - private static JTextField inputField; - private static JButton updateButton; - private static final Rectangle scrollPaneBounds = new Rectangle(2, 44, 496, 454); + 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) { - frame = new JFrame("YAML Editor"); + JFrame frame = new JFrame("YAML Editor"); + frame.setResizable(false); frame.setSize(500, 500); - inputField = new JTextField("[[5,6],[7,8]]", 50); - + JTextField inputField = new JTextField("[[5,6],[7,8],[9,10]]", 50); inputField.setBounds(2, 2, 100, 20); data = new String[][] {{"0", "1", "2"}, {"1", "3", "4"}}; @@ -55,7 +52,7 @@ public void tableChanged(TableModelEvent e) { JTextField outputField = new JTextField(50); outputField.setBounds(104, 2, 100, 20); - updateButton = new JButton("Unpack Array"); + JButton updateButton = new JButton("Unpack Array"); updateButton.setBounds(2, 22, 100, 20); updateButton.addActionListener(new ActionListener() { @Override @@ -91,8 +88,6 @@ public void actionPerformed(ActionEvent e) { JButton repackButton = new JButton("Repack Array"); repackButton.setBounds(104, 22, 100, 20); repackButton.addActionListener(new ActionListener() { - - @SuppressWarnings("unchecked") @Override public void actionPerformed(ActionEvent e) { String[][] outputData = new String[data.length][]; @@ -105,7 +100,6 @@ public void actionPerformed(ActionEvent e) { } outputField.setText(Arrays.deepToString(outputData).replaceAll("\\s+","")); } - }); frame.add(inputField); From 5473fb9e2392b761ded7766e4efb9b42be5ca82b Mon Sep 17 00:00:00 2001 From: jcrm1 <52137472+jcrm1@users.noreply.github.com> Date: Fri, 30 Sep 2022 16:24:48 -0400 Subject: [PATCH 09/43] Swap to 1D array --- .../src/com/jcrm1/arc/yamleditor/Main.java | 133 ++++++++++++------ 1 file changed, 91 insertions(+), 42 deletions(-) diff --git a/rktl_autonomy/YAMLEditor/src/com/jcrm1/arc/yamleditor/Main.java b/rktl_autonomy/YAMLEditor/src/com/jcrm1/arc/yamleditor/Main.java index cbcac07f3..141ca5d18 100644 --- a/rktl_autonomy/YAMLEditor/src/com/jcrm1/arc/yamleditor/Main.java +++ b/rktl_autonomy/YAMLEditor/src/com/jcrm1/arc/yamleditor/Main.java @@ -8,6 +8,7 @@ import javax.swing.JButton; import javax.swing.JFrame; +import javax.swing.JLabel; import javax.swing.JScrollPane; import javax.swing.JTable; import javax.swing.JTextField; @@ -15,7 +16,7 @@ import javax.swing.event.TableModelListener; import javax.swing.table.DefaultTableModel; /** - * Unpack into a table, edit, and repack a YAML-styled 2D array + * Unpack into a table, edit, and repack two YAML arrays * * @author Campbell McClendon * @@ -30,8 +31,14 @@ public static void main(String[] args) { frame.setResizable(false); frame.setSize(500, 500); - JTextField inputField = new JTextField("[[5,6],[7,8],[9,10]]", 50); - inputField.setBounds(2, 2, 100, 20); + 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); @@ -46,67 +53,109 @@ public void tableChanged(TableModelEvent e) { } }); + JLabel errorLabel = new JLabel(""); + errorLabel.setBounds(225, 2, 100, 20); + JScrollPane scrollPane = new JScrollPane(table); scrollPane.setBounds(scrollPaneBounds); - JTextField outputField = new JTextField(50); - outputField.setBounds(104, 2, 100, 20); + 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 updateButton = new JButton("Unpack Array"); - updateButton.setBounds(2, 22, 100, 20); - updateButton.addActionListener(new ActionListener() { + JButton unpackButton = new JButton("Unpack Arrays"); + unpackButton.setBounds(2, 24, 100, 20); + unpackButton.addActionListener(new ActionListener() { @Override public void actionPerformed(ActionEvent e) { - String text = clip(inputField.getText()); - String[] byID = text.split(Pattern.quote("],[")); - if (byID.length < 2) { - System.err.println("Malformed input data"); - System.exit(1); - } - String[][] newData = new String[byID.length][]; - int prevValuesLength = -1; - for (int id = 0; id < byID.length; id++) { - String[] values = clip(byID[id]).split(","); - if (prevValuesLength != -1) if (values.length != prevValuesLength) { - System.err.println("Malformed input data"); - System.exit(1); + 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; } - newData[id] = values; - } - 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]; + 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(); } - data = dataWithID; - tableModel.setDataVector(dataWithID, titles); } }); - JButton repackButton = new JButton("Repack Array"); - repackButton.setBounds(104, 22, 100, 20); + JButton repackButton = new JButton("Repack Arrays"); + repackButton.setBounds(398, 24, 100, 20); repackButton.addActionListener(new ActionListener() { @Override public void actionPerformed(ActionEvent e) { - String[][] outputData = 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]; + 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]; } - outputData[x] = line; + winOutputField.setText(Arrays.deepToString(winValues).replaceAll("\\s+","")); + loseOutputField.setText(Arrays.deepToString(loseValues).replaceAll("\\s+","")); + } catch (Exception exception) { + errorLabel.setText("ERROR"); + exception.printStackTrace(); } - outputField.setText(Arrays.deepToString(outputData).replaceAll("\\s+","")); } }); - frame.add(inputField); - frame.add(updateButton); + 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.add(outputField); frame.setLayout(null); frame.setDefaultCloseOperation(JFrame.EXIT_ON_CLOSE); From e2019d0ef3dbe810bc73a466036ba8ee0546165c Mon Sep 17 00:00:00 2001 From: jcrm1 <52137472+jcrm1@users.noreply.github.com> Date: Fri, 30 Sep 2022 16:26:43 -0400 Subject: [PATCH 10/43] Remove Eclipse project metadata from .gitignore --- .gitignore | 1 - rktl_autonomy/YAMLEditor/.project | 17 +++++++++++++++++ 2 files changed, 17 insertions(+), 1 deletion(-) create mode 100644 rktl_autonomy/YAMLEditor/.project diff --git a/.gitignore b/.gitignore index 35ddc54f2..449a1a4a5 100644 --- a/.gitignore +++ b/.gitignore @@ -6,7 +6,6 @@ __pycache__/ # Java rktl_autonomy/YAMLEditor/bin rktl_autonomy/YAMLEditor/.classpath -rktl_autonomy/YAMLEditor/.project rktl_autonomy/YAMLEditor/.settings # C extensions 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 + + From ee47d3892012dd1d65d0d817068683b9872b54af Mon Sep 17 00:00:00 2001 From: jcrm1 <52137472+jcrm1@users.noreply.github.com> Date: Fri, 30 Sep 2022 16:36:55 -0400 Subject: [PATCH 11/43] Cleanup .gitignore(s), add README.md and built jar --- .gitignore | 6 +----- rktl_autonomy/YAMLEditor/.gitignore | 7 +++++++ rktl_autonomy/YAMLEditor/README.md | 7 +++++++ rktl_autonomy/YAMLEditor/jars/YAMLEditor.jar | Bin 0 -> 14309 bytes .../src/com/jcrm1/arc/yamleditor/Main.java | 2 +- 5 files changed, 16 insertions(+), 6 deletions(-) create mode 100644 rktl_autonomy/YAMLEditor/.gitignore create mode 100644 rktl_autonomy/YAMLEditor/README.md create mode 100644 rktl_autonomy/YAMLEditor/jars/YAMLEditor.jar diff --git a/.gitignore b/.gitignore index 449a1a4a5..29961e5e1 100644 --- a/.gitignore +++ b/.gitignore @@ -2,11 +2,7 @@ __pycache__/ *.py[cod] *.class - -# Java -rktl_autonomy/YAMLEditor/bin -rktl_autonomy/YAMLEditor/.classpath -rktl_autonomy/YAMLEditor/.settings +*$py.class # C extensions *.so 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/README.md b/rktl_autonomy/YAMLEditor/README.md new file mode 100644 index 000000000..bf271cf3e --- /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 `build/` diff --git a/rktl_autonomy/YAMLEditor/jars/YAMLEditor.jar b/rktl_autonomy/YAMLEditor/jars/YAMLEditor.jar new file mode 100644 index 0000000000000000000000000000000000000000..55bdb728bcf5810b01e298f3102d145a5d1b72e8 GIT binary patch literal 14309 zcmbt*1z23!vM%oK?ykYz-Q9z`yIXLFAi>=|cyO2Cu8k8kXwU$G2QzbCGIP(&dFQ_R z8ou37(!Fcd+Nf&v;2;#ULuh0uV&faJtfh3KW^#Tn&<m1 z1=}e~33k&u{bv+6^zrSvYnUXOjhNgoz;r>XHNrxso>C{NQigF}%yMDw-|VglMLD;d z2cX$EzF}s21tm0$zf0c?s8ZgWMqh|`_xC?*_XbgvfrOg#ifMoM@_B1seiUE61q=`n z6wv?p7GN*`wRbXS{Cf_#KR8T{Z7dy}P5*%t=`WmCCNBTLjs0)9E$v)Po$L&4{y~iD zpTw*TohbH~TZ)QHU;qUVUViSGBS&}I;rXb+>8svGWKQZiw=1t#HlrX_(5rXf;^gXG! z9g@{wU0w0yjR*!4@tEPhiabS(0wY4!&EtD@q~@7etF0}lN>sF#=QU{CsW?`zh8i8Q zRqZwx&rD&l$85w`r6$0wzd^I_=iyEDLP^G6IF_2DtpLuE9g!o@XFTFSTg#->no3b@ zhPHJOctoG9(V4fB1~{w=$jH(p)eHi)&d4_8acQW#GBgdp$Bswn6Vk2KU2|atsKR}+ z#kHGl!jox)yA6{6R2XC`y=0_WT&*jv+C$!VBdjW8P1Cfk*%XW)I@dW?b8A2R0Jq$- zx}e`d&RfWgiWxeuH5wxN02lqOveXu^AUjm_RfaS77`+YyAb-+3FFRj;wC4>yNrj#^ zf(ggfX3On06J!igh9=jyMCb`8WB+3+ZiA^1F^)S)9sjn2cji5;M=a)?cON33Gxw2K zd|z8Y3h!O>P!PhnO0TmqMd!_-?nKNpwjfTbk2PN<3=f@6o_=@mPKkEfbI@%GkTYZfjAqeC(anJKi-H_Te{mL-N_H+Qf!Ipz$M4SSZ5;AT{tyT>aml=qVg@zFQSW0%(oui<&i z)}g$ZCuL_RW7*%ppwn|w}29ycO zcX^?@x~i&bgAx&@3KA1cArg{S+O)!`2vgyZy-91K(nRGsg|a6|pqr3*y*OX=|<~fq5&p%Qt;{Cb8MJ=#s((iN`)OhwRM-&k|Ir&7!B_O$mQL3A< zSy=5#YNf-JrJVZ}Et>A(2Hcp@2w+N+m>}cwa3skYC~iUX?C*#$mQAHT@9=jJk6s#0 zN0Vc7K|eiNFuN{1#_?BmLgIi1Ey_@Eqm|68Jm)Qqi}xy?Qjs^{Crj#9G95pU0J+}V z0cV)x3i|iv@dcsO?HVnpXWDeT@GGP##G7VjnJ48OUw{}6V`P+~Bnv>78X|*{Rqt=x zM#?kT$zt-jF>1XXgb|xuZ;o{JAbj;?(H)hGDliaG^NTlMx6yy^NuK|$C;xDs2DB#L zF1DY)W~wQ+vyCvC99*#y=4UfDCFAZw6tH@;6aylne$qbX?#*Mmth5*;z)IhGy0$lt zw?*U30>OoLf?}LrZKJ!bwY|61*+YZU{ZoBgP4~>u5X@vf%ccLubJp3d@ALDLvjibX zjgTr06*4D>DWdpQuw^u-mr3(R3|4XlAlXbjlnqdGXI$Liw-)<0f4L2h2l=uS?G7v% z&ibJtyi6-7KFhbys~4TBtGfoyy+Y8kv`MCWG4p5Ar8s-JYiw#=3)3K{dEsuiImrW3h6I*vN&Z|<%=_gYp4`Nli zT@wU&pGGdV2mMs}0DElN#lwx~s zVT5ZS=x}yH=7ofOAmk-tXvf1mA4c_o8BAJq^R}g-w|0Xa+9;F+qVh1kiz?CuZ9Btu zzpy?bR#u4#i**H?d{C)mCewsag)^c``N9b(lt_tb4iD;kgZ_5B_>DwKB-xVwhk&HD zV)EtMPiPTNXTr%7x|FFH!_*hd=f<))Xk%Ph22MugRdUraxJYt8Hq5wlrh zv3JjF_t_Yi&fR2sTVc@EEd_f~>cOQaQI(;*dPCs(kWDjzE3m(W;ri z3dZ2Y#7$>PF57Dt(9BZ9yx(S z09^JRkvjBMatFtQ0YNF-lt4t5UY2YJ9kNN3Zccuh>$PzjD0DU{w{%CJw*!>tQoTJq zj9BW4qgXdZy4ZjZdA*X-7c8n64}8Hn4GeU*AswO+4uk}X4*s22kv6LFJ)%uqE|zBL z1MY*DRwt?+WFMzy(UUiCjFo}vPz6f2iFwD4xk!t*+@dWAcqiy6+CJkYfu7IO@RLG@ z#>q&voO7Y)3^CnE^jS*9HXE;dHf!V`z|wS5QAdTbQfAj(<7sEY_(1C`o$oF?yNo$r z7e;>Ewob|g1pCjsh_96K=O7Do5>Fv>;Du4lTg(zy80O851q#O)&VNzSMNLl(c4c!8 zRK@MyxoZlvN5{j3y(+(p#Ty(6f6MII{%Dsy$>M+SA%&siZqgykQQ}E14mr!7v6$at zTPC^=Rvrlw@0;p0OA$4mGsYzbG!RLear|M}GM9T+t^!NE$ix%EO5p(qHw0@-aMcJ4OzV;D)}(-RkE(Ie~82k#uU zi_C_eeXIvph8htVVh?iF)ujCxK`K?7WOS_pv_uY`d=7R12A2y29KaEv65T9pV!8o> zuSaOd*3Pp@7hau$Vj(+M=K|=xhZgT;Sp&i`1SFKaZ<~U8%TEXD~ zjG>HSlqkEW2z6OEoW-$1>dHHNVpC<=iU2>kyfjcW;>Ecd%-!^XYUeeF$H& zqfRS7ob2_aGajEGsLpAKZ7N?R1)U^XZyUddpcM^&`A zIdS5lEJ;Q$R6%82m82*fL$$9{_Ttl#k(`k78cV_<<=QLk>J+F$+lX^mt*CDX_1MB- zNOvy@EF=^Qp<&DoG!W1Q>OUPX1^$cUZr^rB0V9b zKnRZ%30_2sA!$>I+QFbPDhf^mIc04z6+*&=7A6+PJ`V`wvB1QR9Tc!UQR3fbVo)`- z-%ZV~)wgZmM z+>3AO0C2?@oOSI&n)Yc*jl&7k&*av2^ZbSfr%brakg%AO^G4tM#U2tIEW?U~5;I%3 zx!olZr?HOIGB1i2x?vsF>BUx*iX}pT;& zUO`ZVkm)iw$ry{lT^GPps8u%e5e3()bRSN3FC9~InHFe~{Si)3l)*|9MPwSuORPYe zFvM~t$hWPT6y2kLabKt>kLk2>#XR)D%tHH2eXRX04R{4ym4+i)J9W<8H((8pU0h6a zR=rvSQW7XW)|197Xya03Sb1goVHlhP8#3Fh(~u5JhXb%9gL~Ti9H}ZH*sSrHIfVdF z#duL#%ZPlQ6(rC?(F{=oVH9X9$y3qHr?!LY*alqY>GxzpU9flI?UFK@RTgwT%&Wa8 zFyV1Yf@C@PkAXZx2eH?_B3(k#E(chLAFdd$vW;wR(NNoJB`8yAT1|~6W0~BeLr~lA z(w@k8i0X!*;TxEhq7!(K4MN#(~qLKgeS-hDt# zPDL{;%FwtGnF3h%cdZX!v)89b1^gcHyYY^2jNqcOyNqE*Ycm4gxZ~zow&k7co~E{~ ze$wT}i{l8kuLkeDcBpD4xDPz5a`?0br7sd_Xq6(cQ%oy-S(;(k@8p4`ss<6tq6Y5T z+-A&x&n*on64hiKW*S<*Z2YDLN5p8@Ivi~qiz&CqAl%%!(eO<)j-*lemgv0Ozz>U$ z43F@7?qDLtM_Q|8O0>#nIH+4HJO)%6&kAS#UUSwk?!(TyhA(>ovq{@=TOZZ#x*}pc zT1M+-2YI?OG;5||s-h%-g|>n(E|fa-yT8=88flnB4;lHR6F)P5CQ}S0bNhVktCuQ) z5Od$vMVW6sR zeHxj4?mLn`2c&t>-S-F5)r2{{bk!Dbu%j1SaGE2zAZ#UqN1IlI6PJN|xT{(E&E&Mc z6$c=m6VsPkh^<1$*6387&6GxD#e(BxMryL2#~({!+KIRpZW(L;C{VMLXc9q9LH-1{ zI*2muHJBw?X~!}KN5fn?%6$46R*&DziY-EuNwH^t4#a+@l^u%#G4Lj>6-*5oXO)kO zlkbXK9>1bc_llIG;Wjvy@t`2Vy~;QXnEIZK5T|qtSq)yabYU;7OE&{J>PkWId7OlU zkKj83GGcmf0;3pVD266Ig|2Og$U;dcfFFU-V@%Ps!zX z@Ya&6P5>O$0+oI7Kp9wafZ*_~jA2`_S|26x9A+3k*vo2lqcP1EcP8seN8 z71xvQKi+)-%E18KRmQZTu%)%=s|tm>$c!S1?Zc))rZr+2t4Iy5EO}J#QBY4B1y9h5 z?$fyPN_?rFcBxKu19f*q#k~UdkMZQ~0ZFGSTWE@4k3Vf(=uIhSqqa4YVO#Ij@TlBd z%fB0%yTP*Dc+U|5%V$|cX?tBrne>U&S7!QZmX(R0?+Jz}#x6Eq10mxX`gLQtCurC$ zd)XMmApdD&koy1J7{m>YUF@Aa|ELA2=-Q(Qq4CxG(9r>uwJp&rl&y3y8Y+CJgNqqO zxBJSalu&$elj9le+#TJg?^GT!Q%hsg?+$_lMm)@AV618o)8Q{&&oZ4S(?)LzbGks5 z`m$j{@(v+=RhFt*;6k_##EN&sNY0zB5j-|MSeyCJBD#6uKp%$l=3_y6;?A5u?nmYh zoP0)HFO_7bgTJG-YhC@C5!VW8C_ys=w|h5-8qc-QATr_RiKWd1+n3HZU?~p@9~$l3 zj~`dfq*{4tKrPU5<{|IWtyn(M!^#aeT)csbgYx<7XTu0$B{puYH*{YW;|)LS#8MK( z#qu40X@L6Bfs?#JEVxiUJ*Hr;e8^qOy+^&05z32e4SCXtOcJT*rHo)@n5srM2hu(Pi)y@wL|Y$d zp=D)IW`LrV>NKg1fNjv>8yAmP57)2`o;VY; z)j0iA7c!>YwL&QkR=wlf1!=gPe8;eZa#2GAe72Q&^U@d=s~{uS{C1(QWJyK=3mmmP z)Y}Fmbo64j3SyQQhxpqqJVELFB659&c+>~#KKnPnRiHojj46q{)GSk8Frc6CkB#@A zVXjwz$k^WYH!S6M2RdGb2m(~SVam@82MW+@~ z%$ytKgqkX2r0CZ3D8_X4=j2VN-x>JaNMKLWku^0c5x24Ik8$ijOqlvVKHkH3i>M~1 z8fiBD2w46UB(#hcVB3ifs^Nye3u&gpsaKsTnn}EdNov^I3t-7{zn@W;BExc8c^HJd z#s4-Mf0fii%3W=9uY0I1Us^~B8agN0%vFSlwO7^Wo-KAxBeET^!*zt*C;UVvm^Y5} zQE3hhYr=ty$(AToD;8}AuC^UwOgonHQ&>8Z``b54x2SVQi@ zYlf&`7ZFs4-?1TnMk~by%gSM`URR({Q?AU*FJ%YeRGA3^Z2DfFDl{8*{oDf1x*AGE zDkf(=InV1T42G6fmdQ&#poqn*_@E*+pAtN%y~Og9RMQRCK+NRk0AuQRB*VRfEFw4q z5)pfX6Zp&l-YU5L_gSJI64S21fB}U~NKKEp840@Bn8p#0j^dOE>uoN<3m$3uag3-X zGYfEuaKE9<X4Ze#2df zX5C#iUcw6fkFfgRu2kkfR;oI{8C4DIIcv`;p$0r1g&3X8U>*gB6kJJ1EC7SPNfDqe zs<>Pn<~C}^VaHx{?bq}04K@4BpD@CiBea z)8(2%=fh=`C(x%&+X#w)81heVGZ{R0ZN+A3(f7ydswn6-t|NnrcN^TLM8VY+GFZ%} z*|UbXdoC>1zL9T(;DS_o&M=l^@7@ipk#9I<0aqN+N_c|MvlOrS%{bj>Nsn)sQ+o>= zti8iXr&o{2p*tm1t0yj!Feoz9$lmt?-D|D`U!dECulXWl%@3$5kGh8F?c}nn5ONoW z)o&E#%tiB_cdR#iD8O7QrT44f)qG+AZrf2VW@Fy*ztB5cpf?7Y?f^^7<$)kW!O=D* zibx7&V5uSLuVOLCaq80kUDod%{jG`gcFuOiD{l25f-c2VK{{l17@D{>b3UUCURKKE z7-6y-8U&4JGb_f33vcx;7wuVUaJ|FJ)NP`O+lr?{B$9U$4hHxmI*SZ5j2V;RS7Ujw zjBb(zDbN~Mw~=;zq-pDAy>IImAmn()d}AZVv|2lyP>b!0-@F4w2?D8t4~EC&MscsVNcof;AQI4OrO677hw=HzHqGuaN>*kwT<{5L zoGlAH|Il;qEt^KQ+K<)dk(ME6wfah@CMSv?)DaJwp%OS?irWDLth&MLD)q1XYYO)o5Z1G-+ZzRSqY;so* zP^s>^jS8tc-_=)j|Jm_L;%)#*H)|mdtUF6VwC1BG=cVVS^LoEMxq7@eTb^FF@hae( z&SXCMh9^eV_g*cNZ_5W-mE-=Q$7zCQe2bi>?|5nhDv-Nvn?Z0}V6|(g=a#NWuP^i* zt)EmH=4!dt;wKiTzjL)h@XT`f#h!B8@hPUp`G$`TEoGd3+>v)`Dn2VW{sNabhLUz_ z!QM(Wk3Txd@OiCXwEB(0j5qP_y1$~4{*Hx;J8zvz_F$+DFKHVII0Gp#*7zW+k91@- z-I%|_Q;CfaKW?1MxI`Sd5px}jBaU|+bK*gyPs9VFf2eVqD83oK6AAbhvf$5TbqSjqTHm3IEdVQ(k64sP4xl`TN1rVl;Pl?K*2k`3i()UG zY;(IUAY?+h;ZA}*)aN4|#}mwC`SM9>530Wow=NBz-JuOkS$9Wf*hYLP3Zvj?p+EF_0B*stKrSt4^4hIQ$2k4fwa7^tg})3rQbdE zI789*Yee#iE_%6EkwBK6HK`p{O(t37rJ#u~j3l+*##u3+Lzc{^Z%q4SyK{v2c%@ku z$1p;koWm3#WH-&uaO6ElhUXGromIUve$ao-KdSG>obX54>(oq+Ak-bH}44BOYbt_WleME$1?fOJi9jbeJ6+Q{h#Z=4Up^)OOY+c zKfvtyOwebt({q?J=zvS?*?#fz05#^?v063E6B9isAZQmCME5<@BnUoQ6eU1vbwsASy~`q?}KHF>IXO| zRvO4kQGU4#y*1| z5H7)J=xj*U%5o4S9yEbKu~=^$REgdzieFo{QBl%_a2?9)IL8*swIQdZ93 zKyzWgF@K12utLZHmI$>$#az$6~X zjE9_*4qrBzn5GcjL>OfhF2IZ+gOAdo4Hbzvx!Ksb(0fKlc4_efexPFXfWMt%5_Tc< z#MT`d2A`J#EwCy)!aSHC^-@30vP>a|fvPUanez?co}aL8axYQ67-XC=iLlPJb>mDU zO9AhdH5qiGNg%~i(y+TFvVqyP!>DBP6?CEr%~2WDJ|EJO$cSA8*Cxop+%b_s=b#+2 zVS6%U^Xrh#5{t!f9t)T|Y)bI%Ea$~)+ zyoqBpu1#0E>nlXJl%fkCsaaF*X8$m$+O9QP4I%p0v*ptd>&R+u%18Sy48@xvZ=)VG zcu-%hU8(q$*c3GSC&{Pq`|O3_=6{&nuPbU|H2erTEj+|YXXr{7IEgFXZ`i*wSD4-4>o|+C z(b7b6Ic~W;Z9B6&YjZ!(hp*c-F zF6q5xcCKJ8(O6nmL|H(JI_ot2Uv)P2Wu}a-uRIlLHg)RNdO+mj&K zl>u1;J<3sJFgH7_s}keQYGza_dtL?##PDR#mCZyl(>p5vtLYz7SY`$p2gdPxg9FLBTEs`@-WjGZD~SlOXVe~dv@&s-dJDNX zJ~e3qmWWA1r0iTHx2mNpVs&~Ku!_-nArc)5iOt|j;hywhjv{KGwHTx!;_eNjvd-6F;lPotyVF|)RMA} zd5^Q{<(2I*Y2rS@88YeY-`Q=jo_QRyau`7$<~)f%ybe-Ag6w>k5XiVg3kTfV(^+V1 zGP4T+)Q|;C%&SRq9(KFWZ&Knox_0+%x4P(M_CVyKh}nu5C_~)oN;5nV{Rki}I$lR%edgJ;y`7 z>cgr%lIPozvhi>=srm#|vHc|C>s-wbM`Rt5MU3dqvj>85d!1JLox60{F;p~n*wreO zlJb`7IVY?!ew~-j;P#uC4aP@@Qr^#M z)#dUiz?b{?>^eB)Upy$x$z83!Hk@B5uk3-s6t_^Cou%^5!i zTEoGWcME}i05yAa?V>_+sG8)8CQCB_{;CplA-x90dMdEZ@#7M(B_hL0I7*QpqJlLr z?x=({mc`l1>n zOFL(D1`mq;I~OI772iB09Ufqp_*PGIXjmXodOE<5zq(b}JvaC__HCTC#A^1cMY@L% z@$~NeFv!%CSw)_RaXuw=lolxsukGg*zSPtZ%m=9S=LH{8Z}zRRS;g=m%8{$nPH5A*ZFBeyuI&B{1c41Tq7u@~(NXx{H2xZRUWKg3!X-(<8 z3=Q#tGNul1`zXAsN&BlMK!w2);VmdiMGu$7-YknDVH3Z{6UNOJ2E&|=O*t5glicQR z+Jcbkh#_bX(tCvEIcFf`xUMkHkoa2Y4YJq}Ox7))<9P>l;@fY7JfJeveR7QW0X|vZwFQ2I=%WL4 z-vFwu9kF8UKM-|?Ak>;B?}xcjK-IQHd&l3=@}_qp{w$@8nX!(~1N);QaeKel#Y-DS z1K-Tw7ICDJ4DSq)X~@*R?N+cTQY2%Cxl)(# ze5BT+o!`-_>mQ|GkWOvcBkT%xu(l@n*rwv+dG}IjS%dwmujjZv0FV=|CKuKao!sJB zlw1$We@`827D+6vbX)*6VoK0we?c*goBu_3ypw|ap2zXpe(N?rtJ#9W)vrYQN;cvt z;EBA}AIAI%@%7xWSK#5j|8i#d{8AxSlmP}o1NvDr`*n)pb>iaB^mp?YuUcS${(j=& zr{h@XV=(j@$=6c8v3=D;I(h(C&j)jnEzKpf78YD zS7X28ldtUwKM4)(zc%)7nic+P?^jgm74`f{^jQCY_Wp!{zMB14g#Q(g`tvTakMl18 z>+dCAA*`PSjr%M3^iP&vA+4Xp^e4IB0j|Hd`RC}?{U5S_0=)hlhrdD$ua%vjl=M$( zeg(k(>b76YNPkwzPw?yA_zM*Go60|siodG-H68r3$||z|r1Fny)L&Kp`lNWxBYzSn c^?y+LuTTVkiDjUd2MijJ{>zmE)9btc1GQgYr2qf` literal 0 HcmV?d00001 diff --git a/rktl_autonomy/YAMLEditor/src/com/jcrm1/arc/yamleditor/Main.java b/rktl_autonomy/YAMLEditor/src/com/jcrm1/arc/yamleditor/Main.java index 141ca5d18..5df4de958 100644 --- a/rktl_autonomy/YAMLEditor/src/com/jcrm1/arc/yamleditor/Main.java +++ b/rktl_autonomy/YAMLEditor/src/com/jcrm1/arc/yamleditor/Main.java @@ -20,7 +20,7 @@ * * @author Campbell McClendon * - * @version 0.1 + * @version 1.0 */ public class Main { private static final Rectangle scrollPaneBounds = new Rectangle(2, 44, 496, 426); From 25faccf8e3e0516055b7ba76fa7115a807868717 Mon Sep 17 00:00:00 2001 From: jcrm1 <52137472+jcrm1@users.noreply.github.com> Date: Fri, 30 Sep 2022 16:37:45 -0400 Subject: [PATCH 12/43] Remove extra line from .gitignore --- .gitignore | 1 - 1 file changed, 1 deletion(-) diff --git a/.gitignore b/.gitignore index 29961e5e1..bf5d198a2 100644 --- a/.gitignore +++ b/.gitignore @@ -1,7 +1,6 @@ # Byte-compiled / optimized / DLL files __pycache__/ *.py[cod] -*.class *$py.class # C extensions From 13f71ac583a512737d30a2aa81aa7aed4c07f711 Mon Sep 17 00:00:00 2001 From: jcrm1 <52137472+jcrm1@users.noreply.github.com> Date: Fri, 30 Sep 2022 16:43:00 -0400 Subject: [PATCH 13/43] Update README.md --- rktl_autonomy/YAMLEditor/README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rktl_autonomy/YAMLEditor/README.md b/rktl_autonomy/YAMLEditor/README.md index bf271cf3e..c97d0314c 100644 --- a/rktl_autonomy/YAMLEditor/README.md +++ b/rktl_autonomy/YAMLEditor/README.md @@ -4,4 +4,4 @@ Intended for use with reward values -Built jar in `build/` +Built jar in `jars/` From a2399e4edfb8672ba0eb93d0319cf983165954ab Mon Sep 17 00:00:00 2001 From: jcrm1 <52137472+jcrm1@users.noreply.github.com> Date: Sat, 1 Oct 2022 00:55:34 -0400 Subject: [PATCH 14/43] Change package to com.purduearc Change package to com.purduearc and update prebuilt jar --- rktl_autonomy/YAMLEditor/jars/YAMLEditor.jar | Bin 14309 -> 14213 bytes .../arc => purduearc}/yamleditor/Main.java | 2 +- 2 files changed, 1 insertion(+), 1 deletion(-) rename rktl_autonomy/YAMLEditor/src/com/{jcrm1/arc => purduearc}/yamleditor/Main.java (99%) diff --git a/rktl_autonomy/YAMLEditor/jars/YAMLEditor.jar b/rktl_autonomy/YAMLEditor/jars/YAMLEditor.jar index 55bdb728bcf5810b01e298f3102d145a5d1b72e8..bb3ed6c7dc0aac5ffbf10a6f1dd9b4de9459c850 100644 GIT binary patch delta 6514 zcmaJ_2RK|!+g`nku)3_M(M7LIL=US)?Z%9PTd-`j)gnaiy+?}@Em{bIgh=!r zA%1+{|GjVC@4x=Ju5;$OX6}3DIWu!+p1Hregt-uDs9<1%LAbcMpek&cL?UkB?R`SI z6Q!RK=MM-=4L*{jkYKc=P`Rdvr=W<>p?uAu_O!QH7o5f27|K$Ta|wR`v=5H9H2J)Nt9|0K*S%f;;} z?;nW7qk9-vKmbn6THbo(PXdlnnASJ=r@KNW13HlNe{^f*;`Fy}|A`>Ja_k!6>go2x)7rw#>i@w~{}p+9Y3SnRn zesG?wI%%nzNn!NbYRDr~8r9Qu6bYqAA_>1xD;&$I&j#aqZ&8*(H#jh`fbgW__9_HM z_IdR^S!Wq!N?!C3c<`)8smM!b4`lU`Evr5cdR`U+@^$bhBe27#+iZ) z!5$L1QYN1pql^eCP|h(qoHOLza!sZcqW9K=Y~D!;LjuSNV;@0J0Q#f6K;aZuGm)Oa z>ANM=o>qENM|Bq8ODR36>~|HuVQW6DqS2~jda(-Ge#|G1Iew|2W#O(edLvQRv%LA&; zKB_jK<_1^DYAwHUCm=_$d`Euf_O`BcFumnIltS9F*kzdeWQNvvz&LH4WNJs>(Z1ez zP*{$$QkBlT@)7V}eEdab)%OJ_uG>3qzAgL?uVLU;lj53h?hXx$_mym+Um8+G9 zgZsMOlTHsE1R}rs(f-;b{#fjY5?X@4mLx4A7(&KqKTb-+gr#XDA56hrt1)jVr}42M z!powDRwQG>w*$1ebB3sgZ~$VUhE=C1*5D(S2IJDn)~Iy;HmzsVT=VvC_f0B9Z4K4@ zUdm~kf8}$Z&|^u{RV8e5N?iVEnYR_-s%!>%ma1T|;(d zW?-S#fw$3M7Ky$qh2N7 zwc3I!UE=5M$53N$X({kFg|G<)3rS=$g1nDxkEeT=bbCX9*?TWv%Foh>dx~E`xj-(IpvA`_O`r4ihCBd{+26w*} zMLQuUi8FlKUt1ORy-vlRDX7E}QFgSxbLrqp=-B-}_Q^m!2l|W<&{&?U-hdxD0`_%n z>2X3Y!9b?Fe3Vy*6{oEvZ2TI0b-ZQhx4E4DgLCT)h3#d2k8dj4iSNmbNb~NPmN9AD zEcbYWsAJ7$Z*_2stBCCIBI&!(Jfsm_UZ%R8HPUVeGCh}L7tu7zmnf#vpy&Md@I4s2 zQr%J`r!KfM_=3t3KyuPpOSI-C4p`w4ekf^QUcTsNkF``?F{Rj=nC5rAtD#2}!y5b? z{bL9Bhd3b04v&uX9eqwyfq=RobTqSUZLqF5n^sTk#J1cXsWYNGk}u7&2O8Sdcn*3b zS({LxGA7}Ggyyz_A#xd}eMHHfi;)>A(Qde2I{#ka>Fy3XJZ0zSQ zN;aW#ky*F{WNz&cb(JRwfn-ZPCh@k&U20B`+&R8!Kq}AKVb0a5>G_8Dk1Rj(9tTkh z;e_8xPr2(%8sDRW?(|Y5i!!cJs3PCX$hVXAD6&zeMu0RxEs9*}7IupCSGV)Fc#+gq zegjb^37RL`FWg=&ExR`j376=gPJ5(uj&{6Cq}(zuf!l$6a(V?vOlYp_WVbhm)<*py zr0^_C$hCl(UEPzq?(`;>SI=Jqs3Y%8t*_;yZb@*K5cB&Fzm$9t1=xK~PU&1&<} z$xPMDOrUJT?>m*43R{g*fkb*7kCJD9bybYp33n3baN?n5Wg>X|sp=m`vQqnSxVe9aS>BMoNd7n;Fekx}Ik* zE3s-u7(~!SIjdxKr3WFEwNQ(BWRpm+q=TT)Z@{0Qfj_F0`BXiKsgA{^tvQMds4n_+1dZiG2=wJ=f|2zMcbUBX;*Y ztI$`L$w^jp8#wUhTPLe``hIBuaUW}&iA+`;!ds-FYc{N)-)><)n&+2?=2!|h~kkM z)iahIe`CsI+b%NKZ`~%RSkO%)Yf0u-eH8w87)kv)99{^t~WPEY|p>yqW#)84{o`zLG}BAEU1(CzFu!h9`#| zRkC6Lu7+i2civONy&u0Te7RvWcj`7qTyg*VXFCG!0J5`+NL-qSmy7tK_2*dECq*A! zJ3k#Z2qZ=DkCWn`A(@1gE3#yKa zwvAYULkh*QX9b_&>!{1FJ!!b+o)~TFV1llI`*G{~MPT*`^j^os#gK3iLu=V9g&ZLx zeZd%O5j`X_LrAYwUUAK_;&Zy)&`3k}bS@s9X(f{DL)Cu6aM4e!G~mc%62B*)+_PWc zYtpdan$mO)!Ovuq{U@f)=;JIyb{$shg@dM5n{o3K19)^Q>?PJUa$Vd9`?1F5O4wL5 zDeaHh1_H1nza}azF;8NJu5hL88R=>MR-c67jXyXJbVFAU+>foKTl(FjP0?^RmdN zVJoR~U=sK|I{2NNewK5h&A^qCh0m@m84mizCogbx-|(4MKi=`TsBI$27~!>~8p#;) z_wQdrgv3{yLaj6uaU>q=xY{q7YtP>H$W>H4$$GQ-FcJs2NQv7#(NaH+X~9v>9ND${VX3G$r%%$lEOq>xazo)vn8XJ0=nHA z`qDF#TvRJSm5?xKVN7M>v1G7*iiSxI10fz}e?ehk0TW!l$v=h3)%7i-=nDdIg{W;N zUaKJk18tPx{Q^LtxLv8jw6CUmXQYu!^MNb$B7d7;vdGJ{u5CW7TI5+U>yR;-V@>w} za}y(p4Py=RF_Nw;vf0fK$Ko;DNws-g5ywCgRiK(($+s6Z@xpgpaBC%{c>4yxgU-5F z$M|`-{rOq4cnsap(=?1+WffLfPK8DM29H+Qf|541lqWDA8zq^YrrFPtqP>=@Y_QKm z{s^MB>xJDIf=d@)97EuGYiGN)*8jJCBjKdx50WB_ETL`K0yN{zj+%2`Zi_cr+=4Vi z{jvl7Y6Ehv)6e$UiF_fHf^K~TdDnE0C9ts2aKApVQ(*qqSQas1g;hcX{IVYD7c_zf zlOQuzECmc=u<9~-Be8x7ILO>4(4E}gI!}ugaY(BRisa-y@d|`Z=&a{o-Wfw5{0(>E zO3k;(k$xq~t~Gy@l;2CgAf4BK@{~C*V0^uPtRqa|i=d#xFa003N{9t2XhYcY7YYU8 z^XB$`@j`*mdeHYA!6#9R9D8R$Bv0d=*ww!|e-Dn6ZHbnhtLjUh zy-MpZ1DLHywZFw~?_+I0y4^aUM)KMOlUyi1CKmG6yD?PQ64J%S>YCB~| zTdHMls4)vDemm!u5PADqX2q*j0|2*t1-4B?KhxlP=lg9^DdX?n-7~fqIM-VXUzs(F z=v4q}it~@H<&Uc%(;v4eggHAe+B-x7J2?;$%kosv^2drZvk6`;ksv`Rm&wBkWU8*c zhVlm?L6blsV2&O(YB#zunpM!@=etDvvAXi^w^{e~1@EPWMem(Y-oL$%U?8JCMP4Ld zshUl}D*-Xiln+9j)X;KRhVNjdfUbv>*J$(c9PxY~f%uX$j)62$}_ljGz zTqXo3!rS7y$epQK_I|5AI>`%WXq-%s^~D5ZR%H$!BS(C?pC39<9|bSV;!D2STZxZO zrE?(F4!3?T!CcB{W%az9R2R`UtK?68Pr6w*n35D|Q3<}qFDpB&^rO3J18-rj0@21n z3)EE^*<`LiNoSL_YJ8+-sL*E1DGzX^Lm#z%%R*Dhm}aRJm}ET*x6DO7M8)dLHEqR# zeHzm_c*cv&$%F;i3>Fp(pT?1?ac%E1jr_jAhjda5GHvz8k=1_1JMi}%hw7Gdz#@blqI!33V9NTvW6qIy=BC=abeU}@X&sNkujQbptIsJ z3sgs6(gnxGqbW2Q<5X1d1TQ2Nk@W~?5ZU|wPG@Kcb4QcH;AD_R`bSM!X>ml==0Y={ zn6;mbcik~k-)M<}DNNbfGkQ7xQAxbf+i5CdPB@r#SxM#fV0=$(X<4CXOz!Lb(m*-& z`SI5w`7m^u412q9LfzCDUJo!eK&}6&Xfxz--q)TX5co;r&9OVVEk&U8Io%*e4EYhQ(aI5 zh1@6|SK_4tHQ4g*SxIz~y=veoHP+~gSYdxp?P?lbz_>2A*79z6ZLKhS$of+rHh30> z9Se&6Yva=JlcPlHg$a0dBQQpQZ~EnPlCtgOI~q04sZ!;K)$1fX69-EJgNra&M?5mm zBI@11$a}Z98lzi0$P}V>k`05rC1sj{S%OG&+jj2H38VD~Li(Cz0tsph)%>~x<;D;^ zs>xZHeMXDAWa0FqZIMVxp0!Dvp5lCifv7z9!31B8(np?2ro9uCF+h`@M-HpIdL8eQ z7BW@=Ony!{^M;JMP_SgwD_uc)>1R_&!!E%b-_c<{l%*h1rhShStE10FG|7$3AU6v5 z^zE0vQ~I++%xuzP$EjW?w*aLK_Bc{ENkmPOc%7%=DV*Kub0OQiES*?O)8+W}=>||N=HG_7Kz9n(4SSkD6@h$e12M>?HyH(pX zb$zIa0#eQt4Zs0kG&zESP~E-Tbc(ifNriZen~~`WO%rTMbY zk`8@XKwc|niCYlLa5e>H!hc}rT%cP+?!oz5Jr(mKe|(63dx9G=`7?aX~Weyu(e#)Xlq*u_Q{wIYjD= zAq2MloE~e6%+H97Tv7Rp^0yXcd4GpF!Lwgv!V{*O>Q(NdLnEtnerZyI6)Sea5l?4n;m9`mrTB(tSr>Sevn`G9 zeuq%bF6}b@!Z9TclKL8i)Xh7dHQVEFmrZakKrHp1unqDY)3mF*@WfWueEG#vJWSw7 zY1-?cjayM^dJdA#XKWhLeXRZR4dJ2{#JKNAKCl;Xj^pkztypzVt@jO9ptOI=H!!8e zwZ2DY$PSK*LvNeXgaFYZkB;EsTzhUYb)+`sXZ^g4pXSp0Gq%BIkk-g>-IMdPmoHJ? zdtT2I91_Q@$K6lg3niW8usw->m1Y^K1@>bLaZBspHEM+&HnaKY)l@{q|5h`;kT`9# z6~D)mu8dk5AcdOb0Xcy3W_HiUCC2rsPx;WVcJ1oKe|IINYN(*0gF*jMsC90{VMG`o zl>Yw`gs%dDf2V%LG9MZApBTdbt-i@OuHM%YvbB|?ovXX`mF$Ivjt}~e+>GGkr$r+} z!1(E}5pdqt%>6x zW`zm4u8DuYmHtVjg#6vhE8;!eYr=J`iI5hdAP#5#M?DzfEkbtF=-*f7b=3J+y3PIv Z$()@UF)D&fOa>wY-MgA1CBAFu{{WAdHK_mq delta 6594 zcmaJ`1z1$w)~2OnXaF$>9?xDt!1_5D^ZX^Yyr8}g%Q;-lr8A&OB@Vo#0 zF5i9re?4cOy=I?xt+UR~wciuwy6uXiu7r$2gn*8YjxZP^k$}SqK>}e*pDW%(TtU%X z4u`^pFhEsu&M&o{qZ7HV#VMb3S}Z2tkPx-MiFIS*{|+c7Z&>5}1n?mq-JChaOk!J) z&HaMN5u#ls^~@TqaG0uK9`Dbi65;#P<@B+P&}L&FhQRty(ro{b#JbU!=^M}r&7Dc& zxddMbU#+ZMUO#BdJf`2|l3I;`ChpqEb z)Or*3ApgOMl6^!%b_$Syol-o8+?5Z4x(gM-($$&U!P3JS^dF)96Nmc%XYOJ7|6s}g z3kx=PcCxmz_i}|f0q z3p796$hDQ^a55g~slffGXbQIRkc}kV>k_66QoP6<+*WH%p1#vpDfJId*~0y<2|`1e zyi{cg&&FCivcq<)M3+{G%~?y%^WTraWam=``Kv+;dHsv_-lyg>qlUB=+kaGOI3w$e z9pC8VPW^>%zO$D_gKb79?Fykj#L4UvtH9oUlO^LPH|ZU^+^4qjNZ&7hN{%BgwtmR3 zttchRaZ^|lE~>~mN)oeRV~5Jt>yBk~GGD&v{srUx+Nw;NANwR{Lzob>enq7jp(YvK zgBvhJ*LCUpREp!~e&V9jO#6E4At4|T-%ZU2s|xVJ9Qm*D34|F^6VjQ@0x0NFHH@A^ zNjMwSmknjq7mFf%hHV601PV`syRL4|VDZ#!5HP~W4d*B(Xe{GL<4i8_w)698cH-TU znhGY4_0#B(qU}0g1sPObwG3XHNr9}v%`UjT&QHcoBp^~bc#6A>eEXtrm2ur83-SFv zyBrvalecI!c*^54OL27FmNh`iZ1p8cI;U<_4#)S$I<>Tga%P1#dfB`F$UBWSsB;|K z5S*$nN{$lC6=gA}Xx;6?F7KcDN@2aIml4V(3Yc}TwYe%FT`1uUuHDwZleIDkzv5G7 z)@%gmDb!7`^-n%=$;aV)hnM{V=f% z%K*nX!WoHGO~faV*s^Ki2?Xqd?eUqp8KCZx=5@EX|l*%ovm!bO;)oAzj`$ zLCp%1UPTijMhq?_`oOB0z+o>iQp3OO|+Z#xLyWU3sg30gCZ`g|CaB%f}b(_F4&l$ACjC2#RZ`Lj^-zk+v+`SmJwZdMwHng%>|YRf$uJeY1+ zqptMBFxmPIz2$QO;yn%C(cE&4T29|Gegwhn(6)ZJ{xF zlQIMfGB6qVB}46<6|}E%3A|dc`?B(ug-1Y~KBN zyDs}EM8o6I`LnV-_qivu9b#MAA&Al7x-$jS$t8sbB-!4c!zrpx8J-kXGvv}ZedDlR{{-$LXr`tVJ%=HgL0 zv(#j&KIjV_gEu_adR-ZH69V}2OtURlmVX+J=QFN@Nh*GI8(95Z+%qU2g@hlZN!p0d z3t7Jut%klQ=yYyG0ybe8R1`2Jx_)dSK{^rOu33wGWJAnm)IKr_oM)o?%8r`1}S$|q`50kqp|5Os?e zTXtH$Vl`UX{ozhx^euF#T=a-!qy2n=iqWq*Mj&pt#ZG||z<94wxp-6yo*G_vihvp( z*j{vC)D<2i+%ipu%7)5;p_vy<@eRq4Nx|DcMpfNN^Qe4s<^pnD8v2Q$CUHa0;1@=b z-SSfTH5v218?xxv{ZICF*rrdKYvY#4Wylu}H${Bra5K?;fa54vwfXo52{fD^e!8PF zJ5orCt%r@0$x_o(5*H*FVf@ zqk~Q0G{~{n3}$aorT0AVc0Pyyj@wwRQeU0PnUzK}(&Y1D`wmA^X7)9yK45Jjj8rID zS69gY(d4^$pP57td(A?i5U$$@8~D$=0nB{y$``Hb^+2zDS&t^ z`0opy2oqkUC3#lv9A^l;o%RlT`?n zZlMI6$eg2PKL>ga8fEuI+t^OWQ)HbQcY|^ZLb8O_-5kcvv=<4y@{Hs>^t9nl1C%}{ zj*uoNnlNfadhYFRuhKFuH#WHI`s`PVy`{%oC@}PO7T*Xr!Lq`8&U;3yW4Lif`#%++axMELoIiMxh} zw%IA6H-sczsKzrODX+(1a+9G!)L8}O0*J%AebT+zVSnB4S@tQ6nCO+oIWs{}k=ZfF z2H#?Bct7`)Rg`0vn;HYO16WZaV(^m;6&?JmHD_{jcd0W3|FGb7=xQW z-Z(~AiZEZhc95u_4Fp(p5#b;Q8Eh*gEXAb|ao)<`MqXy;Bt)Lc<}AysGmRxqPm~{; zSLD%3`&gb_GlgqrkeQx;yQFr^9DDiVl;}Om8ofVz@3$b`Yea%ue5Uhfr};wknE%JmJb%PYV+oOlk-FK|>5zrpGAisg`|qy%Jaaf098V_ipZ zj4t~mj(wy66{9s8mkwxqzz2TT9}w#<%F;G=9Ln7`cco3)ogt`g3+yii&FdLfQ?-)ouQwu{{c?9{=X( zvt~y|yX?28_iN2j*4w8-cZsR#qyKNMp?`RG5@zLw3^ly2qQx-H1)+sJYtWK8U<+@s zw67WTcj(?h9w%qC@RKRkdwP#F! zl6F)+K7d|WvTenaf_444AN6Y zYdz|gz=qQ4+aVY=8wDh!i`a#n5q_(zc%BcXZCpuz?T(!-+#$7|LlaEkk+$bBS#W@_~X+n@ypl07#1*6pu$*dPM$PPg5Gk zsz`}Ssz?*jFay7*b}e>0i847CpJxiul(iapQp9QXvQ6f$6(pQR+Oknm)ljz0j{P}A zEL~i-b*~*!DPxkcj(3LXGTb6>RVwSXu1wS6C=s|ZosDa<)C{YEhs9u3BG)z+rpodC zs9@|C8I!tKV2J*3AQo1)jd2|4KdHi{+vu+T)CG^MIW2KQ=@PNJ~nBl7TOxv5&ipVjI)*7nRbJUc=ScUQLGxRvjaP}sO9 zq`lK@E%P93>1z~cgy%Tb;2Kgf9f9XX;w$bg2s<`nb65L2XRE30<>e{{=-8}|Vx(h0 zbpF#^?x?kx+8^Y{B;>Y7{M5(u@49P-5mw;y|= zx^9rxS?+&T2vAg}4+-4T!@SE_+!(f*YNQB%H%^{cFY#^BVMs7)FP6nDf4O`kU_`R^x>_vhIBEmx`bCs)^5f< zO#YShy_IsvQYbJSJEjtuuZz#0EI!94r+q^y1Um$PUX6ZbVbNj^U*!(26eo)<1^|RUZ4@eWgW7iVbqc( zq%Y&MBrg?L6)Xb#rv2{?MWo2)?-ENLI1Fy)boO>swZls$`VXlNtXkI4{2zlK+M^ z&L)~xQRAQheaM=+#}&>r3C#auGTP20aw+P5>iYRSKdaG>$ve21RPjVP>K1y-SPj9n zy~VrF19l4h0(N1~K!h0gyy+jR8D;_!zo&L#5Pl$O|6K;~7p;tlgo*HP>KQf;0wV6g zzJY*$kQh8bL;#GD=kcF^hx|Y!NkkZk2nd7lVE?I#;{hW1-dC})t1BTQ5h47WV%It6 z@6eIFqseZO5fDV~p7yu2CGsl#u~y~+k*e( zzwdeSzXb#MkTri`2dm}>(t;i!{^H9YqVD=Y$K=0I|7OZP?g;jW-SD4mrpY)(1wqg0#5zBk9H6nb{3=5Bndn4Raa* diff --git a/rktl_autonomy/YAMLEditor/src/com/jcrm1/arc/yamleditor/Main.java b/rktl_autonomy/YAMLEditor/src/com/purduearc/yamleditor/Main.java similarity index 99% rename from rktl_autonomy/YAMLEditor/src/com/jcrm1/arc/yamleditor/Main.java rename to rktl_autonomy/YAMLEditor/src/com/purduearc/yamleditor/Main.java index 5df4de958..67809ae9a 100644 --- a/rktl_autonomy/YAMLEditor/src/com/jcrm1/arc/yamleditor/Main.java +++ b/rktl_autonomy/YAMLEditor/src/com/purduearc/yamleditor/Main.java @@ -1,4 +1,4 @@ -package com.jcrm1.arc.yamleditor; +package com.purduearc.yamleditor; import java.awt.Rectangle; import java.awt.event.ActionEvent; From 44d3a859671f6fb215d78e4c073e0de15bbc6b9d Mon Sep 17 00:00:00 2001 From: Abuynits Date: Sun, 2 Oct 2022 18:52:51 +0000 Subject: [PATCH 15/43] added in car.py changes for sim --- rktl_sim/src/simulator/car.py | 87 +++++++++++++++++++++++++---------- rktl_sim/src/simulator/sim.py | 6 +-- 2 files changed, 66 insertions(+), 27 deletions(-) diff --git a/rktl_sim/src/simulator/car.py b/rktl_sim/src/simulator/car.py index 2a617c7bf..e065defca 100644 --- a/rktl_sim/src/simulator/car.py +++ b/rktl_sim/src/simulator/car.py @@ -10,12 +10,34 @@ 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,25 +45,12 @@ 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): # get current yaw angle - _, orient = self.getPose() + _, orient = self.get_pose() theta = p.getEulerFromQuaternion(orient)[2] if self.simulate_effort: @@ -85,7 +94,12 @@ def step(self, cmd, dt): controlMode=p.VELOCITY_CONTROL, forces=(5000, 5000, 5000)) - def getPose(self, noise=None): + 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) @@ -95,24 +109,49 @@ def getPose(self, noise=None): else: pos = np.random.normal(pos, noise['pos']) orient = np.random.normal(orient, noise['orient']) + print("position: pos:",pos, "quaternion",p.getQuaternionFromEuler(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) orient = link_state[1] linear, angular = link_state[6:8] heading = p.getEulerFromQuaternion(orient)[2] r_inv = np.array([[math.cos(heading), -math.sin(heading), 0.], [math.sin(heading), math.cos(heading), 0.], - [0., 0., 1.]], dtype=np.float) + [0., 0., 1.]], dtype=np.float) linear = r_inv @ linear - return (linear, angular) + print("velocity: linear:",linear, "angular",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)) + print("reseting car to new pose and orient") + 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). + """ + print("x",pos[0],"y",pos[1]) + print("x1",self.init_pos[0],"y1",self.init_pos[1]) + 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])) + print(val) + dist = math.sqrt(val) + return dist < self._LENGTH \ No newline at end of file diff --git a/rktl_sim/src/simulator/sim.py b/rktl_sim/src/simulator/sim.py index d0aa354c1..dc74ff6b1 100644 --- a/rktl_sim/src/simulator/sim.py +++ b/rktl_sim/src/simulator/sim.py @@ -218,9 +218,9 @@ def getCarPose(self, add_noise=False): # TODO: Provide translation from ARC IDs to Sim IDs if add_noise: - return cars[0].getPose(noise=self.car_noise) + return cars[0].get_pose(noise=self.car_noise) else: - return cars[0].getPose(noise=None) + return cars[0].get_pose(noise=None) def getCarVelocity(self): cars = list(self._cars.values()) @@ -228,7 +228,7 @@ def getCarVelocity(self): return None # TODO: Provide translation from ARC IDs to Sim IDs - return cars[0].getVelocity() + return cars[0].get_velocity() def getBallPose(self, add_noise=False): if self._ballID is None: From 69ca65ffe6c9707d583e023a450398b923c3d894 Mon Sep 17 00:00:00 2001 From: jcrm1 <52137472+jcrm1@users.noreply.github.com> Date: Sun, 2 Oct 2022 15:49:02 -0400 Subject: [PATCH 16/43] Make training accept multiple experiments --- rktl_autonomy/config/rocket_league.yaml | 4 ++-- rktl_autonomy/scripts/modular_train.py | 21 ++++++++++++------- rktl_autonomy/scripts/train_rocket_league.py | 15 ++++++------- .../src/rktl_autonomy/env_counter.py | 9 ++++++++ .../rktl_autonomy/rocket_league_interface.py | 11 +++++----- 5 files changed, 38 insertions(+), 22 deletions(-) create mode 100644 rktl_autonomy/src/rktl_autonomy/env_counter.py diff --git a/rktl_autonomy/config/rocket_league.yaml b/rktl_autonomy/config/rocket_league.yaml index 4b8d62d7e..436b55c05 100644 --- a/rktl_autonomy/config/rocket_league.yaml +++ b/rktl_autonomy/config/rocket_league.yaml @@ -6,9 +6,9 @@ reward: # # reward as a function of squared distance from the ball to the goal # goal_dist_sq: -1.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 index 4a72293b5..72c94df3f 100755 --- a/rktl_autonomy/scripts/modular_train.py +++ b/rktl_autonomy/scripts/modular_train.py @@ -1,9 +1,14 @@ from train_rocket_league import train - -if __name__ == '__main__' : - - chnage_params(ydgfydgsyufgf) - - train() - - +import yaml +from rktl_autonomy import EnvCounter +import sys +from pathlib import Path + +if __name__ == '__main__': + numEnvsAllowed = 24 + if len(sys.argv) == 2: + numEnvsAllowed = int(sys.argv[1]) + file = yaml.load(open(Path(__file__).parent.parent / "config" / "rocket_league.yaml"), Loader=yaml.FullLoader) + numGroups = len(file["reward"]["win"]) + for i in range(numGroups): + train(n_envs=numEnvsAllowed / numGroups, env_counter=EnvCounter()) diff --git a/rktl_autonomy/scripts/train_rocket_league.py b/rktl_autonomy/scripts/train_rocket_league.py index 5bf8974ac..9e7e64c9e 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 -def train(n_envs = 24, n_saves = 100, n_steps = 240000000) : + +def train(n_envs=24, n_saves=100, n_steps=240000000, env_counter=None): 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=n_envs, vec_env_cls=SubprocVecEnv) + env = make_vec_env(RocketLeagueInterface, env_kwargs={'run_id': run_id, 'env_counter': env_counter}, + n_envs=n_envs, vec_env_cls=SubprocVecEnv) model = PPO("MlpPolicy", env) @@ -42,7 +42,8 @@ def train(n_envs = 24, n_saves = 100, n_steps = 240000000) : # 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() \ No newline at end of file + train() 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..f4d7caa0d --- /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 + + 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 ad94d5a41..cff1b9d54 100755 --- a/rktl_autonomy/src/rktl_autonomy/rocket_league_interface.py +++ b/rktl_autonomy/src/rktl_autonomy/rocket_league_interface.py @@ -35,11 +35,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): + def __init__(self, eval=False, launch_file=['rktl_autonomy', 'rocket_league_train.launch'], launch_args=[], run_id=None, env_counter=None): super().__init__(node_name='rocket_league_agent', eval=eval, launch_file=launch_file, launch_args=launch_args, run_id=run_id) - - ## Constants + # Constants + self.instance_id = env_counter.get_current_counter() # Actions + env_counter.count_env() self._MIN_VELOCITY = -rospy.get_param('/cars/throttle/max_speed') self._MAX_VELOCITY = rospy.get_param('/cars/throttle/max_speed') self._MIN_CURVATURE = -tan(rospy.get_param('/cars/steering/max_throw')) / rospy.get_param('cars/length') @@ -75,8 +76,8 @@ def __init__(self, eval=False, launch_file=['rktl_autonomy', 'rocket_league_trai self._CONSTANT_REWARD = rospy.get_param('~reward/constant', 0.0) 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._WIN_REWARD = rospy.get_param('~reward/win', 100.0) - self._LOSS_REWARD = rospy.get_param('~reward/loss', 0.0) + self._WIN_REWARD = rospy.get_param('~reward/win', 100.0)[self.instance_id] + self._LOSS_REWARD = rospy.get_param('~reward/loss', 0.0)[self.instance_id] 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) From 1026379b9697142cc146ac6014027e17bd14bb86 Mon Sep 17 00:00:00 2001 From: jcrm1 <52137472+jcrm1@users.noreply.github.com> Date: Thu, 6 Oct 2022 19:38:10 -0400 Subject: [PATCH 17/43] Fix imports Export EnvCounter in rktl_autonomy --- rktl_autonomy/src/rktl_autonomy/__init__.py | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/rktl_autonomy/src/rktl_autonomy/__init__.py b/rktl_autonomy/src/rktl_autonomy/__init__.py index 1de5ecf94..ea25bc610 100644 --- a/rktl_autonomy/src/rktl_autonomy/__init__.py +++ b/rktl_autonomy/src/rktl_autonomy/__init__.py @@ -3,10 +3,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"] \ No newline at end of file + "RocketLeagueInterface", + "EnvCounter"] From bd2cb7f0475cffb504b0329cdafc5df659dba6ba Mon Sep 17 00:00:00 2001 From: jcrm1 <52137472+jcrm1@users.noreply.github.com> Date: Thu, 6 Oct 2022 19:57:52 -0400 Subject: [PATCH 18/43] Fix file loading --- rktl_autonomy/scripts/modular_train.py | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/rktl_autonomy/scripts/modular_train.py b/rktl_autonomy/scripts/modular_train.py index 72c94df3f..39baf8bbe 100755 --- a/rktl_autonomy/scripts/modular_train.py +++ b/rktl_autonomy/scripts/modular_train.py @@ -2,13 +2,17 @@ import yaml from rktl_autonomy import EnvCounter import sys +import os from pathlib import Path if __name__ == '__main__': numEnvsAllowed = 24 if len(sys.argv) == 2: numEnvsAllowed = int(sys.argv[1]) - file = yaml.load(open(Path(__file__).parent.parent / "config" / "rocket_league.yaml"), Loader=yaml.FullLoader) + configFolder = os.path.join(os.path.join(os.path.join(Path(__file__), os.pardir), os.pardir), Path("config")) + configFile = Path(os.path.abspath(os.path.join(configFolder, "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): - train(n_envs=numEnvsAllowed / numGroups, env_counter=EnvCounter()) + train(n_envs=int(numEnvsAllowed / numGroups), env_counter=EnvCounter()) From 1b00386999366ec2e709e68f9216b507a46c9e2b Mon Sep 17 00:00:00 2001 From: Abuynits Date: Sat, 8 Oct 2022 23:25:57 +0000 Subject: [PATCH 19/43] fixed get_ball_velocity --- rktl_sim/nodes/simulator | 2 +- rktl_sim/src/simulator/sim.py | 32 ++++----- rktl_sim/urdf/walls.urdf | 126 ++++++++++++++++++++++++++++++++++ 3 files changed, 143 insertions(+), 17 deletions(-) create mode 100644 rktl_sim/urdf/walls.urdf diff --git a/rktl_sim/nodes/simulator b/rktl_sim/nodes/simulator index 469e4b0e8..e3a083bc3 100755 --- a/rktl_sim/nodes/simulator +++ b/rktl_sim/nodes/simulator @@ -234,7 +234,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] diff --git a/rktl_sim/src/simulator/sim.py b/rktl_sim/src/simulator/sim.py index dc74ff6b1..979dacf6e 100644 --- a/rktl_sim/src/simulator/sim.py +++ b/rktl_sim/src/simulator/sim.py @@ -104,7 +104,7 @@ def __init__(self, urdf_paths, field_setup, spawn_bounds, render_enabled): self._walls[brBackwallID] = True self._cars = {} - self._ballID = None + self._ball_id = None self.touched_last = None self.scored = False @@ -126,10 +126,10 @@ def create_ball(self, urdf_name, init_pose=None, init_speed=None, noise=None): random.uniform(self.spawn_bounds[2][0], self.spawn_bounds[2][1]), ] self.initBallPos = None - self._ballID = p.loadURDF( + self._ball_id = p.loadURDF( self.urdf_paths[urdf_name], ballPos, zeroOrient) p.changeDynamics( - bodyUniqueId=self._ballID, + bodyUniqueId=self._ball_id, linkIndex=-1, restitution=0.7, linearDamping=0, @@ -145,9 +145,9 @@ def create_ball(self, urdf_name, init_pose=None, init_speed=None, noise=None): random.uniform(-self._speed_bound, self._speed_bound), 0.0, ] - p.resetBaseVelocity(self._ballID, ballVel, zeroOrient) + p.resetBaseVelocity(self._ball_id, ballVel, zeroOrient) self.ball_noise = noise - return self._ballID + return self._ball_id else: return None @@ -191,8 +191,8 @@ def create_car(self, urdf_name, init_pose=None, noise=None, props=None): 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) + if self._ball_id is not None: + ballContacts = p.getContactPoints(bodyA=self._ball_id) for contact in ballContacts: if contact[2] in self._cars: self.touchedLast = contact[2] @@ -231,9 +231,9 @@ def getCarVelocity(self): return cars[0].get_velocity() def getBallPose(self, add_noise=False): - if self._ballID is None: + 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']: @@ -242,18 +242,18 @@ def getBallPose(self, add_noise=False): pos = np.random.normal(pos, self.ball_noise['pos']) 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) + return p.getBaseVelocity(self._ball_id) def reset(self): self.scored = False self.winner = None self.touched_last = None - if self._ballID is not None: + if self._ball_id is not None: ballPos = self.initBallPos if ballPos is None: ballPos = [ @@ -262,7 +262,7 @@ def reset(self): 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, ballPos, p.getQuaternionFromEuler([0, 0, 0]) ) ballVel = [ @@ -270,7 +270,7 @@ def reset(self): random.uniform(-self._speed_bound, self._speed_bound), 0.0, ] - p.resetBaseVelocity(self._ballID, ballVel, [0, 0, 0]) + p.resetBaseVelocity(self._ball_id, ballVel, [0, 0, 0]) for car in self._cars.values(): carPos = self.initCarPos 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 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + From 735353f501f8b9352f5dd32cabfefedfac07ed06 Mon Sep 17 00:00:00 2001 From: Abuynits Date: Sat, 8 Oct 2022 23:28:33 +0000 Subject: [PATCH 20/43] fixed get_ball_pose --- rktl_sim/nodes/simulator | 4 ++-- rktl_sim/src/simulator/sim.py | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/rktl_sim/nodes/simulator b/rktl_sim/nodes/simulator index e3a083bc3..a032f5c07 100755 --- a/rktl_sim/nodes/simulator +++ b/rktl_sim/nodes/simulator @@ -193,7 +193,7 @@ class SimulatorROS(object): if self.mode == SimulatorMode.REALISTIC: # Publish ball sync pose - ball_pos, ball_quat = self.sim.getBallPose(add_noise=True) + ball_pos, ball_quat = self.sim.get_ball_pose(add_noise=True) if ball_pos is not None: ball_msg = PoseWithCovarianceStamped() ball_msg.header.stamp = now @@ -226,7 +226,7 @@ class SimulatorROS(object): 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] diff --git a/rktl_sim/src/simulator/sim.py b/rktl_sim/src/simulator/sim.py index 979dacf6e..327a776d4 100644 --- a/rktl_sim/src/simulator/sim.py +++ b/rktl_sim/src/simulator/sim.py @@ -229,8 +229,8 @@ def getCarVelocity(self): # TODO: Provide translation from ARC IDs to Sim IDs return cars[0].get_velocity() - - def getBallPose(self, add_noise=False): + 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._ball_id) From 5c13d0e1c6fbfb57a0be9e749235df81bfe5abe0 Mon Sep 17 00:00:00 2001 From: Abuynits Date: Sat, 8 Oct 2022 23:40:22 +0000 Subject: [PATCH 21/43] added function for configuring spawn bounds and field setup --- rktl_sim/nodes/simulator | 62 ++++++++++++++++++++++++---------------- 1 file changed, 38 insertions(+), 24 deletions(-) diff --git a/rktl_sim/nodes/simulator b/rktl_sim/nodes/simulator index a032f5c07..7c836fd4d 100755 --- a/rktl_sim/nodes/simulator +++ b/rktl_sim/nodes/simulator @@ -31,6 +31,16 @@ class SimulatorROS(object): """ROS wrapper for the simulator.""" 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 + rospy.init_node('simulator') mode = rospy.get_param('~mode') @@ -46,31 +56,8 @@ class SimulatorROS(object): 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) + spawn_bounds,field_setup=self.configure_field() - # 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] - - # 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]] urdf_paths = rospy.get_param('~urdf') for path in urdf_paths.values(): @@ -142,6 +129,33 @@ class SimulatorROS(object): rate.sleep() except rospy.ROSInterruptException: pass + def configure_field(self): + # 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] + + # 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]] + return spawn_bounds,field_setup def check_urdf(self, param): """Validates that URDF exists, then returns path.""" From 8817a798a61b36e98d7ae62490aafd51348144f0 Mon Sep 17 00:00:00 2001 From: Abuynits Date: Sat, 8 Oct 2022 23:45:46 +0000 Subject: [PATCH 22/43] brokne --- rktl_sim/nodes/simulator | 581 +++++++++++++++++++++++++--------- rktl_sim/src/simulator/sim.py | 418 ++++++++++++++---------- 2 files changed, 681 insertions(+), 318 deletions(-) diff --git a/rktl_sim/nodes/simulator b/rktl_sim/nodes/simulator index 7c836fd4d..cbc9bf387 100755 --- a/rktl_sim/nodes/simulator +++ b/rktl_sim/nodes/simulator @@ -4,13 +4,10 @@ License: BSD 3-Clause License Copyright (c) 2020, 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,16 +16,33 @@ from std_srvs.srv import Empty, EmptyResponse from threading import Lock from enum import Enum -# Local library +# local libraries import simulator +from rktl_sim.srv import CreateCar, CreateCarResponse, DeleteCar, DeleteCarResponse 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 -class SimulatorROS(object): - """ROS wrapper for the simulator.""" + 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. + + 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.""" @@ -40,10 +54,10 @@ class SimulatorROS(object): self.ball_init_pose = None self.ball_init_speed = None self.car_properties = None - - rospy.init_node('simulator') - mode = rospy.get_param('~mode') + # setting config parameters (stay constant for the whole simulator run) + rospy.init_node('simulator') + mode = self.get_sim_param('~mode') if mode == 'ideal': self.mode = SimulatorMode.IDEAL elif mode == 'realistic': @@ -51,148 +65,246 @@ 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) + 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) - spawn_bounds,field_setup=self.configure_field() - - - 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) - - # 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 - - self.sim.create_ball('ball', init_pose=ball_init_pose, - init_speed=ball_init_speed, noise=ball_noise) + self.props = { + 'engine': self.get_sim_param('~engine', secondParam=None), + 'dynamics': self.get_sim_param('~dynamics', secondParam=None), + } + + # prep the simulator for a new run, setting all instance parameters for the sim + + self.spawn_bounds = self.get_spawn_bounds() + 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)} - 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 + self.sim = simulator.Sim(self.props, self.urdf_paths, self.spawn_bounds, render_enabled) + print("done creating sim!") + self.sim.create_ball('ball', init_pose=self.ball_init_pose, + init_speed=self.ball_init_speed, noise=self.ball_noise) + print("done creating ball!") - noise = rospy.get_param('~car/sensor_noise', None) - if self.mode == SimulatorMode.IDEAL: - noise = None - - self.sim.create_car('car', - init_pose=rospy.get_param('~car/init_pose', None), - noise=noise, - props=car_properties) - - 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) - - # Node data + self.update_all_cars() + print("done creating cars!") self.cmd_lock = Lock() self.reset_lock = Lock() - self.car_cmd = (0.0, 0.0) self.last_time = None + print("calling reset cb!") + 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) + print("done creating publishers!") # Services rospy.Service('sim_reset', Empty, self.reset_cb) + rospy.Service('sim_reset_ball', Empty, self.reset_ball_cb) + rospy.Service('sim_create_car', CreateCar, self.create_car_cb) + rospy.Service('sim_delete_car', DeleteCar, self.delete_car_cb) + rospy.Service('sim_create_all_cars', Empty, self.create_all_cars_cb) + rospy.Service('sim_delete_all_cars', Empty, self.delete_all_cars_cb) + print("done creating services!") while not rospy.is_shutdown(): self.loop_once() try: rate.sleep() except rospy.ROSInterruptException: pass - def configure_field(self): - # 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] - - # 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]] - return spawn_bounds,field_setup - 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 urdf_path is None: + rospy.signal_shutdown('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 - if not os.path.isfile(param): + if not os.path.isfile(urdf_path): rospy.signal_shutdown( - 'no urdf file exists at path {}'.format(param)) + 'no urdf file exists at path {}'.format(urdf_path)) - def effort_cb(self, effort_msg): + 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 create_car_cb(self, req): + self.reset_lock.acquire() + if req.name in self.car_ids: + self.reset_lock.release() + return CreateCarResponse(False) + + # sim.py class handles the randomization + init_pose = { + "pos": None, + "orient": None + } + if req.reset_to_random_pose is False: + init_pose = { + "pos": [req.init_pos.x, + req.init_pos.y, + req.init_pos.z], + "orient": [0.0, 0.0, req.init_yaw], + } + car_param = [ + { + "name": req.name, + "init_pose": init_pose + } + ] + rospy.set_param('/cars/', car_param) + self.update_all_cars() + + # self.car_ids[req.name] = self.sim.create_car( + # 'car', init_pose, noise=self.car_noise, props=self.car_properties) + # + # # Subscribers + # car_id = self.car_ids[req.name] + # self.car_effort_subs[req.name] = rospy.Subscriber( + # f'/cars/{req.name}/effort', ControlEffort, + # self.effort_cb, callback_args=car_id) + # self.car_cmd_subs[req.name] = rospy.Subscriber( + # f'/cars/{req.name}/command', ControlCommand, + # self.cmd_cb, callback_args=car_id) + # + # if self.mode == SimulatorMode.REALISTIC: + # # Publishers + # self.car_pose_pubs[req.name] = rospy.Publisher( + # f'/cars/{req.name}/pose_sync_early', + # PoseWithCovarianceStamped, queue_size=1) + # self.car_odom_pubs[req.name] = rospy.Publisher( + # f'/cars/{req.name}/odom_truth', Odometry, queue_size=1) + # elif self.mode == SimulatorMode.IDEAL: + # self.car_odom_pubs[req.name] = rospy.Publisher( + # f'/cars/{req.name}/odom', Odometry, queue_size=1) + + self.reset_cb(None) # janky reset call with mandatory none parameter + self.reset_lock.release() + return CreateCarResponse(True) + + def delete_car_cb(self, req): + self.reset_lock.acquire() + if req.name not in self.car_ids: + self.reset_lock.release() + return DeleteCarResponse(False) + + # unsubscribe from the subscribers and publishers + if req.name in self.car_effort_subs: + self.car_effort_subs[req.name].unregister() + del self.car_effort_subs[req.name] + if req.name in self.car_cmd_subs: + self.car_cmd_subs[req.name].unregister() + del self.car_cmd_subs[req.name] + if req.name in self.car_odom_pubs: + self.car_odom_pubs[req.name].unregister() + del self.car_odom_pubs[req.name] + if req.name in self.car_pose_pubs: + self.car_pose_pubs[req.name].unregister() + del self.car_pose_pubs[req.name] + + # delete cars body from pybullet + res = self.sim.delete_car(self.car_ids[req.name]) + del self.car_ids[req.name] + + # delete car from rospy to prevent regeneration + rospy.delete_param(f'/cars/{req.name}') + + self.reset_lock.release() + + return DeleteCarResponse(res) + def reset_cb(self, _): - """Resets simulator.""" + print("inside reset CB!") self.reset_lock.acquire() - self.sim.reset() - self.car_cmd = (0.0, 0.0) + # setting sim parameters (can be modified by the user) + print("got spawn bounds!") + self.spawn_bounds = self.get_spawn_bounds() + + self.sensor_noise = self.get_sim_param('~sensor_noise', secondParam=None) + print("got sensor noise!") + 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) + print("set car sensor noise!") + self.reset_ball_cb(None) + print("reset the ball!!") + 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) + print("reset the sim!!!") + 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') + + # print(dir(self.sim)) + + 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() + print("looping now!") 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(delta_t) + # publish game status status = MatchStatus() if self.sim.scored: if self.sim.winner == "A": @@ -203,30 +315,26 @@ 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_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) - 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.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] @@ -234,9 +342,7 @@ 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 @@ -257,29 +363,196 @@ 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_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: + print(rospy.get_param(f'{path}',secondParam)) + return rospy.get_param(f'{path}',secondParam) + else: + print(rospy.get_param(f'{path}')) + 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: + return (float)(random.uniform(max_param, min_param)) + else: + return (float)(random.uniform(min_param, max_param)) + + 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 get_spawn_bounds(self): + fw = self.get_sim_param("/field/width") + fl = self.get_sim_param("/field/length") + spawn_height = self.get_sim_param('~spawn_height', secondParam=0.06) + wt = self.get_sim_param('/field/wall_thickness') + + spawn_bounds = [[-(fl / 2) + (2 * wt), (fl / 2) - (2 * wt)], + [-(fw / 2) + (2 * wt), (fw / 2) - (2 * wt)], + [spawn_height, spawn_height]] + return spawn_bounds + + def delete_all_cars_cb(self,_=None): + self.reset_lock.acquire() + self.delete_all_cars() + self.reset_cb() + self.reset_lock.release() + return DeleteCarResponse(True) + # no point in having it + def create_all_cars_cb(self,_=None): + self.reset_lock.acquire() + self.update_all_cars() + self.reset_cb() + self.reset_lock.release() + return CreateCarResponse(True) + + def delete_all_cars(self): + """Removes a car's publishers and subscribers and instance-parameters""" + car_configs = self.get_sim_param('~cars', secondParam=[]) + self.reset_lock.acquire() + for car_config in car_configs: + # shutdown if the car does not exit + 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 + # delete the car from the sim side + self.sim.delete_car(self.car_ids[car_name]) + + # delete car's subscribers + if car_name in self.car_effort_subs: + self.car_effort_subs[car_name].unregister() + del self.car_effort_subs[car_name] + if car_name in self.car_cmd_subs: + self.car_cmd_subs[car_name].unregister() + del self.car_cmd_subs[car_name] + # delete cars publishers + if car_name in self.car_odom_pubs: + self.car_odom_pubs[car_name].unregister() + del self.car_odom_pubs[car_name] + if car_name in self.car_pose_pubs: + self.car_pose_pubs[car_name].unregister() + del self.car_pose_pubs[car_name] + # reset all lists to be safe + self.car_ids = {} + self.car_pose_pubs = {} + self.car_odom_pubs = {} + self.car_effort_subs = {} + self.car_cmd_subs = {} + self.reset_lock.release() + + return DeleteCarResponse(True) + + def update_all_cars(self): + """Generates instance-parameters, Subscribers, Publishers for each car.""" + car_configs = self.get_sim_param('~cars', secondParam=[]) + print("=-=-=-=-=-=-=-=-=-=-=-=-=-=-=") + print(car_configs) + + 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 + print("============sub and pubs===================") + print(self.car_properties) + 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 CreateCarResponse(True) + + if __name__ == "__main__": - SimulatorROS() + Simulator() \ No newline at end of file diff --git a/rktl_sim/src/simulator/sim.py b/rktl_sim/src/simulator/sim.py index 327a776d4..d96e5913a 100644 --- a/rktl_sim/src/simulator/sim.py +++ b/rktl_sim/src/simulator/sim.py @@ -14,221 +14,268 @@ # 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, 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 if "plane" in urdf_paths: - self._planeID = p.loadURDF( - urdf_paths["plane"], [0, 0, 0], zeroOrient, useFixedBase=1 + self._plane_id = p.loadURDF( + urdf_paths["plane"], zero_pos, zero_orient, useFixedBase=1 ) - 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 + print(self.props) + self.configure_dynamics(self._plane_id, "floor") + else: + raise self.NoURDFError() + # set the walls for the simulation + if "walls" in urdf_paths: + self._walls_id = p.loadURDF( + urdf_paths["walls"], zero_pos, zero_orient, useFixedBase=1 ) - - self._goalBID = p.loadURDF( - urdf_paths["goal"], field_setup["goalB"], zeroOrient, useFixedBase=1 + self.configure_dynamics(self._walls_id, "walls") + else: + raise self.NoURDFError() + # set the goals for the simulation + self._goal_a_id = None + self._goal_b_id = None + if "goal_a" in urdf_paths and "goal_b" in urdf_paths: + self._goal_a_id = p.loadURDF( + urdf_paths["goal_a"], zero_pos, zero_orient, useFixedBase=1 ) - self._walls = {} - if "sidewall" in urdf_paths: - lSidewallID = p.loadURDF( - urdf_paths["sidewall"], - field_setup["lsidewall"], - zeroOrient, - useFixedBase=1, + self._goal_b_id = p.loadURDF( + urdf_paths["goal_b"], zero_pos, zero_orient, useFixedBase=1 ) - p.changeDynamics(bodyUniqueId=lSidewallID, linkIndex=-1, restitution=1.0) - self._walls[lSidewallID] = True - - rSidewallId = p.loadURDF( - urdf_paths["sidewall"], - field_setup["rsidewall"], - zeroOrient, - useFixedBase=1, - ) - p.changeDynamics(bodyUniqueId=rSidewallId, linkIndex=-1, restitution=1.0) - self._walls[rSidewallId] = True - - if "backwall" in urdf_paths: - # TODO: Improve handling of split walls - flBackwallID = p.loadURDF( - urdf_paths["backwall"], - field_setup["flbackwall"], - zeroOrient, - useFixedBase=1, - ) - p.changeDynamics(bodyUniqueId=flBackwallID, linkIndex=-1, restitution=1.0) - self._walls[flBackwallID] = True - - frBackwallID = p.loadURDF( - urdf_paths["backwall"], - field_setup["frbackwall"], - zeroOrient, - useFixedBase=1, - ) - p.changeDynamics(bodyUniqueId=frBackwallID, linkIndex=-1, restitution=1.0) - self._walls[frBackwallID] = True - - blBackwallID = p.loadURDF( - urdf_paths["backwall"], - field_setup["blbackwall"], - zeroOrient, - useFixedBase=1, - ) - p.changeDynamics(bodyUniqueId=blBackwallID, linkIndex=-1, restitution=1.0) - self._walls[blBackwallID] = True - - brBackwallID = p.loadURDF( - urdf_paths["backwall"], - field_setup["brbackwall"], - zeroOrient, - useFixedBase=1, - ) - p.changeDynamics(bodyUniqueId=brBackwallID, linkIndex=-1, restitution=1.0) - self._walls[brBackwallID] = True - + else: + raise self.NoURDFError() self._cars = {} + 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 = [ + 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.initBallPos = None + self.init_ball_pos = None self._ball_id = p.loadURDF( - self.urdf_paths[urdf_name], ballPos, zeroOrient) - p.changeDynamics( - bodyUniqueId=self._ball_id, - linkIndex=-1, - restitution=0.7, - linearDamping=0, - angularDamping=0, - rollingFriction=0.0001, - spinningFriction=0.001, - ) + self.urdf_paths[urdf_name], ball_pos, zero_orient) + self.configure_dynamics(self._ball_id, "ball") - # 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._ball_id, ballVel, zeroOrient) + # 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, + ] + print("\n\n\n\n\n\n\n\n\n\n==========here7================") + p.resetBaseVelocity(self._ball_id, ball_vel, zero_orient) self.ball_noise = noise 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 = [ + 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 step(self, car_cmd, dt): - """Advance one time-step in the sim.""" + 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 step(self, dt): + """ + 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. + """ if self._ball_id is not None: - ballContacts = p.getContactPoints(bodyA=self._ball_id) - for contact in ballContacts: + ball_contacts = p.getContactPoints(bodyA=self._ball_id) + 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_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].get_pose(noise=self.car_noise) + return self._cars[id].get_pose(noise=noise) else: - return cars[0].get_pose(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].get_velocity() + return self._cars[id].setCmd(cmd) + 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: @@ -242,47 +289,90 @@ def get_ball_pose(self, add_noise=False): pos = np.random.normal(pos, self.ball_noise['pos']) return pos, p.getQuaternionFromEuler([0, 0, 0]) - + def get_ball_velocity(self): if self._ball_id is None: return None return p.getBaseVelocity(self._ball_id) - def reset(self): + 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 + + 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): + print("\n\n\n\n\n\n\n\n\noverlap generation") + 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: - ballPos = self.initBallPos - if ballPos is None: - ballPos = [ + 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._ball_id, 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._ball_id, 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]) \ No newline at end of file From fb425af877072616eb63a00d4fd104e65ccafd43 Mon Sep 17 00:00:00 2001 From: Abuynits Date: Sun, 9 Oct 2022 02:58:51 +0000 Subject: [PATCH 23/43] broken --- rktl_sim/config/simulation.yaml | 83 ++++++++++++++++++++++++++++ rktl_sim/nodes/simulator | 33 ++++++++--- rktl_sim/src/simulator/sim.py | 98 +++++++++++++++++++++++++-------- 3 files changed, 181 insertions(+), 33 deletions(-) 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 cbc9bf387..69e441dd4 100755 --- a/rktl_sim/nodes/simulator +++ b/rktl_sim/nodes/simulator @@ -82,7 +82,7 @@ class Simulator(object): # prep the simulator for a new run, setting all instance parameters for the sim - self.spawn_bounds = self.get_spawn_bounds() + self.spawn_bounds,self.field_setup = self.configure_field() self.car_ids = {} self.car_pose_pubs = {} self.car_odom_pubs = {} @@ -96,7 +96,7 @@ class Simulator(object): 'steering_rate': self.get_sim_param("/cars/steering/rate"), 'simulate_effort': (self.mode == SimulatorMode.REALISTIC)} - self.sim = simulator.Sim(self.props, self.urdf_paths, self.spawn_bounds, render_enabled) + self.sim = simulator.Sim(self.props, self.urdf_paths, self.spawn_bounds, self.field_setup, render_enabled) print("done creating sim!") self.sim.create_ball('ball', init_pose=self.ball_init_pose, init_speed=self.ball_init_speed, noise=self.ball_noise) @@ -258,7 +258,7 @@ class Simulator(object): self.reset_lock.acquire() # setting sim parameters (can be modified by the user) print("got spawn bounds!") - self.spawn_bounds = self.get_spawn_bounds() + self.spawn_bounds,self.field_setup = self.configure_field() self.sensor_noise = self.get_sim_param('~sensor_noise', secondParam=None) print("got sensor noise!") @@ -446,16 +446,31 @@ class Simulator(object): - def get_spawn_bounds(self): - fw = self.get_sim_param("/field/width") - fl = self.get_sim_param("/field/length") - spawn_height = self.get_sim_param('~spawn_height', secondParam=0.06) - wt = self.get_sim_param('/field/wall_thickness') + def configure_field(self): + 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) spawn_bounds = [[-(fl / 2) + (2 * wt), (fl / 2) - (2 * wt)], [-(fw / 2) + (2 * wt), (fw / 2) - (2 * wt)], [spawn_height, spawn_height]] - return spawn_bounds + # 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 delete_all_cars_cb(self,_=None): self.reset_lock.acquire() diff --git a/rktl_sim/src/simulator/sim.py b/rktl_sim/src/simulator/sim.py index d96e5913a..c74ff8203 100644 --- a/rktl_sim/src/simulator/sim.py +++ b/rktl_sim/src/simulator/sim.py @@ -24,7 +24,7 @@ class Sim(object): class NoURDFError(Exception): pass - def __init__(self, props, urdf_paths, spawn_bounds, render_enabled): + 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. @@ -52,35 +52,85 @@ def __init__(self, props, urdf_paths, spawn_bounds, render_enabled): zero_pos = [0.0, 0.0, 0.0] zero_orient = p.getQuaternionFromEuler([0.0, 0.0, 0.0]) self._plane_id = None - if "plane" in urdf_paths: - self._plane_id = p.loadURDF( - urdf_paths["plane"], zero_pos, zero_orient, useFixedBase=1 - ) - print(self.props) - self.configure_dynamics(self._plane_id, "floor") - else: - raise self.NoURDFError() - # set the walls for the simulation - if "walls" in urdf_paths: - self._walls_id = p.loadURDF( - urdf_paths["walls"], zero_pos, zero_orient, useFixedBase=1 - ) - self.configure_dynamics(self._walls_id, "walls") - else: - raise self.NoURDFError() + # set the goals for the simulation self._goal_a_id = None self._goal_b_id = None - if "goal_a" in urdf_paths and "goal_b" in urdf_paths: - self._goal_a_id = p.loadURDF( - urdf_paths["goal_a"], zero_pos, zero_orient, useFixedBase=1 + if "plane" in urdf_paths: + self._planeID = p.loadURDF( + urdf_paths["plane"], [0, 0, 0], zero_orient, useFixedBase=1 ) + p.changeDynamics(bodyUniqueId=self._planeID, linkIndex=-1, restitution=1.0) - self._goal_b_id = p.loadURDF( - urdf_paths["goal_b"], zero_pos, zero_orient, useFixedBase=1 + self._goalAID = None + self._goalBID = None + if "goal" in urdf_paths: + self._goalAID = p.loadURDF( + urdf_paths["goal"], field_setup["goalA"], zero_orient, useFixedBase=1 ) - else: - raise self.NoURDFError() + + self._goalBID = p.loadURDF( + urdf_paths["goal"], field_setup["goalB"], zero_orient, useFixedBase=1 + ) + + self._walls = {} + if "sidewall" in urdf_paths: + lSidewallID = p.loadURDF( + urdf_paths["sidewall"], + field_setup["lsidewall"], + zero_orient, + useFixedBase=1, + ) + p.changeDynamics(bodyUniqueId=lSidewallID, linkIndex=-1, restitution=1.0) + self._walls[lSidewallID] = True + + rSidewallId = p.loadURDF( + urdf_paths["sidewall"], + field_setup["rsidewall"], + zero_orient, + useFixedBase=1, + ) + p.changeDynamics(bodyUniqueId=rSidewallId, linkIndex=-1, restitution=1.0) + self._walls[rSidewallId] = True + + if "backwall" in urdf_paths: + # TODO: Improve handling of split walls + flBackwallID = p.loadURDF( + urdf_paths["backwall"], + field_setup["flbackwall"], + zero_orient, + useFixedBase=1, + ) + p.changeDynamics(bodyUniqueId=flBackwallID, linkIndex=-1, restitution=1.0) + self._walls[flBackwallID] = True + + frBackwallID = p.loadURDF( + urdf_paths["backwall"], + field_setup["frbackwall"], + zero_orient, + useFixedBase=1, + ) + p.changeDynamics(bodyUniqueId=frBackwallID, linkIndex=-1, restitution=1.0) + self._walls[frBackwallID] = True + + blBackwallID = p.loadURDF( + urdf_paths["backwall"], + field_setup["blbackwall"], + zero_orient, + useFixedBase=1, + ) + p.changeDynamics(bodyUniqueId=blBackwallID, linkIndex=-1, restitution=1.0) + self._walls[blBackwallID] = True + + brBackwallID = p.loadURDF( + urdf_paths["backwall"], + field_setup["brbackwall"], + zero_orient, + useFixedBase=1, + ) + p.changeDynamics(bodyUniqueId=brBackwallID, linkIndex=-1, restitution=1.0) + self._walls[brBackwallID] = True + self._cars = {} self._car_data = {} self._ball_id = None From 96970bb06933f45be9045b8e3587c57d231f9ee0 Mon Sep 17 00:00:00 2001 From: Abuynits Date: Sun, 9 Oct 2022 03:20:17 +0000 Subject: [PATCH 24/43] no bugs? --- rktl_sim/nodes/simulator | 38 ++++++++++++++++++++++++++--------- rktl_sim/src/simulator/sim.py | 4 ++-- 2 files changed, 30 insertions(+), 12 deletions(-) diff --git a/rktl_sim/nodes/simulator b/rktl_sim/nodes/simulator index 69e441dd4..d7092e9f8 100755 --- a/rktl_sim/nodes/simulator +++ b/rktl_sim/nodes/simulator @@ -54,6 +54,7 @@ class Simulator(object): 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') @@ -64,6 +65,10 @@ class Simulator(object): self.mode = SimulatorMode.REALISTIC else: rospy.signal_shutdown('unknown sim mode set "{}"'.format(mode)) + + self.cmd_lock = Lock() + self.reset_lock = Lock() + self.car_cmd = (0.0, 0.0) render_enabled = self.get_sim_param('~render', secondParam=False) rate = rospy.Rate(self.get_sim_param('~rate', secondParam=30)) @@ -153,21 +158,34 @@ class Simulator(object): 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).""" + def effort_cb(self, effort_msg,v): + print("eff_cb:",effort_msg) + print("eff_cb:",v) self.cmd_lock.acquire() - self.sim.set_car_command(car_id, - (effort_msg.throttle, effort_msg.steering)) + self.car_cmd = (effort_msg.throttle, effort_msg.steering) self.cmd_lock.release() - def cmd_cb(self, cmd_msg, car_id): - """Sets a car's command with the velocity and curvature. (overwrites effort).""" + def cmd_cb(self, cmd_msg,v): + print("cmd_cb:",cmd_msg) + print("cmd_cb:",v) self.cmd_lock.acquire() - self.sim.set_car_command(car_id, - (cmd_msg.velocity, cmd_msg.curvature)) + self.car_cmd = (cmd_msg.velocity, cmd_msg.curvature) self.cmd_lock.release() + # def effort_cb(self, effort_msg, car_id): + # """Sets a car's effort with throttle and steering (overwrites commands).""" + # self.cmd_lock.acquire() + # self.sim.set_car_command(car_id, + # (effort_msg.throttle, effort_msg.steering)) + # self.cmd_lock.release() + + # 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.sim.set_car_command(car_id, + # (cmd_msg.velocity, cmd_msg.curvature)) + # self.cmd_lock.release() + def create_car_cb(self, req): self.reset_lock.acquire() if req.name in self.car_ids: @@ -302,7 +320,7 @@ class Simulator(object): now = rospy.Time.now() if self.last_time is not None and self.last_time != now: delta_t = (now - self.last_time).to_sec() - self.sim.step(delta_t) + self.sim.step(self.car_cmd,delta_t) # publish game status status = MatchStatus() diff --git a/rktl_sim/src/simulator/sim.py b/rktl_sim/src/simulator/sim.py index c74ff8203..9007c69e8 100644 --- a/rktl_sim/src/simulator/sim.py +++ b/rktl_sim/src/simulator/sim.py @@ -277,7 +277,7 @@ def delete_car(self, car_id): del self._car_data[car_id] return True - def step(self, dt): + def step(self, car_cmd, dt): """ 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. @@ -299,7 +299,7 @@ def step(self, dt): for _ in range(round(dt / p_dt)): # step kinematic objects independently, at max possible rate for car in self._cars.values(): - car.step(p_dt) + car.step(car_cmd,p_dt) p.stepSimulation() def get_car_pose(self, id, add_noise=False): From ebeb747308f6f97ef4a56dbc2e1f54d1637abbb0 Mon Sep 17 00:00:00 2001 From: Abuynits Date: Sun, 9 Oct 2022 03:33:16 +0000 Subject: [PATCH 25/43] added function for reseting the car cmd --- rktl_sim/nodes/simulator | 44 +++++++++++++++++------------------ rktl_sim/src/simulator/car.py | 13 +++++++---- rktl_sim/src/simulator/sim.py | 8 ++++++- 3 files changed, 37 insertions(+), 28 deletions(-) diff --git a/rktl_sim/nodes/simulator b/rktl_sim/nodes/simulator index d7092e9f8..14ea4b027 100755 --- a/rktl_sim/nodes/simulator +++ b/rktl_sim/nodes/simulator @@ -158,34 +158,34 @@ class Simulator(object): 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,v): - print("eff_cb:",effort_msg) - print("eff_cb:",v) - self.cmd_lock.acquire() - self.car_cmd = (effort_msg.throttle, effort_msg.steering) - self.cmd_lock.release() - - def cmd_cb(self, cmd_msg,v): - print("cmd_cb:",cmd_msg) - print("cmd_cb:",v) - self.cmd_lock.acquire() - self.car_cmd = (cmd_msg.velocity, cmd_msg.curvature) - self.cmd_lock.release() - - # def effort_cb(self, effort_msg, car_id): - # """Sets a car's effort with throttle and steering (overwrites commands).""" + # def effort_cb(self, effort_msg,v): + # print("eff_cb:",effort_msg) + # print("eff_cb:",v) # self.cmd_lock.acquire() - # self.sim.set_car_command(car_id, - # (effort_msg.throttle, effort_msg.steering)) + # self.car_cmd = (effort_msg.throttle, effort_msg.steering) # self.cmd_lock.release() - # def cmd_cb(self, cmd_msg, car_id): - # """Sets a car's command with the velocity and curvature. (overwrites effort).""" + # def cmd_cb(self, cmd_msg,v): + # print("cmd_cb:",cmd_msg) + # print("cmd_cb:",v) # self.cmd_lock.acquire() - # self.sim.set_car_command(car_id, - # (cmd_msg.velocity, cmd_msg.curvature)) + # self.car_cmd = (cmd_msg.velocity, cmd_msg.curvature) # self.cmd_lock.release() + def effort_cb(self, effort_msg, car_id): + """Sets a car's effort with throttle and steering (overwrites commands).""" + self.cmd_lock.acquire() + self.sim.set_car_command(car_id, + (effort_msg.throttle, effort_msg.steering)) + self.cmd_lock.release() + + 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.sim.set_car_command(car_id, + (cmd_msg.velocity, cmd_msg.curvature)) + self.cmd_lock.release() + def create_car_cb(self, req): self.reset_lock.acquire() if req.name in self.car_ids: diff --git a/rktl_sim/src/simulator/car.py b/rktl_sim/src/simulator/car.py index e065defca..8b53069f2 100644 --- a/rktl_sim/src/simulator/car.py +++ b/rktl_sim/src/simulator/car.py @@ -48,15 +48,18 @@ def set_properties(self, car_properties): def setCmd(self, cmd): self.cmd = cmd - def step(self, cmd, dt): + def step(self, dt): # get current yaw angle _, 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 @@ -77,11 +80,11 @@ 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) diff --git a/rktl_sim/src/simulator/sim.py b/rktl_sim/src/simulator/sim.py index 9007c69e8..6651411dd 100644 --- a/rktl_sim/src/simulator/sim.py +++ b/rktl_sim/src/simulator/sim.py @@ -276,6 +276,12 @@ def delete_car(self, car_id): del self._cars[car_id] del self._car_data[car_id] return True + """ + Set the command for the car + """ + def set_car_command(car_id,msg): + cars[car_id].setCmd(msg) + def step(self, car_cmd, dt): """ @@ -299,7 +305,7 @@ def step(self, car_cmd, dt): for _ in range(round(dt / p_dt)): # 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 get_car_pose(self, id, add_noise=False): From 70dcd0247ad37a737fa617be553b73e08c9efbe3 Mon Sep 17 00:00:00 2001 From: Abuynits Date: Sun, 9 Oct 2022 03:44:50 +0000 Subject: [PATCH 26/43] cleaned up code --- rktl_sim/nodes/simulator | 37 ------------------------------------- 1 file changed, 37 deletions(-) diff --git a/rktl_sim/nodes/simulator b/rktl_sim/nodes/simulator index 14ea4b027..3fd05ab85 100755 --- a/rktl_sim/nodes/simulator +++ b/rktl_sim/nodes/simulator @@ -158,19 +158,6 @@ class Simulator(object): 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,v): - # print("eff_cb:",effort_msg) - # print("eff_cb:",v) - # self.cmd_lock.acquire() - # self.car_cmd = (effort_msg.throttle, effort_msg.steering) - # self.cmd_lock.release() - - # def cmd_cb(self, cmd_msg,v): - # print("cmd_cb:",cmd_msg) - # print("cmd_cb:",v) - # self.cmd_lock.acquire() - # self.car_cmd = (cmd_msg.velocity, cmd_msg.curvature) - # self.cmd_lock.release() def effort_cb(self, effort_msg, car_id): """Sets a car's effort with throttle and steering (overwrites commands).""" @@ -212,30 +199,6 @@ class Simulator(object): ] rospy.set_param('/cars/', car_param) self.update_all_cars() - - # self.car_ids[req.name] = self.sim.create_car( - # 'car', init_pose, noise=self.car_noise, props=self.car_properties) - # - # # Subscribers - # car_id = self.car_ids[req.name] - # self.car_effort_subs[req.name] = rospy.Subscriber( - # f'/cars/{req.name}/effort', ControlEffort, - # self.effort_cb, callback_args=car_id) - # self.car_cmd_subs[req.name] = rospy.Subscriber( - # f'/cars/{req.name}/command', ControlCommand, - # self.cmd_cb, callback_args=car_id) - # - # if self.mode == SimulatorMode.REALISTIC: - # # Publishers - # self.car_pose_pubs[req.name] = rospy.Publisher( - # f'/cars/{req.name}/pose_sync_early', - # PoseWithCovarianceStamped, queue_size=1) - # self.car_odom_pubs[req.name] = rospy.Publisher( - # f'/cars/{req.name}/odom_truth', Odometry, queue_size=1) - # elif self.mode == SimulatorMode.IDEAL: - # self.car_odom_pubs[req.name] = rospy.Publisher( - # f'/cars/{req.name}/odom', Odometry, queue_size=1) - self.reset_cb(None) # janky reset call with mandatory none parameter self.reset_lock.release() return CreateCarResponse(True) From 590f9950ab1ca701d116870c360b9868e7f15b1d Mon Sep 17 00:00:00 2001 From: Abuynits Date: Tue, 11 Oct 2022 20:12:22 +0000 Subject: [PATCH 27/43] added control gui --- rktl_launch/launch/rocket_league_sim.launch | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/rktl_launch/launch/rocket_league_sim.launch b/rktl_launch/launch/rocket_league_sim.launch index 00a975c97..73cef80a5 100644 --- a/rktl_launch/launch/rocket_league_sim.launch +++ b/rktl_launch/launch/rocket_league_sim.launch @@ -24,6 +24,10 @@ + + + + From a6adcca3b3d4d5861207b12cc3eb9dc0b00ca743 Mon Sep 17 00:00:00 2001 From: Abuynits Date: Sun, 16 Oct 2022 18:41:53 +0000 Subject: [PATCH 28/43] removed broken callback functions and debug print statements --- rktl_sim/nodes/simulator | 159 +++++----------------------------- rktl_sim/src/simulator/car.py | 10 +-- rktl_sim/src/simulator/sim.py | 3 +- 3 files changed, 25 insertions(+), 147 deletions(-) diff --git a/rktl_sim/nodes/simulator b/rktl_sim/nodes/simulator index 3fd05ab85..c75fb93fa 100755 --- a/rktl_sim/nodes/simulator +++ b/rktl_sim/nodes/simulator @@ -102,17 +102,17 @@ class Simulator(object): 'simulate_effort': (self.mode == SimulatorMode.REALISTIC)} self.sim = simulator.Sim(self.props, self.urdf_paths, self.spawn_bounds, self.field_setup, render_enabled) - print("done creating sim!") + self.sim.create_ball('ball', init_pose=self.ball_init_pose, init_speed=self.ball_init_speed, noise=self.ball_noise) - print("done creating ball!") + self.update_all_cars() - print("done creating cars!") + self.cmd_lock = Lock() self.reset_lock = Lock() self.last_time = None - print("calling reset cb!") + self.reset_cb(None) # janky reset call with mandatory none parameter # Publishers @@ -127,16 +127,12 @@ class Simulator(object): elif self.mode == SimulatorMode.IDEAL: self.ball_odom_pub = rospy.Publisher('/ball/odom', Odometry, queue_size=1) - print("done creating publishers!") + # Services - rospy.Service('sim_reset', Empty, self.reset_cb) + rospy.Service('sim_reset_car_&_ball', Empty, self.reset_cb) rospy.Service('sim_reset_ball', Empty, self.reset_ball_cb) - rospy.Service('sim_create_car', CreateCar, self.create_car_cb) - rospy.Service('sim_delete_car', DeleteCar, self.delete_car_cb) - rospy.Service('sim_create_all_cars', Empty, self.create_all_cars_cb) - rospy.Service('sim_delete_all_cars', Empty, self.delete_all_cars_cb) + - print("done creating services!") while not rospy.is_shutdown(): self.loop_once() try: @@ -162,6 +158,7 @@ class Simulator(object): def effort_cb(self, effort_msg, car_id): """Sets a car's effort with throttle and steering (overwrites commands).""" self.cmd_lock.acquire() + self.sim.set_car_command(car_id, (effort_msg.throttle, effort_msg.steering)) self.cmd_lock.release() @@ -172,83 +169,22 @@ class Simulator(object): self.sim.set_car_command(car_id, (cmd_msg.velocity, cmd_msg.curvature)) self.cmd_lock.release() + + def reset_cb(self, _): - def create_car_cb(self, req): - self.reset_lock.acquire() - if req.name in self.car_ids: - self.reset_lock.release() - return CreateCarResponse(False) - - # sim.py class handles the randomization - init_pose = { - "pos": None, - "orient": None - } - if req.reset_to_random_pose is False: - init_pose = { - "pos": [req.init_pos.x, - req.init_pos.y, - req.init_pos.z], - "orient": [0.0, 0.0, req.init_yaw], - } - car_param = [ - { - "name": req.name, - "init_pose": init_pose - } - ] - rospy.set_param('/cars/', car_param) - self.update_all_cars() - self.reset_cb(None) # janky reset call with mandatory none parameter - self.reset_lock.release() - return CreateCarResponse(True) - - def delete_car_cb(self, req): self.reset_lock.acquire() - if req.name not in self.car_ids: - self.reset_lock.release() - return DeleteCarResponse(False) - - # unsubscribe from the subscribers and publishers - if req.name in self.car_effort_subs: - self.car_effort_subs[req.name].unregister() - del self.car_effort_subs[req.name] - if req.name in self.car_cmd_subs: - self.car_cmd_subs[req.name].unregister() - del self.car_cmd_subs[req.name] - if req.name in self.car_odom_pubs: - self.car_odom_pubs[req.name].unregister() - del self.car_odom_pubs[req.name] - if req.name in self.car_pose_pubs: - self.car_pose_pubs[req.name].unregister() - del self.car_pose_pubs[req.name] - - # delete cars body from pybullet - res = self.sim.delete_car(self.car_ids[req.name]) - del self.car_ids[req.name] - - # delete car from rospy to prevent regeneration - rospy.delete_param(f'/cars/{req.name}') - - self.reset_lock.release() - - return DeleteCarResponse(res) - def reset_cb(self, _): - print("inside reset CB!") - self.reset_lock.acquire() # setting sim parameters (can be modified by the user) - print("got spawn bounds!") self.spawn_bounds,self.field_setup = self.configure_field() self.sensor_noise = self.get_sim_param('~sensor_noise', secondParam=None) - print("got sensor noise!") + 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) - print("set car sensor noise!") + self.reset_ball_cb(None) - print("reset the ball!!") + 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"), @@ -256,7 +192,7 @@ class Simulator(object): '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) - print("reset the sim!!!") + self.last_time = None self.reset_lock.release() @@ -270,8 +206,6 @@ class Simulator(object): self.ball_init_pose = self.get_sim_param('~ball/init_pose') self.ball_init_speed = self.get_sim_param('/ball/init_speed') - - # print(dir(self.sim)) self.sim.reset_ball() return EmptyResponse() @@ -279,7 +213,7 @@ class Simulator(object): def loop_once(self): """Step the simulation once step, updating match status, moving and publishing new car and ball positions.""" self.reset_lock.acquire() - print("looping now!") + now = rospy.Time.now() if self.last_time is not None and self.last_time != now: delta_t = (now - self.last_time).to_sec() @@ -312,6 +246,7 @@ class Simulator(object): 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 @@ -385,10 +320,10 @@ class Simulator(object): else: if '~' in path: if secondParam is not None: - print(rospy.get_param(f'{path}',secondParam)) + return rospy.get_param(f'{path}',secondParam) else: - print(rospy.get_param(f'{path}')) + return rospy.get_param(f'{path}') type_rospy = type(rospy_param) @@ -453,63 +388,10 @@ class Simulator(object): return spawn_bounds,field_setup - def delete_all_cars_cb(self,_=None): - self.reset_lock.acquire() - self.delete_all_cars() - self.reset_cb() - self.reset_lock.release() - return DeleteCarResponse(True) - # no point in having it - def create_all_cars_cb(self,_=None): - self.reset_lock.acquire() - self.update_all_cars() - self.reset_cb() - self.reset_lock.release() - return CreateCarResponse(True) - - def delete_all_cars(self): - """Removes a car's publishers and subscribers and instance-parameters""" - car_configs = self.get_sim_param('~cars', secondParam=[]) - self.reset_lock.acquire() - for car_config in car_configs: - # shutdown if the car does not exit - 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 - # delete the car from the sim side - self.sim.delete_car(self.car_ids[car_name]) - - # delete car's subscribers - if car_name in self.car_effort_subs: - self.car_effort_subs[car_name].unregister() - del self.car_effort_subs[car_name] - if car_name in self.car_cmd_subs: - self.car_cmd_subs[car_name].unregister() - del self.car_cmd_subs[car_name] - # delete cars publishers - if car_name in self.car_odom_pubs: - self.car_odom_pubs[car_name].unregister() - del self.car_odom_pubs[car_name] - if car_name in self.car_pose_pubs: - self.car_pose_pubs[car_name].unregister() - del self.car_pose_pubs[car_name] - # reset all lists to be safe - self.car_ids = {} - self.car_pose_pubs = {} - self.car_odom_pubs = {} - self.car_effort_subs = {} - self.car_cmd_subs = {} - self.reset_lock.release() - - return DeleteCarResponse(True) - def update_all_cars(self): """Generates instance-parameters, Subscribers, Publishers for each car.""" car_configs = self.get_sim_param('~cars', secondParam=[]) - print("=-=-=-=-=-=-=-=-=-=-=-=-=-=-=") - print(car_configs) + for car_config in car_configs: init_pose = self.get_sim_param('~cars/init_pose') @@ -524,8 +406,7 @@ class Simulator(object): 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 - print("============sub and pubs===================") - print(self.car_properties) + 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] diff --git a/rktl_sim/src/simulator/car.py b/rktl_sim/src/simulator/car.py index 8b53069f2..9d10ba4ad 100644 --- a/rktl_sim/src/simulator/car.py +++ b/rktl_sim/src/simulator/car.py @@ -112,7 +112,7 @@ def get_pose(self, noise=None): else: pos = np.random.normal(pos, noise['pos']) orient = np.random.normal(orient, noise['orient']) - print("position: pos:",pos, "quaternion",p.getQuaternionFromEuler(orient)) + return pos, p.getQuaternionFromEuler(orient) def get_velocity(self): @@ -126,7 +126,7 @@ def get_velocity(self): [math.sin(heading), math.cos(heading), 0.], [0., 0., 1.]], dtype=np.float) linear = r_inv @ linear - print("velocity: linear:",linear, "angular",angular) + return linear, angular def reset(self, pos, orient): @@ -138,7 +138,7 @@ def reset(self, pos, orient): self._psi = 0.0 p.resetBasePositionAndOrientation(self.id, [0., 0., pos[2]], p.getQuaternionFromEuler(BASE_QUATERNION)) - print("reseting car to new pose and orient") + 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]) @@ -152,9 +152,7 @@ def check_overlap(self, pos): @param pos: The position of the other object. @return: Boolean if the positions overlap (true = overlap). """ - print("x",pos[0],"y",pos[1]) - print("x1",self.init_pos[0],"y1",self.init_pos[1]) + 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])) - print(val) dist = math.sqrt(val) return dist < self._LENGTH \ No newline at end of file diff --git a/rktl_sim/src/simulator/sim.py b/rktl_sim/src/simulator/sim.py index 6651411dd..f27be4037 100644 --- a/rktl_sim/src/simulator/sim.py +++ b/rktl_sim/src/simulator/sim.py @@ -198,7 +198,7 @@ def create_ball(self, urdf_name, init_pose=None, init_speed=None, random.uniform(-self._speed_bound, self._speed_bound), 0.0, ] - print("\n\n\n\n\n\n\n\n\n\n==========here7================") + p.resetBaseVelocity(self._ball_id, ball_vel, zero_orient) self.ball_noise = noise return self._ball_id @@ -383,7 +383,6 @@ def reset_car(self, car, car_properties): car_pos = self.generate_new_car_pos() while self.check_if_pos_overlap(car_pos): - print("\n\n\n\n\n\n\n\n\noverlap generation") car_pos = self.generate_new_car_pos() if car_orient is None: From 03acfc426a4b0d3154bd2316f782c214b9510a29 Mon Sep 17 00:00:00 2001 From: Abuynits Date: Sun, 16 Oct 2022 18:48:12 +0000 Subject: [PATCH 29/43] add method descriptions and formated code --- rktl_sim/nodes/simulator | 67 ++++++++++++++-------------- rktl_sim/src/simulator/car.py | 33 ++++++++------ rktl_sim/src/simulator/sim.py | 82 ++++++++++++++++++++--------------- 3 files changed, 102 insertions(+), 80 deletions(-) diff --git a/rktl_sim/nodes/simulator b/rktl_sim/nodes/simulator index c75fb93fa..3eb83fd10 100755 --- a/rktl_sim/nodes/simulator +++ b/rktl_sim/nodes/simulator @@ -54,7 +54,6 @@ class Simulator(object): 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') @@ -65,7 +64,7 @@ class Simulator(object): self.mode = SimulatorMode.REALISTIC else: rospy.signal_shutdown('unknown sim mode set "{}"'.format(mode)) - + self.cmd_lock = Lock() self.reset_lock = Lock() self.car_cmd = (0.0, 0.0) @@ -87,7 +86,7 @@ class Simulator(object): # prep the simulator for a new run, setting all instance parameters for the sim - self.spawn_bounds,self.field_setup = self.configure_field() + self.spawn_bounds, self.field_setup = self.configure_field() self.car_ids = {} self.car_pose_pubs = {} self.car_odom_pubs = {} @@ -100,20 +99,20 @@ class Simulator(object): '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 = simulator.Sim(self.props, self.urdf_paths, self.spawn_bounds, self.field_setup, render_enabled) + + self.sim = simulator.Sim( + self.props, self.urdf_paths, self.spawn_bounds, self.field_setup, render_enabled) 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() self.cmd_lock = Lock() self.reset_lock = Lock() self.last_time = None - self.reset_cb(None) # janky reset call with mandatory none parameter + self.reset_cb(None) # janky reset call with mandatory none parameter # Publishers self.status_pub = rospy.Publisher( @@ -131,7 +130,6 @@ class Simulator(object): # Services rospy.Service('sim_reset_car_&_ball', Empty, self.reset_cb) rospy.Service('sim_reset_ball', Empty, self.reset_ball_cb) - while not rospy.is_shutdown(): self.loop_once() @@ -144,7 +142,8 @@ class Simulator(object): """Validates that a URDF exists, then returns its file.""" if urdf_path is None: - rospy.signal_shutdown('no urdf path set for "{}"'.format(urdf_path)) + rospy.signal_shutdown( + 'no urdf path set for "{}"'.format(urdf_path)) i = 0 while not os.path.isfile(urdf_path) and i < 5: @@ -169,15 +168,16 @@ class Simulator(object): self.sim.set_car_command(car_id, (cmd_msg.velocity, cmd_msg.curvature)) self.cmd_lock.release() - + def reset_cb(self, _): self.reset_lock.acquire() # setting sim parameters (can be modified by the user) - self.spawn_bounds,self.field_setup = self.configure_field() + self.spawn_bounds, self.field_setup = self.configure_field() - self.sensor_noise = self.get_sim_param('~sensor_noise', secondParam=None) + 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: @@ -191,8 +191,8 @@ class Simulator(object): '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.sim.reset(self.spawn_bounds, self.car_properties, + self.ball_init_pose, self.ball_init_speed) self.last_time = None self.reset_lock.release() @@ -203,7 +203,7 @@ class Simulator(object): 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') @@ -217,7 +217,7 @@ class Simulator(object): now = rospy.Time.now() if self.last_time is not None and self.last_time != now: delta_t = (now - self.last_time).to_sec() - self.sim.step(self.car_cmd,delta_t) + self.sim.step(self.car_cmd, delta_t) # publish game status status = MatchStatus() @@ -250,7 +250,8 @@ class Simulator(object): 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_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] @@ -283,7 +284,8 @@ class Simulator(object): 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_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] @@ -291,7 +293,8 @@ class Simulator(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] - car_linear, car_angular = self.sim.get_car_velocity(self.car_ids[car_name]) + 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] @@ -312,7 +315,7 @@ class Simulator(object): False: An error is thrown if variable does not exist. @return: None or Exception. """ - rospy_param = rospy.get_param(path,secondParam) + rospy_param = rospy.get_param(path, secondParam) if not rospy_param: if returnValue: rospy.logfatal(f'invalid file path: {path}') @@ -321,21 +324,23 @@ class Simulator(object): if '~' in path: if secondParam is not None: - return rospy.get_param(f'{path}',secondParam) + 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)) + 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: @@ -360,9 +365,8 @@ class Simulator(object): 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 = rospy.get_param('/field/width') fl = rospy.get_param('/field/length') wt = rospy.get_param('/field/wall_thickness') @@ -386,12 +390,11 @@ class Simulator(object): 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 + 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') @@ -406,7 +409,7 @@ class Simulator(object): 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] @@ -427,9 +430,9 @@ class Simulator(object): elif self.mode == SimulatorMode.IDEAL: self.car_odom_pubs[car_name] = rospy.Publisher( f'/cars/{car_name}/odom', Odometry, queue_size=1) - + return CreateCarResponse(True) if __name__ == "__main__": - Simulator() \ No newline at end of file + Simulator() diff --git a/rktl_sim/src/simulator/car.py b/rktl_sim/src/simulator/car.py index 9d10ba4ad..ec1729284 100644 --- a/rktl_sim/src/simulator/car.py +++ b/rktl_sim/src/simulator/car.py @@ -14,6 +14,7 @@ JOINT_IDS = (1, 0, 2) # X, Y, W BASE_QUATERNION = [0., 0., 0.] + class Car(object): 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.""" @@ -33,7 +34,7 @@ def __init__(self, car_id, pos, orient, car_properties): self.simulate_effort = car_properties['simulate_effort'] self.set_properties(car_properties) - self.body_link_id = 1 # urdf configuration + self.body_link_id = 1 # urdf configuration self.reset(pos, orient) @@ -62,7 +63,8 @@ def step(self, dt): 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: @@ -86,16 +88,16 @@ def step(self, dt): 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)) + targetVelocities=(x_dot, y_dot, omega), + controlMode=p.VELOCITY_CONTROL, + forces=(5000, 5000, 5000)) def get_pose(self, noise=None): """ @@ -118,7 +120,8 @@ def get_pose(self, noise=None): def get_velocity(self): """Returns the linear and angular velocity of the car.""" - 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] @@ -126,7 +129,7 @@ def get_velocity(self): [math.sin(heading), math.cos(heading), 0.], [0., 0., 1.]], dtype=np.float) linear = r_inv @ linear - + return linear, angular def reset(self, pos, orient): @@ -137,8 +140,9 @@ def reset(self, pos, orient): self._v_rear = 0.0 self._psi = 0.0 - p.resetBasePositionAndOrientation(self.id, [0., 0., pos[2]], p.getQuaternionFromEuler(BASE_QUATERNION)) - + 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]) @@ -152,7 +156,8 @@ def check_overlap(self, pos): @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])) + + 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 \ No newline at end of file + return dist < self._LENGTH diff --git a/rktl_sim/src/simulator/sim.py b/rktl_sim/src/simulator/sim.py index f27be4037..cda935f7d 100644 --- a/rktl_sim/src/simulator/sim.py +++ b/rktl_sim/src/simulator/sim.py @@ -24,7 +24,7 @@ class Sim(object): class NoURDFError(Exception): pass - def __init__(self, props, urdf_paths, spawn_bounds, field_setup,render_enabled): + 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. @@ -60,7 +60,8 @@ def __init__(self, props, urdf_paths, spawn_bounds, field_setup,render_enabled): self._planeID = p.loadURDF( 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 @@ -81,7 +82,8 @@ def __init__(self, props, urdf_paths, spawn_bounds, field_setup,render_enabled): 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( @@ -90,7 +92,8 @@ def __init__(self, props, urdf_paths, spawn_bounds, field_setup,render_enabled): 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: @@ -101,7 +104,8 @@ def __init__(self, props, urdf_paths, spawn_bounds, field_setup,render_enabled): 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( @@ -110,7 +114,8 @@ def __init__(self, props, urdf_paths, spawn_bounds, field_setup,render_enabled): 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( @@ -119,7 +124,8 @@ def __init__(self, props, urdf_paths, spawn_bounds, field_setup,render_enabled): 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( @@ -128,7 +134,8 @@ def __init__(self, props, urdf_paths, spawn_bounds, field_setup,render_enabled): 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 = {} @@ -176,9 +183,12 @@ def create_ball(self, urdf_name, init_pose=None, init_speed=None, self.init_ball_pos = ball_pos else: 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]), + 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( @@ -236,9 +246,12 @@ def create_car(self, urdf_name, init_pose=None, noise=None, car_props=None): init_car_orient = car_orient else: 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]), + 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_orient = [0.0, 0.0, random.uniform(0, 2 * math.pi)] init_car_pos = None @@ -276,12 +289,12 @@ def delete_car(self, car_id): del self._cars[car_id] del self._car_data[car_id] return True - """ - Set the command for the car - """ - def set_car_command(car_id,msg): + + def set_car_command(car_id, msg): + """ + Set the command for the car + """ cars[car_id].setCmd(msg) - def step(self, car_cmd, dt): """ @@ -360,15 +373,16 @@ def reset(self, spawn_bounds, car_properties, ball_init_pose, ball_init_speed): 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 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 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). @@ -381,7 +395,7 @@ def reset_car(self, car, car_properties): 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() @@ -389,7 +403,6 @@ def reset_car(self, car, car_properties): 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. @@ -403,24 +416,25 @@ def check_if_pos_overlap(self, car_pos): 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])] + 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]), + 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._ball_id, ball_pos, p.getQuaternionFromEuler([0, 0, 0]) @@ -430,4 +444,4 @@ def reset_ball(self): random.uniform(-self._speed_bound, self._speed_bound), 0.0, ] - p.resetBaseVelocity(self._ball_id, ball_vel, [0, 0, 0]) \ No newline at end of file + p.resetBaseVelocity(self._ball_id, ball_vel, [0, 0, 0]) From 75ea8b78856d88712d23d1faa2c10b32ae541404 Mon Sep 17 00:00:00 2001 From: Ryan Jordan Date: Sun, 16 Oct 2022 15:15:05 -0400 Subject: [PATCH 30/43] Added multithreading for training --- rktl_autonomy/scripts/modular_train.py | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/rktl_autonomy/scripts/modular_train.py b/rktl_autonomy/scripts/modular_train.py index 39baf8bbe..ce8cbd697 100755 --- a/rktl_autonomy/scripts/modular_train.py +++ b/rktl_autonomy/scripts/modular_train.py @@ -4,6 +4,7 @@ import sys import os from pathlib import Path +from threading import Thread if __name__ == '__main__': numEnvsAllowed = 24 @@ -15,4 +16,6 @@ file = yaml.load(open(configFile), Loader=yaml.FullLoader) numGroups = len(file["reward"]["win"]) for i in range(numGroups): - train(n_envs=int(numEnvsAllowed / numGroups), env_counter=EnvCounter()) + args = (int(numEnvsAllowed / numGroups), 100, 240000000, EnvCounter()) + thread = Thread(target=train, args=args) + thread.start() From a257646b64c2e1febbfabee18a5e355f01444397 Mon Sep 17 00:00:00 2001 From: Abuynits Date: Fri, 21 Oct 2022 00:38:18 +0000 Subject: [PATCH 31/43] tested car randomization --- rktl_launch/config/global_params.yaml | 16 ++++++++++++---- rktl_sim/nodes/simulator | 27 ++++++++++++++++----------- 2 files changed, 28 insertions(+), 15 deletions(-) diff --git a/rktl_launch/config/global_params.yaml b/rktl_launch/config/global_params.yaml index a2b12ffeb..02fb6aa0e 100644 --- a/rktl_launch/config/global_params.yaml +++ b/rktl_launch/config/global_params.yaml @@ -2,8 +2,12 @@ # field dimensions field: - width: 3 - length: 4.25 + width: #3 + max: 3.5 + min: 2.5 + length: #4.25 + max: 5 + min: 3.5 wall_thickness: 0.25 goal: width: 1 @@ -14,9 +18,13 @@ ball: # car dimensions and physical constants cars: - length: 0.12 # front to rear wheel center to center, meters + 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 + 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_sim/nodes/simulator b/rktl_sim/nodes/simulator index 3eb83fd10..e0e8edb44 100755 --- a/rktl_sim/nodes/simulator +++ b/rktl_sim/nodes/simulator @@ -18,7 +18,7 @@ from enum import Enum # local libraries import simulator -from rktl_sim.srv import CreateCar, CreateCarResponse, DeleteCar, DeleteCarResponse +from rktl_sim.srv import CreateCarResponse from rktl_msgs.msg import MatchStatus, ControlCommand, ControlEffort @@ -128,7 +128,7 @@ class Simulator(object): Odometry, queue_size=1) # Services - rospy.Service('sim_reset_car_&_ball', 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(): @@ -305,7 +305,7 @@ class Simulator(object): self.last_time = now self.reset_lock.release() - + def get_sim_param(self, path, returnValue=False, secondParam=None): """ @param secondParam: Specify if you want to pass in a second parameter to rospy. @@ -352,9 +352,14 @@ class Simulator(object): return None # accounting for bugs in yaml file if min_param > max_param: - return (float)(random.uniform(max_param, min_param)) + param_val =(float)(random.uniform(max_param, min_param)) + rospy.set_param(f'{path}',param_val) + + return param_val else: - return (float)(random.uniform(min_param, max_param)) + 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: @@ -367,11 +372,11 @@ class Simulator(object): def configure_field(self): """Configures the field boundries and goals to be used in the simulator.""" - 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) + 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)], @@ -431,7 +436,7 @@ class Simulator(object): self.car_odom_pubs[car_name] = rospy.Publisher( f'/cars/{car_name}/odom', Odometry, queue_size=1) - return CreateCarResponse(True) + return True if __name__ == "__main__": From f37aa8bc32d81c53cc5dd916fa4645ccb605bd7d Mon Sep 17 00:00:00 2001 From: jcrm1 <52137472+jcrm1@users.noreply.github.com> Date: Sun, 23 Oct 2022 15:51:58 -0400 Subject: [PATCH 32/43] Remove broken/unnecessary imports Also fixes integration with modular_training branch --- rktl_sim/nodes/simulator | 1 - 1 file changed, 1 deletion(-) diff --git a/rktl_sim/nodes/simulator b/rktl_sim/nodes/simulator index e0e8edb44..22efcae59 100755 --- a/rktl_sim/nodes/simulator +++ b/rktl_sim/nodes/simulator @@ -18,7 +18,6 @@ from enum import Enum # local libraries import simulator -from rktl_sim.srv import CreateCarResponse from rktl_msgs.msg import MatchStatus, ControlCommand, ControlEffort From 33a289b7194a1afd994afbcf589c12b7d43921fb Mon Sep 17 00:00:00 2001 From: Ryan Jordan Date: Sun, 23 Oct 2022 16:09:41 -0400 Subject: [PATCH 33/43] Load weights from zip file --- rktl_autonomy/scripts/modular_train.py | 3 +-- rktl_autonomy/scripts/train_rocket_league.py | 4 ++++ 2 files changed, 5 insertions(+), 2 deletions(-) diff --git a/rktl_autonomy/scripts/modular_train.py b/rktl_autonomy/scripts/modular_train.py index ce8cbd697..ec7f76d32 100755 --- a/rktl_autonomy/scripts/modular_train.py +++ b/rktl_autonomy/scripts/modular_train.py @@ -10,8 +10,7 @@ numEnvsAllowed = 24 if len(sys.argv) == 2: numEnvsAllowed = int(sys.argv[1]) - configFolder = os.path.join(os.path.join(os.path.join(Path(__file__), os.pardir), os.pardir), Path("config")) - configFile = Path(os.path.abspath(os.path.join(configFolder, "rocket_league.yaml"))) + 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"]) diff --git a/rktl_autonomy/scripts/train_rocket_league.py b/rktl_autonomy/scripts/train_rocket_league.py index 9e7e64c9e..1b7f41f5d 100755 --- a/rktl_autonomy/scripts/train_rocket_league.py +++ b/rktl_autonomy/scripts/train_rocket_league.py @@ -25,6 +25,10 @@ def train(n_envs=24, n_saves=100, n_steps=240000000, env_counter=None): model = PPO("MlpPolicy", env) + # load weights from zip file + previous_weights = expanduser(f'~/catkin_ws/data/rocket_league/model') + model.set_parameters(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"]) From 7ae8ff94b5a378927fdc81ef9f76e7763c5b9791 Mon Sep 17 00:00:00 2001 From: Ryan Jordan Date: Sun, 23 Oct 2022 16:13:47 -0400 Subject: [PATCH 34/43] Print error message if fail to load previous weights --- rktl_autonomy/scripts/train_rocket_league.py | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/rktl_autonomy/scripts/train_rocket_league.py b/rktl_autonomy/scripts/train_rocket_league.py index 1b7f41f5d..27076c13c 100755 --- a/rktl_autonomy/scripts/train_rocket_league.py +++ b/rktl_autonomy/scripts/train_rocket_league.py @@ -26,8 +26,11 @@ def train(n_envs=24, n_saves=100, n_steps=240000000, env_counter=None): model = PPO("MlpPolicy", env) # load weights from zip file - previous_weights = expanduser(f'~/catkin_ws/data/rocket_league/model') - model.set_parameters(previous_weights) + try: + previous_weights = expanduser(f'~/catkin_ws/data/rocket_league/model') + 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}') From 382311c2409161babae48d892c40b67527b9c7af Mon Sep 17 00:00:00 2001 From: Abuynits Date: Sun, 30 Oct 2022 19:04:40 +0000 Subject: [PATCH 35/43] added friction to sim ball --- rktl_sim/nodes/simulator | 10 ++++++++++ rktl_sim/src/simulator/sim.py | 33 ++++++++++++++++++++++++++++++--- 2 files changed, 40 insertions(+), 3 deletions(-) diff --git a/rktl_sim/nodes/simulator b/rktl_sim/nodes/simulator index e0e8edb44..784e851f0 100755 --- a/rktl_sim/nodes/simulator +++ b/rktl_sim/nodes/simulator @@ -305,6 +305,15 @@ class Simulator(object): 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): """ @@ -401,6 +410,7 @@ class Simulator(object): """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') diff --git a/rktl_sim/src/simulator/sim.py b/rktl_sim/src/simulator/sim.py index cda935f7d..716945eb1 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 @@ -301,8 +302,15 @@ def step(self, car_cmd, dt): 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] @@ -312,15 +320,33 @@ def step(self, car_cmd, dt): 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 for car in self._cars.values(): car.step(p_dt) p.stepSimulation() - + 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: @@ -362,6 +388,7 @@ def get_ball_pose(self, add_noise=False): def get_ball_velocity(self): if self._ball_id is None: return None + return p.getBaseVelocity(self._ball_id) def reset(self, spawn_bounds, car_properties, ball_init_pose, ball_init_speed): From 59f397d43d0c7ec49a7cbcada2709a7b57e5c9da Mon Sep 17 00:00:00 2001 From: Ryan Jordan Date: Fri, 2 Dec 2022 00:44:35 +0000 Subject: [PATCH 36/43] fixed env_counter.py --- rktl_autonomy/src/rktl_autonomy/env_counter.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rktl_autonomy/src/rktl_autonomy/env_counter.py b/rktl_autonomy/src/rktl_autonomy/env_counter.py index f4d7caa0d..c77dba7d7 100644 --- a/rktl_autonomy/src/rktl_autonomy/env_counter.py +++ b/rktl_autonomy/src/rktl_autonomy/env_counter.py @@ -3,7 +3,7 @@ def __init__(self): self.counter = 0 def count_env(self): - ++self.counter + self.counter += 1 def get_current_counter(self): return self.counter From 3c30dbf18d2213c2231180c4097a3f5499fea47d Mon Sep 17 00:00:00 2001 From: Ryan Jordan Date: Tue, 31 Jan 2023 23:40:15 +0000 Subject: [PATCH 37/43] fixed error in train_rocket_league.py --- rktl_autonomy/scripts/modular_train.py | 14 ++++++++++++-- rktl_autonomy/scripts/train_rocket_league.py | 4 ++-- 2 files changed, 14 insertions(+), 4 deletions(-) diff --git a/rktl_autonomy/scripts/modular_train.py b/rktl_autonomy/scripts/modular_train.py index ec7f76d32..64a9e0adf 100755 --- a/rktl_autonomy/scripts/modular_train.py +++ b/rktl_autonomy/scripts/modular_train.py @@ -1,3 +1,5 @@ +#!/usr/bin/env python3 + from train_rocket_league import train import yaml from rktl_autonomy import EnvCounter @@ -5,16 +7,24 @@ import os from pathlib import Path from threading import Thread +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, EnvCounter()) - thread = Thread(target=train, args=args) - thread.start() + p = Process(target=train, args=args) + p.start() + print(f'Starting thread {i}/{numGroups}') + # thread = Thread(target=train, args=args) + # thread.start() diff --git a/rktl_autonomy/scripts/train_rocket_league.py b/rktl_autonomy/scripts/train_rocket_league.py index 27076c13c..3eada6598 100755 --- a/rktl_autonomy/scripts/train_rocket_league.py +++ b/rktl_autonomy/scripts/train_rocket_league.py @@ -6,7 +6,7 @@ All rights reserved. """ -from rktl_autonomy import RocketLeagueInterface +from rktl_autonomy import RocketLeagueInterface, EnvCounter from stable_baselines3 import PPO from stable_baselines3.common.vec_env import SubprocVecEnv from stable_baselines3.common.env_util import make_vec_env @@ -16,7 +16,7 @@ import uuid -def train(n_envs=24, n_saves=100, n_steps=240000000, env_counter=None): +def train(n_envs=24, n_saves=100, n_steps=240000000, env_counter=EnvCounter()): 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'] From 84f30962b23b3604c8076e20b4a0fa713b8c91fe Mon Sep 17 00:00:00 2001 From: Ryan Jordan Date: Tue, 31 Jan 2023 23:50:47 +0000 Subject: [PATCH 38/43] replaced EnvCounter class with a simple count --- rktl_autonomy/scripts/modular_train.py | 7 +------ rktl_autonomy/scripts/train_rocket_league.py | 6 +++--- .../src/rktl_autonomy/rocket_league_interface.py | 13 +++++++------ 3 files changed, 11 insertions(+), 15 deletions(-) diff --git a/rktl_autonomy/scripts/modular_train.py b/rktl_autonomy/scripts/modular_train.py index 64a9e0adf..9510d99bc 100755 --- a/rktl_autonomy/scripts/modular_train.py +++ b/rktl_autonomy/scripts/modular_train.py @@ -2,11 +2,8 @@ from train_rocket_league import train import yaml -from rktl_autonomy import EnvCounter import sys import os -from pathlib import Path -from threading import Thread from multiprocessing import Process if __name__ == '__main__': @@ -22,9 +19,7 @@ numGroups = len(file["reward"]["win"]) for i in range(numGroups): - args = (int(numEnvsAllowed / numGroups), 100, 240000000, EnvCounter()) + args = (int(numEnvsAllowed / numGroups), 100, 240000000, i) p = Process(target=train, args=args) p.start() print(f'Starting thread {i}/{numGroups}') - # thread = Thread(target=train, args=args) - # thread.start() diff --git a/rktl_autonomy/scripts/train_rocket_league.py b/rktl_autonomy/scripts/train_rocket_league.py index 3eada6598..008d126e3 100755 --- a/rktl_autonomy/scripts/train_rocket_league.py +++ b/rktl_autonomy/scripts/train_rocket_league.py @@ -6,7 +6,7 @@ All rights reserved. """ -from rktl_autonomy import RocketLeagueInterface, EnvCounter +from rktl_autonomy import RocketLeagueInterface from stable_baselines3 import PPO from stable_baselines3.common.vec_env import SubprocVecEnv from stable_baselines3.common.env_util import make_vec_env @@ -16,11 +16,11 @@ import uuid -def train(n_envs=24, n_saves=100, n_steps=240000000, env_counter=EnvCounter()): +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, 'env_counter': env_counter}, + env = make_vec_env(RocketLeagueInterface, env_kwargs={'run_id': run_id, 'env_number': env_number}, n_envs=n_envs, vec_env_cls=SubprocVecEnv) model = PPO("MlpPolicy", env) diff --git a/rktl_autonomy/src/rktl_autonomy/rocket_league_interface.py b/rktl_autonomy/src/rktl_autonomy/rocket_league_interface.py index cff1b9d54..338477943 100755 --- a/rktl_autonomy/src/rktl_autonomy/rocket_league_interface.py +++ b/rktl_autonomy/src/rktl_autonomy/rocket_league_interface.py @@ -35,12 +35,13 @@ 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, env_counter=None): - super().__init__(node_name='rocket_league_agent', eval=eval, launch_file=launch_file, launch_args=launch_args, run_id=run_id) + 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.instance_id = env_counter.get_current_counter() + self.env_number = env_number # Actions - env_counter.count_env() self._MIN_VELOCITY = -rospy.get_param('/cars/throttle/max_speed') self._MAX_VELOCITY = rospy.get_param('/cars/throttle/max_speed') self._MIN_CURVATURE = -tan(rospy.get_param('/cars/steering/max_throw')) / rospy.get_param('cars/length') @@ -76,8 +77,8 @@ def __init__(self, eval=False, launch_file=['rktl_autonomy', 'rocket_league_trai self._CONSTANT_REWARD = rospy.get_param('~reward/constant', 0.0) 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._WIN_REWARD = rospy.get_param('~reward/win', 100.0)[self.instance_id] - self._LOSS_REWARD = rospy.get_param('~reward/loss', 0.0)[self.instance_id] + self._WIN_REWARD = rospy.get_param('~reward/win', 100.0)[self.env_number] + self._LOSS_REWARD = rospy.get_param('~reward/loss', 0.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) From 7a1ce7447e41150e9384474273954091a2a426d0 Mon Sep 17 00:00:00 2001 From: Daniel Gerblick Date: Wed, 8 Mar 2023 00:26:57 +0000 Subject: [PATCH 39/43] Disabled field and car randomization to fix error --- rktl_launch/config/global_params.yaml | 24 ++++++++++++------------ 1 file changed, 12 insertions(+), 12 deletions(-) diff --git a/rktl_launch/config/global_params.yaml b/rktl_launch/config/global_params.yaml index 02fb6aa0e..2053ccd0c 100644 --- a/rktl_launch/config/global_params.yaml +++ b/rktl_launch/config/global_params.yaml @@ -2,12 +2,12 @@ # field dimensions field: - width: #3 - max: 3.5 - min: 2.5 - length: #4.25 - max: 5 - min: 3.5 + width: 3 + # max: 3.5 + # min: 2.5 + length: 4.25 + # max: 5 + # min: 3.5 wall_thickness: 0.25 goal: width: 1 @@ -18,13 +18,13 @@ ball: # car dimensions and physical constants cars: - length: #0.12 # front to rear wheel center to center, meters - min: 0.10 - max: 0.30 + 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 + 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 From 6527712ce7443c687a06b0a609317daee8532a78 Mon Sep 17 00:00:00 2001 From: jcrm1 <52137472+jcrm1@users.noreply.github.com> Date: Sat, 1 Apr 2023 15:07:26 -0400 Subject: [PATCH 40/43] Fix array index bug in rewards --- rktl_autonomy/scripts/train_rocket_league.py | 2 +- .../src/rktl_autonomy/rocket_league_interface.py | 14 ++++++++++---- 2 files changed, 11 insertions(+), 5 deletions(-) diff --git a/rktl_autonomy/scripts/train_rocket_league.py b/rktl_autonomy/scripts/train_rocket_league.py index 8c106bc54..b8869c442 100755 --- a/rktl_autonomy/scripts/train_rocket_league.py +++ b/rktl_autonomy/scripts/train_rocket_league.py @@ -20,7 +20,7 @@ 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, 'env_number': env_number}, + 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) diff --git a/rktl_autonomy/src/rktl_autonomy/rocket_league_interface.py b/rktl_autonomy/src/rktl_autonomy/rocket_league_interface.py index 22fa9c501..971fe4d19 100755 --- a/rktl_autonomy/src/rktl_autonomy/rocket_league_interface.py +++ b/rktl_autonomy/src/rktl_autonomy/rocket_league_interface.py @@ -78,8 +78,14 @@ 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.env_number] - self._LOSS_REWARD = rospy.get_param('~reward/loss', 0.0)[self.env_number] + 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 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) @@ -189,9 +195,9 @@ def _get_state(self): if self._score != 0: done = True if self._score > 0: - reward += self._WIN_REWARD + reward += self._WIN_REWARD[self.env_number] else: - reward += self._LOSS_REWARD + reward += self._LOSS_REWARD[self.env_number] x, y, __, v, __ = self._car_odom From 6e93c8336a94689265b3d173be915a1648ed4984 Mon Sep 17 00:00:00 2001 From: jcrm1 <52137472+jcrm1@users.noreply.github.com> Date: Sat, 1 Apr 2023 15:08:41 -0400 Subject: [PATCH 41/43] Continue fixing array index bug in rewards --- rktl_autonomy/src/rktl_autonomy/rocket_league_interface.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/rktl_autonomy/src/rktl_autonomy/rocket_league_interface.py b/rktl_autonomy/src/rktl_autonomy/rocket_league_interface.py index 971fe4d19..5db4ee15a 100755 --- a/rktl_autonomy/src/rktl_autonomy/rocket_league_interface.py +++ b/rktl_autonomy/src/rktl_autonomy/rocket_league_interface.py @@ -195,9 +195,9 @@ def _get_state(self): if self._score != 0: done = True if self._score > 0: - reward += self._WIN_REWARD[self.env_number] + reward += self._WIN_REWARD else: - reward += self._LOSS_REWARD[self.env_number] + reward += self._LOSS_REWARD x, y, __, v, __ = self._car_odom From 0dbe07be83c2f6013d8c24cddb0168e027bcfd3b Mon Sep 17 00:00:00 2001 From: jcrm1 <52137472+jcrm1@users.noreply.github.com> Date: Sat, 1 Apr 2023 15:18:16 -0400 Subject: [PATCH 42/43] More fixing reward type bug --- .../rktl_autonomy/rocket_league_interface.py | 18 ++++++++++++------ 1 file changed, 12 insertions(+), 6 deletions(-) diff --git a/rktl_autonomy/src/rktl_autonomy/rocket_league_interface.py b/rktl_autonomy/src/rktl_autonomy/rocket_league_interface.py index 5db4ee15a..c45aa25b6 100755 --- a/rktl_autonomy/src/rktl_autonomy/rocket_league_interface.py +++ b/rktl_autonomy/src/rktl_autonomy/rocket_league_interface.py @@ -78,14 +78,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) - if len(rospy.get_param('~reward/win', [100.0])) >= self.env_number: - self._WIN_REWARD = rospy.get_param('~reward/win', [100.0])[0] + if isinstance(rospy.get_param('~reward/win', [100.0]), int): + self._WIN_REWARD = rospy.get_param('~reward/win', [100.0]) else: - self._WIN_REWARD = rospy.get_param('~reward/win', [100.0])[self.env_number] - if len(rospy.get_param('~reward/loss', [100.0])) >= self.env_number: - self._LOSS_REWARD = rospy.get_param('~reward/loss', [100.0])[0] + 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: - self._LOSS_REWARD = rospy.get_param('~reward/loss', [100.0])[self.env_number] + 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) From b71af0fb3e34da8c7c3bbd46ea7295a8576c8063 Mon Sep 17 00:00:00 2001 From: jcrm1 <52137472+jcrm1@users.noreply.github.com> Date: Tue, 4 Apr 2023 16:43:39 -0400 Subject: [PATCH 43/43] Update rocket_league_sim.launch Only render control GUI when requested --- rktl_launch/launch/rocket_league_sim.launch | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rktl_launch/launch/rocket_league_sim.launch b/rktl_launch/launch/rocket_league_sim.launch index 73cef80a5..83614a149 100644 --- a/rktl_launch/launch/rocket_league_sim.launch +++ b/rktl_launch/launch/rocket_league_sim.launch @@ -26,7 +26,7 @@ - +