def load_objs(name_obj_dat, sim, obj_ids, auto_sleep=True): """ - name_obj_dat: List[(str, List[ transformation as a 4x4 list of lists of floats, int representing the motion type ]) """ static_obj_ids = [] for i, (name, obj_dat) in enumerate(name_obj_dat): if len(obj_ids) == 0: obj_id = add_obj(name, sim) else: obj_id = obj_ids[i] trans = obj_dat[0] obj_type = obj_dat[1] use_trans = mn.Matrix4(trans) sim.set_transformation(use_trans, obj_id) sim.set_linear_velocity(mn.Vector3(0, 0, 0), obj_id) sim.set_angular_velocity(mn.Vector3(0, 0, 0), obj_id) sim.set_object_motion_type(MotionType(obj_type), obj_id) static_obj_ids.append(obj_id) if len(obj_ids) != 0: return obj_ids return static_obj_ids
def test_raycast(): cfg_settings = examples.settings.default_sim_settings.copy() # configure some settings in case defaults change cfg_settings[ "scene"] = "data/scene_datasets/habitat-test-scenes/apartment_1.glb" # enable the physics simulator cfg_settings["enable_physics"] = True # loading the physical scene hab_cfg = examples.settings.make_cfg(cfg_settings) with habitat_sim.Simulator(hab_cfg) as sim: obj_mgr = sim.get_object_template_manager() if (sim.get_physics_simulation_library() != habitat_sim.physics.PhysicsSimulationLibrary.NONE): # only test this if we have a physics simulator and therefore a collision world test_ray_1 = habitat_sim.geo.Ray() test_ray_1.direction = mn.Vector3(1.0, 0, 0) raycast_results = sim.cast_ray(test_ray_1) assert raycast_results.ray.direction == test_ray_1.direction assert raycast_results.has_hits() assert len(raycast_results.hits) == 1 assert np.allclose(raycast_results.hits[0].point, np.array([6.83063, 0, 0]), atol=0.07) assert np.allclose( raycast_results.hits[0].normal, np.array([-0.999587, 0.0222882, -0.0181424]), atol=0.07, ) assert abs(raycast_results.hits[0].ray_distance - 6.831) < 0.001 assert raycast_results.hits[0].object_id == -1 # add a primitive object to the world cube_prim_handle = obj_mgr.get_template_handles("cube")[0] cube_obj_id = sim.add_object_by_handle(cube_prim_handle) sim.set_translation(mn.Vector3(3.0, 0, 0), cube_obj_id) raycast_results = sim.cast_ray(test_ray_1) assert raycast_results.has_hits() assert len(raycast_results.hits) == 2 assert np.allclose(raycast_results.hits[0].point, np.array([2.89355, 0, 0]), atol=0.07) assert np.allclose( raycast_results.hits[0].normal, np.array([-0.998961, -0.0322245, -0.0322245]), atol=0.07, ) assert abs(raycast_results.hits[0].ray_distance - 2.8935) < 0.001 assert raycast_results.hits[0].object_id == 0
def test_scene_bounding_boxes(sim): cfg_settings = examples.settings.default_sim_settings.copy() cfg_settings["scene"] = "data/scene_datasets/habitat-test-scenes/van-gogh-room.glb" hab_cfg = examples.settings.make_cfg(cfg_settings) sim.reconfigure(hab_cfg) scene_graph = sim._sim.get_active_scene_graph() root_node = scene_graph.get_root_node() root_node.compute_cumulative_bb() scene_bb = root_node.cumulative_bb ground_truth = mn.Range3D.from_size( mn.Vector3(-0.775869, -0.0233012, -1.6706), mn.Vector3(6.76937, 3.86304, 3.5359) ) assert ground_truth == scene_bb
def visualize_position(self, position, viz_id=None, r=0.05): """Adds the sphere object to the specified position for visualization purpose.""" if viz_id is None: obj_mgr = self.get_object_template_manager() template = obj_mgr.get_template_by_handle( obj_mgr.get_template_handles("sphere")[0]) template.scale = mn.Vector3(r, r, r) new_template_handle = obj_mgr.register_template( template, "ball_new_viz") viz_id = self.add_object(new_template_handle) make_render_only(viz_id, self) self.set_translation(mn.Vector3(*position), viz_id) return viz_id
def test_unproject(sim): cfg_settings = examples.settings.default_sim_settings.copy() # configure some settings in case defaults change cfg_settings["scene"] = "data/scene_datasets/habitat-test-scenes/apartment_1.glb" cfg_settings["width"] = 101 cfg_settings["height"] = 101 cfg_settings["sensor_height"] = 0 cfg_settings["color_sensor"] = True # loading the scene hab_cfg = examples.settings.make_cfg(cfg_settings) sim.reconfigure(hab_cfg) # position agent sim.agents[0].scene_node.rotation = mn.Quaternion() sim.agents[0].scene_node.translation = mn.Vector3(0.5, 0, 0) # setup camera visual_sensor = sim._sensors["color_sensor"] scene_graph = sim.get_active_scene_graph() scene_graph.set_default_render_camera_parameters(visual_sensor._sensor_object) render_camera = scene_graph.get_default_render_camera() # test unproject center_ray = render_camera.unproject(mn.Vector2i(50, 50)) # middle of the viewport assert np.allclose(center_ray.origin, np.array([0.5, 0, 0]), atol=0.07) assert np.allclose(center_ray.direction, np.array([0, 0, -1.0]), atol=0.02) test_ray_2 = render_camera.unproject( mn.Vector2i(100, 100) ) # bottom right of the viewport assert np.allclose( test_ray_2.direction, np.array([0.569653, -0.581161, -0.581161]), atol=0.07 )
def update(self) -> None: """Updates the camera transformations and performs necessary checks on joint limits and sleep states. """ agent_node = self._sim._default_agent.scene_node inv_T = agent_node.transformation.inverted() for cam_prefix, sensor_names in self._cameras.items(): for sensor_name in sensor_names: sens_obj = self._sim._sensors[sensor_name]._sensor_object cam_info = self.params.cameras[cam_prefix] if cam_info.attached_link_id == -1: link_trans = self.sim_obj.transformation else: link_trans = self.sim_obj.get_link_scene_node( self.params.ee_link ).transformation cam_transform = mn.Matrix4.look_at( cam_info.cam_offset_pos, cam_info.cam_look_at_pos, mn.Vector3(0, 1, 0), ) cam_transform = link_trans @ cam_transform @ cam_info.relative_transform cam_transform = inv_T @ cam_transform sens_obj.node.transformation = cam_transform # Guard against out of limit joints # TODO: should auto clamping be enabled instead? How often should we clamp? if self._limit_robo_joints: self.sim_obj.clamp_joint_limits() self.sim_obj.awake = True
def add_obj(name, sim): if "/BOX_" in name: size_parts = name.split("/BOX_")[-1] size_parts = size_parts.split("_") if len(size_parts) == 1: size_x = size_y = size_z = float(size_parts[0]) else: size_x = float(size_parts[0]) size_y = float(size_parts[1]) size_z = float(size_parts[2]) obj_mgr = sim.get_object_template_manager() template_handle = obj_mgr.get_template_handles("cube")[0] template = obj_mgr.get_template_by_handle(template_handle) template.scale = mn.Vector3(size_x, size_y, size_z) template.requires_lighting = True new_template_handle = obj_mgr.register_template(template, "box_new") obj_id = sim.add_object(new_template_handle) sim.set_object_motion_type(MotionType.DYNAMIC, obj_id) return obj_id PROP_FILE_END = ".object_config.json" use_name = name + PROP_FILE_END obj_id = sim.add_object_by_handle(use_name) return obj_id
def _rotate_local( scene_node: SceneNode, theta: float, axis: int, constraint: Optional[float] = None ): if constraint is not None: rotation = scene_node.rotation if abs(float(rotation.angle())) > 0: ref_vector = mn.Vector3() ref_vector[axis] = 1 if mn.math.angle(ref_vector, rotation.axis().normalized()) > mn.Rad(1e-3): raise RuntimeError( "Constrained look only works for a singular look action type" ) look_vector = rotation.transform_vector(FRONT) if axis == 0: look_angle = mn.Rad(np.arctan2(look_vector[1], -look_vector[2])) elif axis == 1: look_angle = -mn.Rad(np.arctan2(look_vector[0], -look_vector[2])) new_angle = look_angle + mn.Deg(theta) constraint = mn.Deg(constraint) if new_angle > constraint: theta = constraint - look_angle elif new_angle < -constraint: theta = -constraint - look_angle _rotate_local_fns[axis](scene_node, mn.Deg(theta)) scene_node.rotation = scene_node.rotation.normalized()
def __init__(self, urdf_path, sim, limit_robo_joints=True, fixed_base=True): fetch_params = MobileManipulatorParams( arm_joints=list(range(15, 22)), gripper_joints=[23, 24], wheel_joints=[2, 4], arm_init_params=[-0.45, -1.08, 0.1, 0.935, -0.001, 1.573, 0.005], gripper_init_params=[0.00, 0.00], ee_offset=mn.Vector3(0.08, 0, 0), ee_link=22, ee_constraint=np.array([[0.4, 1.2], [-0.7, 0.7], [0.25, 1.5]]), cameras={ "robot_arm": RobotCameraParams( cam_offset_pos=mn.Vector3(0, 0.0, 0.1), cam_look_at_pos=mn.Vector3(0.1, 0.0, 0.0), attached_link_id=22, relative_transform=mn.Matrix4.rotation_y(mn.Deg(-90)) @ mn.Matrix4.rotation_z(mn.Deg(90)), ), "robot_head": RobotCameraParams( cam_offset_pos=mn.Vector3(0.17, 1.2, 0.0), cam_look_at_pos=mn.Vector3(0.75, 1.0, 0.0), attached_link_id=-1, ), "robot_third": RobotCameraParams( cam_offset_pos=mn.Vector3(-0.5, 1.7, -0.5), cam_look_at_pos=mn.Vector3(1, 0.0, 0.75), attached_link_id=-1, ), }, gripper_closed_state=[0.0, 0.0], gripper_open_state=[0.04, 0.04], gripper_state_eps=0.001, arm_mtr_pos_gain=0.3, arm_mtr_vel_gain=0.3, arm_mtr_max_impulse=10.0, wheel_mtr_pos_gain=0.0, wheel_mtr_vel_gain=1.3, wheel_mtr_max_impulse=10.0, base_offset=mn.Vector3(0, 0, 0), base_link_names={ "base_link", "r_wheel_link", "l_wheel_link", "r_wheel_link", "bellows_link", "bellows_link2", "estop_link", "laser_link", "torso_fixed_link", }, ) super().__init__(fetch_params, urdf_path, sim, limit_robo_joints, fixed_base) self.back_joint_id = 6 self.head_rot_jid = 8 self.head_tilt_jid = 9
def draw_sphere(self, r, template_name="ball_new"): obj_mgr = self.get_object_template_manager() template_handle = obj_mgr.get_template_handles("sphere")[0] template = obj_mgr.get_template_by_handle(template_handle) template.scale = mn.Vector3(r, r, r) new_template_handle = obj_mgr.register_template(template, "ball_new") obj_id = self.add_object(new_template_handle) self.set_object_motion_type(MotionType.KINEMATIC, obj_id) return obj_id
def test_scene_bounding_boxes(): cfg_settings = examples.settings.default_sim_settings.copy() cfg_settings["scene"] = "data/scene_datasets/habitat-test-scenes/van-gogh-room.glb" hab_cfg = examples.settings.make_cfg(cfg_settings) hab_cfg_mm = examples.settings.make_cfg(cfg_settings) mm = habitat_sim.metadata.MetadataMediator(hab_cfg.sim_cfg) hab_cfg_mm.metadata_mediator = mm test_list = [hab_cfg, hab_cfg_mm] for ctor_arg in test_list: with habitat_sim.Simulator(ctor_arg) as sim: scene_graph = sim.get_active_scene_graph() root_node = scene_graph.get_root_node() root_node.compute_cumulative_bb() scene_bb = root_node.cumulative_bb ground_truth = mn.Range3D.from_size( mn.Vector3(-0.775869, -0.0233012, -1.6706), mn.Vector3(6.76937, 3.86304, 3.5359), ) assert ground_truth == scene_bb
def update(self) -> None: """Updates the camera transformations and performs necessary checks on joint limits and sleep states. """ agent_node = self._sim._default_agent.scene_node inv_T = agent_node.transformation.inverted() # Update the cameras for sensor_name in self._head_sensor_names: sens_obj = self._sim._sensors[sensor_name]._sensor_object look_at = self.sim_obj.transformation.transform_point( self.params.head_cam_look_pos) cam_pos = self.sim_obj.transformation.transform_point( self.params.head_cam_offset_pos) head_T = mn.Matrix4.look_at(cam_pos, look_at, mn.Vector3(0, 1, 0)) sens_obj.node.transformation = inv_T @ head_T for sensor_name in self._third_sensor_names: sens_obj = self._sim._sensors[sensor_name]._sensor_object look_at = self.sim_obj.transformation.transform_point( self.params.third_cam_look_pos) cam_pos = self.sim_obj.transformation.transform_point( self.params.third_cam_offset_pos) third_T = mn.Matrix4.look_at(cam_pos, look_at, mn.Vector3(0, 1, 0)) sens_obj.node.transformation = inv_T @ third_T for sensor_name in self._arm_sensor_names: sens_obj = self._sim._sensors[sensor_name]._sensor_object arm_T = self._get_arm_cam_transform() sens_obj.node.transformation = inv_T @ arm_T # Guard against out of limit joints # TODO: should auto clamping be enabled instead? How often should we clamp? if self._limit_robo_joints: self.sim_obj.clamp_joint_limits() self.sim_obj.awake = True
def _get_target_trans(self): """ This is how the target transforms should be accessed since multiprocessing does not allow pickling. """ # Preprocess the ep_info making necessary datatype conversions. target_trans = [] for i in range(len(self.ep_info["targets"])): targ_idx, trans = self.ep_info["targets"][i] if len(trans) == 3: # Legacy position only format. trans = mn.Matrix4.translation(mn.Vector3(*trans)) else: trans = mn.Matrix4(trans) target_trans.append((targ_idx, trans)) return target_trans
def place_robot_from_agent( sim: habitat_sim.Simulator, robot: habitat_sim._ext.habitat_sim_bindings.ManagedBulletArticulatedObject, local_base_pos: list, orientation_vector: list, angle_correction: float, ) -> None: """Moves robot to reasonable transformation relative to agent.""" local_base_pos = np.array(local_base_pos) # place the robot root state relative to the agent agent_transform = sim.agents[0].scene_node.transformation_matrix() base_transform = mn.Matrix4.rotation( mn.Rad(angle_correction), mn.Vector3(*orientation_vector) ) base_transform.translation = agent_transform.transform_point(local_base_pos) robot.transformation = base_transform
def snap_to_obj(self, snap_obj_id: int, force: bool = True): """ :param snap_obj_id: Simulator object index. :force: Will transform the object to be in the robot's grasp, even if the object is already in the grasped state. """ if len(self._snap_constraints) != 0: # We were already grabbing something else. raise ValueError( f"Tried snapping to {snap_obj_id} when already snapped to {self._snapped_obj_id}" ) if force: # Set the transformation to be in the robot's hand already. self._sim.set_transformation( self._sim.robot.ee_transform, snap_obj_id ) if snap_obj_id == self._snapped_obj_id: # Already grasping this object. return self._snapped_obj_id = snap_obj_id # Set collision group to GraspedObject so that it doesn't collide # with the links of the robot. self.snap_rigid_obj.override_collision_group( CollisionGroups.UserGroup7 ) def create_hold_constraint(pivot_in_link, pivot_in_obj): c = RigidConstraintSettings() c.object_id_a = self._sim.robot.get_robot_sim_id() c.link_id_a = self._sim.robot.ee_link_id c.object_id_b = self._snapped_obj_id c.pivot_a = pivot_in_link c.pivot_b = pivot_in_obj c.max_impulse = self._config.GRASP_IMPULSE return self._sim.create_rigid_constraint(c) self._snap_constraints = [ create_hold_constraint(mn.Vector3(0.1, 0, 0), mn.Vector3(0, 0, 0)), create_hold_constraint( mn.Vector3(0.0, 0, 0), mn.Vector3(-0.1, 0, 0) ), create_hold_constraint( mn.Vector3(0.1, 0.0, 0.1), mn.Vector3(0.0, 0.0, 0.1) ), ] if any((x == -1 for x in self._snap_constraints)): raise ValueError("Created bad constraint")
def convert_fn(obj_dat): fname = "/".join(obj_dat[0].split("/")[-2:]) if ".urdf" in fname: obj_dat[0] = osp.join("data/replica_cad/urdf", fname) else: obj_dat[0] = obj_dat[0].replace("data/objects/", "data/objects/ycb/") if (len(obj_dat) == 2 and len(obj_dat[1]) == 4 and np.array(obj_dat[1]).shape == (4, 4)): # Specifies the full transformation, no object type return (obj_dat[0], (obj_dat[1], int(MotionType.DYNAMIC))) elif len(obj_dat) == 2 and len(obj_dat[1]) == 3: # Specifies XYZ, no object type trans = mn.Matrix4.translation(mn.Vector3(obj_dat[1])) return (obj_dat[0], (trans, int(MotionType.DYNAMIC))) else: # Specifies the full transformation and the object type return (obj_dat[0], obj_dat[1])
def test_scene_attributes_managers(sim): cfg_settings = examples.settings.default_sim_settings.copy() cfg_settings["scene"] = "data/scene_datasets/habitat-test-scenes/van-gogh-room.glb" cfg_settings["enable_physics"] = True hab_cfg = examples.settings.make_cfg(cfg_settings) sim.reconfigure(hab_cfg) scene_name = cfg_settings["scene"] # get attribute managers scene_mgr = sim.get_scene_template_manager() # perform general tests for this attributes manager template0, _ = perform_general_tests(scene_mgr, scene_name) # verify gravity in template is as expected assert template0.gravity == mn.Vector3(0.0, -9.8, 0.0) # verify creating new template perform_add_blank_template_test(scene_mgr, template0.render_asset_handle)
def simulate_with_moving_agent( sim, duration=1.0, agent_vel=np.array([0, 0, 0]), look_rotation_vel=0.0, get_frames=True, ): sensor_node = sim._sensors["rgba_camera"]._sensor_object.object agent_node = sim.get_agent(0).body.object # simulate dt seconds at 60Hz to the nearest fixed timestep time_step = 1.0 / 60.0 rotation_x = mn.Quaternion.rotation( mn.Deg(look_rotation_vel) * time_step, mn.Vector3(1.0, 0, 0)) print("Simulating " + str(duration) + " world seconds.") observations = [] start_time = sim.get_world_time() while sim.get_world_time() < start_time + duration: # move agent agent_node.translation += agent_vel * time_step # rotate sensor sensor_node.rotation *= rotation_x # Add user transforms for the agent and sensor. We'll use these later during # replay playback. gfx_replay_utils.add_node_user_transform(sim, agent_node, "agent") gfx_replay_utils.add_node_user_transform(sim, sensor_node, "sensor") sim.step_physics(time_step) # save a replay keyframe after every physics step sim.gfx_replay_manager.save_keyframe() if get_frames: observations.append(sim.get_sensor_observations()) return observations
def test_constrainted( scene_graph, control_name, control_axis, actuation_amount, actuation_constraint ): initial_look_angle = mn.Deg( np.random.uniform(-actuation_constraint, actuation_constraint) ) rotation_vector = mn.Vector3() rotation_vector[control_axis] = 1 initial_rotation = mn.Quaternion.rotation( mn.Rad(initial_look_angle), rotation_vector ) node = scene_graph.get_root_node().create_child() node.rotation = initial_rotation spec = habitat_sim.agent.controls.ActuationSpec( actuation_amount, actuation_constraint ) habitat_sim.registry.get_move_fn(control_name)(node, spec) expected_angle = initial_look_angle + mn.Deg( -actuation_amount if control_name in {"look_down", "look_right"} else actuation_amount ) if expected_angle > mn.Deg(actuation_constraint): expected_angle = mn.Deg(actuation_constraint) elif expected_angle < mn.Deg(-actuation_constraint): expected_angle = mn.Deg(-actuation_constraint) final_rotation = node.rotation look_vector = final_rotation.transform_vector(habitat_sim.geo.FRONT) if control_axis == 0: look_angle = mn.Deg(mn.Rad(np.arctan2(look_vector[1], -look_vector[2]))) elif control_axis == 1: look_angle = -mn.Deg(mn.Rad(np.arctan2(look_vector[0], -look_vector[2]))) assert np.abs(float(expected_angle - look_angle)) < 1e-1
def set_object_in_front_of_agent(self, sim, obj_id, z_offset=-1.5): r""" Adds an object in front of the agent at some distance. """ #print("Agent : ") #print(self.get_agent(0)) #agent_transform = sim.agents[0].scene_node.transformation_matrix() agent_transform = self.get_agent(0).scene_node.transformation_matrix() obj_translation = agent_transform.transform_point( np.array([0, 0, z_offset])) sim.set_translation(obj_translation, obj_id) obj_node = sim.get_object_scene_node(obj_id) xform_bb = habitat_sim.geo.get_transformed_bb(obj_node.cumulative_bb, obj_node.transformation) # also account for collision margin of the scene scene_collision_margin = 0.04 y_translation = mn.Vector3( 0, xform_bb.size_y() / 2.0 + scene_collision_margin, 0) sim.set_translation(y_translation + sim.get_translation(obj_id), obj_id)
def update(env, state, a, delta, vel_control, time_step): dt = time_step # angle = quaternion.as_euler_angles(env.habitat_env._sim.get_agent_state().rotation) agent_forward = quat_to_magnum( env.habitat_env._sim.get_agent_state().rotation).transform_vector( mn.Vector3(0, 0, -1.0)) # print("linear velocity", state.v) # print("angular velocity", state.yaw) state.x = env.habitat_env._sim.get_agent_state().position[0] state.y = -env.habitat_env._sim.get_agent_state().position[2] # state.yaw = -angle[1] state.yaw = math.atan2(agent_forward[0], agent_forward[2]) vel_control.linear_velocity = np.array([0, 0, -a]) # local up is y vel_control.angular_velocity = np.array([0, delta, 0]) print("lin", a) print("vel", delta) obs, reward, done, info = env.step(vel_control) return obs, info, state
def place_viz_objs(name_trans, sim, obj_ids): viz_obj_ids = [] for i, (_, assoc_obj_idx, trans) in enumerate(name_trans): if len(obj_ids) == 0: obj_bb = get_aabb(assoc_obj_idx, sim, False) obj_mgr = sim.get_object_template_manager() template = obj_mgr.get_template_by_handle( obj_mgr.get_template_handles("cubeWireframe")[0] ) template.scale = ( mn.Vector3(obj_bb.size_x(), obj_bb.size_y(), obj_bb.size_z()) / 2.0 ) new_template_handle = obj_mgr.register_template( template, "new_obj_viz" ) viz_obj_id = sim.add_object(new_template_handle) else: viz_obj_id = obj_ids[i] make_render_only(viz_obj_id, sim) sim.set_transformation(trans, viz_obj_id) viz_obj_ids.append(viz_obj_id) return viz_obj_ids