def init_common(self):
self._cfg = make_cfg(self._sim_settings)
scene_file = self._sim_settings["scene"]
if (not os.path.exists(scene_file)
and scene_file == default_sim_settings["scene"]):
print(
"Test scenes not downloaded locally, downloading and extracting now..."
)
download_and_unzip(default_sim_settings["test_scene_data_url"],
".")
print("Downloaded and extracted test scenes data.")
# create a simulator (Simulator python class object, not the backend simulator)
self._sim = habitat_sim.Simulator(self._cfg)
random.seed(self._sim_settings["seed"])
self._sim.seed(self._sim_settings["seed"])
recompute_navmesh = self._sim_settings.get("recompute_navmesh")
if recompute_navmesh or not self._sim.pathfinder.is_loaded:
if not self._sim_settings["silent"]:
print("Recomputing navmesh")
navmesh_settings = habitat_sim.NavMeshSettings()
navmesh_settings.set_defaults()
self._sim.recompute_navmesh(self._sim.pathfinder, navmesh_settings)
# initialize the agent at a random start state
return self.init_agent_state(self._sim_settings["default_agent"])
def test_navmesh_area(test_scene):
if not osp.exists(test_scene):
pytest.skip(f"{test_scene} not found")
cfg_settings = examples.settings.default_sim_settings.copy()
cfg_settings["scene"] = test_scene
hab_cfg = examples.settings.make_cfg(cfg_settings)
with habitat_sim.Simulator(hab_cfg) as sim:
# get the initial navmesh area. This test assumes default navmesh assets.
loadedNavMeshArea = sim.pathfinder.navigable_area
if test_scene.endswith("skokloster-castle.glb"):
assert math.isclose(loadedNavMeshArea, 226.65673828125)
elif test_scene.endswith("van-gogh-room.glb"):
assert math.isclose(loadedNavMeshArea, 9.17772102355957)
navmesh_settings = habitat_sim.NavMeshSettings()
navmesh_settings.set_defaults()
assert sim.recompute_navmesh(sim.pathfinder, navmesh_settings)
assert sim.pathfinder.is_loaded
# get the re-computed navmesh area. This test assumes NavMeshSettings default values.
recomputedNavMeshArea1 = sim.pathfinder.navigable_area
if test_scene.endswith("skokloster-castle.glb"):
assert math.isclose(recomputedNavMeshArea1, 565.1781616210938)
elif test_scene.endswith("van-gogh-room.glb"):
assert math.isclose(recomputedNavMeshArea1, 9.17772102355957)
def test_recompute_navmesh(test_scene, sim):
if not osp.exists(test_scene):
pytest.skip(f"{test_scene} not found")
cfg_settings = examples.settings.default_sim_settings.copy()
cfg_settings["scene"] = test_scene
hab_cfg = examples.settings.make_cfg(cfg_settings)
sim.reconfigure(hab_cfg)
# generate random point pairs
num_samples = 100
samples = []
for _ in range(num_samples):
samples.append(
(
sim.pathfinder.get_random_navigable_point(),
sim.pathfinder.get_random_navigable_point(),
)
)
# compute shortest paths between these points on the loaded navmesh
loaded_navmesh_path_results = get_shortest_path(sim, samples)
navmesh_settings = habitat_sim.NavMeshSettings()
navmesh_settings.set_defaults()
assert sim.recompute_navmesh(sim.pathfinder, navmesh_settings)
assert sim.pathfinder.is_loaded
recomputed_navmesh_results = get_shortest_path(sim, samples)
navmesh_settings.agent_radius *= 2.0
assert sim.recompute_navmesh(sim.pathfinder, navmesh_settings)
assert sim.pathfinder.is_loaded # this may not always be viable...
recomputed_2rad_navmesh_results = get_shortest_path(sim, samples)
some_diff = False
for i in range(num_samples):
assert loaded_navmesh_path_results[i][0] == recomputed_navmesh_results[i][0]
if loaded_navmesh_path_results[i][0]:
assert (
loaded_navmesh_path_results[i][1] - recomputed_navmesh_results[i][1]
< EPS
)
if (
loaded_navmesh_path_results[i][0] != recomputed_2rad_navmesh_results[i][0]
or loaded_navmesh_path_results[i][1] - recomputed_2rad_navmesh_results[i][1]
> EPS
):
some_diff = True
assert some_diff
# - **region_merge_size** - Any 2-D regions with a smaller span (cell count) will, if possible, be merged with larger regions. [Limit: >=0]
# - **edge_max_len** - The maximum allowed length for contour edges along the border of the mesh. Extra vertices will be inserted as needed to keep contour edges below this length. A value of zero effectively disables this feature. [Limit: >=0] [ / cell_size]
# - **edge_max_error** - The maximum distance a simplfied contour's border edges should deviate the original raw contour. [Limit: >=0]
# - **verts_per_poly** - The maximum number of vertices allowed for polygons generated during the contour to polygon conversion process.[Limit: >= 3]
# - **detail_sample_dist** - Sets the sampling distance to use when generating the detail mesh. (For height detail only.) [Limits: 0 or >= 0.9] [x cell_size]
# - **detail_sample_max_error** - The maximum distance the detail mesh surface should deviate from heightfield data. (For height detail only.) [Limit: >=0] [x cell_height]
#
#
#
#
# %%
# @markdown ## Recompute NavMesh:
# @markdown Take a moment to edit some parameters and visualize the resulting NavMesh. Consider agent_radius and agent_height as the most impactful starting point. Note that large variations from the defaults for these parameters (e.g. in the case of very small agents) may be better supported by additional changes to cell_size and cell_height.
navmesh_settings = habitat_sim.NavMeshSettings()
# @markdown Choose Habitat-sim defaults (e.g. for point-nav tasks), or custom settings.
use_custom_settings = False # @param {type:"boolean"}
sim.navmesh_visualization = True # @param {type:"boolean"}
navmesh_settings.set_defaults()
if use_custom_settings:
# fmt: off
#@markdown ---
#@markdown ## Configure custom settings (if use_custom_settings):
#@markdown Configure the following NavMeshSettings for customized NavMesh recomputation.
#@markdown **Voxelization parameters**:
navmesh_settings.cell_size = 0.05 #@param {type:"slider", min:0.01, max:0.2, step:0.01}
#default = 0.05
navmesh_settings.cell_height = 0.2 #@param {type:"slider", min:0.01, max:0.4, step:0.01}
#default = 0.2
self._gripped_obj_id = -1
sim_settings = make_default_settings()
# fmt: off
sim_settings["scene"] = "/home/intern/RobotSim_Yolov4/habitat-sim/data/scene_datasets/mp3d/17DRP5sb8fy/17DRP5sb8fy.glb" # @param{type:"string"}
# fmt: on
sim_settings["sensor_pitch"] = 0
sim_settings["sensor_height"] = 0.6
sim_settings["color_sensor_3rd_person"] = True
sim_settings["depth_sensor_1st_person"] = True
sim_settings["semantic_sensor_1st_person"] = True
make_simulator_from_settings(sim_settings)
default_nav_mesh_settings = habitat_sim.NavMeshSettings()
default_nav_mesh_settings.set_defaults()
inflated_nav_mesh_settings = habitat_sim.NavMeshSettings()
inflated_nav_mesh_settings.set_defaults()
inflated_nav_mesh_settings.agent_radius = 0.2
inflated_nav_mesh_settings.agent_height = 1.5
recompute_successful = sim.recompute_navmesh(sim.pathfinder, inflated_nav_mesh_settings)
if not recompute_successful:
print("Failed to recompute navmesh!")
# @markdown ---
# @markdown ### Set other example parameters:
seed = 24 # @param {type:"integer"}
random.seed(seed)
sim.seed(seed)
np.random.seed(seed)
def reconfigure_scene(env, scene_path):
"""This function takes a habitat scene and adds relevant changes to the
scene that make it useful for locobot assistant.
For example, it sets initial positions to be deterministic, and it
adds some humans to the environment a reasonable places
"""
sim = env._robot.base.sim
# these are useful to know to understand co-ordinate normalization
# and where to place objects to be within bounds. They change wildly
# from scene to scene
print("scene bounds: {}".format(sim.pathfinder.get_bounds()))
agent = sim.get_agent(0)
# keep the initial rotation consistent across runs
old_agent_state = agent.get_state()
new_agent_state = habitat_sim.AgentState()
new_agent_state.position = old_agent_state.position
new_agent_state.rotation = np.quaternion(1.0, 0.0, 0.0, 0.0)
agent.set_state(new_agent_state)
env._robot.base.init_state = agent.get_state()
env.initial_rotation = new_agent_state.rotation
###########################
# scene-specific additions
###########################
scene_name = os.path.basename(scene_path).split(".")[0]
if scene_name == "mesh_semantic":
# this must be Replica Dataset
scene_name = os.path.basename(
os.path.dirname(os.path.dirname(scene_path)))
supported_scenes = ["skokloster-castle", "van-gogh-room", "apartment_0"]
if scene_name not in supported_scenes:
print("Scene {} not in supported scenes, so skipping adding objects".
format(scene_name))
return
assets_path = os.path.abspath(
os.path.join(os.path.dirname(__file__), "../test/test_assets"))
if hasattr(sim, 'get_object_template_manager'):
load_object_configs = sim.get_object_template_manager(
).load_object_configs
else:
load_object_configs = sim.load_object_configs
human_male_template_id = load_object_configs(
os.path.join(assets_path, "human_male"))[0]
human_female_template_id = load_object_configs(
os.path.join(assets_path, "human_female"))[0]
if scene_name == "apartment_0":
id_male = sim.add_object(human_male_template_id)
id_female = sim.add_object(human_female_template_id)
print("id_male, id_female: {} {}".format(id_male, id_female))
sim.set_translation([1.2, -0.81, 0.3],
id_female) # apartment_0, female
sim.set_translation([1.2, -0.75, -0.3], id_male) # apartment_0, male
rot = mn.Quaternion.rotation(mn.Deg(-75), mn.Vector3.y_axis())
sim.set_rotation(rot, id_male) # apartment_0
sim.set_rotation(rot, id_female) # apartment_0
elif scene_name == "skokloster-castle":
id_female = sim.add_object(human_female_template_id)
print("id_female: {}".format(id_female))
sim.set_translation([2.0, 3.0, 15.00], id_female) # skokloster castle
elif scene_name == "van-gogh-room":
id_female = sim.add_object(human_female_template_id)
print("id_female: {}".format(id_female))
sim.set_translation([1.0, 0.84, 0.00], id_female) # van-gogh-room
# make the objects STATIC so that collisions work
for obj in [id_male, id_female]:
sim.set_object_motion_type(habitat_sim.physics.MotionType.STATIC, obj)
navmesh_settings = habitat_sim.NavMeshSettings()
navmesh_settings.set_defaults()
sim.recompute_navmesh(sim.pathfinder,
navmesh_settings,
include_static_objects=True)
def test_fetch_robot_wrapper(fixed_base):
# set this to output test results as video for easy investigation
produce_debug_video = False
observations = []
cfg_settings = examples.settings.default_sim_settings.copy()
cfg_settings["scene"] = "NONE"
cfg_settings["enable_physics"] = True
# loading the physical scene
hab_cfg = examples.settings.make_cfg(cfg_settings)
with habitat_sim.Simulator(hab_cfg) as sim:
obj_template_mgr = sim.get_object_template_manager()
rigid_obj_mgr = sim.get_rigid_object_manager()
# setup the camera for debug video (looking at 0,0,0)
sim.agents[0].scene_node.translation = [0.0, -1.0, 2.0]
# add a ground plane
cube_handle = obj_template_mgr.get_template_handles("cubeSolid")[0]
cube_template_cpy = obj_template_mgr.get_template_by_handle(cube_handle)
cube_template_cpy.scale = np.array([5.0, 0.2, 5.0])
obj_template_mgr.register_template(cube_template_cpy)
ground_plane = rigid_obj_mgr.add_object_by_template_handle(cube_handle)
ground_plane.translation = [0.0, -0.2, 0.0]
ground_plane.motion_type = habitat_sim.physics.MotionType.STATIC
# compute a navmesh on the ground plane
navmesh_settings = habitat_sim.NavMeshSettings()
navmesh_settings.set_defaults()
sim.recompute_navmesh(sim.pathfinder, navmesh_settings, True)
sim.navmesh_visualization = True
# add the robot to the world via the wrapper
robot_path = "data/robots/hab_fetch/robots/hab_fetch.urdf"
fetch = fetch_robot.FetchRobot(robot_path, sim, fixed_base=fixed_base)
fetch.reconfigure()
assert fetch.get_robot_sim_id() == 1 # 0 is the ground plane
print(fetch.get_link_and_joint_names())
observations += simulate(sim, 1.0, produce_debug_video)
# retract the arm
observations += fetch._interpolate_arm_control(
[1.2299035787582397, 2.345386505126953],
[fetch.params.arm_joints[1], fetch.params.arm_joints[3]],
1,
30,
produce_debug_video,
)
# ready the arm
observations += fetch._interpolate_arm_control(
[-0.45, 0.1],
[fetch.params.arm_joints[1], fetch.params.arm_joints[3]],
1,
30,
produce_debug_video,
)
# setting arm motor positions
fetch.arm_motor_pos = np.zeros(len(fetch.params.arm_joints))
observations += simulate(sim, 1.0, produce_debug_video)
# set base ground position from navmesh
# NOTE: because the navmesh floats above the collision geometry we should see a pop/settle with dynamics and no fixed base
target_base_pos = sim.pathfinder.snap_point(fetch.sim_obj.translation)
fetch.base_pos = target_base_pos
assert fetch.base_pos == target_base_pos
observations += simulate(sim, 1.0, produce_debug_video)
if fixed_base:
assert np.allclose(fetch.base_pos, target_base_pos)
else:
assert not np.allclose(fetch.base_pos, target_base_pos)
# arm joint queries and setters
print(f" Arm joint velocities = {fetch.arm_velocity}")
fetch.arm_joint_pos = np.ones(len(fetch.params.arm_joints))
fetch.arm_motor_pos = np.ones(len(fetch.params.arm_joints))
print(f" Arm joint positions (should be ones) = {fetch.arm_joint_pos}")
print(f" Arm joint limits = {fetch.arm_joint_limits}")
fetch.arm_motor_pos = fetch.arm_motor_pos
observations += simulate(sim, 1.0, produce_debug_video)
# test gripper state
fetch.open_gripper()
observations += simulate(sim, 1.0, produce_debug_video)
assert fetch.is_gripper_open
assert not fetch.is_gripper_closed
fetch.close_gripper()
observations += simulate(sim, 1.0, produce_debug_video)
assert fetch.is_gripper_closed
assert not fetch.is_gripper_open
# halfway open
fetch.set_gripper_target_state(0.5)
observations += simulate(sim, 0.5, produce_debug_video)
assert not fetch.is_gripper_open
assert not fetch.is_gripper_closed
# kinematic open/close (checked before simulation)
fetch.gripper_joint_pos = fetch.params.gripper_open_state
assert np.allclose(fetch.gripper_joint_pos, fetch.params.gripper_open_state)
assert fetch.is_gripper_open
observations += simulate(sim, 0.2, produce_debug_video)
fetch.gripper_joint_pos = fetch.params.gripper_closed_state
assert fetch.is_gripper_closed
observations += simulate(sim, 0.2, produce_debug_video)
# end effector queries
print(f" End effector link id = {fetch.ee_link_id}")
print(f" End effector local offset = {fetch.ee_local_offset}")
print(f" End effector transform = {fetch.ee_transform}")
print(
f" End effector translation (at current state) = {fetch.calculate_ee_forward_kinematics(fetch.sim_obj.joint_positions)}"
)
invalid_ef_target = np.array([100.0, 200.0, 300.0])
print(
f" Clip end effector target ({invalid_ef_target}) to reach = {fetch.clip_ee_to_workspace(invalid_ef_target)}"
)
# produce some test debug video
if produce_debug_video:
from habitat_sim.utils import viz_utils as vut
vut.make_video(
observations,
"color_sensor",
"color",
"test_fetch_robot_wrapper__fixed_base=" + str(fixed_base),
open_vid=True,
)
def test_recompute_navmesh(test_scene):
if not osp.exists(test_scene):
pytest.skip(f"{test_scene} not found")
cfg_settings = examples.settings.default_sim_settings.copy()
cfg_settings["scene"] = test_scene
hab_cfg = examples.settings.make_cfg(cfg_settings)
with habitat_sim.Simulator(hab_cfg) as sim:
sim.navmesh_visualization = True
assert sim.navmesh_visualization
sim.navmesh_visualization = False
assert not sim.navmesh_visualization
# generate random point pairs
num_samples = 100
samples = []
for _ in range(num_samples):
samples.append(
(
sim.pathfinder.get_random_navigable_point(),
sim.pathfinder.get_random_navigable_point(),
)
)
# compute shortest paths between these points on the loaded navmesh
loaded_navmesh_path_results = get_shortest_path(sim, samples)
assert len(sim.pathfinder.build_navmesh_vertices()) > 0
assert len(sim.pathfinder.build_navmesh_vertex_indices()) > 0
hab_cfg = examples.settings.make_cfg(cfg_settings)
agent_config = hab_cfg.agents[hab_cfg.sim_cfg.default_agent_id]
agent_config.radius *= 2.0
sim.reconfigure(hab_cfg)
# compute shortest paths between these points on the loaded navmesh with twice radius
loaded_navmesh_2rad_path_results = get_shortest_path(sim, samples)
navmesh_settings = habitat_sim.NavMeshSettings()
navmesh_settings.set_defaults()
assert sim.recompute_navmesh(sim.pathfinder, navmesh_settings)
assert sim.pathfinder.is_loaded
assert len(sim.pathfinder.build_navmesh_vertices()) > 0
assert len(sim.pathfinder.build_navmesh_vertex_indices()) > 0
recomputed_navmesh_results = get_shortest_path(sim, samples)
navmesh_settings.agent_radius *= 2.0
assert sim.recompute_navmesh(sim.pathfinder, navmesh_settings)
assert sim.pathfinder.is_loaded # this may not always be viable...
recomputed_2rad_navmesh_results = get_shortest_path(sim, samples)
some_diff = False
for i in range(num_samples):
assert loaded_navmesh_path_results[i][0] == recomputed_navmesh_results[i][0]
assert (
loaded_navmesh_2rad_path_results[i][0]
== recomputed_2rad_navmesh_results[i][0]
)
if loaded_navmesh_path_results[i][0]:
assert (
abs(
loaded_navmesh_path_results[i][1]
- recomputed_navmesh_results[i][1]
)
< EPS
)
if recomputed_2rad_navmesh_results[i][0]:
assert (
abs(
loaded_navmesh_2rad_path_results[i][1]
- recomputed_2rad_navmesh_results[i][1]
)
< EPS
)
if (
loaded_navmesh_path_results[i][0]
!= recomputed_2rad_navmesh_results[i][0]
or loaded_navmesh_path_results[i][1]
- recomputed_2rad_navmesh_results[i][1]
> EPS
):
some_diff = True
assert some_diff
