def _compute_relative_pose(self, pan, tilt):
pan_link = 0.1 # length of pan link
tilt_link = 0.1 # length of tilt link
sensor_offset_tilt = np.asarray([0.0, 0.0, -1 * tilt_link])
quat_cam_to_pan = habUtils.quat_from_angle_axis(
-1 * tilt, np.asarray([1.0, 0.0, 0.0])
)
sensor_offset_pan = habUtils.quat_rotate_vector(
quat_cam_to_pan, sensor_offset_tilt
)
sensor_offset_pan += np.asarray([0.0, pan_link, 0.0])
quat_pan_to_base = habUtils.quat_from_angle_axis(
-1 * pan, np.asarray([0.0, 1.0, 0.0])
)
sensor_offset_base = habUtils.quat_rotate_vector(
quat_pan_to_base, sensor_offset_pan
)
sensor_offset_base += np.asarray([0.0, 0.5, 0.1]) # offset w.r.t base
# translation
quat = quat_cam_to_pan * quat_pan_to_base
return sensor_offset_base, quat.inverse()
def _fix_transform(self):
"""Return the fixed transform needed to correct habitat-sim agent co-ordinates"""
if self.transform is None:
self.transform = habUtils.quat_from_angle_axis(
np.pi / 2, np.asarray([0.0, 1.0, 0.0]))
self.transform = self.transform * habUtils.quat_from_angle_axis(
-np.pi / 2, np.asarray([1.0, 0.0, 0.0]))
return self.transform
def initialize_agent(self, agent_id, initial_state=None):
agent = self.get_agent(agent_id=agent_id)
if initial_state is None:
initial_state = AgentState()
initial_state.position = self._sim.pathfinder.get_random_navigable_point(
)
initial_state.rotation = utils.quat_from_angle_axis(
np.random.uniform(0, 2.0 * np.pi), np.array([0, 1, 0]))
agent.set_state(initial_state)
self._last_state = agent.state
return agent
def init_physics_test_scene(self, num_objects):
object_position = np.array([-0.569043, 2.04804,
13.6156]) # above the castle table
# turn agent toward the object
print("turning agent toward the physics!")
agent_state = self._sim.get_agent(0).get_state()
agent_to_obj = object_position - agent_state.position
agent_local_forward = np.array([0, 0, -1.0])
flat_to_obj = np.array([agent_to_obj[0], 0.0, agent_to_obj[2]])
flat_dist_to_obj = np.linalg.norm(flat_to_obj)
flat_to_obj /= flat_dist_to_obj
# move the agent closer to the objects if too far (this will be projected back to floor in set)
if flat_dist_to_obj > 3.0:
agent_state.position = object_position - flat_to_obj * 3.0
# unit y normal plane for rotation
det = (flat_to_obj[0] * agent_local_forward[2] -
agent_local_forward[0] * flat_to_obj[2])
turn_angle = math.atan2(det, np.dot(agent_local_forward, flat_to_obj))
agent_state.rotation = utils.quat_from_angle_axis(
turn_angle, np.array([0, 1.0, 0]))
# need to move the sensors too
for sensor in agent_state.sensor_states:
agent_state.sensor_states[sensor].rotation = agent_state.rotation
agent_state.sensor_states[
sensor].position = agent_state.position + np.array([0, 1.5, 0])
self._sim.get_agent(0).set_state(agent_state)
# hard coded dimensions of maximum bounding box for all 3 default objects:
max_union_bb_dim = np.array([0.125, 0.19, 0.26])
# add some objects in a grid
object_lib_size = self._sim.get_physics_object_library_size()
object_init_grid_dim = (3, 1, 3)
object_init_grid = {}
assert (
object_lib_size > 0
), "!!!No objects loaded in library, aborting object instancing example!!!"
for obj_id in range(num_objects):
rand_obj_index = random.randint(0, object_lib_size - 1)
# rand_obj_index = 0 # overwrite for specific object only
object_init_cell = (
random.randint(-object_init_grid_dim[0],
object_init_grid_dim[0]),
random.randint(-object_init_grid_dim[1],
object_init_grid_dim[1]),
random.randint(-object_init_grid_dim[2],
object_init_grid_dim[2]),
)
while object_init_cell in object_init_grid:
object_init_cell = (
random.randint(-object_init_grid_dim[0],
object_init_grid_dim[0]),
random.randint(-object_init_grid_dim[1],
object_init_grid_dim[1]),
random.randint(-object_init_grid_dim[2],
object_init_grid_dim[2]),
)
object_id = self._sim.add_object(rand_obj_index)
object_init_grid[object_init_cell] = object_id
object_offset = np.array([
max_union_bb_dim[0] * object_init_cell[0],
max_union_bb_dim[1] * object_init_cell[1],
max_union_bb_dim[2] * object_init_cell[2],
])
self._sim.set_translation(object_position + object_offset,
object_id)
print("added object: " + str(object_id) + " of type " +
str(rand_obj_index) + " at: " +
str(object_position + object_offset) + " | " +
str(object_init_cell))
