diff --git a/.circleci/config.yml b/.circleci/config.yml index 0bab9da156..bb4c0c7e61 100644 --- a/.circleci/config.yml +++ b/.circleci/config.yml @@ -298,6 +298,9 @@ jobs: wget http://dl.fbaipublicfiles.com/habitat/objects_v0.1.zip unzip objects_v0.1.zip -d data/objects/ rm objects_v0.1.zip + wget http://dl.fbaipublicfiles.com/habitat/locobot_merged.zip + unzip locobot_merged.zip -d data/objects/ + rm locobot_merged.zip cd .. fi - run: diff --git a/docs/conf.py b/docs/conf.py index ee63939b0d..77befd0aeb 100644 --- a/docs/conf.py +++ b/docs/conf.py @@ -38,6 +38,7 @@ "pages/lighting-setups.rst", "pages/notebooks.rst", "pages/image-extractor.rst", + "pages/rigid-object-tutorial.rst", ] PLUGINS = [ diff --git a/docs/pages/images/rigid-object-tutorial-images/dynamic_control.gif b/docs/pages/images/rigid-object-tutorial-images/dynamic_control.gif new file mode 100644 index 0000000000..63b4f2c19d Binary files /dev/null and b/docs/pages/images/rigid-object-tutorial-images/dynamic_control.gif differ diff --git a/docs/pages/images/rigid-object-tutorial-images/kinematic_interactions.gif b/docs/pages/images/rigid-object-tutorial-images/kinematic_interactions.gif new file mode 100644 index 0000000000..be607c0b8d Binary files /dev/null and b/docs/pages/images/rigid-object-tutorial-images/kinematic_interactions.gif differ diff --git a/docs/pages/images/rigid-object-tutorial-images/kinematic_update.gif b/docs/pages/images/rigid-object-tutorial-images/kinematic_update.gif new file mode 100644 index 0000000000..2e1b10566b Binary files /dev/null and b/docs/pages/images/rigid-object-tutorial-images/kinematic_update.gif differ diff --git a/docs/pages/images/rigid-object-tutorial-images/local_velocity_control.gif b/docs/pages/images/rigid-object-tutorial-images/local_velocity_control.gif new file mode 100644 index 0000000000..5b42d39710 Binary files /dev/null and b/docs/pages/images/rigid-object-tutorial-images/local_velocity_control.gif differ diff --git a/docs/pages/images/rigid-object-tutorial-images/robot_control.gif b/docs/pages/images/rigid-object-tutorial-images/robot_control.gif new file mode 100644 index 0000000000..dcba246b5b Binary files /dev/null and b/docs/pages/images/rigid-object-tutorial-images/robot_control.gif differ diff --git a/docs/pages/images/rigid-object-tutorial-images/sim_basics.gif b/docs/pages/images/rigid-object-tutorial-images/sim_basics.gif new file mode 100644 index 0000000000..a0dfdcc8bd Binary files /dev/null and b/docs/pages/images/rigid-object-tutorial-images/sim_basics.gif differ diff --git a/docs/pages/images/rigid-object-tutorial-images/velocity_control.gif b/docs/pages/images/rigid-object-tutorial-images/velocity_control.gif new file mode 100644 index 0000000000..525a7674d2 Binary files /dev/null and b/docs/pages/images/rigid-object-tutorial-images/velocity_control.gif differ diff --git a/docs/pages/index.rst b/docs/pages/index.rst index 95072819c8..a83d651517 100644 --- a/docs/pages/index.rst +++ b/docs/pages/index.rst @@ -19,6 +19,7 @@ over 10,000 FPS multi-process on a single GPU! - :ref:`Creating a stereo agent ` - :ref:`Working with light setups ` - :ref:`Extracting Images ` +- :ref:`Interactive Rigid Objects ` .. note-warning:: diff --git a/docs/pages/rigid-object-tutorial.rst b/docs/pages/rigid-object-tutorial.rst new file mode 100644 index 0000000000..a3a321eb40 --- /dev/null +++ b/docs/pages/rigid-object-tutorial.rst @@ -0,0 +1,196 @@ +:ref-prefix: + habitat_sim.simulator + habitat_sim.sim + habitat_sim.agent + habitat_sim.attributes + habitat_sim.scene + +Interactive Rigid Objects +######################### + +:summary: This tutorial demonstrates rigid object interactions in Habitat-sim -- instancing, dynamic simulation, and kinematic manipulation. + +.. contents:: + :class: m-block m-default + +First, download the `example objects`_ and extract them into path/to/habitat-sim/data/objects/. + +.. _example objects: http://dl.fbaipublicfiles.com/habitat/objects_v0.1.zip + +The example code below is runnable via: + +.. code:: shell-session + + $ python path/to/habitat-sim/examples/tutorials/rigid_object_tutorial.py + +Import necessary modules, define some convenience functions, and initialize the :ref:`Simulator` and :ref:`Agent`. + +.. include:: ../../examples/tutorials/rigid_object_tutorial.py + :code: py + :start-after: # [setup] + :end-before: # [/setup] + +.. include:: ../../examples/tutorials/rigid_object_tutorial.py + :code: py + :start-after: # [initialize] + :end-before: # [/initialize] + +`Simulation Quickstart`_ +======================== + +Basic rigid body simulation can be achieved by simply loading a template, instancing an object, and stepping the physical world. +In this example, a sphere object template is loaded and the object is instanced in the scene above the table. +When the simulation is stepped, it falls under the force of gravity and reacts to collisions with the scene. + +.. include:: ../../examples/tutorials/rigid_object_tutorial.py + :code: py + :start-after: # [basics] + :end-before: # [/basics] + +.. image:: images/rigid-object-tutorial-images/sim_basics.gif + :width: 20em + +Forces and torques can be applied to the object with :ref:`Simulator.apply_force` and :ref:`Simulator.apply_torque`. +Instantanious initial velocities can also be set with :ref:`Simulator.set_linear_velocity` and :ref:`Simulator.set_angular_velocity`. + +In the example below, a constant anti-gravity force is applied to the boxes' centers of mass (COM) causing them to float in the air. +A constant torque is also applied which gradually increases the angular velocity of the boxes. +A sphere is then thrown at the boxes by applying an initial velocity. + +Note that forces and torques are treated as constant within each call to :ref:`Simulator.step_physics` and are cleared afterward. + +.. include:: ../../examples/tutorials/rigid_object_tutorial.py + :code: py + :start-after: # [dynamic_control] + :end-before: # [/dynamic_control] + +.. image:: images/rigid-object-tutorial-images/dynamic_control.gif + :width: 20em + + +`Kinematic Object Placement`_ +============================= + +Often it is enough to set the desired state of an object directly. +In these cases the computational overhead of running full dynamic simulation may not be necessary to achieve a desired result. +Setting the object to :ref:`habitat_sim.physics.MotionType.KINEMATIC` with :ref:`Simulator.set_object_motion_type` specifies that the object's state will be directly controlled. + +In the example below, a kinematic can is placed in the scene which will not react to physical events such as collision with dynamically simulated objects. +However, it will still act as a collision object for other scene objects as in the following example. + +.. include:: ../../examples/tutorials/rigid_object_tutorial.py + :code: py + :start-after: # [kinematic_interactions] + :end-before: # [/kinematic_interactions] + +.. image:: images/rigid-object-tutorial-images/kinematic_interactions.gif + :width: 20em + + +`Kinematic Velocity Control`_ +============================= + + +To move a kinematic object, the state can be set directly before each simulation step. +This is useful for synchronizing the simulation state of objects to a known state such as a dataset trajectory, input device, or motion capture. + +.. include:: ../../examples/tutorials/rigid_object_tutorial.py + :code: py + :start-after: # [kinematic_update] + :end-before: # [/kinematic_update] + +.. image:: images/rigid-object-tutorial-images/kinematic_update.gif + :width: 20em + +However, when applying model or algorithmic control it is more convenient to specify a constant linear and angular velocity for the object which will be simulated without manual integration. +The object's :ref:`habitat_sim.physics.VelocityControl` structure provides this functionality and can be acquired via :ref:`Simulator.get_object_velocity_control`. +Once paramters are set, control takes affect immediately on the next simulation step as shown in the following example. + +.. include:: ../../examples/tutorials/rigid_object_tutorial.py + :code: py + :start-after: # [velocity_control] + :end-before: # [/velocity_control] + +.. image:: images/rigid-object-tutorial-images/velocity_control.gif + :width: 20em + +Velocities can also be specified in the local space of the object to easily apply velocity control for continuous agent actions. + +.. include:: ../../examples/tutorials/rigid_object_tutorial.py + :code: py + :start-after: # [local_velocity_control] + :end-before: # [/local_velocity_control] + +.. image:: images/rigid-object-tutorial-images/local_velocity_control.gif + :width: 20em + +`Embodied Agents`_ +================== + +For this tutorial section, you will need to download the `merged locobot asset`_ and extract it into path/to/habitat-sim/data/objects/ + +.. _merged locobot asset: http://dl.fbaipublicfiles.com/habitat/locobot_merged.zip + +Previous stages of this tutorial have covered adding objects to the world and manipulating them by setting positions, velocity, forces, and torques. +In all of these examples, the agent has been a passive onlooker observing the scene. +However, the agent can also be attached to a simulated object for embodiement and control. +This can be done by passing the :ref:`Agent`'s scene node to the :ref:`Simulator.add_object` function. + +In this example, the agent is embodied by a rigid robot asset and the :ref:`habitat_sim.physics.VelocityControl` structure is used to control the robot's actions. + +.. include:: ../../examples/tutorials/rigid_object_tutorial.py + :code: py + :start-after: # [embodied_agent] + :end-before: # [/embodied_agent] + +.. image:: images/rigid-object-tutorial-images/robot_control.gif + :width: 20em + +`Feature Detail Review`_ +======================== + +Adding/Removing Objects +*********************** + +Objects can be instanced from templates (i.e. :ref:`PhysicsObjectAttributes`) into the scene by template id with :ref:`Simulator.add_object` or by template string key with :ref:`Simulator.add_object_by_handle`. +These functions return a unique id which can be used to refer to the object instance. In the case of errors in construction, -1 is returned. + +By default, a new :ref:`SceneNode` will be created when an object is instanced. However, the object can be attached to an existing :ref:`SceneNode` (e.g. that of the :ref:`Agent`) if provided. This is demonstrated in `Embodied Agents`_. + +Object instances can be removed by id with :ref:`Simulator.remove_object`. Optionally, the object's :ref:`SceneNode` can be left behind in the :ref:`SceneGraph` when it is removed (e.g. to prevent deletion of an embodied :ref:`Agent`'s :ref:`SceneNode`). + +:ref:`Simulator.get_existing_object_ids` will return a list of unique object ids for all objects instanced in the scene. + +MotionType +********** + +Objects can be configured to fill different roles in a simulated scene by assigning a :ref:`habitat_sim.physics.MotionType`: + +- :ref:`habitat_sim.physics.MotionType.DYNAMIC` + + Dynamic object states are driven by simulation. These objects are affected by scene forces such as gravity, collision impulses, and programmatically applied forces and torques. + + Constant forces and torques can be applied to these objects with :ref:`Simulator.apply_force` and :ref:`Simulator.apply_torque`. + These are cleared after each call to :ref:`Simulator.step_physics`. + + Instantanious initial velocities can also be set for these objects with :ref:`Simulator.set_linear_velocity` and :ref:`Simulator.set_angular_velocity`. + +- :ref:`habitat_sim.physics.MotionType.KINEMATIC` + + Kinematic object states are not affected by scene dynamics, but can be set directly via :ref:`Simulator.set_transformation`, :ref:`Simulator.set_rotation`, and :ref:`Simulator.set_translation`. + +- :ref:`habitat_sim.physics.MotionType.STATIC` + + Static object states are not expected to change and cannot be affected by scene dynamics or programmatic state setters. + +VelocityControl +*************** + +Each object's :ref:`habitat_sim.physics.VelocityControl` structure provides a simple interface for setting up continuous velocity control of the object in either the global or local coordinate frame. +This can be queried from the simulator with :ref:`Simulator.get_object_velocity_control`. + +For :ref:`habitat_sim.physics.MotionType.KINEMATIC` objects, velocity control will directly modify the object's rigid state. + +For :ref:`habitat_sim.physics.MotionType.DYNAMIC` object, velocity control will set the initial velocity of the object before simualting. +In this case, velocity will be more accurate with smaller timestep requests to :ref:`Simulator.step_physics`. +Note that dynamics such as forces, collisions, and gravity will affect these objects, but expect extreme damping as velocities are being manually set before each timestep when controlled. diff --git a/examples/tutorials/rigid_object_tutorial.py b/examples/tutorials/rigid_object_tutorial.py new file mode 100644 index 0000000000..465bdaa9da --- /dev/null +++ b/examples/tutorials/rigid_object_tutorial.py @@ -0,0 +1,388 @@ +# [setup] +import math +import os + +import cv2 +import magnum as mn +import numpy as np + +import habitat_sim +import habitat_sim.utils.common as ut + +dir_path = os.path.dirname(os.path.realpath(__file__)) +data_path = os.path.join(dir_path, "../../data") +output_path = os.path.join(dir_path, "rigid_object_tutorial_output/") + + +def make_video_cv2(observations, prefix="", open_vid=True, multi_obs=False): + videodims = (720, 540) + fourcc = cv2.VideoWriter_fourcc("m", "p", "4", "v") + video = cv2.VideoWriter(output_path + prefix + ".mp4", fourcc, 60, videodims) + thumb_size = (int(videodims[0] / 5), int(videodims[1] / 5)) + outline_frame = np.ones((thumb_size[1] + 2, thumb_size[0] + 2, 3), np.uint8) * 150 + for ob in observations: + + # If in RGB/RGBA format, change first to RGB and change to BGR + bgr_im_1st_person = ob["rgba_camera_1stperson"][..., 0:3][..., ::-1] + + if multi_obs: + # embed the 1st person RBG frame into the 3rd person frame + bgr_im_3rd_person = ob["rgba_camera_3rdperson"][..., 0:3][..., ::-1] + resized_1st_person_rgb = cv2.resize( + bgr_im_1st_person, thumb_size, interpolation=cv2.INTER_AREA + ) + x_offset = 50 + y_offset_rgb = 50 + bgr_im_3rd_person[ + y_offset_rgb - 1 : y_offset_rgb + outline_frame.shape[0] - 1, + x_offset - 1 : x_offset + outline_frame.shape[1] - 1, + ] = outline_frame + bgr_im_3rd_person[ + y_offset_rgb : y_offset_rgb + resized_1st_person_rgb.shape[0], + x_offset : x_offset + resized_1st_person_rgb.shape[1], + ] = resized_1st_person_rgb + + # embed the 1st person DEPTH frame into the 3rd person frame + # manually normalize depth into [0, 1] so that images are always consistent + d_im = np.clip(ob["depth_camera_1stperson"], 0, 10) + d_im /= 10.0 + bgr_d_im = cv2.cvtColor((d_im * 255).astype(np.uint8), cv2.COLOR_GRAY2BGR) + resized_1st_person_depth = cv2.resize( + bgr_d_im, thumb_size, interpolation=cv2.INTER_AREA + ) + y_offset_d = y_offset_rgb + 10 + thumb_size[1] + bgr_im_3rd_person[ + y_offset_d - 1 : y_offset_d + outline_frame.shape[0] - 1, + x_offset - 1 : x_offset + outline_frame.shape[1] - 1, + ] = outline_frame + bgr_im_3rd_person[ + y_offset_d : y_offset_d + resized_1st_person_depth.shape[0], + x_offset : x_offset + resized_1st_person_depth.shape[1], + ] = resized_1st_person_depth + + # write the video frame + video.write(bgr_im_3rd_person) + else: + # write the 1st person observation to video + video.write(bgr_im_1st_person) + video.release() + if open_vid: + os.system("open " + output_path + prefix + ".mp4") + + +def remove_all_objects(sim): + for id in sim.get_existing_object_ids(): + sim.remove_object(id) + + +def place_agent(sim): + # place our agent in the scene + agent_state = habitat_sim.AgentState() + agent_state.position = [-0.15, -0.7, 1.0] + # agent_state.position = [-0.15, -1.6, 1.0] + agent_state.rotation = np.quaternion(-0.83147, 0, 0.55557, 0) + agent = sim.initialize_agent(0, agent_state) + return agent.scene_node.transformation_matrix() + + +def make_configuration(): + # simulator configuration + backend_cfg = habitat_sim.SimulatorConfiguration() + backend_cfg.scene.id = "data/scene_datasets/habitat-test-scenes/apartment_1.glb" + backend_cfg.enable_physics = True + + # sensor configurations + # Note: all sensors must have the same resolution + # setup 2 rgb sensors for 1st and 3rd person views + camera_resolution = [540, 720] + sensors = { + "rgba_camera_1stperson": { + "sensor_type": habitat_sim.SensorType.COLOR, + "resolution": camera_resolution, + "position": [0.0, 0.6, 0.0], + "orientation": [0.0, 0.0, 0.0], + }, + "depth_camera_1stperson": { + "sensor_type": habitat_sim.SensorType.DEPTH, + "resolution": camera_resolution, + "position": [0.0, 0.6, 0.0], + "orientation": [0.0, 0.0, 0.0], + }, + "rgba_camera_3rdperson": { + "sensor_type": habitat_sim.SensorType.COLOR, + "resolution": camera_resolution, + "position": [0.0, 1.0, 0.3], + "orientation": [-45, 0.0, 0.0], + }, + } + + sensor_specs = [] + for sensor_uuid, sensor_params in sensors.items(): + sensor_spec = habitat_sim.SensorSpec() + sensor_spec.uuid = sensor_uuid + sensor_spec.sensor_type = sensor_params["sensor_type"] + sensor_spec.resolution = sensor_params["resolution"] + sensor_spec.position = sensor_params["position"] + sensor_spec.orientation = sensor_params["orientation"] + sensor_specs.append(sensor_spec) + + # agent configuration + agent_cfg = habitat_sim.agent.AgentConfiguration() + agent_cfg.sensor_specifications = sensor_specs + + return habitat_sim.Configuration(backend_cfg, [agent_cfg]) + + +def simulate(sim, dt=1.0, get_frames=True): + # simulate dt seconds at 60Hz to the nearest fixed timestep + print("Simulating " + str(dt) + " world seconds.") + observations = [] + start_time = sim.get_world_time() + while sim.get_world_time() < start_time + dt: + sim.step_physics(1.0 / 60.0) + if get_frames: + observations.append(sim.get_sensor_observations()) + + return observations + + +# [/setup] + +# This is wrapped such that it can be added to a unit test +def main(make_video=True, show_video=True): + if make_video: + if not os.path.exists(output_path): + os.mkdir(output_path) + + # [initialize] + # create the simulator + cfg = make_configuration() + sim = habitat_sim.Simulator(cfg) + agent_transform = place_agent(sim) + + # [/initialize] + + # [basics] + + # load some object templates from configuration files + sphere_template_id = sim.load_object_configs( + str(os.path.join(data_path, "test_assets/objects/sphere")) + )[0] + + # add an object to the scene + id_1 = sim.add_object(sphere_template_id) + sim.set_translation(np.array([2.50, 0, 0.2]), id_1) + + # simulate + observations = simulate(sim, dt=1.5, get_frames=make_video) + + if make_video: + make_video_cv2(observations, prefix="sim_basics", open_vid=show_video) + + # [/basics] + + remove_all_objects(sim) + + # [dynamic_control] + + observations = [] + # search for an object template by key sub-string + cheezit_template_handle = sim.get_template_handles("data/objects/cheezit")[0] + box_positions = [ + np.array([2.39, -0.37, 0]), + np.array([2.39, -0.64, 0]), + np.array([2.39, -0.91, 0]), + np.array([2.39, -0.64, -0.22]), + np.array([2.39, -0.64, 0.22]), + ] + box_orientation = mn.Quaternion.rotation( + mn.Rad(math.pi / 2.0), np.array([-1.0, 0, 0]) + ) + # instance and place the boxes + box_ids = [] + for b in range(5): + box_ids.append(sim.add_object_by_handle(cheezit_template_handle)) + sim.set_translation(box_positions[b], box_ids[b]) + sim.set_rotation(box_orientation, box_ids[b]) + + # get the object's initialization attributes (all boxes initialized with same mass) + object_init_template = sim.get_object_initialization_template(box_ids[0]) + # anti-gravity force f=m(-g) + anti_grav_force = -1.0 * sim.get_gravity() * object_init_template.get_mass() + + # throw a sphere at the boxes from the agent position + sphere_template = sim.get_object_template(sphere_template_id) + sphere_template.set_scale(np.array([0.5, 0.5, 0.5])) + sphere_id = sim.add_object(sphere_template_id) + sim.set_translation( + sim.agents[0].get_state().position + np.array([0, 1.0, 0]), sphere_id + ) + # get the vector from the sphere to a box + target_direction = sim.get_translation(box_ids[0]) - sim.get_translation(sphere_id) + # apply an initial velocity for one step + sim.set_linear_velocity(target_direction * 5, sphere_id) + sim.set_angular_velocity(np.array([0, -1.0, 0]), sphere_id) + + start_time = sim.get_world_time() + dt = 3.0 + while sim.get_world_time() < start_time + dt: + # set forces/torques before stepping the world + for box_id in box_ids: + sim.apply_force(anti_grav_force, np.array([0, 0.0, 0]), box_id) + sim.apply_torque(np.array([0, 0.01, 0]), box_id) + sim.step_physics(1.0 / 60.0) + observations.append(sim.get_sensor_observations()) + + if make_video: + make_video_cv2(observations, prefix="dynamic_control", open_vid=show_video) + + # [/dynamic_control] + + remove_all_objects(sim) + + # [kinematic_interactions] + + chefcan_template_handle = sim.get_template_handles("data/objects/chefcan")[0] + id_1 = sim.add_object_by_handle(chefcan_template_handle) + sim.set_translation(np.array([2.4, -0.64, 0]), id_1) + # set one object to kinematic + sim.set_object_motion_type(habitat_sim.physics.MotionType.KINEMATIC, id_1) + + # drop some dynamic objects + id_2 = sim.add_object_by_handle(chefcan_template_handle) + sim.set_translation(np.array([2.4, -0.64, 0.28]), id_2) + id_3 = sim.add_object_by_handle(chefcan_template_handle) + sim.set_translation(np.array([2.4, -0.64, -0.28]), id_3) + id_4 = sim.add_object_by_handle(chefcan_template_handle) + sim.set_translation(np.array([2.4, -0.3, 0]), id_4) + + # simulate + observations = simulate(sim, dt=1.5, get_frames=True) + + if make_video: + make_video_cv2( + observations, prefix="kinematic_interactions", open_vid=show_video + ) + + # [/kinematic_interactions] + + remove_all_objects(sim) + + # [kinematic_update] + observations = [] + + clamp_template_handle = sim.get_template_handles("data/objects/largeclamp")[0] + id_1 = sim.add_object_by_handle(clamp_template_handle) + sim.set_object_motion_type(habitat_sim.physics.MotionType.KINEMATIC, id_1) + sim.set_translation(np.array([0.8, 0, 0.5]), id_1) + + start_time = sim.get_world_time() + dt = 1.0 + while sim.get_world_time() < start_time + dt: + # manually control the object's kinematic state + sim.set_translation(sim.get_translation(id_1) + np.array([0, 0, 0.01]), id_1) + sim.set_rotation( + mn.Quaternion.rotation(mn.Rad(0.05), np.array([-1.0, 0, 0])) + * sim.get_rotation(id_1), + id_1, + ) + sim.step_physics(1.0 / 60.0) + observations.append(sim.get_sensor_observations()) + + if make_video: + make_video_cv2(observations, prefix="kinematic_update", open_vid=show_video) + + # [/kinematic_update] + + # [velocity_control] + + # get object VelocityControl structure and setup control + vel_control = sim.get_object_velocity_control(id_1) + vel_control.linear_velocity = np.array([0, 0, -1.0]) + vel_control.angular_velocity = np.array([4.0, 0, 0]) + vel_control.controlling_lin_vel = True + vel_control.controlling_ang_vel = True + + observations = simulate(sim, dt=1.0, get_frames=True) + + # reverse linear direction + vel_control.linear_velocity = np.array([0, 0, 1.0]) + + observations += simulate(sim, dt=1.0, get_frames=True) + + make_video_cv2(observations, prefix="velocity_control", open_vid=True) + + # [/velocity_control] + + # [local_velocity_control] + + vel_control.linear_velocity = np.array([0, 0, 2.3]) + vel_control.angular_velocity = np.array([-4.3, 0.0, 0]) + vel_control.lin_vel_is_local = True + vel_control.ang_vel_is_local = True + + observations = simulate(sim, dt=1.5, get_frames=True) + + # video rendering + make_video_cv2(observations, prefix="local_velocity_control", open_vid=True) + + # [/local_velocity_control] + + # [embodied_agent] + + # load the lobot_merged asset + locobot_template_id = sim.load_object_configs( + str(os.path.join(data_path, "objects/locobot_merged")) + )[0] + + # add robot object to the scene with the agent/camera SceneNode attached + id_1 = sim.add_object(locobot_template_id, sim.agents[0].scene_node) + sim.set_translation(np.array([1.75, -1.02, 0.4]), id_1) + + vel_control = sim.get_object_velocity_control(id_1) + vel_control.linear_velocity = np.array([0, 0, -1.0]) + vel_control.angular_velocity = np.array([0.0, 2.0, 0]) + + # simulate robot dropping into place + observations = simulate(sim, dt=1.5, get_frames=make_video) + + vel_control.controlling_lin_vel = True + vel_control.controlling_ang_vel = True + vel_control.lin_vel_is_local = True + vel_control.ang_vel_is_local = True + + # simulate forward and turn + observations += simulate(sim, dt=1.0, get_frames=make_video) + + vel_control.controlling_lin_vel = False + vel_control.angular_velocity = np.array([0.0, 1.0, 0]) + + # simulate turn only + observations += simulate(sim, dt=1.5, get_frames=make_video) + + vel_control.angular_velocity = np.array([0.0, 0.0, 0]) + vel_control.controlling_lin_vel = True + vel_control.controlling_ang_vel = True + + # simulate forward only with damped angular velocity (reset angular velocity to 0 after each step) + observations += simulate(sim, dt=1.0, get_frames=make_video) + + vel_control.angular_velocity = np.array([0.0, -1.25, 0]) + + # simulate forward and turn + observations += simulate(sim, dt=2.0, get_frames=make_video) + + vel_control.controlling_ang_vel = False + vel_control.controlling_lin_vel = False + + # simulate settling + observations += simulate(sim, dt=3.0, get_frames=make_video) + + # video rendering with embedded 1st person view + make_video_cv2(observations, prefix="robot_control", open_vid=True, multi_obs=True) + + # [/embodied_agent] + + +if __name__ == "__main__": + main(make_video=True, show_video=True) diff --git a/habitat_sim/physics.py b/habitat_sim/physics.py index 407a83dff7..a575a7107b 100644 --- a/habitat_sim/physics.py +++ b/habitat_sim/physics.py @@ -2,6 +2,6 @@ # This source code is licensed under the MIT license found in the # LICENSE file in the root directory of this source tree. -from habitat_sim._ext.habitat_sim_bindings import MotionType +from habitat_sim._ext.habitat_sim_bindings import MotionType, VelocityControl -__all__ = ["MotionType"] +__all__ = ["MotionType", "VelocityControl"] diff --git a/habitat_sim/simulator.py b/habitat_sim/simulator.py index 3bb51d4dce..4970fecc5c 100644 --- a/habitat_sim/simulator.py +++ b/habitat_sim/simulator.py @@ -323,8 +323,11 @@ def load_object_template(self, object_template, object_template_handle): def get_object_initialization_template(self, object_id, scene_id=0): return self._sim.get_object_initialization_template(object_id, scene_id) - def get_template_handle_by_ID(self, object_id): - return self._sim.get_template_handle_by_ID(object_id) + def get_template_handle_by_ID(self, template_id): + return self._sim.get_template_handle_by_ID(template_id) + + def get_template_ID_by_handle(self, template_handle): + return self._sim.get_template_ID_by_handle(template_handle) def get_template_handles(self, search_str): return self._sim.get_template_handles(search_str) diff --git a/pyproject.toml b/pyproject.toml index 107c153015..777f9a1536 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -1,6 +1,6 @@ [tool.isort] skip_glob = ["*/deps/*", "*/build/*", "*/obselete/*"] -known_third_party = ["PIL", "attr", "conf", "demo_runner", "hypothesis", "magnum", "matplotlib", "numba", "numpy", "pytest", "quaternion", "scipy", "settings", "setuptools", "torch", "torchvision", "tqdm"] +known_third_party = ["PIL", "attr", "conf", "cv2", "demo_runner", "hypothesis", "magnum", "matplotlib", "numba", "numpy", "pytest", "quaternion", "scipy", "settings", "setuptools", "torch", "torchvision", "tqdm"] multi_line_output = 3 force_grid_wrap = false line_length = 88 diff --git a/tests/test_examples.py b/tests/test_examples.py index 9f08564fe8..1b422c4722 100644 --- a/tests/test_examples.py +++ b/tests/test_examples.py @@ -7,6 +7,7 @@ import examples.tutorials.lighting_tutorial import examples.tutorials.new_actions +import examples.tutorials.rigid_object_tutorial import examples.tutorials.stereo_agent @@ -22,6 +23,7 @@ (examples.tutorials.stereo_agent, (False,)), (examples.tutorials.new_actions, ()), (examples.tutorials.lighting_tutorial, (False,)), + (examples.tutorials.rigid_object_tutorial, (False,)), ], ) def test_example_modules(example_module, args):