Tutorial on how to add robots

docs/source/user_guide/concepts/controllers.md

@@ -1,23 +1,54 @@
 # Controllers / Action Spaces
 
-Controllers are interfaces between policies and robots. The policy outputs actions to the controller, and the controller converts actions to control signals to the robot. For example, the `arm_pd_ee_delta_pose` controller takes the relative movement of the end-effector as input, and uses [inverse kinematics](https://en.wikipedia.org/wiki/Inverse_kinematics) to convert input actions to target positions of robot joints. The robot uses a [PD controller](https://en.wikipedia.org/wiki/PID_controller) to drive motors to achieve target joint positions.
+Controllers are interfaces between users/policies and robots. Whenever you take a step in an environment and provide an action, that action is sent to a chosen controller that converts actions to control signals to the robot. At the lowest level, all robots in simulation are controlled via joint position or joint velocity controls, effectively specifying where / how fast each joint should go.
 
-**The controller defines the action space of an task.** The robot can have separate controllers for its arm, gripper, and other components. The action space is a concatenation of the action spaces of all controllers.
+For example, the `arm_pd_ee_delta_pose` controller takes the relative movement of the end-effector as input, and uses [inverse kinematics](https://en.wikipedia.org/wiki/Inverse_kinematics) to convert input actions to target positions of robot joints. The robot uses a [PD controller](https://en.wikipedia.org/wiki/PID_controller) to drive motors to achieve target joint positions.
 
-Note that while `pd_ee_delta_pose` type controllers that use IK may be more sample efficient to train / learn from for RL workflows, in GPU simulation running these controllers is not that fast and may slow down RL training.
+There are a few key elements to remember about controllers in ManiSkill
+- The controller defines the action space of a task 
+- The robot can have separate controllers for different groups of joints. The action space is a concatenation of the action spaces of all controllers
+- A single robot may have several sets of controllers that can be used
 
-## Terminology
+
+<!-- Note that while `pd_ee_delta_pose` type controllers that use IK may be more sample efficient to train / learn from for RL workflows, in GPU simulation running these controllers is not that fast and may slow down RL training. -->
+
+The next section will detail each of the pre-built controllers and what they do
+
+## Prebuilt Controllers (Docs WIP)
+
+
+### Passive
+
+```python
+from mani_skill.agents.controllers import PassiveControllerConfig
+```
+
+This controller lets you enforce given joints to be not controlled by actions. An example of this is used for the [CartPole environment](https://github.com/haosulab/ManiSkill2/blob/dev/mani_skill/envs/tasks/control/cartpole.py) which defines the CartPole robot as having passive control over the hinge joint of the CartPole (the CartPole task only allows control of the sliding box).
+
+### PD Joint Position
+
+```python
+from mani_skill.agents.controllers import PDJointPosControllerConfig
+```
+
+With a PD controller, controls the joint positions of the given joints via actions.
+
+## Deep Dive Example of the Franka Emika Panda Robot Controllers:
+
+To help detail how controllers work in detail, below we explain with formulae how the controllers are controlling the root in simulation.
+
+### Terminology
 
 - fixed joint: a joint that can not be controlled. The degree of freedom (DoF) is 0.
 - `qpos` ( $q$ ): controllable joint positions
 - `qvel` ( $\dot{q}$ ): controllable joint velocities
 - target joint position ( $\bar{q}$ ): target position of the motor which drives the joint
 - target joint velocity ( $\bar{\dot{q}}$ ): target velocity of the motor which drives the joint
 - PD controller: control loop based on $F(t) = K_p (\bar{q}(t) - q(t)) + K_d (\bar{\dot{q}}(t) - \dot{q}(t))$. $K_p$ (stiffness) and $K_d$ (damping) are hyperparameters. $F(t)$ is the force of the motor.
-- Augmented PD controller: Passive forces (like gravity) are compensated for the PD controller.
+- Augmented PD controller: Passive forces (like gravity) can be compensated by disabling gravity (they can be computed but this is slow and not necessary).
 - action ( $a$ ): the input of the controller, also the output of the policy
 
-## Supported Controllers (Franka Emika)
+### Supported Controllers
 
 The robot is [Franka Emika](https://github.com/frankaemika/franka_ros), a.k.a Panda. The DoF of the arm is 7. **We use the tool center point (TCP), which is the center between two fingers, as the end-effector.**
 

docs/source/user_guide/tutorials/adding_robots.md → docs/source/user_guide/tutorials/custom_robots.md

@@ -1,4 +1,4 @@
-# Adding Robots
+# Custom Robots
 
 ManiSkill supports importing robots and assets via URDF and MJCF definitions. As ManiSkill was designed to allow one to flexibly change mounted sensors, controllers, and robot embodiments, ManiSkill at the moment does not automatically import those elements into the created robot, they must be defined by you.
 
@@ -7,23 +7,25 @@ In summary, the following elements must be completed before a robot is usable.
 1. Create a robot class and specify a uid, as well as a urdf/mjcf file to import
 2. Define robot controller(s) (e.g. PD joint position control)
 3. (Optional): Define mounted sensors / sensors relative to the robot (e.g. wrist cameras)
+4. (Optional): Define useful keyframes (snapshots of robot state) for users
 
 This tutorial will guide you through on how to implement the Panda robot in ManiSkill. It will also cover some tips/tricks for modelling other categories of robots to ensure the simulation runs fast and accurately (e.g., mobile manipulators like Fetch, quadrupeds like Anymal)
 
 
-## 1. Robot Class and Importing Robot
+## 1. Robot Class and Importing the Robot
 
 To create your own robot (also known as an Agent) you need to inherit the `BaseAgent` class, give it name, and optionally register the agent.
 
 ```python
+import sapien
 from mani_skill.agents.base_agent import BaseAgent
 from mani_skill.agents.registration import register_agent
 @register_agent()
 class MyPanda(BaseAgent):
     uid = "my_panda"
 ```
 
-Registering the agent allows you to create environments that instantiate your robot for you via a string uid. Generally you can now run the following to visualize the robot by ID. 
+Registering the agent allows you to create environments that instantiate your robot for you via a string uid in the future with the code below:
 
 ```python
 env = gym.make("EmptyEnv-v1", robot_uids="my_panda")
@@ -35,6 +37,8 @@ With the base robot class started, now we need to import a robot definition from
 
 To import a URDF/MJCF file, you simply provide a path to the definition file and ManiSkill handles the importing. ManiSkill will automatically create the appropriate articulation with links and joints defined in the given definition file. Internally ManiSkill uses either the URDFLoader or MJCFLoader class in the `mani_skill.utils.building` module.
 
+<!-- TODO (stao): provide some demo script / checkpointer code to let user visualize progress in tutorial -->
+
 #### URDF
 
 To get started, you first need to get a valid URDF file like this one for the Panda robot: https://github.com/haosulab/ManiSkill2/blob/dev/mani_skill/assets/robots/panda/panda_v2.urdf
@@ -47,6 +51,7 @@ class MyPanda(BaseAgent):
     urdf_path = f"path/to/your/robot.urdf"
 ```
 
+Note that there are a number of common issues users may face (often due to incorrectly formatted URDFs / collision meshes) which are documented in the [FAQ / Troubleshooting Section](#faq--troubleshooting)
 
 
 <!-- For a starting template check out https://github.com/haosulab/ManiSkill2/blob/dev/mani_skill/agents/robots/_template/template_robot.py -->
@@ -109,7 +114,6 @@ class Panda(BaseAgent):
         "panda_finger_joint1",
         "panda_finger_joint2",
     ]
-    ee_link_name = "panda_hand_tcp"
 
     arm_stiffness = 1e3
     arm_damping = 1e2
@@ -162,8 +166,63 @@ class Panda(BaseAgent):
 
 We defined two controllers to control the arm joints and one for the gripper. Using a dictionary, you can define multiple control modes that interchangeably use different controllers of the joints. Above we defined a `pd_joint_delta_pos` and a `pd_joint_pos` controller which switch just the controller of the arm joints.
 
-Stiffness corresponds with the P and damping corresponds with the D of PD controllers, see the [controllers page](../concepts/controllers.md#terminology) for more details
+Stiffness corresponds with the P and damping corresponds with the D of PD controllers, see the [controllers page](../concepts/controllers.md#terminology) for more details.
+
+Tuning the values of stiffness, damping, and other properties affect the sim2real transfer of a simulated robot to the real world. At the moment our team is working on developing a better pipeline with documentation for system identification to pick better controllers and/or hyperparameters. 
+
+Note that when taking a robot implemented in another simulator like Mujoco, you usually cannot directly copy the joint hyperparameters to ManiSkill, so you almost always need some manual tuning. 
+
+## 3. Defining Sensors
+
+ManiSkill supports defining sensors mounted onto the robot and sensors positioned relative to the robot by defining the `_sensor_configs` property.
+
+An example of this done in the Panda robot with a real sense camera attached:
+
+```python
+from mani_skill.sensors.camera import CameraConfig
+class PandaRealSensed435(Panda):
+    # ...
+    @property
+    def _sensor_configs(self):
+        return [
+            CameraConfig(
+                uid="hand_camera",
+                pose=sapien.Pose(p=[0, 0, 0], q=[1, 0, 0, 0]),
+                width=128,
+                height=128,
+                fov=np.pi / 2,
+                near=0.01,
+                far=100,
+                mount=self.robot.links_map["camera_link"],
+            )
+        ]
+```
+
+You simply return a sensor config (here we use a CameraConfig) to define the sensor to add, and specify where to mount the sensor. For most sensors, you must define a pose, which is now used as a pose relative to the mount pose. In the example above we add a camera to the camera link / wrist mount of the panda robot (which is already oriented facing the correct direction so the pose defined is just the identity)
+
+## 4. Defining Keyframes
+
+It sometimes is useful to define some predefined robot poses and joint positions that users can initialize to to visualize the robot in poses of interest. This is an idea adopted from [Mujoco's keyframes](https://mujoco.readthedocs.io/en/stable/XMLreference.html#keyframe)
+
+For example, we define a "standing" keyframe for the Unitree H1 robot like so
+
+```python
+from mani_skill.agents.base_agent import BaseAgent, Keyframe
+# ...
+class UnitreeH1(BaseAgent):
+    # ...
+    keyframes = dict(
+        standing=Keyframe(
+            pose=sapien.Pose(p=[0, 0, 0.975]),
+            qpos=np.array([0, 0, 0, 0, 0, 0, 0, -0.4, -0.4, 0.0, 0.0, 0.8, 0.8, 0.0, 0.0, -0.4, -0.4, 0.0, 0.0]) * 1,
+        )
+    )
+```
+
+The keyframe can also specify `qvel` values as well. Using that keyframe you can set the robot to the given pose, qpos, qvel and you can get the desired predefined keyframe
 
+:::{figure} images/unitree_h1_standing.png 
+:::
 
 ## Advanced Tips and Tricks:
 
@@ -173,6 +232,32 @@ Robots like Fetch have a mobile base, which allows translational movement and ro
 
 Instead, similar to many other simulators a "fake" mobile base is made (that is realistic enough to easily do sim2real transfer in terms of the controller). This is made by modifying a URDF of a robot like Fetch, and adding joints that let the base link translate (prismatic joint) and rotate (revolute joint). 
 
+### Tactile Sensing
+
+WIP
+
+For now see the implementation of [Allegro hand with touch sensors](https://github.com/haosulab/ManiSkill2/blob/dev/mani_skill/agents/robots/allegro_hand/allegro_touch.py)
+
 ### Quadrupeds / Legged motion
 
-WIP
+WIP
+
+## FAQ / Troubleshooting
+
+### On Importing URDF files
+
+**Loaded robot does not have the right render materials / colors showing up:**
+Likely caused by improper use of `<material>` tags in the URDF. Double check the material tags each have unique names and are correctly written according to the URDF format
+
+**The collision of the robot seems off (e.g. sinks through floor, objects that should collide are not colliding etc.):**
+
+In the viewer when visualizing the robot you created, click any link on the robot and under the Articulation tab scroll down and click Show collision. This visualizes all collision meshes used for contact simulation and shows you what was loaded from the URDF. You can then edit / modify the `<collision>` tags of the URDF accordingly
+
+**The collision shape looks completely different from the visual (like a convex version of it)**
+
+This can be caused by a few reasons. One may be that your defined base agent has its `load_multiple_collisions` property set to False. If the collision meshes you use have multiple convex shapes that can be loaded (preferrably a .ply or .glb format), then setting `load_multiple_collisions = True` in your custom robot class can work.
+
+Another reason is if your collision mesh is in the .stl format. Our loader has some issues loading .stl files at times and we recommend converting them to `.glb` as that is the easiest for our system to load and interpret. 
+
+Another issue is if your collision mesh does not have multiple convex shapes, you may have to decompose those meshes yourself via a tool like COACD.
+<!-- TODO (stao): Detail a pipeline to semi-automatically do this -->

docs/source/user_guide/tutorials/index.md

@@ -15,7 +15,7 @@ These are tutorials written by the maintainers of ManiSkill and the community, s
 
 custom_tasks
 custom_tasks_advanced
-adding_robots
+custom_robots
 custom_reusable_scenes
 domain_randomization
 ```

mani_skill/agents/robots/panda/panda_realsensed435.py

@@ -1,10 +1,10 @@
 import numpy as np
+import sapien
 
 from mani_skill import PACKAGE_ASSET_DIR
 from mani_skill.agents.registration import register_agent
 from mani_skill.sensors.camera import CameraConfig
 from mani_skill.utils import sapien_utils
-from mani_skill.utils.structs import Pose
 
 from .panda import Panda
 
@@ -21,12 +21,12 @@ def _sensor_configs(self):
         return [
             CameraConfig(
                 uid="hand_camera",
-                pose=Pose.create_from_pq([0, 0, 0], [1, 0, 0, 0]),
+                pose=sapien.Pose(p=[0, 0, 0], q=[1, 0, 0, 0]),
                 width=128,
                 height=128,
                 fov=np.pi / 2,
                 near=0.01,
                 far=100,
-                mount=sapien_utils.get_obj_by_name(self.robot.links, "camera_link"),
+                mount=self.robot.links_map["camera_link"],
             )
         ]

mani_skill/envs/tasks/pick_clutter_ycb.py

@@ -48,7 +48,7 @@ def __init__(
             raise FileNotFoundError(
                 f"Episode json ({episode_json}) is not found."
                 "To download default json:"
-                "`python -m mani_skill2.utils.download_asset pick_clutter_ycb`."
+                "`python -m mani_skill.utils.download_asset pick_clutter_ycb`."
             )
         self._episodes: List[Dict] = load_json(episode_json)
 
@@ -121,7 +121,8 @@ def _load_scene(self, options: dict):
                 obj = builder.build(name=f"set_{i}_{actor_cfg['model_id']}")
                 all_objects.append(obj)
                 if actor_cfg["rep_pts"] is not None:
-                    # TODO (stao): what is rep_pts?, this is taken from ms2 code
+                    # rep_pts is representative points, representing visible points
+                    # we only permit selecting target objects that are visible
                     self.selectable_target_objects[-1].append(obj)
 
         self.all_objects = Actor.merge(all_objects, name="all_objects")

mani_skill/examples/demo_robot.py

@@ -0,0 +1,34 @@
+"""
+Instantiates a empty environment with a floor, and attempts to place any given robot in there
+"""
+
+import sys
+
+import gymnasium as gym
+
+import mani_skill.envs
+from mani_skill.envs.sapien_env import BaseEnv
+
+if __name__ == "__main__":
+    robot = sys.argv[1]
+    # robot in ["panda", "fetch", "xmate3_robotiq"]:
+    env = gym.make(
+        "Empty-v1",
+        enable_shadow=True,
+        robot_uids=robot,
+        render_mode="human",
+        # control_mode="arm_pd_ee_delta_pose_gripper_pd_joint_pos",
+        # shader_dir="rt-fast",
+    )
+    env.reset(seed=0)
+    env: BaseEnv = env.unwrapped
+    viewer = env.render()
+    viewer.paused = True
+
+    while True:
+        env.step(env.action_space.sample())
+        viewer = env.render()
+        # if viewer.window.key_press("n"):
+        #     env.close()
+        #     del env
+        #     break

mani_skill/utils/structs/articulation.py

@@ -28,7 +28,7 @@ class Articulation(BaseStruct[physx.PhysxArticulation]):
 
     links: List[Link]
     """List of Link objects"""
-    link_map: OrderedDict[str, Link]
+    links_map: OrderedDict[str, Link]
     """Maps link name to the Link object"""
     root: Link
     """The root Link object"""
@@ -88,7 +88,7 @@ def create_from_physx_articulations(
             _scene=scene,
             _scene_idxs=scene_idxs,
             links=[],
-            link_map=OrderedDict(),
+            links_map=OrderedDict(),
             root=None,
             joints=[],
             joints_map=OrderedDict(),
@@ -127,7 +127,7 @@ def create_from_physx_articulations(
             if wrapped_link.is_root.any():
                 root = wrapped_link
         self.links = wrapped_links
-        self.link_map = link_map
+        self.links_map = link_map
         self.root = root
 
         # create Joint objects and figure out active joint references
@@ -273,7 +273,7 @@ def get_net_contact_forces(self, link_names: Union[List[str], Tuple[str]]):
             if tuple(link_names) not in self._net_contact_force_queries:
                 bodies = []
                 for k in link_names:
-                    bodies += self.link_map[k]._bodies
+                    bodies += self.links_map[k]._bodies
                 self._net_contact_force_queries[
                     tuple(link_names)
                 ] = self.px.gpu_create_contact_body_impulse_query(bodies)
@@ -291,7 +291,7 @@ def get_net_contact_forces(self, link_names: Union[List[str], Tuple[str]]):
             body_contacts = sapien_utils.get_articulation_contacts(
                 self.px.get_contacts(),
                 self._objs[0],
-                included_links=[self.link_map[k]._objs[0] for k in link_names],
+                included_links=[self.links_map[k]._objs[0] for k in link_names],
             )
             net_force = (
                 common.to_tensor(sapien_utils.compute_total_impulse(body_contacts))
@@ -323,7 +323,7 @@ def find_link_by_name(self, arg0: str) -> Link:
             raise RuntimeError(
                 "Cannot call find_link_by_name when the articulation object is managing articulations of different dofs"
             )
-        return self.link_map[arg0]
+        return self.links_map[arg0]
 
     def get_active_joints(self):
         return self.active_joints

mani_skill/utils/structs/articulation_joint.py

@@ -55,15 +55,15 @@ def create(
         parent_link = None
         if not articulation._merged:
             if physx_joints[0].child_link is not None:
-                child_link = articulation.link_map[
+                child_link = articulation.links_map[
                     "_".join(
                         physx_joints[0]
                         .child_link.name.replace(articulation.name, "", 1)
                         .split("_")[1:]
                     )
                 ]
             if physx_joints[0].parent_link is not None:
-                parent_link = articulation.link_map[
+                parent_link = articulation.links_map[
                     "_".join(
                         physx_joints[0]
                         .parent_link.name.replace(articulation.name, "", 1)