Developer Guide

This page is for extending the stack while preserving the existing state machine and runtime behavior.

Where Things Live

Simulator package:

  • uvms-simulator/bringup/launch/robot_system_multi_interface.launch.py: main launch file and task selection.

  • uvms-simulator/msg and uvms-simulator/srv: simulator-owned dynamics, reset, and camera profile interfaces.

  • uvms-simulator/hardware: ros2_control hardware interfaces for real and simulated vehicle/manipulator.

  • uvms-simulator/bringup/config: launch-time controller-manager configuration.

  • uvms-simulator/description: xacro/URDF descriptions.

  • uvms-simulator/hardware/gstreamer_camera_*: GStreamer camera node and driver.

  • uvms-simulator/bringup/sim_camera_renderer.py: simulated camera renderer with PyVista and Open3D backends.

SimLab package:

  • uvms-simlab/simlab/interactive_control.py: RViz menu layout and UI callback glue.

  • uvms-simlab/simlab/uvms_backend.py: shared backend API used by RViz menus and external service clients.

  • uvms-simlab/msg, uvms-simlab/srv, and uvms-simlab/action: SimLab-owned reference, performance, backend command, and planner action interfaces.

  • uvms-simlab/simlab/robot.py: robot state machine, controller registry, planner/replay dispatch, joystick bridge, grasper handling, logging.

  • uvms-simlab/simlab/controllers: controller implementations, including CmdReplay.

  • uvms-simlab/simlab/utils: shared helper modules. geometry.py contains bounding-volume and marker geometry helpers, frames.py contains frame and pose conversion helpers, meshes.py contains URDF mesh/FCL/point-cloud helpers, interactive_markers.py contains RViz interactive-marker helpers, and path_obstacles.py contains active-path obstacle placement helpers.

  • uvms-simlab/resource/playback_profile: replay profiles.

  • uvms-simlab/resource/dynamics_profiles: whole-robot dynamics parameter profiles selected by replay/reset workflows.

  • uvms-simlab/resource/model_functions: generated CasADi/shared-library model functions grouped by arm, vehicle, and whole_body.

  • uvms-simlab/simlab/planner_action_server.py and planner_action_client.py: planner action server/client.

State Machine Rules

The main runtime modes are:

  • TELEOP: joystick/manual command publishing.

  • PLANNER: feedback control execution and planner trajectories.

  • REPLAY: CSV command replay.

  • REPLAY_SETTLE: hardware-only pre-replay controller settling.

Adding a Controller

Controller choices are class-based. They live under uvms-simlab/simlab/controllers and are registered through DEFAULT_CONTROLLER_CLASSES.

To add a controller:

  • Create a controller file, for example simlab/controllers/my_controller.py.

  • Inherit from ControllerTemplate.

  • Implement vehicle_controller(...) and arm_controller(...).

  • Add the class to DEFAULT_CONTROLLER_CLASSES in simlab/controllers/__init__.py.

The control loop calls the currently selected controller with the latest vehicle/manipulator state and target commands.

Semantics of built-in controllers:

  • PID combines feedback terms with hydrostatic restoring compensation on the vehicle path. The manipulator path is joint-space feedback with configured command limits.

  • InvDyn implements inverse-dynamics control in the computed-torque sense: desired state and desired acceleration are mapped through an estimated model to actuation, with feedback terms correcting model and state-estimation errors.

Example:

import numpy as np

from simlab.controllers.base import ControllerTemplate


class MyController(ControllerTemplate):
    registry_name = "MyController"

    def __init__(self, node, arm_dof=4, robot_prefix=""):
        super().__init__(node, arm_dof=arm_dof, robot_prefix=robot_prefix)
        self.vehicle_kp = np.array([8.0, 8.0, 10.0, 1.0, 1.0, 2.0])
        self.arm_kp = np.array([2.0, 2.0, 1.0, 1.0, 0.0])

    def vehicle_controller(
        self,
        state,
        target_pos,
        target_vel,
        target_acc,
        dt,
    ):
        state = self.vector(state, 12, "state")
        target_pos = self.vector(target_pos, 6, "target_pos")
        target_vel = self.vector(target_vel, 6, "target_vel")
        error = target_pos - state[:6]
        damping = target_vel - state[6:12]
        return self.vehicle_kp * error + 0.1 * damping

    def arm_controller(
        self,
        q,
        q_dot,
        q_ref,
        dq_ref,
        ddq_ref,
        dt,
    ):
        q = self.arm_vector(q, "q")
        q_dot = self.arm_vector(q_dot, "q_dot")
        q_ref = self.arm_vector(q_ref, "q_ref")
        dq_ref = self.arm_vector(dq_ref, "dq_ref")
        return self.arm_kp * (q_ref - q) + 0.05 * (dq_ref - q_dot)

Then register it:

from simlab.controllers.my_controller import MyController

DEFAULT_CONTROLLER_CLASSES = [
    LowLevelPidController,
    LowLevelInvDynController,
    CmdReplayController,
    MyController,
]

After rebuilding, MyController appears in the RViz controller menu.

Adding a Planner

Motion planning code lives under uvms-simlab/simlab/motion_planning. The current stack exposes separable planner, trajectory-generator, and dynamic replanner components, but that separation is not a hard algorithmic rule. An integrated motion-planning method such as GPMP or CHOMP can own path generation, trajectory timing, and dynamic replanning behind one implementation when that is the cleaner model.

All motion-planning algorithms should return a MotionPlanResult from simlab.motion_planning.result. The result declares which execution shape the algorithm produced:

  • MotionPlanKind.PATH: geometric waypoints. The selected trajectory generator, currently ruckig, time-parameterizes the path before the controller tracks it.

  • MotionPlanKind.TIMED_TRAJECTORY: already time-parameterized trajectory samples. This is the natural target for CHOMP/GPMP-style optimizers when they produce timing or derivatives directly.

  • MotionPlanKind.CONTROL_SEQUENCE: direct controls or short-horizon references. This is the natural target for MPC or integrated planner-controller methods.

The current PlanVehicle ROS action transports MotionPlanKind.PATH results for the existing OMPL/Ruckig pipeline. Timed trajectories and direct control-sequence execution need a richer execution transport before they can bypass the trajectory generator at runtime without losing timing, derivative, or control information. The plugin result contract already separates those algorithmic outputs from the split pipeline so CHOMP, GPMP, or MPC-style plugins have a clear target as that execution transport is added.

Planner choices are class-based. They live under uvms-simlab/simlab/motion_planning/planners. The RViz menu and planner action server both read from DEFAULT_PLANNER_CLASSES.

To add a planner:

  • Create a planner file, for example simlab/motion_planning/planners/my_planner.py.

  • Inherit from PlannerTemplate.

  • Add the class to DEFAULT_PLANNER_CLASSES in simlab/motion_planning/planners/__init__.py.

  • Return a MotionPlanResult. Planner plugins should not return raw result dictionaries.

Example:

import numpy as np

from simlab.motion_planning.planners.base import PlannerTemplate
from simlab.motion_planning.result import MotionPlanKind, MotionPlanResult


class MyPlanner(PlannerTemplate):
    registry_name = "MyPlanner"
    visible = True

    def plan_vehicle(
        self,
        *,
        start_xyz,
        start_quat_wxyz,
        goal_xyz,
        goal_quat_wxyz,
        time_limit,
        robot_collision_radius,
    ):
        xyz = np.asarray([start_xyz, goal_xyz], dtype=float)
        quat = np.asarray([start_quat_wxyz, goal_quat_wxyz], dtype=float)
        length = float(np.linalg.norm(xyz[-1] - xyz[0]))
        return MotionPlanResult(
            is_success=True,
            kind=MotionPlanKind.PATH,
            xyz=xyz,
            quat_wxyz=quat,
            path_length_cost=length,
            geom_length=length,
            message="MyPlanner returned a straight-line path.",
        )

Then register it:

from simlab.motion_planning.planners.my_planner import MyPlanner

DEFAULT_PLANNER_CLASSES = [
    RrtStarPlanner,
    BitStarPlanner,
    RrtConnectPlanner,
    MyPlanner,
]

Adding a Trajectory Generator

Vehicle trajectory generators are class-based plugins under uvms-simlab/simlab/motion_planning/trajectory_generators and are registered through DEFAULT_VEHICLE_TRAJECTORY_GENERATOR_CLASSES. The selected generator turns a planner path into the pose, velocity, and acceleration references tracked by the active controller.

To add a trajectory generator:

  • Create a file, for example simlab/motion_planning/trajectory_generators/my_generator.py.

  • Inherit from VehicleTrajectoryGeneratorTemplate.

  • Implement start_from_path(...), update(yaw_blend_factor), and close().

  • Add the class to DEFAULT_VEHICLE_TRAJECTORY_GENERATOR_CLASSES in simlab/motion_planning/trajectory_generators/__init__.py.

  • Select it with the vehicle_trajectory_generator ROS parameter.

Example selection:

ros2 run simlab interactive_controller --ros-args \
  -p vehicle_trajectory_generator:=ruckig

The default generator is ruckig.

Planner and trajectory runtime notes

Dynamic replanning is implemented as part of the motion-planning pipeline, not as a separate algorithm family. The active dynamic replanner samples the remaining trajectory against the shared dynamic-obstacle world. When clearance falls below the configured margin, it asks the selected planner for a replacement path and lets the selected trajectory generator build the new executable trajectory. The current trajectory remains active while this request is pending.

Vehicle path planning uses a spherical vehicle approximation and plans the translation path. Planner requests project vehicle start and goal quaternions to yaw-only before calling OMPL so transient roll or pitch during trajectory tracking does not make an otherwise valid live start state invalid. This projection affects only the planner request; it does not change the controller state.

Adding a Dynamics Backend or Robot Interface

Use this path when the robot itself changes: vehicle geometry, manipulator, thruster layout, actuator protocol, sensor suite, estimator, or dynamics model. A backend/interface change defines what state the rest of the stack can read and how commands reach the real or simulated actuators.

ROS 2 control interface basics:

  • A hardware interface is the boundary between ROS controllers and the robot or simulator.

  • State interfaces are values the robot exposes to ROS: vehicle pose, body velocity, body acceleration, joint position, joint velocity, joint acceleration, measured effort, and similar feedback.

  • Command interfaces are values ROS controllers write: vehicle wrench, thruster commands, joint effort commands, grasper commands, or other actuator inputs.

  • Simulated and real implementations should use the same interface names when they represent the same physical quantity. That is what allows the same controller and logger to run in simulation, hardware, or mixed setups.

State and command contract:

  • Decide the vehicle state convention: position/orientation frame, body velocity frame, acceleration frame, units, and sign convention.

  • Decide the manipulator convention: joint names, joint ordering, units, encoder signs, zero offsets, and grasper treatment.

  • Decide the command convention: vehicle wrench ordering, thruster command ordering, manipulator effort ordering, actuator limits, and saturation behavior.

  • Expose enough state for the rest of the stack to avoid guessing. If acceleration or measured effort is available, publish it as a state interface rather than making controllers or loggers reconstruct it.

Vehicle command allocation:

  • The high-level vehicle controller produces a 6-DOF command such as [Fx, Fy, Fz, Tx, Ty, Tz].

  • The vehicle interface must map that command to the actual thrusters for the specific vehicle.

  • The allocation must account for thruster position, thruster direction, command limits, deadband, PWM/force conversion, and actuator sign.

  • Validate allocation with single-axis tests: pure surge, pure heave, pure yaw, and zero command.

Manipulator command allocation:

  • Keep joint names and ordering explicit. For the current Alpha manipulator convention, the arm joints are axis_e, axis_d, axis_c, axis_b, and grasper axis_a.

  • Map controller effort commands to actuator commands using that explicit ordering.

  • Handle actuator limits, encoder signs, gear ratios, and zero offsets inside the robot interface.

  • If effort sensing is available, expose measured effort. Otherwise expose the best available estimate and make that clear in the interface documentation.

Sensor fusion and state estimation:

  • Decide where sensor fusion lives. Typical inputs are IMU, depth/pressure, DVL, odometry, and actuator feedback.

  • Publish one coherent estimated state to the rest of the stack instead of making each controller fuse sensors independently.

  • Keep frame conversion in the interface or estimator layer. Controllers should receive state in the convention they expect.

  • Use estimator/source timestamps when latency matters.

Dynamics model integration:

  • A simulator backend implements state-update logic: current state plus command produces the next state.

  • A hardware backend needs driver logic: read sensors, decode state, convert commands, and write actuator commands.

  • If a dynamics model is used, document the input ordering, output ordering, frames, units, and signs.

  • Connect vehicle model forces to thruster allocation and manipulator model efforts to joint/actuator mapping.

Where to change code:

  • Add or modify robot interfaces under uvms-simulator/hardware.

  • Add or modify xacro/URDF joints, transmissions, thrusters, sensors, and frames under uvms-simulator/description.

  • Add controller-manager configuration under uvms-simulator/bringup/config.

  • Add launch arguments and hardware selection wiring in uvms-simulator/bringup/launch/robot_system_multi_interface.launch.py.

  • Add SimLab-side readers, wrappers, or parameters under uvms-simlab/simlab only when the low-level state/command contract is clear.

Backend validation sequence:

  • Bring up ros2_control and verify the expected state and command interfaces exist.

  • Run open-loop single-axis actuator tests with conservative limits.

  • Verify frame conventions using known vehicle poses and known joint positions.

  • Verify state-estimation output with actuators disabled.

  • Test zero command, then low-amplitude command profiles, before enabling planner execution.