Js ipm (#920)

* wip

* wip

* fsdf

* rggre

* modified the cam library to closer to standard CV notation.

* refactored the navigator path plaNNING, PID, walker to integrate the RL walk as a main mode. seperated head control from navigator. created a walk interface that can switch engine types. cleaned up navigator to be a lot more streame lined. fixed most of the unit tests.

* made yaw cmds scale  more inline with x,y. Seperated yaw control from x,y control.  cleaned up the notation of using the PID. extended head controller logic to account when it doesnt see the object and to send a yaw cmd to the walk engine when the head motors cant center the target.

* ff

Author: JonathanSpraggett

…ontrol/soccer_pycontrol/test/__init__.py .ai/mcp/mcp.jsonsoccer_control/soccer_pycontrol/test/__init__.py renamed to .ai/mcp/mcp.json soccer_control/soccer_pycontrol/test/__init__.py renamed to .ai/mcp/mcp.json

@@ -0,0 +1 @@
+LD_LIBRARY_PATH=$LD_LIBRARY_PATH:/opt/ros/humble/lib/

.env

@@ -25,7 +25,7 @@ walking_roll_offset: 0.0
 # Walk parameters - if double_support_ratio is not set to 0, should be greater than replan_frequency
 # Timing parameters
 control_frequency: 0.005
-single_support_duration: 0.3 # Duration of single support phase [s]
+single_support_duration: 0.25 # Duration of single support phase [s]
 single_support_timesteps: 10 # Number of planning timesteps per single support phase
 double_support_ratio: 0.0 # Ratio of double support (0.0 to 1.0)
 startend_double_support_ratio: 1.5 # Ratio duration of supports for starting and stopping walk
@@ -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.15 # Trunk pitch angle [rad]
+walk_trunk_pitch: 0.2 # Trunk pitch angle [rad]
 walk_foot_rise_ratio: 0.2 # Time ratio for the foot swing plateau (0.0 to 1.0)
 
 # Feet parameters

soccer_control/soccer_pycontrol/config/bez2/bez2_sim_mujoco.yaml

@@ -87,7 +87,7 @@ def apply_phase_difference_roll_feedback(self, t, imu_pose: Transformation, robo
         F = self.walking_pid_roll.update(y_n)
         return min(t + F, robot_path.duration())
 
-    def reset_imus(self):
+    def reset(self):
         """
         Reset the walking and standing PID values
         """

soccer_control/soccer_pycontrol/soccer_pycontrol/exp/stabilize_phase.py

@@ -13,36 +13,38 @@ def wrapToPi(num: float) -> float:  # TODO put in common
     return rem
 
 
-# class MotorData:
-#     def __init__(self, motor_names: dict):
-#         self.motor_names = motor_names
-#         self.data = [0.0] * len(motor_names)
-#
-#     def __getitem__(self, index):
-#         if isinstance(index, slice):
-#             if type(index.start) is str:
-#                 return self.data[self.motor_names[index.start][1] : self.motor_names[index.stop][1] + 1]
-#             else:
-#                 return self.data[slice(index.start, index.stop)]
-#         if type(index) is str:
-#             return self.data[self.motor_names[index][1]]
-#
-#         return self.data[index]
-#
-#     def __setitem__(self, index, value):
-#         if isinstance(index, slice):
-#             if type(index.start) is str:
-#                 self.data[self.motor_names[index.start][1] : self.motor_names[index.stop][1] + 1] = value
-#             else:
-#                 self.data[slice(index.start, index.stop)] = value
-#         else:
-#             if type(index) is str:
-#                 self.data[self.motor_names[index][1]] = value
-#             else:
-#                 self.data[index] = value
-#
-#     def reset(self):
-#         self.data = [0.0] * len(self.motor_names)
+class MotorData:
+    def __init__(self, motor_names: dict):
+        self.motor_names = motor_names
+        self.data = [0.0] * len(motor_names)
+
+    def __getitem__(self, index):
+        if isinstance(index, slice):
+            if type(index.start) is str:
+                return self.data[self.motor_names[index.start][1] : self.motor_names[index.stop][1] + 1]
+            else:
+                return self.data[slice(index.start, index.stop)]
+        if type(index) is str:
+            return self.data[self.motor_names[index][1]]
+
+        return self.data[index]
+
+    def __setitem__(self, index, value):
+        if isinstance(index, slice):
+            if type(index.start) is str:
+                self.data[self.motor_names[index.start][1] : self.motor_names[index.stop][1] + 1] = value
+            else:
+                self.data[slice(index.start, index.stop)] = value
+        else:
+            if type(index) is str:
+                self.data[self.motor_names[index][1]] = value
+            else:
+                self.data[index] = value
+
+    def reset(self):
+        self.data = [0.0] * len(self.motor_names)
+
+
 class MotorControl:
     """
     Class controls access to motor information and sets motor angles in MuJoCo
@@ -63,7 +65,7 @@ def __init__(self, model: mujoco.MjModel, data: mujoco.MjData):
         self.ctrl_dofs_to_index = {}
         for name in self.dof_names:
             self.ctrl_dofs_to_index[name] = self.get_actuator_index(name)
-        print()  # TODO this whole thing should be a dict
+
         self.configuration = np.zeros(self.dof)
         self.configuration_offset = np.zeros(self.dof)
 

soccer_control/soccer_pycontrol/soccer_pycontrol/model/motor_control.py

@@ -0,0 +1,59 @@
+from soccer_common import PID
+
+
+class HeadControl:
+    def __init__(self, bez):
+        self.ball_dx = 0
+        self.ball_dy = 0.7
+        self.last_ball_pixel = [0, 0]
+        self.bez = bez
+        self.ball_x_pid = PID(
+            Kp=0.05,
+            Kd=0,
+            Ki=0.03,
+            setpoint=0,
+            output_limits=(-3.14, 3.14),
+        )
+
+        self.ball_y_pid = PID(
+            Kp=-0.05,
+            Kd=0,
+            Ki=-0.03,
+            setpoint=2.4,
+            output_limits=(0.1, 1.3),
+        )
+
+        ctrl_dt = 1/50.0
+        self.n_substeps = int(round(ctrl_dt/self.bez.world.dt))
+        self.counter = 0
+        self.time_since_last_center = 0
+        self.time_since_last_ball = 0
+
+
+    def track_ball(self,ball_pixel=[0, 0]):
+        self.counter += 1
+        self.time_since_last_center += 1
+        self.time_since_last_ball += 1
+        ret_msg = -self.ball_dx
+        if self.counter % self.n_substeps == 0:
+            if ball_pixel != self.last_ball_pixel:
+                self.time_since_last_ball = 0
+                # print(f"here: {ball_pixel}, {self.ball_dx}, {self.ball_dy}")
+                self.last_ball_pixel = ball_pixel
+                self.ball_dx = self.ball_x_pid.update(3.2 - ball_pixel[0] / 100.0) # TODO change with center pixel from cam settign
+                self.ball_dy = self.ball_y_pid.update(ball_pixel[1] / 100.0)
+            if abs(ball_pixel[0] - 320.5) < 30:
+                self.time_since_last_center = 0
+
+            # 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.set_single_motor("head_pitch", self.ball_dy)
+            self.bez.motor_control.set_motor()
+        # print(f"here: {self.time_since_last_center * self.bez.world.dt}, {self.time_since_last_ball * self.bez.world.dt} {self.ball_dx}, {self.ball_dy}")
+        if self.time_since_last_center * self.bez.world.dt > 1.0:
+            ret_msg =  -self.ball_dx
+        if self.time_since_last_ball * self.bez.world.dt > 1.0:
+            self.bez.motor_control.set_head_target_angles([0, 0.4])
+            self.bez.motor_control.set_motor()
+            ret_msg = 0 #-self.bez.sensors.get_pose().orientation_euler[0]
+        return ret_msg