diff --git a/docs/basicusage.md b/docs/basicusage.md new file mode 100644 index 0000000000..d9e8710f0a --- /dev/null +++ b/docs/basicusage.md @@ -0,0 +1,33 @@ +# Basic Usage + +## Running Standardized Environments +**robosuite** offers a set of standardized manipulation tasks for benchmarking purposes. These pre-defined environments can be easily instantiated with the `make` function. The APIs we provide to interact with our environments are simple and similar to the ones used by [OpenAI Gym](https://github.com/openai/gym/). Below is a minimalistic example of how to interact with an environment. + +```python +import numpy as np +import robosuite as suite + +# create environment instance +env = suite.make( + env_name="Lift", # try with other tasks like "Stack" and "Door" + robots="Panda", # try with other robots like "Sawyer" and "Jaco" + has_renderer=True, + has_offscreen_renderer=False, + use_camera_obs=False, +) + +# reset the environment +env.reset() + +for i in range(1000): + action = np.random.randn(*env.action_spec[0].shape) + obs, reward, done, info = env.step(action) # take action in the environment + env.render() # render on display +```` + +This script above creates a simulated environment with the on-screen renderer, which is useful for visualization and qualitative evaluation. The `step()` function takes an `action` as input and returns a tuple of `(obs, reward, done, info)` where `obs` is an `OrderedDict` containing observations `[(name_string, np.array), ...]`, `reward` is the immediate reward obtained per step, `done` is a Boolean flag indicating if the episode has terminated and `info` is a dictionary which contains additional metadata. + +Many other parameters can be configured for each environment. They provide functionalities such as headless rendering, getting pixel observations, changing camera settings, using reward shaping, and adding extra low-level observations. Please refer to [Environment](modules/environments) modules and the [Environment class](simulation/environment) APIs for further details. + +Demo scripts that showcase various features of **robosuite** are available [here](demos). The purpose of each script and usage instructions can be found at the beginning of each file. + diff --git a/docs/changelog.md b/docs/changelog.md new file mode 100644 index 0000000000..9a85deff01 --- /dev/null +++ b/docs/changelog.md @@ -0,0 +1,12 @@ +# Changelog + + + +## Version 1.5.0 + +
+

Breaking API changes

+
+ +
+
\ No newline at end of file diff --git a/docs/conf.py b/docs/conf.py index e459ea01df..47880736b7 100755 --- a/docs/conf.py +++ b/docs/conf.py @@ -42,6 +42,11 @@ "sphinx.ext.autodoc", "recommonmark", # use Sphinx-1.4 or newer "nbsphinx", + "sphinx_togglebutton", +] + +myst_enable_extensions = [ + "dollarmath", ] mathjax_config = { diff --git a/docs/demos.md b/docs/demos.md index 1bb643b67b..5ddb2665ae 100644 --- a/docs/demos.md +++ b/docs/demos.md @@ -179,3 +179,50 @@ The `demo_renderers.py` script shows how to use different renderers with the sim $ python demo_renderers.py --renderer default ``` The `--renderer` flag can be set to `mujoco` or `default(default) + +### Exporting to USD +Exporting to USD allows users to render **robosuite** trajectories in external renderers such as NVIDIA Omniverse and Blender. In order to export to USD you must install the required dependencies for the exporter. +```sh +$ pip install usd-core pillow tqdm +``` +Once the dependencies are installed, the USD exporter can be imported via `from robosuite.utils.usd import exporter`. The `USDExporter` class in the `exporter` module handles exporting all nessecary assets and USD files associated with a **robosuite** trajectory. + +First, instantiate a **robosuite** environment. Each environment has an MjModel and MjData instance associated with it. These attributes can be retrieved using +```python +model = env.sim.model._model +data = env.sim.data._data +``` +Both `model` and `data` are used by the USD exporter. Once a robosuite environment is defined, create a `USDExporter` object with the following arguments. + +* `model` (required): an MjModel instance. +* `max_geom`: Optional integer specifying the maximum number of geoms that +can be rendered in the same scene. If None this will be chosen +automatically based on the estimated maximum number of renderable +geoms in the model. +* `output_directory_name`: name of root directory to store outputted frames +and assets generated by the USD renderer. +and assets by the USD renderer. +* `light_intensity`: default intensity of the lights in the external renderer. +* `shareable`: use relative paths to assets instead of absolute paths to allow +files to be shared across users. +* `online`: set to true if using USD exporter for online rendering. This value +is set to true when rendering with Isaac Sim. If online is set to true, shareable must be false. +* `framerate`: framerate of the exported scene when rendered +* `camera_names`: list of fixed cameras defined in the mujoco model to render. +* `stage`: predefined stage to add objects in the scene to. +* `verbose`: decides whether to print updates. + +`USDExporter` is adapted from [MuJoCo](https://github.com/google-deepmind/mujoco). In order to add a new frame in the outputted USD trajectory, call `update_scene` in the `exporter` module. + +```python +exp = exporter.USDExporter(model=model, output_directory_name="usd_demo") +exp.update_scene(data) +``` + +This updates all geoms in the scene with their current poses from simulation. To save a USD trajectory, use the `save_scene` method. + +```python +exp.save_scene(filetype="usd") +``` + +Users are able to save scenes as .usd, .usda, or .usdc files. For a more comprehensive example of the USD renderer, please refer to the [`demo_usd_export.py`]() script. This demonstration allows users to teleoperate a robot with a device (i.e. keyboard or spacemouse) and save the collected trajectory as a USD file. diff --git a/docs/index.rst b/docs/index.rst index c36c4093fd..a67f3b9b80 100644 --- a/docs/index.rst +++ b/docs/index.rst @@ -12,7 +12,7 @@ Welcome to robosuite's documentation! overview installation - quickstart + basicusage demos .. toctree:: @@ -21,7 +21,7 @@ Welcome to robosuite's documentation! modules/overview modules/robots - modules/controllers.ipynb + modules/controllers modules/objects modules/environments modules/sensors @@ -30,6 +30,13 @@ Welcome to robosuite's documentation! .. toctree:: :maxdepth: 1 + :caption: Tutorials + + tutorials/add_controller + tutorials/add_environment + +.. toctree:: + :maxdepth: 3 :caption: Simulation API simulation/robot @@ -67,6 +74,7 @@ Welcome to robosuite's documentation! :caption: Miscellaneous references + changelog acknowledgement Indices and tables diff --git a/docs/modeling/arena.rst b/docs/modeling/arena.rst index 9116598d11..4889c05894 100644 --- a/docs/modeling/arena.rst +++ b/docs/modeling/arena.rst @@ -13,3 +13,55 @@ Base Arena .. automethod:: __init__ .. automethod:: set_origin .. automethod:: set_camera + +Empty Arena +----------- + +.. autoclass:: robosuite.models.arenas.empty_arena.EmptyArena + + .. automethod:: __init__ + +Bins Arena +---------- + +.. autoclass:: robosuite.models.arenas.bins_arena.BinsArena + + .. automethod:: __init__ + .. automethod:: configure_location + +Pegs Arena +---------- + +.. autoclass:: robosuite.models.arenas.pegs_arena.PegsArena + + .. automethod:: __init__ + +Table Arena +----------- + +.. autoclass:: robosuite.models.arenas.table_arena.TableArena + + .. automethod:: __init__ + .. automethod:: configure_location + .. autoproperty:: table_top_abs + +Wipe Arena +---------- + +.. autoclass:: robosuite.models.arenas.wipe_arena.WipeArena + + .. automethod:: __init__ + .. automethod:: configure_location + .. automethod:: reset_arena + .. automethod:: sample_start_pos + .. automethod:: sample_path_pos + +MultiTable Arena +---------------- + +.. autoclass:: robosuite.models.arenas.multi_table_arena.MultiTableArena + + .. automethod:: __init__ + .. automethod:: _add_table + .. automethod:: configure_location + .. automethod:: _postprocess_arena diff --git a/docs/modeling/robot_model.rst b/docs/modeling/robot_model.rst index f6611c6fb6..71673dad78 100644 --- a/docs/modeling/robot_model.rst +++ b/docs/modeling/robot_model.rst @@ -12,6 +12,9 @@ file and also contains relevant hard-coded information from that XML. This repre .. automethod:: set_base_ori .. automethod:: set_joint_attribute .. automethod:: add_base + .. automethod:: add_mount + .. automethod:: add_mobile_base + .. automethod:: add_leg_base .. autoproperty:: dof .. autoproperty:: default_base .. autoproperty:: default_controller_config diff --git a/docs/modules/controllers.rst b/docs/modules/controllers.rst new file mode 100644 index 0000000000..ab0594440f --- /dev/null +++ b/docs/modules/controllers.rst @@ -0,0 +1,261 @@ +Controllers +============== + +Composite Controllers +--------------------- + +Basic +****** + + +WholeBody IK +************* + + +ThirdParty Controllers +*********************** + + +Workflow of Loading Configs +**************************** +Loading configs for composite controllers is critical for selecting the correct control mode with well-tuned parameters. We provide a list of default controller configs for the composite controllers, and also support easy specification of your custom controller config file. A config file is defined in a json file. + +An example of the controller config file is shown below (many parameters are omitted in `...` for brevity): + +.. toggle:: + + Example for defining BASIC controller. + + .. code-block:: json + + { + "type": "BASIC", + "body_parts_controller_configs": { + "arms": { + "right": { + "type": "OSC_POSE", + "input_max": 1, + "input_min": -1, + "output_max": [0.05, 0.05, 0.05, 0.5, 0.5, 0.5], + "output_min": [-0.05, -0.05, -0.05, -0.5, -0.5, -0.5], + "kp": 150, + ... + }, + "left": { + "type": "OSC_POSE", + ... + } + }, + "torso": { + "type" : "JOINT_POSITION", + ... + }, + "head": { + "type" : "JOINT_POSITION", + ... + }, + "base": { + "type": "JOINT_VELOCITY", + ... + }, + "legs": { + "type": "JOINT_POSITION", + ... + } + } + } + + + +Part Controllers +------------------ + +Part controllers are used to determine the type of high-level control over a given robot arm. While all arms are directly controlled via their joint torques, the inputted action space for a given environment can vary depending on the type of desired control. Our controller options include ``OSC_POSE``, ``OSC_POSITION``, ``JOINT_POSITION``, ``JOINT_VELOCITY``, and ``JOINT_TORQUE``. + +For ``OSC_POSE``, ``OSC_POSITION``, and ``JOINT_POSITION``, we include three variants that determine the action space. The most common variant is to use a predefined and constant set of impedance parameters; in that case, the action space only includes the desired pose, position, or joint configuration. We also include the option to specify either the stiffness values (and the damping will be automatically set to values that lead to a critically damped system), or all impedance parameters, both stiffness and damping, as part of the action at each step. These two variants lead to extended action spaces that can control the stiffness and damping behavior of the controller in a variable manner, providing full control to the policy/solution over the contact and dynamic behavior of the robot. + +When using any position-based control (``OSC``, ``IK``, or ``JOINT_POSITION`` controllers), input actions are, by default, +interpreted as delta values from the current state. + +When using any end-effector pose controller (``IK``, ``OSC_POSE``), delta rotations from the current end-effector orientation +in the form of axis-angle coordinates ``(ax, ay, az)``, where the direction represents the axis and the magnitude +represents the angle (in radians). Note that for ``OSC``, the rotation axes are taken relative to the global world +coordinate frame, whereas for ``IK``, the rotation axes are taken relative to the end-effector origin, NOT the global world coordinate frame! + +During runtime, the execution of the controllers is as follows. Controllers receive a desired configuration (reference value) and output joint torques that try to minimize the error between the current configuration and the desired one. Policies and solutions provide these desired configurations, elements of some action space, at what we call simulated policy frequency (:math:`f_{p}`), e.g., 20Hz or 30Hz. **robosuite** will execute several iterations composed of a controller execution and a simulation step at simulation frequency, :math:`f_s` (:math:`f_s = N\cdot f_p`), using the same reference signal until a new action is provided by the policy/solution. In these iterations, while the desired configuration is kept constant, the current state of the robot is changing, and thus, the error. + +In the following we summarize the options, variables, and the control laws (equations) that convert desired values from the policy/solution and current robot states into executable joint torques to minimize the difference. + +Joint Space Control - Torque +********************************* +Controller Type: ``JOINT_TORQUE`` + +Action Dimensions (not including gripper): ``n`` (number of joints) + +Since our controllers transform the desired values from the policies/solutions into joint torques, if these values are already joint torques, there is a one-to-one mapping between the reference value from the policy/solution and the output value from the joint torque controller at each step: :math:`\tau = \tau_d`. + +.. math:: + \begin{equation} + \tau = \tau_d + \end{equation} + +Joint Space Control - Velocity +********************************* +Controller Type: ``JOINT_VELOCITY`` + +Action Dimensions (not including gripper): ``n`` (number of joints) + +To control joint velocities, we create a proportional (P) control law between the desired value provided by the policy/solution (interpreted as desired velocity of each joint) and the current joint velocity of the robot. This control law, parameterized by a proportional constant, :math:`k_p`, generates joint torques to execute at each simulation step: + +.. math:: + \tau = k_p (\dot{q}_d - \dot{q}) + + +Joint Space Control - Position with Fixed Impedance +******************************************************** +Controller Type: ``JOINT_POSITION`` + +Impedance: fixed + +Action Dimensions (not including gripper): ``n`` (number of joints) + +In joint position control, we create a proportional-derivative (PD) control law between the desired value provided by the policy/solution (interpreted as desired configuration for each joint) and the current joint positions of the robot. The control law that generates the joint torques to execute is parameterized by proportional and derivative gains, :math:`k_p` and :math:`k_v`, and defined as + +.. math:: + \begin{equation} + \tau = \Lambda \left[k_p \Delta_q - k_d\dot{q}\right] + \end{equation} + +where :math:`\Delta_q = q_d - q` is the difference between current and desired joint configurations, and :math:`\Lambda` is the inertia matrix, that we use to scale the error to remove the dynamic effects of the mechanism. The stiffness and damping parameters, :math:`k_p` and :math:`k_d`, are determined in construction and kept fixed. + +Joint Space Control - Position with Variable Stiffness +*********************************************************** +Controller Type: ``JOINT_POSITION`` + +Impedance: variable_kp + +Action Dimensions (not including gripper): ``2n`` (number of joints) + +The control law is the same as for fixed impedance but, in this controller, :math:`k_p`` can be determined by the policy/solution at each policy step. + +Joint Space Control - Position with Variable Impedance +*********************************************************** +Controller Type: ``JOINT_POSITION`` + +Impedance: variable + +Action Dimensions (not including gripper): ``3n`` (number of joints) + +Again, the control law is the same in the two previous control types, but now both the stiffness and damping parameters, :math:`k_p` and :math:`k_d`, are controllable by the policy/solution and can be changed at each step. + +Operational Space Control - Pose with Fixed Impedance +********************************************************** +Controller Type: ``OSC_POSE`` + +Impedance: fixed + +Action Dimensions (not including gripper): ``6`` + +In the ``OSC_POSE`` controller, the desired value is the 6D pose (position and orientation) of a controlled frame. We follow the formalism from `[Khatib87] `_. Our control frame is always the ``eef_site`` defined in the `Gripper Model `_, placed at the end of the last link for robots without gripper or between the fingers for robots with gripper. The operational space control framework (OSC) computes the necessary joint torques to minimize the error between the desired and the current pose of the ``eef_site`` with the minimal kinematic energy. + +Given a desired pose :math:`\mathbf{x}_{\mathit{des}}` and the current end-effector pose, , we first compute the end-effector acceleration that would help minimize the error between both, assumed. PD (proportional-derivative) control schema to improve convergence and stability. For that, we first decompose into a desired position, :math:`p_d \in \mathbb{R}^3`, and a desired orientation, :math:`R_d \in \mathbb{SO}(3)`. The end-effector acceleration to minimize the error should increase with the difference between desired end-effector pose and current pose, :math:`p` and :math:`R` (proportional term), and decrease with the current end-effector velocity, :math:`v` and :math:`\omega` (derivative term). + +We then compute the robot actuation (joint torques) to achieve the desired end-effector space accelerations leveraging the kinematic and dynamic models of the robot with the dynamically-consistent operational space formulation in `[Khatib1995a] `_. First, we compute the wrenches at the end-effector that corresponds to the desired accelerations, :math:`{f}\in\mathbb{R}^{6}`. +Then, we map the wrenches in end-effector space :math:`{f}` to joint torque commands with the end-effector Jacobian at the current joint configuration :math:`J=J(q)`: :math:`\tau = J^T{f}`. + +Thus, the function that maps end-effector space position and orientation to low-level robot commands is (:math:`\textrm{ee} = \textrm{\it end-effector space}`): + +.. math:: + + \begin{equation} + \begin{aligned} + \tau &= J_p[\Lambda_p[k_p^p (p_d - p) - k_v^p v]] + J_R[\Lambda_R\left[k_p^R(R_d \ominus R) - k_d^R \omega \right]] + \end{aligned} + \end{equation} + +where :math:`\Lambda_p` and :math:`\Lambda_R` are the parts corresponding to position and orientation in :math:`\Lambda \in \mathbb{R}^{6\times6}`, the inertial matrix in the end-effector frame that decouples the end-effector motions, :math:`J_p` and :math:`J_R` are the position and orientation parts of the end-effector Jacobian, and :math:`\ominus` corresponds to the subtraction in :math:`\mathbb{SO}(3)`. The difference between current and desired position (:math:`\Delta_p= p_d - p`) and between current and desired orientation (:math:`\Delta_R = R_d \ominus R`) can be used as alternative policy action space, :math:`\mathcal{A}`. :math:`k_p^p`, :math:`k_p^d`, :math:`k_p^R`, and :math:`k_d^R` are vectors of proportional and derivative gains for position and orientation (parameters :math:`\kappa`), respectively, set once at initialization and kept fixed. + +Operational Space Control - Pose with Variable Stiffness +************************************************************* +Controller Type: ``OSC_POSE`` + +Impedance: variable_kp + +Action Dimensions (not including gripper): ``12`` + +The control law is the same as ``OSC_POSE`` but, in this case, the stiffness of the controller, :math:`k_p`, is part of the action space and can be controlled and changed at each time step by the policy/solution. The damping parameters, :math:`k_d`, are set to maintain the critically damped behavior of the controller. + +Operational Space Control - Pose with Variable Impedance +********************************************************* +Controller Type: ``OSC_POSE`` + +Impedance: variable + +Action Dimensions (not including gripper): ``18`` + +The control law is the same as in the to previous controllers, but now both the stiffness and the damping, :math:`k_p` and :math:`k_d`, are part of the action space and can be controlled and changed at each time step by the policy/solution. + + +Configurations +--------------- + +The `config directory `_ provides a set of default configuration files that hold default examples of parameters relevant to individual controllers. Note that when creating your controller config templates of a certain type of controller, the listed parameters in the default example are required and should be specified accordingly. + +Note: Each robot has its own default controller configuration which is called by default unless a different controller config is called. + +Below, a brief overview and description of each subset of controller parameters are shown: + +Controller Settings +******************** +* ``type``: Type of controller to control. Can be ``OSC_POSE``, ``OSC_POSITION``, ``IK_POSE``, ``JOINT_POSITION``, ``JOINT_VELOCITY``, or ``JOINT_TORQUE`` +* ``interpolation``: If not ``null``, specified type of interpolation to use between desired actions. Currently only ``linear`` is supported. +* ``ramp_ratio``: If using ``linear`` interpolation, specifies the proportion of allotted timesteps (value from [0, 1]) over which to execute the interpolated commands. +* ``{...}_limits``: Limits for that specific controller. E.g.: for a ``JOINT_POSITION``, the relevant limits are its joint positions, ``qpos_limits`` . Can be either a 2-element list (same min/max limits across entire relevant space), or a list of lists (specific limits for each component) +* ``ik_{pos, ori}_limit``: Only applicable for IK controller. Limits the magnitude of the desired relative change in position / orientation. +* ``{input,output}_{min,max}``: Scaling ranges for mapping action space inputs into controller inputs. Settings these limits will automatically clip the action space input to be within the ``input_{min,max}`` before mapping the requested value into the specified ``output_{min,max}`` range. Can be either a scalar (same limits across entire action space), or a list (specific limits for each action component) +* ``kp``: Where relevant, specifies the proportional gain for the controller. Can be either be a scalar (same value for all controller dimensions), or a list (specific values for each dimension) +* ``damping_ratio``: Where relevant, specifies the damping ratio constant for the controller. +* ``impedance_mode``: For impedance-based controllers (``OSC_*``, ``JOINT_POSITION``), determines the impedance mode for the controller, i.e. the nature of the impedance parameters. It can be ``fixed``, ``variable``, or ``variable_kp`` (kd is adjusted to provide critically damped behavior). +* ``kp_limits, damping_ratio_limits``: Only relevant if ``impedance_mode`` is set to ``variable`` or ``variable_kp``. Sets the limits for the resulting action space for variable impedance gains. +* ``control_delta``: Only relevant for ``OSC_POSE`` or ``OSC_POSITION`` controllers. ``true`` interprets input actions as delta values from the current robot end-effector position. Otherwise, assumed to be absolute (global) values +* ``uncouple_pos_ori``: Only relevant for ``OSC_POSE``. ``true`` decouples the desired position and orientation torques when executing the controller + +Loading a Controller +--------------------- +By default, if no controller configuration is specified during environment creation, then ``JOINT_VELOCITY`` controllers with robot-specific configurations will be used. + +Using a Default Controller Configuration +***************************************** +Any controller can be used with its default configuration, and can be easily loaded into a given environment by calling its name as shown below (where ``controller_name`` is one of acceptable controller ``type`` strings): + +.. code-block:: python + + import robosuite as suite + from robosuite import load_controller_config + + # Load the desired controller's default config as a dict + config = load_controller_config(default_controller=controller_name) + + # Create environment + env = suite.make("Lift", robots="Panda", controller_configs=config, ... ) + + +Using a Custom Controller Configuration +**************************************** +A custom controller configuration can also be used by simply creating a new config (``.json``) file with the relevant parameters as specified above. All robosuite environments have an optional ``controller_configs`` argument that can be used to pass in specific controller settings. Note that this is expected to be a ``dict``, so the new configuration must be read in and parsed as a ``dict`` before passing it during the environment ``robosuite.make(...)`` call. A brief example script showing how to import a custom controller configuration is shown below. + + +.. code-block:: python + + import robosuite as suite + from robosuite import load_controller_config + + # Path to config file + controller_fpath = "/your/custom/config/filepath/here/filename.json" + + # Import the file as a dict + config = load_controller_config(custom_fpath=controller_fpath) + + # Create environment + env = suite.make("Lift", robots="Panda", controller_configs=config, ... ) + diff --git a/docs/modules/robots.rst b/docs/modules/robots.rst new file mode 100644 index 0000000000..565a9d633f --- /dev/null +++ b/docs/modules/robots.rst @@ -0,0 +1,164 @@ +Robots +======= + +.. figure:: ../images/robot_module.png + +**Robots** are a key component in **robosuite**, and serve as the embodiment of a given agent as well as the central interaction point within an environment and key interface to MuJoCo for the robot-related state and control. **robosuite** captures this level of abstraction with the `Robot <../simulation/robot.html>`_-based classes, with support for both single-armed and bimanual variations. In turn, the Robot class is centrally defined by a `RobotModel <../modeling/robot_model.html>`_, `MountModel <../modeling/robot_model.html#mount-model>`_, and `Controller(s) <../simulation/controller.html>`_. Subclasses of the :class:`RobotModel` class may also include additional models as well; for example, the `ManipulatorModel <../modeling/robot_model.html#manipulator-model>`_ class also includes `GripperModel(s) <../modeling/robot_model.html#gripper-model>`_ (with no gripper being represented by a dummy class). + +The high-level features of **robosuite**'s robots are described as follows: + +* **Diverse and Realistic Models**: **robosuite** provides models for 8 commercially-available manipulator robots (including the bimanual Baxter robot), 7 grippers (including the popular Robotiq 140 / 85 models), and 6 controllers, with model properties either taken directly from the company website or raw spec sheets. + +* **Modularized Support**: Robots are designed to be plug-n-play -- any combinations of robots, models, and controllers can be used, assuming the given environment is intended for the desired robot configuration. Because each robot is assigned a unique ID number, multiple instances of identical robots can be instantiated within the simulation without error. + +* **Self-Enclosed Abstraction**: For a given task and environment, any information relevant to the specific robot instance can be found within the properties and methods within that instance. This means that each robot is responsible for directly setting its initial state within the simulation at the start of each episode, and also directly controls the robot in simulation via torques outputted by its controller's transformed actions. + +Usage +====== +Below, we discuss the usage and functionality of the robots over the course of its program lifetime. + +Initialization +-------------- +During environment creation (``suite.make(...)``), individual robots are both instantiated and initialized. The desired RobotModel, MountModel, and Controller(s) (where multiple and/or additional models may be specified, e.g. for manipulator bimanual robots) are loaded into each robot, with the models being passed into the environment to compose the final MuJoCo simulation object. Each robot is then set to its initial state. + +Runtime +------- +During a given simulation episode (each ``env.step(...)`` call), the environment will receive a set of actions and distribute them accordingly to each robot, according to their respective action spaces. Each robot then converts these actions into low-level torques via their respective controllers, and directly executes these torques in the simulation. At the conclusion of the environment step, each robot will pass its set of robot-specific observations to the environment, which will then concatenate and append additional task-level observations before passing them as output from the ``env.step(...)`` call. + +Callables +--------- +At any given time, each robot has a set of ``properties`` whose real-time values can be accessed at any time. These include specifications for a given robot, such as its DoF, action dimension, and torque limits, as well as proprioceptive values, such as its joint positions and velocities. Additionally, if the robot is enabled with any sensors, those readings can also be polled as well. A full list of robot properties can be found in the `Robots API <../simulation/robot.html>`_ section. + +Models +====== +**robosuite** is designed to be generalizable to multiple robotic domains. The current release focuses on manipulator robots. For adding new robots, we provide a `rudimentary guide `_ on how to import raw Robot and Gripper models (based on a URDF source file) into robosuite. + +Manipulators +------------ +**robosuite** currently supports seven commercially-available manipulator robot models. We briefly describe each individual model along with its features below: + + +Panda +~~~~~ +.. image:: ../images/models/robot_model_Panda.png + :alt: panda_robot + :align: left + :width: 50% + +- DoF: 7 +- Parallel jaw gripper +- Fixed base + +Sawyer +~~~~~~ +.. image:: ../images/models/robot_model_Sawyer.png + :alt: sawyer_robot + :align: left + :width: 50% + +- DoF: 7 +- Parallel jaw gripper +- Fixed base + +LBR IIWA 7 +~~~~~~~~~~ +.. image:: ../images/models/robot_model_IIWA.png + :alt: iiwa_robot + :align: left + :width: 50% + +- DoF: 7 +- ... +- ... + +Jaco +~~~~ +.. image:: ../images/models/robot_model_Jaco.png + :alt: jaco_robot + :align: left + :width: 50% + +- DoF: 7 +- ... +- ... + +Kinova Gen3 +~~~~~~~~~~~ +.. image:: ../images/models/robot_model_Kinova3.png + :alt: kinova3_robot + :align: left + :width: 50% + + + +- DoF: 7 +- Parallel jaw gripper +- Fixed base + +UR5e +~~~~ +.. image:: ../images/models/robot_model_UR5e.png + :alt: ur5e_robot + :align: left + :width: 50% + +- DoF: 6 +- Parallel jaw gripper +- Fixed base + +Baxter +~~~~~~ +.. image:: ../images/models/robot_model_Baxter.png + :alt: baxter_robot + :align: left + :width: 50% + +- DoF: 14 +- ... +- ... + +Tiago +~~~~~~ +.. image:: ... + :alt: tiago_robot + :align: left + :width: 50% + +- DoF: ... +- ... +- ... + +GR1 +~~~~~~ +.. image:: ... + :alt: gr1_robot + :align: left + :width: 50% + +- DoF: ... +- ... +- ... + +Create Your Own Robot +---------------------- + + + +.. code-block:: python + + @register_robot_class("LeggedRobot") + class GR1SchunkSVH(GR1): + """ + Variant of GR1 robot with SchunkSVH hands. + """ + + @property + def default_gripper(self): + """ + Since this is bimanual robot, returns dict with `'right'`, `'left'` keywords corresponding to their respective + values + + Returns: + dict: Dictionary containing arm-specific gripper names + """ + return {"right": "SchunkSvhRightHand", "left": "SchunkSvhLeftHand"} diff --git a/docs/quickstart.md b/docs/quickstart.md deleted file mode 100644 index e5058411da..0000000000 --- a/docs/quickstart.md +++ /dev/null @@ -1,96 +0,0 @@ -# Quick Start - -## Running Standardized Environments -**robosuite** offers a set of standardized manipulation tasks for benchmarking purposes. These pre-defined environments can be easily instantiated with the `make` function. The APIs we provide to interact with our environments are simple and similar to the ones used by [OpenAI Gym](https://github.com/openai/gym/). Below is a minimalistic example of how to interact with an environment. - -```python -import numpy as np -import robosuite as suite - -# create environment instance -env = suite.make( - env_name="Lift", # try with other tasks like "Stack" and "Door" - robots="Panda", # try with other robots like "Sawyer" and "Jaco" - has_renderer=True, - has_offscreen_renderer=False, - use_camera_obs=False, -) - -# reset the environment -env.reset() - -for i in range(1000): - action = np.random.randn(env.robots[0].dof) # sample random action - obs, reward, done, info = env.step(action) # take action in the environment - env.render() # render on display -```` - -This script above creates a simulated environment with the on-screen renderer, which is useful for visualization and qualitative evaluation. The `step()` function takes an `action` as input and returns a tuple of `(obs, reward, done, info)` where `obs` is an `OrderedDict` containing observations `[(name_string, np.array), ...]`, `reward` is the immediate reward obtained per step, `done` is a Boolean flag indicating if the episode has terminated and `info` is a dictionary which contains additional metadata. - -Many other parameters can be configured for each environment. They provide functionalities such as headless rendering, getting pixel observations, changing camera settings, using reward shaping, and adding extra low-level observations. Please refer to [Environment](modules/environments) modules and the [Environment class](simulation/environment) APIs for further details. - -Demo scripts that showcase various features of **robosuite** are available [here](demos). The purpose of each script and usage instructions can be found at the beginning of each file. - -## Building Your Own Environments -**robosuite** offers great flexibility in creating your own environments. A [task](modeling/task) typically involves the participation of a [robot](modeling/robot_model) with [grippers](modeling/robot_model.html#gripper-model) as its end-effectors, an [arena](modeling/arena) (workspace), and [objects](modeling/object_model) that the robot interacts with. For a detailed overview of our design architecture, please check out the [Overview](modules/overview) page in Modules. Our Modeling APIs provide methods of composing these modularized elements into a scene, which can be loaded in MuJoCo for simulation. To build your own environments, we recommend you take a look at the [Environment classes](simulation/environment) which have used these APIs to define robotics environments and tasks and the [source code](https://github.com/ARISE-Initiative/robosuite/tree/master/robosuite/environments) of our standardized environments. Below we walk through a step-by-step example of building a new tabletop manipulation environment with our APIs. - -**Step 1: Creating the world.** All mujoco object definitions are housed in an xml. We create a [MujocoWorldBase](source/robosuite.models) class to do it. -```python -from robosuite.models import MujocoWorldBase - -world = MujocoWorldBase() -``` - -**Step 2: Creating the robot.** The class housing the xml of a robot can be created as follows. -```python -from robosuite.models.robots import Panda - -mujoco_robot = Panda() -``` -We can add a gripper to the robot by creating a gripper instance and calling the add_gripper method on a robot. -```python -from robosuite.models.grippers import gripper_factory - -gripper = gripper_factory('PandaGripper') -mujoco_robot.add_gripper(gripper) -``` -To add the robot to the world, we place the robot on to a desired position and merge it into the world -```python -mujoco_robot.set_base_xpos([0, 0, 0]) -world.merge(mujoco_robot) -``` - -**Step 3: Creating the table.** We can initialize the [TableArena](source/robosuite.models.arenas) instance that creates a table and the floorplane -```python -from robosuite.models.arenas import TableArena - -mujoco_arena = TableArena() -mujoco_arena.set_origin([0.8, 0, 0]) -world.merge(mujoco_arena) -``` - -**Step 4: Adding the object.** For details of `MujocoObject`, refer to the documentation about [MujocoObject](modeling/object_model), we can create a ball and add it to the world. -```python -from robosuite.models.objects import BallObject -from robosuite.utils.mjcf_utils import new_joint - -sphere = BallObject( - name="sphere", - size=[0.04], - rgba=[0, 0.5, 0.5, 1]).get_obj() -sphere.set('pos', '1.0 0 1.0') -world.worldbody.append(sphere) -``` - -**Step 5: Running Simulation.** Once we have created the object, we can obtain a `mujoco_py` model by running -```python -model = world.get_model(mode="mujoco") -``` -This is an `MjModel` instance that can then be used for simulation. For example, -```python -import mujoco - -data = mujoco.MjData(model) -while data.time < 1: - mujoco.mj_step(model, data) -``` diff --git a/docs/simulation/controller.rst b/docs/simulation/controller.rst index 1ff5856c62..9997e0d363 100644 --- a/docs/simulation/controller.rst +++ b/docs/simulation/controller.rst @@ -6,15 +6,77 @@ values are mapped into command torques. By default, all controllers have a pre-d properities, though specific controllers may extend and / or override the default functionality found in the base class. -Base Controller ---------------- -.. autoclass:: robosuite.controllers.base_controller.Controller + +Composite Controllers +---------------------- +Composite controllers are controllers that are composed of multiple sub-controllers. They are used to control the entire robot, including all of its parts. What happens is that when an action vector is commanded to the robot, the action will be split into multipl sub-actions, each of which will be sent to the corresponding sub-controller. To understand the action split, use the function `robosuite.robots.robot.print_action_info()`. To create the action easily, we also provide a helper function `robosuite.robots.robot.create_action_vector()` which takes the action dictionary as inputs and return the action vector with correct dimensions. For controller actions whose input dimentions does not match the robot's degrees of freedoms, you need to write your own `create_action_vector()` function inside the custom composite controller so that the robot's function can retrieve the information properly. + +Composite Controller (Base class) +********************************* +.. autoclass:: robosuite.controllers.composite.composite_controller.CompositeController + + .. automethod:: load_controller_config + .. automethod:: _init_controllers + .. automethod:: _validate_composite_controller_specific_config + .. automethod:: setup_action_split_idx + .. automethod:: set_goal + .. automethod:: reset + .. automethod:: run_controller + .. automethod:: get_control_dim + .. automethod:: get_controller_base_pose + .. automethod:: update_state + .. automethod:: get_controller + .. autoproperty:: action_limits + +HybridMobileBase +**************** +.. autoclass:: robosuite.controllers.composite.composite_controller.HybridMobileBase + + .. automethod:: set_goal + .. autoproperty:: action_limits + .. automethod:: create_action_vector + +WholeBody +********* +.. autoclass:: robosuite.controllers.composite.composite_controller.WholeBody + + .. automethod:: _init_controllers + .. automethod:: _init_joint_action_policy + .. automethod:: setup_action_split_idx + .. automethod:: setup_whole_body_controller_action_split_idx + .. automethod:: set_goal + .. automethod:: update_state + .. autoproperty:: action_limits + .. automethod:: create_action_vector + .. automethod:: print_action_info + .. automethod:: print_action_info_dict + +WholeBodyIK +*********** +.. autoclass:: robosuite.controllers.composite.composite_controller.WholeBodyIK + + .. automethod:: _validate_composite_controller_specific_config + .. automethod:: _init_joint_action_policy + + + + +Part Controllers +----------------- +Part controllers are equivalent to controllers in robosuite up to `v1.4`. Starting from `v1.5`, we need to accommodate the diverse embodiments, and the original controllers are changed to controllers for specific parts, such as arms, heads, legs, torso, etc. + +Controller (Base class) +************************ +.. autoclass:: robosuite.controllers.parts.controller.Controller .. automethod:: run_controller .. automethod:: scale_action + .. automethod:: update_reference_data + .. automethod:: _update_single_reference .. automethod:: update .. automethod:: update_base_pose + .. automethod:: update_origin .. automethod:: update_initial_joints .. automethod:: clip_torques .. automethod:: reset_goal @@ -24,62 +86,125 @@ Base Controller .. autoproperty:: control_limits .. autoproperty:: name +Joint Position Controller (generic) +************************************ +.. autoclass:: robosuite.controllers.parts.generic.joint_pos.JointPositionController -Joint Position Controller -------------------------- - -.. autoclass:: robosuite.controllers.joint_pos.JointPositionController - + .. automethod:: update_base_pose .. automethod:: set_goal + .. automethod:: run_controller .. automethod:: reset_goal .. autoproperty:: control_limits + .. autoproperty:: name +Joint Velocity Controller (generic) +************************************ +.. autoclass:: robosuite.controllers.parts.generic.joint_vel.JointVelocityController + .. automethod:: set_goal + .. automethod:: run_controller + .. automethod:: reset_goal + .. autoproperty:: name -Joint Velocity Controller -------------------------- -.. autoclass:: robosuite.controllers.joint_vel.JointVelocityController +Joint Torque Controller (generic) +********************************** +.. autoclass:: robosuite.controllers.parts.generic.joint_tor.JointTorqueController .. automethod:: set_goal + .. automethod:: run_controller .. automethod:: reset_goal + .. autoproperty:: name + +Operational Space Controller (arm) +********************************** +.. autoclass:: osc.OperationalSpaceController -Joint Torque Controller ------------------------ + .. automethod:: set_goal + .. automethod:: world_to_origin_frame + .. automethod:: compute_goal_pos + .. automethod:: goal_origin_to_eef_pose + .. automethod:: compute_goal_orientation + .. automethod:: run_controller + .. automethod:: update_origin + .. automethod:: update_initial_joints + .. automethod:: reset_goal + .. autoproperty:: control_limits + .. autoproperty:: nam -.. autoclass:: robosuite.controllers.joint_tor.JointTorqueController +Inverse Kinematics Controller (arm) +************************************ +.. autoclass:: robosuite.controllers.parts.arm.ik.InverseKinematicsController + + .. automethod:: get_control + .. automethod:: compute_joint_positions .. automethod:: set_goal + .. automethod:: run_controller + .. automethod:: update_initial_joints .. automethod:: reset_goal + .. automethod:: _clip_ik_input + .. automethod:: _make_input + .. automethod:: _get_current_error + .. autoproperty:: control_limits + .. autoproperty:: name + +Mobile Base Controller (mobile base) +************************************* +.. autoclass:: robosuite.controllers.parts.mobile_base.mobile_base_controller.MobileBaseController + + .. automethod:: get_base_pose + .. automethod:: reset + .. automethod:: run_controller + .. automethod:: scale_action + .. automethod:: update + .. automethod:: update_initial_joints + .. automethod:: clip_torques + .. automethod:: reset_goal + .. automethod:: nums2array + .. autoproperty:: torque_compensation + .. autoproperty:: actuator_limits + .. autoproperty:: control_limits + .. autoproperty:: name -Operation Space Controller --------------------------- -.. autoclass:: robosuite.controllers.osc.OperationalSpaceController +Mobile Base Joint Velocity Controller (mobile base) +**************************************************** +.. autoclass:: robosuite.controllers.parts.mobile_base.joint_vel.MobileBaseJointVelocityController .. automethod:: set_goal + .. automethod:: run_controller .. automethod:: reset_goal .. autoproperty:: control_limits + .. autoproperty:: name -Inverse Kinematics Controller ------------------------------ +Gripper Controller (base class) +******************************** +.. autoclass:: robosuite.controllers.parts.gripper.gripper_controller.GripperController -.. autoclass:: robosuite.controllers.ik.InverseKinematicsController + .. automethod:: run_controller + .. automethod:: scale_action + .. automethod:: update + .. automethod:: update_base_pose + .. automethod:: update_initial_joints + .. automethod:: clip_torques + .. automethod:: reset_goal + .. automethod:: nums2array + .. autoproperty:: torque_compensation + .. autoproperty:: actuator_limits + .. autoproperty:: control_limits + .. autoproperty:: name + + +Simple Grip Controller (gripper) +********************************* +.. autoclass:: robosuite.controllers.parts.gripper.simple_grip.SimpleGripController - .. automethod:: setup_inverse_kinematics - .. automethod:: sync_state - .. automethod:: sync_ik_robot - .. automethod:: ik_robot_eef_joint_cartesian_pose - .. automethod:: get_control - .. automethod:: inverse_kinematics - .. automethod:: joint_positions_for_eef_command - .. automethod:: bullet_base_pose_to_world_pose .. automethod:: set_goal + .. automethod:: run_controller .. automethod:: reset_goal .. autoproperty:: control_limits - .. automethod:: _clip_ik_input - .. automethod:: _make_input - .. automethod:: _get_current_error + .. autoproperty:: name diff --git a/docs/simulation/robot.rst b/docs/simulation/robot.rst index 07fdab7feb..2d7227f7d8 100644 --- a/docs/simulation/robot.rst +++ b/docs/simulation/robot.rst @@ -5,37 +5,37 @@ The ``Robot`` class defines a simulation object encapsulating a robot model, gri Base Robot ---------- - .. autoclass:: robosuite.robots.robot.Robot + .. automethod:: _load_controller + .. automethod:: _postprocess_part_controller_config .. automethod:: load_model .. automethod:: reset_sim .. automethod:: reset .. automethod:: setup_references .. automethod:: setup_observables + .. automethod:: _create_arm_sensors + .. automethod:: _create_base_sensors .. automethod:: control .. automethod:: check_q_limits - .. automethod:: visualize - .. automethod:: pose_in_base_from_name - .. automethod:: set_robot_joint_positions - .. automethod:: get_sensor_measurement + .. autoproperty:: is_mobile .. autoproperty:: action_limits + .. automethod:: _input2dict .. autoproperty:: torque_limits .. autoproperty:: action_dim .. autoproperty:: dof + .. automethod:: pose_in_base_from_name + .. automethod:: set_robot_joint_positions .. autoproperty:: js_energy - .. autoproperty:: joint_indexes - .. autoproperty:: is_mobile .. autoproperty:: _joint_positions .. autoproperty:: _joint_velocities - - -Fixed Base Robot ------------------ - -.. autoclass:: robosuite.robots.fixed_base_robot.FixedBaseRobot - - .. automethod:: grip_action + .. autoproperty:: joint_indexes + .. autoproperty:: arm_joint_indexes + .. automethod:: get_sensor_measurement + .. automethod:: visualize + .. automethod:: _visualize_grippers + .. autoproperty:: action_limits + .. autoproperty:: is_mobile .. autoproperty:: ee_ft_integral .. autoproperty:: ee_force .. autoproperty:: ee_torque @@ -46,8 +46,23 @@ Fixed Base Robot .. autoproperty:: _hand_orn .. autoproperty:: _hand_vel .. autoproperty:: _hand_ang_vel + .. automethod:: _load_arm_controllers + .. automethod:: enable_parts + .. automethod:: enabled + .. automethod:: create_action_vector + .. automethod:: print_action_info + .. automethod:: print_action_info_dict + .. automethod:: get_gripper_name + .. automethod:: has_part + .. autoproperty:: _joint_split_idx + .. autoproperty:: part_controllers +Fixed Base Robot +---------------- +Tabletop manipulators. + +.. autoclass:: robosuite.robots.fixed_base_robot.FixedBaseRobot Mobile Base Robot ----------------- @@ -55,5 +70,68 @@ Mobile Base Robot .. autoclass:: robosuite.robots.mobile_base_robot.MobileBaseRobot + .. automethod:: _load_controller + .. automethod:: load_model + .. automethod:: reset + .. automethod:: setup_references + .. automethod:: control + .. automethod:: setup_observables + .. autoproperty:: action_limits + .. autoproperty:: is_mobile + .. autoproperty:: _action_split_indexes + +Mobile robot +------------- +Base class for wheeled and legged robots. + +.. autoclass:: robosuite.robots.mobile_robot.MobileRobot + + .. automethod:: _load_controller + .. automethod:: _load_base_controller + .. automethod:: _load_torso_controller + .. automethod:: _load_head_controller + .. automethod:: load_model + .. automethod:: reset + .. automethod:: setup_references .. automethod:: control + .. automethod:: setup_observables + .. automethod:: _create_base_sensors + .. automethod:: enable_parts + .. autoproperty:: is_mobile + .. autoproperty:: base + .. autoproperty:: torso + .. autoproperty:: head + .. autoproperty:: legs + .. autoproperty:: _action_split_indexes + +Wheeled Robot +------------- +Mobile robots with wheeled bases. + +.. autoclass:: robosuite.robots.wheeled_robot.WheeledRobot + + .. automethod:: _load_controller + .. automethod:: load_model .. automethod:: reset + .. automethod:: setup_references + .. automethod:: control + .. automethod:: setup_observables + .. autoproperty:: action_limits + + +Legged Robot +------------ +Robots with legs. + +.. autoclass:: robosuite.robots.legged_robot.LeggedRobot + + .. automethod:: _load_leg_controllers + .. automethod:: _load_controller + .. automethod:: load_model + .. automethod:: reset + .. automethod:: setup_references + .. automethod:: control + .. automethod:: setup_observables + .. autoproperty:: action_limits + .. autoproperty:: is_legs_actuated + .. autoproperty:: num_leg_joints diff --git a/robosuite/controllers/composite/__init__.py b/robosuite/controllers/composite/__init__.py index 8be1766a82..84173bb274 100644 --- a/robosuite/controllers/composite/__init__.py +++ b/robosuite/controllers/composite/__init__.py @@ -1,12 +1,11 @@ from .composite_controller import CompositeController, HybridMobileBase, WholeBodyIK -from robosuite.examples.third_party_controller.mink_controller import WholeBodyMinkIK -from .composite_controller import COMPOSITE_CONTROLLERS_DICT +from .composite_controller import REGISTERED_COMPOSITE_CONTROLLERS_DICT -ALL_COMPOSITE_CONTROLLERS = COMPOSITE_CONTROLLERS_DICT.keys() +ALL_COMPOSITE_CONTROLLERS = REGISTERED_COMPOSITE_CONTROLLERS_DICT.keys() def composite_controller_factory(type, sim, robot_model, grippers): - assert type in COMPOSITE_CONTROLLERS_DICT, f"{type} controller is specified, but not imported or loaded" + assert type in REGISTERED_COMPOSITE_CONTROLLERS_DICT, f"{type} controller is specified, but not imported or loaded" # Note: Currently we assume that the init arguments are same for all composite controllers. The situation might change given new controllers in the future, and we will adjust accodingly. # The default composite controllers are explicitly initialized without using the COMPOSITE_CONTORLLERS @@ -16,7 +15,5 @@ def composite_controller_factory(type, sim, robot_model, grippers): return HybridMobileBase(sim, robot_model, grippers) elif type == "WHOLE_BODY_IK": return WholeBodyIK(sim, robot_model, grippers) - elif type == "WHOLE_BODY_MINK_IK": - return WholeBodyMinkIK(sim, robot_model, grippers) else: - return COMPOSITE_CONTROLLERS_DICT[type](sim, robot_model, grippers) + return REGISTERED_COMPOSITE_CONTROLLERS_DICT[type](sim, robot_model, grippers) diff --git a/robosuite/controllers/composite/composite_controller.py b/robosuite/controllers/composite/composite_controller.py index 359594c392..b832827c38 100644 --- a/robosuite/controllers/composite/composite_controller.py +++ b/robosuite/controllers/composite/composite_controller.py @@ -12,7 +12,7 @@ from robosuite.utils.ik_utils import IKSolver, get_nullspace_gains from robosuite.utils.log_utils import ROBOSUITE_DEFAULT_LOGGER -COMPOSITE_CONTROLLERS_DICT = {} +REGISTERED_COMPOSITE_CONTROLLERS_DICT = {} def register_composite_controller(target_class): @@ -23,7 +23,7 @@ def register_composite_controller(target_class): key = "_".join(re.sub(r"([A-Z0-9])", r" \1", target_class.__name__).split()).upper() else: key = target_class.name - COMPOSITE_CONTROLLERS_DICT[key] = target_class + REGISTERED_COMPOSITE_CONTROLLERS_DICT[key] = target_class return target_class diff --git a/robosuite/controllers/composite/composite_controller_factory.py b/robosuite/controllers/composite/composite_controller_factory.py index 7f7c3ffc49..3a67b227c5 100644 --- a/robosuite/controllers/composite/composite_controller_factory.py +++ b/robosuite/controllers/composite/composite_controller_factory.py @@ -4,7 +4,7 @@ from typing import Dict, Literal, Optional import robosuite -from robosuite.controllers.composite.composite_controller import COMPOSITE_CONTROLLERS_DICT +from robosuite.controllers.composite.composite_controller import REGISTERED_COMPOSITE_CONTROLLERS_DICT from robosuite.controllers.parts.controller_factory import load_part_controller_config from robosuite.utils.log_utils import ROBOSUITE_DEFAULT_LOGGER @@ -52,7 +52,7 @@ def load_composite_controller_config(controller: Optional[str] = None, robot: Op controller_fpath = controller else: assert ( - controller in COMPOSITE_CONTROLLERS_DICT + controller in REGISTERED_COMPOSITE_CONTROLLERS_DICT ), f"Controller {controller} not found in COMPOSITE_CONTROLLERS_DICT" # Load from robosuite/controllers/config/default/composite/ controller_name = controller.lower()