diff --git a/docs/tutorials/add_controller.md b/docs/tutorials/add_controller.md new file mode 100644 index 0000000000..9a25ce178c --- /dev/null +++ b/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": { + ... + }, + ... +} +``` \ No newline at end of file diff --git a/docs/tutorials/add_environment.md b/docs/tutorials/add_environment.md new file mode 100644 index 0000000000..b562b0a705 --- /dev/null +++ b/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) +``` + diff --git a/robosuite/robots/robot.py b/robosuite/robots/robot.py index 74783a9b97..9a2bade2a5 100644 --- a/robosuite/robots/robot.py +++ b/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 diff --git a/robosuite/scripts/collect_human_demonstrations.py b/robosuite/scripts/collect_human_demonstrations.py index 7f4ec128ab..a78ad2d639 100644 --- a/robosuite/scripts/collect_human_demonstrations.py +++ b/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()