def example(self, vel, frame=1, folder=''): ''' vel : n x 6 velocity vector ''' vel[:, 2] = -vel[:, 2] # conventions Z axis vel[:, 1] = -vel[:, 1] # conventions Y axis vel[:, 5] = -vel[:, 5] # conventions Z axis vel[:, 4] = -vel[:, 4] # conventions Y axis #vel[:, 0] = -vel[:, 0] # conventions agent_id = default_sim_settings["default_agent"] agent = self._sim._default_agent color_img = None depth_img = None for i in range(vel.shape[0]): state = agent.get_state() self.agent_controller(agent, vel[i]) observations = self._sim.get_sensor_observations() if self.depth_type == 'FLOW': color_img, prev_color_img = self.save_color_observation( observations, frame, i + 1, folder) elif self.depth_type == 'TRUE': color_img = self.save_color_observation( observations, frame, i + 1, folder) depth_img = self.save_depth_observation(observations, frame, i + 1, folder) if self.depth_type == 'TRUE': return color_img, depth_img elif self.depth_type == 'FLOW': return color_img, prev_color_img, depth_img
def init_agent_state(self, agent_id, init_state): start_state = habitat_sim.agent.AgentState() x, y, z, w, p, q, r = init_state start_state.position = np.array([x, y, z]).astype('float32') start_state.rotation = np.quaternion(w, p, q, r) agent = self._sim.initialize_agent(agent_id, start_state) start_state = agent.get_state() return start_state
def get_agent_pose(self): agent = self._sim._default_agent state = agent.get_state() position = state.position rotation = state.rotation pose = [ position[0], position[1], position[2], rotation.w, rotation.x, rotation.y, rotation.z ] return pose
def init_agent_state(self, agent_id): # initialize the agent at a random start state agent = self._sim.initialize_agent(agent_id) start_state = agent.get_state() if (start_state.position != self.init_position).any(): start_state.position = self.init_position start_state.rotation = q.from_float_array(self.init_rotation) start_state.sensor_states = dict() ## Initialize sensor agent.set_state(start_state) return start_state
def init_agent_state(self, agent_id): # initialize the agent at a random start state agent = self._sim.initialize_agent(agent_id) start_state = agent.get_state() # force starting position on first floor (try 100 samples) num_start_tries = 0 while start_state.position[1] > 0.5 and num_start_tries < 100: start_state.position = self._sim.pathfinder.get_random_navigable_point() num_start_tries += 1 agent.set_state(start_state) if not self._sim_settings["silent"]: print( "start_state.position\t", start_state.position, "start_state.rotation\t", start_state.rotation, ) return start_state