Merge pull request #528 from ARISE-Initiative/v1.5.0

V1.5.0 docs

docs/tutorials/add_controller.md

@@ -0,0 +1,55 @@
+
+## Adding Third Party Controllers
+
+To use a third-party controller with robosuite, you'll need to:
+1. Create a new class that subclasses one of the composite controllers in `robosuite/controllers/composite/composite_controller.py`.
+2. Register the composite controller with the decorator `@register_composite_controller`.
+3. Implement composite specific functionality that ultimately provides control input to the underlying `part_controller`'s.
+4. Import the new class so that it gets added to robosuite's `REGISTERED_COMPOSITE_CONTROLLERS_DICT` via the `@register_composite_controller` decorator.
+5. Provide controller specific configs and the new controller's `type` in a json file.
+
+For the new composite controllers subclassing `WholeBody`, you'll mainly need to update `joint_action_policy`.
+
+We provide an example of how to use a third-party `WholeBodyMinkIK` composite controller with robosuite, in the `robosuite/examples/third_party_controller/` directory. You can run the command `python teleop_mink.py` example script to see a third-party controller in action. Note: to run this specific example, you'll need to `pip install mink`.
+
+
+Steps 1 and 2:
+
+In `robosuite/examples/third_party_controller/mink_controller.py`:
+
+```
+@register_composite_controller
+class WholeBodyMinkIK(WholeBody):
+    name = "WHOLE_BODY_MINK_IK"
+```
+
+Step 3:
+
+In `robosuite/examples/third_party_controller/mink_controller.py`, add logic specific to the new composite controller:
+
+```
+self.joint_action_policy = IKSolverMink(...)
+```
+
+Step 4:
+
+In `teleop_mink.py`, we import:
+
+```
+from robosuite.examples.third_party_controller.mink_controller import WholeBodyMinkIK
+```
+
+Step 5:
+
+In `robosuite/examples/third_party_controller/default_mink_ik_gr1.json`, we add configs specific to our new composite controller. and also set the `type` to
+match the `name` specified in `WholeBodyMinkIK`:
+
+```
+{
+    "type": "WHOLE_BODY_MINK_IK",  # set the correct type
+    "composite_controller_specific_configs": {
+            ...
+    },
+    ...
+}
+```

docs/tutorials/add_environment.md

@@ -0,0 +1,65 @@
+## 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)
+```
+

robosuite/robots/robot.py

@@ -2,6 +2,7 @@
 import json
 import os
 from collections import OrderedDict
+from typing import Optional
 
 import numpy as np
 
@@ -504,7 +505,7 @@ def check_q_limits(self):
             bool: True if this arm is near its joint limits
         """
         tolerance = 0.1
-        for (qidx, (q, q_limits)) in enumerate(
+        for qidx, (q, q_limits) in enumerate(
             zip(self.sim.data.qpos[self._ref_joint_pos_indexes], self.sim.model.jnt_range[self._ref_joint_indexes])
         ):
             if q_limits[0] != q_limits[1] and not (q_limits[0] + tolerance < q < q_limits[1] - tolerance):
@@ -613,6 +614,23 @@ def set_robot_joint_positions(self, jpos):
         self.sim.data.qpos[self._ref_joint_pos_indexes] = jpos
         self.sim.forward()
 
+    def set_gripper_joint_positions(self, jpos: np.ndarray, gripper_arm: Optional[str] = None):
+        """
+        Helper method to force gripper joint positions to the passed values.
+
+        Args:
+            jpos (np.array): Joint jpos to manually set the gripper to
+            gripper_arm: arm corresponding to the gripper for which to set the gripper joint jpos.
+                If None, use default arm.
+        """
+        if gripper_arm is None:
+            gripper_arm = self.arms[0]
+        if self.has_gripper[gripper_arm]:
+            self.sim.data.qpos[self._ref_gripper_joint_pos_indexes[gripper_arm]] = jpos
+            self.sim.forward()
+        else:
+            raise ValueError(f"No gripper found for arm {gripper_arm}")
+
     @property
     def js_energy(self):
         """
@@ -919,7 +937,7 @@ def create_action_vector(self, action_dict):
             return self.composite_controller.create_action_vector(action_dict)
         else:
             full_action_vector = np.zeros(self.action_dim)
-            for (part_name, action_vector) in action_dict.items():
+            for part_name, action_vector in action_dict.items():
                 if part_name not in self._action_split_indexes:
                     ROBOSUITE_DEFAULT_LOGGER.debug(f"{part_name} is not specified in the action space")
                     continue

robosuite/scripts/collect_human_demonstrations.py

@@ -84,6 +84,8 @@ def collect_human_trajectory(env, device, arm):
         env_action = [robot.create_action_vector(all_prev_gripper_actions[i]) for i, robot in enumerate(env.robots)]
         env_action[device.active_robot] = active_robot.create_action_vector(action_dict)
         env_action = np.concatenate(env_action)
+        for gripper_ac in all_prev_gripper_actions[device.active_robot]:
+            all_prev_gripper_actions[device.active_robot][gripper_ac] = action_dict[gripper_ac]
 
         env.step(env_action)
         env.render()