Skip to content

Drone Obstacle Avoidance

cbfpy.examples.drone_demo

Drone Obstacle Avoidance Demo

We use velocity control, which can be easily applied to other drones (for instance, PX4-controlled quadrotors can take in velocity commands)

The CBF uses a reduced model of the drone dynamics, while the simulation environment does reflect a (somewhat-accurate) model of the true drone dynamics

Here, our state is the position and velocity of the drone: z = [position, velocity] and the control input is the velocity of the drone: u = [velocity]

We also incorporate the state of the obstacle as an additional input to the CBF: z_obs = [position, velocity]

Note: since there is gravity in this simulation, the obstacle will fall to the ground initially. This is fine: just use the mouse to drag it around near the drone to see the CBF response

See https://danielpmorton.github.io/drone_fencing/ for a demo of this on real hardware, and see the "point robot obstacle avoidance" demo in CBFpy for a simplified version of this demo

DroneConfig

Bases: CBFConfig

Config for the drone obstacle avoidance / safe set containment demo

Source code in cbfpy/examples/drone_demo.py 
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
class DroneConfig(CBFConfig):
    """Config for the drone obstacle avoidance / safe set containment demo"""

    def __init__(self):
        self.mass = 1.0
        self.pos_min = jnp.array([-2.0, -2.0, 0.7])
        self.pos_max = jnp.array([2.0, 2.0, 2.0])
        init_z_obs = jnp.array([3.0, 1.0, 1.0, -0.1, -0.1, -0.1])
        super().__init__(
            n=6,  # State = [position, velocity]
            m=3,  # Control = [velocity]
            relax_cbf=True,
            init_args=(init_z_obs,),
            cbf_relaxation_penalty=1e6,
        )

    def f(self, z):
        # Assume we are directly controlling the robot's velocity
        return jnp.zeros(self.n)

    def g(self, z):
        # Assume we are directly controlling the robot's velocity
        return jnp.block([[jnp.eye(3)], [jnp.zeros((3, 3))]])

    def h_1(self, z, z_obs):
        obstacle_radius = 0.25
        robot_radius = 0.25
        padding = 0.1
        pos_robot = z[:3]
        vel_robot = z[3:]
        pos_obs = z_obs[:3]
        vel_obs = z_obs[3:]
        dist_between_centers = jnp.linalg.norm(pos_obs - pos_robot)
        dir_obs_to_robot = (pos_robot - pos_obs) / dist_between_centers
        collision_velocity_component = (vel_obs - vel_robot).T @ dir_obs_to_robot
        lookahead_time = 2.0
        padding = 0.1
        h_obstacle_avoidance = jnp.array(
            [
                dist_between_centers
                - collision_velocity_component * lookahead_time
                - obstacle_radius
                - robot_radius
                - padding
            ]
        )
        h_box_containment = jnp.concatenate([self.pos_max - z[:3], z[:3] - self.pos_min])
        return jnp.concatenate([h_obstacle_avoidance, h_box_containment])

    def alpha(self, h):
        return jnp.array([3.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0]) * h