IMU-Driven 3-DOF Robot Simulation

A ROS 2 Jazzy + Gazebo simulation of a 3-DOF gantry robot racing along a track, driven entirely by live human motion — a pair of wireless torso IMUs, or the potentiometer-based TES rig — mapped in real time to the robot's X, Y, and rotational joints.

Technical Highlights

Interchangeable torso-signal inputs — live Bluetooth IMUs (via bleak) as the primary input, a potentiometer-based TES rig over serial as an earlier hardware revision, plus keyboard teleop and a scripted joint sweep for testing — all publishing to the same three joint-velocity topics, so swapping control methods is just swapping which node you launch.
3-DOF gantry robot — two prismatic joints (X/Y travel) and one revolute joint (yaw), driven by std_msgs/Float64 velocity commands bridged into Gazebo via ros_gz_bridge, with a POV camera and a top-down camera bridged back out as ROS 2 image topics.
Dynamic obstacles + collision handling — three obstacles oscillate across the track on a sine-wave trajectory, and a dedicated collision node zeroes all joint velocities on contact while tracking collision counts per track sector, with a cooldown so one hit isn't double-counted.
Sector timing & logging — the track is split into four sectors by track position; a timing node times the robot through each and logs sector times plus collision counts to a CSV once the robot reaches the finish sector.

The simulated robot — joint_controller_demo — is a 3-DOF gantry riding a rail: two prismatic joints (x_axis_joint, y_axis_joint) for forward/back and lateral travel, and one revolute joint (rotational_joint) for yaw. Each joint is driven by a std_msgs/Float64 velocity command published on its own topic, which ros_gz_bridge relays into Gazebo. A POV camera and a top-down camera are mounted on the robot and bridged back out to ROS 2 as image topics, so the sim can be watched from the robot's own perspective as well as from above.

What actually drives those three topics is interchangeable. The primary input is a pair of Bluetooth IMUs strapped to a rider's torso: imu_control.py connects over BLE, reads roll/pitch/yaw from notification packets, and publishes them — scaled — directly as the robot's X/Y/rotation commands, so the rider's own body orientation steers the gantry. A second, earlier hardware revision of the same idea reads three potentiometer values off an Arduino over USB serial instead of BLE. Keyboard teleop and a scripted joint sweep round out the input options, for testing without any hardware attached at all. Since every source publishes to the same three topics, only one runs at a time.

three_dof_robot/
├── launch/test.launch.py         # Main entry point: Gazebo + bridge + control nodes
├── model/
│   ├── bot/robot.sdf              # 3-DOF robot model (joints, cameras, contact sensor)
│   └── dynamic_object/*.sdf       # Moving obstacle models
├── worlds/
│   ├── test.world                 # Track world used by the default launch file
│   └── obstacle_course.world      # Alternate obstacle-course world
├── parameters/bridge_parameters.yaml  # ros_gz_bridge topic map (ROS <-> Gazebo)
└── src/
    ├── imu_control.py             # BLE IMU input (primary control source)
    ├── old_TES_serial_control.py  # Serial potentiometer input (legacy TES hardware)
    ├── keyboard_joint_control.py  # Manual teleop
    ├── test_joint_controllers.py  # Scripted joint sweep test
    ├── collision_handler_node.py  # Collision detection + e-stop
    ├── rotor_position.py          # Pose republisher
    ├── sector_timing.py           # Lap/sector timing + CSV logging
    ├── dynamic_obstacles_controller.py  # Moving obstacle driver
    └── globals.py                 # Shared sector/collision state, CSV path config

Package layout — reproduced from the repo's own README.

The track itself isn't static: three obstacles drift back and forth across it on a sine-wave trajectory, one phase-shifted 180° from the other two. A contact sensor on the robot feeds a dedicated collision node, which zeros all three joint velocities the instant it detects a hit — stopping the robot outright — and tallies collisions per track sector, with a cooldown so a single contact isn't counted twice. Track progress is measured the same way: the course is split into four sectors by track-position thresholds, and a timing node times how long the robot spends in each one, writing sector times and collision counts to a CSV once the robot crosses into the finish sector.

A few rough edges are still open, by design rather than oversight: joint-command scaling is a fixed divide-by-100 in both input scripts rather than a runtime-adjustable gain; the BLE IMU addresses and the CSV output paths are hard-coded to the specific hardware and machine this was developed on; and the sector-boundary thresholds are duplicated between the collision node and the timing node rather than defined once, so both need updating together if the track geometry changes. None of these block the simulation from running — they're the known list of what to clean up next.