def __mul__(self, other):
if isinstance(other, SE3_Noise):
return SE3_Noise(
self.rot * other.rot,
self.trans + quat_rotate_vector(self.rot, other.trans),
)
else:
return quat_rotate_vector(self.rot, other) + self.trans
def change_pos(obj_pos, obj_rot):
rotation_habitat_replica = quat_from_two_vectors(np.array([0, 0, -1]),
np.array([0, -1, 0]))
rotation_replica_habitat = rotation_habitat_replica.inverse()
obj_quat = quaternion.from_float_array(obj_rot).inverse()
obj_pos = quat_rotate_vector(rotation_replica_habitat, obj_pos)
obj_position_replica = quat_rotate_vector(obj_quat, obj_pos)
print(obj_position_replica)
return obj_position_replica
def test_agent_sensor_orientations(my_agent):
my_agent_state = my_agent.get_state()
agent_orientation = my_agent_state.rotation
agent_position = my_agent_state.position
sensors_orientation = my_agent_state.sensor_states
my_agent_state.rotation = quaternion.from_rotation_vector(
[0.0, np.pi / 8, 0.0])
my_agent_state.position = [10.0, 5.0, 3.0]
my_agent_state.sensor_states[
"left_rgb_sensor"].rotation = quaternion.from_rotation_vector(
[0.0, -np.pi / 8, 0.0])
my_agent_state.sensor_states[
"right_rgb_sensor"].rotation = quaternion.from_rotation_vector(
[0.0, -np.pi / 8, 0.0])
my_agent_state.sensor_states[
"depth_sensor"].rotation = quaternion.from_rotation_vector(
[0.0, -np.pi / 8, 0.0])
d = quat_rotate_vector(my_agent_state.rotation.inverse(),
agent_position - my_agent_state.position)
r = my_agent_state.rotation.inverse(
) * my_agent_state.sensor_states["left_rgb_sensor"].rotation
my_agent.set_state(my_agent_state, infer_sensor_states=False)
my_agent_state = my_agent.get_state()
agent_orientation = my_agent_state.rotation
sensors_orientation = my_agent_state.sensor_states
return
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 convert_replica_coord(coord, rotation):
rotation_replica_habitat = quat_from_two_vectors(np.array([0, 0, -1]),
np.array([0, -1, 0]))
obj_rotation = quaternion.from_float_array([rotation[-1]] + rotation[:-1])
obj_coord = quat_rotate_vector(rotation_replica_habitat * obj_rotation,
coord)
return obj_coord
def _strafe_impl(scene_node: habitat_sim.SceneNode, forward_amount: float,
strafe_angle: float):
forward_ax = (
np.array(scene_node.absolute_transformation().rotation_scaling())
@ habitat_sim.geo.FRONT)
rotation = quat_from_angle_axis(np.deg2rad(strafe_angle),
habitat_sim.geo.UP)
move_ax = quat_rotate_vector(rotation, forward_ax)
scene_node.translate_local(move_ax * forward_amount)
def load_connectivity(connectivity_path, scenes=None):
file_format = connectivity_path + "{}_connectivity.json"
if scenes:
scans = scenes
else:
scans = read(connectivity_path + "scans.txt")
connectivity = {}
for scan in scans:
data = read_json(file_format.format(scan))
G = load_nav_graph(data)
distances = dict(nx.all_pairs_dijkstra_path_length(G))
paths = dict(nx.all_pairs_dijkstra_path(G))
positions = {}
visibility = {}
idxtoid = {}
idtoidx = {}
for i, item in enumerate(data):
idxtoid[i] = item['image_id']
idtoidx[item['image_id']] = i
pt_mp3d = np.array([
item['pose'][3], item['pose'][7],
item['pose'][11] - item['height']
])
q_habitat_mp3d = quat_from_two_vectors(np.array([0.0, 0.0, -1.0]),
habitat_sim.geo.GRAVITY)
pt_habitat = quat_rotate_vector(q_habitat_mp3d, pt_mp3d)
positions[item['image_id']] = pt_habitat.tolist()
visibility[item['image_id']] = {
"included": item["included"],
"visible": item["visible"],
"unobstructed": item["unobstructed"],
}
connectivity[scan] = {
"viewpoints": positions,
"distances": distances,
"paths": paths,
"visibility": visibility,
"idxtoid": idxtoid,
"idtoidx": idtoidx,
}
return connectivity
def inverse(self):
return SE3_Noise(self.rot.inverse(),
quat_rotate_vector(self.rot.inverse(), -self.trans))