sim.physics

crazyflow.sim.physics

Physics models for the simulation.

Classes

FirstPrinciplesData

Attributes

J instance-attribute

Inertia matrix of the drone.

J_inv instance-attribute

Inverse of the inertia matrix of the drone.

L instance-attribute

Arm length of the drone.

drag_matrix instance-attribute

Drag matrix of the drone.

gravity_vec instance-attribute

Gravity vector of the drone.

mass instance-attribute

Mass of the drone.

mixing_matrix instance-attribute

Mixing matrix of the drone.

prop_inertia instance-attribute

Inertia of the propeller.

rotor_dyn_coef instance-attribute

Rotor speed dynamics time constant of the drone.

rpm2thrust instance-attribute

Force constant of the drone.

rpm2torque instance-attribute

Torque constant of the drone.

Functions

create(n_worlds, n_drones, drone_model, device) staticmethod

Create a default set of parameters for the simulation.

Source code in crazyflow/sim/physics.py

@staticmethod
def create(
    n_worlds: int, n_drones: int, drone_model: str, device: Device
) -> FirstPrinciplesData:
    """Create a default set of parameters for the simulation."""
    p = load_params("first_principles", drone_model)
    J = jax.device_put(jnp.tile(p["J"][None, None, :, :], (n_worlds, n_drones, 1, 1)), device)
    return FirstPrinciplesData(
        mass=jnp.full((n_worlds, n_drones, 1), p["mass"], device=device),
        L=jnp.asarray(p["L"], device=device),
        prop_inertia=jnp.asarray(p["prop_inertia"], device=device),
        gravity_vec=jnp.asarray(p["gravity_vec"], device=device),
        J=J,
        J_inv=jnp.linalg.inv(J),
        rpm2thrust=jnp.asarray(p["rpm2thrust"], device=device),
        rpm2torque=jnp.asarray(p["rpm2torque"], device=device),
        mixing_matrix=jnp.asarray(p["mixing_matrix"], device=device),
        drag_matrix=jnp.asarray(p["drag_matrix"], device=device),
        rotor_dyn_coef=jnp.asarray(p["rotor_dyn_coef"], device=device),
    )

Physics

Bases: str, Enum

Physics mode for the simulation.

SoRpyData

Attributes

J instance-attribute

Inertia matrix of the drone.

J_inv instance-attribute

Inverse of the inertia matrix of the drone.

acc_coef instance-attribute

Coefficient for the acceleration.

cmd_f_coef instance-attribute

Coefficient for the collective thrust.

cmd_rpy_coef instance-attribute

Coefficient for the roll pitch yaw command dynamics.

gravity_vec instance-attribute

Gravity vector of the drone.

mass instance-attribute

Mass of the drone.

rpy_coef instance-attribute

Coefficient for the roll pitch yaw dynamics.

rpy_rates_coef instance-attribute

Coefficient for the roll pitch yaw rates dynamics.

Functions

create(n_worlds, n_drones, drone_model, device) staticmethod

Create a default set of parameters for the simulation.

Source code in crazyflow/sim/physics.py

@staticmethod
def create(n_worlds: int, n_drones: int, drone_model: str, device: Device) -> SoRpyData:
    """Create a default set of parameters for the simulation."""
    p = load_params("so_rpy", drone_model)
    J = jax.device_put(jnp.tile(p["J"][None, None, :, :], (n_worlds, n_drones, 1, 1)), device)
    return SoRpyData(
        mass=jnp.full((n_worlds, n_drones, 1), p["mass"], device=device),
        gravity_vec=jnp.asarray(p["gravity_vec"], device=device),
        J=J,
        J_inv=jnp.linalg.inv(J),
        acc_coef=jnp.asarray(p["acc_coef"], device=device),
        cmd_f_coef=jnp.asarray(p["cmd_f_coef"], device=device),
        rpy_coef=jnp.asarray(p["rpy_coef"], device=device),
        rpy_rates_coef=jnp.asarray(p["rpy_rates_coef"], device=device),
        cmd_rpy_coef=jnp.asarray(p["cmd_rpy_coef"], device=device),
    )

SoRpyRotorData

Attributes

J instance-attribute

Inertia matrix of the drone.

J_inv instance-attribute

Inverse of the inertia matrix of the drone.

acc_coef instance-attribute

Acceleration coefficient of the drone.

cmd_f_coef instance-attribute

Collective thrust coefficient of the drone.

cmd_rpy_coef instance-attribute

Roll pitch yaw command coefficient of the drone.

gravity_vec instance-attribute

Gravity vector of the drone.

mass instance-attribute

Mass of the drone.

rpy_coef instance-attribute

Roll pitch yaw coefficient of the drone.

rpy_rates_coef instance-attribute

Roll pitch yaw rates coefficient of the drone.

thrust_time_coef instance-attribute

Rotor coefficient of the drone.

Functions

create(n_worlds, n_drones, drone_model, device) staticmethod

Create a default set of parameters for the simulation.

Source code in crazyflow/sim/physics.py

@staticmethod
def create(n_worlds: int, n_drones: int, drone_model: str, device: Device) -> SoRpyRotorData:
    """Create a default set of parameters for the simulation."""
    p = load_params("so_rpy_rotor", drone_model)
    J = jax.device_put(jnp.tile(p["J"][None, None, :, :], (n_worlds, n_drones, 1, 1)), device)
    return SoRpyRotorData(
        mass=jnp.full((n_worlds, n_drones, 1), p["mass"], device=device),
        gravity_vec=jnp.asarray(p["gravity_vec"], device=device),
        J=J,
        J_inv=jnp.linalg.inv(J),
        thrust_time_coef=jnp.asarray(p["thrust_time_coef"], device=device),
        acc_coef=jnp.asarray(p["acc_coef"], device=device),
        cmd_f_coef=jnp.asarray(p["cmd_f_coef"], device=device),
        rpy_coef=jnp.asarray(p["rpy_coef"], device=device),
        rpy_rates_coef=jnp.asarray(p["rpy_rates_coef"], device=device),
        cmd_rpy_coef=jnp.asarray(p["cmd_rpy_coef"], device=device),
    )

SoRpyRotorDragData

Attributes

J instance-attribute

Inertia matrix of the drone.

J_inv instance-attribute

Inverse of the inertia matrix of the drone.

acc_coef instance-attribute

Acceleration coefficient of the drone.

cmd_f_coef instance-attribute

Collective thrust coefficient of the drone.

cmd_rpy_coef instance-attribute

Roll pitch yaw command coefficient of the drone.

drag_matrix instance-attribute

Linear drag coefficient matrix of the drone.

gravity_vec instance-attribute

Gravity vector of the drone.

mass instance-attribute

Mass of the drone.

rpy_coef instance-attribute

Roll pitch yaw coefficient of the drone.

rpy_rates_coef instance-attribute

Roll pitch yaw rates coefficient of the drone.

thrust_time_coef instance-attribute

Rotor coefficient of the drone.

Functions

create(n_worlds, n_drones, drone_model, device) staticmethod

Create a default set of parameters for the simulation.

Source code in crazyflow/sim/physics.py

@staticmethod
def create(
    n_worlds: int, n_drones: int, drone_model: str, device: Device
) -> SoRpyRotorDragData:
    """Create a default set of parameters for the simulation."""
    p = load_params("so_rpy_rotor_drag", drone_model)
    J = jax.device_put(jnp.tile(p["J"][None, None, :, :], (n_worlds, n_drones, 1, 1)), device)
    return SoRpyRotorDragData(
        mass=jnp.full((n_worlds, n_drones, 1), p["mass"], device=device),
        gravity_vec=jnp.asarray(p["gravity_vec"], device=device),
        J=J,
        J_inv=jnp.linalg.inv(J),
        thrust_time_coef=jnp.asarray(p["thrust_time_coef"], device=device),
        acc_coef=jnp.asarray(p["acc_coef"], device=device),
        cmd_f_coef=jnp.asarray(p["cmd_f_coef"], device=device),
        rpy_coef=jnp.asarray(p["rpy_coef"], device=device),
        rpy_rates_coef=jnp.asarray(p["rpy_rates_coef"], device=device),
        cmd_rpy_coef=jnp.asarray(p["cmd_rpy_coef"], device=device),
        drag_matrix=jnp.asarray(p["drag_matrix"], device=device),
    )

Functions

first_principles_physics(data)

Compute the forces and torques from the first principle physics model.

Source code in crazyflow/sim/physics.py

def first_principles_physics(data: SimData) -> SimData:
    """Compute the forces and torques from the first principle physics model."""
    params: FirstPrinciplesData = data.params
    vel, _, acc, ang_acc, rotor_acc = first_principles_dynamics(
        pos=data.states.pos,
        quat=data.states.quat,
        vel=data.states.vel,
        ang_vel=data.states.ang_vel,
        cmd=data.controls.rotor_vel,
        rotor_vel=data.states.rotor_vel,
        dist_f=data.states.force,
        dist_t=data.states.torque,
        mass=params.mass,
        L=params.L,
        prop_inertia=params.prop_inertia,
        gravity_vec=params.gravity_vec,
        J=params.J,
        J_inv=params.J_inv,
        rpm2thrust=params.rpm2thrust,
        rpm2torque=params.rpm2torque,
        mixing_matrix=params.mixing_matrix,
        drag_matrix=params.drag_matrix,
        rotor_dyn_coef=params.rotor_dyn_coef,
    )
    states_deriv = data.states_deriv.replace(
        vel=vel, ang_vel=data.states.ang_vel, acc=acc, ang_acc=ang_acc, rotor_acc=rotor_acc
    )
    return data.replace(states_deriv=states_deriv)

so_rpy_physics(data)

Compute the forces and torques from the so_rpy physics model.

Source code in crazyflow/sim/physics.py

def so_rpy_physics(data: SimData) -> SimData:
    """Compute the forces and torques from the so_rpy physics model."""
    params: SoRpyData = data.params
    vel, _, acc, ang_acc, _ = so_rpy_dynamics(
        pos=data.states.pos,
        quat=data.states.quat,
        vel=data.states.vel,
        ang_vel=data.states.ang_vel,
        cmd=data.controls.attitude.cmd,
        dist_f=data.states.force,
        dist_t=data.states.torque,
        mass=params.mass,
        gravity_vec=params.gravity_vec,
        J=params.J,
        J_inv=params.J_inv,
        acc_coef=params.acc_coef,
        cmd_f_coef=params.cmd_f_coef,
        rpy_coef=params.rpy_coef,
        rpy_rates_coef=params.rpy_rates_coef,
        cmd_rpy_coef=params.cmd_rpy_coef,
    )
    states_deriv = data.states_deriv.replace(
        vel=vel, ang_vel=data.states.ang_vel, acc=acc, ang_acc=ang_acc
    )
    return data.replace(states_deriv=states_deriv)

so_rpy_rotor_drag_physics(data)

Compute the forces and torques from the so_rpy_rotor_drag physics model.

Source code in crazyflow/sim/physics.py

def so_rpy_rotor_drag_physics(data: SimData) -> SimData:
    """Compute the forces and torques from the so_rpy_rotor_drag physics model."""
    params: SoRpyRotorDragData = data.params
    vel, _, acc, ang_acc, rotor_acc = so_rpy_rotor_drag_dynamics(
        pos=data.states.pos,
        quat=data.states.quat,
        vel=data.states.vel,
        ang_vel=data.states.ang_vel,
        cmd=data.controls.attitude.cmd,
        rotor_vel=data.states.rotor_vel,
        dist_f=data.states.force,
        dist_t=data.states.torque,
        mass=params.mass,
        gravity_vec=params.gravity_vec,
        J=params.J,
        J_inv=params.J_inv,
        thrust_time_coef=params.thrust_time_coef,
        acc_coef=params.acc_coef,
        cmd_f_coef=params.cmd_f_coef,
        rpy_coef=params.rpy_coef,
        rpy_rates_coef=params.rpy_rates_coef,
        cmd_rpy_coef=params.cmd_rpy_coef,
        drag_matrix=params.drag_matrix,
    )
    states_deriv = data.states_deriv.replace(
        vel=vel, ang_vel=data.states.ang_vel, acc=acc, ang_acc=ang_acc, rotor_acc=rotor_acc
    )
    return data.replace(states_deriv=states_deriv)

so_rpy_rotor_physics(data)

Compute the forces and torques from the so_rpy_rotor physics model.

Source code in crazyflow/sim/physics.py

def so_rpy_rotor_physics(data: SimData) -> SimData:
    """Compute the forces and torques from the so_rpy_rotor physics model."""
    params: SoRpyRotorData = data.params
    vel, _, acc, ang_acc, rotor_acc = so_rpy_rotor_dynamics(
        pos=data.states.pos,
        quat=data.states.quat,
        vel=data.states.vel,
        ang_vel=data.states.ang_vel,
        rotor_vel=data.states.rotor_vel,
        cmd=data.controls.attitude.cmd,
        dist_f=data.states.force,
        dist_t=data.states.torque,
        mass=params.mass,
        gravity_vec=params.gravity_vec,
        J=params.J,
        J_inv=params.J_inv,
        thrust_time_coef=params.thrust_time_coef,
        acc_coef=params.acc_coef,
        cmd_f_coef=params.cmd_f_coef,
        rpy_coef=params.rpy_coef,
        rpy_rates_coef=params.rpy_rates_coef,
        cmd_rpy_coef=params.cmd_rpy_coef,
    )
    states_deriv = data.states_deriv.replace(
        vel=vel, ang_vel=data.states.ang_vel, acc=acc, ang_acc=ang_acc, rotor_acc=rotor_acc
    )
    return data.replace(states_deriv=states_deriv)