udaan 1.0.1__py3-none-any.whl

udaan/__init__.py

@@ -0,0 +1,33 @@
+"""Udaan: Quadrotor dynamics and control for cable-suspended payload systems.
+
+Udaan provides geometric control on SE(3) × S² manifolds for quadrotor systems
+with cable-suspended payloads, supporting multiple physics backends.
+"""
+
+from __future__ import annotations
+
+import os
+from pathlib import Path
+
+# Package path for asset loading
+_FOLDER_PATH, _FILE_PATH = os.path.split(os.path.abspath(os.path.dirname(__file__)))
+PATH = _FOLDER_PATH
+PACKAGE_DIR = Path(__file__).parent
+
+# Core types and exceptions
+# Submodules
+from . import control, core, manif, models, utils
+
+# Version
+__version__ = "1.0.1"
+
+__all__ = [
+    "core",
+    "models",
+    "control",
+    "utils",
+    "manif",
+    "PATH",
+    "PACKAGE_DIR",
+    "__version__",
+]

udaan/__main__.py

@@ -0,0 +1,5 @@
+"""Allow running udaan as: python -m udaan"""
+
+from udaan.cli import app
+
+app()

udaan/cli/__init__.py

@@ -0,0 +1,41 @@
+"""Udaan command-line interface."""
+
+import typer
+
+_ctx = {"help_option_names": ["-h", "--help"]}
+
+app = typer.Typer(
+    name="udaan",
+    help="Simulation and geometric control of aerial manipulation systems.",
+    no_args_is_help=True,
+    context_settings=_ctx,
+)
+
+# --- Subcommand groups ---
+from .generate import generate_app  # noqa: E402
+from .run import run_app  # noqa: E402
+
+app.add_typer(run_app, name="run")
+app.add_typer(generate_app, name="generate-xml")
+
+
+# --- Top-level commands ---
+@app.command("version")
+def version():
+    """Show udaan version."""
+    from udaan import __version__
+
+    typer.echo(f"udaan {__version__}")
+
+
+def parse_vec(s: str | None, default):
+    """Parse 'x,y,z' string to numpy array, or return default."""
+    import numpy as np
+
+    if s is None:
+        return default
+    try:
+        return np.array([float(x) for x in s.split(",")])
+    except ValueError:
+        typer.echo(f"Invalid position format: '{s}'. Expected 'x,y,z'.", err=True)
+        raise typer.Exit(1) from None

udaan/cli/demos.py

@@ -0,0 +1,79 @@
+"""Fleet demo presets.
+
+Each demo has:
+- config(): returns {"num_quadrotors": N, "disturbances": {...}}
+- configure(fleet): sets up controllers on the fleet
+"""
+
+import numpy as np
+
+
+def _l1_comparison_config():
+    return {
+        "num_quadrotors": 3,
+        "disturbances": {
+            1: {"mass": 0.25, "offset": [0.2, 0.2, -0.05]},
+            2: {"mass": 0.25, "offset": [0.2, 0.2, -0.05]},
+        },
+    }
+
+
+def _l1_comparison_configure(fleet):
+    """Nominal PD vs full L1 (position + attitude) with disturbance."""
+    from udaan.control.quadrotor import PositionL1Controller
+    from udaan.control.quadrotor.geometric_l1_attitude import GeometricL1AttitudeController
+
+    traj = lambda t: (np.array([0.0, 0.0, 1.0]), np.zeros(3), np.zeros(3))
+
+    # quad 0 (red): PD + GeomAtt, NO disturbance — reference baseline
+    fleet[0].position_controller.setpoint = traj
+    fleet._labels[0] = "PD+Geom (clean)"
+
+    # quad 1 (blue): PD + GeomAtt, disturbed — shows drift
+    fleet[1].position_controller.setpoint = traj
+    fleet._labels[1] = "PD+Geom (disturbed)"
+
+    # quad 2 (green): PD_L1 + GeomL1Att, disturbed — full L1
+    m = fleet[2].mass
+    fleet[2].position_controller = PositionL1Controller(mass=m, setpoint=traj)
+    fleet[2].attitude_controller = GeometricL1AttitudeController(mass=m, inertia=fleet[2].inertia)
+    fleet._labels[2] = "L1+GeomL1 (disturbed)"
+
+
+def _gain_sweep_config():
+    return {
+        "num_quadrotors": 4,
+        "disturbances": {},
+    }
+
+
+def _gain_sweep_configure(fleet):
+    """PD with increasing gains (soft -> aggressive)."""
+    from udaan.control.quadrotor import PositionPDController
+
+    traj = lambda t: (np.array([0.0, 0.0, 2.0]), np.zeros(3), np.zeros(3))
+    gain_scales = [0.25, 0.5, 1.0, 2.0]
+    labels = ["very soft", "soft", "nominal", "aggressive"]
+
+    for i, (scale, label) in enumerate(zip(gain_scales, labels, strict=True)):
+        fleet[i].position_controller = PositionPDController(
+            mass=fleet[i].mass,
+            setpoint=traj,
+            kp=scale * np.array([4.0, 4.0, 8.0]),
+            kd=scale * np.array([3.0, 3.0, 6.0]),
+        )
+        fleet._labels[i] = f"PD ({label})"
+
+
+DEMOS = {
+    "l1-comparison": {
+        "config": _l1_comparison_config,
+        "configure": _l1_comparison_configure,
+        "description": "PD (clean) vs PD (disturbed) vs L1 (disturbed)",
+    },
+    "gain-sweep": {
+        "config": _gain_sweep_config,
+        "configure": _gain_sweep_configure,
+        "description": "PD with increasing gains (soft -> aggressive)",
+    },
+}

udaan/cli/generate.py

@@ -0,0 +1,32 @@
+"""MuJoCo XML generation commands."""
+
+import typer
+
+from . import _ctx
+
+generate_app = typer.Typer(help="Generate MuJoCo XML model files.", context_settings=_ctx)
+
+
+@generate_app.command("multi-quad")
+def multi_quad(
+    num_quads: int = typer.Option(3, "--num-quads", "-n", help="Number of quadrotors."),
+    output: str = typer.Option("multi_quad_pointmass.xml", "--output", "-o", help="Output file."),
+    verbose: bool = typer.Option(False, "--verbose", "-v"),
+):
+    """Generate MJCF XML for multi-quadrotor pointmass payload."""
+    import udaan as U
+
+    U.utils.xml_model_generator.multi_quad_pointmass(nQ=num_quads, filename=output, verbose=verbose)
+    typer.echo(f"Written to {output}")
+
+
+@generate_app.command("comparison")
+def comparison(
+    output: str = typer.Option("quadrotor_comparison.xml", "--output", "-o", help="Output file."),
+    verbose: bool = typer.Option(False, "--verbose", "-v"),
+):
+    """Generate MJCF XML for quadrotor comparison model."""
+    import udaan as U
+
+    U.utils.xml_model_generator.quadrotor_comparison(filename=output, verbose=verbose)
+    typer.echo(f"Written to {output}")

udaan/cli/run.py

@@ -0,0 +1,199 @@
+"""Simulation run commands."""
+
+import typer
+
+from . import _ctx
+
+run_app = typer.Typer(help="Run a simulation.", context_settings=_ctx)
+
+# Common option for recording
+_record_option = typer.Option(None, "--record", "-r", help="Save recording to file (.gif or .mp4).")
+
+
+def _hold_viewer(mdl):
+    """Keep MuJoCo viewer open after simulation if it has one."""
+    if hasattr(mdl, "_mjMdl") and mdl._mjMdl is not None:
+        mdl._mjMdl.wait_for_close()
+
+
+def _setup_recording(mdl, record_path):
+    """Enable recording on a model's viewer."""
+    if record_path is None:
+        return
+    mj = mdl._mjMdl if hasattr(mdl, "_mjMdl") else None
+    if mj and mj._viewer:
+        mj._viewer._record_path = record_path
+        mj._viewer._frames = []
+
+
+@run_app.command("quadrotor")
+def quadrotor(
+    time: float = typer.Option(10.0, "--time", "-t", help="Simulation duration in seconds."),
+    render: bool = typer.Option(True, help="Enable visualization."),
+    backend: str = typer.Option(
+        "mujoco", "--backend", "-b", help="Physics backend: mujoco or base."
+    ),
+    record: str | None = _record_option,
+    position: str | None = typer.Option(
+        None, "--position", "-p", help="Initial position as 'x,y,z'."
+    ),
+):
+    """Simulate a quadrotor with geometric SE(3) control."""
+    import numpy as np
+
+    import udaan as U
+
+    from . import parse_vec
+
+    x0 = parse_vec(position, default=np.array([1.0, 1.0, 0.0]))
+    mdl_module = getattr(U.models, backend)
+    mdl = mdl_module.Quadrotor(render=render)
+    _setup_recording(mdl, record)
+    typer.echo(f"Running quadrotor ({backend}) for {time}s ...")
+    mdl.simulate(tf=time, position=x0)
+    _hold_viewer(mdl)
+
+
+@run_app.command("quad-payload")
+def quad_payload(
+    time: float = typer.Option(10.0, "--time", "-t", help="Simulation duration in seconds."),
+    render: bool = typer.Option(True, help="Enable visualization."),
+    backend: str = typer.Option(
+        "mujoco", "--backend", "-b", help="Physics backend: mujoco or base."
+    ),
+    model_type: str = typer.Option(
+        "tendon", "--model-type", "-m", help="Cable model: tendon, links, or cable."
+    ),
+    record: str | None = _record_option,
+    position: str | None = typer.Option(
+        None, "--position", "-p", help="Initial payload position as 'x,y,z'."
+    ),
+):
+    """Simulate a quadrotor with cable-suspended payload."""
+    import numpy as np
+
+    import udaan as U
+
+    from . import parse_vec
+
+    x0 = parse_vec(position, default=np.array([-1.0, 2.0, 0.5]))
+    mdl_module = getattr(U.models, backend)
+    if backend == "mujoco":
+        mdl = mdl_module.QuadrotorCSPayload(render=render, model=model_type)
+    else:
+        mdl = mdl_module.QuadrotorCSPayload(render=render)
+    _setup_recording(mdl, record)
+    typer.echo(f"Running quad-payload ({backend}, {model_type}) for {time}s ...")
+    mdl.simulate(tf=time, payload_position=x0)
+    _hold_viewer(mdl)
+
+
+@run_app.command("multi-quad")
+def multi_quad(
+    time: float = typer.Option(10.0, "--time", "-t", help="Simulation duration in seconds."),
+    render: bool = typer.Option(True, help="Enable visualization."),
+    num_quads: int = typer.Option(3, "--num-quads", "-n", help="Number of quadrotors."),
+    record: str | None = _record_option,
+    position: str | None = typer.Option(
+        None, "--position", "-p", help="Initial payload position as 'x,y,z'."
+    ),
+):
+    """Simulate multiple quadrotors with cable-suspended pointmass payload."""
+    import numpy as np
+
+    import udaan as U
+
+    from . import parse_vec
+
+    x0 = parse_vec(position, default=np.array([-1.0, 2.0, 0.5]))
+    mdl = U.models.mujoco.MultiQuadrotorCSPointmass(render=render, num_quadrotors=num_quads)
+    _setup_recording(mdl, record)
+    typer.echo(f"Running multi-quad ({num_quads} quads) for {time}s ...")
+    mdl.simulate(tf=time, xL=x0)
+    _hold_viewer(mdl)
+
+
+@run_app.command("multi-quad-rigid")
+def multi_quad_rigid(
+    time: float = typer.Option(10.0, "--time", "-t", help="Simulation duration in seconds."),
+    render: bool = typer.Option(True, help="Enable visualization."),
+    record: str | None = _record_option,
+    position: str | None = typer.Option(
+        None, "--position", "-p", help="Initial payload position as 'x,y,z'."
+    ),
+):
+    """Simulate multiple quadrotors with cable-suspended rigid-body payload."""
+    import numpy as np
+
+    import udaan as U
+
+    from . import parse_vec
+
+    x0 = parse_vec(position, default=np.array([0.0, 0.0, 0.5]))
+    mdl = U.models.mujoco.MultiQuadRigidbody(render=render)
+    _setup_recording(mdl, record)
+    typer.echo(f"Running multi-quad rigid ({mdl.nQ} quads) for {time}s ...")
+    mdl.simulate(tf=time, xL=x0)
+    _hold_viewer(mdl)
+
+
+@run_app.command("fleet")
+def fleet(
+    time: float = typer.Option(10.0, "--time", "-t", help="Simulation duration in seconds."),
+    render: bool = typer.Option(True, help="Enable visualization."),
+    num_quads: int = typer.Option(3, "--num-quads", "-n", help="Number of quadrotors."),
+    demo: str | None = typer.Option(None, "--demo", "-d", help="Run a preset demo."),
+    verbose: bool = typer.Option(False, "--verbose", "-v", help="Log quad states during sim."),
+    record: str | None = _record_option,
+    position: str | None = typer.Option(
+        None, "--position", "-p", help="Initial position as 'x,y,z'."
+    ),
+):
+    """Compare N quadrotors with independent controllers side-by-side.
+
+    Available demos: l1-comparison, gain-sweep
+    """
+    import logging
+
+    import numpy as np
+
+    import udaan as U
+
+    if verbose:
+        logging.basicConfig(level=logging.DEBUG, format="%(message)s")
+
+    from . import parse_vec
+    from .demos import DEMOS
+
+    x0 = parse_vec(position, default=np.array([0.0, 0.0, 1.0]))
+
+    if demo is not None:
+        if demo not in DEMOS:
+            typer.echo(f"Unknown demo: '{demo}'. Available: {', '.join(DEMOS.keys())}", err=True)
+            raise typer.Exit(1)
+
+        entry = DEMOS[demo]
+        params = entry["config"]()
+        nQ = params["num_quadrotors"]
+
+        f = U.models.mujoco.QuadrotorFleet(
+            num_quadrotors=nQ, render=render, disturbances=params["disturbances"]
+        )
+        entry["configure"](f)
+        if record and f._mjMdl._viewer:
+            f._mjMdl._viewer._record_path = record
+            f._mjMdl._viewer._frames = []
+        typer.echo(f"Running demo '{demo}' ({nQ} quads) for {time}s ...")
+        f.simulate(tf=time, position=x0)
+    else:
+        f = U.models.mujoco.QuadrotorFleet(num_quadrotors=num_quads, render=render)
+
+        traj = lambda t: (np.array([0.0, 0.0, 2.0]), np.zeros(3), np.zeros(3))
+        for q in f.quadrotors:
+            q.position_controller.setpoint = traj
+
+        if record and f._mjMdl._viewer:
+            f._mjMdl._viewer._record_path = record
+            f._mjMdl._viewer._frames = []
+        typer.echo(f"Running fleet ({num_quads} quads) for {time}s ...")
+        f.simulate(tf=time, position=x0)

udaan/control/__init__.py

@@ -0,0 +1,66 @@
+import numpy as np
+
+
+class Gains:
+    def __init__(self, kp=np.zeros(3), kd=np.zeros(3), ki=np.zeros(3)):
+        self.kp = kp
+        self.kd = kd
+        self.ki = ki
+        return
+
+
+class Controller:
+    def __init__(self):
+        self.freq = 500.0
+        self._g = 9.81
+        self._ge3 = np.array([0.0, 0.0, self._g])
+        self._e1 = np.array([1.0, 0.0, 0.0])
+        self._e2 = np.array([0.0, 1.0, 0.0])
+        self._e3 = np.array([0.0, 0.0, 1.0])
+        self._gravity = np.array([0.0, 0.0, -self._g])
+        return
+
+    def compute(self, *args):
+        raise NotImplementedError
+
+
+class PDController(Controller):
+    def __init__(self, **kwargs):
+        super().__init__()
+        self._gains = Gains()
+
+        if "kp" in kwargs:
+            self._gains.kp = kwargs["kp"]
+        if "kd" in kwargs:
+            self._gains.kd = kwargs["kd"]
+
+        if "setpoint" in kwargs:
+            self.setpoint = kwargs["setpoint"]
+        else:
+            self.setpoint = lambda t: (
+                np.array([0.0, 0.0, 1.0]),
+                np.zeros(3),
+                np.zeros(3),
+            )
+        return
+
+    def compute(self, *args):
+        t = args[0]
+        s, ds = args[1][0], args[1][1]
+        sd, dsd, d2sd = self.setpoint(t)
+        e = s - sd
+        de = ds - dsd
+        u = -self._gains.kp * e - self._gains.kd * de + d2sd
+        return u
+
+
+from . import quadrotor as quadrotor  # noqa: E402
+from . import quadrotor_cspayload as quadrotor_cspayload  # noqa: E402
+
+__all__ = [
+    "Gains",
+    "Controller",
+    "PDController",
+    "quadrotor",
+    "quadrotor_cspayload",
+]

udaan/control/quadrotor/__init__.py

@@ -0,0 +1,15 @@
+"""Quadrotor controllers."""
+
+from .geometric_attitude import GeometricAttitudeController as GeometricAttitudeController
+from .geometric_l1_attitude import GeometricL1AttitudeController as GeometricL1AttitudeController
+from .position_l1 import PositionL1Controller as PositionL1Controller
+from .position_pd import PositionPDController as PositionPDController
+from .propeller_allocation import DirectPropellerForceController as DirectPropellerForceController
+
+__all__ = [
+    "PositionPDController",
+    "GeometricAttitudeController",
+    "DirectPropellerForceController",
+    "PositionL1Controller",
+    "GeometricL1AttitudeController",
+]

udaan/control/quadrotor/geometric_attitude.py

@@ -0,0 +1,80 @@
+"""Geometric attitude controller on SE(3) for quadrotor.
+
+Lee, Leok, McClamroch (2010) https://ieeexplore.ieee.org/document/5717652
+"""
+
+import numpy as np
+
+from ...control import Controller, Gains
+from ...utils import hat, printc_fail
+
+
+class GeometricAttitudeController(Controller):
+    """Geometric tracking control of a quadrotor UAV on SE(3)."""
+
+    def __init__(self, **kwargs):
+        self._gains = Gains(kp=np.array([2.4, 2.4, 1.35]), kd=np.array([0.35, 0.35, 0.225]))
+
+        self._inertia = np.eye(3)
+        self._inertia_inv = np.eye(3)
+        self.mass = 1.0
+
+        if "inertia" in kwargs:
+            self.inertia = kwargs["inertia"]
+        else:
+            printc_fail("Inertia not provided")
+        if "mass" in kwargs:
+            self.mass = kwargs["mass"]
+
+    @property
+    def inertia(self):
+        return self._inertia
+
+    @inertia.setter
+    def inertia(self, inertia):
+        self._inertia = inertia
+        self._inertia_inv = np.linalg.inv(inertia)
+
+    def _cmd_accel_to_cmd_att(self, accel):
+        """Convert desired acceleration to desired attitude.
+
+        Returns identity for near-zero acceleration to avoid singularity.
+        """
+        MIN_ACCEL_NORM = 0.001
+
+        norm_accel = np.linalg.norm(accel)
+        if norm_accel < MIN_ACCEL_NORM:
+            return np.eye(3)
+
+        b3 = accel / norm_accel
+
+        b1d = np.array([1.0, 0.0, 0.0])
+        b3_b1d = np.cross(b3, b1d)
+        norm_b3_b1d = np.linalg.norm(b3_b1d)
+
+        if norm_b3_b1d < 1e-6:
+            b1d = np.array([0.0, 1.0, 0.0])
+            b3_b1d = np.cross(b3, b1d)
+            norm_b3_b1d = np.linalg.norm(b3_b1d)
+
+        b1 = (-1 / norm_b3_b1d) * np.cross(b3, b3_b1d)
+        b2 = np.cross(b3, b1)
+
+        Rd = np.column_stack([b1, b2, b3])
+        return Rd
+
+    def compute(self, *args):
+        t = args[0]
+        R, Omega = args[1][0], args[1][1]
+        thrust_force = args[2]
+        Rd = self._cmd_accel_to_cmd_att(thrust_force)
+        Omegad = np.zeros(3)
+        dOmegad = np.zeros(3)
+
+        tmp = 0.5 * (Rd.T @ R - R.T @ Rd)
+        eR = np.array([tmp[2, 1], tmp[0, 2], tmp[1, 0]])  # vee-map
+        eOmega = Omega - R.T @ Rd @ Omegad
+        M = -self._gains.kp * eR - self._gains.kd * eOmega + np.cross(Omega, self.inertia @ Omega)
+        M += -1 * self.inertia @ (hat(Omega) @ R.T @ Rd @ Omegad - R.T @ Rd @ dOmegad)
+        f = thrust_force.dot(R[:, 2])
+        return f, M