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