├── .github
└── workflows
│ ├── ruff.yml
│ └── unittest.yml
├── .gitignore
├── LICENSE
├── README.md
├── assets
├── df_clf_cbf_comp.jpg
├── dp_sensitive_to_init_pos.jpg
├── model_input.jpg
├── result_anim.gif
└── result_plot.png
├── config
└── config.yaml
├── core
├── controllers
│ ├── base_controller.py
│ ├── quadrotor_clf_cbf_qp.py
│ └── quadrotor_diffusion_policy.py
├── dataset
│ └── quadrotor_dataset.py
├── env
│ └── planar_quadrotor.py
├── networks
│ ├── conditional_unet1d.py
│ ├── conv1d_components.py
│ └── positional_embedding.py
└── trainers
│ ├── base_trainer.py
│ └── quadrotor_diffusion_policy_trainer.py
├── demo.ipynb
├── learning_note.md
├── pyproject.toml
├── tests
├── __init__.py
├── config
│ └── test_config.yaml
├── fixtures
│ └── test_dataset.joblib
├── test_datasets.py
└── test_networks.py
├── train.ipynb
└── utils
├── normalizers.py
├── utils.py
└── visualization.py
/.github/workflows/ruff.yml:
--------------------------------------------------------------------------------
1 | name: Ruff
2 | on: [push, pull_request]
3 | jobs:
4 | ruff:
5 | runs-on: ubuntu-latest
6 | steps:
7 | - uses: actions/checkout@v4
8 | - uses: chartboost/ruff-action@v1
9 |
--------------------------------------------------------------------------------
/.github/workflows/unittest.yml:
--------------------------------------------------------------------------------
1 | name: Python Unit Tests
2 |
3 | # Defines when the action should run. This workflow triggers on push and pull requests to the main branch.
4 | on:
5 | push:
6 | branches: [ main ]
7 | pull_request:
8 | branches: [ main ]
9 |
10 | # Jobs that the workflow will execute
11 | jobs:
12 | run-unittests:
13 | # The type of runner that the job will run on
14 | runs-on: ubuntu-latest
15 | strategy:
16 | matrix:
17 | python-version: ["3.10"]
18 |
19 | # Steps represent a sequence of tasks that will be executed as part of the job
20 | steps:
21 | # Checks-out your repository under $GITHUB_WORKSPACE, so your job can access it
22 | - uses: actions/checkout@v3
23 |
24 | # Sets up a Python environment
25 | - name: Set up Python ${{ matrix.python-version }}
26 | uses: actions/setup-python@v4
27 | with:
28 | python-version: ${{ matrix.python-version }}
29 |
30 | # Install dependencies
31 | - name: Install dependencies
32 | run: |
33 | python -m pip install --upgrade pip
34 | pip install torch==1.13.1 torchvision==0.14.1 jax==0.4.23 jaxlib==0.4.23 diffusers==0.18.2 joblib
35 |
36 | # Run unittests using the Python unittest module
37 | - name: Run unittest
38 | run: python -m unittest discover
39 |
--------------------------------------------------------------------------------
/.gitignore:
--------------------------------------------------------------------------------
1 |
2 | *.pyc
3 | tmp_imgs/*.*
4 | *-checkpoint.py
5 | *-checkpoint.ipynb
6 | *-checkpoint.yaml
--------------------------------------------------------------------------------
/LICENSE:
--------------------------------------------------------------------------------
1 | MIT License
2 |
3 | Copyright (c) 2024 shaoanlu
4 |
5 | Permission is hereby granted, free of charge, to any person obtaining a copy
6 | of this software and associated documentation files (the "Software"), to deal
7 | in the Software without restriction, including without limitation the rights
8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
9 | copies of the Software, and to permit persons to whom the Software is
10 | furnished to do so, subject to the following conditions:
11 |
12 | The above copyright notice and this permission notice shall be included in all
13 | copies or substantial portions of the Software.
14 |
15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
21 | SOFTWARE.
22 |
--------------------------------------------------------------------------------
/README.md:
--------------------------------------------------------------------------------
1 | # diffusion_policy_quadrotor
2 | This repository provides a demonstration of imitation learning using a diffusion policy on quadrotor control. The implementation is adapted from the official Diffusion Policy [repository](https://github.com/real-stanford/diffusion_policy) with an additional feature of using CBf-CLF controller to improve the safety of the generated trajectory.
3 |
4 | ## Result
5 | The control task is to drive the quadrotor from the initial position (0, 0) to the goal position (5, 5) without collision with the obstacles. The animation shows the denoising process of the diffusion policy predicting future trajectory followed by the quadrotor applying the actions.
6 |
7 |
8 |
9 |
10 | ## Usage
11 | The notebook `demo.ipynb` demonstrates a closed-loop simulation using the diffusion policy controller for quadrotor collision avoidance. You can run it in colab [](https://colab.research.google.com/github/shaoanlu/diffusion_policy_quadrotor/blob/main/demo.ipynb).
12 |
13 | The training script is provided as `train.ipynb`.
14 |
15 | ## Dependencies
16 | The program was developed and tested in the following environment.
17 | - Python 3.10
18 | - `torch==2.2.1`
19 | - `jax==0.4.26`
20 | - `jaxlib==0.4.26`
21 | - `diffusers==0.27.2`
22 | - `torchvision==0.14.1`
23 | - `gdown` (to download pre-trained weights)
24 | - `joblib` (format of training data)
25 |
26 | ## Diffusion policy
27 | The policy takes 1) the latest N step of observation $o_t$ (position and velocity) and 2) the encoding of obstacle information $O_{BST}$ (a flattened 7x7 grid with obstacle radius as values) as input. The outputs are N steps of actions $a_t$ (future position and future velocity).
28 |
29 |
30 |
31 | *The quadrotor icon is from [flaticon](https://www.flaticon.com/free-icon/quadcopter_5447794).
32 |
33 |
34 | ### Deviation from the original implementation
35 | - Add a linear layer before the Mish activation to the condition encoder of `ConditionalResidualBlock1D`. This is to prevent the activation from truncating large negative values from the normalized observation.
36 | - A CLF-CBF-QP controller is implemented and used to modify the noisy actions during the denoising process of the policy. By default, this controller is disabled.
37 | - A finetuned model for single-step inference is used by default.
38 |
39 |
40 |
41 |
42 | ## References
43 | Papers
44 | - [Diffusion Policy: Visuomotor Policy Learning via Action Diffusion](https://diffusion-policy.cs.columbia.edu/) [arXiv:2303.04137]
45 | - [3D Diffusion Policy: Generalizable Visuomotor Policy Learning via Simple 3D Representations](https://3d-diffusion-policy.github.io/) [arXiv:2403.03954]
46 | - [Fine-Tuning Image-Conditional Diffusion Models is Easier than You Think](https://gonzalomartingarcia.github.io/diffusion-e2e-ft/) [arXiv:2409.11355]
47 |
48 | Videos and Lectures
49 | - [DeepLearning.AI: How Diffusion Models Work](https://www.deeplearning.ai/short-courses/how-diffusion-models-work/)
50 | - [[论文速览]Diffusion Policy: Visuomotor Policy Learning via Action Diff.[2303.04137]](https://www.bilibili.com/video/BV1Cu411Y7d7)
51 | - [[論文導讀]Planning with Diffusion for Flexible Behavior Synthesis解說 (含程式碼)](https://youtu.be/ciCcvWutle4)
52 | - [6.4210 Fall 2023 Lecture 18: Visuomotor Policies (via Behavior Cloning)](https://youtu.be/i-303tTtEig)
53 |
54 | ## Learning note
55 | ### Failure case: the diffusion policy controller failed to extrapolate from training data
56 | Figure: A failure case of the controller.
57 | - The left figure is a trajectory in the training data.
58 | - The middle figure is the closed-loop simulation result of the controller starting from the SAME initial position as the training data.
59 | - The right figure is the closed-loop simulation result of the controller starting from a DIFFERENT initial position, which resulted in a trajectory with collision.
60 |
61 |
62 |
63 | Refer to [`learning_note.md`](learning_note.md) for other notes.
64 |
65 |
--------------------------------------------------------------------------------
/assets/df_clf_cbf_comp.jpg:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/shaoanlu/diffusion_policy_quadrotor/58781ae2ad1e7b4ff83b40d4726a3eae4f2cefff/assets/df_clf_cbf_comp.jpg
--------------------------------------------------------------------------------
/assets/dp_sensitive_to_init_pos.jpg:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/shaoanlu/diffusion_policy_quadrotor/58781ae2ad1e7b4ff83b40d4726a3eae4f2cefff/assets/dp_sensitive_to_init_pos.jpg
--------------------------------------------------------------------------------
/assets/model_input.jpg:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/shaoanlu/diffusion_policy_quadrotor/58781ae2ad1e7b4ff83b40d4726a3eae4f2cefff/assets/model_input.jpg
--------------------------------------------------------------------------------
/assets/result_anim.gif:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/shaoanlu/diffusion_policy_quadrotor/58781ae2ad1e7b4ff83b40d4726a3eae4f2cefff/assets/result_anim.gif
--------------------------------------------------------------------------------
/assets/result_plot.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/shaoanlu/diffusion_policy_quadrotor/58781ae2ad1e7b4ff83b40d4726a3eae4f2cefff/assets/result_plot.png
--------------------------------------------------------------------------------
/config/config.yaml:
--------------------------------------------------------------------------------
1 | name: "planar_quadrotor"
2 |
3 | pred_horizon: 96
4 | obs_horizon: 2
5 | action_horizon: 10
6 |
7 | controller:
8 | common:
9 | sampling_time: 0.1 # sec
10 | use_single_step_inference: true
11 | networks:
12 | obs_dim: 6
13 | action_dim: 6
14 | obstacle_encode_dim: 49
15 | noise_scheduler:
16 | type: "ddpm"
17 | ddpm:
18 | num_train_timesteps: 100 # number of diffusion iterations
19 | beta_schedule: "squaredcos_cap_v2"
20 | clip_sample: true # required when predict_epsilon=False
21 | prediction_type: "epsilon"
22 | ddim: # faster inference
23 | num_train_timesteps: 100
24 | beta_schedule: "squaredcos_cap_v2"
25 | clip_sample: true
26 | prediction_type: "epsilon"
27 | dpmsolver: # faster inference, experimental
28 | num_train_timesteps: 100
29 | beta_schedule: "squaredcos_cap_v2"
30 | prediction_type: "epsilon"
31 | use_karras_sigmas: true
32 |
33 | cbf_clf_controller:
34 | denoising_guidance_step: 100 # equals num_train_timesteps
35 | cbf_alpha: 10.0
36 | clf_gamma: 0.03
37 | penalty_slack_cbf: 1.0e+3
38 | penalty_slack_clf: 1.0
39 |
40 | trainer:
41 | use_ema: true
42 | batch_size: 256
43 | optimizer:
44 | name: "adamw"
45 | learning_rate: 1.0e-4
46 | weight_decay: 1.0e-6
47 | lr_scheduler:
48 | name: "cosine"
49 | num_warmup_steps: 500
50 |
51 | dataloader:
52 | batch_size: 256
53 |
54 | normalizer:
55 | action:
56 | min: [-2.6515920785069467, -10.169477462768555, -4.270973491668701, -13.883374419993748, -1.9637073183059692, -20.395111083984375]
57 | max: [8.0543811917305, 6.976394176483154, 8.477932775914669, 11.327190831233095, 2.5276688146591186, 18.05487823486328]
58 | observation:
59 | min: [-2.649836301803589, -9.564324378967285, -4.264063358306885, -13.772777557373047, -1.9476231336593628, -17.225351333618164]
60 | max: [8.05234146118164, 6.976299285888672, 8.474746704101562, 10.96181583404541, 2.5151419639587402, 18.054880142211914]
61 |
62 | simulator:
63 | m_q: 1.0 # kg
64 | I_xx: 0.1 # kg.m^2
65 | l_q: 0.3 # m, length of the quadrotor
66 | g: 9.81
67 | dt: 0.01
68 |
--------------------------------------------------------------------------------
/core/controllers/base_controller.py:
--------------------------------------------------------------------------------
1 | import numpy as np
2 | from typing import Dict, List
3 |
4 |
5 | class BaseController:
6 | def __init__(self, device: str = "cuda"):
7 | self.device = device
8 |
9 | def predict_action(self, obs_dict: Dict[str, List]) -> np.ndarray:
10 | raise NotImplementedError()
11 |
12 | def reset(self):
13 | # reset state for stateful policies
14 | pass
15 |
--------------------------------------------------------------------------------
/core/controllers/quadrotor_clf_cbf_qp.py:
--------------------------------------------------------------------------------
1 | from typing import Dict, List
2 | import numpy as np
3 | import osqp
4 | from scipy import sparse
5 |
6 | from core.controllers.base_controller import BaseController
7 |
8 |
9 | class QuadrotorCLFCBFController(BaseController):
10 | """
11 | A CLF-CBF safety filter assuming a simple velocity-controled dynamics
12 | y_dot = u1
13 | z_dot = u2
14 | Barrier funciton h is defined as the distances to each obstacle
15 | """
16 |
17 | def __init__(self, config: Dict, device: str = "cuda"):
18 | super().__init__(device)
19 | self.obstacle_info = {"center": [], "radius": []}
20 | self.set_config(config)
21 |
22 | def predict_action(self, obs_dict: Dict[str, List], control: np.ndarray, target_position: np.ndarray) -> np.ndarray:
23 | for center, radius in zip(obs_dict["obstacle_info"]["center"], obs_dict["obstacle_info"]["radius"]):
24 | self.set_obstacle(center, radius)
25 |
26 | safe_command = self.clf_cbf_control(
27 | state=obs_dict["state"],
28 | control=control,
29 | obs_center=self.obstacle_info["center"],
30 | obs_radius=self.obstacle_info["radius"],
31 | cbf_alpha=self.cbf_alpha,
32 | clf_gamma=self.clf_gamma,
33 | penalty_slack_cbf=self.penalty_slack_cbf,
34 | penalty_slack_clf=self.penalty_slack_clf,
35 | target_position=target_position,
36 | )
37 | return safe_command
38 |
39 | def set_obstacle(self, center: tuple, radius: float):
40 | self.obstacle_info = {"center": [], "radius": []}
41 | self.obstacle_info["center"].append(center)
42 | self.obstacle_info["radius"].append(radius)
43 |
44 | def set_config(self, config: Dict):
45 | self.cbf_alpha = config["cbf_clf_controller"]["cbf_alpha"]
46 | self.clf_gamma = config["cbf_clf_controller"]["clf_gamma"]
47 | self.penalty_slack_cbf = config["cbf_clf_controller"]["penalty_slack_cbf"]
48 | self.penalty_slack_clf = config["cbf_clf_controller"]["penalty_slack_clf"]
49 | self.denoising_guidance_step = config["cbf_clf_controller"]["denoising_guidance_step"]
50 | self.quadrotor_params = config["simulator"]
51 |
52 | @staticmethod
53 | def _barrier_func(y, z, obs_y, obs_z, obs_r) -> float:
54 | return (y - obs_y) ** 2 + (z - obs_z) ** 2 - (obs_r) ** 2
55 |
56 | @staticmethod
57 | def _barrier_func_dot(y, z, obs_y, obs_z) -> list:
58 | return [2 * (y - obs_y), 2 * (z - obs_z)]
59 |
60 | @staticmethod
61 | def _lyapunoc_func(y, z, des_y, des_z) -> float:
62 | return (y - des_y) ** 2 + (z - des_z) ** 2
63 |
64 | @staticmethod
65 | def _lyapunov_func_dot(y, z, des_y, des_z) -> list:
66 | return [2 * (y - des_y), 2 * (z - des_z)]
67 |
68 | @staticmethod
69 | def _define_QP_problem_data(
70 | u1: float,
71 | u2: float,
72 | cbf_alpha: float,
73 | clf_gamma: float,
74 | penalty_slack_cbf: float,
75 | penalty_slack_clf: float,
76 | h: list,
77 | coeffs_dhdx: list,
78 | v: list,
79 | coeffs_dvdx: list,
80 | vmin=-15.0,
81 | vmax=15.0,
82 | ):
83 | vmin, vmax = -15.0, 15.0
84 |
85 | P = sparse.csc_matrix([[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, penalty_slack_cbf, 0], [0, 0, 0, penalty_slack_clf]])
86 | q = np.array([-u1, -u2, 0, 0])
87 | A = sparse.csc_matrix(
88 | [c for c in coeffs_dhdx]
89 | + [c for c in coeffs_dvdx]
90 | + [[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]]
91 | )
92 | lb = np.array([-cbf_alpha * h_ for h_ in h] + [-np.inf for _ in v] + [vmin, vmin, 0, 0])
93 | ub = np.array([np.inf for _ in h] + [-clf_gamma * v_ for v_ in v] + [vmax, vmax, np.inf, np.inf])
94 | return P, q, A, lb, ub
95 |
96 | @staticmethod
97 | def _get_quadrotor_state(state):
98 | y, y_dot, z, z_dot, phi, phi_dot = state
99 | return y, y_dot, z, z_dot, phi, phi_dot
100 |
101 | def _calculate_cbf_coeffs(self, state: np.ndarray, obs_center: List, obs_radius: List, minimal_distance: float):
102 | """
103 | Let barrier function be h and system state x, the CBF constraint
104 | h_dot(x) >= - alpha * h + δ
105 | """
106 | h = [] # barrier values (here, remaining distance to each obstacle)
107 | coeffs_dhdx = [] # dhdt = dhdx * dxdt = dhdx * u
108 | for center, radius in zip(obs_center, obs_radius):
109 | y, _, z, _, _, _ = self._get_quadrotor_state(state)
110 | h.append(self._barrier_func(y, z, center[0], center[1], radius + minimal_distance))
111 | # Additional [1, 0] incorporates the CBF slack variable into the constraint
112 | coeffs_dhdx.append(self._barrier_func_dot(y, z, center[0], center[1]) + [1, 0])
113 | return h, coeffs_dhdx
114 |
115 | def _calculate_clf_coeffs(self, state: np.ndarray, target_y: float, _target_z: float):
116 | """
117 | Let Lyapunov function be v and system state x, the CBF constraint
118 | v_dot(x) - δ <= - gamma * v
119 | """
120 | y, _, z, _, _, _ = self._get_quadrotor_state(state)
121 | v = [self._lyapunoc_func(y, z, target_y, _target_z)]
122 | # Additional [0, -1] incorporates the CLF slack variable into the constraint
123 | coeffs_dvdx = [self._lyapunov_func_dot(y, z, target_y, _target_z) + [0, -1]]
124 | return v, coeffs_dvdx
125 |
126 | def clf_cbf_control(
127 | self,
128 | state: np.ndarray,
129 | control: np.ndarray,
130 | obs_center: List,
131 | obs_radius: List,
132 | cbf_alpha: float = 15.0,
133 | clf_gamma: float = 0.01,
134 | penalty_slack_cbf: float = 1e2,
135 | penalty_slack_clf: float = 1.0,
136 | target_position: tuple = (5.0, 5.0),
137 | ):
138 | """
139 | Calculate the safe command by solveing the following optimization problem
140 |
141 | minimize || u - u_nom ||^2 + k * δ^2
142 | u, δ
143 | s.t.
144 | h'(x) ≥ -𝛼 * h(x) - δ1
145 | v'(x) ≤ -γ * v(x) + δ2
146 | u_min ≤ u ≤ u_max
147 | 0 ≤ δ1,δ2 ≤ inf
148 | where
149 | u = [ux, uy] is the control input in x and y axis respectively.
150 | δ is the slack variable
151 | h(x) is the control barrier function and h'(x) its derivative
152 | v(x) is the lyapunov function and v'(x) its derivative
153 |
154 | The problem above can be formulated as QP (ref: https://osqp.org/docs/solver/index.html)
155 |
156 | minimize 1/2 * x^T * Px + q^T x
157 | x
158 | s.t.
159 | l ≤ Ax ≤ u
160 | where
161 | x = [ux, uy, δ1, δ2]
162 |
163 | """
164 | u1, u2 = control
165 | target_y, target_z = target_position
166 |
167 | # Calculate values of the barrier function and coeffs in h_dot to state
168 | h, coeffs_dhdx = self._calculate_cbf_coeffs(state, obs_center, obs_radius, self.quadrotor_params["l_q"])
169 | # Calculate value of the lyapunov function and coeffs in v_dot to state
170 | v, coeffs_dvdx = self._calculate_clf_coeffs(state, target_y, target_z)
171 |
172 | # Define problem
173 | P, q, A, lb, ub = self._define_QP_problem_data(
174 | u1, u2, cbf_alpha, clf_gamma, penalty_slack_cbf, penalty_slack_clf, h, coeffs_dhdx, v, coeffs_dvdx
175 | )
176 |
177 | # Solve QP
178 | prob = osqp.OSQP()
179 | prob.setup(P, q, A, lb, ub, verbose=False, time_limit=0)
180 | # Solve QP problem
181 | res = prob.solve()
182 |
183 | safe_u1, safe_u2, _, _ = res.x
184 | return np.array([safe_u1, safe_u2])
185 |
--------------------------------------------------------------------------------
/core/controllers/quadrotor_diffusion_policy.py:
--------------------------------------------------------------------------------
1 | from typing import Dict, List
2 |
3 | import numpy as np
4 | import torch
5 | from diffusers.schedulers.scheduling_ddim import DDIMScheduler
6 | from diffusers.schedulers.scheduling_ddpm import DDPMScheduler
7 | from diffusers.schedulers.scheduling_dpmsolver_multistep import (
8 | DPMSolverMultistepScheduler,
9 | )
10 |
11 | from core.controllers.base_controller import BaseController
12 | from core.controllers.quadrotor_clf_cbf_qp import QuadrotorCLFCBFController
13 | from core.networks.conditional_unet1d import ConditionalUnet1D
14 | from utils.normalizers import BaseNormalizer
15 |
16 |
17 | def build_networks_from_config(config: Dict):
18 | action_dim = config["controller"]["networks"]["action_dim"]
19 | obs_dim = config["controller"]["networks"]["obs_dim"]
20 | obs_horizon = config["obs_horizon"]
21 | obstacle_encode_dim = config["controller"]["networks"]["obstacle_encode_dim"]
22 | return ConditionalUnet1D(input_dim=action_dim, global_cond_dim=obs_dim * obs_horizon + obstacle_encode_dim)
23 |
24 |
25 | def build_noise_scheduler_from_config(config: Dict):
26 | type_noise_scheduler = config["controller"]["noise_scheduler"]["type"]
27 | if type_noise_scheduler.lower() == "ddpm":
28 | return DDPMScheduler(
29 | num_train_timesteps=config["controller"]["noise_scheduler"]["ddpm"]["num_train_timesteps"],
30 | beta_schedule=config["controller"]["noise_scheduler"]["ddpm"]["beta_schedule"],
31 | clip_sample=config["controller"]["noise_scheduler"]["ddpm"]["clip_sample"],
32 | prediction_type=config["controller"]["noise_scheduler"]["ddpm"]["prediction_type"],
33 | )
34 | elif type_noise_scheduler.lower() == "ddim":
35 | return DDIMScheduler(
36 | num_train_timesteps=config["controller"]["noise_scheduler"]["ddim"]["num_train_timesteps"],
37 | beta_schedule=config["controller"]["noise_scheduler"]["ddim"]["beta_schedule"],
38 | clip_sample=config["controller"]["noise_scheduler"]["ddim"]["clip_sample"],
39 | prediction_type=config["controller"]["noise_scheduler"]["ddim"]["prediction_type"],
40 | )
41 | elif type_noise_scheduler.lower() == "dpmsolver":
42 | return DPMSolverMultistepScheduler(
43 | num_train_timesteps=config["controller"]["noise_scheduler"]["dpmsolver"]["num_train_timesteps"],
44 | beta_schedule=config["controller"]["noise_scheduler"]["dpmsolver"]["beta_schedule"],
45 | prediction_type=config["controller"]["noise_scheduler"]["dpmsolver"]["prediction_type"],
46 | use_karras_sigmas=config["controller"]["noise_scheduler"]["dpmsolver"]["use_karras_sigmas"],
47 | )
48 | else:
49 | raise NotImplementedError
50 |
51 |
52 | class QuadrotorDiffusionPolicy(BaseController):
53 | def __init__(
54 | self,
55 | model: ConditionalUnet1D,
56 | noise_scheduler: DDPMScheduler,
57 | normalizer: BaseNormalizer,
58 | clf_cbf_controller: QuadrotorCLFCBFController,
59 | config: Dict,
60 | device: str = "cuda",
61 | ):
62 | self.device = device
63 | self.net = model
64 | self.noise_scheduler = noise_scheduler
65 | self.normalizer = normalizer
66 |
67 | self.set_config(config)
68 | self.net.to(self.device)
69 |
70 | self.clf_cbf_controller = clf_cbf_controller
71 | self.use_clf_cbf_guidance = False if clf_cbf_controller is None else True
72 |
73 | def predict_action(self, obs_dict: Dict[str, List]) -> np.ndarray:
74 | # stack the observations
75 | obs_seq = np.stack(obs_dict["state"])
76 | # normalize observation and make it 1D
77 | nobs = self.normalizer.normalize_data(obs_seq, stats=self.norm_stats["obs"])
78 | nobs = nobs.flatten()
79 | # concat obstacle information to observations
80 | nobs = np.concatenate([nobs] + obs_dict["obs_encode"], axis=0)
81 | # device transfer
82 | nobs = torch.from_numpy(nobs).to(self.device, dtype=torch.float32)
83 |
84 | # infer action
85 | with torch.no_grad():
86 | # reshape observation to (1, obs_horizon*obs_dim+obstacle_encode_dim)
87 | obs_cond = nobs.unsqueeze(0).flatten(start_dim=1)
88 |
89 | # initialize action from Guassian noise
90 | noisy_action = torch.randn((1, self.pred_horizon, self.action_dim), device=self.device)
91 | naction = noisy_action
92 |
93 | # init scheduler
94 | self.noise_scheduler.set_timesteps(self.noise_scheduler.config.num_train_timesteps)
95 |
96 | # denoise
97 | denoise_timesteps = (
98 | self.noise_scheduler.timesteps[:1] if self.use_single_step_inference else self.noise_scheduler.timesteps
99 | )
100 | for k in denoise_timesteps:
101 | # predict noise
102 | noise_pred = self.net(sample=naction, timestep=k, global_cond=obs_cond)
103 | # inverse diffusion step (remove noise)
104 | if self.use_single_step_inference:
105 | naction = noisy_action - noise_pred
106 | else:
107 | naction = self.noise_scheduler.step(model_output=noise_pred, timestep=k, sample=naction).prev_sample
108 |
109 | if self.use_clf_cbf_guidance:
110 | diffusing_action = self.normalizer.unnormalize_data(
111 | naction.detach().to("cpu").numpy().squeeze(), stats=self.norm_stats["act"]
112 | ) # (pred_horizon, 6)
113 | if k < self.clf_cbf_controller.denoising_guidance_step:
114 | refined_action = diffusing_action.copy()
115 | for idx, act in enumerate(diffusing_action):
116 | clf_cbf_obs, pred_control, target_position = self._preprocess_cbf_clf_input(
117 | obs_dict, act, diffusing_action
118 | )
119 | safe_yz_velocity = self.clf_cbf_controller.predict_action(
120 | obs_dict=clf_cbf_obs,
121 | control=pred_control,
122 | target_position=target_position,
123 | )
124 | refined_action[idx, ...] = self._calculate_refined_action_step(act, safe_yz_velocity)
125 | naction = self.normalizer.normalize_data(np.array(refined_action), stats=self.norm_stats["act"])
126 | naction = torch.from_numpy(naction).to(self.device, dtype=torch.float32).unsqueeze(0)
127 |
128 | # unnormalize action
129 | naction = naction.detach().to("cpu").numpy()
130 | # (1, pred_horizon, action_dim)
131 | naction = naction[0]
132 | action_pred = self.normalizer.unnormalize_data(naction, stats=self.norm_stats["act"])
133 |
134 | # only take action_horizon number of actions
135 | start = self.obs_horizon - 1
136 | end = start + self.action_horizon
137 | action = action_pred[start:end, :] # (action_horizon, action_dim)
138 |
139 | return action
140 |
141 | def load_weights(self, ckpt_path: str):
142 | state_dict = torch.load(ckpt_path, map_location=self.device)
143 | self.net.load_state_dict(state_dict)
144 |
145 | def set_config(self, config: Dict):
146 | self.obs_horizon = config["obs_horizon"]
147 | self.action_horizon = config["action_horizon"]
148 | self.pred_horizon = config["pred_horizon"]
149 | self.action_dim = config["controller"]["networks"]["action_dim"]
150 | self.sampling_time = config["controller"]["common"]["sampling_time"]
151 | self.norm_stats = {
152 | "act": config["normalizer"]["action"],
153 | "obs": config["normalizer"]["observation"],
154 | }
155 | self.quadrotor_params = config["simulator"]
156 | self.use_single_step_inference = config.get("controller").get("common").get("use_single_step_inference", False)
157 |
158 | def calculate_force_command(self, state: np.ndarray, ref_state: np.ndarray) -> np.ndarray:
159 | y, y_dot, z, z_dot, phi, phi_dot = state
160 | yr, yr_dot, zr, zr_dot, phir, phir_dot = ref_state
161 | (
162 | dt,
163 | m_q,
164 | ) = self.quadrotor_params["dt"], self.quadrotor_params["m_q"]
165 | g, I_xx = self.quadrotor_params["g"], self.quadrotor_params["I_xx"]
166 | # how on earth do you want to calculate acceleration from position signals
167 | # est_zr_dot = (zr - z) / dt
168 | # est_phir_dot = (phir - phi) / dt
169 | zr_ddot = (zr_dot - z_dot) / dt
170 | phir_ddot = (phir_dot - phi_dot) / dt
171 | return np.array([m_q * (g + zr_ddot), I_xx * phir_ddot])
172 |
173 | def _preprocess_cbf_clf_input(
174 | self, obs_dict: Dict[str, List], pred_action: np.ndarray, diffusing_action: np.ndarray
175 | ):
176 | pred_state = pred_action
177 | pred_control = pred_action[[1, 3]]
178 | target_position_y, target_position_z = diffusing_action[-1, [0, 2]] # myoptic planning of CLF
179 | target_position = (target_position_y, target_position_z)
180 | obstacle_info = {"center": obs_dict["obs_center"], "radius": obs_dict["obs_radius"]}
181 | return {"state": pred_state, "obstacle_info": obstacle_info}, pred_control, target_position
182 |
183 | def _calculate_refined_action_step(self, pred_act, safe_yz_velocity):
184 | refined_step_action = pred_act.copy()
185 | refined_step_action[0] += safe_yz_velocity[0] * self.sampling_time
186 | refined_step_action[2] += safe_yz_velocity[1] * self.sampling_time
187 | refined_step_action[1] = safe_yz_velocity[0]
188 | refined_step_action[3] = safe_yz_velocity[1]
189 | refined_step_action[4] = -np.arctan(safe_yz_velocity[0] / safe_yz_velocity[1])
190 | # refinedstep_action[5] = (refinedstep_action[4] - pred_act[4]) / self.sampling_time
191 | return refined_step_action
192 |
--------------------------------------------------------------------------------
/core/dataset/quadrotor_dataset.py:
--------------------------------------------------------------------------------
1 | import joblib
2 | import torch
3 | from typing import Dict
4 | import numpy as np
5 |
6 |
7 | def create_sample_indices(
8 | episode_ends: np.ndarray,
9 | sequence_length: int,
10 | pad_before: int = 0,
11 | pad_after: int = 0,
12 | ):
13 | indices = list()
14 | for i in range(len(episode_ends)):
15 | start_idx = 0
16 | if i > 0:
17 | start_idx = episode_ends[i - 1]
18 | end_idx = episode_ends[i]
19 | episode_length = end_idx - start_idx
20 |
21 | min_start = -pad_before
22 | max_start = episode_length - sequence_length + pad_after
23 |
24 | # range stops one idx before end
25 | for idx in range(min_start, max_start + 1):
26 | buffer_start_idx = max(idx, 0) + start_idx
27 | buffer_end_idx = min(idx + sequence_length, episode_length) + start_idx
28 | start_offset = buffer_start_idx - (idx + start_idx)
29 | end_offset = (idx + sequence_length + start_idx) - buffer_end_idx
30 | sample_start_idx = 0 + start_offset
31 | sample_end_idx = sequence_length - end_offset
32 | indices.append([buffer_start_idx, buffer_end_idx, sample_start_idx, sample_end_idx])
33 | indices = np.array(indices)
34 | return indices
35 |
36 |
37 | def sample_sequence(
38 | train_data,
39 | sequence_length,
40 | buffer_start_idx,
41 | buffer_end_idx,
42 | sample_start_idx,
43 | sample_end_idx,
44 | ):
45 | result = dict()
46 | for key, input_arr in train_data.items():
47 | sample = input_arr[buffer_start_idx:buffer_end_idx]
48 | data = sample
49 | if (sample_start_idx > 0) or (sample_end_idx < sequence_length):
50 | data = np.zeros(shape=(sequence_length,) + input_arr.shape[1:], dtype=input_arr.dtype)
51 | if sample_start_idx > 0:
52 | data[:sample_start_idx] = sample[0]
53 | if sample_end_idx < sequence_length:
54 | data[sample_end_idx:] = sample[-1]
55 | data[sample_start_idx:sample_end_idx] = sample
56 | result[key] = data
57 | return result
58 |
59 |
60 | # normalize data
61 | def get_data_stats(data):
62 | data = data.reshape(-1, data.shape[-1])
63 | stats = {"min": np.min(data, axis=0), "max": np.max(data, axis=0)}
64 | return stats
65 |
66 |
67 | def normalize_data(data, stats):
68 | # nomalize to [0,1]
69 | ndata = (data - stats["min"]) / (stats["max"] - stats["min"])
70 | # normalize to [-1, 1]
71 | ndata = ndata * 2 - 1
72 | return ndata
73 |
74 |
75 | def unnormalize_data(ndata, stats):
76 | ndata = (ndata + 1) / 2
77 | data = ndata * (stats["max"] - stats["min"]) + stats["min"]
78 | return data
79 |
80 |
81 | def preprocess_dataset(dataset):
82 | dataset["obs_encode"] = []
83 | dataset["episode_ends"] = []
84 |
85 | current_idx = 0
86 | for s, c in zip(dataset["state"], dataset["control"]):
87 | dataset["episode_ends"].append(current_idx + s.shape[0])
88 | current_idx += s.shape[0]
89 |
90 | for s, info in zip(dataset["state"], dataset["info"]):
91 | obs_encode = np.zeros((7, 7))
92 | for center, radius in zip(info["obs_center"], info["obs_radius"]):
93 | obs_encode[tuple((center - 1).astype(np.int32))] = radius
94 | dataset["obs_encode"].append(np.vstack([obs_encode.flatten()] * s.shape[0]))
95 | return dataset
96 |
97 |
98 | # dataset
99 | class PlanarQuadrotorStateDataset(torch.utils.data.Dataset):
100 | def __init__(
101 | self,
102 | dataset_path: str,
103 | config: Dict,
104 | ):
105 | self.set_config(config)
106 | # read from zarr dataset
107 | dataset_root = joblib.load(dataset_path)
108 | # proprocessing
109 | dataset_root = preprocess_dataset(dataset_root)
110 | # All demonstration episodes are concatinated in the first dimension N
111 | train_data = {
112 | # (N, action_dim)
113 | "action": np.concatenate(dataset_root["desired_state"], axis=0),
114 | # (N, obs_dim)
115 | "obs": np.concatenate(dataset_root["state"], axis=0),
116 | # (N, 7x7)
117 | "obstacle": np.concatenate(dataset_root["obs_encode"], axis=0),
118 | }
119 | # Marks one-past the last index for each episode
120 | episode_ends = dataset_root["episode_ends"]
121 |
122 | # compute start and end of each state-action sequence
123 | # also handles padding
124 | indices = create_sample_indices(
125 | episode_ends=episode_ends,
126 | sequence_length=self.pred_horizon,
127 | # add padding such that each timestep in the dataset are seen
128 | pad_before=self.obs_horizon - 1,
129 | pad_after=self.action_horizon - 1,
130 | )
131 |
132 | # compute statistics and normalized data to [-1,1]
133 | stats = dict()
134 | normalized_train_data = dict()
135 | for key, data in train_data.items():
136 | if key == "obstacle":
137 | continue
138 | stats[key] = get_data_stats(data)
139 | normalized_train_data[key] = normalize_data(data, stats[key])
140 | normalized_train_data["obstacle"] = train_data["obstacle"]
141 |
142 | self.indices = indices
143 | self.stats = stats
144 | self.normalized_train_data = normalized_train_data
145 |
146 | def __len__(self):
147 | # all possible segments of the dataset
148 | return len(self.indices)
149 |
150 | def __getitem__(self, idx):
151 | # get the start/end indices for this datapoint
152 | buffer_start_idx, buffer_end_idx, sample_start_idx, sample_end_idx = self.indices[idx]
153 |
154 | # get nomralized data using these indices
155 | nsample = sample_sequence(
156 | train_data=self.normalized_train_data,
157 | sequence_length=self.pred_horizon,
158 | buffer_start_idx=buffer_start_idx,
159 | buffer_end_idx=buffer_end_idx,
160 | sample_start_idx=sample_start_idx,
161 | sample_end_idx=sample_end_idx,
162 | )
163 |
164 | # discard unused observations
165 | # (obs_horiuzon * obs_dim + obstacle_encode_dim)
166 | nsample["obs"] = np.concatenate(
167 | [nsample["obs"][: self.obs_horizon, :].flatten(), nsample["obstacle"][0]],
168 | axis=0,
169 | )
170 | return nsample
171 |
172 | def set_config(self, config: Dict):
173 | self.config = config
174 | self.obs_horizon = config["obs_horizon"]
175 | self.action_horizon = config["action_horizon"]
176 | self.pred_horizon = config["pred_horizon"]
177 |
--------------------------------------------------------------------------------
/core/env/planar_quadrotor.py:
--------------------------------------------------------------------------------
1 | from functools import partial
2 | from typing import Optional
3 |
4 | import jax
5 | from jax import numpy as jnp
6 |
7 | # Default quadrotor parameters
8 | m_q = 1.0 # kg
9 | I_xx = 0.1 # kg.m^2
10 | l_q = 0.3 # m, length of the quadrotor
11 | g = 9.81
12 |
13 |
14 | class PlanarQuadrotorEnv:
15 | def __init__(self, config: dict = None, state: Optional[jnp.ndarray] = None):
16 | if config is None:
17 | self.m_q = m_q
18 | self.I_xx = I_xx
19 | self.g = g
20 | self.l_q = l_q
21 | else:
22 | self.m_q = config["simulator"]["m_q"]
23 | self.I_xx = config["simulator"]["I_xx"]
24 | self.g = config["simulator"]["g"]
25 | self.l_q = config["simulator"]["l_q"]
26 |
27 | self.state: Optional[jnp.ndarray] = state
28 |
29 | @partial(jax.jit, static_argnums=0)
30 | def step(self, state=None, control=[0, 0], dt: float = 0.01):
31 | """
32 | dynamics with JAX-compatible code.
33 |
34 | Equations are from the Aerial Robotics coursera lecture
35 | https://www.coursera.org/lecture/robotics-flight/2-d-quadrotor-control-kakc6
36 | """
37 | if state is None:
38 | state = self.state
39 | if state is None:
40 | raise Exception("state variable is not defined.")
41 |
42 | y, y_dot, z, z_dot, phi, phi_dot = state
43 | u1, u2 = control
44 | # Quadrotor dynamics
45 | y_ddot = -u1 * jnp.sin(phi) / self.m_q
46 | z_ddot = -self.g + u1 * jnp.cos(phi) / self.m_q
47 | phi_ddot = u2 / self.I_xx
48 |
49 | next_state = (
50 | state
51 | + jnp.array(
52 | [
53 | y_dot + y_ddot * dt,
54 | y_ddot,
55 | z_dot + z_ddot * dt,
56 | z_ddot,
57 | phi_dot + phi_ddot * dt,
58 | phi_ddot,
59 | ]
60 | )
61 | * dt
62 | )
63 | self.state = next_state
64 | return next_state
65 |
--------------------------------------------------------------------------------
/core/networks/conditional_unet1d.py:
--------------------------------------------------------------------------------
1 | import torch
2 | import torch.nn as nn
3 | from typing import Union
4 |
5 | from core.networks.conv1d_components import Downsample1d, Upsample1d, Conv1dBlock
6 | from core.networks.positional_embedding import SinusoidalPosEmb
7 |
8 |
9 | class ConditionalResidualBlock1D(nn.Module):
10 | def __init__(self, in_channels, out_channels, cond_dim, kernel_size=3, n_groups=8):
11 | super().__init__()
12 |
13 | self.blocks = nn.ModuleList(
14 | [
15 | Conv1dBlock(in_channels, out_channels, kernel_size, n_groups=n_groups),
16 | Conv1dBlock(out_channels, out_channels, kernel_size, n_groups=n_groups),
17 | ]
18 | )
19 |
20 | # FiLM modulation https://arxiv.org/abs/1709.07871
21 | # predicts per-channel scale and bias
22 | cond_channels = out_channels * 2
23 | self.out_channels = out_channels
24 | self.cond_encoder = nn.Sequential(
25 | nn.Linear(cond_dim, cond_dim), nn.Mish(), nn.Linear(cond_dim, cond_channels), nn.Unflatten(-1, (-1, 1))
26 | )
27 |
28 | # make sure dimensions compatible
29 | self.residual_conv = nn.Conv1d(in_channels, out_channels, 1) if in_channels != out_channels else nn.Identity()
30 |
31 | def forward(self, x, cond):
32 | """
33 | x : [ batch_size x in_channels x horizon ]
34 | cond : [ batch_size x cond_dim]
35 |
36 | returns:
37 | out : [ batch_size x out_channels x horizon ]
38 | """
39 | out = self.blocks[0](x)
40 | embed = self.cond_encoder(cond)
41 |
42 | embed = embed.reshape(embed.shape[0], 2, self.out_channels, 1)
43 | scale = embed[:, 0, ...]
44 | bias = embed[:, 1, ...]
45 | out = scale * out + bias
46 |
47 | out = self.blocks[1](out)
48 | out = out + self.residual_conv(x)
49 | return out
50 |
51 |
52 | class ConditionalUnet1D(nn.Module):
53 | def __init__(
54 | self,
55 | input_dim,
56 | global_cond_dim,
57 | diffusion_step_embed_dim=256,
58 | down_dims=[256, 512, 1024],
59 | kernel_size=5,
60 | n_groups=8,
61 | ):
62 | """
63 | input_dim: Dim of actions.
64 | global_cond_dim: Dim of global conditioning applied with FiLM
65 | in addition to diffusion step embedding. This is usually obs_horizon * obs_dim
66 | diffusion_step_embed_dim: Size of positional encoding for diffusion iteration k
67 | down_dims: Channel size for each UNet level.
68 | The length of this array determines numebr of levels.
69 | kernel_size: Conv kernel size
70 | n_groups: Number of groups for GroupNorm
71 | """
72 |
73 | super().__init__()
74 | all_dims = [input_dim] + list(down_dims)
75 | start_dim = down_dims[0]
76 |
77 | dsed = diffusion_step_embed_dim
78 | diffusion_step_encoder = nn.Sequential(
79 | SinusoidalPosEmb(dsed),
80 | nn.Linear(dsed, dsed * 4),
81 | nn.Mish(),
82 | nn.Linear(dsed * 4, dsed),
83 | )
84 | cond_dim = dsed + global_cond_dim
85 |
86 | in_out = list(zip(all_dims[:-1], all_dims[1:]))
87 | mid_dim = all_dims[-1]
88 | self.mid_modules = nn.ModuleList(
89 | [
90 | ConditionalResidualBlock1D(
91 | mid_dim,
92 | mid_dim,
93 | cond_dim=cond_dim,
94 | kernel_size=kernel_size,
95 | n_groups=n_groups,
96 | ),
97 | ConditionalResidualBlock1D(
98 | mid_dim,
99 | mid_dim,
100 | cond_dim=cond_dim,
101 | kernel_size=kernel_size,
102 | n_groups=n_groups,
103 | ),
104 | ]
105 | )
106 |
107 | down_modules = nn.ModuleList([])
108 | for ind, (dim_in, dim_out) in enumerate(in_out):
109 | is_last = ind >= (len(in_out) - 1)
110 | down_modules.append(
111 | nn.ModuleList(
112 | [
113 | ConditionalResidualBlock1D(
114 | dim_in,
115 | dim_out,
116 | cond_dim=cond_dim,
117 | kernel_size=kernel_size,
118 | n_groups=n_groups,
119 | ),
120 | ConditionalResidualBlock1D(
121 | dim_out,
122 | dim_out,
123 | cond_dim=cond_dim,
124 | kernel_size=kernel_size,
125 | n_groups=n_groups,
126 | ),
127 | Downsample1d(dim_out) if not is_last else nn.Identity(),
128 | ]
129 | )
130 | )
131 |
132 | up_modules = nn.ModuleList([])
133 | for ind, (dim_in, dim_out) in enumerate(reversed(in_out[1:])):
134 | is_last = ind >= (len(in_out) - 1)
135 | up_modules.append(
136 | nn.ModuleList(
137 | [
138 | ConditionalResidualBlock1D(
139 | dim_out * 2,
140 | dim_in,
141 | cond_dim=cond_dim,
142 | kernel_size=kernel_size,
143 | n_groups=n_groups,
144 | ),
145 | ConditionalResidualBlock1D(
146 | dim_in,
147 | dim_in,
148 | cond_dim=cond_dim,
149 | kernel_size=kernel_size,
150 | n_groups=n_groups,
151 | ),
152 | Upsample1d(dim_in) if not is_last else nn.Identity(),
153 | ]
154 | )
155 | )
156 |
157 | final_conv = nn.Sequential(
158 | Conv1dBlock(start_dim, start_dim, kernel_size=kernel_size),
159 | nn.Conv1d(start_dim, input_dim, 1),
160 | )
161 |
162 | self.diffusion_step_encoder = diffusion_step_encoder
163 | self.up_modules = up_modules
164 | self.down_modules = down_modules
165 | self.final_conv = final_conv
166 |
167 | def forward(
168 | self,
169 | sample: torch.Tensor,
170 | timestep: Union[torch.Tensor, float, int],
171 | global_cond=None,
172 | ):
173 | """
174 | x: (B,T,input_dim)
175 | timestep: (B,) or int, diffusion step
176 | global_cond: (B,global_cond_dim)
177 | output: (B,T,input_dim)
178 | """
179 | # (B,T,C)
180 | sample = sample.moveaxis(-1, -2)
181 | # (B,C,T)
182 |
183 | # 1. time
184 | timesteps = timestep
185 | if not torch.is_tensor(timesteps):
186 | # TODO: this requires sync between CPU and GPU. So try to pass timesteps as tensors if you can
187 | timesteps = torch.tensor([timesteps], dtype=torch.long, device=sample.device)
188 | elif torch.is_tensor(timesteps) and len(timesteps.shape) == 0:
189 | timesteps = timesteps[None].to(sample.device)
190 | # broadcast to batch dimension in a way that's compatible with ONNX/Core ML
191 | timesteps = timesteps.expand(sample.shape[0])
192 |
193 | global_feature = self.diffusion_step_encoder(timesteps)
194 |
195 | if global_cond is not None:
196 | global_feature = torch.cat([global_feature, global_cond], axis=-1)
197 |
198 | x = sample
199 | h = []
200 | for idx, (resnet, resnet2, downsample) in enumerate(self.down_modules):
201 | x = resnet(x, global_feature)
202 | x = resnet2(x, global_feature)
203 | h.append(x)
204 | x = downsample(x)
205 |
206 | for mid_module in self.mid_modules:
207 | x = mid_module(x, global_feature)
208 |
209 | for idx, (resnet, resnet2, upsample) in enumerate(self.up_modules):
210 | x = torch.cat((x, h.pop()), dim=1)
211 | x = resnet(x, global_feature)
212 | x = resnet2(x, global_feature)
213 | x = upsample(x)
214 |
215 | x = self.final_conv(x)
216 |
217 | # (B,C,T)
218 | x = x.moveaxis(-1, -2)
219 | # (B,T,C)
220 | return x
221 |
--------------------------------------------------------------------------------
/core/networks/conv1d_components.py:
--------------------------------------------------------------------------------
1 | import torch.nn as nn
2 |
3 |
4 | class Downsample1d(nn.Module):
5 | def __init__(self, dim):
6 | super().__init__()
7 | self.conv = nn.Conv1d(dim, dim, 3, 2, 1)
8 |
9 | def forward(self, x):
10 | return self.conv(x)
11 |
12 |
13 | class Upsample1d(nn.Module):
14 | def __init__(self, dim):
15 | super().__init__()
16 | self.conv = nn.ConvTranspose1d(dim, dim, 4, 2, 1)
17 |
18 | def forward(self, x):
19 | return self.conv(x)
20 |
21 |
22 | class Conv1dBlock(nn.Module):
23 | """
24 | Conv1d --> GroupNorm --> Mish
25 | """
26 |
27 | def __init__(self, inp_channels, out_channels, kernel_size, n_groups=8):
28 | super().__init__()
29 |
30 | self.block = nn.Sequential(
31 | nn.Conv1d(inp_channels, out_channels, kernel_size, padding=kernel_size // 2),
32 | nn.GroupNorm(n_groups, out_channels),
33 | nn.Mish(),
34 | )
35 |
36 | def forward(self, x):
37 | return self.block(x)
38 |
--------------------------------------------------------------------------------
/core/networks/positional_embedding.py:
--------------------------------------------------------------------------------
1 | import math
2 | import torch
3 | import torch.nn as nn
4 |
5 |
6 | class SinusoidalPosEmb(nn.Module):
7 | def __init__(self, dim):
8 | super().__init__()
9 | self.dim = dim
10 |
11 | def forward(self, x):
12 | device = x.device
13 | half_dim = self.dim // 2
14 | emb = math.log(10000) / (half_dim - 1)
15 | emb = torch.exp(torch.arange(half_dim, device=device) * -emb)
16 | emb = x[:, None] * emb[None, :]
17 | emb = torch.cat((emb.sin(), emb.cos()), dim=-1)
18 | return emb
19 |
--------------------------------------------------------------------------------
/core/trainers/base_trainer.py:
--------------------------------------------------------------------------------
1 | from abc import abstractmethod
2 |
3 |
4 | class BaseDiffusionPolicyTrainer:
5 | def __init__(
6 | self,
7 | ):
8 | pass
9 |
10 | @abstractmethod
11 | def train(self, *args, **kwargs):
12 | raise NotImplementedError
13 |
14 | @abstractmethod
15 | def save_checkpoint(self, path: str):
16 | raise NotImplementedError
17 |
--------------------------------------------------------------------------------
/core/trainers/quadrotor_diffusion_policy_trainer.py:
--------------------------------------------------------------------------------
1 | from diffusers.optimization import get_scheduler
2 | from diffusers.training_utils import EMAModel
3 | import numpy as np
4 | import torch
5 | import torch.nn as nn
6 | from tqdm.auto import tqdm
7 | from typing import Dict, Optional
8 |
9 | from core.controllers.quadrotor_diffusion_policy import build_noise_scheduler_from_config
10 | from core.dataset.quadrotor_dataset import PlanarQuadrotorStateDataset
11 | from core.trainers.base_trainer import BaseDiffusionPolicyTrainer
12 | from utils.utils import get_device
13 |
14 |
15 | def build_dataloader_from_dataset_and_config(config: Dict, dataset: torch.utils.data.Dataset):
16 | return torch.utils.data.DataLoader(
17 | dataset,
18 | batch_size=config["trainer"]["batch_size"],
19 | shuffle=True,
20 | pin_memory=True,
21 | )
22 |
23 |
24 | class PlanarQuadrotorDiffusionPolicyTrainer(BaseDiffusionPolicyTrainer):
25 | def __init__(
26 | self, net: nn.Module, dataset: PlanarQuadrotorStateDataset, config: Dict, device: Optional[str] = None
27 | ):
28 | self.net = net
29 | self.noise_scheduler = build_noise_scheduler_from_config(config)
30 | self.dataset = dataset
31 | self.set_config(config)
32 | self.device = get_device() if device is None else torch.device(device)
33 |
34 | self.net.to(self.device)
35 |
36 | # build optimizer
37 | if config["trainer"]["optimizer"]["name"].lower() == "adamw":
38 | self.optimizer = torch.optim.AdamW(
39 | params=self.net.parameters(),
40 | lr=config["trainer"]["optimizer"]["learning_rate"],
41 | weight_decay=config["trainer"]["optimizer"]["weight_decay"],
42 | )
43 | else:
44 | raise NotImplementedError
45 |
46 | # build dataset
47 | self.dataloader = build_dataloader_from_dataset_and_config(config, dataset)
48 |
49 | # set EMA
50 | self.use_ema = config["trainer"]["use_ema"]
51 | self.ema = EMAModel(parameters=self.net.parameters(), power=0.75) if self.use_ema else None
52 |
53 | def prepare_inputs(self, batch):
54 | # data normalized in dataset
55 | # device transfer
56 | obs_cond = batch["obs"].to(self.device, dtype=torch.float32) # FiLM conditioning
57 | action = batch["action"].to(self.device, dtype=torch.float32)
58 | batch_size = obs_cond.shape[0]
59 | return obs_cond, action, batch_size
60 |
61 | def optimization_step(self, action, obs_cond, batch_size):
62 | # sample noise to add to actions
63 | noise = torch.randn(action.shape, device=self.device)
64 |
65 | # sample a diffusion iteration for each data point
66 | timesteps = torch.randint(
67 | 0, self.noise_scheduler.config.num_train_timesteps, (batch_size,), device=self.device
68 | ).long()
69 |
70 | # add noise to the clean images according to the noise magnitude at each diffusion iteration
71 | # (this is the forward diffusion process)
72 | noisy_actions = self.noise_scheduler.add_noise(action, noise, timesteps)
73 |
74 | # predict the noise residual
75 | noise_pred = self.net(noisy_actions, timesteps, global_cond=obs_cond)
76 |
77 | # L2 loss
78 | loss = nn.functional.mse_loss(noise_pred, noise)
79 |
80 | # optimize
81 | loss.backward()
82 | self.optimizer.step()
83 | self.optimizer.zero_grad()
84 | # step lr scheduler every batch
85 | # this is different from standard pytorch behavior
86 | self.lr_scheduler.step()
87 |
88 | # update Exponential Moving Average of the model weights
89 | if self.use_ema:
90 | self.ema.step(self.net.parameters())
91 |
92 | return loss
93 |
94 | def train(self, num_epochs: int, save_ckpt_epoch: int = None):
95 | if save_ckpt_epoch is None:
96 | save_ckpt_epoch = num_epochs
97 |
98 | # set learning rate scheduler
99 | self.lr_scheduler = get_scheduler(
100 | name=self.config["trainer"]["lr_scheduler"]["name"],
101 | optimizer=self.optimizer,
102 | num_warmup_steps=self.config["trainer"]["lr_scheduler"]["num_warmup_steps"],
103 | num_training_steps=len(self.dataloader) * num_epochs,
104 | )
105 |
106 | # training loop
107 | trn_loss = []
108 | with tqdm(range(num_epochs), desc="Epoch") as tglobal:
109 | # epoch loop
110 | for epoch_idx in tglobal:
111 | epoch_loss = list()
112 | # batch loop
113 | with tqdm(self.dataloader, desc="Batch", leave=False) as tepoch:
114 | for nbatch in tepoch:
115 | obs_cond, action, B = self.prepare_inputs(nbatch)
116 | loss = self.optimization_step(action, obs_cond, B)
117 |
118 | # logging
119 | loss_cpu = loss.item()
120 | epoch_loss.append(loss_cpu)
121 | tepoch.set_postfix(loss=loss_cpu)
122 |
123 | tglobal.set_postfix(loss=np.mean(epoch_loss))
124 | trn_loss.append(np.mean(epoch_loss))
125 |
126 | # save intermediate ckpt
127 | if (epoch_idx + 1) % save_ckpt_epoch == 0:
128 | self.save_checkpoint(path=f"ckpt_ep{epoch_idx}.ckpt")
129 |
130 | return trn_loss
131 |
132 | def save_checkpoint(self, path: str):
133 | save_model = self.net
134 | if self.config["trainer"]["use_ema"]:
135 | self.ema.copy_to(save_model.parameters())
136 | torch.save(save_model.state_dict(), path)
137 |
138 | def set_config(self, config: Dict):
139 | self.config = config
140 | self.obs_horizon = config["obs_horizon"]
141 | self.obs_dim = config["controller"]["networks"]["obs_dim"]
142 | self.action_horizon = config["action_horizon"]
143 | self.action_dim = config["controller"]["networks"]["action_dim"]
144 | self.pred_horizon = config["pred_horizon"]
145 | self.obstacle_encode_dim = config["controller"]["networks"]["obstacle_encode_dim"]
146 |
147 |
148 | class PlanarQuadrotorDiffusionPolicyFineTuningTrainer(PlanarQuadrotorDiffusionPolicyTrainer):
149 | def __init__(self, net: nn.Module, dataset: PlanarQuadrotorStateDataset, config: Dict, device: str | None = None):
150 | super().__init__(net, dataset, config, device)
151 |
152 | def optimization_step(self, action, obs_cond, batch_size):
153 | # sample noise to add to actions
154 | noise = torch.randn(action.shape, device=self.device)
155 |
156 | # sample a diffusion iteration for each data point
157 | timesteps = torch.randint(
158 | self.noise_scheduler.config.num_train_timesteps - 1,
159 | self.noise_scheduler.config.num_train_timesteps,
160 | (batch_size,),
161 | device=self.device,
162 | ).long()
163 |
164 | # add noise to the clean images according to the noise magnitude at each diffusion iteration
165 | # (this is the forward diffusion process)
166 | noisy_actions = self.noise_scheduler.add_noise(action, noise, timesteps)
167 |
168 | # predict the noise residual
169 | noise_pred = self.net(noisy_actions, timesteps, global_cond=obs_cond)
170 |
171 | # L2 loss
172 | loss = nn.functional.mse_loss(noisy_actions - noise_pred, action)
173 |
174 | # optimize
175 | loss.backward()
176 | self.optimizer.step()
177 | self.optimizer.zero_grad()
178 | # step lr scheduler every batch
179 | # this is different from standard pytorch behavior
180 | self.lr_scheduler.step()
181 |
182 | # update Exponential Moving Average of the model weights
183 | if self.use_ema:
184 | self.ema.step(self.net.parameters())
185 |
186 | return loss
187 |
--------------------------------------------------------------------------------
/demo.ipynb:
--------------------------------------------------------------------------------
1 | {
2 | "cells": [
3 | {
4 | "cell_type": "markdown",
5 | "id": "970e2b9e-ccce-47b2-bccc-9471841cce44",
6 | "metadata": {},
7 | "source": [
8 | "## Installation required in Colab"
9 | ]
10 | },
11 | {
12 | "cell_type": "code",
13 | "execution_count": 1,
14 | "id": "tckkiSjNY0Is",
15 | "metadata": {
16 | "colab": {
17 | "base_uri": "https://localhost:8080/"
18 | },
19 | "id": "tckkiSjNY0Is",
20 | "outputId": "627b5bff-c792-4954-947f-40baab0de93c"
21 | },
22 | "outputs": [
23 | {
24 | "name": "stdout",
25 | "output_type": "stream",
26 | "text": [
27 | "Cloning into 'diffusion_policy_quadrotor'...\n",
28 | "remote: Enumerating objects: 155, done.\u001b[K\n",
29 | "remote: Counting objects: 100% (155/155), done.\u001b[K\n",
30 | "remote: Compressing objects: 100% (104/104), done.\u001b[K\n",
31 | "remote: Total 155 (delta 66), reused 111 (delta 38), pack-reused 0\u001b[K\n",
32 | "Receiving objects: 100% (155/155), 4.19 MiB | 25.69 MiB/s, done.\n",
33 | "Resolving deltas: 100% (66/66), done.\n"
34 | ]
35 | }
36 | ],
37 | "source": [
38 | "! git clone https://github.com/shaoanlu/diffusion_policy_quadrotor.git"
39 | ]
40 | },
41 | {
42 | "cell_type": "code",
43 | "execution_count": 2,
44 | "id": "7rrgEckSZEdx",
45 | "metadata": {
46 | "colab": {
47 | "base_uri": "https://localhost:8080/"
48 | },
49 | "id": "7rrgEckSZEdx",
50 | "outputId": "5cd7bcf5-5b4c-4a48-996b-7257cdf7f03e"
51 | },
52 | "outputs": [
53 | {
54 | "name": "stdout",
55 | "output_type": "stream",
56 | "text": [
57 | "/content/diffusion_policy_quadrotor\n",
58 | "\u001b[0m\u001b[01;34massets\u001b[0m/ \u001b[01;34mconfig\u001b[0m/ \u001b[01;34mcore\u001b[0m/ demo.ipynb LICENSE pyproject.toml README.md \u001b[01;34mutils\u001b[0m/\n"
59 | ]
60 | }
61 | ],
62 | "source": [
63 | "%cd diffusion_policy_quadrotor\n",
64 | "%ls"
65 | ]
66 | },
67 | {
68 | "cell_type": "code",
69 | "execution_count": 3,
70 | "id": "rK2Hjrq_Y4T6",
71 | "metadata": {
72 | "id": "rK2Hjrq_Y4T6"
73 | },
74 | "outputs": [],
75 | "source": [
76 | "%%capture\n",
77 | "!pip3 install torch==1.13.1 torchvision==0.14.1 diffusers==0.18.2 jax==0.4.23 jaxlib==0.4.23"
78 | ]
79 | },
80 | {
81 | "cell_type": "markdown",
82 | "id": "ee7a0434-3333-4502-9563-adcc12d4a413",
83 | "metadata": {
84 | "id": "ee7a0434-3333-4502-9563-adcc12d4a413"
85 | },
86 | "source": [
87 | "## Description\n",
88 | "\n",
89 | "This notebook demonstrate using a diffusion policy controller to drive a quadrotor moving from (0, 0) to (5, 5) with random circle obstacles presented."
90 | ]
91 | },
92 | {
93 | "cell_type": "code",
94 | "execution_count": 1,
95 | "id": "fffc0bb0-584b-4602-885f-a1cfa06d21b6",
96 | "metadata": {
97 | "id": "fffc0bb0-584b-4602-885f-a1cfa06d21b6"
98 | },
99 | "outputs": [],
100 | "source": [
101 | "%load_ext autoreload\n",
102 | "%autoreload 2"
103 | ]
104 | },
105 | {
106 | "cell_type": "code",
107 | "execution_count": 2,
108 | "id": "670f6c7f-93c9-47dc-b2e4-81ea8949b3f9",
109 | "metadata": {
110 | "id": "670f6c7f-93c9-47dc-b2e4-81ea8949b3f9"
111 | },
112 | "outputs": [],
113 | "source": [
114 | "import numpy as np\n",
115 | "import os\n",
116 | "import torch\n",
117 | "import yaml\n",
118 | "import collections\n",
119 | "from tqdm.auto import tqdm\n",
120 | "import gdown"
121 | ]
122 | },
123 | {
124 | "cell_type": "code",
125 | "execution_count": 3,
126 | "id": "fa8f1de5-924a-4889-aacc-bac7601012c5",
127 | "metadata": {
128 | "colab": {
129 | "base_uri": "https://localhost:8080/",
130 | "height": 87,
131 | "referenced_widgets": [
132 | "49a3b00a700b40f8b1de991477efe2b9",
133 | "fb587d9dbfa84d72af2b3c45d46c2cc1",
134 | "7487091f00dd4e3bba795ebab6b0f5b1",
135 | "a817704da5184ef6922fb679bf0c395e",
136 | "a3a8dd4705b64b5f9b21b2e3d4ce24b8",
137 | "3a953a43ed574a4c8e8ebc52d5993347",
138 | "1900dfab0c6e4f8a8350f2d024fc22c6",
139 | "a9afd6d521f44216adb119a54f531376",
140 | "632b2841ae2d44b18e67fe288ba136a1",
141 | "01969ff8ad4140328cda2a14df8fe439",
142 | "fca3ddf1eb6d439ba86878e00e4560fd"
143 | ]
144 | },
145 | "id": "fa8f1de5-924a-4889-aacc-bac7601012c5",
146 | "outputId": "730f9c66-b606-4840-f614-540a89379eef"
147 | },
148 | "outputs": [],
149 | "source": [
150 | "from utils.normalizers import LinearNormalizer\n",
151 | "from core.controllers.quadrotor_diffusion_policy import QuadrotorDiffusionPolicy, build_networks_from_config, build_noise_scheduler_from_config\n",
152 | "from core.controllers.quadrotor_clf_cbf_qp import QuadrotorCLFCBFController\n",
153 | "from core.env.planar_quadrotor import PlanarQuadrotorEnv\n",
154 | "from utils.visualization import visualize_quadrotor_simulation_result"
155 | ]
156 | },
157 | {
158 | "cell_type": "markdown",
159 | "id": "06cf3d67-3f96-457e-a634-77b6e5f2e31e",
160 | "metadata": {
161 | "id": "06cf3d67-3f96-457e-a634-77b6e5f2e31e"
162 | },
163 | "source": [
164 | "## Load the config file"
165 | ]
166 | },
167 | {
168 | "cell_type": "code",
169 | "execution_count": 4,
170 | "id": "0c8c8f90-8ac2-4f87-943d-e41b7ff9ff6a",
171 | "metadata": {
172 | "id": "0c8c8f90-8ac2-4f87-943d-e41b7ff9ff6a",
173 | "scrolled": true
174 | },
175 | "outputs": [],
176 | "source": [
177 | "with open(\"config/config.yaml\", \"r\") as file:\n",
178 | " config = yaml.safe_load(file)\n",
179 | "\n",
180 | "# Whether to use a finetuned model trained following tricks mentioned in\n",
181 | "# [Fine-Tuning Image-Conditional Diffusion Models is Easier than You Think](https://arxiv.org/abs/2409.11355)\n",
182 | "use_single_step_inference = config.get(\"controller\").get(\"common\").get(\"use_single_step_inference\", False)"
183 | ]
184 | },
185 | {
186 | "cell_type": "markdown",
187 | "id": "dd56c89a-520f-46b5-b9d6-ebef0160b184",
188 | "metadata": {
189 | "id": "dd56c89a-520f-46b5-b9d6-ebef0160b184"
190 | },
191 | "source": [
192 | "## Instantiate the controller"
193 | ]
194 | },
195 | {
196 | "cell_type": "code",
197 | "execution_count": 5,
198 | "id": "cb0707a7-3d66-46da-b722-2b2f42d8e099",
199 | "metadata": {
200 | "id": "cb0707a7-3d66-46da-b722-2b2f42d8e099"
201 | },
202 | "outputs": [],
203 | "source": [
204 | "clf_cbf_controller = QuadrotorCLFCBFController(config=config)\n",
205 | "\n",
206 | "controller = QuadrotorDiffusionPolicy(\n",
207 | " model=build_networks_from_config(config),\n",
208 | " noise_scheduler=build_noise_scheduler_from_config(config),\n",
209 | " normalizer=LinearNormalizer(),\n",
210 | " clf_cbf_controller=None, # set as clf_cbf_controller to enable CLF-CBF traj refinement\n",
211 | " config=config,\n",
212 | " device=\"cuda\" if torch.cuda.is_available() else \"cpu\",\n",
213 | ")"
214 | ]
215 | },
216 | {
217 | "cell_type": "markdown",
218 | "id": "cac28d88-e304-46b3-bc28-5fc54cd3705d",
219 | "metadata": {
220 | "id": "cac28d88-e304-46b3-bc28-5fc54cd3705d"
221 | },
222 | "source": [
223 | "## Download and load pretrained weights"
224 | ]
225 | },
226 | {
227 | "cell_type": "code",
228 | "execution_count": null,
229 | "id": "727e3937-dd21-46a2-9fff-c41c1d653022",
230 | "metadata": {},
231 | "outputs": [],
232 | "source": [
233 | "# download pretrained weights from Google drive\n",
234 | "if use_single_step_inference:\n",
235 | " ckpts_path = \"ema_noise_pred_net2_ph96_oh2_ah10_v8_singlestepFT.ckpt\"\n",
236 | " if not os.path.isfile(ckpts_path):\n",
237 | " gdown.download(id=\"1UhxlzoQ6DOt0HZhokzU4ktl2MfzlA2Ii\", output=ckpts_path, quiet=False)\n",
238 | "else:\n",
239 | " ckpts_path = \"ema_noise_pred_net2_ph96_oh2_ah10_v8.ckpt\"\n",
240 | " if not os.path.isfile(ckpts_path):\n",
241 | " gdown.download(id=\"1-as6EqMLECxU7IVLZZIEDAcXMox_RkI_\", output=ckpts_path, quiet=False)\n",
242 | "\n",
243 | "# load weights\n",
244 | "controller.load_weights(ckpts_path)"
245 | ]
246 | },
247 | {
248 | "cell_type": "markdown",
249 | "id": "a0fda0f4-f5cb-4fa9-86da-abf991edfb4b",
250 | "metadata": {
251 | "id": "a0fda0f4-f5cb-4fa9-86da-abf991edfb4b"
252 | },
253 | "source": [
254 | "## Instantiate the simulator"
255 | ]
256 | },
257 | {
258 | "cell_type": "code",
259 | "execution_count": 7,
260 | "id": "6b0ae93e-8fe6-40af-92e6-12643aa21f6d",
261 | "metadata": {
262 | "id": "6b0ae93e-8fe6-40af-92e6-12643aa21f6d"
263 | },
264 | "outputs": [],
265 | "source": [
266 | "sim = PlanarQuadrotorEnv(config)"
267 | ]
268 | },
269 | {
270 | "cell_type": "code",
271 | "execution_count": null,
272 | "id": "a679998d-7a2e-48dc-b0a4-30c24314e307",
273 | "metadata": {
274 | "id": "a679998d-7a2e-48dc-b0a4-30c24314e307"
275 | },
276 | "outputs": [],
277 | "source": []
278 | },
279 | {
280 | "cell_type": "markdown",
281 | "id": "1f1c50bc-e529-432f-92a9-9bb3c6e9fdf7",
282 | "metadata": {
283 | "id": "1f1c50bc-e529-432f-92a9-9bb3c6e9fdf7"
284 | },
285 | "source": [
286 | "## Run simulation"
287 | ]
288 | },
289 | {
290 | "cell_type": "code",
291 | "execution_count": 8,
292 | "id": "eecaed67-9814-4a62-aa7f-2caf5d5b43aa",
293 | "metadata": {
294 | "id": "eecaed67-9814-4a62-aa7f-2caf5d5b43aa"
295 | },
296 | "outputs": [],
297 | "source": [
298 | "def generate_random_obstacles():\n",
299 | " num_obstacles = np.random.randint(1, 8)\n",
300 | " obs_center, obs_radius = np.empty((num_obstacles, 2)), np.ones((num_obstacles,))\n",
301 | " for obs_idx in range(num_obstacles): # set XY position of each obstacle\n",
302 | " obs_center[obs_idx, ...] = np.random.randint(1, 8, size=(2,))\n",
303 | " obs_radius[obs_idx] = np.random.uniform(0.2, 1.5)\n",
304 | " return obs_center, obs_radius\n",
305 | "\n",
306 | "def encode_obstacle_info(obs_center: np.ndarray, obs_radius: np.ndarray):\n",
307 | " obs_encode = np.zeros((7, 7))\n",
308 | " for center, radius in zip(obs_center, obs_radius):\n",
309 | " obs_encode[tuple((center-1).astype(np.int32))] = radius\n",
310 | " obs_encode = obs_encode.flatten()\n",
311 | " return obs_encode"
312 | ]
313 | },
314 | {
315 | "cell_type": "code",
316 | "execution_count": 9,
317 | "id": "3250c190-d380-423c-8455-21a6c1b2c093",
318 | "metadata": {
319 | "colab": {
320 | "base_uri": "https://localhost:8080/",
321 | "height": 67,
322 | "referenced_widgets": [
323 | "fa8ca951c0f54d3c965b136791c54f60",
324 | "14869b876e6143a9848a8421b8da93c3",
325 | "b00b242d48c34b739b335e6137bbec52",
326 | "ba8a57462eb4492880c4980ee126f888",
327 | "7afde10b06494393ab9b53818c580fdc",
328 | "bc399470fb094590a12ba13e98c1f6a8",
329 | "eba7a2b036374b1c9c329d884ce16ad4",
330 | "13066600e23245888e019523e0200005",
331 | "304615b68c9d496ea35928d1546df4ca",
332 | "3de6654be9de4d22b15fb28ad7625f57",
333 | "81e23a9379b34c0cb78e375d47a63a89"
334 | ]
335 | },
336 | "id": "3250c190-d380-423c-8455-21a6c1b2c093",
337 | "outputId": "4c9609f0-a944-42f7-909c-c23cab24dbf0"
338 | },
339 | "outputs": [
340 | {
341 | "data": {
342 | "application/vnd.jupyter.widget-view+json": {
343 | "model_id": "3d93b38cf8264ebe80428df1989880d1",
344 | "version_major": 2,
345 | "version_minor": 0
346 | },
347 | "text/plain": [
348 | "Eval: 0%| | 0/400 [00:00, ?it/s]"
349 | ]
350 | },
351 | "metadata": {},
352 | "output_type": "display_data"
353 | }
354 | ],
355 | "source": [
356 | "np.random.seed(123)\n",
357 | "\n",
358 | "# Env parameters\n",
359 | "max_steps = 400\n",
360 | "dt = 0.01\n",
361 | "ratio_sim_ts = 10 # ratio of sampling time between the simulator and the controller\n",
362 | "\n",
363 | "# get first observation\n",
364 | "state = np.array([0.0, 0.0, 0.0, 0.0, 0.0, 0.0]) # [y, y_dot, z, z_dot, phi, phi_dot]\n",
365 | "states = [state] # `states` is a list containing the states over time\n",
366 | "controls = [np.array([0, 0])]\n",
367 | "obs_center, obs_radius = generate_random_obstacles()\n",
368 | "obs_encode = encode_obstacle_info(obs_center, obs_radius)\n",
369 | "obs = {\n",
370 | " \"state\": collections.deque([state] * controller.obs_horizon, maxlen=controller.obs_horizon),\n",
371 | " \"obs_encode\": [obs_encode],\n",
372 | " \"obs_center\": obs_center,\n",
373 | " \"obs_radius\": obs_radius,\n",
374 | "}\n",
375 | "\n",
376 | "# termimnation params\n",
377 | "done = False\n",
378 | "step_idx = 0\n",
379 | "\n",
380 | "with tqdm(total=max_steps, desc=\"Eval\") as pbar:\n",
381 | " while not done:\n",
382 | " # controller inference\n",
383 | " action = controller.predict_action(obs)\n",
384 | "\n",
385 | " # execute action_horizon steps without replanning\n",
386 | " for i in range(action.shape[0]):\n",
387 | " # stepping env\n",
388 | " command = controller.calculate_force_command(state, action[i])\n",
389 | " for _ in range(ratio_sim_ts):\n",
390 | " state = sim.step(state, command, dt=dt/ratio_sim_ts)\n",
391 | " # save observations and controls\n",
392 | " obs[\"state\"].append(state.copy())\n",
393 | " states.append(state.copy())\n",
394 | " controls.append(action[i].copy())\n",
395 | "\n",
396 | " # update progress bar\n",
397 | " step_idx += 1\n",
398 | " pbar.update(1)\n",
399 | " if step_idx > max_steps:\n",
400 | " done = True\n",
401 | " if done:\n",
402 | " break"
403 | ]
404 | },
405 | {
406 | "cell_type": "code",
407 | "execution_count": null,
408 | "id": "b006ad91-e853-4a03-80c5-7ecb142bc475",
409 | "metadata": {
410 | "id": "b006ad91-e853-4a03-80c5-7ecb142bc475"
411 | },
412 | "outputs": [],
413 | "source": []
414 | },
415 | {
416 | "cell_type": "markdown",
417 | "id": "602bdd60-2b90-48db-a29b-7f8645501142",
418 | "metadata": {
419 | "id": "602bdd60-2b90-48db-a29b-7f8645501142"
420 | },
421 | "source": [
422 | "## Visualize result"
423 | ]
424 | },
425 | {
426 | "cell_type": "code",
427 | "execution_count": 10,
428 | "id": "b92bcec2-d95d-4361-9b92-991346d83950",
429 | "metadata": {
430 | "colab": {
431 | "base_uri": "https://localhost:8080/",
432 | "height": 435
433 | },
434 | "id": "b92bcec2-d95d-4361-9b92-991346d83950",
435 | "outputId": "0d8c5a83-8002-4deb-a87b-48a04af6d0af"
436 | },
437 | "outputs": [
438 | {
439 | "data": {
440 | "image/png": "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",
441 | "text/plain": [
442 | ""
443 | ]
444 | },
445 | "metadata": {},
446 | "output_type": "display_data"
447 | }
448 | ],
449 | "source": [
450 | "states = np.array(states)\n",
451 | "visualize_quadrotor_simulation_result(sim, states, obs_center=obs_center, obs_radius=obs_radius)"
452 | ]
453 | },
454 | {
455 | "cell_type": "code",
456 | "execution_count": null,
457 | "id": "33740290-22ae-45bd-822f-0bea782691bd",
458 | "metadata": {
459 | "id": "33740290-22ae-45bd-822f-0bea782691bd"
460 | },
461 | "outputs": [],
462 | "source": []
463 | },
464 | {
465 | "cell_type": "code",
466 | "execution_count": null,
467 | "id": "f15cae17-f337-4ba0-a4ea-5a10a8bc0390",
468 | "metadata": {
469 | "id": "f15cae17-f337-4ba0-a4ea-5a10a8bc0390"
470 | },
471 | "outputs": [],
472 | "source": []
473 | },
474 | {
475 | "cell_type": "code",
476 | "execution_count": null,
477 | "id": "7f889988-1fef-4c11-b113-457247db185c",
478 | "metadata": {
479 | "id": "7f889988-1fef-4c11-b113-457247db185c"
480 | },
481 | "outputs": [],
482 | "source": []
483 | },
484 | {
485 | "cell_type": "code",
486 | "execution_count": null,
487 | "id": "6c503b86-5211-4403-a830-b8a100e73b97",
488 | "metadata": {
489 | "id": "6c503b86-5211-4403-a830-b8a100e73b97"
490 | },
491 | "outputs": [],
492 | "source": []
493 | },
494 | {
495 | "cell_type": "code",
496 | "execution_count": null,
497 | "id": "f0772564-5e68-4884-a266-f5467db4a433",
498 | "metadata": {
499 | "id": "f0772564-5e68-4884-a266-f5467db4a433"
500 | },
501 | "outputs": [],
502 | "source": []
503 | },
504 | {
505 | "cell_type": "code",
506 | "execution_count": null,
507 | "id": "3c1ad555-40ce-4917-b0f7-e26dce4d485d",
508 | "metadata": {
509 | "id": "3c1ad555-40ce-4917-b0f7-e26dce4d485d"
510 | },
511 | "outputs": [],
512 | "source": []
513 | }
514 | ],
515 | "metadata": {
516 | "accelerator": "GPU",
517 | "colab": {
518 | "gpuType": "T4",
519 | "provenance": []
520 | },
521 | "kernelspec": {
522 | "display_name": "Python 3 (ipykernel)",
523 | "language": "python",
524 | "name": "python3"
525 | },
526 | "language_info": {
527 | "codemirror_mode": {
528 | "name": "ipython",
529 | "version": 3
530 | },
531 | "file_extension": ".py",
532 | "mimetype": "text/x-python",
533 | "name": "python",
534 | "nbconvert_exporter": "python",
535 | "pygments_lexer": "ipython3",
536 | "version": "3.10.14"
537 | },
538 | "widgets": {
539 | "application/vnd.jupyter.widget-state+json": {
540 | "state": {},
541 | "version_major": 2,
542 | "version_minor": 0
543 | }
544 | }
545 | },
546 | "nbformat": 4,
547 | "nbformat_minor": 5
548 | }
549 |
--------------------------------------------------------------------------------
/learning_note.md:
--------------------------------------------------------------------------------
1 | ## Learning note
2 | ### Main insight
3 | - The model should learn the residuals (gradient of the denoising) if possible. This greatly stabilizes the training.
4 | - Advantages of diffusion model: 1) capability of modeling multi-modality, 2) stable training, and 3) temporally output consistency.
5 | - Iteratively add training data of failure modes to make extrapolation into interpolation.
6 | ### Scribbles
7 | - The trained policy does not 100% reach the goal without collision (there is no collision in its training data).
8 | - Unable to recover from OOD data.
9 | - Long observation might be harmful to the performance, possibly due to the increased possibility of model overfitting.
10 | - The diffusion policy [paper](https://arxiv.org/pdf/2303.04137) also discusses this in its appendix.
11 | - I feel we don't need diffusion models for the simple task in this repo, supervised learning might be equally effective.
12 | - The controller struggles with performance issues when extreme values (maximum or minimum) are presented in the conditional vector.
13 | - For instance, it collides more on obstacles with a maximum radius of 1.5.
14 | - Collect more data to make everything interpolations instead of extrapolations.
15 | - Even though the loss curve appears saturated, the performance of the controller can still improve as training continues.
16 | - The training loss curves of the diffusion model are extremely smooth btw.
17 | - On the contrary, it might be difficult to know if the model is overfitting or not by looking at the trajectory as well as the the denoising process.
18 | - But in general I feel there is little harm training duffusion model as long as possible.
19 | - DDPM and DDIM samplers yield the best result.
20 | - Inference is not in real-time. The controller is set to sun 100Hz.
21 |
22 | ### Possible reasons for failures on collision avoidance
23 | 1. There is no data having collision in the training data.
24 | 2. Policy learned with imitation learning can exhibit accumulated error during closed-loop control
25 |
26 | When the quadrotor getting too close to the obstacles (due to 2), the input state becomes OOD (due to 1), therefore the diffusion policy is unable to recover from such situation.
27 |
28 | - Possible fix: adding training data that the quadrotor recovers from collision.
29 |
30 | ### Things that didn't work
31 | - Tried encoding distances to each obstacle. Did not observe improvement in terms of collision avoidance.
32 | - Tried using vision encoder to replace obstacle encoding. Didn't see performance improvement.
--------------------------------------------------------------------------------
/pyproject.toml:
--------------------------------------------------------------------------------
1 | [tool.ruff]
2 |
3 | # lint.select = ["A", "ANN", "B", "C90", "D", "E", "F", "I", "N", "COM", "DTZ", "PD", "RUF", "TID", "UP", "W"]
4 | # lint.ignore = ["D203", "D212"]
5 |
6 | lint.fixable = ["I", "RUF100"]
7 | lint.unfixable = []
8 |
9 | # Exclude a variety of commonly ignored directories.
10 | exclude = [
11 | ".bzr",
12 | ".direnv",
13 | ".eggs",
14 | ".git",
15 | ".hg",
16 | ".mypy_cache",
17 | ".nox",
18 | ".pants.d",
19 | ".pytype",
20 | ".ruff_cache",
21 | ".svn",
22 | ".tox",
23 | ".venv",
24 | "__pypackages__",
25 | "_build",
26 | "buck-out",
27 | "build",
28 | "dist",
29 | "node_modules",
30 | "venv",
31 | ]
32 |
33 | line-length = 120
34 |
35 | # Allow unused variables when underscore-prefixed.
36 | lint.dummy-variable-rgx = "^(_+|(_+[a-zA-Z0-9_]*[a-zA-Z0-9]+?))$"
37 |
38 | target-version = "py310"
39 |
40 | [tool.ruff.lint.mccabe]
41 | max-complexity = 10
42 |
43 |
44 | [tool.pyright]
45 | typeCheckingMode = "lazy"
46 | defineConstant = { DEBUG = true }
47 |
48 | reportMissingImports = false
49 | reportMissingTypeStubs = false
50 | reportInvalidStringEscapeSequence = false
51 |
52 | pythonVersion = "3.10"
53 | pythonPlatform = "Linux"
54 |
--------------------------------------------------------------------------------
/tests/__init__.py:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/shaoanlu/diffusion_policy_quadrotor/58781ae2ad1e7b4ff83b40d4726a3eae4f2cefff/tests/__init__.py
--------------------------------------------------------------------------------
/tests/config/test_config.yaml:
--------------------------------------------------------------------------------
1 | name: "planar_quadrotor"
2 |
3 | pred_horizon: 96
4 | obs_horizon: 2
5 | action_horizon: 10
6 |
7 | controller:
8 | networks:
9 | obs_dim: 6
10 | action_dim: 6
11 | obstacle_encode_dim: 49
12 | noise_scheduler:
13 | type: "ddpm"
14 | ddpm:
15 | num_train_timesteps: 100 # number of diffusion iterations
16 | beta_schedule: "squaredcos_cap_v2"
17 | clip_sample: true # required when predict_epsilon=False
18 | prediction_type: "epsilon"
19 | ddim: # faster inference
20 | num_train_timesteps: 100
21 | beta_schedule: "squaredcos_cap_v2"
22 | clip_sample: true
23 | prediction_type: "epsilon"
24 | dpmsolver: # faster inference, experimental
25 | num_train_timesteps: 100
26 | beta_schedule: "squaredcos_cap_v2"
27 | prediction_type: "epsilon"
28 | use_karras_sigmas: true
29 |
30 | trainer:
31 | use_ema: true
32 | batch_size: 8
33 | optimizer:
34 | name: "adamw"
35 | learning_rate: 1.0e-4
36 | weight_decay: 1.0e-6
37 | lr_scheduler:
38 | name: "cosine"
39 | num_warmup_steps: 500
40 |
41 | dataloader:
42 | batch_size: 3
43 |
44 | normalizer:
45 | action:
46 | min: [-2.6515920785069467, -10.169477462768555, -4.270973491668701, -13.883374419993748, -1.9637073183059692, -20.395111083984375]
47 | max: [8.0543811917305, 6.976394176483154, 8.477932775914669, 11.327190831233095, 2.5276688146591186, 18.05487823486328]
48 | observation:
49 | min: [-2.649836301803589, -9.564324378967285, -4.264063358306885, -13.772777557373047, -1.9476231336593628, -17.225351333618164]
50 | max: [8.05234146118164, 6.976299285888672, 8.474746704101562, 10.96181583404541, 2.5151419639587402, 18.054880142211914]
51 |
52 | simulator:
53 | m_q: 1.0 # kg
54 | I_xx: 0.1 # kg.m^2
55 | l_q: 0.3 # m, length of the quadrotor
56 | g: 9.81
57 | dt: 0.01
58 |
--------------------------------------------------------------------------------
/tests/fixtures/test_dataset.joblib:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/shaoanlu/diffusion_policy_quadrotor/58781ae2ad1e7b4ff83b40d4726a3eae4f2cefff/tests/fixtures/test_dataset.joblib
--------------------------------------------------------------------------------
/tests/test_datasets.py:
--------------------------------------------------------------------------------
1 | import torch
2 | import unittest
3 | import yaml
4 |
5 | from core.dataset.quadrotor_dataset import PlanarQuadrotorStateDataset
6 |
7 |
8 | class TestPlanarQuadrotorStateDataset(unittest.TestCase):
9 | def setUp(self):
10 | self.dataset_path = "tests/fixtures/test_dataset.joblib"
11 |
12 | with open("tests/config/test_config.yaml", "r") as file:
13 | self.config = yaml.safe_load(file)
14 |
15 | self.obs_horizon = self.config["obs_horizon"]
16 | self.action_horizon = self.config["action_horizon"]
17 | self.pred_horizon = self.config["pred_horizon"]
18 | self.action_dim = self.config["controller"]["networks"]["action_dim"]
19 | self.obs_dim = self.config["controller"]["networks"]["obs_dim"]
20 | self.obstacle_encode_dim = self.config["controller"]["networks"]["obstacle_encode_dim"]
21 |
22 | def test_init(self):
23 | dataset = PlanarQuadrotorStateDataset(dataset_path=self.dataset_path, config=self.config)
24 | self.assertTrue(isinstance(dataset, PlanarQuadrotorStateDataset))
25 |
26 | def test_iter(self):
27 | batch_size = self.config["dataloader"]["batch_size"]
28 | dataset = PlanarQuadrotorStateDataset(dataset_path=self.dataset_path, config=self.config)
29 | dataloader = torch.utils.data.DataLoader(
30 | dataset,
31 | batch_size=batch_size,
32 | shuffle=True,
33 | pin_memory=True,
34 | )
35 |
36 | # batch context matches expectecd shapes
37 | batch = next(iter(dataloader))
38 | self.assertEqual(batch["obs"].shape, (batch_size, self.obs_dim * self.obs_horizon + self.obstacle_encode_dim))
39 | self.assertEqual(batch["action"].shape, (batch_size, self.pred_horizon, self.action_dim))
40 |
41 |
42 | if __name__ == "__main__":
43 | unittest.main()
44 |
--------------------------------------------------------------------------------
/tests/test_networks.py:
--------------------------------------------------------------------------------
1 | import torch
2 | import unittest
3 | import yaml
4 |
5 | from core.networks.conditional_unet1d import ConditionalUnet1D
6 |
7 |
8 | class TestConditionalUnet1D(unittest.TestCase):
9 | def setUp(self):
10 | with open("tests/config/test_config.yaml", "r") as file:
11 | self.config = yaml.safe_load(file)
12 |
13 | self.obs_horizon = self.config["obs_horizon"]
14 | self.action_horizon = self.config["action_horizon"]
15 | self.pred_horizon = self.config["pred_horizon"]
16 | self.action_dim = self.config["controller"]["networks"]["action_dim"]
17 | self.obs_dim = self.config["controller"]["networks"]["obs_dim"]
18 | self.obstacle_encode_dim = self.config["controller"]["networks"]["obstacle_encode_dim"]
19 |
20 | def test_init(self):
21 | noise_pred_net = ConditionalUnet1D(
22 | input_dim=self.action_dim, global_cond_dim=self.obs_dim * self.obs_horizon + self.obstacle_encode_dim
23 | )
24 | self.assertTrue(isinstance(noise_pred_net, ConditionalUnet1D))
25 |
26 | def test_inference(self):
27 | net = ConditionalUnet1D(
28 | input_dim=self.action_dim, global_cond_dim=self.obs_dim * self.obs_horizon + self.obstacle_encode_dim
29 | )
30 |
31 | # example inputs
32 | noised_action = torch.randn((1, self.pred_horizon, self.action_dim))
33 | obs = torch.zeros((1, self.obs_horizon * self.obs_dim + self.obstacle_encode_dim))
34 | diffusion_iter = torch.zeros((1,))
35 |
36 | # the noise prediction network
37 | # takes noisy action, diffusion iteration and observation as input
38 | # predicts the noise added to action
39 | noise = net(sample=noised_action, timestep=diffusion_iter, global_cond=obs.flatten(start_dim=1))
40 | # removing noise
41 | denoised_action = noised_action - noise
42 |
43 | self.assertEqual(denoised_action.shape, (1, self.pred_horizon, self.action_dim))
44 |
45 |
46 | if __name__ == "__main__":
47 | unittest.main()
48 |
--------------------------------------------------------------------------------
/utils/normalizers.py:
--------------------------------------------------------------------------------
1 | import numpy as np
2 |
3 |
4 | class BaseNormalizer:
5 | def normalize_data(self, *args, **kwargs):
6 | raise NotImplementedError()
7 |
8 | def unnormalize_data(self, *args, **kwargs):
9 | raise NotImplementedError()
10 |
11 |
12 | class LinearNormalizer(BaseNormalizer):
13 | def __init__(self):
14 | pass
15 |
16 | def normalize_data(self, data, stats):
17 | # nomalize to [0,1]
18 | ndata = (data - np.array(stats["min"])) / (np.array(stats["max"]) - np.array(stats["min"]))
19 | # normalize to [-1, 1]
20 | ndata = ndata * 2 - 1
21 | return ndata
22 |
23 | def unnormalize_data(self, ndata, stats):
24 | ndata = (ndata + 1) / 2
25 | data = ndata * (np.array(stats["max"]) - np.array(stats["min"])) + np.array(stats["min"])
26 | return data
27 |
--------------------------------------------------------------------------------
/utils/utils.py:
--------------------------------------------------------------------------------
1 | import torch
2 |
3 |
4 | def get_device():
5 | if torch.cuda.is_available():
6 | device = "cuda"
7 | else:
8 | device = "cpu"
9 | return torch.device(device)
10 |
--------------------------------------------------------------------------------
/utils/visualization.py:
--------------------------------------------------------------------------------
1 | import numpy as np
2 | from matplotlib import pyplot as plt
3 |
4 |
5 | def visualize_quadrotor_simulation_result(
6 | quadrotor, states: np.ndarray, obs_center: np.ndarray, obs_radius: np.ndarray
7 | ):
8 | def plot_obstacles(obs_center, obs_radius):
9 | for obs_p, obs_r in zip(obs_center, obs_radius):
10 | circle = plt.Circle(
11 | tuple(obs_p),
12 | obs_r,
13 | color="grey",
14 | fill=True,
15 | linestyle="--",
16 | linewidth=2,
17 | alpha=0.5,
18 | )
19 | plt.gca().add_artist(circle)
20 |
21 | plt.figure()
22 | plt.gca().set_aspect("equal", adjustable="box")
23 | ys = [s[0] for s in states]
24 | zs = [s[2] for s in states]
25 | phis = [s[4] for s in states]
26 | # Generate circle for CBF
27 | plot_obstacles(obs_center, obs_radius)
28 |
29 | # Plot the trajectory
30 | plt.scatter(ys, zs)
31 |
32 | # Plot quadrotor pose
33 | y_ = [(y - quadrotor.l_q * np.cos(phi), y + quadrotor.l_q * np.cos(phi)) for (y, phi) in zip(ys[::10], phis[::10])]
34 | z_ = [(z - quadrotor.l_q * np.sin(phi), z + quadrotor.l_q * np.sin(phi)) for (z, phi) in zip(zs[::10], phis[::10])]
35 | for yy, zz in zip(y_, z_):
36 | plt.plot(yy, zz, marker="o", color="r", alpha=0.5)
37 |
38 | # plot start and end point
39 | init_x, init_y = states[0][0], states[0][2]
40 | plt.scatter(init_x, init_y, s=200, color="green", alpha=0.75, label="init. position")
41 | plt.scatter(5.0, 5.0, s=200, color="purple", alpha=0.75, label="target position")
42 |
43 | plt.xlim(-0.5, 7)
44 | plt.ylim(-0.5, 7)
45 | plt.grid()
46 | plt.show()
47 |
--------------------------------------------------------------------------------