init

.gitignore

@@ -0,0 +1,135 @@
+.vscode
+data
+outputs
+**/.ipynb_checkpoints
+*.ipynb
+
+# Byte-compiled / optimized / DLL files
+__pycache__/
+*.py[cod]
+*$py.class
+
+# C extensions
+*.so
+
+# Distribution / packaging
+.Python
+build/
+develop-eggs/
+dist/
+downloads/
+eggs/
+.eggs/
+lib/
+lib64/
+parts/
+sdist/
+var/
+wheels/
+pip-wheel-metadata/
+share/python-wheels/
+*.egg-info/
+.installed.cfg
+*.egg
+MANIFEST
+
+# PyInstaller
+#  Usually these files are written by a python script from a template
+#  before PyInstaller builds the exe, so as to inject date/other infos into it.
+*.manifest
+*.spec
+
+# Installer logs
+pip-log.txt
+pip-delete-this-directory.txt
+
+# Unit test / coverage reports
+htmlcov/
+.tox/
+.nox/
+.coverage
+.coverage.*
+.cache
+nosetests.xml
+coverage.xml
+*.cover
+*.py,cover
+.hypothesis/
+.pytest_cache/
+
+# Translations
+*.mo
+*.pot
+
+# Django stuff:
+*.log
+local_settings.py
+db.sqlite3
+db.sqlite3-journal
+
+# Flask stuff:
+instance/
+.webassets-cache
+
+# Scrapy stuff:
+.scrapy
+
+# Sphinx documentation
+docs/_build/
+
+# PyBuilder
+target/
+
+# Jupyter Notebook
+.ipynb_checkpoints
+
+# IPython
+profile_default/
+ipython_config.py
+
+# pyenv
+.python-version
+
+# pipenv
+#   According to pypa/pipenv#598, it is recommended to include Pipfile.lock in version control.
+#   However, in case of collaboration, if having platform-specific dependencies or dependencies
+#   having no cross-platform support, pipenv may install dependencies that don't work, or not
+#   install all needed dependencies.
+#Pipfile.lock
+
+# PEP 582; used by e.g. github.com/David-OConnor/pyflow
+__pypackages__/
+
+# Celery stuff
+celerybeat-schedule
+celerybeat.pid
+
+# SageMath parsed files
+*.sage.py
+
+# Environments
+.env
+.venv
+env/
+venv/
+ENV/
+env.bak/
+venv.bak/
+
+# Spyder project settings
+.spyderproject
+.spyproject
+
+# Rope project settings
+.ropeproject
+
+# mkdocs documentation
+/site
+
+# mypy
+.mypy_cache/
+.dmypy.json
+dmypy.json
+
+# Pyre type checker
+.pyre/

LICENSE

@@ -0,0 +1,21 @@
+MIT License
+
+Copyright (c) 2022 Cheng Chi
+
+Permission is hereby granted, free of charge, to any person obtaining a copy
+of this software and associated documentation files (the "Software"), to deal
+in the Software without restriction, including without limitation the rights
+to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+copies of the Software, and to permit persons to whom the Software is
+furnished to do so, subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included in all
+copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+SOFTWARE.

README.md

@@ -0,0 +1,116 @@
+# Iterative Residual Policy
+This repository contains the source code for the paper [Iterative Residual Policy for Goal-Conditioned Dynamic Manipulation of Deformable Objects](https://irp.cs.columbia.edu/). This paper has been accepted to RSS 2022.
+
+<img src="images/teaser.jpg" alt="drawing" width="500"/>
+
+## Cite this work
+```
+@inproceedings{chi2022irp,
+  title={Iterative Residual Policy for Goal-Conditioned Dynamic Manipulation of Deformable Objects},
+  author={Chi, Cheng and Burchfiel, Benjamin and Cousineau, Eric and Feng, Siyuan and Song, Shuran},
+  booktitle={The IEEE International Conference on Computer Vision (ICCV)},
+  year={2022}
+}
+```
+
+## Datasets
+IRP Rope Dataset [required for eval] (7.63GB)
+* [Google Drive (Recommended)](https://drive.google.com/file/d/1uv7APODe6yl3vTaBLfOOz5Q9APdZdkqd/view?usp=sharing) 
+* [Columbia Server](https://irp.cs.columbia.edu/data/irp_rope.zarr.tar)
+
+## Pretrained Models
+IRP Rope [action + tracking] (914MB)
+* [Google Drive (Recommended)](https://drive.google.com/file/d/1_lODhez-JeGKbQvxHHfD3NBHBT-TSvrD/view?usp=sharing) 
+* [Columbia Server](https://irp.cs.columbia.edu/data/checkpoints.tar)
+
+## Usage
+### Installation (🖥️ Basic)
+A conda [environment.yml](./environment.yml) for `python=3.8, pytorch=1.9.0 and cudatoolkit=11.2` is provided. 
+```
+conda env create --file environment.yml
+```
+Please try [mambaforge](https://github.com/conda-forge/miniforge) for better dependency conflict resolution.
+```
+mamba env create --file environment.yml
+```
+
+### Installation (👾 Simulation)
+* Install [Mujoco 2.1.0](https://github.com/deepmind/mujoco/releases/tag/2.1.0)
+* Install [mujoco-py 2.0.2](https://github.com/openai/mujoco-py) and carefully follow instructions.
+* Install [abr_control](https://github.com/cheng-chi/abr_control) with Mujoco.
+
+### Installation (🦾 Real Robot)
+#### Hardware
+* [Stereolabs ZED 2i Camera](https://www.stereolabs.com/zed-2i/)
+* [UR5-CB3](https://www.universal-robots.com/cb3) or [UR5e](https://www.universal-robots.com/products/ur5-robot/) ([RTDE Interface](https://www.universal-robots.com/articles/ur/interface-communication/real-time-data-exchange-rtde-guide/) is required)
+* [Millibar Robotics Manual Tool Changer](https://www.millibar.com/manual-tool-changer/) (only need robot side)
+* 3D Print [Quick Change Plate](https://cad.onshape.com/documents/2298872dc9e43725186484ff/w/a74a4b142d00ea670de5fc6b/e/dcf3c1f5418eb0ffac59e848?renderMode=0&uiState=625c8355314d4c5f8c688959) to mount the wooden extension stick to EEF.
+* 3/8 inch Square Wooden Dowel
+* [8mm Cotton Rope](https://www.amazon.com/gp/product/B08TWMNV4P)
+* Wood screws
+* Duct tape 😛
+
+#### Software
+* Install [Zed SDK](https://www.stereolabs.com/developers/release/) and [pyzed](https://www.stereolabs.com/docs/app-development/python/install/)
+* Install [ur_rtde](https://sdurobotics.gitlab.io/ur_rtde/)
+
+### Evaluation (🖥️ Basic)
+Under project root (i.e. `irp/`), create `data` folder and download [IRP Rope Dataset](#datasets) as well as [Pretrained Models](#pretrained-models). Extract tar files
+
+```
+$ cd data
+$ tar -xvf checkpoints.tar
+$ tar -xvf irp_rope.zarr.tar
+```
+Activate environment
+```
+$ conda activate irp
+(irp) $
+```
+Run dataset evaluation script. Use `action.gpu_id` to select GPU on multi-GPU systems.
+```
+(irp) $ python eval_irp_rope_dataset.py action.gpu_id=0
+```
+
+A `log.pkl` file will be saved to the ad-hoc output directory created by [hydra](https://hydra.cc/docs/intro/). Add command-line argument `offline=False` to enable wandb logging (recommended).
+
+Numbers reported in our paper is generated using this method.
+
+### Evaluation (👾 Simulation)
+Extract data and checkpoints following [basic](#evaluation-🖥️-basic) (required).
+Install dependencies following [sim installation](#installation-👾-simulation).
+
+Run simulation evaluation script.
+```
+(irp) $ python eval_irp_rope_dataset.py action.gpu_id=0
+```
+
+Note that this method has not been extensively tested and is mainly provided for future development. Mujoco might crash due to numerical instability (i.e. Nan), which is better handled in the dataset.
+
+### Evaluation (🦾 Real Robot)
+Extract data and checkpoints following [basic](#evaluation-🖥️-basic) (required).
+Install dependencies following [real installation](#installation-🦾-real-robot).
+
+Initialize UR5 robot and write down `<robot_ip>`. Move robot in teach mode close to joint configuration `[-90,-70,150,-170,-90,-90]` to prevent unexpected movement.
+
+Run `ur5_camera_calibration_app` to create homography calibration file to `data/calibration.pkl`. Use `A/D` keys to move `j2` and `W/S` keys to move `j3`. Click on the tip of the wooden extension for calibration.
+```
+(irp) $ python ur5_camera_calibration_app.py --ip <robot_ip> -o data/calibration.pkl
+```
+Example for calibration result, the red and blue crosses should be fairly close.
+<img src="images/calib_example.png" alt="drawing" width="400"/>
+
+
+Run real eval script.
+```
+(irp) $ python eval_irp_rope_real.py
+```
+Result and videos will be saved to the hydra ad-hoc output directory.
+
+### Training
+To train IRP model from scratch:
+```
+(irp) $ python train_irp.py
+```
+
+In case of inaccurate tracking, use `video_labeler.py` to generate tracking labels and `train_tracker.py` to train tracking model.

abr_control_mod/mujoco_utils.py

@@ -0,0 +1,121 @@
+from typing import Sequence, Tuple
+
+import numpy as np
+import mujoco_py as mjp
+
+def get_rope_body_ids(
+        model: mjp.cymj.PyMjModel, 
+        prefix='B',
+        check_topology=True) -> np.ndarray:
+
+    rope_body_names = list()
+    name_idx_map = dict()
+    for body_name in model.body_names:
+        if body_name.startswith(prefix):
+            rope_body_names.append(body_name)
+            name_idx_map[body_name] = int(body_name.strip(prefix))
+
+    rope_body_names = sorted(rope_body_names, key=lambda x: name_idx_map[x])
+    rope_body_ids = [model.body_name2id(x) for x in rope_body_names]
+
+    if check_topology:
+        for i in range(1, len(rope_body_ids)):
+            assert(model.body_parentid[rope_body_ids[i]] \
+                == rope_body_ids[i-1])
+    return np.array(rope_body_ids)
+
+def get_cloth_body_ids(
+        model: mjp.cymj.PyMjModel,
+        prefix='B') -> np.ndarray:
+    name_idx_map = dict()
+    imax = 0
+    jmax = 0
+    for body_name in model.body_names:
+        if body_name.startswith(prefix):
+            i, j = [int(x) for x in body_name.strip(prefix).split('_')]
+            name_idx_map[body_name] = (i,j)
+            imax = max(imax, i)
+            jmax = max(jmax, j)
+
+    id_arr = np.zeros((imax+1, jmax+1), dtype=np.int64)
+    for key, value in name_idx_map.items():
+        body_id = model.body_name2id(key)
+        id_arr[value] = body_id
+    return id_arr
+
+def get_body_center_of_mass(
+        data: mjp.cymj.PyMjData,
+        body_ids: np.ndarray
+        ) -> np.ndarray:
+    return data.xipos[body_ids]
+
+def apply_force_com(
+        model: mjp.cymj.PyMjModel, 
+        data: mjp.cymj.PyMjData,
+        body_id: int,
+        force: np.ndarray):
+    com_point = data.xipos[body_id]
+    torque = np.zeros(3)
+    mjp.functions.mj_applyFT(
+        model, data, 
+        force, torque, com_point, 
+        body_id, data.qfrc_applied)
+
+def apply_force_com_batch(
+        model: mjp.cymj.PyMjModel, 
+        data: mjp.cymj.PyMjData,
+        body_ids: Sequence[int],
+        forces: np.ndarray):
+    assert(len(body_ids) == len(forces))
+    for i in range(len(body_ids)):
+        apply_force_com(model, data, body_ids[i], forces[i])
+
+def clear_forces(data):
+    data.qfrc_applied[:] = 0
+
+def apply_impulse_com_batch(
+        sim: mjp.cymj.MjSim,
+        body_ids: Sequence[int],
+        impulses: np.ndarray):
+
+    dt = sim.model.opt.timestep
+    forces = impulses / dt
+    apply_force_com_batch(
+        model=sim.model, data=sim.data,
+        body_ids=body_ids,
+        forces=forces)
+    sim.step()
+    clear_forces(sim.data)
+
+def get_rope_dof_idxs(
+        model: mjp.cymj.PyMjModel, 
+        prefix='B',
+        check_topology=True
+        ) -> Tuple[np.ndarray, np.ndarray]:
+    rope_body_ids = get_rope_body_ids(
+        model, prefix, check_topology)
+
+    dof_body_ids = list()
+    dof_idxs = list()
+    for body_id in rope_body_ids:
+        num_dof = model.body_dofnum[body_id]
+        if num_dof == 0:
+            continue
+        assert(num_dof == 2)
+        dof_adr = model.body_dofadr[body_id]
+        idxs = [dof_adr + i for i in range(num_dof)]
+        dof_idxs.append(idxs)
+        dof_body_ids.append(body_id)
+    return np.array(dof_idxs), np.array(dof_body_ids)
+
+def get_mujoco_state(sim):
+    q = np.copy(sim.data.qpos)
+    dq = np.copy(sim.data.qvel)
+    u = np.copy(sim.data.ctrl)
+    return (q, dq, u)
+
+def set_mujoco_state(sim, state):
+    q, dq, u = state
+    sim.data.qpos[:] = q
+    sim.data.qvel[:] = dq
+    sim.data.ctrl[:] = u