Add Franka Simulation package to SERL

README.md

@@ -0,0 +1,5 @@
+# SERL: A Software Suite for Sample-Efficient Robotic Reinforcement Learning
+
+## Installation
+- Conda Environment:
+    - create an environment with `conda create -n serl_dev python=3.10`

franka_sim/.gitignore

@@ -0,0 +1,162 @@
+# 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/
+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/
+cover/
+
+# 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
+.pybuilder/
+target/
+
+# Jupyter Notebook
+.ipynb_checkpoints
+
+# IPython
+profile_default/
+ipython_config.py
+
+# pyenv
+#   For a library or package, you might want to ignore these files since the code is
+#   intended to run in multiple environments; otherwise, check them in:
+# .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
+
+# poetry
+#   Similar to Pipfile.lock, it is generally recommended to include poetry.lock in version control.
+#   This is especially recommended for binary packages to ensure reproducibility, and is more
+#   commonly ignored for libraries.
+#   https://python-poetry.org/docs/basic-usage/#commit-your-poetrylock-file-to-version-control
+#poetry.lock
+
+# pdm
+#   Similar to Pipfile.lock, it is generally recommended to include pdm.lock in version control.
+#pdm.lock
+#   pdm stores project-wide configurations in .pdm.toml, but it is recommended to not include it
+#   in version control.
+#   https://pdm.fming.dev/#use-with-ide
+.pdm.toml
+
+# PEP 582; used by e.g. github.com/David-OConnor/pyflow and github.com/pdm-project/pdm
+__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/
+
+# pytype static type analyzer
+.pytype/
+
+# Cython debug symbols
+cython_debug/
+
+# PyCharm
+#  JetBrains specific template is maintained in a separate JetBrains.gitignore that can
+#  be found at https://github.com/github/gitignore/blob/main/Global/JetBrains.gitignore
+#  and can be added to the global gitignore or merged into this file.  For a more nuclear
+#  option (not recommended) you can uncomment the following to ignore the entire idea folder.
+#.idea/
+
+*.mp4

franka_sim/README.md

@@ -0,0 +1,15 @@
+# Intro:
+This package provide a simple Franka arm and Robotiq Gripper simulator written in Mujoco.
+It includes a state-based and a vision-based Franka lift cube task environment.
+
+# Installation:
+- From `serl` folder, cd into `franka_sim`.
+- In your `serl` conda environment, run `pip install -e .` to install this package.
+- run `pip install -r requirements.txt` to install sim dependencies.
+
+# Explore the Environments
+- Run `python franka_sim/test/test_gym_env_human.py` to launch a display window and visualize the task.
+
+# Credits:
+- This simulation is initially built by [Kevin Zakka](https://kzakka.com/).
+- Under Kevin's permission, we adopted a Gymnasium environment based on it.

franka_sim/franka_sim/__init__.py

@@ -0,0 +1,18 @@
+from franka_sim.mujoco_gym_env import MujocoGymEnv, GymRenderingSpec
+__all__ = [
+    "MujocoGymEnv",
+    "GymRenderingSpec",
+]
+
+from gymnasium.envs.registration import register
+register(
+    id='PandaPickCube-v0',
+    entry_point='franka_sim.envs:PandaPickCubeGymEnv',
+    max_episode_steps=100,
+)
+register(
+    id='PandaPickCubeVision-v0',
+    entry_point='franka_sim.envs:PandaPickCubeGymEnv',
+    max_episode_steps=100,
+    kwargs={'image_obs': True},
+)

franka_sim/franka_sim/controllers/__init__.py

@@ -0,0 +1,5 @@
+from franka_sim.controllers.opspace import opspace
+
+__all__ = [
+    "opspace",
+]

franka_sim/franka_sim/controllers/opspace.py

@@ -0,0 +1,179 @@
+from typing import Optional, Tuple, Union
+import numpy as np
+from dm_robotics.transformations import transformations as tr
+import mujoco
+
+
+def pd_control(
+    x: np.ndarray,
+    x_des: np.ndarray,
+    dx: np.ndarray,
+    kp_kv: np.ndarray,
+    ddx_max: float = 0.0,
+) -> np.ndarray:
+    # Compute error.
+    x_err = x - x_des
+    dx_err = dx
+
+    # Apply gains.
+    x_err *= -kp_kv[:, 0]
+    dx_err *= -kp_kv[:, 1]
+
+    # Limit maximum error.
+    if ddx_max > 0.0:
+        x_err_sq_norm = np.sum(x_err**2)
+        ddx_max_sq = ddx_max**2
+        if x_err_sq_norm > ddx_max_sq:
+            x_err *= ddx_max / np.sqrt(x_err_sq_norm)
+
+    return x_err + dx_err
+
+
+def pd_control_orientation(
+    quat: np.ndarray,
+    quat_des: np.ndarray,
+    w: np.ndarray,
+    kp_kv: np.ndarray,
+    dw_max: float = 0.0,
+) -> np.ndarray:
+    # Compute error.
+    quat_err = tr.quat_diff_active(source_quat=quat_des, target_quat=quat)
+    ori_err = tr.quat_to_axisangle(quat_err)
+    w_err = w
+
+    # Apply gains.
+    ori_err *= -kp_kv[:, 0]
+    w_err *= -kp_kv[:, 1]
+
+    # Limit maximum error.
+    if dw_max > 0.0:
+        ori_err_sq_norm = np.sum(ori_err**2)
+        dw_max_sq = dw_max**2
+        if ori_err_sq_norm > dw_max_sq:
+            ori_err *= dw_max / np.sqrt(ori_err_sq_norm)
+
+    return ori_err + w_err
+
+
+def opspace(
+    model,
+    data,
+    site_id,
+    dof_ids: np.ndarray,
+    pos: Optional[np.ndarray] = None,
+    ori: Optional[np.ndarray] = None,
+    joint: Optional[np.ndarray] = None,
+    pos_gains: Union[Tuple[float, float, float], np.ndarray] = (200.0, 200.0, 200.0),
+    ori_gains: Union[Tuple[float, float, float], np.ndarray] = (200.0, 200.0, 200.0),
+    damping_ratio: float = 1.0,
+    nullspace_stiffness: float = 0.5,
+    max_pos_acceleration: Optional[float] = None,
+    max_ori_acceleration: Optional[float] = None,
+    gravity_comp: bool = True,
+) -> np.ndarray:
+    if pos is None:
+        x_des = data.site_xpos[site_id]
+    else:
+        x_des = np.asarray(pos)
+    if ori is None:
+        xmat = data.site_xmat[site_id].reshape((3, 3))
+        quat_des = tr.mat_to_quat(xmat.reshape((3, 3)))
+    else:
+        ori = np.asarray(ori)
+        if ori.shape == (3, 3):
+            quat_des = tr.mat_to_quat(ori)
+        else:
+            quat_des = ori
+    if joint is None:
+        q_des = data.qpos[dof_ids]
+    else:
+        q_des = np.asarray(joint)
+
+    kp = np.asarray(pos_gains)
+    kd = damping_ratio * 2 * np.sqrt(kp)
+    kp_kv_pos = np.stack([kp, kd], axis=-1)
+
+    kp = np.asarray(ori_gains)
+    kd = damping_ratio * 2 * np.sqrt(kp)
+    kp_kv_ori = np.stack([kp, kd], axis=-1)
+
+    kp_joint = np.full((len(dof_ids),), nullspace_stiffness)
+    kd_joint = damping_ratio * 2 * np.sqrt(kp_joint)
+    kp_kv_joint = np.stack([kp_joint, kd_joint], axis=-1)
+
+    ddx_max = max_pos_acceleration if max_pos_acceleration is not None else 0.0
+    dw_max = max_ori_acceleration if max_ori_acceleration is not None else 0.0
+
+    # Get current state.
+    q = data.qpos[dof_ids]
+    dq = data.qvel[dof_ids]
+
+    # Compute Jacobian of the eef site in world frame.
+    J_v = np.zeros((3, model.nv), dtype=np.float64)
+    J_w = np.zeros((3, model.nv), dtype=np.float64)
+    mujoco.mj_jacSite(
+        model,
+        data,
+        J_v,
+        J_w,
+        site_id,
+    )
+    J_v = J_v[:, dof_ids]
+    J_w = J_w[:, dof_ids]
+    J = np.concatenate([J_v, J_w], axis=0)
+
+    # Compute position PD control.
+    x = data.site_xpos[site_id]
+    dx = J_v @ dq
+    ddx = pd_control(
+        x=x,
+        x_des=x_des,
+        dx=dx,
+        kp_kv=kp_kv_pos,
+        ddx_max=ddx_max,
+    )
+
+    # Compute orientation PD control.
+    quat = tr.mat_to_quat(data.site_xmat[site_id].reshape((3, 3)))
+    if quat @ quat_des < 0.0:
+        quat *= -1.0
+    w = J_w @ dq
+    dw = pd_control_orientation(
+        quat=quat,
+        quat_des=quat_des,
+        w=w,
+        kp_kv=kp_kv_ori,
+        dw_max=dw_max,
+    )
+
+    # Compute inertia matrix in joint space.
+    M = np.zeros((model.nv, model.nv), dtype=np.float64)
+    mujoco.mj_fullM(model, M, data.qM)
+    M = M[dof_ids, :][:, dof_ids]
+
+    # Compute inertia matrix in task space.
+    M_inv = np.linalg.inv(M)
+    Mx_inv = J @ M_inv @ J.T
+    if abs(np.linalg.det(Mx_inv)) >= 1e-2:
+        Mx = np.linalg.inv(Mx_inv)
+    else:
+        Mx = np.linalg.pinv(Mx_inv, rcond=1e-2)
+
+    # Compute generalized forces.
+    ddx_dw = np.concatenate([ddx, dw], axis=0)
+    tau = J.T @ Mx @ ddx_dw
+
+    # Add joint task in nullspace.
+    ddq = pd_control(
+        x=q,
+        x_des=q_des,
+        dx=dq,
+        kp_kv=kp_kv_joint,
+        ddx_max=0.0,
+    )
+    Jnull = M_inv @ J.T @ Mx
+    tau += (np.eye(len(q)) - J.T @ Jnull.T) @ ddq
+
+    if gravity_comp:
+        tau += data.qfrc_bias[dof_ids]
+    return tau

franka_sim/franka_sim/envs/__init__.py

@@ -0,0 +1,5 @@
+from franka_sim.envs.panda_pick_gym_env import PandaPickCubeGymEnv
+
+__all__ = [
+    "PandaPickCubeGymEnv",
+]