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))