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
