learning tweaks

simulation.py

@@ -82,7 +82,7 @@ class Simulation:
                 jointIndex=joint_index,
                 controlMode=p.POSITION_CONTROL,
                 targetPosition=float(target_angle),
-                force=500,
+                force=30,
                 physicsClientId=self.physics_client
             )
 
@@ -116,7 +116,7 @@ class Simulation:
 
         p.resetBasePositionAndOrientation(self.robot_id, pos, orn, physicsClientId=self.physics_client)
         p.resetBaseVelocity(self.robot_id, linearVelocity=lin_v, angularVelocity=ang_v, physicsClientId=self.physics_client)
-
+    
     # --- TELEMETRY (GETTERS) ---
 
     def get_robot_pose(self) -> Tuple[List[float], List[float]]:
@@ -141,6 +141,23 @@ class Simulation:
         joint_states = p.getJointStates(self.robot_id, self.revolute_joints, physicsClientId=self.physics_client)
         return np.array([state[0] for state in joint_states], dtype=np.float32)
 
+    def _get_urdf_joint_limits(self) -> Tuple[np.ndarray, np.ndarray]:
+        """Dynamically reads lower and upper limits for all revolute joints from PyBullet."""
+        lower_limits = []
+        upper_limits = []
+        
+        # Iterate through joints in PyBullet
+        for j_idx in range(p.getNumJoints(self.robot_id, physicsClientId=self.physics_client)):
+            info = p.getJointInfo(self.robot_id, j_idx, physicsClientId=self.physics_client)
+            joint_type = info[2]
+            
+            # Only collect limits for revolute joints
+            if joint_type == p.JOINT_REVOLUTE:
+                lower_limits.append(info[8])  # Index 8 = jointLowerLimit
+                upper_limits.append(info[9])  # Index 9 = jointUpperLimit
+                
+        return np.array(lower_limits, dtype=np.float32), np.array(upper_limits, dtype=np.float32)
+
     # --- SIMULATION LIFECYCLE CONTROLS ---
 
     def step(self) -> None:
@@ -153,8 +170,13 @@ class Simulation:
         for j in range(num_joints):
             p.changeVisualShape(self.robot_id, j, rgbaColor=rgba, physicsClientId=self.physics_client)
 
-    def settle_and_measure_height(self, steps: int = 200, fallback_height: float = 0.122) -> float:
+    def settle_and_measure_height(
+        self, target_angles: Optional[np.ndarray] = None, steps: int = 100, fallback_height: float = 0.122
+    ) -> float:
+        """Settles the robot into the ground while actively holding target joint angles."""
         for _ in range(steps):
+            if target_angles is not None:
+                self.set_robot_joint_angles(target_angles)
             self.step()
         pos, _ = self.get_robot_pose()
         return pos[2] if pos[2] > 0.0 else fallback_height