FocusHand

A Complete Platform for Haptic Teleoperation, Force Feedback Modeling, and Transformer-Based Grasp Prediction

FocusHand integrates ESP32 firmware, MuJoCo simulation, ROS communication, data logging, and a Transformer sequence model into a unified research framework for studying:

• Soft-object grasp dynamics\
• Contact force modeling\
• Data-driven control\
• Teleoperation + haptic feedback\
• Multi-step action prediction (action chunking)

This README provides detailed setup instructions, module explanations, and training workflow for the entire system.

---

** Repository Structure**

FocusHand/
│
├── Esp32firmware/               
│   ├── wifi_task.cpp
│   ├── motor_task.cpp
│   ├── force_task.cpp
│   └── ...
│
├── mujoco/
│   ├── grasp_controller_3_5.py
│   ├── udp_to_ros_bridge_3.py
│   ├── grasp_ai.py
│   ├── logger5.py
│   └── models/
│
└── transformer/
    ├── tool/
    │   ├── csv_to_pt.py
    │   ├── compute_norm_from_pt.py
    ├── train_transformer_simplified.py
    └── model/

---

1. ESP32 Firmware

The ESP32 firmware acts as the hardware interface of FocusHand. It controls motors, reads sensors, and communicates with the PC via UDP.

1.1 Features

• WiFi connection and static IP assignment\
• UDP communication (ESP32 → PC)\
• AS5600 magnetic encoder reading\
• FSR tactile sensor reading\
• Real-time torque estimation\
• Contact force processing / filtering\
• Motor control for X/Z axes

---

1.2 WiFi Configuration

Edit the following in wifi_task.cpp:

const char* ssid = "xxxx";
const char* password = "xxxxxxxx";

// Network configuration
IPAddress ip(192,168,xx,xx);       // ESP32 IP
IPAddress server(192,168,xx,xx);    // Host PC IP
uint16_t serverPort = 5001;          // Server port
uint16_t localPort  = 5000;          // Local port
IPAddress gateway(192,168,xx,xx);
IPAddress subnet(255,255,255,0);

ESP32 and PC must be on the same network.

---

1.3 UDP Packet Format

Each packet contains the real‑time state for both force and motor control:

Field Meaning

---

knob_state Angle from AS5600 encoder motor_torque Estimated torque fsr_value Force sensor reading tcp_force Filtered contact force force_timestamp Timestamp of force reading motor_timestamp Timestamp of motor update id Event ID for synchronization

This data is consumed by ROS and fed into MuJoCo/Transformer pipeline.

---

2. MuJoCo + ROS Simulation

This module simulates an Allegro hand in MuJoCo and links it with real-time ESP32 data using ROS.

2.1 Requirements

• ROS 1 (Melodic/Noetic recommended)\
• MuJoCo ≥ 3.0.0\
• Python ≥ 3.9\
• pip install mujoco

---

2.2 Configure MuJoCo Model Path

Set your model path in grasp_controller_3_5.py:

DEFAULT_MODEL_PATH = rospy.get_param(
    '/grasp_controller_absz/model_path',
    '/home/xxx/mujoco/wonik_allegro/left_hand_soft.xml'
)

This XML describes the Allegro Hand + soft-object environment.

---

2.3 Running the MuJoCo Controller

1. Start ROS:

   roscore

2. Launch MuJoCo + controller:

   python3 grasp_controller_3_5.py

This: - Loads the MuJoCo model\

• Starts ROS publishers/subscribers\
• Opens the GUI

---

2.4 Connecting ESP32 to ROS

Set ESP32 IP inside the file udp_to_ros_bridge_3.py:

ESP32_IP = "192.168.132.xxx"

Run:

python3 udp_to_ros_bridge_3.py

Now the system flow becomes:

ESP32 → UDP → ROS → grasp_controller → MuJoCo

---

3. Transformer-Based Grasp Policy

The Transformer is trained on recorded sequences to predict future multi-step actions: - next grasp command\

• next Z command

This implements action chunking, reducing control horizon and producing smoother behavior.

---

4. Data Logging (for training)

Use:

python3 logger5.py

Logger outputs: - A detailed CSV file\

• Force--position plots\
• Command trajectory plots\
• A time-synced dataset ready for conversion

Logging continues until you press q.

---

5. Transformer Training Pipeline

All training scripts are under transformer/.

5.1 Step 1 --- Convert CSV → Tensor Dataset

python .\tool\csv_to_pt.py `            
    --csv-glob ".\tool\train\*.csv" `
    --out-pt .\dataset\train.pt `      
    --time-col time `
    --resample-hz 0 `
    --seq-len 120 --pred-h 50 `
    --col-grip cmd --col-z hand_z `
    --col-fn tcp_normal --col-ft tcp_tangent --col-ftotal tcp_force `
    --target-cols cmd,hand_z

This: - merges CSVs\

• aligns force + state data\
• extracts sequence windows

---

5.2 Step 2 --- Compute Normalization Statistics

python transformer/tool/compute_norm_from_pt.py \
    --input state_data.pt \
    --output norm_stats.pt

Normalization is crucial for stable Transformer training.

---

5.3 Step 3 --- Train the Transformer

python .\tool\csv_to_pt.py `            
    --csv-glob ".\tool\train\*.csv" `
    --out-pt .\dataset\train.pt `      
    --time-col time `
    --resample-hz 0 `
    --seq-len 120 --pred-h 50 `
    --col-grip cmd --col-z hand_z `
    --col-fn tcp_normal --col-ft tcp_tangent --col-ftotal tcp_force `
    --target-cols cmd,hand_z

The model: - encodes 10-step history\

• predicts 5-step future actions\
• uses MSE + multi-step consistency loss

Output: - trained .pt checkpoint\

• training curves\
• config logging

---

6. Online Control With Transformer

Set checkpoint path:

self.CKPT_PATH = rospy.get_param("~ckpt_path", "state_only_gripz_tf_f.pt")

Run:

python3 grasp_ai.py _ckpt_path:=your_model.pt

Model will publish multi-step action chunks to the MuJoCo controller.

---

7. Full System Architecture

                ┌────────────────────────┐
                │        ESP32           │
                │  Encoder / FSR / BLDC  │
                └───────────┬────────────┘
                           UDP
                            │
            ┌───────────────┴────────────────┐
            │     udp_to_ros_bridge_3.py     │
            └───────────────┬────────────────┘
                          ROS Topics
                            │
                  ┌─────────┴─────────┐
                  │ grasp_controller  │
                  │  (MuJoCo + ROS)   │
                  └─────────┬─────────┘
                            │
                            │ optional
                            ▼
                  ┌────────────────────┐
                  │    grasp_ai.py     │
                  │ Transformer policy │
                  └────────────────────┘

Data Logger → CSV → PT → norm → training → model.pt → grasp_ai.py

---