tuned the ready state for more stability. integrated in a test the NN based walking policy that is deployed through onnx. in the process broke old walk engine so it can be used for more navigation. the ONNX walk policy has a few test and has great perfornmance across the board. added some performance notes.

Author: JonathanSpraggett

.idea/workspace.xml

@@ -0,0 +1,90 @@
+cd ~/ros2_ws/src/mujoco_playground/mujoco_playground/_src/locomotion/bez2
+ mjx-testspeed --mjcf=xmls/scene_mjx_feetonly_flat_terrain.xml --base_path=.
+
+
+
+ berk
+  Total JIT time: 31.45 s
+ Total simulation time: 1.16 s
+ Total steps per second: 880664
+ Total realtime factor: 1761.33 x
+ Total time per step: 1.14 µs
+
+
+ g1
+ Total JIT time: 49.57 s
+ Total simulation time: 1.84 s
+ Total steps per second: 556957
+ Total realtime factor: 1113.91 x
+ Total time per step: 1.80 µs
+
+ bez2 only head
+
+ Total JIT time: 21.88 s
+ Total simulation time: 5.00 s
+ Total steps per second: 204634
+ Total realtime factor: 409.27 x
+ Total time per step: 4.89 µs
+
+bez2 only feet  box
+ Total JIT time: 21.48 s
+ Total simulation time: 0.99 s
+ Total steps per second: 1038202
+ Total realtime factor: 2076.40 x
+ Total time per step: 0.96 µs
+
+ bez2  only feet mesh
+   Total JIT time: 31.45 s
+ Total simulation time: 2.58 s
+ Total steps per second: 397097
+ Total realtime factor: 794.19 x
+ Total time per step: 2.52 µ
+
+  bez2  only feet convex mesh
+   Total JIT time: 33.26 s
+ Total simulation time: 1.29 s
+ Total steps per second: 791963
+ Total realtime factor: 1583.93 x
+ Total time per step: 1.26 µs
+
+ go1 feet only
+
+ Total JIT time: 16.25 s
+ Total simulation time: 0.64 s
+ Total steps per second: 1608187
+ Total realtime factor: 6432.75 x
+ Total time per step: 0.62 µs
+
+ go1 scene_mjx_fullcollisions_flat_terrain.xml
+
+  Total JIT time: 32.26 s
+ Total simulation time: 3.62 s
+ Total steps per second: 282638
+ Total realtime factor: 1130.55 x
+ Total time per step: 3.54 µs
+
+go1 scene_mjx_flat_terrain.xml
+
+Total JIT time: 20.63 s
+ Total simulation time: 1.23 s
+ Total steps per second: 835247
+ Total realtime factor: 3340.99 x
+ Total time per step: 1.20 µs
+
+ op3 feet only
+
+ Total JIT time: 33.70 s
+ Total simulation time: 1.38 s
+ Total steps per second: 740388
+ Total realtime factor: 2961.55 x
+ Total time per step: 1.35 µs
+
+bez2 dof 20
+
+time to jit: 0:02:13.416952
+time to train: 0:53:36.504378
+
+bez2 dof 12
+
+time to jit: 0:02:13.416952
+time to train: 0:53:36.504378

MUJOCO_LOG.TXT

@@ -1,49 +1,12 @@
-construct==2.10.67
-cv_bridge==3.2.1
-fast_simplification==0.1.12
-filterpy==1.4.5
-gdown==4.5.4
-geometry_msgs==4.9.0
-ischedule==1.2.7
-launch_ros==0.19.10
-matplotlib==3.5.1
-mujoco==3.3.5
-mujoco_mjx==3.3.5
-nav_msgs==4.9.0
-numpy==2.3.2
-open3d==0.17.0
-opencv_python==4.10.0.84
-pandas==2.3.2
-pinocchio==0.4.3
-placo==0.9.14
-protobuf==3.20.2
-protobuf==3.12.4
-psutil==5.9.8
-psutil==5.9.0
-pybullet==3.2.7
-pygame==2.5.2
-pyserial==3.5
-pytest==7.4.4
-pytest==6.2.5
-pyvista==0.46.3
-PyYAML==6.0.2
-PyYAML==5.4.1
-PyYAML==6.0.2
-rclpy==3.3.17
-rosgraph_msgs==1.2.2
-scipy==1.8.0
-segment_anything==1.0
-self==2020.12.3
-sensor_msgs==4.9.0
-setuptools==71.0.0
-setuptools==59.6.0
-std_msgs==4.9.0
-std_srvs==4.9.0
-tf==1.1.0
-tf2_py==0.25.16
-tf2_ros_py==0.25.16
-timeout_decorator==0.5.0
-#torch==2.3.1
-transforms3d==0.4.2
-trimesh==4.5.3
-#  pipreqs .
+torch==2.8.0
+torchvision==0.23.0
+opencv-python==4.12.0.88
+placo==0.9.16
+matplotlib==3.10.7
+pandas==2.3.3
+tqdm==4.67.1
+ultralytics==8.3.187
+seaborn==0.13.2
+gitpython>=3.1.30
+pillow>=10.3.0
+setuptools>=70.0.0

mjx_notes.txt

@@ -35,7 +35,7 @@ replan_timesteps: 10 # Replanning each n timesteps
 # Posture parameters
 walk_com_height: 0.24 # Constant height for the CoM [m]
 walk_foot_height: 0.05 # Height of foot rising while walking [m]
-walk_trunk_pitch: 0.2 # Trunk pitch angle [rad]
+walk_trunk_pitch: 0.15 # Trunk pitch angle [rad]
 walk_foot_rise_ratio: 0.2 # Time ratio for the foot swing plateau (0.0 to 1.0)
 
 # Feet parameters

requirements.txt

@@ -102,6 +102,19 @@ def get_qdot(self, name: str):
         """
         addr = self.model.jnt_dofadr[self.dofs_to_index[name]]
         return self.data.qvel[addr]
+    def get_q_legs(self):
+        names = ["left_hip_yaw", "left_hip_roll", "left_hip_pitch", "left_knee", "left_ankle_pitch", "left_ankle_roll", "right_hip_yaw", "right_hip_roll", "right_hip_pitch", "right_knee", "right_ankle_pitch", "right_ankle_roll"]
+        index = []
+        for name in names:
+            index.append(self.get_q(name))
+        return index
+
+    def get_qvel_legs(self):
+        names = ["left_hip_yaw", "left_hip_roll", "left_hip_pitch", "left_knee", "left_ankle_pitch", "left_ankle_roll", "right_hip_yaw", "right_hip_roll", "right_hip_pitch", "right_knee", "right_ankle_pitch", "right_ankle_roll"]
+        index = []
+        for name in names:
+            index.append(self.get_qdot(name))
+        return index
 
     def set_q(self, name: str, value: float):
         """
@@ -170,6 +183,10 @@ def set_motor(self) -> None:
         indexes = list(self.ctrl_dofs_to_index.values())
         self.data.ctrl[indexes] = self.get_angles()
 
+    def set_motor_head(self) -> None:
+        indexes = self.get_dof_indexes(["head_yaw", "head_pitch"])
+        self.data.ctrl[indexes] = self.get_angles()[:2]
+
     def get_dof_indexes(self, names: List[str]) -> list:
         index = []
         for name in names:

soccer_control/soccer_pycontrol/config/bez2/bez2_sim_mujoco.yaml

@@ -35,6 +35,9 @@ def __init__(
         sim: bool = True,
     ):
 
+        self.dx = 0
+        self.dy = 0
+        self.dtheta = 0
         self.world = world
         self.bez = bez
         self.imu_feedback_enabled = imu_feedback_enabled
@@ -59,7 +62,7 @@ def __init__(
         )  # TODO could also mod if balance is decreasing
 
         self.nav_yaw_pid = PID(
-            Kp=0.2,
+            Kp=0.5,
             Kd=0,
             Ki=0,
             setpoint=0,
@@ -109,18 +112,18 @@ def walk_pose(self, target_goal: Transformation):
             self.nav_yaw_pid.reset()
             self.nav_yaw_pid.setpoint = target_goal.orientation_euler[0]
 
-            dx, dy = find_new_vel(goal_loc=target_goal.position[:2])
+            self.dx, self.dy = find_new_vel(goal_loc=target_goal.position[:2])
             # he = self.heading_error(target_goal.orientation_euler[0], pose.orientation_euler[0])
             # dtheta = math.copysign(0.5, he)
-            self.foot_step_planner.setup_walk(dx, dy)
+            # self.foot_step_planner.setup_walk(dx, dy)
             self.walker.t = 0
             # TODO fix and add to a nav could add a funct for pybullet or python
             # TODO could have a balancing mode by default could use the COM
             # TODO for ball could just remove
 
         if (
             position_error(pose.position[:2], target_goal.position[:2]) > self.error_tol
-            or abs(heading_error(target_goal.orientation_euler[0], pose.orientation_euler[0])) > self.error_tol
+            # or abs(heading_error(target_goal.orientation_euler[0], pose.orientation_euler[0])) > self.error_tol
         ):  # self.bez.sensors.get_pose() #TODO about 20% or 40% error
             pose = (
                 self.bez.sensors.get_pose()
@@ -139,12 +142,12 @@ def walk_pose(self, target_goal: Transformation):
             # TODO make  a 2d unit test
             self.nav_x_pid.setpoint = goal.position[0]
             self.nav_y_pid.setpoint = goal.position[1]
-            dx = self.nav_x_pid.update(0)
-            dy = self.nav_y_pid.update(0)
-            dtheta = self.nav_yaw_pid.update(pose.orientation_euler[0])
-            # print(f"dx: {round(dx, 3)} dy: {round(dy, 3)} dtheta: {round(dtheta, 3)} err_x: {round(x_error, 3)} err_y: {round(y_error, 3)} err_theta: {round(head_error, 3)}")
-            self.foot_step_planner.configure_planner(dx, dy, dtheta)
-            self.walker.walk_loop()  # TODO move main loop out of here
+            self.dx = self.nav_x_pid.update(0)
+            self.dy = self.nav_y_pid.update(0)
+            self.dtheta = self.nav_yaw_pid.update(pose.orientation_euler[0])
+            # print(f"dx: {round(self.dx, 3)} dy: {round(self.dy, 3)} dtheta: {round(self.dtheta, 3)} err_x: {round(x_error, 3)} err_y: {round(y_error, 3)} err_theta: {round(head_error, 3)}")
+            # self.foot_step_planner.configure_planner(dx, dy, dtheta)
+            # self.walker.walk_loop()  # TODO move main loop out of here
         else:
             self.ready()
             self.walker.enable_walking = False
@@ -162,8 +165,8 @@ def walk_ball(self, target_goal: Transformation, ball_pixel):
             self.nav_yaw_pid.reset()
             self.nav_yaw_pid.setpoint = 0  # TODO add  yaw modes
 
-            dx, dy = find_new_vel(goal_loc=target_goal.position[:2])
-            self.foot_step_planner.setup_walk(dx, dy)
+            self.dx, self.dy = find_new_vel(goal_loc=target_goal.position[:2])
+            # self.foot_step_planner.setup_walk(dx, dy)
             self.walker.t = 0
             # TODO fix and add to a nav could add a funct for pybullet or python
             # TODO could have a balancing mode by default could use the COM
@@ -178,13 +181,13 @@ def walk_ball(self, target_goal: Transformation, ball_pixel):
             # TODO replace with pure pursuit
             # TODO make  a 2d unit test
 
-            dx = self.nav_x_pid.update(0)
-            dy = self.nav_y_pid.update(0)
+            self.dx = self.nav_x_pid.update(0)
+            self.dy = self.nav_y_pid.update(0)
 
-            dtheta = self.nav_yaw_pid.update(self.bez.sensors.get_pose().orientation_euler[0])
+            self.dtheta = self.nav_yaw_pid.update(self.bez.sensors.get_pose().orientation_euler[0])
             # print(round(dx, 3), " ", round(dy, 3), " ", round(dtheta, 3), " ", round(x_error, 3), " ", round(y_error, 3), " ", round(head_error, 3))
-            self.foot_step_planner.configure_planner(dx, dy, dtheta)
-
+            # self.foot_step_planner.configure_planner(self.dx, self.dy, self.dtheta)
+            #
             self.walker.walk_loop(target_goal.position, ball_pixel)
         # else:
         #     self.ready()

soccer_control/soccer_pycontrol/soccer_pycontrol/model/motor_control.py

@@ -32,7 +32,7 @@ def __init__(self, bez: Bez, planner: FootStepPlanner, imu_feedback_enabled: boo
             Kd=0,
             Ki=-0.03,
             setpoint=2.4,
-            output_limits=(0.4, 1.3),
+            output_limits=(0.1, 1.3),
         )
 
     def reset_walk(self):
@@ -42,13 +42,13 @@ def reset_walk(self):
     # TODO could make input a vector
 
     def walk_loop(self, target_goal=[0, 0], ball_pixel=[0, 0]):  # TODO set default to something better
-        self.foot_step_planner.plan_steps(self.t)
-        self.bez.motor_control.set_angles_from_placo(self.foot_step_planner.robot)
+        # self.foot_step_planner.plan_steps(self.t)
+        # self.bez.motor_control.set_angles_from_placo(self.foot_step_planner.robot)
 
-        if self.imu_feedback_enabled and self.bez.sensors.imu_ready:
-            [_, pitch, roll] = self.bez.sensors.get_imu()
-            # print(pitch,"  ", roll)
-            self.stabilize_walk(pitch, roll)
+        # if self.imu_feedback_enabled and self.bez.sensors.imu_ready:
+        #     [_, pitch, roll] = self.bez.sensors.get_imu()
+        #     # print(pitch,"  ", roll)
+        #     self.stabilize_walk(pitch, roll)
 
         # self.foot_step_planner.head_movement(target_goal)
         if ball_pixel != self.last_ball:
@@ -57,7 +57,7 @@ def walk_loop(self, target_goal=[0, 0], ball_pixel=[0, 0]):  # TODO set default
             self.last_ball = ball_pixel
             self.ball_dx = self.ball_x_pid.update(3.2 - ball_pixel[0] / 100.0)
             self.ball_dy = self.ball_y_pid.update(ball_pixel[1] / 100.0)
-        # print(f"{ball_pixel}, {self.ball_dx}, {self.ball_dy}")
+        print(f"{ball_pixel}, {self.ball_dx}, {self.ball_dy}")
         self.bez.motor_control.set_head_target_angles([-self.ball_dx, self.ball_dy])
         # self.bez.motor_control.configuration["head_pitch"] = self.ball_dy
 
@@ -92,7 +92,8 @@ def walk_loop(self, target_goal=[0, 0], ball_pixel=[0, 0]):  # TODO set default
         # self.bez.motor_control.set_left_leg_target_angles([0, 0, 0, 0, 0, 0])
         # self.bez.motor_control.set_head_target_angles(
         #     [0, 0])
-        self.bez.motor_control.set_motor()
+
+        self.bez.motor_control.set_motor_head()
 
         self.t = self.foot_step_planner.step(self.t)