From 457a8be2fbe5b893107b47d01b62346c09a07bfe Mon Sep 17 00:00:00 2001 From: Steven Shi Date: Tue, 4 Jul 2023 21:43:45 -0500 Subject: [PATCH 01/28] add metrics to simulator --- omnigibson/envs/env_base.py | 30 ++++++++- omnigibson/reward_functions/__init__.py | 1 + omnigibson/reward_functions/energy_metric.py | 58 +++++++++++++++++ omnigibson/reward_functions/step_metric.py | 17 +++++ .../reward_functions/task_success_metric.py | 30 +++++++++ .../reward_functions/wall_time_metric.py | 17 +++++ omnigibson/tasks/task_base.py | 62 +++++++++++++++++++ .../termination_conditions/predicate_goal.py | 10 +++ .../termination_condition_base.py | 9 +++ 9 files changed, 233 insertions(+), 1 deletion(-) create mode 100644 omnigibson/reward_functions/energy_metric.py create mode 100644 omnigibson/reward_functions/step_metric.py create mode 100644 omnigibson/reward_functions/task_success_metric.py create mode 100644 omnigibson/reward_functions/wall_time_metric.py diff --git a/omnigibson/envs/env_base.py b/omnigibson/envs/env_base.py index 82a25747e..07342584e 100644 --- a/omnigibson/envs/env_base.py +++ b/omnigibson/envs/env_base.py @@ -12,6 +12,11 @@ from omnigibson.utils.python_utils import assert_valid_key, merge_nested_dicts, create_class_from_registry_and_config,\ Recreatable +from omnigibson.utils.python_utils import assert_valid_key, merge_nested_dicts, create_class_from_registry_and_config,\ + Recreatable + +import time + # Create module logger log = create_module_logger(module_name=__name__) @@ -58,6 +63,9 @@ def __init__( self._loaded = None self._current_episode = 0 + self._prev_sim_end_ts = 0 + self._cur_sim_start_ts = 0 + # Variables reset at the beginning of each episode self._current_step = 0 @@ -364,7 +372,7 @@ def _populate_info(self, info): """ info["episode_length"] = self._current_step - def step(self, action): + def step(self, action, time_step=False): """ Apply robot's action and return the next state, reward, done and info, following OpenAI Gym's convention @@ -403,8 +411,15 @@ def step(self, action): # Grab observations obs = self.get_obs() + if time_step: + self._cur_sim_start_ts = time.clock() + # Grab reward, done, and info, and populate with internal info reward, done, info = self.task.step(self, action) + + if time_step: + self._prev_sim_end_ts = time.clock() + self._populate_info(info) if done and self._automatic_reset: @@ -423,6 +438,9 @@ def _reset_variables(self): """ self._current_episode += 1 self._current_step = 0 + self._prev_sim_end_ts = 0 + self._cur_sim_start_ts = 0 + # TODO: Match super class signature? def reset(self): @@ -453,6 +471,16 @@ def reset(self): return obs + @property + def wall_time_step(self): + """ + Returns: + int: wall clock step duration + """ + if self._prev_sim_end_ts == 0 or self._cur_sim_start_ts == 0: + return 0 + return self._cur_sim_start_ts - self._prev_sim_end_ts + @property def episode_steps(self): """ diff --git a/omnigibson/reward_functions/__init__.py b/omnigibson/reward_functions/__init__.py index d667bfd0b..2edad9577 100644 --- a/omnigibson/reward_functions/__init__.py +++ b/omnigibson/reward_functions/__init__.py @@ -3,3 +3,4 @@ from omnigibson.reward_functions.point_goal_reward import PointGoalReward from omnigibson.reward_functions.potential_reward import PotentialReward from omnigibson.reward_functions.reaching_goal_reward import ReachingGoalReward +from omnigibson.reward_functions.step_metric import StepMetric diff --git a/omnigibson/reward_functions/energy_metric.py b/omnigibson/reward_functions/energy_metric.py new file mode 100644 index 000000000..e6f95e671 --- /dev/null +++ b/omnigibson/reward_functions/energy_metric.py @@ -0,0 +1,58 @@ +from omnigibson.reward_functions.reward_function_base import BaseRewardFunction +import numpy as np + + +class EnergyMetric(BaseRewardFunction): + """ + Energy Metric + + Measures displacement * mass for every link + + Args: + measure_work: If true, measure beginning and end delta rather than step by step delta + """ + + def __init__(self, measure_work=False): + # Run super + super().__init__() + self._reward = 0 + self.initialized = False + self.state_cache = {} + self.link_masses = {} + self.measure_work = measure_work + + def calculate_displacement(self, posrot, posrot2): + return np.linalg.norm(posrot[0] - posrot2[0]) + + def _step(self, task, env, action): + new_state_cache = {} + for obj in env.scene.objects: + for link_name, link in obj._links.items(): + pos, rot = link.get_position_orientation() + new_state_cache[link_name] = (pos, rot) + + if not self.initialized: + self.initialized = True + self.state_cache = new_state_cache + + for obj in env.scene.objects: + for link_name, link in obj._links.items(): + self.link_masses[link_name] = link.mass + return 0.0, {} + + work_metric = 0.0 + for linkname, posrot in new_state_cache.items(): + work_metric += self.calculate_displacement(posrot, self.state_cache[linkname]) * self.link_masses[linkname] + + if self.measure_work: + self._reward = 0 + if not self.measure_work: + self.state_cache = new_state_cache + + self._reward += work_metric + return self._reward, {} + + def reset(self, task, env): + super().reset(task, env) + self.state_cache = {} + self.initialized = False diff --git a/omnigibson/reward_functions/step_metric.py b/omnigibson/reward_functions/step_metric.py new file mode 100644 index 000000000..17fb8b30d --- /dev/null +++ b/omnigibson/reward_functions/step_metric.py @@ -0,0 +1,17 @@ +from omnigibson.reward_functions.reward_function_base import BaseRewardFunction + + +class StepMetric(BaseRewardFunction): + """ + Step Metric + Metric for each simulator step + """ + + def __init__(self): + # Run super + super().__init__() + self._reward = 0 + + def _step(self, task, env, action): + self._reward += 1 + return self._reward, {} diff --git a/omnigibson/reward_functions/task_success_metric.py b/omnigibson/reward_functions/task_success_metric.py new file mode 100644 index 000000000..c09c82c10 --- /dev/null +++ b/omnigibson/reward_functions/task_success_metric.py @@ -0,0 +1,30 @@ +from omnigibson.reward_functions.reward_function_base import BaseRewardFunction + +class TaskSuccessMetric(BaseRewardFunction): + """ + TaskSuccessMetric + Metric for partial or full task success + """ + + def __init__(self): + # Run super + super().__init__() + self._reward = 0 + + def _step(self, task, env, action): + successes = [] + partial_successes = [] + for termination_condition in task._termination_conditions.values(): + if termination_condition.partial_success >= 0.0: + partial_successes.append(termination_condition.partial_success) + done, success = termination_condition.step(task, env, action) + # success <=> done and non failure + successes.append(success) + if sum(successes) > 0: + self._reward = 1.0 + elif partial_successes: + self._reward = sum(partial_successes) / len(partial_successes) + else: + self._reward = 0.0 + # Populate info + return self._reward, {} diff --git a/omnigibson/reward_functions/wall_time_metric.py b/omnigibson/reward_functions/wall_time_metric.py new file mode 100644 index 000000000..8d5d8da64 --- /dev/null +++ b/omnigibson/reward_functions/wall_time_metric.py @@ -0,0 +1,17 @@ +from omnigibson.reward_functions.reward_function_base import BaseRewardFunction + + +class WallTimeMetric(BaseRewardFunction): + """ + WallTimeMetric + Metric for wall time accumulated in policy steps + """ + + def __init__(self): + # Run super + super().__init__() + self._reward = 0 + + def _step(self, task, env, action): + self._reward += env.wall_time_step + return self._reward, {} diff --git a/omnigibson/tasks/task_base.py b/omnigibson/tasks/task_base.py index 6ec84aeb3..367e44623 100644 --- a/omnigibson/tasks/task_base.py +++ b/omnigibson/tasks/task_base.py @@ -5,6 +5,11 @@ from omnigibson.utils.gym_utils import GymObservable +from omnigibson.reward_functions.step_metric import StepMetric +from omnigibson.reward_functions.task_success_metric import TaskSuccessMetric +from omnigibson.reward_functions.wall_time_metric import WallTimeMetric +from omnigibson.reward_functions.energy_metric import EnergyMetric + REGISTERED_TASKS = dict() @@ -45,6 +50,7 @@ def __init__(self, termination_config=None, reward_config=None): # Generate reward and termination functions self._termination_conditions = self._create_termination_conditions() self._reward_functions = self._create_reward_functions() + self._metric_functions = self._create_metric_functions() # Store other internal vars that will be populated at runtime self._loaded = False @@ -132,6 +138,23 @@ def _create_reward_functions(self): """ raise NotImplementedError() + def _create_metric_functions(self): + """ + Creates the metric functions in the environment + + Returns: + dict of BaseRewardFunction: Metric functions created for this task + """ + metrics = dict() + + metrics['steps'] = StepMetric() + metrics['task_success'] = TaskSuccessMetric() + metrics['wall_time'] = WallTimeMetric() + metrics['energy'] = EnergyMetric() + metrics['work'] = EnergyMetric(measure_work=True) + + return metrics + def _reset_scene(self, env): """ Task-specific scene reset. Default is the normal scene reset @@ -180,6 +203,10 @@ def reset(self, env): termination_condition.reset(self, env) for reward_function in self._reward_functions.values(): reward_function.reset(self, env) + for metric_function in self._metric_functions.values(): + metric_function.reset(self, env) + metric_function._reward = 0.0 + # Fill in low dim obs dim so we can use this to create the observation space later self._low_dim_obs_dim = len(self.get_obs(env=env, flatten_low_dim=True)["low_dim"]) @@ -245,6 +272,38 @@ def _step_reward(self, env, action, info=None): return total_reward, total_info + def _step_metrics(self, env, action, info=None): + """ + Step and aggregate metric functions + + Args: + env (Environment): Environment instance + action (n-array): 1D flattened array of actions executed by all agents in the environment + info (None or dict): Any info to return + + Returns: + 2-tuple: + - list[float]: metrics at the current timestep + - dict: any information passed through this function or generated by this function + """ + # Make sure info is a dict + total_info = dict() if info is None else info + # We'll also store individual reward split as well + breakdown_dict = dict() + # Aggregate rewards over all reward functions + total_metric = 0.0 + for metric_name, metric_function in self._metric_functions.items(): + metric, metric_info = metric_function.step(self, env, action) + total_metric += metric + breakdown_dict[metric_name] = metric + total_info[metric] = metric_info + + # Store breakdown dict + total_info["metric_breakdown"] = breakdown_dict + + return total_metric, total_info + + @abstractmethod def _get_obs(self, env): """ @@ -307,11 +366,14 @@ def step(self, env, action): # (since some rewards can rely on termination conditions to update) done, done_info = self._step_termination(env=env, action=action) reward, reward_info = self._step_reward(env=env, action=action) + metric_score, metrics_info = self._step_metrics(env=env, action=action) # Update the internal state of this task self._reward = reward + self._metrics = metric_score self._done = done self._info = { + "metrics": metrics_info, "reward": reward_info, "done": done_info, } diff --git a/omnigibson/termination_conditions/predicate_goal.py b/omnigibson/termination_conditions/predicate_goal.py index 81f3f273d..68a8accfc 100644 --- a/omnigibson/termination_conditions/predicate_goal.py +++ b/omnigibson/termination_conditions/predicate_goal.py @@ -43,3 +43,13 @@ def goal_status(self): of the predicates matching either of those conditions """ return self._goal_status + + + @property + def partial_success(self): + """ + Returns: + float: partial success if supposed, -1.0 otherwise + """ + assert self._done is not None, "At least one step() must occur before partial_success can be calculated!" + return len(satisfied) / (len(satisfied) + len(unsatisfied)) diff --git a/omnigibson/termination_conditions/termination_condition_base.py b/omnigibson/termination_conditions/termination_condition_base.py index f0860fd46..ed86e5178 100644 --- a/omnigibson/termination_conditions/termination_condition_base.py +++ b/omnigibson/termination_conditions/termination_condition_base.py @@ -91,6 +91,15 @@ def success(self): assert self._done is not None, "At least one step() must occur before success can be calculated!" return self._done and self._terminate_is_success + @property + def partial_success(self): + """ + Returns: + float: partial success if supposed, -1.0 otherwise + """ + assert self._done is not None, "At least one step() must occur before partial_success can be calculated!" + return -1.0 + @classproperty def _terminate_is_success(cls): """ From 1910c1bf4f35399539dde97953b337e7ed013f41 Mon Sep 17 00:00:00 2001 From: Steven Shi Date: Tue, 18 Jul 2023 21:44:01 -0500 Subject: [PATCH 02/28] build-push action for docker --- .github/workflows/ci.yml | 29 +++++++++++++++++++++++++++++ 1 file changed, 29 insertions(+) create mode 100644 .github/workflows/ci.yml diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml new file mode 100644 index 000000000..6a48d1877 --- /dev/null +++ b/.github/workflows/ci.yml @@ -0,0 +1,29 @@ +name: ci + +on: + push: + branches: + - 'main' + +jobs: + docker: + runs-on: ubuntu-latest + steps: + - + name: Set up QEMU + uses: docker/setup-qemu-action@v2 + - + name: Set up Docker Buildx + uses: docker/setup-buildx-action@v2 + - + name: Login to Docker Hub + uses: docker/login-action@v2 + with: + username: ${{ secrets.DOCKERHUB_USERNAME }} + password: ${{ secrets.DOCKERHUB_TOKEN }} + - + name: Build and push + uses: docker/build-push-action@v4 + with: + push: true + tags: user/app:latest From b536a4c8340eff4c77cf84fba2a43c6a022740e7 Mon Sep 17 00:00:00 2001 From: Frank Yang Date: Tue, 30 Jul 2024 14:39:13 -0700 Subject: [PATCH 03/28] action primitives initial commit --- omnigibson/envs/env_base.py | 20 +++-- omnigibson/reward_functions/__init__.py | 3 + omnigibson/tasks/task_base.py | 30 ++------ .../termination_conditions/predicate_goal.py | 2 +- .../termination_condition_base.py | 4 +- scripts/setup.sh | 76 +++++++++---------- 6 files changed, 64 insertions(+), 71 deletions(-) diff --git a/omnigibson/envs/env_base.py b/omnigibson/envs/env_base.py index faecc706e..4981b9263 100644 --- a/omnigibson/envs/env_base.py +++ b/omnigibson/envs/env_base.py @@ -548,8 +548,13 @@ def _populate_info(self, info): if self._scene_graph_builder is not None: info["scene_graph"] = self.get_scene_graph() - def _pre_step(self, action): + def _pre_step(self, action, time_step=False): """Apply the pre-sim-step part of an environment step, i.e. apply the robot actions.""" + + # record the start time + if time_step: + self._cur_sim_start_ts = time.clock() + # If the action is not a dictionary, convert into a dictionary if not isinstance(action, dict) and not isinstance(action, gym.spaces.Dict): action_dict = dict() @@ -575,15 +580,9 @@ def _post_step(self, action, time_step=False): if self._scene_graph_builder is not None: self._scene_graph_builder.step(self.scene) - if time_step: - self._cur_sim_start_ts = time.clock() - # Grab reward, done, and info, and populate with internal info reward, done, info = self.task.step(self, action) - if time_step: - self._prev_sim_end_ts = time.clock() - self._populate_info(info) info["obs_info"] = obs_info @@ -605,6 +604,11 @@ def _post_step(self, action, time_step=False): # Increment step self._current_step += 1 + + # record end time + if time_step: + self._prev_sim_end_ts = time.clock() + return obs, reward, terminated, truncated, info def step(self, action, time_step=False): @@ -625,7 +629,7 @@ def step(self, action, time_step=False): - bool: truncated, i.e. whether this episode ended due to a time limit etc. - dict: info, i.e. dictionary with any useful information """ - self._pre_step(action) + self._pre_step(action, time_step=time_step) og.sim.step() return self._post_step(action, time_step=time_step) diff --git a/omnigibson/reward_functions/__init__.py b/omnigibson/reward_functions/__init__.py index fbae36401..ca392da51 100644 --- a/omnigibson/reward_functions/__init__.py +++ b/omnigibson/reward_functions/__init__.py @@ -4,4 +4,7 @@ from omnigibson.reward_functions.potential_reward import PotentialReward from omnigibson.reward_functions.reaching_goal_reward import ReachingGoalReward from omnigibson.reward_functions.step_metric import StepMetric +from omnigibson.reward_functions.wall_time_metric import WallTimeMetric +from omnigibson.reward_functions.energy_metric import EnergyMetric +from omnigibson.reward_functions.task_success_metric import TaskSuccessMetric from omnigibson.reward_functions.reward_function_base import REGISTERED_REWARD_FUNCTIONS, BaseRewardFunction diff --git a/omnigibson/tasks/task_base.py b/omnigibson/tasks/task_base.py index 217fd0ca9..10a18dbf7 100644 --- a/omnigibson/tasks/task_base.py +++ b/omnigibson/tasks/task_base.py @@ -6,10 +6,7 @@ from omnigibson.utils.gym_utils import GymObservable from omnigibson.utils.python_utils import Registerable, classproperty -from omnigibson.reward_functions.step_metric import StepMetric -from omnigibson.reward_functions.task_success_metric import TaskSuccessMetric -from omnigibson.reward_functions.wall_time_metric import WallTimeMetric -from omnigibson.reward_functions.energy_metric import EnergyMetric +from omnigibson.reward_functions import StepMetric, TaskSuccessMetric, WallTimeMetric, EnergyMetric REGISTERED_TASKS = dict() @@ -288,36 +285,26 @@ def _step_reward(self, env, action, info=None): return total_reward, total_info - def _step_metrics(self, env, action, info=None): + def _step_metrics(self, env, action): """ Step and aggregate metric functions Args: env (Environment): Environment instance action (n-array): 1D flattened array of actions executed by all agents in the environment - info (None or dict): Any info to return Returns: - 2-tuple: - - list[float]: metrics at the current timestep - - dict: any information passed through this function or generated by this function + - the break down of the metric scores and the metric info """ - # Make sure info is a dict - total_info = dict() if info is None else info - # We'll also store individual reward split as well + + # We'll also store individual reward split breakdown_dict = dict() - # Aggregate rewards over all reward functions - total_metric = 0.0 + for metric_name, metric_function in self._metric_functions.items(): - metric, metric_info = metric_function.step(self, env, action) - total_metric += metric + metric, _ = metric_function.step(self, env, action) breakdown_dict[metric_name] = metric - total_info[metric] = metric_info - - # Store breakdown dict - total_info["metric_breakdown"] = breakdown_dict - return total_metric, total_info + return breakdown_dict @abstractmethod @@ -379,7 +366,6 @@ def step(self, env, action): # Make sure we're initialized assert self._loaded, "Task must be loaded using load() before calling step()!" - # We calculate termination conditions first and then rewards # (since some rewards can rely on termination conditions to update) done, done_info = self._step_termination(env=env, action=action) reward, reward_info = self._step_reward(env=env, action=action) diff --git a/omnigibson/termination_conditions/predicate_goal.py b/omnigibson/termination_conditions/predicate_goal.py index ff45f993f..525461c8f 100644 --- a/omnigibson/termination_conditions/predicate_goal.py +++ b/omnigibson/termination_conditions/predicate_goal.py @@ -50,7 +50,7 @@ def goal_status(self): def partial_success(self): """ Returns: - float: partial success if supposed, -1.0 otherwise + float: partial success if supported, -1.0 otherwise """ assert self._done is not None, "At least one step() must occur before partial_success can be calculated!" return len(satisfied) / (len(satisfied) + len(unsatisfied)) diff --git a/omnigibson/termination_conditions/termination_condition_base.py b/omnigibson/termination_conditions/termination_condition_base.py index 01f2f3a69..fbfcbe20a 100644 --- a/omnigibson/termination_conditions/termination_condition_base.py +++ b/omnigibson/termination_conditions/termination_condition_base.py @@ -97,10 +97,10 @@ def success(self): def partial_success(self): """ Returns: - float: partial success if supposed, -1.0 otherwise + float: partial success if supported, None otherwise """ assert self._done is not None, "At least one step() must occur before partial_success can be calculated!" - return -1.0 + return None @classproperty def _terminate_is_success(cls): diff --git a/scripts/setup.sh b/scripts/setup.sh index d46bdd10e..0c6474f2b 100755 --- a/scripts/setup.sh +++ b/scripts/setup.sh @@ -20,49 +20,49 @@ done echo -e "\nUsing Isaac Sim at $ISAAC_SIM_PATH\n" -# # Choose venv name -# echo "The new conda environment will be named omnigibson by default." -# read -p "If you want to use a different name, please type in here (press enter to skip) >>> " conda_name -# conda_name=${conda_name:-omnigibson} -# echo -e "\nUsing $conda_name as the conda environment name\n" +# Choose venv name +echo "The new conda environment will be named omnigibson by default." +read -p "If you want to use a different name, please type in here (press enter to skip) >>> " conda_name +conda_name=${conda_name:-omnigibson} +echo -e "\nUsing $conda_name as the conda environment name\n" -# # Get Python version from Isaac Sim -# ISAAC_PYTHON_VERSION=$(${ISAAC_SIM_PATH}/python.sh -c "import platform; print(platform.python_version())") -# ISAAC_PYTHON_VERSION="${ISAAC_PYTHON_VERSION##*$'\n'}" # get rid of conda activation warnings -# echo Using Python version $ISAAC_PYTHON_VERSION matching your current Isaac Sim version +# Get Python version from Isaac Sim +ISAAC_PYTHON_VERSION=$(${ISAAC_SIM_PATH}/python.sh -c "import platform; print(platform.python_version())") +ISAAC_PYTHON_VERSION="${ISAAC_PYTHON_VERSION##*$'\n'}" # get rid of conda activation warnings +echo Using Python version $ISAAC_PYTHON_VERSION matching your current Isaac Sim version -# # Create a conda environment with the appropriate python version -# source $(conda info --base)/etc/profile.d/conda.sh -# conda create -y -n $conda_name python=${ISAAC_PYTHON_VERSION} +# Create a conda environment with the appropriate python version +source $(conda info --base)/etc/profile.d/conda.sh +conda create -y -n $conda_name python=${ISAAC_PYTHON_VERSION} -# # Now activate the omnigibson environment -# conda activate $conda_name +# Now activate the omnigibson environment +conda activate $conda_name -# mkdir -p ${CONDA_PREFIX}/etc/conda/activate.d -# mkdir -p ${CONDA_PREFIX}/etc/conda/deactivate.d -# touch ${CONDA_PREFIX}/etc/conda/activate.d/env_vars.sh -# touch ${CONDA_PREFIX}/etc/conda/deactivate.d/env_vars.sh -# # We add some preprocessing information so that the Isaac Sim paths are linked to this environment upon startup -# # See https://docs.conda.io/projects/conda/en/latest/user-guide/tasks/manage-environments.html#macos-and-linux for reference -# CONDA_ACT_FILE="${CONDA_PREFIX}/etc/conda/activate.d/env_vars.sh" -# echo '#!/bin/sh' > ${CONDA_ACT_FILE} -# echo "export LD_LIBRARY_PATH_OLD=\$LD_LIBRARY_PATH" >> ${CONDA_ACT_FILE} -# echo "export PYTHONPATH_OLD=\$PYTHONPATH" >> ${CONDA_ACT_FILE} -# echo "source ${ISAAC_SIM_PATH}/setup_conda_env.sh" >> ${CONDA_ACT_FILE} +mkdir -p ${CONDA_PREFIX}/etc/conda/activate.d +mkdir -p ${CONDA_PREFIX}/etc/conda/deactivate.d +touch ${CONDA_PREFIX}/etc/conda/activate.d/env_vars.sh +touch ${CONDA_PREFIX}/etc/conda/deactivate.d/env_vars.sh +# We add some preprocessing information so that the Isaac Sim paths are linked to this environment upon startup +# See https://docs.conda.io/projects/conda/en/latest/user-guide/tasks/manage-environments.html#macos-and-linux for reference +CONDA_ACT_FILE="${CONDA_PREFIX}/etc/conda/activate.d/env_vars.sh" +echo '#!/bin/sh' > ${CONDA_ACT_FILE} +echo "export LD_LIBRARY_PATH_OLD=\$LD_LIBRARY_PATH" >> ${CONDA_ACT_FILE} +echo "export PYTHONPATH_OLD=\$PYTHONPATH" >> ${CONDA_ACT_FILE} +echo "source ${ISAAC_SIM_PATH}/setup_conda_env.sh" >> ${CONDA_ACT_FILE} -# CONDA_DEACT_FILE="${CONDA_PREFIX}/etc/conda/deactivate.d/env_vars.sh" -# echo '#!/bin/sh' > ${CONDA_DEACT_FILE} -# echo "export LD_LIBRARY_PATH=\$LD_LIBRARY_PATH_OLD" >> ${CONDA_DEACT_FILE} -# echo "export PYTHONPATH=\$PYTHONPATH_OLD" >> ${CONDA_DEACT_FILE} -# echo "unset ISAAC_PATH" >> ${CONDA_DEACT_FILE} -# echo "unset CARB_APP_PATH" >> ${CONDA_DEACT_FILE} -# echo "unset LD_LIBRARY_PATH_OLD" >> ${CONDA_DEACT_FILE} -# echo "unset PYTHONPATH_OLD" >> ${CONDA_DEACT_FILE} +CONDA_DEACT_FILE="${CONDA_PREFIX}/etc/conda/deactivate.d/env_vars.sh" +echo '#!/bin/sh' > ${CONDA_DEACT_FILE} +echo "export LD_LIBRARY_PATH=\$LD_LIBRARY_PATH_OLD" >> ${CONDA_DEACT_FILE} +echo "export PYTHONPATH=\$PYTHONPATH_OLD" >> ${CONDA_DEACT_FILE} +echo "unset ISAAC_PATH" >> ${CONDA_DEACT_FILE} +echo "unset CARB_APP_PATH" >> ${CONDA_DEACT_FILE} +echo "unset LD_LIBRARY_PATH_OLD" >> ${CONDA_DEACT_FILE} +echo "unset PYTHONPATH_OLD" >> ${CONDA_DEACT_FILE} -# # Install omnigibson! -# pip install -e . +# Install omnigibson! +pip install -e . -# # Cycle conda environment so that all dependencies are propagated -# conda deactivate +# Cycle conda environment so that all dependencies are propagated +conda deactivate -# echo -e "\nOmniGibson successfully installed! Please run conda activate $conda_name to activate the environment.\n" +echo -e "\nOmniGibson successfully installed! Please run conda activate $conda_name to activate the environment.\n" From 067cded4578d0f8c1d3d016ec9c59dce8412a543 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Cem=20G=C3=B6kmen?= <1408354+cgokmen@users.noreply.github.com> Date: Tue, 30 Jul 2024 14:44:51 -0700 Subject: [PATCH 04/28] Delete .github/workflows/ci.yml --- .github/workflows/ci.yml | 29 ----------------------------- 1 file changed, 29 deletions(-) delete mode 100644 .github/workflows/ci.yml diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml deleted file mode 100644 index 6a48d1877..000000000 --- a/.github/workflows/ci.yml +++ /dev/null @@ -1,29 +0,0 @@ -name: ci - -on: - push: - branches: - - 'main' - -jobs: - docker: - runs-on: ubuntu-latest - steps: - - - name: Set up QEMU - uses: docker/setup-qemu-action@v2 - - - name: Set up Docker Buildx - uses: docker/setup-buildx-action@v2 - - - name: Login to Docker Hub - uses: docker/login-action@v2 - with: - username: ${{ secrets.DOCKERHUB_USERNAME }} - password: ${{ secrets.DOCKERHUB_TOKEN }} - - - name: Build and push - uses: docker/build-push-action@v4 - with: - push: true - tags: user/app:latest From a14f99d50a36e22b44ea2b059cbc5f17118b56aa Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Tue, 30 Jul 2024 21:45:18 +0000 Subject: [PATCH 05/28] [pre-commit.ci] auto fixes from pre-commit.com hooks for more information, see https://pre-commit.ci --- omnigibson/envs/env_base.py | 16 ++++++---------- omnigibson/reward_functions/__init__.py | 6 +++--- omnigibson/reward_functions/energy_metric.py | 5 +++-- .../reward_functions/task_success_metric.py | 3 ++- omnigibson/tasks/task_base.py | 15 ++++++--------- .../termination_conditions/predicate_goal.py | 3 +-- .../termination_condition_base.py | 2 +- scripts/download_datasets.py | 17 ++++++++++++----- 8 files changed, 34 insertions(+), 33 deletions(-) diff --git a/omnigibson/envs/env_base.py b/omnigibson/envs/env_base.py index 4981b9263..6601b2c5e 100644 --- a/omnigibson/envs/env_base.py +++ b/omnigibson/envs/env_base.py @@ -1,3 +1,4 @@ +import time from copy import deepcopy import gymnasium as gym @@ -25,12 +26,6 @@ merge_nested_dicts, ) from omnigibson.utils.ui_utils import create_module_logger -from omnigibson.utils.python_utils import assert_valid_key, merge_nested_dicts, create_class_from_registry_and_config,\ - Recreatable - - -import time - # Create module logger log = create_module_logger(module_name=__name__) @@ -57,7 +52,9 @@ def __init__(self, configs, in_vec_env=False): self.metadata = {"render.modes": ["rgb_array"]} # Initialize other placeholders that will be filled in later - self._initial_pos_z_offset = None # how high to offset object placement to account for one action step of dropping + self._initial_pos_z_offset = ( + None # how high to offset object placement to account for one action step of dropping + ) self._task = None self._loaded = None self._current_episode = 0 @@ -554,7 +551,7 @@ def _pre_step(self, action, time_step=False): # record the start time if time_step: self._cur_sim_start_ts = time.clock() - + # If the action is not a dictionary, convert into a dictionary if not isinstance(action, dict) and not isinstance(action, gym.spaces.Dict): action_dict = dict() @@ -664,7 +661,6 @@ def _reset_variables(self): self._prev_sim_end_ts = 0 self._cur_sim_start_ts = 0 - def reset(self, get_obs=True, **kwargs): """ Reset episode. @@ -728,7 +724,7 @@ def last_step_wall_time(self): int: return the amount of wall time the last simulation step took """ if self._prev_sim_end_ts == 0 or self._cur_sim_start_ts == 0: - return 0 + return 0 return self._cur_sim_start_ts - self._prev_sim_end_ts @property diff --git a/omnigibson/reward_functions/__init__.py b/omnigibson/reward_functions/__init__.py index ca392da51..b68c8494d 100644 --- a/omnigibson/reward_functions/__init__.py +++ b/omnigibson/reward_functions/__init__.py @@ -1,10 +1,10 @@ from omnigibson.reward_functions.collision_reward import CollisionReward +from omnigibson.reward_functions.energy_metric import EnergyMetric from omnigibson.reward_functions.grasp_reward import GraspReward from omnigibson.reward_functions.point_goal_reward import PointGoalReward from omnigibson.reward_functions.potential_reward import PotentialReward from omnigibson.reward_functions.reaching_goal_reward import ReachingGoalReward +from omnigibson.reward_functions.reward_function_base import REGISTERED_REWARD_FUNCTIONS, BaseRewardFunction from omnigibson.reward_functions.step_metric import StepMetric -from omnigibson.reward_functions.wall_time_metric import WallTimeMetric -from omnigibson.reward_functions.energy_metric import EnergyMetric from omnigibson.reward_functions.task_success_metric import TaskSuccessMetric -from omnigibson.reward_functions.reward_function_base import REGISTERED_REWARD_FUNCTIONS, BaseRewardFunction +from omnigibson.reward_functions.wall_time_metric import WallTimeMetric diff --git a/omnigibson/reward_functions/energy_metric.py b/omnigibson/reward_functions/energy_metric.py index e6f95e671..130e916e5 100644 --- a/omnigibson/reward_functions/energy_metric.py +++ b/omnigibson/reward_functions/energy_metric.py @@ -1,6 +1,7 @@ -from omnigibson.reward_functions.reward_function_base import BaseRewardFunction import numpy as np +from omnigibson.reward_functions.reward_function_base import BaseRewardFunction + class EnergyMetric(BaseRewardFunction): """ @@ -19,7 +20,7 @@ def __init__(self, measure_work=False): self.initialized = False self.state_cache = {} self.link_masses = {} - self.measure_work = measure_work + self.measure_work = measure_work def calculate_displacement(self, posrot, posrot2): return np.linalg.norm(posrot[0] - posrot2[0]) diff --git a/omnigibson/reward_functions/task_success_metric.py b/omnigibson/reward_functions/task_success_metric.py index c09c82c10..1345be899 100644 --- a/omnigibson/reward_functions/task_success_metric.py +++ b/omnigibson/reward_functions/task_success_metric.py @@ -1,9 +1,10 @@ from omnigibson.reward_functions.reward_function_base import BaseRewardFunction + class TaskSuccessMetric(BaseRewardFunction): """ TaskSuccessMetric - Metric for partial or full task success + Metric for partial or full task success """ def __init__(self): diff --git a/omnigibson/tasks/task_base.py b/omnigibson/tasks/task_base.py index 10a18dbf7..5a10433c4 100644 --- a/omnigibson/tasks/task_base.py +++ b/omnigibson/tasks/task_base.py @@ -3,11 +3,10 @@ import numpy as np +from omnigibson.reward_functions import EnergyMetric, StepMetric, TaskSuccessMetric, WallTimeMetric from omnigibson.utils.gym_utils import GymObservable from omnigibson.utils.python_utils import Registerable, classproperty -from omnigibson.reward_functions import StepMetric, TaskSuccessMetric, WallTimeMetric, EnergyMetric - REGISTERED_TASKS = dict() @@ -156,11 +155,11 @@ def _create_metric_functions(self): """ metrics = dict() - metrics['steps'] = StepMetric() - metrics['task_success'] = TaskSuccessMetric() - metrics['wall_time'] = WallTimeMetric() - metrics['energy'] = EnergyMetric() - metrics['work'] = EnergyMetric(measure_work=True) + metrics["steps"] = StepMetric() + metrics["task_success"] = TaskSuccessMetric() + metrics["wall_time"] = WallTimeMetric() + metrics["energy"] = EnergyMetric() + metrics["work"] = EnergyMetric(measure_work=True) return metrics @@ -217,7 +216,6 @@ def reset(self, env): metric_function.reset(self, env) metric_function._reward = 0.0 - def _step_termination(self, env, action, info=None): """ Step and aggregate termination conditions @@ -306,7 +304,6 @@ def _step_metrics(self, env, action): return breakdown_dict - @abstractmethod def _get_obs(self, env): """ diff --git a/omnigibson/termination_conditions/predicate_goal.py b/omnigibson/termination_conditions/predicate_goal.py index 525461c8f..118571a69 100644 --- a/omnigibson/termination_conditions/predicate_goal.py +++ b/omnigibson/termination_conditions/predicate_goal.py @@ -45,12 +45,11 @@ def goal_status(self): """ return self._goal_status - @property def partial_success(self): """ Returns: - float: partial success if supported, -1.0 otherwise + float: partial success if supported, -1.0 otherwise """ assert self._done is not None, "At least one step() must occur before partial_success can be calculated!" return len(satisfied) / (len(satisfied) + len(unsatisfied)) diff --git a/omnigibson/termination_conditions/termination_condition_base.py b/omnigibson/termination_conditions/termination_condition_base.py index fbfcbe20a..24c53a079 100644 --- a/omnigibson/termination_conditions/termination_condition_base.py +++ b/omnigibson/termination_conditions/termination_condition_base.py @@ -97,7 +97,7 @@ def success(self): def partial_success(self): """ Returns: - float: partial success if supported, None otherwise + float: partial success if supported, None otherwise """ assert self._done is not None, "At least one step() must occur before partial_success can be calculated!" return None diff --git a/scripts/download_datasets.py b/scripts/download_datasets.py index 52600c215..417d61c52 100644 --- a/scripts/download_datasets.py +++ b/scripts/download_datasets.py @@ -1,13 +1,16 @@ """ Helper script to download OmniGibson dataset and assets. """ + import os + os.environ["OMNIGIBSON_NO_OMNIVERSE"] = "1" -from omnigibson.macros import gm -from omnigibson.utils.asset_utils import download_og_dataset, download_assets import click +from omnigibson.macros import gm +from omnigibson.utils.asset_utils import download_assets, download_og_dataset + def main(): # Only execute if the dataset path or asset path does not exist @@ -17,7 +20,9 @@ def main(): print(f"OmniGibson will now install data under the following locations:") print(f" dataset (~25GB): {gm.DATASET_PATH}") print(f" assets (~2.5GB): {gm.ASSET_PATH}") - print(f"If you want to install data under a different path, please change the DATA_PATH variable in omnigibson/macros.py and rerun scripts/download_dataset.py.") + print( + f"If you want to install data under a different path, please change the DATA_PATH variable in omnigibson/macros.py and rerun scripts/download_dataset.py." + ) if click.confirm("Do you want to continue?"): # Only download if the dataset path doesn't exist if not dataset_exists: @@ -31,8 +36,10 @@ def main(): print("\nOmniGibson setup completed!\n") else: - print("You chose not to install dataset for now. You can install it later by running python scripts/download_dataset.py.") + print( + "You chose not to install dataset for now. You can install it later by running python scripts/download_dataset.py." + ) if __name__ == "__main__": - main() \ No newline at end of file + main() From b58f38c1cc158e980a9512f898b0c787af484835 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Cem=20G=C3=B6kmen?= <1408354+cgokmen@users.noreply.github.com> Date: Tue, 30 Jul 2024 14:49:09 -0700 Subject: [PATCH 06/28] Delete scripts/download_datasets.py --- scripts/download_datasets.py | 45 ------------------------------------ 1 file changed, 45 deletions(-) delete mode 100644 scripts/download_datasets.py diff --git a/scripts/download_datasets.py b/scripts/download_datasets.py deleted file mode 100644 index 417d61c52..000000000 --- a/scripts/download_datasets.py +++ /dev/null @@ -1,45 +0,0 @@ -""" -Helper script to download OmniGibson dataset and assets. -""" - -import os - -os.environ["OMNIGIBSON_NO_OMNIVERSE"] = "1" - -import click - -from omnigibson.macros import gm -from omnigibson.utils.asset_utils import download_assets, download_og_dataset - - -def main(): - # Only execute if the dataset path or asset path does not exist - dataset_exists, assets_exist = os.path.exists(gm.DATASET_PATH), os.path.exists(gm.ASSET_PATH) - if not (dataset_exists and assets_exist): - # Ask user which dataset to install - print(f"OmniGibson will now install data under the following locations:") - print(f" dataset (~25GB): {gm.DATASET_PATH}") - print(f" assets (~2.5GB): {gm.ASSET_PATH}") - print( - f"If you want to install data under a different path, please change the DATA_PATH variable in omnigibson/macros.py and rerun scripts/download_dataset.py." - ) - if click.confirm("Do you want to continue?"): - # Only download if the dataset path doesn't exist - if not dataset_exists: - print("Downloading dataset...") - download_og_dataset() - - # Only download if the asset path doesn't exist - if not assets_exist: - print("Downloading assets...") - download_assets() - - print("\nOmniGibson setup completed!\n") - else: - print( - "You chose not to install dataset for now. You can install it later by running python scripts/download_dataset.py." - ) - - -if __name__ == "__main__": - main() From 30de8447202304afd660be75fefd3e0cf1829adf Mon Sep 17 00:00:00 2001 From: Frank Yang Date: Tue, 30 Jul 2024 16:37:22 -0700 Subject: [PATCH 07/28] metric refactor --- omnigibson/envs/env_base.py | 60 +++++++++---------- omnigibson/metrics/__init__.py | 5 ++ .../energy_metric.py | 39 ++++++------ omnigibson/metrics/metrics_base.py | 59 ++++++++++++++++++ omnigibson/metrics/step_metric.py | 19 ++++++ .../task_success_metric.py | 16 ++--- omnigibson/metrics/wall_time_metric.py | 19 ++++++ omnigibson/reward_functions/__init__.py | 5 +- omnigibson/reward_functions/step_metric.py | 17 ------ .../reward_functions/wall_time_metric.py | 17 ------ omnigibson/tasks/task_base.py | 11 ++-- 11 files changed, 167 insertions(+), 100 deletions(-) create mode 100644 omnigibson/metrics/__init__.py rename omnigibson/{reward_functions => metrics}/energy_metric.py (56%) create mode 100644 omnigibson/metrics/metrics_base.py create mode 100644 omnigibson/metrics/step_metric.py rename omnigibson/{reward_functions => metrics}/task_success_metric.py (70%) create mode 100644 omnigibson/metrics/wall_time_metric.py delete mode 100644 omnigibson/reward_functions/step_metric.py delete mode 100644 omnigibson/reward_functions/wall_time_metric.py diff --git a/omnigibson/envs/env_base.py b/omnigibson/envs/env_base.py index 6601b2c5e..0e692dbd3 100644 --- a/omnigibson/envs/env_base.py +++ b/omnigibson/envs/env_base.py @@ -1,4 +1,3 @@ -import time from copy import deepcopy import gymnasium as gym @@ -26,6 +25,12 @@ merge_nested_dicts, ) from omnigibson.utils.ui_utils import create_module_logger +from omnigibson.utils.python_utils import assert_valid_key, merge_nested_dicts, create_class_from_registry_and_config,\ + Recreatable + + +import time + # Create module logger log = create_module_logger(module_name=__name__) @@ -51,22 +56,6 @@ def __init__(self, configs, in_vec_env=False): self.render_mode = "rgb_array" self.metadata = {"render.modes": ["rgb_array"]} - # Initialize other placeholders that will be filled in later - self._initial_pos_z_offset = ( - None # how high to offset object placement to account for one action step of dropping - ) - self._task = None - self._loaded = None - self._current_episode = 0 - - self._prev_sim_end_ts = 0 - self._cur_sim_start_ts = 0 - - # Variables reset at the beginning of each episode - self._current_step = 0 - # Store if we are part of a vec env - self.in_vec_env = in_vec_env - # Convert config file(s) into a single parsed dict configs = configs if isinstance(configs, list) or isinstance(configs, tuple) else [configs] @@ -136,7 +125,7 @@ def __init__(self, configs, in_vec_env=False): self.load() # If we are not in a vec env, we can play ourselves. Otherwise we wait for the vec env to play. - if not self.in_vec_env: + if not in_vec_env: og.sim.play() self.post_play_load() @@ -545,13 +534,13 @@ def _populate_info(self, info): if self._scene_graph_builder is not None: info["scene_graph"] = self.get_scene_graph() - def _pre_step(self, action, time_step=False): + def _pre_step(self, action): """Apply the pre-sim-step part of an environment step, i.e. apply the robot actions.""" # record the start time - if time_step: - self._cur_sim_start_ts = time.clock() - + # record the start time of the simulation step in the beginning of the step + self. _cur_sim_start_ts = time.clock() + # If the action is not a dictionary, convert into a dictionary if not isinstance(action, dict) and not isinstance(action, gym.spaces.Dict): action_dict = dict() @@ -568,7 +557,7 @@ def _pre_step(self, action, time_step=False): for robot in self.robots: robot.apply_action(action_dict[robot.name]) - def _post_step(self, action, time_step=False): + def _post_step(self, action): """Apply the post-sim-step part of an environment step, i.e. grab observations and return the step results.""" # Grab observations obs, obs_info = self.get_obs() @@ -603,12 +592,12 @@ def _post_step(self, action, time_step=False): self._current_step += 1 # record end time - if time_step: - self._prev_sim_end_ts = time.clock() + # record the end time of the simulation step in the end of the step + self._prev_sim_end_ts = time.clock() return obs, reward, terminated, truncated, info - def step(self, action, time_step=False): + def step(self, action): """ Apply robot's action and return the next state, reward, done and info, following OpenAI Gym's convention @@ -626,9 +615,9 @@ def step(self, action, time_step=False): - bool: truncated, i.e. whether this episode ended due to a time limit etc. - dict: info, i.e. dictionary with any useful information """ - self._pre_step(action, time_step=time_step) + self._pre_step(action) og.sim.step() - return self._post_step(action, time_step=time_step) + return self._post_step(action) def render(self): """Render the environment for debug viewing.""" @@ -656,10 +645,14 @@ def _reset_variables(self): """ Reset bookkeeping variables for the next new episode. """ + self._current_episode += 1 self._current_step = 0 - self._prev_sim_end_ts = 0 - self._cur_sim_start_ts = 0 + + # reset the start and end time of the simulation step + self._prev_sim_end_ts = None + self._cur_sim_start_ts = None + def reset(self, get_obs=True, **kwargs): """ @@ -723,8 +716,11 @@ def last_step_wall_time(self): Returns: int: return the amount of wall time the last simulation step took """ - if self._prev_sim_end_ts == 0 or self._cur_sim_start_ts == 0: - return 0 + + assert self._prev_sim_end_ts < self._cur_sim_start_ts, "end time from the previous iteration must be less than the start time of the current iteration" + # return 0 if the simulation has not started yet + if not self._prev_sim_end_ts or not self._cur_sim_start_ts: + return 0 return self._cur_sim_start_ts - self._prev_sim_end_ts @property diff --git a/omnigibson/metrics/__init__.py b/omnigibson/metrics/__init__.py new file mode 100644 index 000000000..95afc967f --- /dev/null +++ b/omnigibson/metrics/__init__.py @@ -0,0 +1,5 @@ +from omnigibson.metrics.step_metric import StepMetric +from omnigibson.metrics.task_success_metric import TaskSuccessMetric +from omnigibson.metrics.wall_time_metric import WallTimeMetric +from omnigibson.metrics.energy_metric import EnergyMetric +from omnigibson.metrics.metrics_base import BaseMetric diff --git a/omnigibson/reward_functions/energy_metric.py b/omnigibson/metrics/energy_metric.py similarity index 56% rename from omnigibson/reward_functions/energy_metric.py rename to omnigibson/metrics/energy_metric.py index 130e916e5..705c57858 100644 --- a/omnigibson/reward_functions/energy_metric.py +++ b/omnigibson/metrics/energy_metric.py @@ -1,9 +1,9 @@ import numpy as np -from omnigibson.reward_functions.reward_function_base import BaseRewardFunction +from omnigibson.metrics.metrics_base import BaseMetric -class EnergyMetric(BaseRewardFunction): +class EnergyMetric(BaseMetric): """ Energy Metric @@ -14,46 +14,47 @@ class EnergyMetric(BaseRewardFunction): """ def __init__(self, measure_work=False): + # Run super super().__init__() - self._reward = 0 - self.initialized = False - self.state_cache = {} + self.state_cache = None self.link_masses = {} - self.measure_work = measure_work - def calculate_displacement(self, posrot, posrot2): - return np.linalg.norm(posrot[0] - posrot2[0]) + # parameter for checking if this is a work or energy metric. If true, measure work done, else measure energy + self.measure_work = measure_work def _step(self, task, env, action): + + # calculate the current pose of all object links new_state_cache = {} for obj in env.scene.objects: for link_name, link in obj._links.items(): pos, rot = link.get_position_orientation() new_state_cache[link_name] = (pos, rot) - if not self.initialized: - self.initialized = True + if not self.state_cache: self.state_cache = new_state_cache for obj in env.scene.objects: for link_name, link in obj._links.items(): self.link_masses[link_name] = link.mass - return 0.0, {} + + return 0.0 work_metric = 0.0 for linkname, posrot in new_state_cache.items(): - work_metric += self.calculate_displacement(posrot, self.state_cache[linkname]) * self.link_masses[linkname] - if self.measure_work: - self._reward = 0 + # TODO: this computation is very slow, consider using a more efficient method + # TODO: this method needs to be updated to account for object addition and removal + posrot2 = self.state_cache[linkname] + work_metric += np.linalg.norm(posrot[0], posrot2[0]) * self.link_masses[linkname] + if not self.measure_work: self.state_cache = new_state_cache - - self._reward += work_metric - return self._reward, {} + + self._metric = work_metric if not self.measure_work else (self._metric + work_metric) + return self._metric def reset(self, task, env): super().reset(task, env) - self.state_cache = {} - self.initialized = False + self.state_cache = None diff --git a/omnigibson/metrics/metrics_base.py b/omnigibson/metrics/metrics_base.py new file mode 100644 index 000000000..60f66cbd0 --- /dev/null +++ b/omnigibson/metrics/metrics_base.py @@ -0,0 +1,59 @@ +from abc import ABCMeta, abstractmethod +from copy import deepcopy + +from omnigibson.utils.python_utils import Registerable, classproperty + +class BaseMetric(Registerable, metaclass=ABCMeta): + """ + Base Metric class + Metric-specific reset and step methods are implemented in subclasses + """ + + def __init__(self): + # Store internal vars that will be filled in at runtime + self._metric = 0 + + @abstractmethod + def _step(self, task, env, action): + """ + Step the metric function and compute the metric at the current timestep. Overwritten by subclasses. + + Args: + task (BaseTask): Task instance + env (Environment): Environment instance + action (n-array): 1D flattened array of actions executed by all agents in the environment + + Returns: + 2-tuple: + - bool: computed metric + - dict: any metric-related information for this specific metric + """ + raise NotImplementedError() + + def step(self, task, env, action): + """ + Step the metricfunction and compute the metric at the current timestep. Overwritten by subclasses + + Args: + task (BaseTask): Task instance + env (Environment): Environment instance + action (n-array): 1D flattened array of actions executed by all agents in the environment + + Returns: + 2-tuple: + - bool: computed metric + - dict: any metric-related information for this specific metric + """ + # Step internally and store output + self._metric = self._step(task=task, env=env, action=action) + + # Return metric + return self._metric + + def reset(self, task, env): + """ + General metrics reset + """ + + # Reset internal vars + self._metric = None \ No newline at end of file diff --git a/omnigibson/metrics/step_metric.py b/omnigibson/metrics/step_metric.py new file mode 100644 index 000000000..d53b29e6b --- /dev/null +++ b/omnigibson/metrics/step_metric.py @@ -0,0 +1,19 @@ +from omnigibson.metrics.metrics_base import BaseMetric + + +class StepMetric(BaseMetric): + """ + Step Metric + Metric for each simulator step + """ + + def __init__(self): + # Run super + super().__init__() + + def _step(self, task, env, action): + self._metric += 1 + return self._metric + + def reset(self, task, env): + super().reset(task, env) diff --git a/omnigibson/reward_functions/task_success_metric.py b/omnigibson/metrics/task_success_metric.py similarity index 70% rename from omnigibson/reward_functions/task_success_metric.py rename to omnigibson/metrics/task_success_metric.py index 1345be899..890725c4d 100644 --- a/omnigibson/reward_functions/task_success_metric.py +++ b/omnigibson/metrics/task_success_metric.py @@ -1,7 +1,7 @@ -from omnigibson.reward_functions.reward_function_base import BaseRewardFunction +from omnigibson.metrics.metrics_base import BaseMetric -class TaskSuccessMetric(BaseRewardFunction): +class TaskSuccessMetric(BaseMetric): """ TaskSuccessMetric Metric for partial or full task success @@ -10,7 +10,6 @@ class TaskSuccessMetric(BaseRewardFunction): def __init__(self): # Run super super().__init__() - self._reward = 0 def _step(self, task, env, action): successes = [] @@ -22,10 +21,13 @@ def _step(self, task, env, action): # success <=> done and non failure successes.append(success) if sum(successes) > 0: - self._reward = 1.0 + self._metric = 1.0 elif partial_successes: - self._reward = sum(partial_successes) / len(partial_successes) + self._metric = sum(partial_successes) / len(partial_successes) else: - self._reward = 0.0 + self._metric = 0.0 # Populate info - return self._reward, {} + return self._metric + + def reset(self, task, env): + super().reset(task, env) diff --git a/omnigibson/metrics/wall_time_metric.py b/omnigibson/metrics/wall_time_metric.py new file mode 100644 index 000000000..86adf79b1 --- /dev/null +++ b/omnigibson/metrics/wall_time_metric.py @@ -0,0 +1,19 @@ +from omnigibson.metrics.metrics_base import BaseMetric + + +class WallTimeMetric(BaseMetric): + """ + WallTimeMetric + Metric for wall time accumulated in policy steps + """ + + def __init__(self): + # Run super + super().__init__() + + def _step(self, task, env, action): + self._metric += env.last_step_wall_time + return self._metric + + def reset(self, task, env): + super().reset(task, env) diff --git a/omnigibson/reward_functions/__init__.py b/omnigibson/reward_functions/__init__.py index b68c8494d..c9d2f8f36 100644 --- a/omnigibson/reward_functions/__init__.py +++ b/omnigibson/reward_functions/__init__.py @@ -1,10 +1,7 @@ from omnigibson.reward_functions.collision_reward import CollisionReward -from omnigibson.reward_functions.energy_metric import EnergyMetric from omnigibson.reward_functions.grasp_reward import GraspReward from omnigibson.reward_functions.point_goal_reward import PointGoalReward from omnigibson.reward_functions.potential_reward import PotentialReward from omnigibson.reward_functions.reaching_goal_reward import ReachingGoalReward from omnigibson.reward_functions.reward_function_base import REGISTERED_REWARD_FUNCTIONS, BaseRewardFunction -from omnigibson.reward_functions.step_metric import StepMetric -from omnigibson.reward_functions.task_success_metric import TaskSuccessMetric -from omnigibson.reward_functions.wall_time_metric import WallTimeMetric + diff --git a/omnigibson/reward_functions/step_metric.py b/omnigibson/reward_functions/step_metric.py deleted file mode 100644 index 17fb8b30d..000000000 --- a/omnigibson/reward_functions/step_metric.py +++ /dev/null @@ -1,17 +0,0 @@ -from omnigibson.reward_functions.reward_function_base import BaseRewardFunction - - -class StepMetric(BaseRewardFunction): - """ - Step Metric - Metric for each simulator step - """ - - def __init__(self): - # Run super - super().__init__() - self._reward = 0 - - def _step(self, task, env, action): - self._reward += 1 - return self._reward, {} diff --git a/omnigibson/reward_functions/wall_time_metric.py b/omnigibson/reward_functions/wall_time_metric.py deleted file mode 100644 index e691fd089..000000000 --- a/omnigibson/reward_functions/wall_time_metric.py +++ /dev/null @@ -1,17 +0,0 @@ -from omnigibson.reward_functions.reward_function_base import BaseRewardFunction - - -class WallTimeMetric(BaseRewardFunction): - """ - WallTimeMetric - Metric for wall time accumulated in policy steps - """ - - def __init__(self): - # Run super - super().__init__() - self._reward = 0 - - def _step(self, task, env, action): - self._reward += env.last_step_wall_time - return self._reward, {} diff --git a/omnigibson/tasks/task_base.py b/omnigibson/tasks/task_base.py index 5a10433c4..51b798f90 100644 --- a/omnigibson/tasks/task_base.py +++ b/omnigibson/tasks/task_base.py @@ -3,7 +3,7 @@ import numpy as np -from omnigibson.reward_functions import EnergyMetric, StepMetric, TaskSuccessMetric, WallTimeMetric +from omnigibson.metrics import EnergyMetric, StepMetric, TaskSuccessMetric, WallTimeMetric from omnigibson.utils.gym_utils import GymObservable from omnigibson.utils.python_utils import Registerable, classproperty @@ -54,6 +54,7 @@ def __init__(self, termination_config=None, reward_config=None): # Store other internal vars that will be populated at runtime self._loaded = False self._reward = None + self._metrics = 0 self._done = None self._success = None self._info = None @@ -191,6 +192,7 @@ def _reset_variables(self, env): """ # By default, reset reward, done, and info self._reward = None + self._metrics = 0 self._done = False self._success = False self._info = None @@ -214,7 +216,6 @@ def reset(self, env): reward_function.reset(self, env) for metric_function in self._metric_functions.values(): metric_function.reset(self, env) - metric_function._reward = 0.0 def _step_termination(self, env, action, info=None): """ @@ -299,10 +300,12 @@ def _step_metrics(self, env, action): breakdown_dict = dict() for metric_name, metric_function in self._metric_functions.items(): - metric, _ = metric_function.step(self, env, action) + metric = metric_function.step(self, env, action) breakdown_dict[metric_name] = metric - return breakdown_dict + # TODO: metric score is currently summed, work on the global coefficients + metric_score = sum(breakdown_dict.values()) + return metric_score, breakdown_dict @abstractmethod def _get_obs(self, env): From e32ebb408e2358730858843563056af2f824ad21 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Tue, 30 Jul 2024 23:37:43 +0000 Subject: [PATCH 08/28] [pre-commit.ci] auto fixes from pre-commit.com hooks for more information, see https://pre-commit.ci --- omnigibson/envs/env_base.py | 18 +++++++----------- omnigibson/metrics/__init__.py | 4 ++-- omnigibson/metrics/energy_metric.py | 4 ++-- omnigibson/metrics/metrics_base.py | 3 ++- omnigibson/metrics/step_metric.py | 2 +- omnigibson/metrics/task_success_metric.py | 2 +- omnigibson/metrics/wall_time_metric.py | 2 +- omnigibson/reward_functions/__init__.py | 1 - 8 files changed, 16 insertions(+), 20 deletions(-) diff --git a/omnigibson/envs/env_base.py b/omnigibson/envs/env_base.py index 0e692dbd3..041f0afe5 100644 --- a/omnigibson/envs/env_base.py +++ b/omnigibson/envs/env_base.py @@ -1,3 +1,4 @@ +import time from copy import deepcopy import gymnasium as gym @@ -25,12 +26,6 @@ merge_nested_dicts, ) from omnigibson.utils.ui_utils import create_module_logger -from omnigibson.utils.python_utils import assert_valid_key, merge_nested_dicts, create_class_from_registry_and_config,\ - Recreatable - - -import time - # Create module logger log = create_module_logger(module_name=__name__) @@ -539,8 +534,8 @@ def _pre_step(self, action): # record the start time # record the start time of the simulation step in the beginning of the step - self. _cur_sim_start_ts = time.clock() - + self._cur_sim_start_ts = time.clock() + # If the action is not a dictionary, convert into a dictionary if not isinstance(action, dict) and not isinstance(action, gym.spaces.Dict): action_dict = dict() @@ -653,7 +648,6 @@ def _reset_variables(self): self._prev_sim_end_ts = None self._cur_sim_start_ts = None - def reset(self, get_obs=True, **kwargs): """ Reset episode. @@ -717,10 +711,12 @@ def last_step_wall_time(self): int: return the amount of wall time the last simulation step took """ - assert self._prev_sim_end_ts < self._cur_sim_start_ts, "end time from the previous iteration must be less than the start time of the current iteration" + assert ( + self._prev_sim_end_ts < self._cur_sim_start_ts + ), "end time from the previous iteration must be less than the start time of the current iteration" # return 0 if the simulation has not started yet if not self._prev_sim_end_ts or not self._cur_sim_start_ts: - return 0 + return 0 return self._cur_sim_start_ts - self._prev_sim_end_ts @property diff --git a/omnigibson/metrics/__init__.py b/omnigibson/metrics/__init__.py index 95afc967f..78e8934b7 100644 --- a/omnigibson/metrics/__init__.py +++ b/omnigibson/metrics/__init__.py @@ -1,5 +1,5 @@ +from omnigibson.metrics.energy_metric import EnergyMetric +from omnigibson.metrics.metrics_base import BaseMetric from omnigibson.metrics.step_metric import StepMetric from omnigibson.metrics.task_success_metric import TaskSuccessMetric from omnigibson.metrics.wall_time_metric import WallTimeMetric -from omnigibson.metrics.energy_metric import EnergyMetric -from omnigibson.metrics.metrics_base import BaseMetric diff --git a/omnigibson/metrics/energy_metric.py b/omnigibson/metrics/energy_metric.py index 705c57858..ba4d3efc4 100644 --- a/omnigibson/metrics/energy_metric.py +++ b/omnigibson/metrics/energy_metric.py @@ -38,7 +38,7 @@ def _step(self, task, env, action): for obj in env.scene.objects: for link_name, link in obj._links.items(): self.link_masses[link_name] = link.mass - + return 0.0 work_metric = 0.0 @@ -51,7 +51,7 @@ def _step(self, task, env, action): if not self.measure_work: self.state_cache = new_state_cache - + self._metric = work_metric if not self.measure_work else (self._metric + work_metric) return self._metric diff --git a/omnigibson/metrics/metrics_base.py b/omnigibson/metrics/metrics_base.py index 60f66cbd0..2878d46bb 100644 --- a/omnigibson/metrics/metrics_base.py +++ b/omnigibson/metrics/metrics_base.py @@ -3,6 +3,7 @@ from omnigibson.utils.python_utils import Registerable, classproperty + class BaseMetric(Registerable, metaclass=ABCMeta): """ Base Metric class @@ -56,4 +57,4 @@ def reset(self, task, env): """ # Reset internal vars - self._metric = None \ No newline at end of file + self._metric = None diff --git a/omnigibson/metrics/step_metric.py b/omnigibson/metrics/step_metric.py index d53b29e6b..084e8d870 100644 --- a/omnigibson/metrics/step_metric.py +++ b/omnigibson/metrics/step_metric.py @@ -14,6 +14,6 @@ def __init__(self): def _step(self, task, env, action): self._metric += 1 return self._metric - + def reset(self, task, env): super().reset(task, env) diff --git a/omnigibson/metrics/task_success_metric.py b/omnigibson/metrics/task_success_metric.py index 890725c4d..03bc5a6cb 100644 --- a/omnigibson/metrics/task_success_metric.py +++ b/omnigibson/metrics/task_success_metric.py @@ -28,6 +28,6 @@ def _step(self, task, env, action): self._metric = 0.0 # Populate info return self._metric - + def reset(self, task, env): super().reset(task, env) diff --git a/omnigibson/metrics/wall_time_metric.py b/omnigibson/metrics/wall_time_metric.py index 86adf79b1..a5ceb8ba9 100644 --- a/omnigibson/metrics/wall_time_metric.py +++ b/omnigibson/metrics/wall_time_metric.py @@ -14,6 +14,6 @@ def __init__(self): def _step(self, task, env, action): self._metric += env.last_step_wall_time return self._metric - + def reset(self, task, env): super().reset(task, env) diff --git a/omnigibson/reward_functions/__init__.py b/omnigibson/reward_functions/__init__.py index c9d2f8f36..2f10acdce 100644 --- a/omnigibson/reward_functions/__init__.py +++ b/omnigibson/reward_functions/__init__.py @@ -4,4 +4,3 @@ from omnigibson.reward_functions.potential_reward import PotentialReward from omnigibson.reward_functions.reaching_goal_reward import ReachingGoalReward from omnigibson.reward_functions.reward_function_base import REGISTERED_REWARD_FUNCTIONS, BaseRewardFunction - From a15102a5a1dd1eba4348cb91b75ebfdceca653d2 Mon Sep 17 00:00:00 2001 From: Frank Yang Date: Tue, 30 Jul 2024 16:41:38 -0700 Subject: [PATCH 09/28] updated energy metric comment --- omnigibson/metrics/energy_metric.py | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/omnigibson/metrics/energy_metric.py b/omnigibson/metrics/energy_metric.py index 705c57858..fd0783b49 100644 --- a/omnigibson/metrics/energy_metric.py +++ b/omnigibson/metrics/energy_metric.py @@ -32,6 +32,7 @@ def _step(self, task, env, action): pos, rot = link.get_position_orientation() new_state_cache[link_name] = (pos, rot) + # if the state cache is empty, set it to the current state and return 0 if not self.state_cache: self.state_cache = new_state_cache @@ -41,6 +42,7 @@ def _step(self, task, env, action): return 0.0 + # calculate the energy spent from the previous state to the current state work_metric = 0.0 for linkname, posrot in new_state_cache.items(): @@ -49,10 +51,12 @@ def _step(self, task, env, action): posrot2 = self.state_cache[linkname] work_metric += np.linalg.norm(posrot[0], posrot2[0]) * self.link_masses[linkname] + # if measuring energy, update the state cache if not self.measure_work: self.state_cache = new_state_cache - - self._metric = work_metric if not self.measure_work else (self._metric + work_metric) + + # update the metric accordingly, either set the work done (measuring work) or add it to previous energy spent (measuring energy) + self._metric = work_metric if self.measure_work else (self._metric + work_metric) return self._metric def reset(self, task, env): From 420edd22c87f1c37e13f5ddf59b8459397ad0e23 Mon Sep 17 00:00:00 2001 From: Frank Yang Date: Tue, 30 Jul 2024 16:44:08 -0700 Subject: [PATCH 10/28] setting metric to 0 --- omnigibson/metrics/metrics_base.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/omnigibson/metrics/metrics_base.py b/omnigibson/metrics/metrics_base.py index 2878d46bb..caff1052b 100644 --- a/omnigibson/metrics/metrics_base.py +++ b/omnigibson/metrics/metrics_base.py @@ -57,4 +57,4 @@ def reset(self, task, env): """ # Reset internal vars - self._metric = None + self._metric = 0 From ba7e99255ffd57c1985b92becbddf0ce195ad17c Mon Sep 17 00:00:00 2001 From: Frank Yang Date: Wed, 31 Jul 2024 16:21:35 -0700 Subject: [PATCH 11/28] metrics change --- omnigibson/envs/env_base.py | 11 +++++----- omnigibson/metrics/energy_metric.py | 2 +- omnigibson/metrics/metrics_base.py | 6 ++--- omnigibson/metrics/task_success_metric.py | 22 ++++++++++++------- omnigibson/tasks/behavior_task.py | 9 +++----- .../termination_conditions/predicate_goal.py | 4 ++-- .../termination_condition_base.py | 4 ++-- 7 files changed, 30 insertions(+), 28 deletions(-) diff --git a/omnigibson/envs/env_base.py b/omnigibson/envs/env_base.py index 041f0afe5..2793cac4b 100644 --- a/omnigibson/envs/env_base.py +++ b/omnigibson/envs/env_base.py @@ -534,7 +534,7 @@ def _pre_step(self, action): # record the start time # record the start time of the simulation step in the beginning of the step - self._cur_sim_start_ts = time.clock() + self._cur_sim_start_ts = time.perf_counter() # If the action is not a dictionary, convert into a dictionary if not isinstance(action, dict) and not isinstance(action, gym.spaces.Dict): @@ -588,7 +588,7 @@ def _post_step(self, action): # record end time # record the end time of the simulation step in the end of the step - self._prev_sim_end_ts = time.clock() + self._prev_sim_end_ts = time.perf_counter() return obs, reward, terminated, truncated, info @@ -711,12 +711,13 @@ def last_step_wall_time(self): int: return the amount of wall time the last simulation step took """ - assert ( - self._prev_sim_end_ts < self._cur_sim_start_ts - ), "end time from the previous iteration must be less than the start time of the current iteration" # return 0 if the simulation has not started yet if not self._prev_sim_end_ts or not self._cur_sim_start_ts: return 0 + + assert ( + self._prev_sim_end_ts < self._cur_sim_start_ts + ), "end time from the previous iteration must be less than the start time of the current iteration" return self._cur_sim_start_ts - self._prev_sim_end_ts @property diff --git a/omnigibson/metrics/energy_metric.py b/omnigibson/metrics/energy_metric.py index 83f9ee6b5..94d03f395 100644 --- a/omnigibson/metrics/energy_metric.py +++ b/omnigibson/metrics/energy_metric.py @@ -49,7 +49,7 @@ def _step(self, task, env, action): # TODO: this computation is very slow, consider using a more efficient method # TODO: this method needs to be updated to account for object addition and removal posrot2 = self.state_cache[linkname] - work_metric += np.linalg.norm(posrot[0], posrot2[0]) * self.link_masses[linkname] + work_metric += np.linalg.norm(posrot[0] - posrot2[0]) * self.link_masses[linkname] # if measuring energy, update the state cache if not self.measure_work: diff --git a/omnigibson/metrics/metrics_base.py b/omnigibson/metrics/metrics_base.py index caff1052b..2f16aff6a 100644 --- a/omnigibson/metrics/metrics_base.py +++ b/omnigibson/metrics/metrics_base.py @@ -1,10 +1,8 @@ from abc import ABCMeta, abstractmethod from copy import deepcopy -from omnigibson.utils.python_utils import Registerable, classproperty - - -class BaseMetric(Registerable, metaclass=ABCMeta): +# from omnigibson.utils.python_utils import Registerable, classproperty +class BaseMetric(): """ Base Metric class Metric-specific reset and step methods are implemented in subclasses diff --git a/omnigibson/metrics/task_success_metric.py b/omnigibson/metrics/task_success_metric.py index 03bc5a6cb..881975ba2 100644 --- a/omnigibson/metrics/task_success_metric.py +++ b/omnigibson/metrics/task_success_metric.py @@ -13,20 +13,26 @@ def __init__(self): def _step(self, task, env, action): successes = [] - partial_successes = [] + partial_success = 0 + + # Evaluate termination conditions for termination_condition in task._termination_conditions.values(): - if termination_condition.partial_success >= 0.0: - partial_successes.append(termination_condition.partial_success) + + # Check if partial success is supported, and if so, store the score (e.g. Behavior Task) + if termination_condition.partial_success: + partial_success = task.success_score + done, success = termination_condition.step(task, env, action) - # success <=> done and non failure successes.append(success) - if sum(successes) > 0: + + # Calculate metric + if any(successes): self._metric = 1.0 - elif partial_successes: - self._metric = sum(partial_successes) / len(partial_successes) + elif partial_success > 0: + self._metric = partial_success else: self._metric = 0.0 - # Populate info + return self._metric def reset(self, task, env): diff --git a/omnigibson/tasks/behavior_task.py b/omnigibson/tasks/behavior_task.py index 23e1818f2..995c81028 100644 --- a/omnigibson/tasks/behavior_task.py +++ b/omnigibson/tasks/behavior_task.py @@ -10,6 +10,7 @@ get_natural_goal_conditions, get_natural_initial_conditions, get_object_scope, + get_reward ) import omnigibson as og @@ -285,12 +286,8 @@ def get_potential(self, env): Returns: float: Computed potential """ - # Evaluate the first ground goal state option as the potential - _, satisfied_predicates = evaluate_goal_conditions(self.ground_goal_state_options[0]) - success_score = len(satisfied_predicates["satisfied"]) / ( - len(satisfied_predicates["satisfied"]) + len(satisfied_predicates["unsatisfied"]) - ) - return -success_score + self.success_score = get_reward(self.ground_goal_state_options) + return -self.success_score def initialize_activity(self, env): """ diff --git a/omnigibson/termination_conditions/predicate_goal.py b/omnigibson/termination_conditions/predicate_goal.py index 118571a69..3cb0eb12a 100644 --- a/omnigibson/termination_conditions/predicate_goal.py +++ b/omnigibson/termination_conditions/predicate_goal.py @@ -49,7 +49,7 @@ def goal_status(self): def partial_success(self): """ Returns: - float: partial success if supported, -1.0 otherwise + bool: returns true if partial success is supported, false otherwise """ assert self._done is not None, "At least one step() must occur before partial_success can be calculated!" - return len(satisfied) / (len(satisfied) + len(unsatisfied)) + return True diff --git a/omnigibson/termination_conditions/termination_condition_base.py b/omnigibson/termination_conditions/termination_condition_base.py index 24c53a079..5fe381695 100644 --- a/omnigibson/termination_conditions/termination_condition_base.py +++ b/omnigibson/termination_conditions/termination_condition_base.py @@ -97,10 +97,10 @@ def success(self): def partial_success(self): """ Returns: - float: partial success if supported, None otherwise + bool: returns true if partial success is supported, false otherwise """ assert self._done is not None, "At least one step() must occur before partial_success can be calculated!" - return None + return False @classproperty def _terminate_is_success(cls): From ae74fc5fcaeeeaedfb826d1bf14434d36d469932 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Wed, 31 Jul 2024 23:22:17 +0000 Subject: [PATCH 12/28] [pre-commit.ci] auto fixes from pre-commit.com hooks for more information, see https://pre-commit.ci --- omnigibson/envs/env_base.py | 2 +- omnigibson/metrics/metrics_base.py | 3 ++- omnigibson/metrics/task_success_metric.py | 2 +- omnigibson/tasks/behavior_task.py | 2 +- 4 files changed, 5 insertions(+), 4 deletions(-) diff --git a/omnigibson/envs/env_base.py b/omnigibson/envs/env_base.py index 2793cac4b..1a7f0ade9 100644 --- a/omnigibson/envs/env_base.py +++ b/omnigibson/envs/env_base.py @@ -714,7 +714,7 @@ def last_step_wall_time(self): # return 0 if the simulation has not started yet if not self._prev_sim_end_ts or not self._cur_sim_start_ts: return 0 - + assert ( self._prev_sim_end_ts < self._cur_sim_start_ts ), "end time from the previous iteration must be less than the start time of the current iteration" diff --git a/omnigibson/metrics/metrics_base.py b/omnigibson/metrics/metrics_base.py index 2f16aff6a..63432e9e3 100644 --- a/omnigibson/metrics/metrics_base.py +++ b/omnigibson/metrics/metrics_base.py @@ -1,8 +1,9 @@ from abc import ABCMeta, abstractmethod from copy import deepcopy + # from omnigibson.utils.python_utils import Registerable, classproperty -class BaseMetric(): +class BaseMetric: """ Base Metric class Metric-specific reset and step methods are implemented in subclasses diff --git a/omnigibson/metrics/task_success_metric.py b/omnigibson/metrics/task_success_metric.py index 881975ba2..d8635686d 100644 --- a/omnigibson/metrics/task_success_metric.py +++ b/omnigibson/metrics/task_success_metric.py @@ -21,7 +21,7 @@ def _step(self, task, env, action): # Check if partial success is supported, and if so, store the score (e.g. Behavior Task) if termination_condition.partial_success: partial_success = task.success_score - + done, success = termination_condition.step(task, env, action) successes.append(success) diff --git a/omnigibson/tasks/behavior_task.py b/omnigibson/tasks/behavior_task.py index 995c81028..c18fb45ed 100644 --- a/omnigibson/tasks/behavior_task.py +++ b/omnigibson/tasks/behavior_task.py @@ -10,7 +10,7 @@ get_natural_goal_conditions, get_natural_initial_conditions, get_object_scope, - get_reward + get_reward, ) import omnigibson as og From 328e7723a13bd2a6bfd47bb6172c7c89b610320a Mon Sep 17 00:00:00 2001 From: Frank Yang Date: Fri, 2 Aug 2024 13:44:54 -0700 Subject: [PATCH 13/28] fixed base robot fix --- omnigibson/objects/controllable_object.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/omnigibson/objects/controllable_object.py b/omnigibson/objects/controllable_object.py index e607f9495..160820317 100644 --- a/omnigibson/objects/controllable_object.py +++ b/omnigibson/objects/controllable_object.py @@ -576,7 +576,7 @@ def get_control_dict(self): # TODO: Move gravity force computation dummy to this class instead of BaseRobot fcns["gravity_force"] = lambda: ( ControllableObjectViewAPI.get_generalized_gravity_forces(self.articulation_root_path) - if self.fixed_base + if self.fixed_base or self._dummy is None else ControllableObjectViewAPI.get_generalized_gravity_forces(self._dummy.articulation_root_path) ) fcns["cc_force"] = lambda: ControllableObjectViewAPI.get_coriolis_and_centrifugal_forces( From 0b3b1d1b947e4e189538a1836ccf0441d4937a7d Mon Sep 17 00:00:00 2001 From: Frank Yang Date: Mon, 5 Aug 2024 11:46:15 -0700 Subject: [PATCH 14/28] object initialization bugfix --- omnigibson/examples/environments/behavior_env_demo.py | 1 + omnigibson/tasks/behavior_task.py | 2 +- 2 files changed, 2 insertions(+), 1 deletion(-) diff --git a/omnigibson/examples/environments/behavior_env_demo.py b/omnigibson/examples/environments/behavior_env_demo.py index ddb6d0e3d..d1010c800 100644 --- a/omnigibson/examples/environments/behavior_env_demo.py +++ b/omnigibson/examples/environments/behavior_env_demo.py @@ -5,6 +5,7 @@ import omnigibson as og from omnigibson.macros import gm from omnigibson.utils.ui_utils import choose_from_options +from omnigibson.tasks.behavior_task import BehaviorTask # Make sure object states are enabled gm.ENABLE_OBJECT_STATES = True diff --git a/omnigibson/tasks/behavior_task.py b/omnigibson/tasks/behavior_task.py index c18fb45ed..13d7875ba 100644 --- a/omnigibson/tasks/behavior_task.py +++ b/omnigibson/tasks/behavior_task.py @@ -377,7 +377,7 @@ def _get_obs(self, env): low_dim_obs = dict() # Batch rpy calculations for much better efficiency - objs_exist = {obj: obj.exists for obj in self.object_scope.values() if not obj.is_system} + objs_exist = {obj: obj.exists for obj in self.object_scope.values() if not obj.is_system and obj.states[Pose]._initialized} objs_rpy = T.quat2euler( np.array( [ From 628490b2e87a00f4ecc508acf9669b76fda0b7d6 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Mon, 5 Aug 2024 18:46:38 +0000 Subject: [PATCH 15/28] [pre-commit.ci] auto fixes from pre-commit.com hooks for more information, see https://pre-commit.ci --- omnigibson/examples/environments/behavior_env_demo.py | 2 +- omnigibson/tasks/behavior_task.py | 4 +++- 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/omnigibson/examples/environments/behavior_env_demo.py b/omnigibson/examples/environments/behavior_env_demo.py index d1010c800..1e6c36ee6 100644 --- a/omnigibson/examples/environments/behavior_env_demo.py +++ b/omnigibson/examples/environments/behavior_env_demo.py @@ -4,8 +4,8 @@ import omnigibson as og from omnigibson.macros import gm -from omnigibson.utils.ui_utils import choose_from_options from omnigibson.tasks.behavior_task import BehaviorTask +from omnigibson.utils.ui_utils import choose_from_options # Make sure object states are enabled gm.ENABLE_OBJECT_STATES = True diff --git a/omnigibson/tasks/behavior_task.py b/omnigibson/tasks/behavior_task.py index 13d7875ba..73e71155b 100644 --- a/omnigibson/tasks/behavior_task.py +++ b/omnigibson/tasks/behavior_task.py @@ -377,7 +377,9 @@ def _get_obs(self, env): low_dim_obs = dict() # Batch rpy calculations for much better efficiency - objs_exist = {obj: obj.exists for obj in self.object_scope.values() if not obj.is_system and obj.states[Pose]._initialized} + objs_exist = { + obj: obj.exists for obj in self.object_scope.values() if not obj.is_system and obj.states[Pose]._initialized + } objs_rpy = T.quat2euler( np.array( [ From dbec742da22ebafbeb4ca8bfe1c7c87b76a8336a Mon Sep 17 00:00:00 2001 From: Frank Yang Date: Tue, 6 Aug 2024 16:01:41 -0700 Subject: [PATCH 16/28] metric tests for work added --- .../environments/behavior_env_demo.py | 1 - tests/test_metric_functions.py | 94 +++++++++++++++++++ 2 files changed, 94 insertions(+), 1 deletion(-) create mode 100644 tests/test_metric_functions.py diff --git a/omnigibson/examples/environments/behavior_env_demo.py b/omnigibson/examples/environments/behavior_env_demo.py index 1e6c36ee6..ddb6d0e3d 100644 --- a/omnigibson/examples/environments/behavior_env_demo.py +++ b/omnigibson/examples/environments/behavior_env_demo.py @@ -4,7 +4,6 @@ import omnigibson as og from omnigibson.macros import gm -from omnigibson.tasks.behavior_task import BehaviorTask from omnigibson.utils.ui_utils import choose_from_options # Make sure object states are enabled diff --git a/tests/test_metric_functions.py b/tests/test_metric_functions.py new file mode 100644 index 000000000..154a13eff --- /dev/null +++ b/tests/test_metric_functions.py @@ -0,0 +1,94 @@ +import os +from typing import OrderedDict + +import yaml +import numpy as np + +import omnigibson as og +from omnigibson.macros import gm +from omnigibson.tasks.behavior_task import BehaviorTask +from bddl.condition_evaluation import evaluate_state + +# Make sure object states are enabled +gm.ENABLE_OBJECT_STATES = True +gm.USE_GPU_DYNAMICS = True + +def setup_env(): + + # Generates a BEHAVIOR Task environment using presampled cache + config_filename = os.path.join(og.example_config_path, "fetch_behavior.yaml") + cfg = yaml.load(open(config_filename, "r"), Loader=yaml.FullLoader) + cfg["task"]["online_object_sampling"] = False + + if og.sim is not None: + # Make sure sim is stopped + og.sim.stop() + + # Load the environment + env = og.Environment(configs=cfg) + + return env + +def test_behavior_reset(): + + env = setup_env() + + action = env.action_space.sample() + state, reward, terminated, truncated, info = env.step(action) + metrics = info["metrics"] + + assert isinstance(metrics, dict) + + env.reset() + + # perform a step with no action + action = OrderedDict([('robot0', [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0])]) + state, reward, terminated, truncated, info = env.step(action) + + metrics = info["metrics"] + + assert metrics["steps"] == 0, "Step metric was not reset" + assert metrics["task_success"] == 0, "Task success metric was not reset" + assert metrics["wall_time"] == 0, "Wall time metric was not reset" + assert metrics["energy"] == 0, "Energy metric was not reset" + assert metrics["work"] == 0, "Work metric was not reset" + +def test_behavior_task_work_metric(): + + env = setup_env() + + # perform a step with no action + action = OrderedDict([('robot0', [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0])]) + state, reward, terminated, truncated, info = env.step(action) + + metrics = info["metrics"] + + assert isinstance(metrics, dict) + + # assert that one step is taken + assert metrics["steps"] == 1 + + # cache the initial position and orientation of the robot + position, orientation = env.robots[0].get_position_orientation() + + # move the robot to a new position and orientation, and then back to the initial position and orientation + env.robots[0].set_position_orientation([10, 10, 0]) + env.step(action) + + env.robots[0].set_position_orientation(position, orientation) + state, reward, terminated, truncated, info = env.step(action) + metrics = info["metrics"] + + # since simulator is running, the work done is non-zero due to random link movements. Assert that the work / robot mass is below a threshold + + # calculate robot mass + robot_mass = 0 + for link in env.robots[0].links.values(): + robot_mass += link.mass + + assert np.allclose(metrics["work"] / robot_mass, 0, atol=1e-3) + + # Always close the environment at the end + env.close() + + From f2a9997c7b21bcd1923837a9b7265b5da99a56ef Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Tue, 6 Aug 2024 23:02:02 +0000 Subject: [PATCH 17/28] [pre-commit.ci] auto fixes from pre-commit.com hooks for more information, see https://pre-commit.ci --- tests/test_metric_functions.py | 15 ++++++++------- 1 file changed, 8 insertions(+), 7 deletions(-) diff --git a/tests/test_metric_functions.py b/tests/test_metric_functions.py index 154a13eff..ec0274310 100644 --- a/tests/test_metric_functions.py +++ b/tests/test_metric_functions.py @@ -1,18 +1,19 @@ import os from typing import OrderedDict -import yaml import numpy as np +import yaml +from bddl.condition_evaluation import evaluate_state import omnigibson as og from omnigibson.macros import gm from omnigibson.tasks.behavior_task import BehaviorTask -from bddl.condition_evaluation import evaluate_state # Make sure object states are enabled gm.ENABLE_OBJECT_STATES = True gm.USE_GPU_DYNAMICS = True + def setup_env(): # Generates a BEHAVIOR Task environment using presampled cache @@ -29,6 +30,7 @@ def setup_env(): return env + def test_behavior_reset(): env = setup_env() @@ -42,7 +44,7 @@ def test_behavior_reset(): env.reset() # perform a step with no action - action = OrderedDict([('robot0', [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0])]) + action = OrderedDict([("robot0", [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0])]) state, reward, terminated, truncated, info = env.step(action) metrics = info["metrics"] @@ -53,16 +55,17 @@ def test_behavior_reset(): assert metrics["energy"] == 0, "Energy metric was not reset" assert metrics["work"] == 0, "Work metric was not reset" + def test_behavior_task_work_metric(): env = setup_env() # perform a step with no action - action = OrderedDict([('robot0', [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0])]) + action = OrderedDict([("robot0", [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0])]) state, reward, terminated, truncated, info = env.step(action) metrics = info["metrics"] - + assert isinstance(metrics, dict) # assert that one step is taken @@ -90,5 +93,3 @@ def test_behavior_task_work_metric(): # Always close the environment at the end env.close() - - From d267a51ee26b4640fe8a5c2d6af6c5ea6acebcb6 Mon Sep 17 00:00:00 2001 From: Frank Yang Date: Mon, 12 Aug 2024 14:17:42 -0700 Subject: [PATCH 18/28] pending for _dummy --- omnigibson/configs/fetch_behavior.yaml | 7 +++ omnigibson/envs/env_base.py | 2 +- omnigibson/metrics/__init__.py | 2 +- omnigibson/metrics/metrics_base.py | 12 +++-- omnigibson/metrics/step_metric.py | 8 ++-- omnigibson/metrics/task_success_metric.py | 8 ++-- omnigibson/metrics/wall_time_metric.py | 8 ++-- ...energy_metric.py => work_energy_metric.py} | 48 ++++++++++--------- omnigibson/objects/controllable_object.py | 4 +- omnigibson/tasks/behavior_task.py | 33 ++++++++++++- omnigibson/tasks/dummy_task.py | 4 ++ omnigibson/tasks/grasp_task.py | 4 ++ omnigibson/tasks/point_navigation_task.py | 6 ++- omnigibson/tasks/point_reaching_task.py | 4 ++ omnigibson/tasks/task_base.py | 39 ++++++++------- .../termination_conditions/predicate_goal.py | 1 - .../termination_condition_base.py | 1 - tests/test_metric_functions.py | 17 +++++-- 18 files changed, 140 insertions(+), 68 deletions(-) rename omnigibson/metrics/{energy_metric.py => work_energy_metric.py} (50%) diff --git a/omnigibson/configs/fetch_behavior.yaml b/omnigibson/configs/fetch_behavior.yaml index 517ad4325..97f7ba833 100644 --- a/omnigibson/configs/fetch_behavior.yaml +++ b/omnigibson/configs/fetch_behavior.yaml @@ -84,3 +84,10 @@ task: max_steps: 500 reward_config: r_potential: 1.0 + metric_config: + step: 0.05 + task_success: 0.6 + wall_time: 0.05 + work: 0.2 + energy: 0.2 + diff --git a/omnigibson/envs/env_base.py b/omnigibson/envs/env_base.py index 1a7f0ade9..1cc5ae952 100644 --- a/omnigibson/envs/env_base.py +++ b/omnigibson/envs/env_base.py @@ -532,10 +532,10 @@ def _populate_info(self, info): def _pre_step(self, action): """Apply the pre-sim-step part of an environment step, i.e. apply the robot actions.""" - # record the start time # record the start time of the simulation step in the beginning of the step self._cur_sim_start_ts = time.perf_counter() + # If the action is not a dictionary, convert into a dictionary if not isinstance(action, dict) and not isinstance(action, gym.spaces.Dict): action_dict = dict() diff --git a/omnigibson/metrics/__init__.py b/omnigibson/metrics/__init__.py index 78e8934b7..e21abf90c 100644 --- a/omnigibson/metrics/__init__.py +++ b/omnigibson/metrics/__init__.py @@ -1,4 +1,4 @@ -from omnigibson.metrics.energy_metric import EnergyMetric +from omnigibson.metrics.work_energy_metric import WorkEnergyMetric from omnigibson.metrics.metrics_base import BaseMetric from omnigibson.metrics.step_metric import StepMetric from omnigibson.metrics.task_success_metric import TaskSuccessMetric diff --git a/omnigibson/metrics/metrics_base.py b/omnigibson/metrics/metrics_base.py index 63432e9e3..387538d96 100644 --- a/omnigibson/metrics/metrics_base.py +++ b/omnigibson/metrics/metrics_base.py @@ -10,8 +10,9 @@ class BaseMetric: """ def __init__(self): - # Store internal vars that will be filled in at runtime - self._metric = 0 + + # Store internal vars that will be filled in at runtime. The is a metric dictionary computed metric subclasses + self.metric = None @abstractmethod def _step(self, task, env, action): @@ -44,11 +45,12 @@ def step(self, task, env, action): - bool: computed metric - dict: any metric-related information for this specific metric """ + # Step internally and store output - self._metric = self._step(task=task, env=env, action=action) + self.metric = self._step(task=task, env=env, action=action) # Return metric - return self._metric + return self.metric def reset(self, task, env): """ @@ -56,4 +58,4 @@ def reset(self, task, env): """ # Reset internal vars - self._metric = 0 + self.metric = None diff --git a/omnigibson/metrics/step_metric.py b/omnigibson/metrics/step_metric.py index 084e8d870..10c7f6587 100644 --- a/omnigibson/metrics/step_metric.py +++ b/omnigibson/metrics/step_metric.py @@ -8,12 +8,12 @@ class StepMetric(BaseMetric): """ def __init__(self): - # Run super - super().__init__() + #initialize step + self._metric = 0 def _step(self, task, env, action): self._metric += 1 - return self._metric + return {"step": self._metric} def reset(self, task, env): - super().reset(task, env) + self._metric = 0 diff --git a/omnigibson/metrics/task_success_metric.py b/omnigibson/metrics/task_success_metric.py index d8635686d..80e37248f 100644 --- a/omnigibson/metrics/task_success_metric.py +++ b/omnigibson/metrics/task_success_metric.py @@ -8,8 +8,8 @@ class TaskSuccessMetric(BaseMetric): """ def __init__(self): - # Run super - super().__init__() + # initialize task success metric + self._metric = 0 def _step(self, task, env, action): successes = [] @@ -33,7 +33,7 @@ def _step(self, task, env, action): else: self._metric = 0.0 - return self._metric + return {"task_success": self._metric} def reset(self, task, env): - super().reset(task, env) + self._metric = 0 diff --git a/omnigibson/metrics/wall_time_metric.py b/omnigibson/metrics/wall_time_metric.py index a5ceb8ba9..c27ca947e 100644 --- a/omnigibson/metrics/wall_time_metric.py +++ b/omnigibson/metrics/wall_time_metric.py @@ -8,12 +8,12 @@ class WallTimeMetric(BaseMetric): """ def __init__(self): - # Run super - super().__init__() + # initialize wall time metric + self._metric = 0 def _step(self, task, env, action): self._metric += env.last_step_wall_time - return self._metric + return {"wall_time": self._metric} def reset(self, task, env): - super().reset(task, env) + self._metric = 0 diff --git a/omnigibson/metrics/energy_metric.py b/omnigibson/metrics/work_energy_metric.py similarity index 50% rename from omnigibson/metrics/energy_metric.py rename to omnigibson/metrics/work_energy_metric.py index 94d03f395..b78b1c13f 100644 --- a/omnigibson/metrics/energy_metric.py +++ b/omnigibson/metrics/work_energy_metric.py @@ -1,28 +1,23 @@ import numpy as np from omnigibson.metrics.metrics_base import BaseMetric - - -class EnergyMetric(BaseMetric): +class WorkEnergyMetric(BaseMetric): """ - Energy Metric + Work and Energy Metric Measures displacement * mass for every link - Args: - measure_work: If true, measure beginning and end delta rather than step by step delta """ - def __init__(self, measure_work=False): + def __init__(self): - # Run super - super().__init__() + # Run work and energy metric initialization + self._work_metric = 0 + self._energy_metric = 0 self.state_cache = None + self.prev_state_cache = None self.link_masses = {} - # parameter for checking if this is a work or energy metric. If true, measure work done, else measure energy - self.measure_work = measure_work - def _step(self, task, env, action): # calculate the current pose of all object links @@ -40,25 +35,34 @@ def _step(self, task, env, action): for link_name, link in obj._links.items(): self.link_masses[link_name] = link.mass - return 0.0 + return {"work": 0, "energy": 0} # calculate the energy spent from the previous state to the current state work_metric = 0.0 + energy_metric = 0.0 for linkname, posrot in new_state_cache.items(): # TODO: this computation is very slow, consider using a more efficient method # TODO: this method needs to be updated to account for object addition and removal - posrot2 = self.state_cache[linkname] - work_metric += np.linalg.norm(posrot[0] - posrot2[0]) * self.link_masses[linkname] + init_posrot = self.state_cache[linkname] + work_metric += np.linalg.norm(posrot[0] - init_posrot[0]) * self.link_masses[linkname] - # if measuring energy, update the state cache - if not self.measure_work: - self.state_cache = new_state_cache + if self.prev_state_cache is not None: + prev_posrot = self.prev_state_cache[linkname] + energy_metric += np.linalg.norm(posrot[0] - prev_posrot[0]) * self.link_masses[linkname] + + # update the prev_state cache for energy measurement + self.prev_state_cache = new_state_cache - # update the metric accordingly, either set the work done (measuring work) or add it to previous energy spent (measuring energy) - self._metric = work_metric if self.measure_work else (self._metric + work_metric) - return self._metric + # update the metric accordingly, set the work done (measuring work) and add energy spent this step to previous energy spent (measuring energy) + self._work_metric = work_metric + self._energy_metric += energy_metric + return {"work": self._work_metric, "energy": self._energy_metric} def reset(self, task, env): - super().reset(task, env) + + # reset both _work_metric and _energy_metric, and the state cache + self._work_metric = 0 + self._energy_metric = 0 self.state_cache = None + self.prev_state_cache = None diff --git a/omnigibson/objects/controllable_object.py b/omnigibson/objects/controllable_object.py index 160820317..5994602cc 100644 --- a/omnigibson/objects/controllable_object.py +++ b/omnigibson/objects/controllable_object.py @@ -574,9 +574,11 @@ def get_control_dict(self): fcns["joint_effort"] = lambda: ControllableObjectViewAPI.get_joint_efforts(self.articulation_root_path) fcns["mass_matrix"] = lambda: ControllableObjectViewAPI.get_mass_matrix(self.articulation_root_path) # TODO: Move gravity force computation dummy to this class instead of BaseRobot + breakpoint() fcns["gravity_force"] = lambda: ( ControllableObjectViewAPI.get_generalized_gravity_forces(self.articulation_root_path) - if self.fixed_base or self._dummy is None + if self.fixed_base + # or self._dummy is None else ControllableObjectViewAPI.get_generalized_gravity_forces(self._dummy.articulation_root_path) ) fcns["cc_force"] = lambda: ControllableObjectViewAPI.get_coriolis_and_centrifugal_forces( diff --git a/omnigibson/tasks/behavior_task.py b/omnigibson/tasks/behavior_task.py index 73e71155b..923793860 100644 --- a/omnigibson/tasks/behavior_task.py +++ b/omnigibson/tasks/behavior_task.py @@ -18,6 +18,7 @@ from omnigibson.macros import gm from omnigibson.object_states import Pose from omnigibson.reward_functions.potential_reward import PotentialReward +from omnigibson.metrics import WorkEnergyMetric, StepMetric, TaskSuccessMetric, WallTimeMetric from omnigibson.robots.robot_base import BaseRobot from omnigibson.scenes.interactive_traversable_scene import InteractiveTraversableScene from omnigibson.scenes.scene_base import Scene @@ -67,6 +68,7 @@ def __init__( highlight_task_relevant_objects=False, termination_config=None, reward_config=None, + metric_config=None, ): # Make sure object states are enabled assert gm.ENABLE_OBJECT_STATES, "Must set gm.ENABLE_OBJECT_STATES=True in order to use BehaviorTask!" @@ -121,7 +123,7 @@ def __init__( ) # Run super init - super().__init__(termination_config=termination_config, reward_config=reward_config) + super().__init__(termination_config=termination_config, reward_config=reward_config, metric_config=metric_config) @classmethod def get_cached_activity_scene_filename( @@ -195,6 +197,22 @@ def _create_reward_functions(self): ) return rewards + + def _create_metric_functions(self): + + """ + Creates the metric functions in the environment + + Returns: + dict of BaseRewardFunction: Metric functions created for Behavior task + """ + metrics = dict() + + metrics["steps"] = StepMetric() + metrics["task_success"] = TaskSuccessMetric() + metrics["wall_time"] = WallTimeMetric() + metrics["energy_work"] = WorkEnergyMetric() + return metrics def _load(self, env): # Initialize the current activity @@ -378,7 +396,8 @@ def _get_obs(self, env): # Batch rpy calculations for much better efficiency objs_exist = { - obj: obj.exists for obj in self.object_scope.values() if not obj.is_system and obj.states[Pose]._initialized + obj: obj.exists for obj in self.object_scope.values() if not obj.is_system + # and obj.states[Pose]._initialized } objs_rpy = T.quat2euler( np.array( @@ -569,3 +588,13 @@ def default_reward_config(cls): return { "r_potential": 1.0, } + + @classproperty + def default_metric_config(cls): + return { + "step": 0.05, + "task_success": 0.6, + "wall_time": 0.05, + "work": 0.2, + "energy": 0.2, + } diff --git a/omnigibson/tasks/dummy_task.py b/omnigibson/tasks/dummy_task.py index d711a32c8..4b55b7046 100644 --- a/omnigibson/tasks/dummy_task.py +++ b/omnigibson/tasks/dummy_task.py @@ -22,6 +22,10 @@ def _create_termination_conditions(self): def _create_reward_functions(self): # Do nothing return dict() + + def _create_metric_functions(self): + # Do nothing + return dict() def _get_obs(self, env): # No task-specific obs of any kind diff --git a/omnigibson/tasks/grasp_task.py b/omnigibson/tasks/grasp_task.py index cb2c0bfe8..575210e0e 100644 --- a/omnigibson/tasks/grasp_task.py +++ b/omnigibson/tasks/grasp_task.py @@ -78,6 +78,10 @@ def _create_reward_functions(self): rewards = dict() rewards["grasp"] = GraspReward(self.obj_name, **self._reward_config) return rewards + + def _create_metric_functions(self): + # No metric functions + return dict() def _reset_agent(self, env): robot = env.robots[0] diff --git a/omnigibson/tasks/point_navigation_task.py b/omnigibson/tasks/point_navigation_task.py index 3c161dd80..fd4ca6f9d 100644 --- a/omnigibson/tasks/point_navigation_task.py +++ b/omnigibson/tasks/point_navigation_task.py @@ -145,7 +145,11 @@ def _create_reward_functions(self): ) return rewards - + + def _create_metric_functions(self): + # No metrics for this task + return dict() + def _load(self, env): # Load visualization self._load_visualization_markers(env=env) diff --git a/omnigibson/tasks/point_reaching_task.py b/omnigibson/tasks/point_reaching_task.py index cfd63aebd..b19908e48 100644 --- a/omnigibson/tasks/point_reaching_task.py +++ b/omnigibson/tasks/point_reaching_task.py @@ -104,6 +104,10 @@ def _create_termination_conditions(self): return terminations + def _create_metric_functions(self): + # No metrics for this task + return dict() + def _sample_initial_pose_and_goal_pos(self, env, max_trials=100): # Run super first initial_pos, initial_ori, goal_pos = super()._sample_initial_pose_and_goal_pos(env=env, max_trials=max_trials) diff --git a/omnigibson/tasks/task_base.py b/omnigibson/tasks/task_base.py index 51b798f90..5a27926b8 100644 --- a/omnigibson/tasks/task_base.py +++ b/omnigibson/tasks/task_base.py @@ -3,7 +3,6 @@ import numpy as np -from omnigibson.metrics import EnergyMetric, StepMetric, TaskSuccessMetric, WallTimeMetric from omnigibson.utils.gym_utils import GymObservable from omnigibson.utils.python_utils import Registerable, classproperty @@ -26,7 +25,7 @@ class BaseTask(GymObservable, Registerable, metaclass=ABCMeta): in with the default config. See cls.default_reward_config for default values used """ - def __init__(self, termination_config=None, reward_config=None): + def __init__(self, termination_config=None, reward_config=None, metric_config=None): # Make sure configs are dictionaries termination_config = dict() if termination_config is None else termination_config reward_config = dict() if reward_config is None else reward_config @@ -46,6 +45,8 @@ def __init__(self, termination_config=None, reward_config=None): self._reward_config = self.default_reward_config self._reward_config.update(reward_config) + self._metric_config = metric_config + # Generate reward and termination functions self._termination_conditions = self._create_termination_conditions() self._reward_functions = self._create_reward_functions() @@ -154,15 +155,9 @@ def _create_metric_functions(self): Returns: dict of BaseRewardFunction: Metric functions created for this task """ - metrics = dict() - - metrics["steps"] = StepMetric() - metrics["task_success"] = TaskSuccessMetric() - metrics["wall_time"] = WallTimeMetric() - metrics["energy"] = EnergyMetric() - metrics["work"] = EnergyMetric(measure_work=True) - - return metrics + + # The metric functions are individually configured in the task class + raise NotImplementedError() def _reset_scene(self, env): """ @@ -296,15 +291,17 @@ def _step_metrics(self, env, action): - the break down of the metric scores and the metric info """ - # We'll also store individual reward split + # We'll also store individual metric split breakdown_dict = dict() + metric_score = 0 - for metric_name, metric_function in self._metric_functions.items(): + for metric_function in self._metric_functions.values(): metric = metric_function.step(self, env, action) - breakdown_dict[metric_name] = metric + breakdown_dict.update(metric) + + for metric_name, metric_score in breakdown_dict.items(): + metric_score += metric_score * self._metric_config[metric_name] - # TODO: metric score is currently summed, work on the global coefficients - metric_score = sum(breakdown_dict.values()) return metric_score, breakdown_dict @abstractmethod @@ -458,6 +455,16 @@ def default_termination_config(cls): will raise an error! """ raise NotImplementedError() + + @classproperty + def default_metric_config(cls): + """ + Returns: + dict: Default metric configuration for this class. Should include any kwargs necessary for + any of the metric classes. Note: this default config should be fully verbose -- any keys + inputted in the constructor but NOT found in this default config will raise an error! + """ + raise NotImplementedError() @classproperty def _do_not_register_classes(cls): diff --git a/omnigibson/termination_conditions/predicate_goal.py b/omnigibson/termination_conditions/predicate_goal.py index 3cb0eb12a..75f690ce0 100644 --- a/omnigibson/termination_conditions/predicate_goal.py +++ b/omnigibson/termination_conditions/predicate_goal.py @@ -51,5 +51,4 @@ def partial_success(self): Returns: bool: returns true if partial success is supported, false otherwise """ - assert self._done is not None, "At least one step() must occur before partial_success can be calculated!" return True diff --git a/omnigibson/termination_conditions/termination_condition_base.py b/omnigibson/termination_conditions/termination_condition_base.py index 5fe381695..e62c250ad 100644 --- a/omnigibson/termination_conditions/termination_condition_base.py +++ b/omnigibson/termination_conditions/termination_condition_base.py @@ -99,7 +99,6 @@ def partial_success(self): Returns: bool: returns true if partial success is supported, false otherwise """ - assert self._done is not None, "At least one step() must occur before partial_success can be calculated!" return False @classproperty diff --git a/tests/test_metric_functions.py b/tests/test_metric_functions.py index 154a13eff..853f6e446 100644 --- a/tests/test_metric_functions.py +++ b/tests/test_metric_functions.py @@ -34,6 +34,7 @@ def test_behavior_reset(): env = setup_env() action = env.action_space.sample() + action['robot0'] = np.zeros_like(action['robot0']) state, reward, terminated, truncated, info = env.step(action) metrics = info["metrics"] @@ -42,23 +43,27 @@ def test_behavior_reset(): env.reset() # perform a step with no action - action = OrderedDict([('robot0', [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0])]) + action = OrderedDict([('robot0', np.zeros_like(env.action_space.sample()))]) state, reward, terminated, truncated, info = env.step(action) metrics = info["metrics"] - assert metrics["steps"] == 0, "Step metric was not reset" + assert metrics["step"] == 0, "Step metric was not reset" assert metrics["task_success"] == 0, "Task success metric was not reset" assert metrics["wall_time"] == 0, "Wall time metric was not reset" assert metrics["energy"] == 0, "Energy metric was not reset" assert metrics["work"] == 0, "Work metric was not reset" + og.clear() + def test_behavior_task_work_metric(): env = setup_env() # perform a step with no action - action = OrderedDict([('robot0', [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0])]) + action = env.action_space.sample() + action['robot0'] = np.zeros_like(action['robot0']) + state, reward, terminated, truncated, info = env.step(action) metrics = info["metrics"] @@ -66,7 +71,7 @@ def test_behavior_task_work_metric(): assert isinstance(metrics, dict) # assert that one step is taken - assert metrics["steps"] == 1 + assert metrics["step"] == 1 # cache the initial position and orientation of the robot position, orientation = env.robots[0].get_position_orientation() @@ -89,6 +94,8 @@ def test_behavior_task_work_metric(): assert np.allclose(metrics["work"] / robot_mass, 0, atol=1e-3) # Always close the environment at the end - env.close() + og.clear() +if __name__ == "__main__": + test_behavior_task_work_metric() From b6fc0eac3180abd5cd47cc1c0b9b9928e50492ca Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Mon, 12 Aug 2024 21:19:12 +0000 Subject: [PATCH 19/28] [pre-commit.ci] auto fixes from pre-commit.com hooks for more information, see https://pre-commit.ci --- omnigibson/envs/env_base.py | 1 - omnigibson/metrics/__init__.py | 2 +- omnigibson/metrics/step_metric.py | 2 +- omnigibson/metrics/work_energy_metric.py | 2 ++ omnigibson/objects/controllable_object.py | 2 +- omnigibson/tasks/behavior_task.py | 15 +++++++++------ omnigibson/tasks/dummy_task.py | 2 +- omnigibson/tasks/grasp_task.py | 2 +- omnigibson/tasks/point_navigation_task.py | 4 ++-- omnigibson/tasks/point_reaching_task.py | 2 +- omnigibson/tasks/task_base.py | 6 +++--- tests/test_metric_functions.py | 9 +++++---- 12 files changed, 27 insertions(+), 22 deletions(-) diff --git a/omnigibson/envs/env_base.py b/omnigibson/envs/env_base.py index 1cc5ae952..f4e5f92ec 100644 --- a/omnigibson/envs/env_base.py +++ b/omnigibson/envs/env_base.py @@ -535,7 +535,6 @@ def _pre_step(self, action): # record the start time of the simulation step in the beginning of the step self._cur_sim_start_ts = time.perf_counter() - # If the action is not a dictionary, convert into a dictionary if not isinstance(action, dict) and not isinstance(action, gym.spaces.Dict): action_dict = dict() diff --git a/omnigibson/metrics/__init__.py b/omnigibson/metrics/__init__.py index e21abf90c..a8a5328c3 100644 --- a/omnigibson/metrics/__init__.py +++ b/omnigibson/metrics/__init__.py @@ -1,5 +1,5 @@ -from omnigibson.metrics.work_energy_metric import WorkEnergyMetric from omnigibson.metrics.metrics_base import BaseMetric from omnigibson.metrics.step_metric import StepMetric from omnigibson.metrics.task_success_metric import TaskSuccessMetric from omnigibson.metrics.wall_time_metric import WallTimeMetric +from omnigibson.metrics.work_energy_metric import WorkEnergyMetric diff --git a/omnigibson/metrics/step_metric.py b/omnigibson/metrics/step_metric.py index 10c7f6587..88bfa6768 100644 --- a/omnigibson/metrics/step_metric.py +++ b/omnigibson/metrics/step_metric.py @@ -8,7 +8,7 @@ class StepMetric(BaseMetric): """ def __init__(self): - #initialize step + # initialize step self._metric = 0 def _step(self, task, env, action): diff --git a/omnigibson/metrics/work_energy_metric.py b/omnigibson/metrics/work_energy_metric.py index b78b1c13f..83ddb7e44 100644 --- a/omnigibson/metrics/work_energy_metric.py +++ b/omnigibson/metrics/work_energy_metric.py @@ -1,6 +1,8 @@ import numpy as np from omnigibson.metrics.metrics_base import BaseMetric + + class WorkEnergyMetric(BaseMetric): """ Work and Energy Metric diff --git a/omnigibson/objects/controllable_object.py b/omnigibson/objects/controllable_object.py index 5994602cc..592d843ae 100644 --- a/omnigibson/objects/controllable_object.py +++ b/omnigibson/objects/controllable_object.py @@ -577,7 +577,7 @@ def get_control_dict(self): breakpoint() fcns["gravity_force"] = lambda: ( ControllableObjectViewAPI.get_generalized_gravity_forces(self.articulation_root_path) - if self.fixed_base + if self.fixed_base # or self._dummy is None else ControllableObjectViewAPI.get_generalized_gravity_forces(self._dummy.articulation_root_path) ) diff --git a/omnigibson/tasks/behavior_task.py b/omnigibson/tasks/behavior_task.py index 923793860..28fdb6a8d 100644 --- a/omnigibson/tasks/behavior_task.py +++ b/omnigibson/tasks/behavior_task.py @@ -16,9 +16,9 @@ import omnigibson as og import omnigibson.utils.transform_utils as T from omnigibson.macros import gm +from omnigibson.metrics import StepMetric, TaskSuccessMetric, WallTimeMetric, WorkEnergyMetric from omnigibson.object_states import Pose from omnigibson.reward_functions.potential_reward import PotentialReward -from omnigibson.metrics import WorkEnergyMetric, StepMetric, TaskSuccessMetric, WallTimeMetric from omnigibson.robots.robot_base import BaseRobot from omnigibson.scenes.interactive_traversable_scene import InteractiveTraversableScene from omnigibson.scenes.scene_base import Scene @@ -123,7 +123,9 @@ def __init__( ) # Run super init - super().__init__(termination_config=termination_config, reward_config=reward_config, metric_config=metric_config) + super().__init__( + termination_config=termination_config, reward_config=reward_config, metric_config=metric_config + ) @classmethod def get_cached_activity_scene_filename( @@ -197,9 +199,8 @@ def _create_reward_functions(self): ) return rewards - + def _create_metric_functions(self): - """ Creates the metric functions in the environment @@ -396,7 +397,9 @@ def _get_obs(self, env): # Batch rpy calculations for much better efficiency objs_exist = { - obj: obj.exists for obj in self.object_scope.values() if not obj.is_system + obj: obj.exists + for obj in self.object_scope.values() + if not obj.is_system # and obj.states[Pose]._initialized } objs_rpy = T.quat2euler( @@ -588,7 +591,7 @@ def default_reward_config(cls): return { "r_potential": 1.0, } - + @classproperty def default_metric_config(cls): return { diff --git a/omnigibson/tasks/dummy_task.py b/omnigibson/tasks/dummy_task.py index 4b55b7046..5f30f760f 100644 --- a/omnigibson/tasks/dummy_task.py +++ b/omnigibson/tasks/dummy_task.py @@ -22,7 +22,7 @@ def _create_termination_conditions(self): def _create_reward_functions(self): # Do nothing return dict() - + def _create_metric_functions(self): # Do nothing return dict() diff --git a/omnigibson/tasks/grasp_task.py b/omnigibson/tasks/grasp_task.py index 575210e0e..f8eec91bc 100644 --- a/omnigibson/tasks/grasp_task.py +++ b/omnigibson/tasks/grasp_task.py @@ -78,7 +78,7 @@ def _create_reward_functions(self): rewards = dict() rewards["grasp"] = GraspReward(self.obj_name, **self._reward_config) return rewards - + def _create_metric_functions(self): # No metric functions return dict() diff --git a/omnigibson/tasks/point_navigation_task.py b/omnigibson/tasks/point_navigation_task.py index fd4ca6f9d..4a1d73b57 100644 --- a/omnigibson/tasks/point_navigation_task.py +++ b/omnigibson/tasks/point_navigation_task.py @@ -145,11 +145,11 @@ def _create_reward_functions(self): ) return rewards - + def _create_metric_functions(self): # No metrics for this task return dict() - + def _load(self, env): # Load visualization self._load_visualization_markers(env=env) diff --git a/omnigibson/tasks/point_reaching_task.py b/omnigibson/tasks/point_reaching_task.py index b19908e48..3e598ff98 100644 --- a/omnigibson/tasks/point_reaching_task.py +++ b/omnigibson/tasks/point_reaching_task.py @@ -107,7 +107,7 @@ def _create_termination_conditions(self): def _create_metric_functions(self): # No metrics for this task return dict() - + def _sample_initial_pose_and_goal_pos(self, env, max_trials=100): # Run super first initial_pos, initial_ori, goal_pos = super()._sample_initial_pose_and_goal_pos(env=env, max_trials=max_trials) diff --git a/omnigibson/tasks/task_base.py b/omnigibson/tasks/task_base.py index 5a27926b8..7a1d08a02 100644 --- a/omnigibson/tasks/task_base.py +++ b/omnigibson/tasks/task_base.py @@ -155,7 +155,7 @@ def _create_metric_functions(self): Returns: dict of BaseRewardFunction: Metric functions created for this task """ - + # The metric functions are individually configured in the task class raise NotImplementedError() @@ -455,13 +455,13 @@ def default_termination_config(cls): will raise an error! """ raise NotImplementedError() - + @classproperty def default_metric_config(cls): """ Returns: dict: Default metric configuration for this class. Should include any kwargs necessary for - any of the metric classes. Note: this default config should be fully verbose -- any keys + any of the metric classes. Note: this default config should be fully verbose -- any keys inputted in the constructor but NOT found in this default config will raise an error! """ raise NotImplementedError() diff --git a/tests/test_metric_functions.py b/tests/test_metric_functions.py index 6bc8a1f6f..8c0bb764c 100644 --- a/tests/test_metric_functions.py +++ b/tests/test_metric_functions.py @@ -36,7 +36,7 @@ def test_behavior_reset(): env = setup_env() action = env.action_space.sample() - action['robot0'] = np.zeros_like(action['robot0']) + action["robot0"] = np.zeros_like(action["robot0"]) state, reward, terminated, truncated, info = env.step(action) metrics = info["metrics"] @@ -45,7 +45,7 @@ def test_behavior_reset(): env.reset() # perform a step with no action - action = OrderedDict([('robot0', np.zeros_like(env.action_space.sample()))]) + action = OrderedDict([("robot0", np.zeros_like(env.action_space.sample()))]) state, reward, terminated, truncated, info = env.step(action) metrics = info["metrics"] @@ -58,13 +58,14 @@ def test_behavior_reset(): og.clear() + def test_behavior_task_work_metric(): env = setup_env() # perform a step with no action action = env.action_space.sample() - action['robot0'] = np.zeros_like(action['robot0']) + action["robot0"] = np.zeros_like(action["robot0"]) state, reward, terminated, truncated, info = env.step(action) @@ -98,6 +99,6 @@ def test_behavior_task_work_metric(): # Always close the environment at the end og.clear() + if __name__ == "__main__": test_behavior_task_work_metric() - From b20bc4a5bb79a788b198bfa33c3cdead12bcd379 Mon Sep 17 00:00:00 2001 From: Frank Yang Date: Tue, 13 Aug 2024 13:23:45 -0700 Subject: [PATCH 20/28] metric refactoring --- omnigibson/configs/fetch_behavior.yaml | 2 + omnigibson/envs/env_base.py | 9 +- omnigibson/metrics/work_energy_metric.py | 36 +++++-- omnigibson/objects/controllable_object.py | 36 ++++++- omnigibson/robots/robot_base.py | 16 ---- omnigibson/tasks/behavior_task.py | 5 +- omnigibson/tasks/dummy_task.py | 5 + omnigibson/tasks/grasp_task.py | 5 + omnigibson/tasks/point_navigation_task.py | 5 + omnigibson/tasks/point_reaching_task.py | 2 +- omnigibson/tasks/task_base.py | 9 +- tests/test_dump_load_states.py | 3 + tests/test_metric_functions.py | 112 ++++++++++++++++------ 13 files changed, 173 insertions(+), 72 deletions(-) diff --git a/omnigibson/configs/fetch_behavior.yaml b/omnigibson/configs/fetch_behavior.yaml index 97f7ba833..35604c0f8 100644 --- a/omnigibson/configs/fetch_behavior.yaml +++ b/omnigibson/configs/fetch_behavior.yaml @@ -90,4 +90,6 @@ task: wall_time: 0.05 work: 0.2 energy: 0.2 + rotation: 0.1 + translation: 0.9 diff --git a/omnigibson/envs/env_base.py b/omnigibson/envs/env_base.py index 1cc5ae952..923bd740a 100644 --- a/omnigibson/envs/env_base.py +++ b/omnigibson/envs/env_base.py @@ -533,8 +533,7 @@ def _pre_step(self, action): """Apply the pre-sim-step part of an environment step, i.e. apply the robot actions.""" # record the start time of the simulation step in the beginning of the step - self._cur_sim_start_ts = time.perf_counter() - + self._cur_sim_start_ts = og.sim.current_time # If the action is not a dictionary, convert into a dictionary if not isinstance(action, dict) and not isinstance(action, gym.spaces.Dict): @@ -588,7 +587,7 @@ def _post_step(self, action): # record end time # record the end time of the simulation step in the end of the step - self._prev_sim_end_ts = time.perf_counter() + self._prev_sim_end_ts = og.sim.current_time return obs, reward, terminated, truncated, info @@ -714,9 +713,9 @@ def last_step_wall_time(self): # return 0 if the simulation has not started yet if not self._prev_sim_end_ts or not self._cur_sim_start_ts: return 0 - + assert ( - self._prev_sim_end_ts < self._cur_sim_start_ts + self._prev_sim_end_ts <= self._cur_sim_start_ts ), "end time from the previous iteration must be less than the start time of the current iteration" return self._cur_sim_start_ts - self._prev_sim_end_ts diff --git a/omnigibson/metrics/work_energy_metric.py b/omnigibson/metrics/work_energy_metric.py index b78b1c13f..df0a2d863 100644 --- a/omnigibson/metrics/work_energy_metric.py +++ b/omnigibson/metrics/work_energy_metric.py @@ -1,6 +1,7 @@ import numpy as np from omnigibson.metrics.metrics_base import BaseMetric +import omnigibson.utils.transform_utils as T class WorkEnergyMetric(BaseMetric): """ Work and Energy Metric @@ -9,7 +10,7 @@ class WorkEnergyMetric(BaseMetric): """ - def __init__(self): + def __init__(self, metric_config=None): # Run work and energy metric initialization self._work_metric = 0 @@ -17,6 +18,7 @@ def __init__(self): self.state_cache = None self.prev_state_cache = None self.link_masses = {} + self.metric_config = metric_config def _step(self, task, env, action): @@ -27,29 +29,45 @@ def _step(self, task, env, action): pos, rot = link.get_position_orientation() new_state_cache[link_name] = (pos, rot) + # compute this every step to account for object addition and removal + self.link_masses[link_name] = link.mass + # if the state cache is empty, set it to the current state and return 0 if not self.state_cache: self.state_cache = new_state_cache - - for obj in env.scene.objects: - for link_name, link in obj._links.items(): - self.link_masses[link_name] = link.mass - + self.prev_state_cache = new_state_cache return {"work": 0, "energy": 0} # calculate the energy spent from the previous state to the current state work_metric = 0.0 energy_metric = 0.0 + for linkname, posrot in new_state_cache.items(): # TODO: this computation is very slow, consider using a more efficient method - # TODO: this method needs to be updated to account for object addition and removal + + # check if the link is originally in the state cache, if not, skip it to account for object addition and removal + if linkname not in self.state_cache: + continue + init_posrot = self.state_cache[linkname] - work_metric += np.linalg.norm(posrot[0] - init_posrot[0]) * self.link_masses[linkname] + position, orientation = T.relative_pose_transform(posrot[0], posrot[1], init_posrot[0], init_posrot[1]) + work_metric += np.linalg.norm(position) * self.link_masses[linkname] * self.metric_config["translation"] + + # calculate the energy spent in rotation. TODO: this is a very rough approximation, consider using a more accurate method + work_metric += np.linalg.norm(orientation) * self.link_masses[linkname] * self.metric_config["rotation"] + + # check if the link is in the prev_state_cache, if not, skip it to account for object addition and removal + if linkname not in self.prev_state_cache: + continue if self.prev_state_cache is not None: prev_posrot = self.prev_state_cache[linkname] - energy_metric += np.linalg.norm(posrot[0] - prev_posrot[0]) * self.link_masses[linkname] + position, orientation = T.relative_pose_transform(posrot[0], posrot[1], prev_posrot[0], prev_posrot[1]) + energy_metric += np.linalg.norm(position) * self.link_masses[linkname] * self.metric_config["translation"] + + # calculate the energy spent in rotation. TODO: this is a very rough approximation, consider using a more accurate method + energy_metric += np.linalg.norm(orientation) * self.link_masses[linkname] * self.metric_config["rotation"] # update the prev_state cache for energy measurement self.prev_state_cache = new_state_cache diff --git a/omnigibson/objects/controllable_object.py b/omnigibson/objects/controllable_object.py index 5994602cc..e449f86d0 100644 --- a/omnigibson/objects/controllable_object.py +++ b/omnigibson/objects/controllable_object.py @@ -12,7 +12,11 @@ from omnigibson.utils.constants import PrimType from omnigibson.utils.python_utils import CachedFunctions, assert_valid_key, merge_nested_dicts from omnigibson.utils.ui_utils import create_module_logger -from omnigibson.utils.usd_utils import ControllableObjectViewAPI +from omnigibson.utils.usd_utils import ( + ControllableObjectViewAPI, + absolute_prim_path_to_scene_relative, + add_asset_to_stage, +) # Create module logger log = create_module_logger(module_name=__name__) @@ -190,6 +194,29 @@ def load(self, scene): ), "Stored control frequency does not match environment's render timestep." return prim + + def _load(self): + # Run super first + + prim = super()._load() + + # Also import dummy object if this robot is not fixed base AND it has a controller that + # requires generalized gravity forces. We incur a relatively heavy cost at every step if we + # have to move the dummy. So we only do this if we absolutely need to. + if not self.fixed_base: + dummy_path = self.prim_path.replace("controllable__", "dummy__") + dummy_prim = add_asset_to_stage(asset_path=self._dummy_usd_path, prim_path=dummy_path) + self._dummy = BaseObject( + name=f"{self.name}_dummy", + relative_prim_path=absolute_prim_path_to_scene_relative(self.scene, dummy_path), + scale=self._load_config.get("scale", None), + visible=False, + fixed_base=True, + visual_only=True, + ) + self._dummy.load(self.scene) + + return prim def _load_controllers(self): """ @@ -573,12 +600,11 @@ def get_control_dict(self): fcns["joint_velocity"] = lambda: ControllableObjectViewAPI.get_joint_velocities(self.articulation_root_path) fcns["joint_effort"] = lambda: ControllableObjectViewAPI.get_joint_efforts(self.articulation_root_path) fcns["mass_matrix"] = lambda: ControllableObjectViewAPI.get_mass_matrix(self.articulation_root_path) - # TODO: Move gravity force computation dummy to this class instead of BaseRobot - breakpoint() fcns["gravity_force"] = lambda: ( ControllableObjectViewAPI.get_generalized_gravity_forces(self.articulation_root_path) - if self.fixed_base - # or self._dummy is None + + # check if dummy is None to account for the fact that the dummy may not be loaded yet + if self.fixed_base or self._dummy is None else ControllableObjectViewAPI.get_generalized_gravity_forces(self._dummy.articulation_root_path) ) fcns["cc_force"] = lambda: ControllableObjectViewAPI.get_coriolis_and_centrifugal_forces( diff --git a/omnigibson/robots/robot_base.py b/omnigibson/robots/robot_base.py index ed0c8050b..8b744b167 100644 --- a/omnigibson/robots/robot_base.py +++ b/omnigibson/robots/robot_base.py @@ -172,22 +172,6 @@ def _load(self): # Run super first prim = super()._load() - # Also import dummy object if this robot is not fixed base AND it has a controller that - # requires generalized gravity forces. We incur a relatively heavy cost at every step if we - # have to move the dummy. So we only do this if we absolutely need to. - if not self.fixed_base: - dummy_path = self.prim_path.replace("controllable__", "dummy__") - dummy_prim = add_asset_to_stage(asset_path=self._dummy_usd_path, prim_path=dummy_path) - self._dummy = BaseObject( - name=f"{self.name}_dummy", - relative_prim_path=absolute_prim_path_to_scene_relative(self.scene, dummy_path), - scale=self._load_config.get("scale", None), - visible=False, - fixed_base=True, - visual_only=True, - ) - self._dummy.load(self.scene) - return prim def _post_load(self): diff --git a/omnigibson/tasks/behavior_task.py b/omnigibson/tasks/behavior_task.py index 923793860..131690257 100644 --- a/omnigibson/tasks/behavior_task.py +++ b/omnigibson/tasks/behavior_task.py @@ -211,7 +211,7 @@ def _create_metric_functions(self): metrics["steps"] = StepMetric() metrics["task_success"] = TaskSuccessMetric() metrics["wall_time"] = WallTimeMetric() - metrics["energy_work"] = WorkEnergyMetric() + metrics["energy_work"] = WorkEnergyMetric(metric_config=self._metric_config) return metrics def _load(self, env): @@ -397,7 +397,6 @@ def _get_obs(self, env): # Batch rpy calculations for much better efficiency objs_exist = { obj: obj.exists for obj in self.object_scope.values() if not obj.is_system - # and obj.states[Pose]._initialized } objs_rpy = T.quat2euler( np.array( @@ -597,4 +596,6 @@ def default_metric_config(cls): "wall_time": 0.05, "work": 0.2, "energy": 0.2, + "rotation": 0.1, + "translation": 0.9 } diff --git a/omnigibson/tasks/dummy_task.py b/omnigibson/tasks/dummy_task.py index 4b55b7046..f20f75c4b 100644 --- a/omnigibson/tasks/dummy_task.py +++ b/omnigibson/tasks/dummy_task.py @@ -49,3 +49,8 @@ def default_termination_config(cls): def default_reward_config(cls): # Empty dict return {} + + @classproperty + def default_metric_config(cls): + # Empty dict + return {} diff --git a/omnigibson/tasks/grasp_task.py b/omnigibson/tasks/grasp_task.py index 575210e0e..ade25da92 100644 --- a/omnigibson/tasks/grasp_task.py +++ b/omnigibson/tasks/grasp_task.py @@ -243,3 +243,8 @@ def default_reward_config(cls): "eef_orientation_penalty_coef": 0.001, "regularization_coef": 0.01, } + + @classproperty + def default_metric_config(cls): + # Empty dict + return {} diff --git a/omnigibson/tasks/point_navigation_task.py b/omnigibson/tasks/point_navigation_task.py index fd4ca6f9d..862c5b027 100644 --- a/omnigibson/tasks/point_navigation_task.py +++ b/omnigibson/tasks/point_navigation_task.py @@ -499,3 +499,8 @@ def default_reward_config(cls): "r_collision": 0.1, "r_pointgoal": 10.0, } + + @classproperty + def default_metric_config(cls): + # Empty dict + return {} diff --git a/omnigibson/tasks/point_reaching_task.py b/omnigibson/tasks/point_reaching_task.py index b19908e48..8803a2838 100644 --- a/omnigibson/tasks/point_reaching_task.py +++ b/omnigibson/tasks/point_reaching_task.py @@ -139,4 +139,4 @@ def _get_obs(self, env): def get_current_pos(self, env): # Current position is the robot's EEF, not base! - return env.robots[self._robot_idn].get_eef_position() + return env.robots[self._robot_idn].get_eef_position() \ No newline at end of file diff --git a/omnigibson/tasks/task_base.py b/omnigibson/tasks/task_base.py index 5a27926b8..1064d339e 100644 --- a/omnigibson/tasks/task_base.py +++ b/omnigibson/tasks/task_base.py @@ -29,8 +29,9 @@ def __init__(self, termination_config=None, reward_config=None, metric_config=No # Make sure configs are dictionaries termination_config = dict() if termination_config is None else termination_config reward_config = dict() if reward_config is None else reward_config + metric_config = dict() if metric_config is None else metric_config - # Sanity check termination and reward conditions -- any keys found in the inputted config but NOT + # Sanity check termination, reward, and metric conditions -- any keys found in the inputted config but NOT # found in the default config should raise an error unknown_termination_keys = set(termination_config.keys()) - set(self.default_termination_config.keys()) assert ( @@ -38,14 +39,16 @@ def __init__(self, termination_config=None, reward_config=None, metric_config=No ), f"Got unknown termination config keys inputted: {unknown_termination_keys}" unknown_reward_keys = set(reward_config.keys()) - set(self.default_reward_config.keys()) assert len(unknown_reward_keys) == 0, f"Got unknown reward config keys inputted: {unknown_reward_keys}" + unknown_metric_keys = set(metric_config.keys()) - set(self.default_metric_config.keys()) + assert len(unknown_metric_keys) == 0, f"Got unknown metric config keys inputted: {unknown_metric_keys}" # Combine with defaults and store internally self._termination_config = self.default_termination_config self._termination_config.update(termination_config) self._reward_config = self.default_reward_config self._reward_config.update(reward_config) - - self._metric_config = metric_config + self._metric_config = self.default_metric_config + self._metric_config.update(metric_config) # Generate reward and termination functions self._termination_conditions = self._create_termination_conditions() diff --git a/tests/test_dump_load_states.py b/tests/test_dump_load_states.py index ab1b05fcd..37321f5f6 100644 --- a/tests/test_dump_load_states.py +++ b/tests/test_dump_load_states.py @@ -45,3 +45,6 @@ def test_dump_load_serialized(env): for system_name, system_class in SYSTEM_EXAMPLES.items(): system = env.scene.get_system(system_name) system.clear() + +if __name__ == "__main__": + test_dump_load() diff --git a/tests/test_metric_functions.py b/tests/test_metric_functions.py index 6bc8a1f6f..94b4ac94b 100644 --- a/tests/test_metric_functions.py +++ b/tests/test_metric_functions.py @@ -4,10 +4,12 @@ import numpy as np import yaml from bddl.condition_evaluation import evaluate_state +from omnigibson.objects import DatasetObject import omnigibson as og from omnigibson.macros import gm from omnigibson.tasks.behavior_task import BehaviorTask +import omnigibson.utils.transform_utils as T # Make sure object states are enabled gm.ENABLE_OBJECT_STATES = True @@ -27,77 +29,125 @@ def setup_env(): # Load the environment env = og.Environment(configs=cfg) - + env.reset() return env +env = setup_env() -def test_behavior_reset(): - - env = setup_env() +def test_behavior_task_work_metric(): + # perform a step with no action action = env.action_space.sample() action['robot0'] = np.zeros_like(action['robot0']) + state, reward, terminated, truncated, info = env.step(action) + metrics = info["metrics"] assert isinstance(metrics, dict) - env.reset() + # assert that one step is taken + assert metrics["step"] == 1 - # perform a step with no action - action = OrderedDict([('robot0', np.zeros_like(env.action_space.sample()))]) - state, reward, terminated, truncated, info = env.step(action) + # cache the initial position and orientation of the robot + position, orientation = env.robots[0].get_position_orientation() + + # move the robot to a new position and orientation, and then back to the initial position and orientation + env.robots[0].set_position_orientation([10, 10, 0]) + env.step(action) + env.robots[0].set_position_orientation(position, orientation) + state, reward, terminated, truncated, info = env.step(action) metrics = info["metrics"] - assert metrics["step"] == 0, "Step metric was not reset" - assert metrics["task_success"] == 0, "Task success metric was not reset" - assert metrics["wall_time"] == 0, "Wall time metric was not reset" - assert metrics["energy"] == 0, "Energy metric was not reset" - assert metrics["work"] == 0, "Work metric was not reset" + # since simulator is running, the work done is non-zero due to random link movements. Assert that the work / robot mass is below a threshold - og.clear() + # calculate robot mass + robot_mass = 0 + for link in env.robots[0].links.values(): + robot_mass += link.mass -def test_behavior_task_work_metric(): + assert np.allclose(metrics["work"] / robot_mass, 0, atol=1e-1) + env.reset() - env = setup_env() +def test_behavior_task_energy_metric(): # perform a step with no action action = env.action_space.sample() action['robot0'] = np.zeros_like(action['robot0']) + env.step(action) + + # cache the initial position and orientation of the robot + position, orientation = env.robots[0].get_position_orientation() + # apply shift to the robot + shift_position, shift_orientation = np.array([10, 10, 0]), np.array([0.05, 0, 0, 0.1]) + env.robots[0].set_position_orientation(shift_position, shift_orientation) state, reward, terminated, truncated, info = env.step(action) + energy = info["metrics"]["energy"] - metrics = info["metrics"] + # shift the robot back to the initial position and orientation + env.robots[0].set_position_orientation(position, orientation) + state, reward, terminated, truncated, info = env.step(action) + new_energy = info["metrics"]["energy"] - assert isinstance(metrics, dict) + # since simulator is running, the work done is non-zero due to random link movements. Assert that the work / robot mass is below a threshold - # assert that one step is taken - assert metrics["step"] == 1 + # calculate robot mass + robot_mass = 0 + for link in env.robots[0].links.values(): + robot_mass += link.mass - # cache the initial position and orientation of the robot - position, orientation = env.robots[0].get_position_orientation() + # assert that the total energy spent is twice as much as the energy spent applying the shift + assert np.allclose((2 * energy - new_energy) / robot_mass, 0, atol=1e-3) + env.reset() - # move the robot to a new position and orientation, and then back to the initial position and orientation - env.robots[0].set_position_orientation([10, 10, 0]) + + +def test_behavior_task_object_addition_removal(): + + # perform a step with no action + action = env.action_space.sample() + action['robot0'] = np.zeros_like(action['robot0']) env.step(action) - env.robots[0].set_position_orientation(position, orientation) + env.robots[0].set_position_orientation([10, 10, 0]) state, reward, terminated, truncated, info = env.step(action) metrics = info["metrics"] - # since simulator is running, the work done is non-zero due to random link movements. Assert that the work / robot mass is below a threshold + work, energy = metrics["work"], metrics["energy"] + + + # add another apple to the environment + apple = DatasetObject( + name="apple_unique", + category="apple", + model="agveuv", + ) + + env.scene.add_object(apple) + # perform a step with no action + state, reward, terminated, truncated, info = env.step(action) + metrics = info["metrics"] + + add_work, add_energy = metrics["work"], metrics["energy"] # calculate robot mass robot_mass = 0 for link in env.robots[0].links.values(): robot_mass += link.mass - assert np.allclose(metrics["work"] / robot_mass, 0, atol=1e-3) + assert np.allclose((work - add_work) / robot_mass, 0, atol=1e-1) + assert np.allclose((energy - add_energy) / robot_mass, 0, atol=1e-1) - # Always close the environment at the end - og.clear() + og.sim.remove_object(obj=apple) + state, reward, terminated, truncated, info = env.step(action) + metrics = info["metrics"] -if __name__ == "__main__": - test_behavior_task_work_metric() + remove_work, remove_energy = metrics["work"], metrics["energy"] + + assert np.allclose((add_work - remove_work) / robot_mass, 0, atol=1e-1) + assert np.allclose((add_energy - remove_energy) / robot_mass, 0, atol=1e-1) + + env.reset() From a60f36c377d0d1c3fb9683543cb435560abe4818 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Tue, 13 Aug 2024 20:26:39 +0000 Subject: [PATCH 21/28] [pre-commit.ci] auto fixes from pre-commit.com hooks for more information, see https://pre-commit.ci --- omnigibson/metrics/work_energy_metric.py | 2 ++ omnigibson/tasks/dummy_task.py | 2 +- omnigibson/tasks/grasp_task.py | 2 +- omnigibson/tasks/point_navigation_task.py | 2 +- omnigibson/tasks/point_reaching_task.py | 2 +- tests/test_dump_load_states.py | 1 + tests/test_metric_functions.py | 20 ++++++++++---------- 7 files changed, 17 insertions(+), 14 deletions(-) diff --git a/omnigibson/metrics/work_energy_metric.py b/omnigibson/metrics/work_energy_metric.py index 7cbb36de4..6072a0b60 100644 --- a/omnigibson/metrics/work_energy_metric.py +++ b/omnigibson/metrics/work_energy_metric.py @@ -1,8 +1,10 @@ import numpy as np from omnigibson.metrics.metrics_base import BaseMetric + <<<<<<< HEAD import omnigibson.utils.transform_utils as T + ======= diff --git a/omnigibson/tasks/dummy_task.py b/omnigibson/tasks/dummy_task.py index 693584859..b9fed388e 100644 --- a/omnigibson/tasks/dummy_task.py +++ b/omnigibson/tasks/dummy_task.py @@ -49,7 +49,7 @@ def default_termination_config(cls): def default_reward_config(cls): # Empty dict return {} - + @classproperty def default_metric_config(cls): # Empty dict diff --git a/omnigibson/tasks/grasp_task.py b/omnigibson/tasks/grasp_task.py index 8d5a86015..060dcef44 100644 --- a/omnigibson/tasks/grasp_task.py +++ b/omnigibson/tasks/grasp_task.py @@ -243,7 +243,7 @@ def default_reward_config(cls): "eef_orientation_penalty_coef": 0.001, "regularization_coef": 0.01, } - + @classproperty def default_metric_config(cls): # Empty dict diff --git a/omnigibson/tasks/point_navigation_task.py b/omnigibson/tasks/point_navigation_task.py index b2586cee4..786c801de 100644 --- a/omnigibson/tasks/point_navigation_task.py +++ b/omnigibson/tasks/point_navigation_task.py @@ -499,7 +499,7 @@ def default_reward_config(cls): "r_collision": 0.1, "r_pointgoal": 10.0, } - + @classproperty def default_metric_config(cls): # Empty dict diff --git a/omnigibson/tasks/point_reaching_task.py b/omnigibson/tasks/point_reaching_task.py index a694c1688..3e598ff98 100644 --- a/omnigibson/tasks/point_reaching_task.py +++ b/omnigibson/tasks/point_reaching_task.py @@ -139,4 +139,4 @@ def _get_obs(self, env): def get_current_pos(self, env): # Current position is the robot's EEF, not base! - return env.robots[self._robot_idn].get_eef_position() \ No newline at end of file + return env.robots[self._robot_idn].get_eef_position() diff --git a/tests/test_dump_load_states.py b/tests/test_dump_load_states.py index 37321f5f6..0976d1d07 100644 --- a/tests/test_dump_load_states.py +++ b/tests/test_dump_load_states.py @@ -46,5 +46,6 @@ def test_dump_load_serialized(env): system = env.scene.get_system(system_name) system.clear() + if __name__ == "__main__": test_dump_load() diff --git a/tests/test_metric_functions.py b/tests/test_metric_functions.py index 94b4ac94b..c5b6e2b5c 100644 --- a/tests/test_metric_functions.py +++ b/tests/test_metric_functions.py @@ -4,12 +4,12 @@ import numpy as np import yaml from bddl.condition_evaluation import evaluate_state -from omnigibson.objects import DatasetObject import omnigibson as og +import omnigibson.utils.transform_utils as T from omnigibson.macros import gm +from omnigibson.objects import DatasetObject from omnigibson.tasks.behavior_task import BehaviorTask -import omnigibson.utils.transform_utils as T # Make sure object states are enabled gm.ENABLE_OBJECT_STATES = True @@ -32,13 +32,15 @@ def setup_env(): env.reset() return env + env = setup_env() + def test_behavior_task_work_metric(): # perform a step with no action action = env.action_space.sample() - action['robot0'] = np.zeros_like(action['robot0']) + action["robot0"] = np.zeros_like(action["robot0"]) state, reward, terminated, truncated, info = env.step(action) @@ -70,11 +72,12 @@ def test_behavior_task_work_metric(): assert np.allclose(metrics["work"] / robot_mass, 0, atol=1e-1) env.reset() + def test_behavior_task_energy_metric(): # perform a step with no action action = env.action_space.sample() - action['robot0'] = np.zeros_like(action['robot0']) + action["robot0"] = np.zeros_like(action["robot0"]) env.step(action) # cache the initial position and orientation of the robot @@ -98,17 +101,16 @@ def test_behavior_task_energy_metric(): for link in env.robots[0].links.values(): robot_mass += link.mass - # assert that the total energy spent is twice as much as the energy spent applying the shift + # assert that the total energy spent is twice as much as the energy spent applying the shift assert np.allclose((2 * energy - new_energy) / robot_mass, 0, atol=1e-3) env.reset() - def test_behavior_task_object_addition_removal(): # perform a step with no action action = env.action_space.sample() - action['robot0'] = np.zeros_like(action['robot0']) + action["robot0"] = np.zeros_like(action["robot0"]) env.step(action) env.robots[0].set_position_orientation([10, 10, 0]) @@ -117,7 +119,6 @@ def test_behavior_task_object_addition_removal(): work, energy = metrics["work"], metrics["energy"] - # add another apple to the environment apple = DatasetObject( name="apple_unique", @@ -145,9 +146,8 @@ def test_behavior_task_object_addition_removal(): metrics = info["metrics"] remove_work, remove_energy = metrics["work"], metrics["energy"] - + assert np.allclose((add_work - remove_work) / robot_mass, 0, atol=1e-1) assert np.allclose((add_energy - remove_energy) / robot_mass, 0, atol=1e-1) env.reset() - From c7eed0ec07aaf53fb64f3fbe889f7aabee289005 Mon Sep 17 00:00:00 2001 From: Frank Yang Date: Tue, 13 Aug 2024 13:27:27 -0700 Subject: [PATCH 22/28] refactoring --- omnigibson/envs/env_base.py | 5 ----- omnigibson/metrics/work_energy_metric.py | 5 ----- omnigibson/objects/controllable_object.py | 5 ----- omnigibson/tasks/behavior_task.py | 7 ------- 4 files changed, 22 deletions(-) diff --git a/omnigibson/envs/env_base.py b/omnigibson/envs/env_base.py index d4ea467ea..923bd740a 100644 --- a/omnigibson/envs/env_base.py +++ b/omnigibson/envs/env_base.py @@ -533,13 +533,8 @@ def _pre_step(self, action): """Apply the pre-sim-step part of an environment step, i.e. apply the robot actions.""" # record the start time of the simulation step in the beginning of the step -<<<<<<< HEAD self._cur_sim_start_ts = og.sim.current_time -======= - self._cur_sim_start_ts = time.perf_counter() - ->>>>>>> b6fc0eac3180abd5cd47cc1c0b9b9928e50492ca # If the action is not a dictionary, convert into a dictionary if not isinstance(action, dict) and not isinstance(action, gym.spaces.Dict): action_dict = dict() diff --git a/omnigibson/metrics/work_energy_metric.py b/omnigibson/metrics/work_energy_metric.py index 7cbb36de4..df0a2d863 100644 --- a/omnigibson/metrics/work_energy_metric.py +++ b/omnigibson/metrics/work_energy_metric.py @@ -1,12 +1,7 @@ import numpy as np from omnigibson.metrics.metrics_base import BaseMetric -<<<<<<< HEAD import omnigibson.utils.transform_utils as T -======= - - ->>>>>>> b6fc0eac3180abd5cd47cc1c0b9b9928e50492ca class WorkEnergyMetric(BaseMetric): """ Work and Energy Metric diff --git a/omnigibson/objects/controllable_object.py b/omnigibson/objects/controllable_object.py index b023c8b7f..e449f86d0 100644 --- a/omnigibson/objects/controllable_object.py +++ b/omnigibson/objects/controllable_object.py @@ -602,14 +602,9 @@ def get_control_dict(self): fcns["mass_matrix"] = lambda: ControllableObjectViewAPI.get_mass_matrix(self.articulation_root_path) fcns["gravity_force"] = lambda: ( ControllableObjectViewAPI.get_generalized_gravity_forces(self.articulation_root_path) -<<<<<<< HEAD # check if dummy is None to account for the fact that the dummy may not be loaded yet if self.fixed_base or self._dummy is None -======= - if self.fixed_base - # or self._dummy is None ->>>>>>> b6fc0eac3180abd5cd47cc1c0b9b9928e50492ca else ControllableObjectViewAPI.get_generalized_gravity_forces(self._dummy.articulation_root_path) ) fcns["cc_force"] = lambda: ControllableObjectViewAPI.get_coriolis_and_centrifugal_forces( diff --git a/omnigibson/tasks/behavior_task.py b/omnigibson/tasks/behavior_task.py index 4ca667b41..9bbbe0de1 100644 --- a/omnigibson/tasks/behavior_task.py +++ b/omnigibson/tasks/behavior_task.py @@ -397,14 +397,7 @@ def _get_obs(self, env): # Batch rpy calculations for much better efficiency objs_exist = { -<<<<<<< HEAD obj: obj.exists for obj in self.object_scope.values() if not obj.is_system -======= - obj: obj.exists - for obj in self.object_scope.values() - if not obj.is_system - # and obj.states[Pose]._initialized ->>>>>>> b6fc0eac3180abd5cd47cc1c0b9b9928e50492ca } objs_rpy = T.quat2euler( np.array( From 88c2105583e2cd9f52bbc5a69a5ade771aa7563e Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Tue, 13 Aug 2024 20:28:32 +0000 Subject: [PATCH 23/28] [pre-commit.ci] auto fixes from pre-commit.com hooks for more information, see https://pre-commit.ci --- omnigibson/envs/env_base.py | 4 ++-- omnigibson/metrics/work_energy_metric.py | 10 +++++++--- omnigibson/objects/controllable_object.py | 5 ++--- omnigibson/tasks/behavior_task.py | 6 ++---- 4 files changed, 13 insertions(+), 12 deletions(-) diff --git a/omnigibson/envs/env_base.py b/omnigibson/envs/env_base.py index 923bd740a..877baad86 100644 --- a/omnigibson/envs/env_base.py +++ b/omnigibson/envs/env_base.py @@ -534,7 +534,7 @@ def _pre_step(self, action): # record the start time of the simulation step in the beginning of the step self._cur_sim_start_ts = og.sim.current_time - + # If the action is not a dictionary, convert into a dictionary if not isinstance(action, dict) and not isinstance(action, gym.spaces.Dict): action_dict = dict() @@ -713,7 +713,7 @@ def last_step_wall_time(self): # return 0 if the simulation has not started yet if not self._prev_sim_end_ts or not self._cur_sim_start_ts: return 0 - + assert ( self._prev_sim_end_ts <= self._cur_sim_start_ts ), "end time from the previous iteration must be less than the start time of the current iteration" diff --git a/omnigibson/metrics/work_energy_metric.py b/omnigibson/metrics/work_energy_metric.py index e7d6c9d0f..3341aeb10 100644 --- a/omnigibson/metrics/work_energy_metric.py +++ b/omnigibson/metrics/work_energy_metric.py @@ -1,7 +1,7 @@ import numpy as np -from omnigibson.metrics.metrics_base import BaseMetric import omnigibson.utils.transform_utils as T +from omnigibson.metrics.metrics_base import BaseMetric class WorkEnergyMetric(BaseMetric): @@ -66,10 +66,14 @@ def _step(self, task, env, action): if self.prev_state_cache is not None: prev_posrot = self.prev_state_cache[linkname] position, orientation = T.relative_pose_transform(posrot[0], posrot[1], prev_posrot[0], prev_posrot[1]) - energy_metric += np.linalg.norm(position) * self.link_masses[linkname] * self.metric_config["translation"] + energy_metric += ( + np.linalg.norm(position) * self.link_masses[linkname] * self.metric_config["translation"] + ) # calculate the energy spent in rotation. TODO: this is a very rough approximation, consider using a more accurate method - energy_metric += np.linalg.norm(orientation) * self.link_masses[linkname] * self.metric_config["rotation"] + energy_metric += ( + np.linalg.norm(orientation) * self.link_masses[linkname] * self.metric_config["rotation"] + ) # update the prev_state cache for energy measurement self.prev_state_cache = new_state_cache diff --git a/omnigibson/objects/controllable_object.py b/omnigibson/objects/controllable_object.py index e449f86d0..f315e3887 100644 --- a/omnigibson/objects/controllable_object.py +++ b/omnigibson/objects/controllable_object.py @@ -194,10 +194,10 @@ def load(self, scene): ), "Stored control frequency does not match environment's render timestep." return prim - + def _load(self): # Run super first - + prim = super()._load() # Also import dummy object if this robot is not fixed base AND it has a controller that @@ -602,7 +602,6 @@ def get_control_dict(self): fcns["mass_matrix"] = lambda: ControllableObjectViewAPI.get_mass_matrix(self.articulation_root_path) fcns["gravity_force"] = lambda: ( ControllableObjectViewAPI.get_generalized_gravity_forces(self.articulation_root_path) - # check if dummy is None to account for the fact that the dummy may not be loaded yet if self.fixed_base or self._dummy is None else ControllableObjectViewAPI.get_generalized_gravity_forces(self._dummy.articulation_root_path) diff --git a/omnigibson/tasks/behavior_task.py b/omnigibson/tasks/behavior_task.py index 9bbbe0de1..df54f1b66 100644 --- a/omnigibson/tasks/behavior_task.py +++ b/omnigibson/tasks/behavior_task.py @@ -396,9 +396,7 @@ def _get_obs(self, env): low_dim_obs = dict() # Batch rpy calculations for much better efficiency - objs_exist = { - obj: obj.exists for obj in self.object_scope.values() if not obj.is_system - } + objs_exist = {obj: obj.exists for obj in self.object_scope.values() if not obj.is_system} objs_rpy = T.quat2euler( np.array( [ @@ -598,5 +596,5 @@ def default_metric_config(cls): "work": 0.2, "energy": 0.2, "rotation": 0.1, - "translation": 0.9 + "translation": 0.9, } From a9ef20ee6da224933e9ccebae88be9d63fd991f0 Mon Sep 17 00:00:00 2001 From: Frank Yang Date: Wed, 14 Aug 2024 18:51:09 -0700 Subject: [PATCH 24/28] corrected rotational inertia calculation --- omnigibson/configs/fetch_behavior.yaml | 4 +-- omnigibson/envs/env_base.py | 29 +++------------------- omnigibson/metrics/wall_time_metric.py | 2 +- omnigibson/metrics/work_energy_metric.py | 29 +++++++++++++++------- omnigibson/tasks/behavior_task.py | 4 +-- omnigibson/tasks/task_base.py | 31 ++++++++++++++++++++++++ tests/test_metric_functions.py | 4 +-- 7 files changed, 62 insertions(+), 41 deletions(-) diff --git a/omnigibson/configs/fetch_behavior.yaml b/omnigibson/configs/fetch_behavior.yaml index 35604c0f8..ef1a062ef 100644 --- a/omnigibson/configs/fetch_behavior.yaml +++ b/omnigibson/configs/fetch_behavior.yaml @@ -90,6 +90,6 @@ task: wall_time: 0.05 work: 0.2 energy: 0.2 - rotation: 0.1 - translation: 0.9 + rotation: 1 + translation: 1 diff --git a/omnigibson/envs/env_base.py b/omnigibson/envs/env_base.py index 923bd740a..e381f1760 100644 --- a/omnigibson/envs/env_base.py +++ b/omnigibson/envs/env_base.py @@ -1,4 +1,3 @@ -import time from copy import deepcopy import gymnasium as gym @@ -532,8 +531,8 @@ def _populate_info(self, info): def _pre_step(self, action): """Apply the pre-sim-step part of an environment step, i.e. apply the robot actions.""" - # record the start time of the simulation step in the beginning of the step - self._cur_sim_start_ts = og.sim.current_time + # perform necessary pre-step actions for the task + self.task.pre_step(action) # If the action is not a dictionary, convert into a dictionary if not isinstance(action, dict) and not isinstance(action, gym.spaces.Dict): @@ -586,8 +585,8 @@ def _post_step(self, action): self._current_step += 1 # record end time - # record the end time of the simulation step in the end of the step - self._prev_sim_end_ts = og.sim.current_time + # perform necessary post-step actions for the task + self.task.post_step(action) return obs, reward, terminated, truncated, info @@ -643,10 +642,6 @@ def _reset_variables(self): self._current_episode += 1 self._current_step = 0 - # reset the start and end time of the simulation step - self._prev_sim_end_ts = None - self._cur_sim_start_ts = None - def reset(self, get_obs=True, **kwargs): """ Reset episode. @@ -703,22 +698,6 @@ def reset(self, get_obs=True, **kwargs): return obs, {} - @property - def last_step_wall_time(self): - """ - Returns: - int: return the amount of wall time the last simulation step took - """ - - # return 0 if the simulation has not started yet - if not self._prev_sim_end_ts or not self._cur_sim_start_ts: - return 0 - - assert ( - self._prev_sim_end_ts <= self._cur_sim_start_ts - ), "end time from the previous iteration must be less than the start time of the current iteration" - return self._cur_sim_start_ts - self._prev_sim_end_ts - @property def episode_steps(self): """ diff --git a/omnigibson/metrics/wall_time_metric.py b/omnigibson/metrics/wall_time_metric.py index c27ca947e..a0d926563 100644 --- a/omnigibson/metrics/wall_time_metric.py +++ b/omnigibson/metrics/wall_time_metric.py @@ -12,7 +12,7 @@ def __init__(self): self._metric = 0 def _step(self, task, env, action): - self._metric += env.last_step_wall_time + self._metric += task.last_step_wall_time return {"wall_time": self._metric} def reset(self, task, env): diff --git a/omnigibson/metrics/work_energy_metric.py b/omnigibson/metrics/work_energy_metric.py index e7d6c9d0f..b765e79f6 100644 --- a/omnigibson/metrics/work_energy_metric.py +++ b/omnigibson/metrics/work_energy_metric.py @@ -19,20 +19,27 @@ def __init__(self, metric_config=None): self._energy_metric = 0 self.state_cache = None self.prev_state_cache = None - self.link_masses = {} + + # stores object mass and rotational inertia + self.link_info = {} self.metric_config = metric_config def _step(self, task, env, action): # calculate the current pose of all object links new_state_cache = {} + self.link_info = {} for obj in env.scene.objects: for link_name, link in obj._links.items(): pos, rot = link.get_position_orientation() new_state_cache[link_name] = (pos, rot) # compute this every step to account for object addition and removal - self.link_masses[link_name] = link.mass + # compute link aabb and rototaional inertia, assuming ellipsoid shape and uniform density + aabb = link.aabb[1] - link.aabb[0] + mass = link.mass + inertia = 1 / 5 * mass * np.array([aabb[0] ** 2 + aabb[1] ** 2, aabb[1] ** 2 + aabb[2] ** 2, aabb[2] ** 2 + aabb[0] ** 2]) + self.link_info[link_name] = (mass, inertia) # if the state cache is empty, set it to the current state and return 0 if not self.state_cache: @@ -53,11 +60,13 @@ def _step(self, task, env, action): continue init_posrot = self.state_cache[linkname] + mass, inertia = self.link_info[linkname] position, orientation = T.relative_pose_transform(posrot[0], posrot[1], init_posrot[0], init_posrot[1]) - work_metric += np.linalg.norm(position) * self.link_masses[linkname] * self.metric_config["translation"] - - # calculate the energy spent in rotation. TODO: this is a very rough approximation, consider using a more accurate method - work_metric += np.linalg.norm(orientation) * self.link_masses[linkname] * self.metric_config["rotation"] + orientation = T.quat2axisangle(orientation) + work_metric += np.linalg.norm(position) * mass * self.metric_config["translation"] + + # calculate the energy spent in rotation + work_metric += 0.5 * np.dot(inertia, orientation ** 2) * self.metric_config["rotation"] # check if the link is in the prev_state_cache, if not, skip it to account for object addition and removal if linkname not in self.prev_state_cache: @@ -66,10 +75,11 @@ def _step(self, task, env, action): if self.prev_state_cache is not None: prev_posrot = self.prev_state_cache[linkname] position, orientation = T.relative_pose_transform(posrot[0], posrot[1], prev_posrot[0], prev_posrot[1]) - energy_metric += np.linalg.norm(position) * self.link_masses[linkname] * self.metric_config["translation"] + orientation = T.quat2axisangle(orientation) + energy_metric += np.linalg.norm(position) * mass * self.metric_config["translation"] - # calculate the energy spent in rotation. TODO: this is a very rough approximation, consider using a more accurate method - energy_metric += np.linalg.norm(orientation) * self.link_masses[linkname] * self.metric_config["rotation"] + # calculate the energy spent in rotation + energy_metric += 0.5 * np.dot(inertia, orientation ** 2) * self.metric_config["rotation"] # update the prev_state cache for energy measurement self.prev_state_cache = new_state_cache @@ -86,3 +96,4 @@ def reset(self, task, env): self._energy_metric = 0 self.state_cache = None self.prev_state_cache = None + self.link_info = {} diff --git a/omnigibson/tasks/behavior_task.py b/omnigibson/tasks/behavior_task.py index 9bbbe0de1..e09451349 100644 --- a/omnigibson/tasks/behavior_task.py +++ b/omnigibson/tasks/behavior_task.py @@ -597,6 +597,6 @@ def default_metric_config(cls): "wall_time": 0.05, "work": 0.2, "energy": 0.2, - "rotation": 0.1, - "translation": 0.9 + "rotation": 1, + "translation": 1 } diff --git a/omnigibson/tasks/task_base.py b/omnigibson/tasks/task_base.py index 32e71f149..9f7c1bb1f 100644 --- a/omnigibson/tasks/task_base.py +++ b/omnigibson/tasks/task_base.py @@ -1,6 +1,7 @@ from abc import ABCMeta, abstractmethod from copy import deepcopy +import time import numpy as np from omnigibson.utils.gym_utils import GymObservable @@ -195,6 +196,10 @@ def _reset_variables(self, env): self._success = False self._info = None + # reset the start and end time of the simulation step + self._prev_sim_end_ts = None + self._cur_sim_start_ts = None + def reset(self, env): """ Resets this task in the environment @@ -383,6 +388,15 @@ def step(self, env, action): } return self._reward, self._done, deepcopy(self._info) + + def pre_step(self, action): + + # record the real start time of the simulation step + self._cur_sim_start_ts = time.perf_counter() + + def post_step(self, action): + # record the real end time of the simulation step + self._prev_sim_end_ts = time.perf_counter() @property def name(self): @@ -427,6 +441,23 @@ def info(self): """ assert self._info is not None, "At least one step() must occur before info can be calculated!" return self._info + + @property + def last_step_wall_time(self): + """ + Returns: + int: return the amount of wall time the last simulation step took + """ + + # return 0 if the simulation has not started yet + if not self._prev_sim_end_ts or not self._cur_sim_start_ts: + return 0 + + assert ( + self._prev_sim_end_ts < self._cur_sim_start_ts + ), "end time from the previous iteration must be less than the start time of the current iteration" + return self._cur_sim_start_ts - self._prev_sim_end_ts + @classproperty def valid_scene_types(cls): diff --git a/tests/test_metric_functions.py b/tests/test_metric_functions.py index c5b6e2b5c..a4f6a6bd1 100644 --- a/tests/test_metric_functions.py +++ b/tests/test_metric_functions.py @@ -102,7 +102,7 @@ def test_behavior_task_energy_metric(): robot_mass += link.mass # assert that the total energy spent is twice as much as the energy spent applying the shift - assert np.allclose((2 * energy - new_energy) / robot_mass, 0, atol=1e-3) + assert np.allclose((2 * energy - new_energy) / robot_mass, 0, atol=1e-2) env.reset() @@ -150,4 +150,4 @@ def test_behavior_task_object_addition_removal(): assert np.allclose((add_work - remove_work) / robot_mass, 0, atol=1e-1) assert np.allclose((add_energy - remove_energy) / robot_mass, 0, atol=1e-1) - env.reset() + env.reset() \ No newline at end of file From 9f513142c7bc40b0c95b4b53850023b04df92689 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Thu, 15 Aug 2024 01:52:58 +0000 Subject: [PATCH 25/28] [pre-commit.ci] auto fixes from pre-commit.com hooks for more information, see https://pre-commit.ci --- omnigibson/envs/env_base.py | 2 +- omnigibson/metrics/work_energy_metric.py | 13 +++++++++---- omnigibson/tasks/behavior_task.py | 2 +- omnigibson/tasks/task_base.py | 9 ++++----- tests/test_metric_functions.py | 2 +- 5 files changed, 16 insertions(+), 12 deletions(-) diff --git a/omnigibson/envs/env_base.py b/omnigibson/envs/env_base.py index e381f1760..a143b55dc 100644 --- a/omnigibson/envs/env_base.py +++ b/omnigibson/envs/env_base.py @@ -533,7 +533,7 @@ def _pre_step(self, action): # perform necessary pre-step actions for the task self.task.pre_step(action) - + # If the action is not a dictionary, convert into a dictionary if not isinstance(action, dict) and not isinstance(action, gym.spaces.Dict): action_dict = dict() diff --git a/omnigibson/metrics/work_energy_metric.py b/omnigibson/metrics/work_energy_metric.py index 5eeb30979..6ff3f7a9f 100644 --- a/omnigibson/metrics/work_energy_metric.py +++ b/omnigibson/metrics/work_energy_metric.py @@ -38,7 +38,12 @@ def _step(self, task, env, action): # compute link aabb and rototaional inertia, assuming ellipsoid shape and uniform density aabb = link.aabb[1] - link.aabb[0] mass = link.mass - inertia = 1 / 5 * mass * np.array([aabb[0] ** 2 + aabb[1] ** 2, aabb[1] ** 2 + aabb[2] ** 2, aabb[2] ** 2 + aabb[0] ** 2]) + inertia = ( + 1 + / 5 + * mass + * np.array([aabb[0] ** 2 + aabb[1] ** 2, aabb[1] ** 2 + aabb[2] ** 2, aabb[2] ** 2 + aabb[0] ** 2]) + ) self.link_info[link_name] = (mass, inertia) # if the state cache is empty, set it to the current state and return 0 @@ -64,9 +69,9 @@ def _step(self, task, env, action): position, orientation = T.relative_pose_transform(posrot[0], posrot[1], init_posrot[0], init_posrot[1]) orientation = T.quat2axisangle(orientation) work_metric += np.linalg.norm(position) * mass * self.metric_config["translation"] - + # calculate the energy spent in rotation - work_metric += 0.5 * np.dot(inertia, orientation ** 2) * self.metric_config["rotation"] + work_metric += 0.5 * np.dot(inertia, orientation**2) * self.metric_config["rotation"] # check if the link is in the prev_state_cache, if not, skip it to account for object addition and removal if linkname not in self.prev_state_cache: @@ -79,7 +84,7 @@ def _step(self, task, env, action): energy_metric += np.linalg.norm(position) * mass * self.metric_config["translation"] # calculate the energy spent in rotation - energy_metric += 0.5 * np.dot(inertia, orientation ** 2) * self.metric_config["rotation"] + energy_metric += 0.5 * np.dot(inertia, orientation**2) * self.metric_config["rotation"] # update the prev_state cache for energy measurement self.prev_state_cache = new_state_cache diff --git a/omnigibson/tasks/behavior_task.py b/omnigibson/tasks/behavior_task.py index 7380a6883..547078474 100644 --- a/omnigibson/tasks/behavior_task.py +++ b/omnigibson/tasks/behavior_task.py @@ -596,5 +596,5 @@ def default_metric_config(cls): "work": 0.2, "energy": 0.2, "rotation": 1, - "translation": 1 + "translation": 1, } diff --git a/omnigibson/tasks/task_base.py b/omnigibson/tasks/task_base.py index 9f7c1bb1f..4885d4409 100644 --- a/omnigibson/tasks/task_base.py +++ b/omnigibson/tasks/task_base.py @@ -1,7 +1,7 @@ +import time from abc import ABCMeta, abstractmethod from copy import deepcopy -import time import numpy as np from omnigibson.utils.gym_utils import GymObservable @@ -388,7 +388,7 @@ def step(self, env, action): } return self._reward, self._done, deepcopy(self._info) - + def pre_step(self, action): # record the real start time of the simulation step @@ -441,7 +441,7 @@ def info(self): """ assert self._info is not None, "At least one step() must occur before info can be calculated!" return self._info - + @property def last_step_wall_time(self): """ @@ -452,13 +452,12 @@ def last_step_wall_time(self): # return 0 if the simulation has not started yet if not self._prev_sim_end_ts or not self._cur_sim_start_ts: return 0 - + assert ( self._prev_sim_end_ts < self._cur_sim_start_ts ), "end time from the previous iteration must be less than the start time of the current iteration" return self._cur_sim_start_ts - self._prev_sim_end_ts - @classproperty def valid_scene_types(cls): """ diff --git a/tests/test_metric_functions.py b/tests/test_metric_functions.py index a4f6a6bd1..775119c5d 100644 --- a/tests/test_metric_functions.py +++ b/tests/test_metric_functions.py @@ -150,4 +150,4 @@ def test_behavior_task_object_addition_removal(): assert np.allclose((add_work - remove_work) / robot_mass, 0, atol=1e-1) assert np.allclose((add_energy - remove_energy) / robot_mass, 0, atol=1e-1) - env.reset() \ No newline at end of file + env.reset() From 85dabe907dffe525a8df888aba82cd604388b029 Mon Sep 17 00:00:00 2001 From: Frank Yang Date: Fri, 16 Aug 2024 16:28:41 -0700 Subject: [PATCH 26/28] metric work energy finished --- omnigibson/envs/env_base.py | 4 +- omnigibson/metrics/work_energy_metric.py | 52 +++++++++++++++--------- omnigibson/prims/rigid_prim.py | 15 +++++++ omnigibson/tasks/task_base.py | 4 +- tests/test_metric_functions.py | 5 ++- 5 files changed, 56 insertions(+), 24 deletions(-) diff --git a/omnigibson/envs/env_base.py b/omnigibson/envs/env_base.py index e381f1760..8c7dfdeab 100644 --- a/omnigibson/envs/env_base.py +++ b/omnigibson/envs/env_base.py @@ -532,7 +532,7 @@ def _pre_step(self, action): """Apply the pre-sim-step part of an environment step, i.e. apply the robot actions.""" # perform necessary pre-step actions for the task - self.task.pre_step(action) + self.task.pre_step() # If the action is not a dictionary, convert into a dictionary if not isinstance(action, dict) and not isinstance(action, gym.spaces.Dict): @@ -586,7 +586,7 @@ def _post_step(self, action): # record end time # perform necessary post-step actions for the task - self.task.post_step(action) + self.task.post_step() return obs, reward, terminated, truncated, info diff --git a/omnigibson/metrics/work_energy_metric.py b/omnigibson/metrics/work_energy_metric.py index 5eeb30979..691f66955 100644 --- a/omnigibson/metrics/work_energy_metric.py +++ b/omnigibson/metrics/work_energy_metric.py @@ -7,9 +7,7 @@ class WorkEnergyMetric(BaseMetric): """ Work and Energy Metric - Measures displacement * mass for every link - """ def __init__(self, metric_config=None): @@ -35,10 +33,9 @@ def _step(self, task, env, action): new_state_cache[link_name] = (pos, rot) # compute this every step to account for object addition and removal - # compute link aabb and rototaional inertia, assuming ellipsoid shape and uniform density - aabb = link.aabb[1] - link.aabb[0] + # store link mass and inertia for energy calculation mass = link.mass - inertia = 1 / 5 * mass * np.array([aabb[0] ** 2 + aabb[1] ** 2, aabb[1] ** 2 + aabb[2] ** 2, aabb[2] ** 2 + aabb[0] ** 2]) + inertia = link.inertia self.link_info[link_name] = (mass, inertia) # if the state cache is empty, set it to the current state and return 0 @@ -51,7 +48,7 @@ def _step(self, task, env, action): work_metric = 0.0 energy_metric = 0.0 - for linkname, posrot in new_state_cache.items(): + for linkname, new_posrot in new_state_cache.items(): # TODO: this computation is very slow, consider using a more efficient method @@ -61,12 +58,7 @@ def _step(self, task, env, action): init_posrot = self.state_cache[linkname] mass, inertia = self.link_info[linkname] - position, orientation = T.relative_pose_transform(posrot[0], posrot[1], init_posrot[0], init_posrot[1]) - orientation = T.quat2axisangle(orientation) - work_metric += np.linalg.norm(position) * mass * self.metric_config["translation"] - - # calculate the energy spent in rotation - work_metric += 0.5 * np.dot(inertia, orientation ** 2) * self.metric_config["rotation"] + work_metric = self.calculate_work_energy(init_posrot, new_posrot, mass, inertia) # check if the link is in the prev_state_cache, if not, skip it to account for object addition and removal if linkname not in self.prev_state_cache: @@ -74,12 +66,7 @@ def _step(self, task, env, action): if self.prev_state_cache is not None: prev_posrot = self.prev_state_cache[linkname] - position, orientation = T.relative_pose_transform(posrot[0], posrot[1], prev_posrot[0], prev_posrot[1]) - orientation = T.quat2axisangle(orientation) - energy_metric += np.linalg.norm(position) * mass * self.metric_config["translation"] - - # calculate the energy spent in rotation - energy_metric += 0.5 * np.dot(inertia, orientation ** 2) * self.metric_config["rotation"] + energy_metric = self.calculate_work_energy(prev_posrot, new_posrot, mass, inertia) # update the prev_state cache for energy measurement self.prev_state_cache = new_state_cache @@ -88,6 +75,33 @@ def _step(self, task, env, action): self._work_metric = work_metric self._energy_metric += energy_metric return {"work": self._work_metric, "energy": self._energy_metric} + + def calculate_work_energy(self, new_posrot, curr_posrot, mass, inertia): + + """ + Calculate the work done and energy spent in a single step + + Args: + new_posrot (tuple): new position and orientation of the link + curr_posrot (tuple): current position and orientation of the link. + use initial posrot for work metric, and previous posrot for energy metric + mass (float): mass of the link + inertia (np.array): rotational inertia of the link + + Returns: + float: work/energy spent in a single step between two states + """ + + position, orientation = T.relative_pose_transform(new_posrot[0], new_posrot[1], curr_posrot[0], curr_posrot[1]) + orientation = T.quat2axisangle(orientation) + + # formula for translational work metric: displacement * mass * translation weight coefficient + work_metric = np.linalg.norm(position) * mass * self.metric_config["translation"] + + # formula for rotational work metric: rotation * moment of inertia * rotation weight coefficient + work_metric += np.dot(orientation, inertia) * self.metric_config["rotation"] + + return work_metric def reset(self, task, env): @@ -96,4 +110,4 @@ def reset(self, task, env): self._energy_metric = 0 self.state_cache = None self.prev_state_cache = None - self.link_info = {} + self.link_info = {} \ No newline at end of file diff --git a/omnigibson/prims/rigid_prim.py b/omnigibson/prims/rigid_prim.py index a698bfcd4..820ea1f81 100644 --- a/omnigibson/prims/rigid_prim.py +++ b/omnigibson/prims/rigid_prim.py @@ -470,6 +470,21 @@ def mass(self): return self.volume * self.density return mass + + @property + def inertia(self): + """ + Returns: + np.ndarray: inertia tensor of the rigid body in kg m^2. + """ + + inertia = self._rigid_prim_view.get_inertias() + + # if no inertia is set, return a zero array + if inertia is None: + return np.zeros((3, 3)) + + return inertia.reshape(3, 3) @mass.setter def mass(self, mass): diff --git a/omnigibson/tasks/task_base.py b/omnigibson/tasks/task_base.py index 9f7c1bb1f..5e0080c82 100644 --- a/omnigibson/tasks/task_base.py +++ b/omnigibson/tasks/task_base.py @@ -389,12 +389,12 @@ def step(self, env, action): return self._reward, self._done, deepcopy(self._info) - def pre_step(self, action): + def pre_step(self): # record the real start time of the simulation step self._cur_sim_start_ts = time.perf_counter() - def post_step(self, action): + def post_step(self): # record the real end time of the simulation step self._prev_sim_end_ts = time.perf_counter() diff --git a/tests/test_metric_functions.py b/tests/test_metric_functions.py index a4f6a6bd1..8c99f80a4 100644 --- a/tests/test_metric_functions.py +++ b/tests/test_metric_functions.py @@ -150,4 +150,7 @@ def test_behavior_task_object_addition_removal(): assert np.allclose((add_work - remove_work) / robot_mass, 0, atol=1e-1) assert np.allclose((add_energy - remove_energy) / robot_mass, 0, atol=1e-1) - env.reset() \ No newline at end of file + env.reset() + +if __name__ == "__main__": + test_behavior_task_work_metric() \ No newline at end of file From 539da44a8cccaef394bfaf3acfa5dae0513ac892 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Tue, 10 Sep 2024 01:58:13 +0000 Subject: [PATCH 27/28] [pre-commit.ci] auto fixes from pre-commit.com hooks for more information, see https://pre-commit.ci --- omnigibson/envs/env_base.py | 2 +- omnigibson/metrics/work_energy_metric.py | 11 +++++------ omnigibson/prims/rigid_prim.py | 6 +++--- omnigibson/simulator.py | 2 +- omnigibson/tasks/task_base.py | 2 +- tests/test_metric_functions.py | 8 +++++--- 6 files changed, 16 insertions(+), 15 deletions(-) diff --git a/omnigibson/envs/env_base.py b/omnigibson/envs/env_base.py index a1c58004b..ace99b0ac 100644 --- a/omnigibson/envs/env_base.py +++ b/omnigibson/envs/env_base.py @@ -541,7 +541,7 @@ def _pre_step(self, action): # perform necessary pre-step actions for the task self.task.pre_step() - + # If the action is not a dictionary, convert into a dictionary if not isinstance(action, dict) and not isinstance(action, gym.spaces.Dict): action_dict = dict() diff --git a/omnigibson/metrics/work_energy_metric.py b/omnigibson/metrics/work_energy_metric.py index be0429e2c..69a78c57c 100644 --- a/omnigibson/metrics/work_energy_metric.py +++ b/omnigibson/metrics/work_energy_metric.py @@ -75,19 +75,18 @@ def _step(self, task, env, action): self._work_metric = work_metric self._energy_metric += energy_metric return {"work": self._work_metric, "energy": self._energy_metric} - - def calculate_work_energy(self, new_posrot, curr_posrot, mass, inertia): + def calculate_work_energy(self, new_posrot, curr_posrot, mass, inertia): """ Calculate the work done and energy spent in a single step Args: - new_posrot (tuple): new position and orientation of the link - curr_posrot (tuple): current position and orientation of the link. + new_posrot (tuple): new position and orientation of the link + curr_posrot (tuple): current position and orientation of the link. use initial posrot for work metric, and previous posrot for energy metric mass (float): mass of the link inertia (th.Tensor): rotational inertia of the link - + Returns: float: work/energy spent in a single step between two states """ @@ -110,4 +109,4 @@ def reset(self, task, env): self._energy_metric = 0 self.state_cache = None self.prev_state_cache = None - self.link_info = {} \ No newline at end of file + self.link_info = {} diff --git a/omnigibson/prims/rigid_prim.py b/omnigibson/prims/rigid_prim.py index 372964aa9..301bdbb10 100644 --- a/omnigibson/prims/rigid_prim.py +++ b/omnigibson/prims/rigid_prim.py @@ -489,20 +489,20 @@ def mass(self): return self.volume * self.density return mass - + @property def inertia(self): """ Returns: np.ndarray: inertia tensor of the rigid body in kg m^2. """ - + inertia = self._rigid_prim_view.get_inertias() # if no inertia is set, return a zero array if inertia is None: return th.zeros((3, 3)) - + return inertia.reshape(3, 3) @mass.setter diff --git a/omnigibson/simulator.py b/omnigibson/simulator.py index 0f807aa34..71a7d0cad 100644 --- a/omnigibson/simulator.py +++ b/omnigibson/simulator.py @@ -279,7 +279,7 @@ def __init__( ), "Stage should not exist when creating a new Simulator instance" # Here we assign self as the Simulator instance and as og.sim, because certain functions - # called downstream during the initialization of this object will try to access og.sim. + # called downstream during the initialization of this object will try to access og.sim. # This makes that possible (and also introduces possible issues around circular dependencies) assert og.sim is None, "Only one Simulator instance can be created at a time!" og.sim = self diff --git a/omnigibson/tasks/task_base.py b/omnigibson/tasks/task_base.py index 505d01436..880b50983 100644 --- a/omnigibson/tasks/task_base.py +++ b/omnigibson/tasks/task_base.py @@ -423,7 +423,7 @@ def step(self, env, action): } return self._reward, self._done, deepcopy(self._info) - + def pre_step(self): # record the real start time of the simulation step diff --git a/tests/test_metric_functions.py b/tests/test_metric_functions.py index d8e963e15..df57da199 100644 --- a/tests/test_metric_functions.py +++ b/tests/test_metric_functions.py @@ -1,9 +1,9 @@ +import math import os from typing import OrderedDict -import math -import yaml import torch as th +import yaml from bddl.condition_evaluation import evaluate_state import omnigibson as og @@ -85,7 +85,9 @@ def test_behavior_task_energy_metric(): position, orientation = env.robots[0].get_position_orientation() # apply shift to the robot - shift_position, shift_orientation = th.tensor([10, 10, 0], dtype=th.float32), th.tensor([0.05, 0, 0, 0.1], dtype=th.float32) + shift_position, shift_orientation = th.tensor([10, 10, 0], dtype=th.float32), th.tensor( + [0.05, 0, 0, 0.1], dtype=th.float32 + ) env.robots[0].set_position_orientation(shift_position, shift_orientation) state, reward, terminated, truncated, info = env.step(action) energy = info["metrics"]["energy"] From 1446b9b7e71daa4e8b4e090e01ed790dc48eb59c Mon Sep 17 00:00:00 2001 From: Frank Yang Date: Tue, 10 Sep 2024 14:08:26 -0700 Subject: [PATCH 28/28] styling fix --- omnigibson/envs/env_base.py | 2 -- 1 file changed, 2 deletions(-) diff --git a/omnigibson/envs/env_base.py b/omnigibson/envs/env_base.py index a1c58004b..e703c7c6e 100644 --- a/omnigibson/envs/env_base.py +++ b/omnigibson/envs/env_base.py @@ -569,7 +569,6 @@ def _post_step(self, action): # Grab reward, done, and info, and populate with internal info reward, done, info = self.task.step(self, action) - self._populate_info(info) info["obs_info"] = obs_info @@ -656,7 +655,6 @@ def _reset_variables(self): """ Reset bookkeeping variables for the next new episode. """ - self._current_episode += 1 self._current_step = 0