|
| 1 | +import math |
| 2 | +from typing import Any, Dict, List |
| 3 | + |
| 4 | +import numpy as np |
| 5 | +from internutopia.core.robot.articulation import ArticulationAction |
| 6 | +from internutopia.core.robot.controller import BaseController |
| 7 | +from internutopia.core.robot.robot import BaseRobot |
| 8 | +from internutopia.core.scene.scene import IScene |
| 9 | + |
| 10 | +from internnav.evaluator.utils.path_plan import world_to_pixel |
| 11 | + |
| 12 | +from ..configs.controllers.flash_controller import VlnMoveByFlashControllerCfg |
| 13 | + |
| 14 | + |
| 15 | +@BaseController.register('VlnMoveByFlashCollisionController') |
| 16 | +class VlnMoveByFlashCollisionController(BaseController): # codespell:ignore |
| 17 | + """ |
| 18 | + Discrete Controller, direct set robot world position to achieve teleport-type locomotion. |
| 19 | + This controller adds collision checking based on depth map from a top-down camera before each flash move. |
| 20 | + If there is an obstacle at the target position, the flash action will be aborted. |
| 21 | + a general controller adaptable to different type of robots. |
| 22 | + """ |
| 23 | + |
| 24 | + def __init__(self, config: VlnMoveByFlashControllerCfg, robot: BaseRobot, scene: IScene) -> None: |
| 25 | + self._user_config = None |
| 26 | + self.current_steps = 0 |
| 27 | + self.steps_per_action = config.steps_per_action if config.steps_per_action is not None else 200 |
| 28 | + |
| 29 | + self.forward_distance = config.forward_distance if config.forward_distance is not None else 0.25 |
| 30 | + self.rotation_angle = config.rotation_angle if config.rotation_angle is not None else 15.0 # in degrees |
| 31 | + self.physics_frequency = config.physics_frequency if config.physics_frequency is not None else 240 |
| 32 | + |
| 33 | + self.forward_speed = self.forward_distance / self.steps_per_action * self.physics_frequency |
| 34 | + self.rotation_speed = np.deg2rad( |
| 35 | + self.rotation_angle / self.steps_per_action * self.physics_frequency |
| 36 | + ) # 200 is the physics_dt |
| 37 | + |
| 38 | + self.current_action = None |
| 39 | + |
| 40 | + super().__init__(config=config, robot=robot, scene=scene) |
| 41 | + |
| 42 | + def get_new_position_and_rotation(self, robot_position, robot_rotation, action): |
| 43 | + """ |
| 44 | + Calculate robot new state by previous state and action. The move should be based on the controller |
| 45 | + settings. |
| 46 | + Caution: the rotation need to reset pitch and roll to prevent robot falling. This may due to no |
| 47 | + adjustment during the whole path and some rotation accumulated |
| 48 | +
|
| 49 | + Args: |
| 50 | + robot_position (np.ndarray): Current world position of the robot, shape (3,), in [x, y, z] format. |
| 51 | + robot_rotation (np.ndarray): Current world orientation of the robot as a quaternion, shape (4,), in [x, y, z, w] format. |
| 52 | + action (int): Discrete action to apply: |
| 53 | + - 0: no movement (stand still) |
| 54 | + - 1: move forward |
| 55 | + - 2: rotate left |
| 56 | + - 3: rotate right |
| 57 | +
|
| 58 | + Returns: |
| 59 | + Tuple[np.ndarray, np.ndarray]: The new robot position and rotation as (position, rotation), |
| 60 | + both in world frame. |
| 61 | + """ |
| 62 | + from omni.isaac.core.utils.rotations import ( |
| 63 | + euler_angles_to_quat, |
| 64 | + quat_to_euler_angles, |
| 65 | + ) |
| 66 | + |
| 67 | + _, _, yaw = quat_to_euler_angles(robot_rotation) |
| 68 | + if action == 1: # forward |
| 69 | + dx = self.forward_distance * math.cos(yaw) |
| 70 | + dy = self.forward_distance * math.sin(yaw) |
| 71 | + new_robot_position = robot_position + [dx, dy, 0] |
| 72 | + new_robot_rotation = robot_rotation |
| 73 | + elif action == 2: # left |
| 74 | + new_robot_position = robot_position |
| 75 | + new_yaw = yaw + math.radians(self.rotation_angle) |
| 76 | + new_robot_rotation = euler_angles_to_quat( |
| 77 | + np.array([0.0, 0.0, new_yaw]) |
| 78 | + ) # using 0 to prevent the robot from falling |
| 79 | + elif action == 3: # right |
| 80 | + new_robot_position = robot_position |
| 81 | + new_yaw = yaw - math.radians(self.rotation_angle) |
| 82 | + new_robot_rotation = euler_angles_to_quat(np.array([0.0, 0.0, new_yaw])) |
| 83 | + else: |
| 84 | + new_robot_position = robot_position |
| 85 | + new_robot_rotation = robot_rotation |
| 86 | + |
| 87 | + return new_robot_position, new_robot_rotation |
| 88 | + |
| 89 | + def reset_robot_state(self, position, orientation): |
| 90 | + """ |
| 91 | + Set robot state to the new position and orientation. |
| 92 | +
|
| 93 | + Args: |
| 94 | + position, orientation: np.array, issac_robot.get_world_pose() |
| 95 | + """ |
| 96 | + robot = self.robot.articulation |
| 97 | + robot._articulation.set_world_pose(position=position, orientation=orientation) |
| 98 | + robot._articulation.set_world_velocity(np.zeros(6)) |
| 99 | + robot._articulation.set_joint_velocities(np.zeros(len(robot.dof_names))) |
| 100 | + robot._articulation.set_joint_positions(np.zeros(len(robot.dof_names))) |
| 101 | + robot._articulation.set_joint_efforts(np.zeros(len(robot.dof_names))) |
| 102 | + |
| 103 | + def get_map_info(self, topdown_global_map_camera): |
| 104 | + """ |
| 105 | + Generate a binary free-space map from a top-down depth camera. Key function for collision checking. |
| 106 | +
|
| 107 | + This function converts depth observations from a top-down global map camera |
| 108 | + into a 2D binary occupancy map, where free space is determined by height |
| 109 | + thresholds relative to the robot base. |
| 110 | +
|
| 111 | + Args: |
| 112 | + topdown_global_map_camera: A top-down depth camera instance providing |
| 113 | + depth observations via `get_data()`. |
| 114 | +
|
| 115 | + Returns: |
| 116 | + np.ndarray: |
| 117 | + A 2D binary map with the same spatial resolution as the input depth |
| 118 | + image. Values are: |
| 119 | + - 1: free space |
| 120 | + - 0: occupied or invalid space |
| 121 | + """ |
| 122 | + |
| 123 | + min_height = self.robot.get_robot_base().get_world_pose()[0][2] + 0.6 # default robot height |
| 124 | + max_height = 1.55 + 8 |
| 125 | + data_info = topdown_global_map_camera.get_data() |
| 126 | + depth = np.array(data_info['depth']) |
| 127 | + flat_surface_mask = np.ones_like(depth, dtype=bool) |
| 128 | + if self.robot.config.type == 'VLNH1Robot': |
| 129 | + depth_mask = ((depth >= min_height) & (depth < max_height)) | ((depth <= 0.5) & (depth > 0.02)) |
| 130 | + elif self.robot.config.type == 'VLNAliengoRobot': |
| 131 | + base_height = self.robot.get_robot_base().get_world_pose()[0][2] |
| 132 | + foot_height = self.robot.get_ankle_height() |
| 133 | + min_height = base_height - foot_height + 0.05 |
| 134 | + depth_mask = (depth >= min_height) & (depth < max_height) |
| 135 | + free_map = np.zeros_like(depth, dtype=int) |
| 136 | + free_map[flat_surface_mask & depth_mask] = 1 # 1: free, 0: occupied |
| 137 | + return free_map |
| 138 | + |
| 139 | + def check_collision(self, position, aperture=200) -> bool: |
| 140 | + """ |
| 141 | + Check if there are any obstacles at the position. |
| 142 | + Generate a depth map based on a top down camera and check the position |
| 143 | +
|
| 144 | + Return: |
| 145 | + bool: True if the position is already occupied |
| 146 | + """ |
| 147 | + topdown_global_map_camera = self.robot.sensors['topdown_camera_500'] |
| 148 | + free_map = self.get_map_info(topdown_global_map_camera, dilation_iterations=2) |
| 149 | + |
| 150 | + # convert position to free map pixel |
| 151 | + camera_pose = topdown_global_map_camera.get_world_pose()[0] |
| 152 | + width, height = topdown_global_map_camera.resolution |
| 153 | + px, py = world_to_pixel(position, camera_pose, aperture, width, height) |
| 154 | + |
| 155 | + px_int, py_int = int(px), int(py) |
| 156 | + # Get a region: (px, py) and one pixel right/down |
| 157 | + robot_size = 3 |
| 158 | + sub_map = free_map[px_int - robot_size : px_int + robot_size, py_int - robot_size : py_int + robot_size] |
| 159 | + return np.any(sub_map == 0) # 1 = free, so (any 0) = collision exists |
| 160 | + |
| 161 | + def forward(self, action: int) -> ArticulationAction: |
| 162 | + """ |
| 163 | + Teleport robot by position, orientation and action |
| 164 | +
|
| 165 | + Args: |
| 166 | + action: int |
| 167 | + 0. discrete action (int): 0: stop, 1: forward, 2: left, 3: right |
| 168 | +
|
| 169 | + Returns: |
| 170 | + ArticulationAction: joint signals to apply (nothing). |
| 171 | + """ |
| 172 | + # get robot new position |
| 173 | + positions, orientations = self.robot.articulation.get_world_pose() |
| 174 | + new_robot_position, new_robot_rotation = self.get_new_position_and_rotation(positions, orientations, action) |
| 175 | + |
| 176 | + # Check if there is a collision with obstacles. Abort the teleport if there is |
| 177 | + if action != 1 or not self.check_collision(new_robot_position): |
| 178 | + # set robot to new state |
| 179 | + self.reset_robot_state(new_robot_position, new_robot_rotation) |
| 180 | + else: |
| 181 | + print("[FLASH CONTROLLER]: Collision detected, flash abort") |
| 182 | + |
| 183 | + # Dummy action to do nothing |
| 184 | + return ArticulationAction() |
| 185 | + |
| 186 | + def action_to_control(self, action: List | np.ndarray) -> ArticulationAction: |
| 187 | + """ |
| 188 | + Convert input action (in 1d array format) to joint signals to apply. |
| 189 | +
|
| 190 | + Args: |
| 191 | + action (List | np.ndarray): 1-element 1d array containing |
| 192 | + 0. discrete action (int): 0: stop, 1: forward, 2: left, 3: right |
| 193 | +
|
| 194 | + Returns: |
| 195 | + ArticulationAction: joint signals to apply. |
| 196 | + """ |
| 197 | + assert len(action) == 1, 'action must contain 1 element' |
| 198 | + return self.forward(action=int(action[0])) |
| 199 | + |
| 200 | + def get_obs(self) -> Dict[str, Any]: |
| 201 | + return { |
| 202 | + 'finished': True, |
| 203 | + } |
0 commit comments