Engineer2026 Control

Control stack for the Illini RoboMaster Engineer robot (2026 cycle).

This repository contains ROS 2 Humble packages for a 6-DOF robotic arm: URDF description, ros2_control setup, MoveIt2 motion planning, and teleoperation via AprilTag cube or keyboard. It targets an OrangePi SBC communicating with an MCU (ESP32/STM32) over UART.

The arm is teleoperated using an AprilTag cube held by the operator. A standalone Python client (arm_vision/) handles all camera work — calibration, AprilTag detection, and pose estimation — and streams the target end-effector pose to the ROS stack over a plain TCP socket. No ROS is required on the client machine.

System Architecture

┌──────────────────────────────────────────────────────────────────┐
│  arm_vision/  (standalone Python — no ROS)                       │
│                                                                  │
│  webcam → AprilTag detection → cube pose → workspace mapping     │
│                                         ↓                        │
│                            6D EE target pose (JSON over TCP)     │
└─────────────────────────────┬────────────────────────────────────┘
                              │ TCP socket (default port 9999)
┌─────────────────────────────▼────────────────────────────────────┐
│  ROS 2 Humble  (runs on OrangePi)                                │
│                                                                  │
│  socket_teleop_node   receives target pose, P-controller         │
│  keyboard_teleop_node ──► /servo_node/delta_twist_cmds           │
│                           /servo_node/delta_joint_cmds           │
│  servo_node (MoveIt2) ──► /arm_controller/joint_trajectory       │
│  move_group                                                      │
│  ros2_control         ──► mock_components  OR  uart_bridge_node → MCU   │
└──────────────────────────────────────────────────────────────────┘

Package Structure

Path	Description
`robotic_arm_v4_urdf/`	Robot URDF, ros2_control config, MoveIt2 config, core launch files
`arm_teleop/`	`socket_teleop_node` (TCP receiver + P-controller) and `keyboard_teleop_node`
`arm_hardware/`	UART bridge node, homing node — communicate with the MCU over UART
`arm_bringup/`	Top-level launch that ties all ROS packages together
`arm_vision/`	Standalone Python client: camera cal, cube cal, detection, socket sender
`scripts/`	Shell helpers for setup, build, and launch

Prerequisites

OS: Ubuntu 22.04 (Jammy) — ROS 2 Humble must already be installed.

Install all ROS and Python dependencies:

bash scripts/setup_ros2_humble_deps.sh

Install standalone client dependencies (no ROS needed):

cd arm_vision
pip install -r requirements.txt

Build (ROS)

source /opt/ros/humble/setup.bash
colcon build --base-paths robotic_arm_v4_urdf arm_teleop arm_hardware arm_bringup
source install/setup.bash

Or use the helper script (builds then launches):

bash scripts/run_ros2_humble_moveit_control.sh

Running

1 — Start the ROS stack

Simulation only — Plan+Execute via RViz (no hardware, no vision):

source install/setup.bash
ros2 launch arm_bringup arm_bringup.launch.py use_teleop:=false

Real robot — Plan+Execute via RViz (no vision pipeline):

ros2 launch arm_bringup arm_bringup.launch.py use_real_robot:=true use_teleop:=false uart_port:=/dev/yourdevice

Real robot + arm_vision socket teleop (full system):

ros2 launch arm_bringup arm_bringup.launch.py use_real_robot:=true uart_port:=/dev/yourdevice

Headless (no RViz):

ros2 launch arm_bringup arm_bringup.launch.py use_moveit_rviz:=false use_teleop:=false

2 — Start the arm_vision client (separate machine or terminal)

cd arm_vision
python main.py run --host <ros-host-ip> --show

The client opens the webcam, detects the AprilTag cube, maps its pose to a target EE position, and streams it to the ROS socket_teleop_node at 30 Hz.

3 — Homing (real robot only)

Homing moves each joint to 0 rad in a collision-aware order. The group sequence is chosen dynamically based on Joint2's current angle at startup:

J2 angle	Sequence	Rationale
J2 ≥ 0° (forward)	`[J2] → [J3,J4,J5,J6] → [J1]`	Retract shoulder first to clear space
J2 < 0° (back)	`[J3] → [J2] → [J4,J5,J6] → [J1]`	Fold elbow first to avoid body collision

Run automatically at startup by adding run_homing:=true to the bringup:

ros2 launch arm_bringup arm_bringup.launch.py use_real_robot:=true uart_port:=/dev/yourdevice run_homing:=true

Or run it as a standalone node while the stack is already running:

source install/setup.bash
ros2 run arm_hardware homing_node

Key parameters (override with --ros-args -p name:=value):

Parameter	Default	Description
`joint_speed_rad_s`	`0.3`	Max speed used to compute each step duration
`min_duration_s`	`2.0`	Minimum time allocated per joint step
`settle_time_s`	`0.5`	Extra settle wait after each step

4 — Joint Monitor (observe live joint angles)

The joint monitor prints joint angles to the terminal while the arm moves or a MoveIt plan is computed. It is a lightweight diagnostic tool — no effect on motion.

Recommended: dedicated terminal (cleanest output)

bash scripts/run_joint_monitor.sh

Or inline via bringup:

ros2 launch arm_bringup arm_bringup.launch.py print_joints:=true

Example output:

[CURRENT]   Joint1=  +0.00°  Joint2= -45.23°  Joint3= +12.10°  Joint4=  +0.00°  Joint5=  +5.50°  Joint6=  +0.00°
[PLAN GOAL] Joint1=  +0.00°  Joint2=  +0.00°  Joint3=  +0.00°  Joint4=  +0.00°  Joint5=  +0.00°  Joint6=  +0.00°

[CURRENT] — updates in-place at each /joint_states tick (live arm position).
[PLAN GOAL] — printed whenever MoveIt computes a plan (drag the interactive marker in RViz and click Plan).

5 — View-only mode (real arm angles in RViz, no motion commands)

Shows live encoder feedback from the MCU as a moving robot model in RViz. No motion commands are ever sent to the hardware in this mode.

bash scripts/run_view_only.sh --port /dev/yourdevice
# with raw-frame debug logging:
bash scripts/run_view_only.sh --port /dev/yourdevice --debug

If --port is not given the script prints available /dev/tty* devices and exits.

This starts only: robot_state_publisher + uart_bridge_node (RX only) + RViz(display.rviz).

6 — Keyboard teleoperation (optional, separate terminal)

source install/setup.bash
ros2 run arm_teleop keyboard_teleop_node

Or:

bash scripts/run_keyboard_teleop.sh

7 — MCU Emulator (test without real hardware)

The emulator creates a virtual serial port pair so you can run and test the full ROS stack — homing, Plan+Execute, and vision teleop — without the real robot connected. It simulates the MCU's motor PID: positions approach the commanded angles with a configurable time constant.

Terminal 1 — start the emulator:

# All joints at zero (default):
python3 scripts/mcu_emulator.py

# Start at non-zero positions to test homing:
python3 scripts/mcu_emulator.py --start 30,-20,15,-10,50,5

# Slower motor response (default tau=0.2s):
python3 scripts/mcu_emulator.py --start 30,-20,15,-10,50,5 --tau 0.5

The emulator prints the virtual serial port path:

╔══════════════════════════════════════════════════════════════╗
║  MCU Emulator Ready                                        ║
║  Connect the bridge with:                                   ║
║    uart_port:=/dev/pts/7                                   ║
╚══════════════════════════════════════════════════════════════╝

Terminal 2 — launch the full ROS stack with that port:

# Plan+Execute from RViz only (no vision):
ros2 launch arm_bringup arm_bringup.launch.py \
  use_real_robot:=true uart_port:=/dev/pts/7 use_teleop:=false

# With homing on startup:
ros2 launch arm_bringup arm_bringup.launch.py \
  use_real_robot:=true uart_port:=/dev/pts/7 use_teleop:=false run_homing:=true

# Full pipeline — vision teleop → MoveIt → emulated arm:
ros2 launch arm_bringup arm_bringup.launch.py \
  use_real_robot:=true uart_port:=/dev/pts/7
# then in a third terminal:
python arm_vision/main.py run --host localhost --show

Emulator output legend:

Color	Tag	Meaning
Cyan	`[MCU RX TRAJ]`	Command frame received from bridge during a trajectory
Cyan	`[MCU RX HOLD]`	Command frame received during idle-hold
Yellow	`[MCU TX MOVE]`	Encoder feedback sent back — motors still converging
Yellow	`[MCU TX HOLD]`	Encoder feedback sent back — motors at target
Red	`[MCU WATCHDOG]`	No command received within timeout — TX gap detected

Emulator options:

Option	Default	Description
`--start J1,J2,J3,J4,J5,J6`	`0,0,0,0,0,0`	Initial motor positions in degrees
`--rate Hz`	`20`	Feedback send rate (should match `send_rate_hz`)
`--tau s`	`0.2`	Motor time constant — how fast position approaches target
`--watchdog s`	`1.0`	Warn if no command received for this long
`--quiet`	off	Only print changes > 0.5° and warnings

8 — Pipeline test (mock arm_vision sender)

Sends fake target poses over TCP without a camera, to test the full post-vision pipeline:

# Interactive mode — type poses manually:
python scripts/test_pose_sender.py

# Demo mode — loops through a preset waypoint sequence:
python scripts/test_pose_sender.py --demo

# Hold a fixed pose:
python scripts/test_pose_sender.py --hold 0.3 0.0 0.2

Calibration Workflow

All calibration is done from the arm_vision/ directory, with no ROS running.

Step 1 — Camera calibration

Print a checkerboard (default 9×6 inner corners, 25 mm squares). Show it from multiple angles while pressing SPACE to capture frames. Press Q when done (≥10 captures recommended).

cd arm_vision
python main.py calibrate-camera

Output: arm_vision/data/camera_calibration.yaml

Step 2 — Cube calibration (5 faces)

For each of the 5 AprilTag faces on the cube:

Point that face toward the camera.
Press SPACE to capture.
Enter the face label when prompted (+X, -X, +Y, -Y, +Z).

python main.py calibrate-cube --side-length 0.06

Output: arm_vision/data/cube_config.yaml

Step 3 — Workspace mapping

Edit arm_vision/config/workspace.yaml to match your physical setup:

Field	Description
`cam_origin`	Neutral cube position in camera frame (m) — where the arm should sit at rest
`ee_origin`	EE position in `base_link` at neutral — read from `/tf` at home pose
`scale`	Amplification: how much robot moves per unit of cube movement
`cam_to_robot`	3×3 rotation matrix mapping camera axes → robot base_link axes

Launch Arguments (`arm_bringup.launch.py`)

Argument	Default	Description
`use_real_robot`	`false`	`true` to start the UART bridge to the MCU
`uart_port`	(required when `use_real_robot:=true`)	Serial device for the MCU connection, e.g. `/dev/ttyUSB0`
`baud_rate`	`115200`	UART baud rate
`use_teleop`	`true`	`false` to skip socket_teleop and use MoveIt Plan+Execute directly
`socket_host`	`0.0.0.0`	TCP bind address for the pose socket
`socket_port`	`9999`	TCP bind port for the pose socket
`use_moveit_rviz`	`true`	Show MoveIt2 RViz panel
`run_homing`	`false`	Run sequential homing on startup (real robot only)
`print_joints`	`false`	Print live joint angles + MoveIt plan goals to terminal

arm_vision CLI Reference

python main.py calibrate-camera  [--device N] [--cols 9] [--rows 6]
                                  [--square-size 0.025] [--output FILE]

python main.py calibrate-cube    [--device N] [--tag-family tag36h11]
                                  [--side-length 0.06] [--camera-cal FILE]
                                  [--output FILE]

python main.py run               [--device N] [--host IP] [--port 9999]
                                  [--camera-cal FILE] [--cube-config FILE]
                                  [--workspace-config FILE] [--show]

UART Protocol & Joint Mapping

Physical layer

Device: configured via uart_port:=/dev/yourdevice at launch time (or set port in hardware_params.yaml)
Baud rate: 115200, 8N1

To find the correct device:

ls /dev/ttyUSB* /dev/ttyACM* /dev/ttyCH341* 2>/dev/null
# plug in the USB-UART cable, then run again and note what appeared

Frame format

Fixed 16-byte binary frame, same structure in both directions:

Offset  Bytes  Field
──────  ─────  ──────────────────────────────────────────────
0       1      SOF = 0xA5  (sync sentinel)
1       1      LEN = 0x0C  (payload byte count = 12)
2–13    12     6 × int16_t, little-endian, centidegrees (1/100 °)
               order: J1 J2 J3 J4 J5 J6
14–15   2      CRC16-MODBUS (poly 0xA001, init 0xFFFF) over bytes [0..13]
               little-endian (lo byte first)
──────  ─────  ──────────────────────────────────────────────
Total   16 bytes

Values are in motor degrees (after sign-flip and gear-ratio). Resolution is 0.01°, range ±327.67°. Frames failing SOF, LEN, or CRC checks are dropped with a [WARN] log; the bridge re-syncs by scanning for the next 0xA5 byte.

Suggested MCU DMA setup (STM32 HAL):

HAL_UART_Receive_DMA(&huart, rx_buf, 16);  // fires once per complete frame

Joint sign & gear ratio

The MCU reports raw motor angles. uart_bridge_node converts to/from URDF joint angles using two per-joint parameters in arm_hardware/config/hardware_params.yaml:

TX: motor_angle = sign × joint_angle × gear_ratio
RX: joint_angle = sign × motor_angle ÷ gear_ratio

Parameter	Default	Description
`joint_sign_flip`	`[-1, -1, 1, -1, -1, 1]`	`+1` = same direction as URDF, `-1` = flip
`joint_gear_ratio`	`[2, 1, 1, 1, 1, 1]`	Motor turns per joint turn (J1 has 2:1 reduction)

Order for both lists: [Joint1, Joint2, Joint3, Joint4, Joint5, Joint6].

To tune these values: run view-only mode, move each joint by hand, and adjust until the RViz model matches the physical arm.

`hardware_params.yaml` key parameters

Parameter	Default	Description
`send_rate_hz`	`20.0`	TX rate to MCU (Hz) — keep ≤ 20 Hz to avoid MCU DMA overrun
`command_timeout_s`	`0.5`	Idle-hold refresh interval — re-TX last position to keep MCU watchdog alive
`override_joint_states`	`false`	Set `true` (real robot) to publish MCU encoder feedback as `/joint_states`
`debug_rx`	`false`	Log every raw RX frame and parsed values to the ROS terminal
`debug_tx`	`false`	Log every TX frame with source tag (`TRAJ` or `HOLD`) and frame counter
`servo_hold_after_traj_s`	`2.0`	Block servo topic messages for this long after a trajectory ends

Hardware

SBC: OrangePi (RK3588, ARM64)
MCU: ESP32/STM32 connected via USB-UART dongle (pass device path as uart_port:=...)
Camera: USB webcam (any OpenCV-compatible device)
Arm: 6-DOF (Joint1 – Joint6)

Serial port access: add your user to the dialout group if the device permission is denied:
sudo usermod -aG dialout $USER   # log out and back in to apply
sudo systemctl stop ModemManager  # prevents ModemManager hijacking ttyACM* devices

Name		Name	Last commit message	Last commit date
Latest commit History 26 Commits
arm_bringup		arm_bringup
arm_hardware		arm_hardware
arm_teleop		arm_teleop
arm_vision		arm_vision
robotic_arm_v4_urdf		robotic_arm_v4_urdf
scripts		scripts
.gitignore		.gitignore
AGENTS.md		AGENTS.md
CLAUDE.md		CLAUDE.md
README.md		README.md

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Repository files navigation

Engineer2026 Control

System Architecture

Package Structure

Prerequisites

Build (ROS)

Running

1 — Start the ROS stack

2 — Start the arm_vision client (separate machine or terminal)

3 — Homing (real robot only)

4 — Joint Monitor (observe live joint angles)

5 — View-only mode (real arm angles in RViz, no motion commands)

6 — Keyboard teleoperation (optional, separate terminal)

7 — MCU Emulator (test without real hardware)

8 — Pipeline test (mock arm_vision sender)

Calibration Workflow

Step 1 — Camera calibration

Step 2 — Cube calibration (5 faces)

Step 3 — Workspace mapping

Launch Arguments (`arm_bringup.launch.py`)

arm_vision CLI Reference

UART Protocol & Joint Mapping

Physical layer

Frame format

Joint sign & gear ratio

`hardware_params.yaml` key parameters

Hardware

About

Uh oh!

Releases

Packages

Uh oh!

Contributors

Uh oh!

Languages

Folders and files

Latest commit

History

Repository files navigation

Engineer2026 Control

System Architecture

Package Structure

Prerequisites

Build (ROS)

Running

1 — Start the ROS stack

2 — Start the arm_vision client (separate machine or terminal)

3 — Homing (real robot only)

4 — Joint Monitor (observe live joint angles)

5 — View-only mode (real arm angles in RViz, no motion commands)

6 — Keyboard teleoperation (optional, separate terminal)

7 — MCU Emulator (test without real hardware)

8 — Pipeline test (mock arm_vision sender)

Calibration Workflow

Step 1 — Camera calibration

Step 2 — Cube calibration (5 faces)

Step 3 — Workspace mapping

Launch Arguments (arm_bringup.launch.py)

arm_vision CLI Reference

UART Protocol & Joint Mapping

Physical layer

Frame format

Joint sign & gear ratio

hardware_params.yaml key parameters

Hardware

About

Resources

Uh oh!

Stars

Watchers

Forks

Releases

Packages 0

Uh oh!

Contributors

Uh oh!

Languages

Launch Arguments (`arm_bringup.launch.py`)

`hardware_params.yaml` key parameters

Packages