fix a few issues introduced with refactoring

Author: danielsanjosepro

crisp_gym/envs/env_wrapper.py

@@ -17,7 +17,7 @@
 import rclpy
 from numpy.typing import NDArray
 
-from crisp_gym.envs.envs.manipulator_env import ManipulatorBaseEnv
+from crisp_gym.envs.manipulator_env import ManipulatorBaseEnv
 
 
 def stack_gym_space(space: gym.Space, repeat: int) -> gym.Space:

crisp_gym/envs/manipulator_env.py

@@ -28,13 +28,14 @@
 from pathlib import Path
 from typing import Any, List, Tuple
 
+from crisp_py.utils.geometry import OrientationRepresentation
 import gymnasium as gym
 import numpy as np
 import rclpy
 from crisp_py.camera import Camera
 from crisp_py.gripper import Gripper
 from crisp_py.robot import Pose, Robot
-from crisp_py.sensors.sensor import make_sensor
+from crisp_py.sensors.sensor import Sensor, make_sensor
 from numpy.typing import NDArray
 from scipy.spatial.transform import Rotation
 from typing_extensions import override
@@ -90,7 +91,7 @@ def __init__(self, config: ManipulatorEnvConfig, namespace: str = ""):
             for camera_config in self.config.camera_configs
         ]
         self.sensors: List = [
-            make_sensor(
+            Sensor(
                 namespace=namespace,
                 sensor_config=sensor_config,
             )
@@ -432,7 +433,7 @@ def action_to_rotation(self, rot_action: np.ndarray) -> Rotation:
 
         Returns:
             Rotation: A scipy Rotation object representing the rotation.
-        """  # noqa: D205
+        """
         if self.config.orientation_representation == OrientationRepresentation.EULER:
             return Rotation.from_euler("xyz", rot_action)
         elif self.config.orientation_representation == OrientationRepresentation.QUATERNION:
@@ -446,8 +447,10 @@ def action_to_rotation(self, rot_action: np.ndarray) -> Rotation:
 
     def clip_position_for_safety(self, position: np.ndarray) -> np.ndarray:
         """Clip the position to ensure safety.
+
         Args:
             position (np.ndarray): The position to be clipped.
+
         Returns:
             np.ndarray: The clipped position.
         """
@@ -534,7 +537,8 @@ def step(self, action: np.ndarray, block: bool = True) -> Tuple[dict, float, boo
         """Step the environment with a Cartesian action.
 
         Args:
-            action (np.ndarray): Cartesian delta action [dx, dy, dz, roll, pitch, yaw, gripper_action].
+            action (np.ndarray): Cartesian delta action [dx, dy, dz, *d_rot_action, gripper_action],
+                                where d_rot_action dimension depends on the chosen orientation representation.
             block (bool): If True, block to maintain the control rate.
 
         Returns:
@@ -543,12 +547,12 @@ def step(self, action: np.ndarray, block: bool = True) -> Tuple[dict, float, boo
         assert action.shape == self.action_space.shape, (
             f"Action shape {action.shape} does not match expected shape {self.action_space.shape}"
         )
-        # assert self.action_space.contains(action), f"Action {action} is not in the action space {self.action_space}"
+        translation = action[:3]
+        rotation = self.action_to_rotation(action[3:-1])
 
-        translation, rotation = action[:3], Rotation.from_euler("xyz", action[3:6])
-
-        target_position = self.robot.target_pose.position + translation
-        target_position[2] = max(target_position[2], self._min_z_height)
+        target_position = self.clip_position_for_safety(
+            self.robot.target_pose.position + translation
+        )
         target_orientation = rotation * self.robot.target_pose.orientation
 
         target_pose = Pose(position=target_position, orientation=target_orientation)

crisp_gym/envs/manipulator_env_config.py

@@ -27,7 +27,7 @@ class ObservationKeys:
     JOINT_OBS = STATE_OBS + ".joints"
     CARTESIAN_OBS = STATE_OBS + ".cartesian"
     TARGET_OBS = STATE_OBS + ".target"
-    SENSOR_OBS = STATE_OBS + ".sensors"
+    SENSOR_OBS = STATE_OBS + ".sensor"
 
     IMAGE_OBS = "observation.images"
 
@@ -172,22 +172,32 @@ def from_yaml(cls, yaml_path: Path, **overrides) -> "ManipulatorEnvConfig":  # n
         Returns:
             ManipulatorEnvConfig: Configured environment instance
         """
-        # TODO: @danielsanjosepro Better validation of YAML contents
         with open(yaml_path, "r") as f:
-            data = yaml.safe_load(f) or {}
+            original_data = yaml.safe_load(f) or {}
 
-        # Apply overrides
-        data.update(overrides)
+        original_data.update(overrides)
 
-        # Handle nested configs that need special treatment
-        if "robot_config" in data and isinstance(data["robot_config"], dict):
-            # Use make_robot_config to handle different robot types
-            data["robot_config"] = make_robot_config(**data["robot_config"])
+        data = dict(original_data)  # Make a shallow copy to modify
 
-        if "gripper_config" in data and isinstance(data["gripper_config"], dict):
+        if "robot_config" in data:
+            if not isinstance(data["robot_config"], dict):
+                raise ValueError("robot_config must be a dictionary in the YAML file.")
+
+            if "from_yaml" in data["robot_config"]:
+                robot_yaml_path = find_config(data["robot_config"]["from_yaml"])
+                if robot_yaml_path is None:
+                    raise FileNotFoundError(
+                        f"Robot config file '{data['robot_config']['from_yaml']}' not found in any CRISP config paths"
+                    )
+                data["robot_config"] = RobotConfig.from_yaml(yaml_path=robot_yaml_path.resolve())
+            else:
+                data["robot_config"] = make_robot_config(**data["robot_config"])
+
+        if "gripper_config" in data:
             gripper_cfg = data["gripper_config"]
+            if not isinstance(gripper_cfg, dict):
+                raise ValueError("gripper_config must be a dictionary in the YAML file.")
             if "from_yaml" in gripper_cfg:
-                # Load from external YAML file
                 gripper_yaml_path = find_config(gripper_cfg["from_yaml"])
                 if gripper_yaml_path is None:
                     raise FileNotFoundError(
@@ -198,16 +208,38 @@ def from_yaml(cls, yaml_path: Path, **overrides) -> "ManipulatorEnvConfig":  # n
                 data["gripper_config"] = GripperConfig(**gripper_cfg)
 
         if "camera_configs" in data and isinstance(data["camera_configs"], list):
-            data["camera_configs"] = [
-                CameraConfig(**cam_cfg) if isinstance(cam_cfg, dict) else cam_cfg
-                for cam_cfg in data["camera_configs"]
-            ]
+            data["camera_configs"] = []  # Reset to fill in properly
+            for camera_cfg in original_data["camera_configs"]:
+                if "from_yaml" in camera_cfg:
+                    camera_yaml_path = find_config(camera_cfg["from_yaml"])
+                    if camera_yaml_path is None:
+                        raise FileNotFoundError(
+                            f"Camera config file '{camera_cfg['from_yaml']}' not found in any CRISP config paths"
+                        )
+                    cam_config = CameraConfig.from_yaml(yaml_path=camera_yaml_path.resolve())
+                    data["camera_configs"].append(cam_config)
+                else:
+                    data["camera_configs"].append(
+                        CameraConfig(**camera_cfg) if isinstance(camera_cfg, dict) else camera_cfg
+                    )
 
         if "sensor_configs" in data and isinstance(data["sensor_configs"], list):
-            data["sensor_configs"] = [
-                SensorConfig(**sensor_cfg) if isinstance(sensor_cfg, dict) else sensor_cfg
-                for sensor_cfg in data["sensor_configs"]
-            ]
+            data["sensor_configs"] = []
+            for sensor_config in original_data["sensor_configs"]:
+                if "from_yaml" in sensor_config:
+                    sensor_yaml_path = find_config(sensor_config["from_yaml"])
+                    if sensor_yaml_path is None:
+                        raise FileNotFoundError(
+                            f"Sensor config file '{sensor_config['from_yaml']}' not found in any CRISP config paths"
+                        )
+                    sensor_cfg = SensorConfig.from_yaml(yaml_path=sensor_yaml_path.resolve())
+                    data["sensor_configs"].append(sensor_cfg)
+                else:
+                    data["sensor_configs"].append(
+                        SensorConfig(**sensor_config)
+                        if isinstance(sensor_config, dict)
+                        else sensor_config
+                    )
 
         return cls(**data)