diff --git a/.idea/grpc_client.iml b/.idea/grpc_client.iml
index 9c1850b..1675d09 100644
--- a/.idea/grpc_client.iml
+++ b/.idea/grpc_client.iml
@@ -5,7 +5,7 @@
-
+
\ No newline at end of file
diff --git a/.idea/misc.xml b/.idea/misc.xml
index 6ce4790..accb38f 100644
--- a/.idea/misc.xml
+++ b/.idea/misc.xml
@@ -3,5 +3,5 @@
-
+
\ No newline at end of file
diff --git a/README.md b/README.md
index cc17057..002e6fd 100644
--- a/README.md
+++ b/README.md
@@ -34,11 +34,17 @@ python -m clients.save_joint_to_csv
python -m clients.http_server
```
+## UI (PyQt)
+
+```bash
+python -m ui.app
+```
+
## Runtime parameters
- gRPC server address/port: set in `clients/base_client.py` (default `192.168.0.222:50052`).
- HTTP server host/port: set in `clients/http_server.py` (default `0.0.0.0:8000`).
- `device_id` defaults to `hc01` in most clients; change in code if needed.
-
+- UI parameters (host/port/device_id/interval) are set in the PyQt window and passed to the runner.
diff --git a/clients/__pycache__/get_pose_client.cpython-39.pyc b/clients/__pycache__/get_pose_client.cpython-39.pyc
new file mode 100644
index 0000000..922e3e1
Binary files /dev/null and b/clients/__pycache__/get_pose_client.cpython-39.pyc differ
diff --git a/clients/__pycache__/movej_client.cpython-39.pyc b/clients/__pycache__/movej_client.cpython-39.pyc
index d2776e4..4aa8f39 100644
Binary files a/clients/__pycache__/movej_client.cpython-39.pyc and b/clients/__pycache__/movej_client.cpython-39.pyc differ
diff --git a/clients/__pycache__/torque_off_client.cpython-39.pyc b/clients/__pycache__/torque_off_client.cpython-39.pyc
new file mode 100644
index 0000000..0b80783
Binary files /dev/null and b/clients/__pycache__/torque_off_client.cpython-39.pyc differ
diff --git a/clients/__pycache__/torque_on_client.cpython-39.pyc b/clients/__pycache__/torque_on_client.cpython-39.pyc
new file mode 100644
index 0000000..956d347
Binary files /dev/null and b/clients/__pycache__/torque_on_client.cpython-39.pyc differ
diff --git a/clients/movej_client.py b/clients/movej_client.py
index 4285b84..a1dde64 100644
--- a/clients/movej_client.py
+++ b/clients/movej_client.py
@@ -264,13 +264,13 @@ hello_r_joint_list = [
test_joint_list = [
- # {'joint_name': 'R_SHOULDER_P', 'rad':0},
+ {'joint_name': 'R_SHOULDER_P', 'rad':0},
{'joint_name': 'R_SHOULDER_R', 'rad': 0},
- # {'joint_name': 'R_SHOULDER_Y', 'rad': 0},
- # {'joint_name': 'R_ELBOW_R', 'rad':0},
- # {'joint_name': 'R_WRIST_P', 'rad': 0},
- # {'joint_name': 'R_WRIST_Y', 'rad': 0},
- # {'joint_name': 'R_WRIST_R', 'rad': 0},
+ {'joint_name': 'R_SHOULDER_Y', 'rad': 0},
+ {'joint_name': 'R_ELBOW_R', 'rad':0},
+ {'joint_name': 'R_WRIST_P', 'rad': 0},
+ {'joint_name': 'R_WRIST_Y', 'rad': 0},
+ {'joint_name': 'R_WRIST_R', 'rad': 0},
]
@@ -279,7 +279,14 @@ test_joint_list = [
#
-def action_hello(client):
+def _get_hand_client(hand_client):
+ if hand_client is None:
+ return DexHandClient(), True
+ return hand_client, False
+
+
+def action_hello(client, hand_client=None):
+ hand_client, created = _get_hand_client(hand_client)
hand_client.set_angles(hand_open, device_id="hand2")
client.send(init_joint_list, vel=1.0, acc=1.0)
client.send(hello_l_joint_list, vel=2.0, acc=2.0)
@@ -290,8 +297,11 @@ def action_hello(client):
time.sleep(0.1)
client.send(init_joint_list, vel=1.5, acc=1.5)
hand_client.set_angles(hand_open, device_id="hand2")
+ if created:
+ hand_client.close()
-def action_air(client):
+def action_air(client, hand_client=None):
+ hand_client, created = _get_hand_client(hand_client)
hand_client.set_angles(hand_touch, device_id="hand2")
client.send(init_joint_list, vel=2.0, acc=2.0)
client.send(head_screen_joint_list, vel=1.0, acc=1.0)
@@ -311,8 +321,11 @@ def action_air(client):
client.send(head_front_joint_list, vel=1.0, acc=1.0)
client.send(init_joint_list, vel=2.0, acc=2.0)
hand_client.set_angles(hand_open, device_id="hand2")
+ if created:
+ hand_client.close()
-def action_music(client):
+def action_music(client, hand_client=None):
+ hand_client, created = _get_hand_client(hand_client)
# init_joint_list
hand_client.set_angles(hand_touch, device_id="hand2")
client.send(init_joint_list, vel=2.0, acc=2.0)
@@ -333,6 +346,8 @@ def action_music(client):
client.send(head_front_joint_list, vel=1.0, acc=1.0)
client.send(init_joint_list, vel=2.0, acc=2.0)
hand_client.set_angles(hand_open, device_id="hand2")
+ if created:
+ hand_client.close()
def action_drive_economy(client):
# client.send(head_screen_joint_list, vel=1.0, acc=1.0)
# client.send(start_joint_list, vel=1.0, acc=1.0)
@@ -369,11 +384,10 @@ def action_drive_comfort(client):
# client.send(head_front_joint_list, vel=1.0, acc=1.0)
# client.send(init_joint_list, vel=1.0, acc=1.0)
-client = MoveJClient()
-hand_client = DexHandClient()
if __name__ == "__main__":
# Initialize client
-
+ client = MoveJClient()
+ hand_client = DexHandClient()
# action_air(client)
# action_hello(client)
client.send(test_joint_list, vel=1.0, acc=1.0)
@@ -398,4 +412,5 @@ if __name__ == "__main__":
# Close client
+ hand_client.close()
client.close()
diff --git a/requirements.txt b/requirements.txt
index 4d66caa..fe040b0 100644
--- a/requirements.txt
+++ b/requirements.txt
@@ -4,5 +4,7 @@ grpcio-tools==1.76.0
matplotlib==3.9.4
protobuf==6.33.2
pydantic==2.12.5
+PyQt5==5.15.11
requests==2.32.5
+yourdfpy
uvicorn==0.38.0
diff --git a/robot_model/xiaoyan_description/dual_arm.urdf b/robot_model/xiaoyan_description/dual_arm.urdf
new file mode 100644
index 0000000..e995eaa
--- /dev/null
+++ b/robot_model/xiaoyan_description/dual_arm.urdf
@@ -0,0 +1,660 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/robot_model/xiaoyan_description/dual_arm.xml b/robot_model/xiaoyan_description/dual_arm.xml
new file mode 100644
index 0000000..24a6d94
--- /dev/null
+++ b/robot_model/xiaoyan_description/dual_arm.xml
@@ -0,0 +1,206 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/robot_model/xiaoyan_description/dual_arm_temp.xml b/robot_model/xiaoyan_description/dual_arm_temp.xml
new file mode 100644
index 0000000..4aa7622
--- /dev/null
+++ b/robot_model/xiaoyan_description/dual_arm_temp.xml
@@ -0,0 +1,347 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/robot_model/xiaoyan_description/left_arm.urdf b/robot_model/xiaoyan_description/left_arm.urdf
new file mode 100644
index 0000000..d62639e
--- /dev/null
+++ b/robot_model/xiaoyan_description/left_arm.urdf
@@ -0,0 +1,281 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/robot_model/xiaoyan_description/left_arm.xml b/robot_model/xiaoyan_description/left_arm.xml
new file mode 100644
index 0000000..3b0c871
--- /dev/null
+++ b/robot_model/xiaoyan_description/left_arm.xml
@@ -0,0 +1,63 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/robot_model/xiaoyan_description/meshes/L_ELBOW_R_S.STL b/robot_model/xiaoyan_description/meshes/L_ELBOW_R_S.STL
new file mode 100644
index 0000000..17e4796
Binary files /dev/null and b/robot_model/xiaoyan_description/meshes/L_ELBOW_R_S.STL differ
diff --git a/robot_model/xiaoyan_description/meshes/L_SHOULDER_P_S.STL b/robot_model/xiaoyan_description/meshes/L_SHOULDER_P_S.STL
new file mode 100644
index 0000000..855e54e
Binary files /dev/null and b/robot_model/xiaoyan_description/meshes/L_SHOULDER_P_S.STL differ
diff --git a/robot_model/xiaoyan_description/meshes/L_SHOULDER_R_S.STL b/robot_model/xiaoyan_description/meshes/L_SHOULDER_R_S.STL
new file mode 100644
index 0000000..1652ea7
Binary files /dev/null and b/robot_model/xiaoyan_description/meshes/L_SHOULDER_R_S.STL differ
diff --git a/robot_model/xiaoyan_description/meshes/L_SHOULDER_Y_S.STL b/robot_model/xiaoyan_description/meshes/L_SHOULDER_Y_S.STL
new file mode 100644
index 0000000..a966726
Binary files /dev/null and b/robot_model/xiaoyan_description/meshes/L_SHOULDER_Y_S.STL differ
diff --git a/robot_model/xiaoyan_description/meshes/L_WRIST_P_S.STL b/robot_model/xiaoyan_description/meshes/L_WRIST_P_S.STL
new file mode 100644
index 0000000..10e6156
Binary files /dev/null and b/robot_model/xiaoyan_description/meshes/L_WRIST_P_S.STL differ
diff --git a/robot_model/xiaoyan_description/meshes/L_WRIST_R_S.STL b/robot_model/xiaoyan_description/meshes/L_WRIST_R_S.STL
new file mode 100644
index 0000000..4255742
Binary files /dev/null and b/robot_model/xiaoyan_description/meshes/L_WRIST_R_S.STL differ
diff --git a/robot_model/xiaoyan_description/meshes/L_WRIST_Y_S.STL b/robot_model/xiaoyan_description/meshes/L_WRIST_Y_S.STL
new file mode 100644
index 0000000..ae6a7f9
Binary files /dev/null and b/robot_model/xiaoyan_description/meshes/L_WRIST_Y_S.STL differ
diff --git a/robot_model/xiaoyan_description/meshes/NECK_P_S.STL b/robot_model/xiaoyan_description/meshes/NECK_P_S.STL
new file mode 100644
index 0000000..b294c7f
Binary files /dev/null and b/robot_model/xiaoyan_description/meshes/NECK_P_S.STL differ
diff --git a/robot_model/xiaoyan_description/meshes/NECK_R_S.STL b/robot_model/xiaoyan_description/meshes/NECK_R_S.STL
new file mode 100644
index 0000000..f9e097e
Binary files /dev/null and b/robot_model/xiaoyan_description/meshes/NECK_R_S.STL differ
diff --git a/robot_model/xiaoyan_description/meshes/NECK_Y_S.STL b/robot_model/xiaoyan_description/meshes/NECK_Y_S.STL
new file mode 100644
index 0000000..f1e1b9b
Binary files /dev/null and b/robot_model/xiaoyan_description/meshes/NECK_Y_S.STL differ
diff --git a/robot_model/xiaoyan_description/meshes/PELVIS_S.STL b/robot_model/xiaoyan_description/meshes/PELVIS_S.STL
new file mode 100644
index 0000000..133cf46
Binary files /dev/null and b/robot_model/xiaoyan_description/meshes/PELVIS_S.STL differ
diff --git a/robot_model/xiaoyan_description/meshes/R_ELBOW_R_S.STL b/robot_model/xiaoyan_description/meshes/R_ELBOW_R_S.STL
new file mode 100644
index 0000000..2fb7b53
Binary files /dev/null and b/robot_model/xiaoyan_description/meshes/R_ELBOW_R_S.STL differ
diff --git a/robot_model/xiaoyan_description/meshes/R_SHOULDER_P_S.STL b/robot_model/xiaoyan_description/meshes/R_SHOULDER_P_S.STL
new file mode 100644
index 0000000..8bfafb1
Binary files /dev/null and b/robot_model/xiaoyan_description/meshes/R_SHOULDER_P_S.STL differ
diff --git a/robot_model/xiaoyan_description/meshes/R_SHOULDER_R_S.STL b/robot_model/xiaoyan_description/meshes/R_SHOULDER_R_S.STL
new file mode 100644
index 0000000..b14ad9b
Binary files /dev/null and b/robot_model/xiaoyan_description/meshes/R_SHOULDER_R_S.STL differ
diff --git a/robot_model/xiaoyan_description/meshes/R_SHOULDER_Y_S.STL b/robot_model/xiaoyan_description/meshes/R_SHOULDER_Y_S.STL
new file mode 100644
index 0000000..aea9b41
Binary files /dev/null and b/robot_model/xiaoyan_description/meshes/R_SHOULDER_Y_S.STL differ
diff --git a/robot_model/xiaoyan_description/meshes/R_WRIST_P_S.STL b/robot_model/xiaoyan_description/meshes/R_WRIST_P_S.STL
new file mode 100644
index 0000000..b48aa44
Binary files /dev/null and b/robot_model/xiaoyan_description/meshes/R_WRIST_P_S.STL differ
diff --git a/robot_model/xiaoyan_description/meshes/R_WRIST_R_S.STL b/robot_model/xiaoyan_description/meshes/R_WRIST_R_S.STL
new file mode 100644
index 0000000..70a997d
Binary files /dev/null and b/robot_model/xiaoyan_description/meshes/R_WRIST_R_S.STL differ
diff --git a/robot_model/xiaoyan_description/meshes/R_WRIST_Y_S.STL b/robot_model/xiaoyan_description/meshes/R_WRIST_Y_S.STL
new file mode 100644
index 0000000..ef15308
Binary files /dev/null and b/robot_model/xiaoyan_description/meshes/R_WRIST_Y_S.STL differ
diff --git a/robot_model/xiaoyan_description/meshes/base_link.STL b/robot_model/xiaoyan_description/meshes/base_link.STL
new file mode 100644
index 0000000..f7dae5c
Binary files /dev/null and b/robot_model/xiaoyan_description/meshes/base_link.STL differ
diff --git a/robot_model/xiaoyan_description/meshes/index_force_sensor_1.STL b/robot_model/xiaoyan_description/meshes/index_force_sensor_1.STL
new file mode 100644
index 0000000..b271a2a
Binary files /dev/null and b/robot_model/xiaoyan_description/meshes/index_force_sensor_1.STL differ
diff --git a/robot_model/xiaoyan_description/meshes/index_force_sensor_2.STL b/robot_model/xiaoyan_description/meshes/index_force_sensor_2.STL
new file mode 100644
index 0000000..f056699
Binary files /dev/null and b/robot_model/xiaoyan_description/meshes/index_force_sensor_2.STL differ
diff --git a/robot_model/xiaoyan_description/meshes/index_force_sensor_3.STL b/robot_model/xiaoyan_description/meshes/index_force_sensor_3.STL
new file mode 100644
index 0000000..7bf3a71
Binary files /dev/null and b/robot_model/xiaoyan_description/meshes/index_force_sensor_3.STL differ
diff --git a/robot_model/xiaoyan_description/meshes/little_force_sensor_1.STL b/robot_model/xiaoyan_description/meshes/little_force_sensor_1.STL
new file mode 100644
index 0000000..c4bffaf
Binary files /dev/null and b/robot_model/xiaoyan_description/meshes/little_force_sensor_1.STL differ
diff --git a/robot_model/xiaoyan_description/meshes/little_force_sensor_2.STL b/robot_model/xiaoyan_description/meshes/little_force_sensor_2.STL
new file mode 100644
index 0000000..c1d599d
Binary files /dev/null and b/robot_model/xiaoyan_description/meshes/little_force_sensor_2.STL differ
diff --git a/robot_model/xiaoyan_description/meshes/little_force_sensor_3.STL b/robot_model/xiaoyan_description/meshes/little_force_sensor_3.STL
new file mode 100644
index 0000000..f5d06b4
Binary files /dev/null and b/robot_model/xiaoyan_description/meshes/little_force_sensor_3.STL differ
diff --git a/robot_model/xiaoyan_description/meshes/middle_force_sensor_1.STL b/robot_model/xiaoyan_description/meshes/middle_force_sensor_1.STL
new file mode 100644
index 0000000..d42d830
Binary files /dev/null and b/robot_model/xiaoyan_description/meshes/middle_force_sensor_1.STL differ
diff --git a/robot_model/xiaoyan_description/meshes/middle_force_sensor_2.STL b/robot_model/xiaoyan_description/meshes/middle_force_sensor_2.STL
new file mode 100644
index 0000000..2feead5
Binary files /dev/null and b/robot_model/xiaoyan_description/meshes/middle_force_sensor_2.STL differ
diff --git a/robot_model/xiaoyan_description/meshes/middle_force_sensor_3.STL b/robot_model/xiaoyan_description/meshes/middle_force_sensor_3.STL
new file mode 100644
index 0000000..bb35da1
Binary files /dev/null and b/robot_model/xiaoyan_description/meshes/middle_force_sensor_3.STL differ
diff --git a/robot_model/xiaoyan_description/meshes/palm_force_sensor.STL b/robot_model/xiaoyan_description/meshes/palm_force_sensor.STL
new file mode 100644
index 0000000..60824bf
Binary files /dev/null and b/robot_model/xiaoyan_description/meshes/palm_force_sensor.STL differ
diff --git a/robot_model/xiaoyan_description/meshes/right_index_1.STL b/robot_model/xiaoyan_description/meshes/right_index_1.STL
new file mode 100644
index 0000000..c17ff30
Binary files /dev/null and b/robot_model/xiaoyan_description/meshes/right_index_1.STL differ
diff --git a/robot_model/xiaoyan_description/meshes/right_index_2.STL b/robot_model/xiaoyan_description/meshes/right_index_2.STL
new file mode 100644
index 0000000..3f5e462
Binary files /dev/null and b/robot_model/xiaoyan_description/meshes/right_index_2.STL differ
diff --git a/robot_model/xiaoyan_description/meshes/right_little_1.STL b/robot_model/xiaoyan_description/meshes/right_little_1.STL
new file mode 100644
index 0000000..cc79788
Binary files /dev/null and b/robot_model/xiaoyan_description/meshes/right_little_1.STL differ
diff --git a/robot_model/xiaoyan_description/meshes/right_little_2.STL b/robot_model/xiaoyan_description/meshes/right_little_2.STL
new file mode 100644
index 0000000..c42f6d7
Binary files /dev/null and b/robot_model/xiaoyan_description/meshes/right_little_2.STL differ
diff --git a/robot_model/xiaoyan_description/meshes/right_middle_1.STL b/robot_model/xiaoyan_description/meshes/right_middle_1.STL
new file mode 100644
index 0000000..a6942b7
Binary files /dev/null and b/robot_model/xiaoyan_description/meshes/right_middle_1.STL differ
diff --git a/robot_model/xiaoyan_description/meshes/right_middle_2.STL b/robot_model/xiaoyan_description/meshes/right_middle_2.STL
new file mode 100644
index 0000000..0f3865f
Binary files /dev/null and b/robot_model/xiaoyan_description/meshes/right_middle_2.STL differ
diff --git a/robot_model/xiaoyan_description/meshes/right_ring_1.STL b/robot_model/xiaoyan_description/meshes/right_ring_1.STL
new file mode 100644
index 0000000..6fc79ca
Binary files /dev/null and b/robot_model/xiaoyan_description/meshes/right_ring_1.STL differ
diff --git a/robot_model/xiaoyan_description/meshes/right_ring_2.STL b/robot_model/xiaoyan_description/meshes/right_ring_2.STL
new file mode 100644
index 0000000..a4cf458
Binary files /dev/null and b/robot_model/xiaoyan_description/meshes/right_ring_2.STL differ
diff --git a/robot_model/xiaoyan_description/meshes/right_thumb_1.STL b/robot_model/xiaoyan_description/meshes/right_thumb_1.STL
new file mode 100644
index 0000000..a4f441c
Binary files /dev/null and b/robot_model/xiaoyan_description/meshes/right_thumb_1.STL differ
diff --git a/robot_model/xiaoyan_description/meshes/right_thumb_2.STL b/robot_model/xiaoyan_description/meshes/right_thumb_2.STL
new file mode 100644
index 0000000..0b29f9b
Binary files /dev/null and b/robot_model/xiaoyan_description/meshes/right_thumb_2.STL differ
diff --git a/robot_model/xiaoyan_description/meshes/right_thumb_3.STL b/robot_model/xiaoyan_description/meshes/right_thumb_3.STL
new file mode 100644
index 0000000..cf5cab1
Binary files /dev/null and b/robot_model/xiaoyan_description/meshes/right_thumb_3.STL differ
diff --git a/robot_model/xiaoyan_description/meshes/right_thumb_4.STL b/robot_model/xiaoyan_description/meshes/right_thumb_4.STL
new file mode 100644
index 0000000..bf147e0
Binary files /dev/null and b/robot_model/xiaoyan_description/meshes/right_thumb_4.STL differ
diff --git a/robot_model/xiaoyan_description/meshes/ring_force_sensor_1.STL b/robot_model/xiaoyan_description/meshes/ring_force_sensor_1.STL
new file mode 100644
index 0000000..40d42a1
Binary files /dev/null and b/robot_model/xiaoyan_description/meshes/ring_force_sensor_1.STL differ
diff --git a/robot_model/xiaoyan_description/meshes/ring_force_sensor_2.STL b/robot_model/xiaoyan_description/meshes/ring_force_sensor_2.STL
new file mode 100644
index 0000000..0e9e95b
Binary files /dev/null and b/robot_model/xiaoyan_description/meshes/ring_force_sensor_2.STL differ
diff --git a/robot_model/xiaoyan_description/meshes/ring_force_sensor_3.STL b/robot_model/xiaoyan_description/meshes/ring_force_sensor_3.STL
new file mode 100644
index 0000000..32c799b
Binary files /dev/null and b/robot_model/xiaoyan_description/meshes/ring_force_sensor_3.STL differ
diff --git a/robot_model/xiaoyan_description/meshes/thumb_force_sensor_1.STL b/robot_model/xiaoyan_description/meshes/thumb_force_sensor_1.STL
new file mode 100644
index 0000000..9993ba8
Binary files /dev/null and b/robot_model/xiaoyan_description/meshes/thumb_force_sensor_1.STL differ
diff --git a/robot_model/xiaoyan_description/meshes/thumb_force_sensor_2.STL b/robot_model/xiaoyan_description/meshes/thumb_force_sensor_2.STL
new file mode 100644
index 0000000..ee1fe22
Binary files /dev/null and b/robot_model/xiaoyan_description/meshes/thumb_force_sensor_2.STL differ
diff --git a/robot_model/xiaoyan_description/meshes/thumb_force_sensor_3.STL b/robot_model/xiaoyan_description/meshes/thumb_force_sensor_3.STL
new file mode 100644
index 0000000..57f9055
Binary files /dev/null and b/robot_model/xiaoyan_description/meshes/thumb_force_sensor_3.STL differ
diff --git a/robot_model/xiaoyan_description/meshes/thumb_force_sensor_4.STL b/robot_model/xiaoyan_description/meshes/thumb_force_sensor_4.STL
new file mode 100644
index 0000000..1842e2e
Binary files /dev/null and b/robot_model/xiaoyan_description/meshes/thumb_force_sensor_4.STL differ
diff --git a/robot_model/xiaoyan_description/right_arm.urdf b/robot_model/xiaoyan_description/right_arm.urdf
new file mode 100644
index 0000000..b270c08
--- /dev/null
+++ b/robot_model/xiaoyan_description/right_arm.urdf
@@ -0,0 +1,322 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/robot_model/xiaoyan_description/right_arm.xml b/robot_model/xiaoyan_description/right_arm.xml
new file mode 100644
index 0000000..f0def49
--- /dev/null
+++ b/robot_model/xiaoyan_description/right_arm.xml
@@ -0,0 +1,63 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/ui/__init__.py b/ui/__init__.py
new file mode 100644
index 0000000..1a1fa0e
--- /dev/null
+++ b/ui/__init__.py
@@ -0,0 +1 @@
+# Package marker for UI modules.
diff --git a/ui/__pycache__/__init__.cpython-39.pyc b/ui/__pycache__/__init__.cpython-39.pyc
new file mode 100644
index 0000000..39c7159
Binary files /dev/null and b/ui/__pycache__/__init__.cpython-39.pyc differ
diff --git a/ui/__pycache__/app.cpython-39.pyc b/ui/__pycache__/app.cpython-39.pyc
new file mode 100644
index 0000000..501cdc5
Binary files /dev/null and b/ui/__pycache__/app.cpython-39.pyc differ
diff --git a/ui/__pycache__/run_action.cpython-39.pyc b/ui/__pycache__/run_action.cpython-39.pyc
new file mode 100644
index 0000000..a6249a1
Binary files /dev/null and b/ui/__pycache__/run_action.cpython-39.pyc differ
diff --git a/ui/app.py b/ui/app.py
new file mode 100644
index 0000000..50e24b0
--- /dev/null
+++ b/ui/app.py
@@ -0,0 +1,783 @@
+import sys
+import time
+from collections import deque
+from pathlib import Path
+from typing import Deque, Dict, Optional, Tuple
+
+import grpc
+import numpy as np
+from PyQt5 import QtCore, QtWidgets
+from matplotlib.backends.backend_qt5agg import FigureCanvasQTAgg as FigureCanvas
+from matplotlib.backends.backend_qt5agg import NavigationToolbar2QT as NavigationToolbar
+from matplotlib.figure import Figure
+try:
+ from yourdfpy import URDF
+except Exception: # pragma: no cover - fallback if yourdfpy not available
+ from urdfpy import URDF
+
+from clients._path_setup import ensure_paths
+
+ensure_paths()
+
+from google.protobuf import timestamp_pb2
+
+from cmvr.api import humanoid_robot_command_pb2 as pb
+from cmvr.api import humanoid_robot_service_pb2_grpc as rpc
+
+DEFAULT_JOINT_ORDER = [
+ "L_SHOULDER_P", "L_SHOULDER_R", "L_SHOULDER_Y",
+ "L_ELBOW_R", "L_WRIST_P", "L_WRIST_Y", "L_WRIST_R",
+ "R_SHOULDER_P", "R_SHOULDER_R", "R_SHOULDER_Y",
+ "R_ELBOW_R", "R_WRIST_P", "R_WRIST_Y", "R_WRIST_R",
+ "HEAD_P", "HEAD_Y", "HEAD_R", "WAIST_Y", "WAIST_R",
+]
+
+URDF_PATH = Path(__file__).resolve().parents[1] / "robot_model" / "xiaoyan_description" / "dual_arm.urdf"
+
+
+class JointStateWorker(QtCore.QThread):
+ updated = QtCore.pyqtSignal(dict)
+ log = QtCore.pyqtSignal(str)
+
+ def __init__(
+ self,
+ address: str,
+ device_id: str,
+ interval: float,
+ joint_names: list[str],
+ timeout: float = 2.0,
+ ) -> None:
+ super().__init__()
+ self.address = address
+ self.device_id = device_id
+ self.interval = interval
+ self.joint_names = joint_names
+ self.timeout = timeout
+ self._stop = False
+ self._channel: Optional[grpc.Channel] = None
+
+ def stop(self) -> None:
+ self._stop = True
+
+ def run(self) -> None:
+ try:
+ self._channel = grpc.insecure_channel(self.address)
+ stub = rpc.HumanoidRobotServiceStub(self._channel)
+ except Exception as exc:
+ self.log.emit(f"[error] connect failed: {exc}")
+ return
+
+ while not self._stop:
+ req = pb.JointRequest()
+ req.header.device_id = self.device_id
+ ts = timestamp_pb2.Timestamp()
+ ts.GetCurrentTime()
+ req.header.timestamp.CopyFrom(ts)
+ try:
+ resp = stub.getJointState(req, timeout=10)
+ except Exception as exc:
+ self.log.emit(f"[error] getJointState failed: {exc}")
+ time.sleep(self.interval)
+ continue
+
+ joint_pos: Dict[str, float] = {}
+ joint_vel: Dict[str, float] = {}
+ for state in resp.state:
+ for name, pos, vel in zip(state.name, state.position, state.velocity):
+ joint_pos[name] = pos
+ joint_vel[name] = vel
+
+ payload: Dict[str, Tuple[float, float]] = {}
+ for name in self.joint_names:
+ payload[name] = (joint_pos.get(name, 0.0), joint_vel.get(name, 0.0))
+
+ self.updated.emit(payload)
+ time.sleep(self.interval)
+ if self._channel:
+ self._channel.close()
+
+
+class PoseWorker(QtCore.QThread):
+ updated = QtCore.pyqtSignal(dict)
+ log = QtCore.pyqtSignal(str)
+
+ def __init__(
+ self,
+ address: str,
+ device_id: str,
+ interval: float,
+ base_link: str,
+ ee_link: str,
+ timeout: float = 2.0,
+ ) -> None:
+ super().__init__()
+ self.address = address
+ self.device_id = device_id
+ self.interval = interval
+ self.base_link = base_link
+ self.ee_link = ee_link
+ self.timeout = timeout
+ self._stop = False
+ self._channel: Optional[grpc.Channel] = None
+
+ def stop(self) -> None:
+ self._stop = True
+
+ def run(self) -> None:
+ try:
+ self._channel = grpc.insecure_channel(self.address)
+ stub = rpc.HumanoidRobotServiceStub(self._channel)
+ except Exception as exc:
+ self.log.emit(f"[error] connect failed: {exc}")
+ return
+
+ while not self._stop:
+ req = pb.GetPose.Request()
+ req.header.device_id = self.device_id
+ req.base_link = self.base_link
+ req.ee_link = self.ee_link
+ ts = timestamp_pb2.Timestamp()
+ ts.GetCurrentTime()
+ req.header.timestamp.CopyFrom(ts)
+ try:
+ resp = stub.getPose(req, timeout=10)
+ except Exception as exc:
+ self.log.emit(f"[error] getPose failed: {exc}")
+ time.sleep(self.interval)
+ continue
+
+ pose = resp.pose
+ payload = {
+ "x": pose.x,
+ "y": pose.y,
+ "z": pose.z,
+ "rx": pose.rx,
+ "ry": pose.ry,
+ "rz": pose.rz,
+ }
+ self.updated.emit(payload)
+ time.sleep(self.interval)
+ if self._channel:
+ self._channel.close()
+
+
+class MainWindow(QtWidgets.QMainWindow):
+ def __init__(self) -> None:
+ super().__init__()
+ self.setWindowTitle("gRPC Client UI")
+ self.resize(900, 600)
+
+ self._pose_process: Optional[QtCore.QProcess] = None
+ self._joint_worker: Optional[JointStateWorker] = None
+ self._pose_worker: Optional[PoseWorker] = None
+ self._plot_selected: set[str] = set()
+ self._last_data: Dict[str, Tuple[float, float]] = {}
+ self._updating_movej_table = False
+ self.urdf: Optional[URDF] = None
+ self.urdf_error: Optional[str] = None
+ self.joint_names: list[str] = DEFAULT_JOINT_ORDER.copy()
+ self.link_names: list[str] = []
+ self._warned_no_joint_match = False
+
+ self._load_urdf()
+
+ central = self._build_center_panel()
+ self.setCentralWidget(central)
+ self._build_docks()
+ self.statusBar().showMessage("Ready")
+
+ def _load_urdf(self) -> None:
+ try:
+ self.urdf = URDF.load(str(URDF_PATH))
+ self.link_names = [link.name for link in self._urdf_links()]
+ self.joint_names = [
+ joint.name for joint in self._urdf_joints() if getattr(joint, "joint_type", "") != "fixed"
+ ]
+ except Exception as exc:
+ self.urdf_error = str(exc)
+ self.urdf = None
+ self.link_names = []
+ self.joint_names = DEFAULT_JOINT_ORDER.copy()
+
+ def _urdf_links(self) -> list:
+ if not self.urdf:
+ return []
+ if hasattr(self.urdf, "links"):
+ return list(self.urdf.links)
+ if hasattr(self.urdf, "link_map"):
+ return list(self.urdf.link_map.values())
+ return []
+
+ def _urdf_joints(self) -> list:
+ if not self.urdf:
+ return []
+ if hasattr(self.urdf, "joints"):
+ return list(self.urdf.joints)
+ if hasattr(self.urdf, "joint_map"):
+ return list(self.urdf.joint_map.values())
+ return []
+
+ def _build_center_panel(self) -> QtWidgets.QWidget:
+ panel = QtWidgets.QWidget()
+ layout = QtWidgets.QVBoxLayout(panel)
+ layout.setContentsMargins(0, 0, 0, 0)
+ layout.setSpacing(0)
+ self.center_tabs = QtWidgets.QTabWidget()
+ self.center_tabs.addTab(self._build_live_panel(), "Plot")
+ self.center_tabs.setCurrentIndex(0)
+ self.center_tabs.currentChanged.connect(self._on_center_tab_changed)
+ layout.addWidget(self.center_tabs)
+ return panel
+
+ def _build_docks(self) -> None:
+ control = QtWidgets.QDockWidget("Control")
+ control.setWidget(self._build_control_panel())
+ control.setMinimumWidth(550)
+ control.setFeatures(QtWidgets.QDockWidget.AllDockWidgetFeatures)
+ self.addDockWidget(QtCore.Qt.LeftDockWidgetArea, control)
+
+ status = QtWidgets.QDockWidget("Joint Status")
+ status.setWidget(self._build_status_panel())
+ status.setMinimumWidth(650)
+ status.setFeatures(QtWidgets.QDockWidget.AllDockWidgetFeatures)
+ self.addDockWidget(QtCore.Qt.RightDockWidgetArea, status)
+
+ log_dock = QtWidgets.QDockWidget("Log")
+ log_dock.setWidget(self._build_log_panel())
+ log_dock.setMinimumHeight(140)
+ log_dock.setMaximumHeight(220)
+ log_dock.setFeatures(QtWidgets.QDockWidget.AllDockWidgetFeatures)
+ self.addDockWidget(QtCore.Qt.BottomDockWidgetArea, log_dock)
+
+ def _build_status_panel(self) -> QtWidgets.QWidget:
+ panel = QtWidgets.QWidget()
+ layout = QtWidgets.QVBoxLayout(panel)
+ splitter = QtWidgets.QSplitter()
+ splitter.setOrientation(QtCore.Qt.Vertical)
+ splitter.addWidget(self._build_table())
+ splitter.addWidget(self._build_pose_panel())
+ splitter.setStretchFactor(0, 3)
+ splitter.setStretchFactor(1, 1)
+ layout.addWidget(splitter)
+ return panel
+
+ def _build_control_panel(self) -> QtWidgets.QWidget:
+ panel = QtWidgets.QWidget()
+ layout = QtWidgets.QVBoxLayout(panel)
+ layout.addWidget(self._build_settings_group())
+ layout.addWidget(self._build_movej_group())
+ layout.addWidget(self._build_actions_group())
+ layout.addStretch(1)
+ return panel
+
+ def _build_settings_group(self) -> QtWidgets.QGroupBox:
+ group = QtWidgets.QGroupBox("Settings")
+ layout = QtWidgets.QVBoxLayout(group)
+ layout.addLayout(self._build_form())
+ return group
+
+ def _build_movej_group(self) -> QtWidgets.QGroupBox:
+ group = QtWidgets.QGroupBox("MoveJ")
+ layout = QtWidgets.QVBoxLayout(group)
+ params = QtWidgets.QFormLayout()
+ params.addRow("MoveJ Vel", self.vel_edit)
+ params.addRow("MoveJ Acc", self.acc_edit)
+ layout.addLayout(params)
+ layout.addWidget(self._build_movej_table())
+ self.movej_btn = QtWidgets.QPushButton("Send MoveJ")
+ self.movej_btn.clicked.connect(self._on_movej)
+ layout.addWidget(self.movej_btn)
+ return group
+
+ def _build_actions_group(self) -> QtWidgets.QGroupBox:
+ group = QtWidgets.QGroupBox("Actions")
+ layout = QtWidgets.QVBoxLayout(group)
+ self.jointstate_toggle = self._build_switch_row("JointState")
+ self.torque_on_btn = QtWidgets.QPushButton("Torque On")
+ self.torque_off_btn = QtWidgets.QPushButton("Torque Off")
+ self.jointstate_toggle.toggled.connect(self._on_jointstate_toggle)
+ self.torque_on_btn.clicked.connect(self._on_torque_on)
+ self.torque_off_btn.clicked.connect(self._on_torque_off)
+ layout.addLayout(self._wrap_switch_row(self.jointstate_toggle, "Start JointState"))
+ layout.addWidget(self.torque_on_btn)
+ layout.addWidget(self.torque_off_btn)
+ return group
+
+ def _build_switch_row(self, name: str) -> QtWidgets.QCheckBox:
+ toggle = QtWidgets.QCheckBox()
+ toggle.setChecked(False)
+ toggle.setFixedSize(56, 28)
+ toggle.setStyleSheet(
+ """
+ QCheckBox {
+ background-color: #bfc5cf;
+ border-radius: 14px;
+ padding: 0px;
+ }
+ QCheckBox:checked {
+ background-color: #2f6fed;
+ }
+ QCheckBox::indicator {
+ width: 22px;
+ height: 22px;
+ border-radius: 11px;
+ background: #ffffff;
+ margin-left: 3px;
+ }
+ QCheckBox::indicator:checked {
+ margin-left: 31px;
+ }
+ """
+ )
+ toggle.setTristate(False)
+ toggle.setObjectName(f"{name}_toggle")
+ return toggle
+
+ def _wrap_switch_row(self, toggle: QtWidgets.QCheckBox, label: str) -> QtWidgets.QHBoxLayout:
+ row = QtWidgets.QHBoxLayout()
+ row.addWidget(toggle)
+ row.addWidget(QtWidgets.QLabel(label))
+ row.addStretch(1)
+ return row
+ def _build_form(self) -> QtWidgets.QLayout:
+ form = QtWidgets.QFormLayout()
+
+ self.host_edit = QtWidgets.QLineEdit("192.168.0.222")
+ self.port_edit = QtWidgets.QSpinBox()
+ self.port_edit.setRange(1, 65535)
+ self.port_edit.setValue(50052)
+ self.device_edit = QtWidgets.QLineEdit("hc01")
+ self.interval_edit = QtWidgets.QDoubleSpinBox()
+ self.interval_edit.setDecimals(3)
+ self.interval_edit.setRange(0.001, 10.0)
+ self.interval_edit.setValue(0.05)
+ self.vel_edit = QtWidgets.QDoubleSpinBox()
+ self.vel_edit.setRange(0.1, 10.0)
+ self.vel_edit.setValue(1.0)
+ self.acc_edit = QtWidgets.QDoubleSpinBox()
+ self.acc_edit.setRange(0.1, 10.0)
+ self.acc_edit.setValue(0.5)
+
+ form.addRow("Host", self.host_edit)
+ form.addRow("Port", self.port_edit)
+ form.addRow("Device ID", self.device_edit)
+ form.addRow("Interval (s)", self.interval_edit)
+
+ return form
+
+ def _build_log(self) -> QtWidgets.QWidget:
+ self.log = QtWidgets.QTextEdit()
+ self.log.setReadOnly(True)
+ return self.log
+
+ def _build_log_panel(self) -> QtWidgets.QWidget:
+ panel = QtWidgets.QWidget()
+ layout = QtWidgets.QVBoxLayout(panel)
+ header = QtWidgets.QHBoxLayout()
+ header.addWidget(QtWidgets.QLabel("Runtime logs"))
+ header.addStretch(1)
+ log_widget = self._build_log()
+ self.clear_log_btn = QtWidgets.QPushButton("Clear")
+ self.clear_log_btn.clicked.connect(log_widget.clear)
+ header.addWidget(self.clear_log_btn)
+ layout.addLayout(header)
+ layout.addWidget(log_widget)
+ return panel
+
+ def _build_pose_panel(self) -> QtWidgets.QGroupBox:
+ group = QtWidgets.QGroupBox("GetPose")
+ layout = QtWidgets.QVBoxLayout(group)
+
+ self.getpose_toggle = self._build_switch_row("GetPose")
+ self.getpose_toggle.toggled.connect(self._on_getpose_toggle)
+ layout.addLayout(self._wrap_switch_row(self.getpose_toggle, "Start GetPose"))
+
+ form = QtWidgets.QFormLayout()
+ self.base_link_edit = QtWidgets.QComboBox()
+ self.ee_link_edit = QtWidgets.QComboBox()
+ if self.link_names:
+ self.base_link_edit.addItems(self.link_names)
+ self.ee_link_edit.addItems(self.link_names)
+ if "PELVIS_S" in self.link_names:
+ self.base_link_edit.setCurrentText("PELVIS_S")
+ if "R_FINGER_TIP" in self.link_names:
+ self.ee_link_edit.setCurrentText("R_FINGER_TIP")
+ else:
+ self.base_link_edit.addItem("PELVIS_S")
+ self.ee_link_edit.addItem("R_FINGER_TIP")
+ form.addRow("Base Link", self.base_link_edit)
+ form.addRow("EE Link", self.ee_link_edit)
+ layout.addLayout(form)
+
+ self.pose_labels = {}
+ grid = QtWidgets.QGridLayout()
+ for i, key in enumerate(["x", "y", "z", "rx", "ry", "rz"]):
+ label = QtWidgets.QLabel(f"{key}: 0.000000")
+ self.pose_labels[key] = label
+ grid.addWidget(label, i // 3, i % 3)
+ layout.addLayout(grid)
+ return group
+
+ def _build_live_panel(self) -> QtWidgets.QWidget:
+ tab = QtWidgets.QWidget()
+ layout = QtWidgets.QVBoxLayout(tab)
+
+ layout.setContentsMargins(0, 0, 0, 0)
+ layout.setSpacing(4)
+
+ self.figure = Figure(figsize=(5, 4))
+ self.canvas = FigureCanvas(self.figure)
+ self.ax_pos = self.figure.add_subplot(2, 1, 1)
+ self.ax_vel = self.figure.add_subplot(2, 1, 2)
+ self.ax_pos.set_title("Position (rad)")
+ self.ax_vel.set_title("Velocity (rad/s)")
+ self.ax_pos.grid(True, alpha=0.3)
+ self.ax_vel.grid(True, alpha=0.3)
+ self.pos_lines: Dict[str, object] = {}
+ self.vel_lines: Dict[str, object] = {}
+
+ self.time_hist: Deque[float] = deque(maxlen=300)
+ self._last_plot_time: Optional[float] = None
+ self.pos_hist: Dict[str, Deque[float]] = {name: deque(maxlen=300) for name in self.joint_names}
+ self.vel_hist: Dict[str, Deque[float]] = {name: deque(maxlen=300) for name in self.joint_names}
+ self._plot_start = time.time()
+
+ self.canvas.setSizePolicy(
+ QtWidgets.QSizePolicy.Expanding, QtWidgets.QSizePolicy.Expanding
+ )
+ self.toolbar = NavigationToolbar(self.canvas, self)
+ layout.addWidget(self.toolbar)
+ layout.addWidget(self.canvas)
+ return tab
+
+ def _build_table(self) -> QtWidgets.QWidget:
+ self.table = QtWidgets.QTableWidget(len(self.joint_names), 4)
+ self.table.setHorizontalHeaderLabels(
+ ["Joint", "Position (rad)", "Position (deg)", "Velocity (rad/s)"]
+ )
+ self.table.verticalHeader().setVisible(False)
+ self.table.setEditTriggers(QtWidgets.QAbstractItemView.NoEditTriggers)
+ self.table.setSelectionMode(QtWidgets.QAbstractItemView.ExtendedSelection)
+ self.table.setSelectionBehavior(QtWidgets.QAbstractItemView.SelectRows)
+ for row, name in enumerate(self.joint_names):
+ self.table.setItem(row, 0, QtWidgets.QTableWidgetItem(name))
+ self.table.setItem(row, 1, QtWidgets.QTableWidgetItem("0.0"))
+ self.table.setItem(row, 2, QtWidgets.QTableWidgetItem("0.0"))
+ self.table.setItem(row, 3, QtWidgets.QTableWidgetItem("0.0"))
+ self.table.horizontalHeader().setStretchLastSection(True)
+ self.table.itemSelectionChanged.connect(self._on_status_selection_changed)
+ return self.table
+
+ def _build_movej_table(self) -> QtWidgets.QWidget:
+ self.movej_table = QtWidgets.QTableWidget(len(self.joint_names), 4)
+ self.movej_table.setHorizontalHeaderLabels(["Use", "Joint", "Deg", "Rad"])
+ self.movej_table.verticalHeader().setVisible(False)
+ self.movej_table.setSizePolicy(
+ QtWidgets.QSizePolicy.Expanding, QtWidgets.QSizePolicy.Expanding
+ )
+ for row, name in enumerate(self.joint_names):
+ use_item = QtWidgets.QTableWidgetItem()
+ use_item.setCheckState(QtCore.Qt.Unchecked)
+ self.movej_table.setItem(row, 0, use_item)
+ self.movej_table.setItem(row, 1, QtWidgets.QTableWidgetItem(name))
+ self.movej_table.setItem(row, 2, QtWidgets.QTableWidgetItem("0.0"))
+ self.movej_table.setItem(row, 3, QtWidgets.QTableWidgetItem("0.0"))
+ self.movej_table.horizontalHeader().setStretchLastSection(True)
+ self._fit_table_height(self.movej_table)
+ self.movej_table.itemChanged.connect(self._on_movej_angle_changed)
+ return self.movej_table
+
+ def _fit_table_height(self, table: QtWidgets.QTableWidget) -> None:
+ header_height = table.horizontalHeader().height()
+ row_height = table.verticalHeader().defaultSectionSize()
+ total = header_height + row_height * table.rowCount() + 2
+ table.setMinimumHeight(total)
+
+ def _append_log(self, text: str) -> None:
+ if hasattr(self, "log") and self.log is not None:
+ self.log.append(text.rstrip())
+
+ def _base_args(self) -> list[str]:
+ return [
+ "-m",
+ "ui.run_action",
+ "--host",
+ self.host_edit.text().strip(),
+ "--port",
+ str(self.port_edit.value()),
+ "--device-id",
+ self.device_edit.text().strip(),
+ ]
+
+ def _start_process(self, args: list[str], keep: bool = False) -> QtCore.QProcess:
+ proc = QtCore.QProcess(self)
+ proc.setProgram(sys.executable)
+ proc.setArguments(args)
+ proc.setWorkingDirectory(str(Path(__file__).resolve().parents[1]))
+ proc.setProcessChannelMode(QtCore.QProcess.MergedChannels)
+ proc.readyReadStandardOutput.connect(
+ lambda p=proc: self._append_log(p.readAllStandardOutput().data().decode("utf-8", "ignore"))
+ )
+ proc.finished.connect(
+ lambda code, status: self._append_log(f"[exit] code={code} status={int(status)}")
+ )
+ proc.start()
+ if keep:
+ return proc
+ proc.finished.connect(proc.deleteLater)
+ return proc
+
+ def _on_movej(self) -> None:
+ joint_list = self._get_selected_movej_joints()
+
+ if not joint_list:
+ self._append_log("[warn] No joints selected for MoveJ.")
+ return
+
+ args = self._base_args() + [
+ "movej",
+ "--vel",
+ str(self.vel_edit.value()),
+ "--acc",
+ str(self.acc_edit.value()),
+ ]
+ self._start_process(args + ["--joint-list", self._encode_joint_list(joint_list)])
+
+ def _on_get_pose(self) -> None:
+ if self._joint_worker and self._joint_worker.isRunning():
+ self._append_log("[info] JointState already running.")
+ return
+ address = f"{self.host_edit.text().strip()}:{self.port_edit.value()}"
+ self._joint_worker = JointStateWorker(
+ address=address,
+ device_id=self.device_edit.text().strip(),
+ interval=float(self.interval_edit.value()),
+ joint_names=self.joint_names,
+ )
+ self._joint_worker.updated.connect(self._update_joint_table)
+ self._joint_worker.log.connect(self._append_log)
+ self._joint_worker.start()
+
+ def _on_stop_pose(self) -> None:
+ if self._joint_worker:
+ self._append_log("[info] Stopping JointState...")
+ self._joint_worker.stop()
+ self._joint_worker = None
+
+ def _on_torque_on(self) -> None:
+ args = self._base_args() + ["torque_on"]
+ self._start_process(args)
+
+ def _on_torque_off(self) -> None:
+ args = self._base_args() + ["torque_off"]
+ self._start_process(args)
+
+ def _on_jointstate_toggle(self, checked: bool) -> None:
+ if checked:
+ self._on_get_pose()
+ else:
+ self._on_stop_pose()
+
+ def _on_getpose_toggle(self, checked: bool) -> None:
+ if checked:
+ self._on_start_get_pose()
+ else:
+ self._on_stop_get_pose()
+
+ def _update_joint_table(self, data: Dict[str, Tuple[float, float]]) -> None:
+ self._last_data = data
+ self._render_joint_table_and_plot()
+
+ def _append_plot(self, name: str, pos: float, vel: float) -> None:
+ t = time.time() - self._plot_start
+ if self._last_plot_time is None or t > self._last_plot_time:
+ self.time_hist.append(t)
+ self._last_plot_time = t
+ self.pos_hist[name].append(pos)
+ self.vel_hist[name].append(vel)
+
+ def _refresh_plot(self, selected: set) -> None:
+ for name in list(self.pos_lines.keys()):
+ if name not in selected:
+ self.pos_lines[name].remove()
+ self.vel_lines[name].remove()
+ del self.pos_lines[name]
+ del self.vel_lines[name]
+
+ for name in selected:
+ if name not in self.pos_lines:
+ (pos_line,) = self.ax_pos.plot([], [], label=name)
+ (vel_line,) = self.ax_vel.plot([], [], label=name)
+ self.pos_lines[name] = pos_line
+ self.vel_lines[name] = vel_line
+ x_data = list(self.time_hist)
+ y_pos_full = list(self.pos_hist[name])
+ y_vel_full = list(self.vel_hist[name])
+ n_pos = min(len(x_data), len(y_pos_full))
+ n_vel = min(len(x_data), len(y_vel_full))
+ if n_pos:
+ self.pos_lines[name].set_data(x_data[-n_pos:], y_pos_full[-n_pos:])
+ else:
+ self.pos_lines[name].set_data([], [])
+ if n_vel:
+ self.vel_lines[name].set_data(x_data[-n_vel:], y_vel_full[-n_vel:])
+ else:
+ self.vel_lines[name].set_data([], [])
+
+ if len(self.time_hist) > 1 and selected:
+ self.ax_pos.set_xlim(self.time_hist[0], self.time_hist[-1])
+ self.ax_vel.set_xlim(self.time_hist[0], self.time_hist[-1])
+ self.ax_pos.relim()
+ self.ax_pos.autoscale_view(scalex=False, scaley=True)
+ self.ax_vel.relim()
+ self.ax_vel.autoscale_view(scalex=False, scaley=True)
+ if selected:
+ self.ax_pos.legend(loc="upper right", fontsize=8, ncol=2)
+ self.ax_vel.legend(loc="upper right", fontsize=8, ncol=2)
+ self.canvas.draw_idle()
+
+ def _reset_plot_data(self) -> None:
+ self.time_hist.clear()
+ for name in self.joint_names:
+ self.pos_hist[name].clear()
+ self.vel_hist[name].clear()
+ self._plot_start = time.time()
+ self._last_plot_time = None
+ for line in self.pos_lines.values():
+ line.set_data([], [])
+ for line in self.vel_lines.values():
+ line.set_data([], [])
+ self.canvas.draw_idle()
+
+ def _encode_joint_list(self, joint_list: list[dict]) -> str:
+ return ";".join(f"{j['joint_name']}:{j['rad']}" for j in joint_list)
+
+ def _get_selected_movej_joints(self) -> list[dict]:
+ joint_list = []
+ for row, name in enumerate(self.joint_names):
+ use_item = self.movej_table.item(row, 0)
+ if use_item and use_item.checkState() == QtCore.Qt.Checked:
+ rad_item = self.movej_table.item(row, 3)
+ try:
+ rad = float(rad_item.text()) if rad_item else 0.0
+ except ValueError:
+ rad = 0.0
+ joint_list.append({"joint_name": name, "rad": rad})
+ return joint_list
+
+ def _on_movej_angle_changed(self, item: QtWidgets.QTableWidgetItem) -> None:
+ if self._updating_movej_table:
+ return
+ col = item.column()
+ if col not in (2, 3):
+ return
+ row = item.row()
+ self._updating_movej_table = True
+ try:
+ if col == 2:
+ try:
+ deg = float(item.text())
+ except ValueError:
+ deg = 0.0
+ rad = deg * 3.141592653589793 / 180.0
+ rad_item = self.movej_table.item(row, 3)
+ if rad_item is None:
+ rad_item = QtWidgets.QTableWidgetItem("0.0")
+ self.movej_table.setItem(row, 3, rad_item)
+ rad_item.setText(f"{rad:.6f}")
+ elif col == 3:
+ try:
+ rad = float(item.text())
+ except ValueError:
+ rad = 0.0
+ deg = rad * 180.0 / 3.141592653589793
+ deg_item = self.movej_table.item(row, 2)
+ if deg_item is None:
+ deg_item = QtWidgets.QTableWidgetItem("0.0")
+ self.movej_table.setItem(row, 2, deg_item)
+ deg_item.setText(f"{deg:.3f}")
+ finally:
+ self._updating_movej_table = False
+
+ def _apply_plot_selection(self) -> None:
+ # Deprecated: plot selection now follows right-side Joint Status selection.
+ pass
+
+ def _on_status_selection_changed(self) -> None:
+ selected_rows = {idx.row() for idx in self.table.selectionModel().selectedRows()}
+ self._plot_selected = {self.joint_names[row] for row in selected_rows}
+ self.statusBar().showMessage(f"Selected joints: {len(self._plot_selected)}")
+ self._render_joint_table_and_plot()
+
+ def _on_start_get_pose(self) -> None:
+ if self._pose_worker and self._pose_worker.isRunning():
+ self._append_log("[info] GetPose already running.")
+ return
+ address = f"{self.host_edit.text().strip()}:{self.port_edit.value()}"
+ self._pose_worker = PoseWorker(
+ address=address,
+ device_id=self.device_edit.text().strip(),
+ interval=float(self.interval_edit.value()),
+ base_link=self.base_link_edit.currentText().strip(),
+ ee_link=self.ee_link_edit.currentText().strip(),
+ )
+ self._pose_worker.updated.connect(self._update_pose)
+ self._pose_worker.log.connect(self._append_log)
+ self._pose_worker.start()
+
+ def _on_stop_get_pose(self) -> None:
+ if self._pose_worker:
+ self._append_log("[info] Stopping GetPose...")
+ self._pose_worker.stop()
+ self._pose_worker = None
+
+ def _update_pose(self, data: Dict[str, float]) -> None:
+ for key, value in data.items():
+ if key in self.pose_labels:
+ self.pose_labels[key].setText(f"{key}: {value:.6f}")
+ return
+
+ def _render_joint_table_and_plot(self) -> None:
+ selected = set(self._plot_selected)
+ if not self._last_data:
+ self._last_data = {name: (0.0, 0.0) for name in self.joint_names}
+
+ selected_lines = []
+ for row, name in enumerate(self.joint_names):
+ pos, vel = self._last_data.get(name, (0.0, 0.0))
+ self.table.item(row, 1).setText(f"{pos:.6f}")
+ self.table.item(row, 2).setText(f"{pos * 180.0 / 3.141592653589793:.3f}")
+ self.table.item(row, 3).setText(f"{vel:.6f}")
+
+ if name in selected:
+ selected_lines.append(f"{name}: {pos:.4f} rad, {vel:.4f} rad/s")
+ self._append_plot(name, pos, vel)
+
+ if selected_lines:
+ self.statusBar().showMessage(
+ f"Selected joints: {len(selected)} | " + " | ".join(selected_lines)
+ )
+ else:
+ self.statusBar().showMessage(f"Selected joints: {len(selected)}")
+
+ if self._is_plot_visible():
+ self._refresh_plot(selected)
+ return
+
+ def _is_plot_visible(self) -> bool:
+ return hasattr(self, "center_tabs") and self.center_tabs.currentIndex() == 0
+
+ def _on_center_tab_changed(self, index: int) -> None:
+ if index == 0:
+ self._render_joint_table_and_plot()
+
+
+def main() -> int:
+ app = QtWidgets.QApplication(sys.argv)
+ window = MainWindow()
+ window.show()
+ return app.exec_()
+
+
+if __name__ == "__main__":
+ raise SystemExit(main())
diff --git a/ui/run_action.py b/ui/run_action.py
new file mode 100644
index 0000000..ce738d5
--- /dev/null
+++ b/ui/run_action.py
@@ -0,0 +1,103 @@
+import argparse
+import sys
+
+from clients._path_setup import ensure_paths
+
+ensure_paths()
+
+
+def parse_args() -> argparse.Namespace:
+ parser = argparse.ArgumentParser(description="Run robot client actions.")
+ parser.add_argument(
+ "action",
+ choices=["movej", "get_pose", "torque_on", "torque_off"],
+ help="Action to run",
+ )
+ parser.add_argument("--host", default="192.168.0.222", help="gRPC server host")
+ parser.add_argument("--port", type=int, default=50052, help="gRPC server port")
+ parser.add_argument("--device-id", default="hc01", help="Device ID")
+ parser.add_argument("--interval", type=float, default=0.05, help="Interval seconds")
+ parser.add_argument("--base-link", default="PELVIS_S", help="Base link for pose")
+ parser.add_argument("--ee-link", default="R_FINGER_TIP", help="End-effector link for pose")
+ parser.add_argument("--vel", type=float, default=1.0, help="MoveJ velocity")
+ parser.add_argument("--acc", type=float, default=0.5, help="MoveJ acceleration")
+ parser.add_argument(
+ "--joint-list",
+ default="",
+ help="MoveJ joint list format: JOINT:RAD;JOINT:RAD",
+ )
+ return parser.parse_args()
+
+
+def main() -> int:
+ args = parse_args()
+ address = f"{args.host}:{args.port}"
+
+ if args.action == "movej":
+ from clients.movej_client import MoveJClient, test_joint_list
+
+ joint_list = test_joint_list
+ if args.joint_list:
+ parsed = []
+ for item in args.joint_list.split(";"):
+ if not item:
+ continue
+ if ":" not in item:
+ continue
+ name, rad_str = item.split(":", 1)
+ try:
+ rad = float(rad_str)
+ except ValueError:
+ rad = 0.0
+ parsed.append({"joint_name": name, "rad": rad})
+ if parsed:
+ joint_list = parsed
+
+ client = MoveJClient(address=address)
+ try:
+ client.send(joint_list, vel=args.vel, acc=args.acc, device_id=args.device_id)
+ print(joint_list)
+ finally:
+ client.close()
+ return 0
+
+ if args.action == "get_pose":
+ from clients.get_pose_client import GetPoseClient
+
+ client = GetPoseClient(address=address)
+ try:
+ client.send(
+ interval=args.interval,
+ device_id=args.device_id,
+ base_link=args.base_link,
+ ee_link=args.ee_link,
+ )
+ finally:
+ client.close()
+ return 0
+
+ if args.action == "torque_on":
+ from clients.torque_on_client import TorqueOnClient
+
+ client = TorqueOnClient(address=address)
+ try:
+ client.send(device_id=args.device_id)
+ finally:
+ client.close()
+ return 0
+
+ if args.action == "torque_off":
+ from clients.torque_off_client import TorqueOffClient
+
+ client = TorqueOffClient(address=address)
+ try:
+ client.send(device_id=args.device_id)
+ finally:
+ client.close()
+ return 0
+
+ return 1
+
+
+if __name__ == "__main__":
+ sys.exit(main())