ran checks

Learning-and-Intelligent-Systems · Nov 2, 2023 · 177ea60 · 177ea60
1 parent 2eaa29e
commit 177ea60
Show file tree

Hide file tree

Showing 5 changed files with 39 additions and 36 deletions.
diff --git a/predicators/behavior_utils/behavior_utils.py b/predicators/behavior_utils/behavior_utils.py
@@ -855,6 +855,7 @@ def get_scene_body_ids(
 
     return ids
 
+
 def get_relevant_scene_body_ids(
     env: "BehaviorEnv",
     include_self: bool = False,
@@ -1110,7 +1111,7 @@ def sample_navigation_params(igibson_behavior_env: "BehaviorEnv",
     Implemented in a separate method to enable code reuse in
     option_model_fns.
     """
-    closeness_limit = 1.00 #2.00
+    closeness_limit = 1.00  #2.00
     nearness_limit = 0.15
     distance = nearness_limit + (
         (closeness_limit - nearness_limit) * rng.random())

diff --git a/predicators/behavior_utils/motion_planner_fns.py b/predicators/behavior_utils/motion_planner_fns.py
@@ -128,7 +128,8 @@ def sample_fn(env: "BehaviorEnv",
         obstacles = get_relevant_scene_body_ids(env)
         if env.robots[0].parts["right_hand"].object_in_hand is not None:
             if env.robots[0].parts["right_hand"].object_in_hand in obstacles:
-                obstacles.remove(env.robots[0].parts["right_hand"].object_in_hand)
+                obstacles.remove(
+                    env.robots[0].parts["right_hand"].object_in_hand)
         plan = plan_base_motion_br(
             robot=env.robots[0],
             end_conf=end_conf,
@@ -287,16 +288,17 @@ def make_grasp_plan(
             end_conf=end_conf,
             hand_limits=((minx, miny, minz), (maxx, maxy, maxz)),
             obstacles=get_relevant_scene_body_ids(env,
-                                         include_self=True,
-                                         include_right_hand=True),
+                                                  include_self=True,
+                                                  include_right_hand=True),
             rng=rng,
         )
         p.restoreState(state)
     else:
         pos = env.robots[0].parts["right_hand"].get_position()
         plan = [[pos[0], pos[1], pos[2]] + list(
             p.getEulerFromQuaternion(
-                env.robots[0].parts["right_hand"].get_orientation())), end_conf]
+                env.robots[0].parts["right_hand"].get_orientation())),
+                end_conf]
 
     # NOTE: This below line is *VERY* important after the
     # pybullet state is restored. The hands keep an internal
@@ -457,7 +459,8 @@ def make_place_plan(
         pos = env.robots[0].parts["right_hand"].get_position()
         plan = [[pos[0], pos[1], pos[2]] + list(
             p.getEulerFromQuaternion(
-                env.robots[0].parts["right_hand"].get_orientation())), end_conf]
+                env.robots[0].parts["right_hand"].get_orientation())),
+                end_conf]
 
     # NOTE: This below line is *VERY* important after the
     # pybullet state is restored. The hands keep an internal

diff --git a/predicators/behavior_utils/option_model_fns.py b/predicators/behavior_utils/option_model_fns.py
@@ -121,8 +121,7 @@ def graspObjectOptionModel(_state: State, env: "BehaviorEnv") -> None:
         # Simulate Arm Movement
         for step in plan:
             env.robots[0].parts["right_hand"].set_position_orientation(
-                step[0:3],
-                p.getQuaternionFromEuler(step[3:6]))
+                step[0:3], p.getQuaternionFromEuler(step[3:6]))
             for _ in range(1):
                 env.step(np.zeros(env.action_space.shape))
 
@@ -161,8 +160,7 @@ def graspObjectOptionModel(_state: State, env: "BehaviorEnv") -> None:
         # Simulate Arm Movement (Backwards)
         for step in reversed(plan):
             env.robots[0].parts["right_hand"].set_position_orientation(
-                step[0:3],
-                p.getQuaternionFromEuler(step[3:6]))
+                step[0:3], p.getQuaternionFromEuler(step[3:6]))
             for _ in range(1):
                 env.step(np.zeros(env.action_space.shape))
 
@@ -220,8 +218,7 @@ def placeOntopObjectOptionModel(_init_state: State,
         # Simulate Arm Movement
         for step in plan:
             env.robots[0].parts["right_hand"].set_position_orientation(
-                step[0:3],
-                p.getQuaternionFromEuler(step[3:6]))
+                step[0:3], p.getQuaternionFromEuler(step[3:6]))
             for _ in range(1):
                 env.step(np.zeros(env.action_space.shape))
 
@@ -244,8 +241,7 @@ def placeOntopObjectOptionModel(_init_state: State,
         # Simulate Arm Movement (Backwards)
         for step in reversed(plan):
             env.robots[0].parts["right_hand"].set_position_orientation(
-                step[0:3],
-                p.getQuaternionFromEuler(step[3:6]))
+                step[0:3], p.getQuaternionFromEuler(step[3:6]))
             for _ in range(1):
                 env.step(np.zeros(env.action_space.shape))
 
@@ -419,9 +415,9 @@ def placeInsideObjectOptionModel(_init_state: State,
 
                     # Simulate Arm Movement
                     for step in plan:
-                        env.robots[0].parts["right_hand"].set_position_orientation(
-                            step[0:3],
-                            p.getQuaternionFromEuler(step[3:6]))
+                        env.robots[0].parts[
+                            "right_hand"].set_position_orientation(
+                                step[0:3], p.getQuaternionFromEuler(step[3:6]))
                         for _ in range(1):
                             env.step(np.zeros(env.action_space.shape))
 
@@ -441,9 +437,9 @@ def placeInsideObjectOptionModel(_init_state: State,
                     )
                     # Simulate Arm Movement (Backwards)
                     for step in reversed(plan):
-                        env.robots[0].parts["right_hand"].set_position_orientation(
-                            step[0:3],
-                            p.getQuaternionFromEuler(step[3:6]))
+                        env.robots[0].parts[
+                            "right_hand"].set_position_orientation(
+                                step[0:3], p.getQuaternionFromEuler(step[3:6]))
                         for _ in range(1):
                             env.step(np.zeros(env.action_space.shape))
 
@@ -529,9 +525,9 @@ def placeUnderObjectOptionModel(_init_state: State,
 
                     # Simulate Arm Movement
                     for step in plan:
-                        env.robots[0].parts["right_hand"].set_position_orientation(
-                            step[0:3],
-                            p.getQuaternionFromEuler(step[3:6]))
+                        env.robots[0].parts[
+                            "right_hand"].set_position_orientation(
+                                step[0:3], p.getQuaternionFromEuler(step[3:6]))
                         for _ in range(1):
                             env.step(np.zeros(env.action_space.shape))
 
@@ -552,12 +548,12 @@ def placeUnderObjectOptionModel(_init_state: State,
 
                     # Simulate Arm Movement (Backwards)
                     for step in reversed(plan):
-                        env.robots[0].parts["right_hand"].set_position_orientation(
-                            step[0:3],
-                            p.getQuaternionFromEuler(step[3:6]))
+                        env.robots[0].parts[
+                            "right_hand"].set_position_orientation(
+                                step[0:3], p.getQuaternionFromEuler(step[3:6]))
                         for _ in range(1):
                             env.step(np.zeros(env.action_space.shape))
-                            
+
                     env.robots[0].parts["right_hand"].set_position_orientation(
                         rh_orig_grasp_position, rh_orig_grasp_orn)
                     # this is running a series of zero action to step
@@ -670,9 +666,9 @@ def placeNextToObjectOptionModel(_init_state: State,
 
                     # Simulate Arm Movement
                     for step in plan:
-                        env.robots[0].parts["right_hand"].set_position_orientation(
-                            step[0:3],
-                            p.getQuaternionFromEuler(step[3:6]))
+                        env.robots[0].parts[
+                            "right_hand"].set_position_orientation(
+                                step[0:3], p.getQuaternionFromEuler(step[3:6]))
                         for _ in range(1):
                             env.step(np.zeros(env.action_space.shape))
 
@@ -693,9 +689,9 @@ def placeNextToObjectOptionModel(_init_state: State,
 
                     # Simulate Arm Movement (Backwards)
                     for step in reversed(plan):
-                        env.robots[0].parts["right_hand"].set_position_orientation(
-                            step[0:3],
-                            p.getQuaternionFromEuler(step[3:6]))
+                        env.robots[0].parts[
+                            "right_hand"].set_position_orientation(
+                                step[0:3], p.getQuaternionFromEuler(step[3:6]))
                         for _ in range(1):
                             env.step(np.zeros(env.action_space.shape))
 

diff --git a/predicators/main.py b/predicators/main.py
@@ -35,7 +35,6 @@
 """
 
 import curses
-import numpy as np
 import logging
 import os
 import sys
@@ -45,6 +44,7 @@
 from typing import List, Optional, Sequence, Tuple
 
 import dill as pkl
+import numpy as np
 
 from predicators import utils
 from predicators.approaches import ApproachFailure, ApproachTimeout, \
@@ -355,7 +355,8 @@ def _run_testing(env: BaseEnv, approach: BaseApproach) -> Metrics:
                         have finished positioning: ")
                     flag = win.getch()
                     while flag == -1 or chr(flag) != 'q':
-                        env.igibson_behavior_env.step(np.zeros(env.action_space.shape))
+                        env.igibson_behavior_env.step(
+                            np.zeros(env.action_space.shape))
                         flag = win.getch()
                     curses.endwin()
                     logging.info("VIDEO CREATION MODE: Starting planning.")

diff --git a/predicators/planning.py b/predicators/planning.py
@@ -1012,7 +1012,9 @@ def _sesame_plan_with_fast_downward(
     potentially add effects to null operators, but this ability is not
     implemented here.
     """
-    init_atoms = utils.abstract(task.init, predicates, skip_allclose_check=True)
+    init_atoms = utils.abstract(task.init,
+                                predicates,
+                                skip_allclose_check=True)
     objects = list(task.init)
     start_time = time.perf_counter()
     # Create the domain and problem strings, then write them to tempfiles.