def set_state(self, state: AgentState, reset_sensors: bool = True):
r"""Sets the agents state
:param state: The state to set the agent to
:param reset_sensors: Whether or not to reset the sensors to their
default intrinsic/extrinsic parameters before setting their
extrinsic state
"""
habitat_sim.errors.assert_obj_valid(self.body)
if isinstance(state.rotation, list):
state.rotation = quat_from_coeffs(state.rotation)
self.body.object.reset_transformation()
self.body.object.translate(state.position)
self.body.object.rotation = quat_to_magnum(state.rotation)
if reset_sensors:
for _, v in self._sensors.items():
v.set_transformation_from_spec()
for k, v in state.sensor_states.items():
assert k in self._sensors
if isinstance(v.rotation, list):
v.rotation = quat_from_coeffs(v.rotation)
s = self._sensors[k]
s.node.reset_transformation()
s.node.translate(
quat_rotate_vector(state.rotation.inverse(),
v.position - state.position))
s.node.rotation = quat_to_magnum(state.rotation.inverse() *
v.rotation)
def set_state(
self,
state: AgentState,
reset_sensors: bool = True,
infer_sensor_states: bool = True,
is_initial: bool = False,
) -> None:
r"""Sets the agents state
:param state: The state to set the agent to
:param reset_sensors: Whether or not to reset the sensors to their
default intrinsic/extrinsic parameters before setting their extrinsic state.
:param infer_sensor_states: Whether or not to infer the location of sensors based on
the new location of the agent base state.
:param is_initial: Whether this state is the initial state of the
agent in the scene. Used for resetting the agent at a later time
Setting ``reset_sensors`` to :py:`False`
allows the agent base state to be moved and the new
sensor locations inferred without changing the configuration of the sensors
with respect to the base state of the agent.
Setting ``infer_sensor_states``
to :py:`False` is useful if you'd like to directly control
the state of a sensor instead of moving the agent.
"""
attr.validate(state)
habitat_sim.errors.assert_obj_valid(self.body)
if isinstance(state.rotation, (list, np.ndarray)):
state.rotation = quat_from_coeffs(state.rotation)
self.body.object.reset_transformation()
self.body.object.translate(state.position)
self.body.object.rotation = quat_to_magnum(state.rotation)
if reset_sensors:
for _, v in self._sensors.items():
v.set_transformation_from_spec()
if not infer_sensor_states:
for k, v in state.sensor_states.items():
assert k in self._sensors
if isinstance(v.rotation, list):
v.rotation = quat_from_coeffs(v.rotation)
s = self._sensors[k]
s.node.reset_transformation()
s.node.translate(
quat_rotate_vector(state.rotation.inverse(),
v.position - state.position))
s.node.rotation = quat_to_magnum(state.rotation.inverse() *
v.rotation)
if is_initial:
self.initial_state = state
def find_path(self, goal_pos: np.ndarray) -> List[Any]:
r"""Finds the sequence actions that greedily follow the geodesic
shortest path from the agent's current position to get to the goal
:param goal_pos: The position of the goal
:return: The list of actions to take. Ends with :py:`None`.
This is roughly equivilent to just calling `next_action_along()` until
it returns :py:`None`, but is faster.
.. note-warning::
Do not use this method if the agent has actuation noise.
Instead, use :ref:`next_action_along` to find the action
to take in a given state, then take that action, and repeat!
"""
self.reset()
state = self.agent.state
path = self.impl.find_path(
quat_to_magnum(state.rotation), state.position, goal_pos
)
if len(path) == 0:
raise errors.GreedyFollowerError()
path = list(map(lambda v: self.action_mapping[v], path))
return path
def test_kinematics(sim):
cfg_settings = examples.settings.default_sim_settings.copy()
cfg_settings[
"scene"] = "data/scene_datasets/habitat-test-scenes/skokloster-castle.glb"
# enable the physics simulator: also clears available actions to no-op
cfg_settings["enable_physics"] = True
cfg_settings["depth_sensor"] = True
# test loading the physical scene
hab_cfg = examples.settings.make_cfg(cfg_settings)
sim.reconfigure(hab_cfg)
assert sim.get_physics_object_library_size() > 0
# test adding an object to the world
object_id = sim.add_object(0)
assert len(sim.get_existing_object_ids()) > 0
# test kinematics
I = np.identity(4)
# test get and set translation
sim.set_translation(np.array([0, 1.0, 0]), object_id)
assert np.allclose(sim.get_translation(object_id), np.array([0, 1.0, 0]))
# test get and set transform
sim.set_transformation(I, object_id)
assert np.allclose(sim.get_transformation(object_id), I)
# test get and set rotation
Q = quat_from_angle_axis(np.pi, np.array([0, 1.0, 0]))
expected = np.eye(4)
expected[0:3, 0:3] = quaternion.as_rotation_matrix(Q)
sim.set_rotation(quat_to_magnum(Q), object_id)
assert np.allclose(sim.get_transformation(object_id), expected)
assert np.allclose(quat_from_magnum(sim.get_rotation(object_id)), Q)
# test object removal
old_object_id = sim.remove_object(object_id)
assert len(sim.get_existing_object_ids()) == 0
assert old_object_id == object_id
object_id = sim.add_object(0)
prev_time = 0.0
for _ in range(2):
# do some kinematics here (todo: translating or rotating instead of absolute)
sim.set_translation(np.random.rand(3), object_id)
T = sim.get_transformation(object_id)
# test getting observation
obs = sim.step(
random.choice(list(hab_cfg.agents[0].action_space.keys())))
# check that time is increasing in the world
assert sim.get_world_time() > prev_time
prev_time = sim.get_world_time()
def step_cas(self, vel_control, dt=1.0 / 30.0):
agent_state = self._default_agent.state
self._num_total_frames += 1
self._last_state = self._default_agent.get_state()
previous_rigid_state = habitat_sim.RigidState(
quat_to_magnum(agent_state.rotation), agent_state.position)
# manually integrate the rigid state
target_rigid_state = vel_control.integrate_transform(
dt, previous_rigid_state)
# snap rigid state to navmesh and set state to object/agent
# calls pathfinder.try_step or self.pathfinder.try_step_no_sliding
end_pos = self.step_filter(previous_rigid_state.translation,
target_rigid_state.translation)
# set the computed state
agent_state.position = end_pos
agent_state.rotation = quat_from_magnum(target_rigid_state.rotation)
self._default_agent.set_state(agent_state)
# Check if a collision occured
dist_moved_before_filter = (target_rigid_state.translation -
previous_rigid_state.translation).dot()
dist_moved_after_filter = (end_pos -
previous_rigid_state.translation).dot()
# NB: There are some cases where ||filter_end - end_pos|| > 0 when a
# collision _didn't_ happen. One such case is going up stairs. Instead,
# we check to see if the the amount moved after the application of the filter
# is _less_ than the amount moved before the application of the filter
EPS = 0.2
collided = (dist_moved_after_filter + EPS) < dist_moved_before_filter
# print("collidded", collided)
# step physics by dt
step_start_Time = time.time()
super().step_world(dt)
self._previous_step_time = time.time() - step_start_Time
observations = self.get_sensor_observations()
# Whether or not the action taken resulted in a collision
observations["collided"] = collided
return observations
def next_action_along(self, goal_pos: np.ndarray) -> Any:
r"""Find the next action to greedily follow the geodesic shortest path
from the agent's current position to get to the goal
:param goal_pos: The position of the goal
:return: The action to take
"""
if self.last_goal is None or not np.allclose(goal_pos, self.last_goal):
self.reset()
self.last_goal = goal_pos
state = self.agent.state
next_act = self.impl.next_action_along(quat_to_magnum(state.rotation),
state.position, goal_pos)
if next_act == GreedyFollowerCodes.ERROR:
raise errors.GreedyFollowerError()
else:
return self.action_mapping[next_act]
def update(env, state, a, delta, vel_control, time_step):
dt = time_step
# angle = quaternion.as_euler_angles(env.habitat_env._sim.get_agent_state().rotation)
agent_forward = quat_to_magnum(
env.habitat_env._sim.get_agent_state().rotation).transform_vector(
mn.Vector3(0, 0, -1.0))
# print("linear velocity", state.v)
# print("angular velocity", state.yaw)
state.x = env.habitat_env._sim.get_agent_state().position[0]
state.y = -env.habitat_env._sim.get_agent_state().position[2]
# state.yaw = -angle[1]
state.yaw = math.atan2(agent_forward[0], agent_forward[2])
vel_control.linear_velocity = np.array([0, 0, -a])
# local up is y
vel_control.angular_velocity = np.array([0, delta, 0])
print("lin", a)
print("vel", delta)
obs, reward, done, info = env.step(vel_control)
return obs, info, state
agent_pos = agent_state.position
# topdown map at agent position
top_down_map = maps.get_topdown_map(
sim.pathfinder, height=agent_pos[1], meters_per_pixel=meters_per_pixel
)
recolor_map = np.array(
[[255, 255, 255], [128, 128, 128], [0, 0, 0]], dtype=np.uint8
)
top_down_map = recolor_map[top_down_map]
grid_dimensions = (top_down_map.shape[0], top_down_map.shape[1])
# convert world agent position to maps module grid point
agent_grid_pos = maps.to_grid(
agent_pos[2], agent_pos[0], grid_dimensions, pathfinder=sim.pathfinder
)
agent_forward = utils.quat_to_magnum(
sim.agents[0].get_state().rotation
).transform_vector(mn.Vector3(0, 0, -1.0))
agent_orientation = math.atan2(agent_forward[0], agent_forward[2])
# draw the agent and trajectory on the map
maps.draw_agent(
top_down_map, agent_grid_pos, agent_orientation, agent_radius_px=8
)
print("\nDisplay topdown map with agent:")
display_map(top_down_map)
# %%
# @markdown ##Saving the NavMesh
# fmt: off
# @markdown An existing NavMesh can be saved with *Pathfinder.save_nav_mesh(filename)*
def reference_path_example(mode):
"""
Saves a video of a shortest path follower agent navigating from a start
position to a goal. Agent follows the ground truth reference path by
navigating to intermediate viewpoints en route to goal.
Args:
mode: 'geodesic_path' or 'greedy'
"""
config = habitat.get_config(
config_paths="configs/test/habitat_r2r_vln_test_continuous.yaml")
config.defrost()
config.TASK.MEASUREMENTS.append("TOP_DOWN_MAP")
config.TASK.SENSORS.append("HEADING_SENSOR")
config.freeze()
with SimpleRLEnv(config=config) as env:
print("Environment creation successful")
sim_time = 30 # @param {type:"integer"}
continuous_nav = True # @param {type:"boolean"}
if continuous_nav:
control_frequency = 10 # @param {type:"slider", min:1, max:30, step:1}
frame_skip = 6 # @param {type:"slider", min:1, max:30, step:1}
fps = control_frequency * frame_skip
print("fps = " + str(fps))
control_sequence = []
# create and configure a new VelocityControl structure
vel_control = habitat_sim.physics.VelocityControl()
vel_control.controlling_lin_vel = True
vel_control.lin_vel_is_local = True
vel_control.controlling_ang_vel = True
vel_control.ang_vel_is_local = True
for episode in range(6):
env.reset()
print(env.habitat_env.current_episode)
episode_id = env.habitat_env.current_episode.episode_id
print(
f"Agent stepping around inside environment. Episode id: {episode_id}"
)
dirname = os.path.join(IMAGE_DIR, "vln_reference_path_example",
mode, "%02d" % episode)
if os.path.exists(dirname):
shutil.rmtree(dirname)
os.makedirs(dirname)
images = []
steps = 0
reference_path = env.habitat_env.current_episode.reference_path
waypoints = np.array(reference_path)
for i in range(waypoints.shape[1] - 1):
if np.abs(np.linalg.norm(waypoints[i + 1, :] -
waypoints[i, :])) < 5:
waypoints = np.delete(waypoints, (i + 1), axis=0)
waypoints = waypoints[0::10]
x_wp = waypoints[:, 0]
y_wp = -waypoints[:, 2]
z_wp = waypoints[:, 1]
ax = x_wp
ay = y_wp
cx, cy, cyaw, ck, s_profile, s = cubic_spline_planner.calc_spline_course(
ax, ay, ds=0.01)
goal = [cx[-1], cy[-1], cyaw[-1]]
agent_forward = quat_to_magnum(
env.habitat_env._sim.get_agent_state(
).rotation).transform_vector(mn.Vector3(0, 0, -1.0))
angle = quaternion.as_euler_angles(
env.habitat_env._sim.get_agent_state().rotation)
init_x = env.habitat_env._sim.get_agent_state().position[0]
init_y = -env.habitat_env._sim.get_agent_state().position[2]
# init_yaw = angle[1]
init_yaw = math.atan2(agent_forward[0], agent_forward[2])
state = State(x=init_x, y=init_y, yaw=init_yaw)
time_step = 1.0 / (30)
T = 500
t, x, y, yaw, vel, omeg, images = closed_loop_prediction(
env, state, cx, cy, cyaw, ck, s_profile, goal, images,
vel_control, time_step)
images_to_video(images,
dirname,
str(episode_id),
fps=int(1.0 / time_step))
images = []
def test_kinematics():
cfg_settings = examples.settings.default_sim_settings.copy()
cfg_settings[
"scene"] = "data/scene_datasets/habitat-test-scenes/skokloster-castle.glb"
# enable the physics simulator: also clears available actions to no-op
cfg_settings["enable_physics"] = True
cfg_settings["depth_sensor"] = True
# test loading the physical scene
hab_cfg = examples.settings.make_cfg(cfg_settings)
with habitat_sim.Simulator(hab_cfg) as sim:
obj_mgr = sim.get_object_template_manager()
assert obj_mgr.get_num_templates() > 0
# test adding an object to the world
# get handle for object 0, used to test
obj_handle_list = obj_mgr.get_template_handles("cheezit")
object_id = sim.add_object_by_handle(obj_handle_list[0])
assert len(sim.get_existing_object_ids()) > 0
# test setting the motion type
assert sim.set_object_motion_type(
habitat_sim.physics.MotionType.STATIC, object_id)
assert (sim.get_object_motion_type(object_id) ==
habitat_sim.physics.MotionType.STATIC)
assert sim.set_object_motion_type(
habitat_sim.physics.MotionType.KINEMATIC, object_id)
assert (sim.get_object_motion_type(object_id) ==
habitat_sim.physics.MotionType.KINEMATIC)
# test kinematics
I = np.identity(4)
# test get and set translation
sim.set_translation(np.array([0, 1.0, 0]), object_id)
assert np.allclose(sim.get_translation(object_id),
np.array([0, 1.0, 0]))
# test object SceneNode
object_node = sim.get_object_scene_node(object_id)
assert np.allclose(sim.get_translation(object_id),
object_node.translation)
# test get and set transform
sim.set_transformation(I, object_id)
assert np.allclose(sim.get_transformation(object_id), I)
# test get and set rotation
Q = quat_from_angle_axis(np.pi, np.array([0, 1.0, 0]))
expected = np.eye(4)
expected[0:3, 0:3] = quaternion.as_rotation_matrix(Q)
sim.set_rotation(quat_to_magnum(Q), object_id)
assert np.allclose(sim.get_transformation(object_id), expected)
assert np.allclose(quat_from_magnum(sim.get_rotation(object_id)), Q)
# test object removal
sim.remove_object(object_id)
assert len(sim.get_existing_object_ids()) == 0
obj_handle_list = obj_mgr.get_template_handles("cheezit")
object_id = sim.add_object_by_handle(obj_handle_list[0])
prev_time = 0.0
for _ in range(2):
# do some kinematics here (todo: translating or rotating instead of absolute)
sim.set_translation(np.random.rand(3), object_id)
T = sim.get_transformation(object_id) # noqa : F841
# test getting observation
sim.step(random.choice(list(
hab_cfg.agents[0].action_space.keys())))
# check that time is increasing in the world
assert sim.get_world_time() > prev_time
prev_time = sim.get_world_time()
sim.remove_object(object_id)
# test attaching/dettaching an Agent to/from physics simulation
agent_node = sim.agents[0].scene_node
obj_handle_list = obj_mgr.get_template_handles("cheezit")
object_id = sim.add_object_by_handle(obj_handle_list[0], agent_node)
sim.set_translation(np.random.rand(3), object_id)
assert np.allclose(agent_node.translation,
sim.get_translation(object_id))
sim.remove_object(object_id, False) # don't delete the agent's node
assert agent_node.translation
# test get/set RigidState
object_id = sim.add_object_by_handle(obj_handle_list[0])
targetRigidState = habitat_sim.bindings.RigidState(
mn.Quaternion(), np.array([1.0, 2.0, 3.0]))
sim.set_rigid_state(targetRigidState, object_id)
objectRigidState = sim.get_rigid_state(object_id)
assert np.allclose(objectRigidState.translation,
targetRigidState.translation)
assert objectRigidState.rotation == targetRigidState.rotation
def test_kinematics():
cfg_settings = examples.settings.default_sim_settings.copy()
cfg_settings[
"scene"] = "data/scene_datasets/habitat-test-scenes/skokloster-castle.glb"
# enable the physics simulator: also clears available actions to no-op
cfg_settings["enable_physics"] = True
cfg_settings["depth_sensor"] = True
# test loading the physical scene
hab_cfg = examples.settings.make_cfg(cfg_settings)
with habitat_sim.Simulator(hab_cfg) as sim:
# get the rigid object attributes manager, which manages
# templates used to create objects
obj_template_mgr = sim.get_object_template_manager()
obj_template_mgr.load_configs("data/objects/example_objects/", True)
assert obj_template_mgr.get_num_templates() > 0
# get the rigid object manager, which provides direct
# access to objects
rigid_obj_mgr = sim.get_rigid_object_manager()
# test adding an object to the world
# get handle for object 0, used to test
obj_handle_list = obj_template_mgr.get_template_handles("cheezit")
cheezit_box = rigid_obj_mgr.add_object_by_template_handle(
obj_handle_list[0])
assert rigid_obj_mgr.get_num_objects() > 0
assert (len(rigid_obj_mgr.get_object_handles()) ==
rigid_obj_mgr.get_num_objects())
# test setting the motion type
cheezit_box.motion_type = habitat_sim.physics.MotionType.STATIC
assert cheezit_box.motion_type == habitat_sim.physics.MotionType.STATIC
cheezit_box.motion_type = habitat_sim.physics.MotionType.KINEMATIC
assert cheezit_box.motion_type == habitat_sim.physics.MotionType.KINEMATIC
# test kinematics
I = np.identity(4)
# test get and set translation
cheezit_box.translation = [0.0, 1.0, 0.0]
assert np.allclose(cheezit_box.translation, np.array([0.0, 1.0, 0.0]))
# test object SceneNode
assert np.allclose(cheezit_box.translation,
cheezit_box.root_scene_node.translation)
# test get and set transform
cheezit_box.transformation = I
assert np.allclose(cheezit_box.transformation, I)
# test get and set rotation
Q = quat_from_angle_axis(np.pi, np.array([0.0, 1.0, 0.0]))
expected = np.eye(4)
expected[0:3, 0:3] = quaternion.as_rotation_matrix(Q)
cheezit_box.rotation = quat_to_magnum(Q)
assert np.allclose(cheezit_box.transformation, expected)
assert np.allclose(quat_from_magnum(cheezit_box.rotation), Q)
# test object removal
rigid_obj_mgr.remove_object_by_id(cheezit_box.object_id)
assert rigid_obj_mgr.get_num_objects() == 0
obj_handle_list = obj_template_mgr.get_template_handles("cheezit")
cheezit_box = rigid_obj_mgr.add_object_by_template_handle(
obj_handle_list[0])
prev_time = 0.0
for _ in range(2):
# do some kinematics here (todo: translating or rotating instead of absolute)
cheezit_box.translation = np.random.rand(3)
T = cheezit_box.transformation # noqa : F841
# test getting observation
sim.step(random.choice(list(
hab_cfg.agents[0].action_space.keys())))
# check that time is increasing in the world
assert sim.get_world_time() > prev_time
prev_time = sim.get_world_time()
rigid_obj_mgr.remove_object_by_id(cheezit_box.object_id)
# test attaching/dettaching an Agent to/from physics simulation
agent_node = sim.agents[0].scene_node
obj_handle_list = obj_template_mgr.get_template_handles("cheezit")
cheezit_agent = rigid_obj_mgr.add_object_by_template_handle(
obj_handle_list[0], agent_node)
cheezit_agent.translation = np.random.rand(3)
assert np.allclose(agent_node.translation, cheezit_agent.translation)
rigid_obj_mgr.remove_object_by_id(
cheezit_agent.object_id,
delete_object_node=False) # don't delete the agent's node
assert agent_node.translation
# test get/set RigidState
cheezit_box = rigid_obj_mgr.add_object_by_template_handle(
obj_handle_list[0])
targetRigidState = habitat_sim.bindings.RigidState(
mn.Quaternion(), np.array([1.0, 2.0, 3.0]))
cheezit_box.rigid_state = targetRigidState
objectRigidState = cheezit_box.rigid_state
assert np.allclose(objectRigidState.translation,
targetRigidState.translation)
assert objectRigidState.rotation == targetRigidState.rotation
def reference_path_example(mode):
"""
Saves a video of a shortest path follower agent navigating from a start
position to a goal. Agent follows the ground truth reference path by
navigating to intermediate viewpoints en route to goal.
Args:
mode: 'geodesic_path' or 'greedy'
"""
show_waypoint_indicators = False
config = habitat.get_config(
config_paths="configs/test/habitat_r2r_vln_test.yaml")
config.defrost()
config.TASK.MEASUREMENTS.append("TOP_DOWN_MAP")
config.TASK.SENSORS.append("HEADING_SENSOR")
config.freeze()
split = 'train'
vln_data_path = '/home/mirshad7/habitat-lab/data/datasets/vln/mp3d/r2r/v1/' + split + '/' + split + '.json.gz'
with gzip.open(vln_data_path, "rt") as f:
deserialized = json.loads(f.read())
val_ids = {}
for i in range(len(deserialized['episodes'])):
val_ids[deserialized['episodes'][i]['episode_id']] = i
new_data_dict = {}
new_data_dict['episodes'] = {}
new_data_dict['instruction_vocab'] = deserialized['instruction_vocab']
new_data_list = []
save_fig = False
steps_dict = {}
with SimpleRLEnv(config=config) as env:
print("Environment creation successful")
# obj_attr_mgr = env.habitat_env._sim.get_object_template_manager()
# remove_all_objects(env.habitat_env._sim)
sim_time = 30 # @param {type:"integer"}
continuous_nav = True # @param {type:"boolean"}
if continuous_nav:
control_frequency = 10 # @param {type:"slider", min:1, max:30, step:1}
frame_skip = 6 # @param {type:"slider", min:1, max:30, step:1}
fps = control_frequency * frame_skip
print("fps = " + str(fps))
control_sequence = []
for action in range(int(sim_time * control_frequency)):
if continuous_nav:
# allow forward velocity and y rotation to vary
control_sequence.append({
"forward_velocity":
random.random() * 2.0, # [0,2)
"rotation_velocity":
(random.random() - 0.5) * 2.0, # [-1,1)
})
else:
control_sequence.append(random.choice(action_names))
# create and configure a new VelocityControl structure
vel_control = habitat_sim.physics.VelocityControl()
vel_control.controlling_lin_vel = True
vel_control.lin_vel_is_local = True
vel_control.controlling_ang_vel = True
vel_control.ang_vel_is_local = True
vis_ids = []
collided_trajectories = []
trajectory_without_collision = True
for episode in range(len(deserialized['episodes'])):
counter = 0
env.reset()
episode_id = env.habitat_env.current_episode.episode_id
print(
f"Agent stepping around inside environment. Episode id: {episode_id}"
)
dirname = os.path.join(IMAGE_DIR, "vln_reference_path_example",
mode, "%02d" % episode)
if os.path.exists(dirname):
shutil.rmtree(dirname)
os.makedirs(dirname)
images = []
steps = 0
reference_path = env.habitat_env.current_episode.reference_path + [
env.habitat_env.current_episode.goals[0].position
]
# manually control the object's kinematic state via velocity integration
time_step = 1.0 / (30)
x = []
y = []
yaw = []
vel = []
omega = []
continuous_path_follower = ContinuousPathFollower(
env.habitat_env._sim, reference_path, waypoint_threshold=0.4)
max_time = 30.0
done = False
EPS = 1e-4
prev_pos = np.linalg.norm(
env.habitat_env._sim.get_agent_state().position)
if show_waypoint_indicators:
for id in vis_ids:
sim.remove_object(id)
vis_ids = setup_path_visualization(env.habitat_env._sim,
continuous_path_follower)
while continuous_path_follower.progress < 1.0:
# print("done",done)
if done:
break
if counter == 150:
counter = 0
collided_trajectories.append(
env.habitat_env.current_episode.episode_id)
trajectory_without_collision = False
break
continuous_path_follower.update_waypoint()
if show_waypoint_indicators:
sim.set_translation(continuous_path_follower.waypoint,
vis_ids[0])
agent_state = env.habitat_env._sim.get_agent_state()
pos = np.linalg.norm(
env.habitat_env._sim.get_agent_state().position)
if abs(pos - prev_pos) < EPS:
counter += 1
previous_rigid_state = habitat_sim.RigidState(
quat_to_magnum(agent_state.rotation), agent_state.position)
v, w = track_waypoint(
continuous_path_follower.waypoint,
previous_rigid_state,
vel_control,
dt=time_step,
)
observations, reward, done, info = env.step(vel_control)
# print(observations)
# save_map(observations, info, images)
prev_pos = pos
x.append(env.habitat_env._sim.get_agent_state().position[0])
y.append(-env.habitat_env._sim.get_agent_state().position[2])
yaw.append(
quaternion.as_euler_angles(
env.habitat_env._sim.get_agent_state().rotation)[1])
vel.append(v)
omega.append(w)
steps += 1
if save_fig: # pragma: no cover
plt.close()
plt.subplots(1)
plt.plot(x, y, "xb", label="input")
plt.grid(True)
plt.axis("equal")
plt.xlabel("x[m]")
plt.ylabel("y[m]")
plt.legend()
pose_title = dirname + 'pose.png'
plt.savefig(pose_title)
plt.subplots(1)
plt.plot(yaw, "-r", label="yaw")
plt.grid(True)
plt.legend()
plt.xlabel("line length[m]")
plt.ylabel("yaw angle[deg]")
yaw_title = dirname + 'yaw.png'
plt.savefig(yaw_title)
plt.subplots(1)
plt.plot(vel, "-r", label="vel")
plt.grid(True)
plt.legend()
plt.xlabel("line length[m]")
plt.ylabel("omega_reference [rad/s^2]")
vel_title = dirname + 'vel.png'
plt.savefig(vel_title)
plt.subplots(1)
plt.plot(omega, "-r", label="v_reference")
plt.grid(True)
plt.legend()
plt.xlabel("line length[m]")
plt.ylabel("v_reference [m/s]")
omega_title = dirname + 'omega.png'
plt.savefig(omega_title)
x = []
y = []
yaw = []
vel = []
omega = []
if trajectory_without_collision:
ids = val_ids[episode_id]
single_data_dict = deserialized['episodes'][ids]
new_data_list.append(single_data_dict)
trajectory_without_collision = True
print(f"Navigated to goal in {steps} steps.")
steps_dict[episode] = steps
# images_to_video(images, dirname, str(episode_id), fps = int (1.0/time_step))
images = []
steps_path = '/home/mirshad7/habitat-lab/train_steps.json.gz'
# new_data_dict['episodes'] = new_data_list
# path = '/home/mirshad7/habitat-lab/data/datasets/vln/mp3d/r2r/robo_vln/train/train.json.gz'
# compress_json.dump(new_data_dict, path)
compress_json.dump(steps_dict, steps_path)
print("collided trajectories:", collided_trajectories)
def RunSimulation(envNum, record, path, Coords, seed):
settings = {"image_height": 720, "image_width": 1080, "random_seed": seed}
# pick a scene file to load
scene_file = path
if record:
os.system("mkdir CapturedFrames")
os.system(("mkdir CapturedFrames/Environment" + str(envNum)))
# --------------------------
# Simulator Setup
# --------------------------
# create a SimulatorConfiguration object
sim_cfg = habitat_sim.SimulatorConfiguration()
sim_cfg.scene.id = scene_file
# configure the simulator to intialize a PhysicsManager
sim_cfg.enable_physics = True
# configure the simulator to load a specific simulator configuration file (define paths you your object models here)
sim_cfg.physics_config_file = "data/default.phys_scene_config.json"
# pick this based on your device
relative_camera_position = [0.0, 0, 0.0]
# Note: all sensors must have the same resolution
sensors = {
"color": {
"sensor_type": habitat_sim.SensorType.COLOR,
"resolution": [settings["image_height"], settings["image_width"]],
"position": relative_camera_position,
},
"depth": {
"sensor_type": habitat_sim.SensorType.DEPTH,
"resolution": [settings["image_height"], settings["image_width"]],
"position": relative_camera_position,
},
}
sensor_specs = []
for sensor_uuid, sensor_params in sensors.items():
sensor_spec = habitat_sim.SensorSpec()
sensor_spec.uuid = sensor_uuid
sensor_spec.sensor_type = sensor_params["sensor_type"]
sensor_spec.resolution = sensor_params["resolution"]
sensor_spec.position = sensor_params["position"]
sensor_specs.append(sensor_spec)
# setup the agent/camera
agent_cfg = habitat_sim.agent.AgentConfiguration()
agent_cfg.sensor_specifications = sensor_specs
agent_cfg.action_space = {
"move_forward":
habitat_sim.agent.ActionSpec(
"move_forward", habitat_sim.agent.ActuationSpec(amount=0.2)),
"turn_left":
habitat_sim.agent.ActionSpec(
"turn_left", habitat_sim.agent.ActuationSpec(amount=10.0)),
}
# generate the full simulator configuration
combined_cfg = habitat_sim.Configuration(sim_cfg, [agent_cfg])
# initialize the simulator
sim = habitat_sim.Simulator(combined_cfg)
sim.agents[0].controls.move_filter_fn = sim._scene_collision_step_filter
# set random seed
random.seed(settings["random_seed"])
sim.seed(settings["random_seed"])
# initialize the agent state
sim.initialize_agent(0)
# ----------------------------------
# Quadrotor Prototype Functionality
# ----------------------------------
# assuming the first entry in the object list corresponds to the quadrotor model
quadrotor_id = sim.add_object(0)
# the object will not be affected by forces such as gravity and will remain static until explicitly moved
sim.set_object_motion_type(habitat_sim.physics.MotionType.KINEMATIC,
quadrotor_id)
## Randomly initialize the Ball
initial_height = 0.00001
possible_initial_point = sim.pathfinder.get_random_navigable_point(
) + np.array([0, initial_height, 0])
sim.set_translation(possible_initial_point, quadrotor_id)
while (sim.contact_test(quadrotor_id)):
possible_initial_point = sim.pathfinder.get_random_navigable_point(
) + np.array([0, initial_height, 0])
sim.set_translation(possible_initial_point, quadrotor_id)
# place the object in the air
# sim.set_translation(np.array([-0.569043, 2.04804, 13.6156]), quadrotor_id)
sim.agents[0].scene_node.translation = sim.get_translation(quadrotor_id)
# static_3rd_person_camera_position = np.array([-0.569043, 2.04804, 12.6156])
static_3rd_person_camera_position = np.array(
sim.get_translation(quadrotor_id)) - np.array([0, 0, 2])
initTranslation = sim.get_translation(quadrotor_id)
for frame in range(len(Coords)):
dx = Coords[frame][0]
dy = Coords[frame][1]
dz = 0
dispInFrame = np.array([dx, dy, dz])
currentTranslation = np.array(sim.get_translation(quadrotor_id))
sim.set_translation(dispInFrame + currentTranslation, quadrotor_id)
if (sim.contact_test(quadrotor_id)):
print("Collided at frame : ", frame)
break
if record:
# get/save 1st person images
# agent_observations = sim.get_sensor_observations()
#save_observation(agent_observations, frame, "agent_")
# move camera to 3rd person view
sim.get_agent(0).scene_node.translation = np.array(
static_3rd_person_camera_position)
sim.get_agent(0).scene_node.rotation = ut.quat_to_magnum(
ut.quat_look_at(sim.get_translation(quadrotor_id),
static_3rd_person_camera_position))
agent_observations = sim.get_sensor_observations()
save_observation(agent_observations, frame, envNum, "3rdperson_")
# reset the agent for the next step
sim.get_agent(0).scene_node.translation = sim.get_translation(
quadrotor_id)
sim.get_agent(0).scene_node.rotation = sim.get_rotation(
quadrotor_id)
if record:
MakeVideo(envNum)
sim.close()
del sim
