Moved all locomotion policies to a proper place and added some new experimental ones. modified object detection to record inference time. added some intermidiate urdf with primitive collisions. Added primitives as the main collision to the main bez2 mjcf. Added classes so that the stls are used for visuals, this increases performance a lot. changed main body link from torso_2023 to torso. Split test_mujoco into walk, and perception for better organization. integrated the onnxcontroller as rl_walk and rl_walk_ros for better modularity. Added a gravity_vector to sensors. updated the ros layer to at least fill out the observation vector for the policy. Added joint state callbacks to motor control and calcuate joint velocities from angles. re-enabled orientation for the pose navigator. should be ready to deploy the policy.

Author: JonathanSpraggett

.idea/workspace.xml

@@ -10,7 +10,10 @@
 
     <depend>rclpy</depend>
     <depend>soccer_common</depend>
-
+    <depend>python3-onnxruntime-pip</depend>
+     <depend>sensor_msgs</depend>
+  <depend>geometry_msgs</depend>
+    <depend>tf2_ros</depend>
     <export>
         <build_type>ament_python</build_type>
     </export>

…r_pycontrol/test/onnx/bez222_policy.onnx …ccer_pycontrol/models/bez222_policy.onnxsoccer_control/soccer_pycontrol/test/onnx/bez222_policy.onnx renamed to soccer_control/soccer_pycontrol/models/bez222_policy.onnx soccer_control/soccer_pycontrol/test/onnx/bez222_policy.onnx renamed to soccer_control/soccer_pycontrol/models/bez222_policy.onnx

@@ -17,6 +17,7 @@
         ("share/" + package_name + "/launch", glob("launch/*.launch")),
         ("share/" + package_name + "/launch", glob("launch/*.yaml")),
         ("share/" + package_name + "/launch", glob("launch/*.py")),
+        ("share/" + package_name + "/models", glob("models/*.onnx")),
     ],
     maintainer="Jonathan Spraggett",
     maintainer_email="jonathanspraggett@gmail.com",
@@ -25,6 +26,7 @@
     entry_points={
         "console_scripts": [
             "soccer_walk = soccer_pycontrol.walk_engine.walk_engine_ros.navigator_ros:main",
+            "soccer_rl_walk = soccer_pycontrol.walk_engine.walk_engine_ros.rl_walk_ros:main",
         ],
     },
 )

soccer_control/soccer_pycontrol/models/bez22_flat_policy.onnx

@@ -32,6 +32,14 @@ def __init__(
 
         self.sensors = Sensors(self.world)
 
+        self.default_angles = np.array([-0.00038 ,0.00058
+                    ,-0.44675 ,0.00074 ,2.50858
+                    ,-0.02901 ,0.06566 ,0.87410 ,-1.54022 ,0.67634 ,-0.07002
+                    ,-0.44962 ,-0.00074 ,2.50967
+                    ,-0.01830 ,0.04949 ,0.85711 ,-1.54784 ,0.69839 ,-0.05164])
+        self.default_leg_angles = np.array([-0.02901, 0.06566, 0.87410, -1.54022, 0.67634, -0.07002
+                                            , -0.01830, 0.04949, 0.85711, -1.54784, 0.69839, -0.05164])
+
     @property
     def status(self) -> BezStatusEnum:
         return self._status
@@ -40,6 +48,9 @@ def status(self) -> BezStatusEnum:
     def status(self, status: BezStatusEnum) -> None:
         self._status = status
 
+    def ready(self): # TODO fix default angles it leans
+        self.motor_control.set_all_angles(self.default_angles)
+        self.motor_control.set_motor()
     def get_parameters(self) -> dict:
         with open(
             expanduser("~")