lgv/grpc_client
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

feat: add ui

Browse Source
This commit is contained in:
lgv 2026-02-25 06:23:32 +08:00
parent 6b9bf3be30
commit 83ae42f019
70 changed files with 2867 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
.idea/grpc_client.iml generated
View File

@ -5,7 +5,7 @@
<sourceFolder url="file://$MODULE_DIR$/generated" isTestSource="false" /> <sourceFolder url="file://$MODULE_DIR$/generated" isTestSource="false" />
<excludeFolder url="file://$MODULE_DIR$/.venv" /> <excludeFolder url="file://$MODULE_DIR$/.venv" />
</content> </content>
<orderEntry type="jdk" jdkName="grpc_client" jdkType="Python SDK" /> <orderEntry type="jdk" jdkName="cmvr-base" jdkType="Python SDK" />
<orderEntry type="sourceFolder" forTests="false" /> <orderEntry type="sourceFolder" forTests="false" />
</component> </component>
</module> </module>

2
.idea/misc.xml generated
View File

@ -3,5 +3,5 @@
<component name="Black"> <component name="Black">
<option name="sdkName" value="Python 3.8 (grpc_client)" /> <option name="sdkName" value="Python 3.8 (grpc_client)" />
</component> </component>
<component name="ProjectRootManager" version="2" project-jdk-name="grpc_client" project-jdk-type="Python SDK" /> <component name="ProjectRootManager" version="2" project-jdk-name="cmvr-base" project-jdk-type="Python SDK" />
</project> </project>

8
README.md
View File

@ -34,11 +34,17 @@ python -m clients.save_joint_to_csv
python -m clients.http_server python -m clients.http_server
``` ```
## UI (PyQt)
```bash
python -m ui.app
```
## Runtime parameters ## Runtime parameters
- gRPC server address/port: set in `clients/base_client.py` (default `192.168.0.222:50052`). - 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`). - 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. - `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.

BIN
clients/__pycache__/get_pose_client.cpython-39.pyc Normal file
View File

Binary file not shown.

BIN
clients/__pycache__/movej_client.cpython-39.pyc
View File

Binary file not shown.

BIN
clients/__pycache__/torque_off_client.cpython-39.pyc Normal file
View File

Binary file not shown.

BIN
clients/__pycache__/torque_on_client.cpython-39.pyc Normal file
View File

Binary file not shown.

39
clients/movej_client.py
View File

@ -264,13 +264,13 @@ hello_r_joint_list = [
test_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_R', 'rad': 0},
# {'joint_name': 'R_SHOULDER_Y', 'rad': 0}, {'joint_name': 'R_SHOULDER_Y', 'rad': 0},
# {'joint_name': 'R_ELBOW_R', 'rad':0}, {'joint_name': 'R_ELBOW_R', 'rad':0},
# {'joint_name': 'R_WRIST_P', 'rad': 0}, {'joint_name': 'R_WRIST_P', 'rad': 0},
# {'joint_name': 'R_WRIST_Y', 'rad': 0}, {'joint_name': 'R_WRIST_Y', 'rad': 0},
# {'joint_name': 'R_WRIST_R', '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") hand_client.set_angles(hand_open, device_id="hand2")
client.send(init_joint_list, vel=1.0, acc=1.0) client.send(init_joint_list, vel=1.0, acc=1.0)
client.send(hello_l_joint_list, vel=2.0, acc=2.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) time.sleep(0.1)
client.send(init_joint_list, vel=1.5, acc=1.5) client.send(init_joint_list, vel=1.5, acc=1.5)
hand_client.set_angles(hand_open, device_id="hand2") 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") hand_client.set_angles(hand_touch, device_id="hand2")
client.send(init_joint_list, vel=2.0, acc=2.0) client.send(init_joint_list, vel=2.0, acc=2.0)
client.send(head_screen_joint_list, vel=1.0, acc=1.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(head_front_joint_list, vel=1.0, acc=1.0)
client.send(init_joint_list, vel=2.0, acc=2.0) client.send(init_joint_list, vel=2.0, acc=2.0)
hand_client.set_angles(hand_open, device_id="hand2") 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 # init_joint_list
hand_client.set_angles(hand_touch, device_id="hand2") hand_client.set_angles(hand_touch, device_id="hand2")
client.send(init_joint_list, vel=2.0, acc=2.0) 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(head_front_joint_list, vel=1.0, acc=1.0)
client.send(init_joint_list, vel=2.0, acc=2.0) client.send(init_joint_list, vel=2.0, acc=2.0)
hand_client.set_angles(hand_open, device_id="hand2") hand_client.set_angles(hand_open, device_id="hand2")
if created:
hand_client.close()
def action_drive_economy(client): def action_drive_economy(client):
# client.send(head_screen_joint_list, vel=1.0, acc=1.0) # client.send(head_screen_joint_list, vel=1.0, acc=1.0)
# client.send(start_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(head_front_joint_list, vel=1.0, acc=1.0)
# client.send(init_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__": if __name__ == "__main__":
# Initialize client # Initialize client
client = MoveJClient()
hand_client = DexHandClient()
# action_air(client) # action_air(client)
# action_hello(client) # action_hello(client)
client.send(test_joint_list, vel=1.0, acc=1.0) client.send(test_joint_list, vel=1.0, acc=1.0)
@ -398,4 +412,5 @@ if __name__ == "__main__":
# Close client # Close client
hand_client.close()
client.close() client.close()

2
requirements.txt
View File

@ -4,5 +4,7 @@ grpcio-tools==1.76.0
matplotlib==3.9.4 matplotlib==3.9.4
protobuf==6.33.2 protobuf==6.33.2
pydantic==2.12.5 pydantic==2.12.5
PyQt5==5.15.11
requests==2.32.5 requests==2.32.5
yourdfpy
uvicorn==0.38.0 uvicorn==0.38.0

660
robot_model/xiaoyan_description/dual_arm.urdf Normal file
View File

@ -0,0 +1,660 @@
<?xml version="1.0" encoding="utf-8"?>
<robot name="dual_arm">
<!-- <mujoco>-->
<!-- <compiler-->
<!-- meshdir="meshes"-->
<!-- balanceinertia="true"-->
<!-- discardvisual="false" />-->
<!-- </mujoco>-->
<!-- <link name="base_link">-->
<!-- <visual>-->
<!-- <origin xyz="0 0 0.6" rpy="0 0 0"/>-->
<!-- <geometry>-->
<!-- <cylinder radius="0.05" length="1.2"/>-->
<!-- </geometry>-->
<!-- <material name="gray">-->
<!-- <color rgba="0.5 0.5 0.5 1.0"/>-->
<!-- </material>-->
<!-- </visual>-->
<!-- <collision>-->
<!-- <origin xyz="0 0 0.6" rpy="0 0 0"/>-->
<!-- <geometry>-->
<!-- <cylinder radius="0.05" length="1.2"/>-->
<!-- </geometry>-->
<!-- </collision>-->
<!-- <inertial>-->
<!-- <origin xyz="0 0 0" rpy="0 0 0"/>-->
<!-- <mass value="25.4469"/>-->
<!-- <inertia-->
<!-- ixx="3.06953"-->
<!-- ixy="0.0"-->
<!-- ixz="0.0"-->
<!-- iyy="3.06953"-->
<!-- iyz="0.0"-->
<!-- izz="0.03181"/>-->
<!-- </inertial>-->
<!-- </link>-->
<!-- <joint name="base_fixed" type="fixed">-->
<!-- <origin rpy="0 0 0" xyz="0 0 1.2"/>-->
<!-- <parent link="base_link"/>-->
<!-- <child link="PELVIS_S"/>-->
<!-- </joint>-->
<link name="PELVIS_S">
<inertial>
<origin xyz="3.78529087037144E-05 3.81781425684836E-07 0.0386396273530852" rpy="0 0 0" />
<mass value="2.10624590271277" />
<inertia
ixx="0.00165706865324979"
ixy="3.13663044821662E-09"
ixz="6.84209533216046E-07"
iyy="0.00136736758630241"
iyz="-1.01014607878816E-10"
izz="0.00168295663089359" />
</inertial>
<visual>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<mesh filename="meshes/PELVIS_S.STL" />
</geometry>
<material name="">
<color rgba="0.698039215686274 0.698039215686274 0.698039215686274 1" />
</material>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<mesh filename="meshes/PELVIS_S.STL" />
</geometry>
</collision>
</link>
<link name="L_SHOULDER_P_S">
<inertial>
<origin xyz="-0.00982258725282134 0.0704593083751867 1.15261874861217E-06" rpy="0 0 0" />
<mass value="0.880737519698403" />
<inertia
ixx="0.000583190308378148"
ixy="-1.53153074560473E-05"
ixz="6.58466471754072E-09"
iyy="0.000445532440067177"
iyz="6.37001404038516E-09"
izz="0.000465648071069952" />
</inertial>
<visual>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<mesh filename="meshes/L_SHOULDER_P_S.STL" />
</geometry>
<material name="">
<color rgba="0.898039215686275 0.917647058823529 0.929411764705882 1" />
</material>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<mesh filename="meshes/L_SHOULDER_P_S.STL" />
</geometry>
</collision>
</link>
<joint name="L_SHOULDER_P" type="revolute">
<origin xyz="0 0.0945 0.042" rpy="0 0 0" />
<parent link="PELVIS_S" />
<child link="L_SHOULDER_P_S" />
<axis xyz="0 1 0" />
<limit lower="-1.57" upper="1.57" effort="120" velocity="3.351" />
</joint>
<link name="L_SHOULDER_R_S">
<inertial>
<origin xyz="-0.0346025282975857 0.091739327988169 -1.67085072999562E-08" rpy="0 0 0" />
<mass value="0.594788424442483" />
<inertia
ixx="0.000380970396712656"
ixy="4.80751844573946E-05"
ixz="-1.34913746586865E-11"
iyy="0.000320962178597474"
iyz="-1.00039763442409E-09"
izz="0.000414771047901155" />
</inertial>
<visual>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<mesh filename="meshes/L_SHOULDER_R_S.STL" />
</geometry>
<material name="">
<color rgba="0.890196078431372 0.890196078431372 0.913725490196078 1" />
</material>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<mesh filename="meshes/L_SHOULDER_R_S.STL" />
</geometry>
</collision>
</link>
<joint name="L_SHOULDER_R" type="revolute">
<origin xyz="0.035 0.0765 0" rpy="0 0 0" />
<parent link="L_SHOULDER_P_S" />
<child link="L_SHOULDER_R_S" />
<axis xyz="1 0 0" />
<limit lower="-2" upper="2" effort="120" velocity="3.351" />
</joint>
<link name="L_SHOULDER_Y_S">
<inertial>
<origin xyz="-0.00440976862014801 0.0863620459174068 9.50748668682166E-09"
rpy="0 0 0" />
<mass value="0.563406026626801" />
<inertia
ixx="0.000327003834287552"
ixy="-1.8057438966771E-05"
ixz="7.28778136267901E-10"
iyy="0.000213830709361531"
iyz="1.49273668517817E-10"
izz="0.000297341029639189" />
</inertial>
<visual>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<mesh filename="meshes/L_SHOULDER_Y_S.STL" />
</geometry>
<material name="">
<color rgba="0.890196078431372 0.890196078431372 0.913725490196078 1" />
</material>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<mesh filename="meshes/L_SHOULDER_Y_S.STL" />
</geometry>
</collision>
</link>
<joint name="L_SHOULDER_Y" type="revolute">
<origin xyz="-0.035 0.1475 0" rpy="0 0 0" />
<parent link="L_SHOULDER_R_S" />
<child link="L_SHOULDER_Y_S" />
<axis xyz="0 1 0" />
<limit lower="-2.18" upper="0" effort="80" velocity="3.8758" />
</joint>
<link name="L_ELBOW_R_S">
<inertial>
<origin xyz="-0.0335624237303443 0.0603199964106564 2.99655911639718E-07" rpy="0 0 0" />
<mass value="0.393571904406493" />
<inertia
ixx="0.00017277119386253"
ixy="2.34549801867355E-05"
ixz="-1.90560556659271E-09"
iyy="0.000155340245267897"
iyz="8.82493073600007E-09"
izz="0.00018104232734159" />
</inertial>
<visual>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<mesh filename="meshes/L_ELBOW_R_S.STL" />
</geometry>
<material name="">
<color rgba="0.890196078431372 0.890196078431372 0.913725490196078 1" />
</material>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<mesh filename="meshes/L_ELBOW_R_S.STL" />
</geometry>
</collision>
</link>
<joint name="L_ELBOW_R" type="revolute">
<origin xyz="0.034 0.1025 0" rpy="0 0 0" />
<parent link="L_SHOULDER_Y_S" />
<child link="L_ELBOW_R_S" />
<axis xyz="1 0 0" />
<limit lower="-2.05" upper="0" effort="50" velocity="4.71" />
</joint>
<link name="L_WRIST_P_S">
<inertial>
<origin xyz="-1.39659173115092E-10 0.0675972604744393 0.019200551565574" rpy="0 0 0" />
<mass value="0.442332465815496" />
<inertia
ixx="0.000476754055455895"
ixy="-4.61505824713347E-15"
ixz="6.03808112272229E-18"
iyy="0.000103466585850499"
iyz="-4.88426705501198E-05"
izz="0.000482956345754213" />
</inertial>
<visual>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<mesh filename="meshes/L_WRIST_P_S.STL" />
</geometry>
<material name="">
<color rgba="0.698039215686274 0.698039215686274 0.698039215686274 1" />
</material>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<mesh filename="meshes/L_WRIST_P_S.STL" />
</geometry>
</collision>
</link>
<joint name="L_WRIST_P" type="revolute">
<origin xyz="-0.034 0.0965 0" rpy="0 0 0" />
<parent link="L_ELBOW_R_S" />
<child link="L_WRIST_P_S" />
<axis xyz="0 1 0" />
<limit lower="0" upper="3.14" effort="50" velocity="4.71" />
</joint>
<link name="L_WRIST_Y_S">
<inertial>
<origin xyz="-0.00464135887331864 -5.06426789392833E-10 -0.0341253783666609" rpy="0 0 0" />
<mass value="0.23573847772002" />
<inertia
ixx="5.10686940455785E-05"
ixy="7.45706654358429E-16"
ixz="6.2619568923368E-06"
iyy="6.00636019624519E-05"
iyz="1.27989654155383E-16"
izz="5.32182903609189E-05" />
</inertial>
<visual>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<mesh filename="meshes/L_WRIST_Y_S.STL" />
</geometry>
<material name="">
<color rgba="0.647058823529412 0.619607843137255 0.588235294117647 1" />
</material>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<mesh filename="meshes/L_WRIST_Y_S.STL" />
</geometry>
</collision>
</link>
<joint name="L_WRIST_Y" type="revolute">
<origin xyz="0 0.1525 0.039" rpy="0 0 0" />
<parent link="L_WRIST_P_S" />
<child link="L_WRIST_Y_S" />
<axis xyz="0 0 1" />
<limit lower="-0.78" upper="0.78" effort="50" velocity="0.79" />
</joint>
<link name="L_WRIST_R_S">
<inertial>
<origin xyz="-0.0161477357620243 0.0955046558857351 -0.00499392489444117" rpy="0 0 0" />
<mass value="0.504894112043562" />
<inertia
ixx="0.000186833711781125"
ixy="-3.99488705622731E-06"
ixz="-1.51920720398336E-06"
iyy="0.000179960980467837"
iyz="3.6992669535716E-06"
izz="0.000280756101568308" />
</inertial>
<visual>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<mesh filename="meshes/L_WRIST_R_S.STL" />
</geometry>
<material name="">
<color rgba="0.890196078431372 0.890196078431372 0.913725490196078 1" />
</material>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<mesh filename="meshes/L_WRIST_R_S.STL" />
</geometry>
</collision>
</link>
<joint name="L_WRIST_R" type="revolute">
<origin xyz="0.0258 0 -0.039" rpy="0 0 0" />
<parent link="L_WRIST_Y_S" />
<child link="L_WRIST_R_S" />
<axis xyz="1 0 0" />
<limit lower="-1.57" upper="0.26" effort="50" velocity="4.71" />
</joint>
<link name="R_SHOULDER_P_S">
<inertial>
<origin xyz="-0.00982258725282141 -0.0704593093873431 -1.15069920925137E-06" rpy="0 0 0" />
<mass value="0.880737519698404" />
<inertia
ixx="0.000583190308378149"
ixy="1.53153074560471E-05"
ixz="-6.58466471736542E-09"
iyy="0.000445532440067178"
iyz="6.37001403998779E-09"
izz="0.000465648071069953" />
</inertial>
<visual>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<mesh filename="meshes/R_SHOULDER_P_S.STL" />
</geometry>
<material name="">
<color rgba="0.890196078431372 0.890196078431372 0.913725490196078 1" />
</material>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<mesh filename="meshes/R_SHOULDER_P_S.STL" />
</geometry>
</collision>
</link>
<joint name="R_SHOULDER_P" type="revolute">
<origin xyz="0 -0.0945 0.042" rpy="0 0 0" />
<parent link="PELVIS_S" />
<child link="R_SHOULDER_P_S" />
<axis xyz="0 -1 0" />
<limit lower="-3.14" upper="3.14" effort="120" velocity="3.351" />
</joint>
<link name="R_SHOULDER_R_S">
<inertial>
<origin xyz="-0.0346025282975784 -0.09173932900033 1.86280643132974E-08" rpy="0 0 0" />
<mass value="0.59478842444248" />
<inertia
ixx="0.000380970396712653"
ixy="-4.80751844573946E-05"
ixz="1.34913746041655E-11"
iyy="0.000320962178597472"
iyz="-1.00039763417375E-09"
izz="0.000414771047901152" />
</inertial>
<visual>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<mesh filename="meshes/R_SHOULDER_R_S.STL" />
</geometry>
<material name="">
<color rgba="0.890196078431372 0.890196078431372 0.913725490196078 1" />
</material>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<mesh filename="meshes/R_SHOULDER_R_S.STL" />
</geometry>
</collision>
</link>
<joint name="R_SHOULDER_R" type="revolute">
<origin xyz="0.035 -0.0765 0" rpy="0 0 0" />
<parent link="R_SHOULDER_P_S" />
<child link="R_SHOULDER_R_S" />
<axis xyz="1 0 0" />
<limit lower="-0.78" upper="1.57" effort="120" velocity="3.351" />
</joint>
<link name="R_SHOULDER_Y_S">
<inertial>
<origin xyz="-0.00440976862014946 -0.0863620469295699 -7.58791862676134E-09" rpy="0 0 0" />
<mass value="0.563406026626801" />
<inertia
ixx="0.000327003834287551"
ixy="1.80574389667711E-05"
ixz="-7.28778136077729E-10"
iyy="0.000213830709361532"
iyz="1.49273668428957E-10"
izz="0.000297341029639189" />
</inertial>
<visual>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<mesh filename="meshes/R_SHOULDER_Y_S.STL" />
</geometry>
<material
name="">
<color rgba="0.890196078431372 0.890196078431372 0.913725490196078 1" />
</material>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<mesh filename="meshes/R_SHOULDER_Y_S.STL" />
</geometry>
</collision>
</link>
<joint name="R_SHOULDER_Y" type="revolute">
<origin xyz="-0.035 -0.1475 0" rpy="0 0 0" />
<parent link="R_SHOULDER_R_S" />
<child link="R_SHOULDER_Y_S" />
<axis xyz="0 -1 0" />
<limit lower="-3.14" upper="3.14" effort="80" velocity="3.8758" />
</joint>
<link name="R_ELBOW_R_S">
<inertial>
<origin xyz="-0.0335624237303424 -0.0603199974228191 -2.97736340082455E-07" rpy="0 0 0" />
<mass value="0.393571904406492" />
<inertia
ixx="0.000172771193862529"
ixy="-2.34549801867353E-05"
ixz="1.90560556668771E-09"
iyy="0.000155340245267897"
iyz="8.82493073602971E-09"
izz="0.00018104232734159" />
</inertial>
<visual>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<mesh filename="meshes/R_ELBOW_R_S.STL" />
</geometry>
<material name="">
<color rgba="0.890196078431372 0.890196078431372 0.913725490196078 1" />
</material>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<mesh filename="meshes/R_ELBOW_R_S.STL" />
</geometry>
</collision>
</link>
<joint name="R_ELBOW_R" type="revolute">
<origin xyz="0.034 -0.1025 0" rpy="0 0 0" />
<parent link="R_SHOULDER_Y_S" />
<child link="R_ELBOW_R_S" />
<axis xyz="1 0 0" />
<limit lower="0" upper="2.05" effort="80" velocity="3.8758" />
</joint>
<link name="R_WRIST_P_S">
<inertial>
<origin xyz="-1.39656577968772E-10 -0.0675972614865965 0.0192005515655728" rpy="0 0 0" />
<mass value="0.442332465815497" />
<inertia
ixx="0.000476754055455896"
ixy="-4.61462558956188E-15"
ixz="-5.91762926004267E-18"
iyy="0.0001034665858505"
iyz="4.88426705501212E-05"
izz="0.000482956345754214" />
</inertial>
<visual>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<mesh filename="meshes/R_WRIST_P_S.STL" />
</geometry>
<material name="">
<color rgba="0.647058823529412 0.619607843137255 0.588235294117647 1" />
</material>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<mesh filename="meshes/R_WRIST_P_S.STL" />
</geometry>
</collision>
</link>
<joint name="R_WRIST_P" type="revolute">
<origin xyz="-0.034 -0.0965 0" rpy="0 0 0" />
<parent link="R_ELBOW_R_S" />
<child link="R_WRIST_P_S" />
<axis xyz="0 -1 0" />
<limit lower="-3.14" upper="3.14" effort="50" velocity="4.71" />
</joint>
<link name="R_WRIST_Y_S">
<inertial>
<origin xyz="-0.00464135887330083 -5.06426456325926E-10 -0.0341253783666614" rpy="0 0 0" />
<mass value="0.235738477720019" />
<inertia
ixx="5.1068694045578E-05"
ixy="7.45771487459288E-16"
ixz="6.26195689233664E-06"
iyy="6.00636019624515E-05"
iyz="1.27828046342987E-16"
izz="5.32182903609188E-05" />
</inertial>
<visual>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<mesh filename="meshes/R_WRIST_Y_S.STL" />
</geometry>
<material name="">
<color rgba="0.647058823529412 0.619607843137255 0.588235294117647 1" />
</material>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<mesh filename="meshes/R_WRIST_Y_S.STL" />
</geometry>
</collision>
</link>
<joint name="R_WRIST_Y" type="revolute">
<origin xyz="0 -0.1525 0.039" rpy="0 0 0" />
<parent link="R_WRIST_P_S" />
<child link="R_WRIST_Y_S" />
<axis xyz="0 0 1" />
<limit lower="-0.78" upper="0.78" effort="50" velocity="0.79" />
</joint>
<link name="R_WRIST_R_S">
<inertial>
<origin xyz="-0.0201642233698132 -0.11074968386657 -0.00598955339232021" rpy="0 0 0" />
<mass value="0.504366058218534" />
<inertia
ixx="0.000185559065855467"
ixy="5.75889766751509E-06"
ixz="2.40683898454438E-06"
iyy="0.000131246007872084"
iyz="1.60533277040148E-06"
izz="0.000271800951396149" />
</inertial>
<visual>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<mesh filename="meshes/R_WRIST_R_S.STL" />
</geometry>
<material
name="">
<color rgba="0.890196078431372 0.890196078431372 0.913725490196078 1" />
</material>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<mesh filename="meshes/R_WRIST_R_S.STL" />
</geometry>
</collision>
</link>
<joint name="R_WRIST_R" type="revolute">
<origin xyz="0.03 0 -0.039" rpy="0 0 0" />
<parent link="R_WRIST_Y_S" />
<child link="R_WRIST_R_S" />
<axis xyz="1 0 0" />
<limit lower="-0.26" upper="1.57" effort="50" velocity="4.71" />
</joint>
<link name="R_FINGER_TIP">
<inertial>
<origin xyz="0 0 0" rpy="0 0 0"/>
<mass value="0"/>
<inertia ixx="1e-6" ixy="0" ixz="0" iyy="1e-6" iyz="0" izz="1e-6"/>
</inertial>
<visual>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<sphere radius="0.004"/>
</geometry>
<material name="">
<color rgba="0 1 1 1"/> <!-- 绿色 -->
</material>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<sphere radius="0.005"/>
</geometry>
</collision>
</link>
<joint name="R_FINGER_TIP_FIXED" type="fixed">
<origin xyz="0.00684256 -0.284077 0.00801525" rpy="0 0 0"/>
<parent link="R_WRIST_R_S"/>
<child link="R_FINGER_TIP"/>
<axis xyz="0 0 1"/>
</joint>
<link name="R_CAM">
<inertial>
<origin xyz="0 0 0" rpy="0 0 0"/>
<mass value="0"/>
<inertia ixx="1e-6" ixy="0" ixz="0" iyy="1e-6" iyz="0" izz="1e-6"/>
</inertial>
<visual>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<sphere radius="0.004"/>
</geometry>
<material name="">
<color rgba="0 1 0 1"/> <!-- 绿色 -->
</material>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<sphere radius="0.005"/>
</geometry>
</collision>
</link>
<joint name="R_CAM_FIXED" type="fixed">
<origin xyz="-0.01212 -0.17655 0.07506" rpy="-1.5707963267 0 3.1415926535"/>
<parent link="R_WRIST_R_S"/>
<child link="R_CAM"/>
<axis xyz="0 0 1"/>
</joint>
</robot>

206
robot_model/xiaoyan_description/dual_arm.xml Normal file
View File

@ -0,0 +1,206 @@
<mujoco model="dual_arm">
<compiler angle="radian" meshdir="meshes/"/>
<default>
<joint damping="10" armature="0.001"/>
</default>
<asset>
<mesh name="PELVIS_S" file="PELVIS_S.STL"/>
<mesh name="L_SHOULDER_P_S" file="L_SHOULDER_P_S.STL"/>
<mesh name="L_SHOULDER_R_S" file="L_SHOULDER_R_S.STL"/>
<mesh name="L_SHOULDER_Y_S" file="L_SHOULDER_Y_S.STL"/>
<mesh name="L_ELBOW_R_S" file="L_ELBOW_R_S.STL"/>
<mesh name="L_WRIST_P_S" file="L_WRIST_P_S.STL"/>
<mesh name="L_WRIST_Y_S" file="L_WRIST_Y_S.STL"/>
<mesh name="L_WRIST_R_S" file="L_WRIST_R_S.STL"/>
<mesh name="R_SHOULDER_P_S" file="R_SHOULDER_P_S.STL"/>
<mesh name="R_SHOULDER_R_S" file="R_SHOULDER_R_S.STL"/>
<mesh name="R_SHOULDER_Y_S" file="R_SHOULDER_Y_S.STL"/>
<mesh name="R_ELBOW_R_S" file="R_ELBOW_R_S.STL"/>
<mesh name="R_WRIST_P_S" file="R_WRIST_P_S.STL"/>
<mesh name="R_WRIST_Y_S" file="R_WRIST_Y_S.STL"/>
<mesh name="R_WRIST_R_S" file="R_WRIST_R_S.STL"/>
<!-- 天空背景，可要可不要 -->
<texture name="skybox" type="skybox"
builtin="gradient"
rgb1="0.4 0.6 0.8"
rgb2="0 0 0"
width="512" height="512"/>
<!-- 蓝色方格地面纹理：builtin="checker" -->
<texture name="grid"
type="2d"
builtin="checker"
width="512" height="512"
rgb1="0.2 0.3 0.4"
rgb2="0.1 0.2 0.3"
mark="cross"
markrgb="0.8 0.8 0.8"/>
<material name="grid_floor"
texture="grid"
texrepeat="5 5"
rgba="1 1 1.0 1"
emission="0.9"
specular="0.5"
shininess="1"
reflectance="0.3"/>
</asset>
<worldbody>
<geom size="0.05 0.6" pos="0 0 0.6" type="cylinder" contype="0" conaffinity="0" group="1" density="0"/>
<geom size="0.05 0.6" pos="0 0 0.6" type="cylinder"/>
<geom pos="0 0 1.2" quat="1 0 0 0" type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.698039 0.698039 0.698039 1" mesh="PELVIS_S"/>
<geom pos="0 0 1.2" quat="1 0 0 0" type="mesh" rgba="0.698039 0.698039 0.698039 1" mesh="PELVIS_S"/>
<body name="L_SHOULDER_P_S" pos="0 0.0945 1.242">
<inertial pos="-0.00982259 0.0704593 1.15262e-06" quat="0.706163 0.705933 -0.0386962 -0.0386741" mass="0.880738" diaginertia="0.000584874 0.000465648 0.000443849"/>
<joint name="L_SHOULDER_P" pos="0 0 0" axis="0 1 0" range="-1.57 0.26" actuatorfrcrange="-120 120"/>
<geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.898039 0.917647 0.929412 1" mesh="L_SHOULDER_P_S"/>
<geom type="mesh" rgba="0.898039 0.917647 0.929412 1" mesh="L_SHOULDER_P_S"/>
<body name="L_SHOULDER_R_S" pos="0.035 0.0765 0">
<inertial pos="-0.0346025 0.0917393 -1.67085e-08" quat="0.358778 0.609303 -0.358823 0.609323" mass="0.594788" diaginertia="0.000414771 0.000407636 0.000294296"/>
<joint name="L_SHOULDER_R" pos="0 0 0" axis="1 0 0" range="-1.57 0.78" actuatorfrcrange="-120 120"/>
<geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.890196 0.890196 0.913725 1" mesh="L_SHOULDER_R_S"/>
<geom type="mesh" rgba="0.890196 0.890196 0.913725 1" mesh="L_SHOULDER_R_S"/>
<body name="L_SHOULDER_Y_S" pos="-0.035 0.1475 0">
<inertial pos="-0.00440977 0.086362 9.50749e-09" quat="0.705001 0.704998 -0.054559 -0.0545441" mass="0.563406" diaginertia="0.000329815 0.000297341 0.000211019"/>
<joint name="L_SHOULDER_Y" pos="0 0 0" axis="0 1 0" range="-3.14 3.14" actuatorfrcrange="-80 80"/>
<geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.890196 0.890196 0.913725 1" mesh="L_SHOULDER_Y_S"/>
<geom type="mesh" rgba="0.890196 0.890196 0.913725 1" mesh="L_SHOULDER_Y_S"/>
<body name="L_ELBOW_R_S" pos="0.034 0.1025 0">
<inertial pos="-0.0335624 0.06032 2.99656e-07" quat="0.674756 0.674714 0.211333 0.211667" mass="0.393572" diaginertia="0.000189078 0.000181042 0.000139034"/>
<joint name="L_ELBOW_R" pos="0 0 0" axis="1 0 0" range="-2.05 0" actuatorfrcrange="-50 50"/>
<geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.890196 0.890196 0.913725 1" mesh="L_ELBOW_R_S"/>
<geom type="mesh" rgba="0.890196 0.890196 0.913725 1" mesh="L_ELBOW_R_S"/>
<body name="L_WRIST_P_S" pos="-0.034 0.0965 0">
<inertial pos="-1.39659e-10 0.0675973 0.0192006" quat="0.467537 0.530481 -0.530481 0.467537" mass="0.442332" diaginertia="0.000489142 0.000476754 9.72811e-05"/>
<joint name="L_WRIST_P" pos="0 0 0" axis="0 1 0" range="-3.14 3.14" actuatorfrcrange="-50 50"/>
<geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.698039 0.698039 0.698039 1" mesh="L_WRIST_P_S"/>
<geom type="mesh" rgba="0.698039 0.698039 0.698039 1" mesh="L_WRIST_P_S"/>
<body name="L_WRIST_Y_S" pos="0 0.1525 0.039">
<inertial pos="-0.00464136 -5.06427e-10 -0.0341254" quat="0.298107 0.641196 0.641196 0.298107" mass="0.235738" diaginertia="6.00636e-05 5.8497e-05 4.579e-05"/>
<joint name="L_WRIST_Y" pos="0 0 0" axis="0 0 1" range="-0.78 0.78" actuatorfrcrange="-50 50"/>
<geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.647059 0.619608 0.588235 1" mesh="L_WRIST_Y_S"/>
<geom type="mesh" rgba="0.647059 0.619608 0.588235 1" mesh="L_WRIST_Y_S"/>
<body name="L_WRIST_R_S" pos="0.0258 0 -0.039">
<inertial pos="-0.0161477 0.0955047 -0.00499392" quat="0.595488 0.369507 -0.591676 0.39847" mass="0.504894" diaginertia="0.000280921 0.000188575 0.000178055"/>
<joint name="L_WRIST_R" pos="0 0 0" axis="1 0 0" range="-1.57 0.26" actuatorfrcrange="-50 50"/>
<geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.890196 0.890196 0.913725 1" mesh="L_WRIST_R_S"/>
<geom type="mesh" rgba="0.890196 0.890196 0.913725 1" mesh="L_WRIST_R_S"/>
</body>
</body>
</body>
</body>
</body>
</body>
</body>
<body name="R_SHOULDER_P_S" pos="0 -0.0945 1.242">
<inertial pos="-0.00982259 -0.0704593 -1.1507e-06" quat="0.706163 0.705933 0.0386962 0.0386741" mass="0.880738" diaginertia="0.000584874 0.000465648 0.000443849"/>
<joint name="R_SHOULDER_P" pos="0 0 0" axis="0 -1 0" range="-3.14 3.14" actuatorfrcrange="-120 120"/>
<geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.890196 0.890196 0.913725 1" mesh="R_SHOULDER_P_S"/>
<geom type="mesh" rgba="0.890196 0.890196 0.913725 1" mesh="R_SHOULDER_P_S"/>
<body name="R_SHOULDER_R_S" pos="0.035 -0.0765 0">
<inertial pos="-0.0346025 -0.0917393 1.86281e-08" quat="0.609323 0.358823 -0.609303 0.358778" mass="0.594788" diaginertia="0.000414771 0.000407636 0.000294296"/>
<joint name="R_SHOULDER_R" pos="0 0 0" axis="1 0 0" range="-0.78 1.57" actuatorfrcrange="-120 120"/>
<geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.890196 0.890196 0.913725 1" mesh="R_SHOULDER_R_S"/>
<geom type="mesh" rgba="0.890196 0.890196 0.913725 1" mesh="R_SHOULDER_R_S"/>
<body name="R_SHOULDER_Y_S" pos="-0.035 -0.1475 0">
<inertial pos="-0.00440977 -0.086362 -7.58792e-09" quat="0.705001 0.704998 0.054559 0.0545441" mass="0.563406" diaginertia="0.000329815 0.000297341 0.000211019"/>
<joint name="R_SHOULDER_Y" pos="0 0 0" axis="0 -1 0" range="-3.14 3.14" actuatorfrcrange="-80 80"/>
<geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.890196 0.890196 0.913725 1" mesh="R_SHOULDER_Y_S"/>
<geom type="mesh" rgba="0.890196 0.890196 0.913725 1" mesh="R_SHOULDER_Y_S"/>
<body name="R_ELBOW_R_S" pos="0.034 -0.1025 0">
<inertial pos="-0.0335624 -0.06032 -2.97736e-07" quat="0.674756 0.674714 -0.211333 -0.211667" mass="0.393572" diaginertia="0.000189078 0.000181042 0.000139034"/>
<joint name="R_ELBOW_R" pos="0 0 0" axis="1 0 0" range="0 2.05" actuatorfrcrange="-80 80"/>
<geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.890196 0.890196 0.913725 1" mesh="R_ELBOW_R_S"/>
<geom type="mesh" rgba="0.890196 0.890196 0.913725 1" mesh="R_ELBOW_R_S"/>
<body name="R_WRIST_P_S" pos="-0.034 -0.0965 0">
<inertial pos="-1.39657e-10 -0.0675973 0.0192006" quat="0.530481 0.467537 -0.467537 0.530481" mass="0.442332" diaginertia="0.000489142 0.000476754 9.72811e-05"/>
<joint name="R_WRIST_P" pos="0 0 0" axis="0 -1 0" range="-3.14 3.14" actuatorfrcrange="-50 50"/>
<geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.647059 0.619608 0.588235 1" mesh="R_WRIST_P_S"/>
<geom type="mesh" rgba="0.647059 0.619608 0.588235 1" mesh="R_WRIST_P_S"/>
<body name="R_WRIST_Y_S" pos="0 -0.1525 0.039">
<inertial pos="-0.00464136 -5.06426e-10 -0.0341254" quat="0.298107 0.641196 0.641196 0.298107" mass="0.235738" diaginertia="6.00636e-05 5.8497e-05 4.579e-05"/>
<joint name="R_WRIST_Y" pos="0 0 0" axis="0 0 1" range="-0.78 0.78" actuatorfrcrange="-50 50"/>
<geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.647059 0.619608 0.588235 1" mesh="R_WRIST_Y_S"/>
<geom type="mesh" rgba="0.647059 0.619608 0.588235 1" mesh="R_WRIST_Y_S"/>
<body name="R_WRIST_R_S" pos="0.03 0 -0.039">
<inertial pos="-0.0201642 -0.11075 -0.00598955" quat="0.483404 0.529786 -0.463203 0.520663" mass="0.504366" diaginertia="0.00027189 0.000186086 0.000130629"/>
<joint name="R_WRIST_R" pos="0 0 0" axis="1 0 0" range="-0.26 1.57" actuatorfrcrange="-50 50"/>
<geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.890196 0.890196 0.913725 1" mesh="R_WRIST_R_S"/>
<geom type="mesh" rgba="0.890196 0.890196 0.913725 1" mesh="R_WRIST_R_S"/>
<geom size="0.004" pos="0.00684256 -0.284077 0.00801525" contype="0" conaffinity="0" group="1" density="0" rgba="0 1 1 1"/>
<geom size="0.005" pos="0.00684256 -0.284077 0.00801525" rgba="0 1 1 1"/>
<geom size="0.004" pos="-0.01212 -0.17655 0.07506" quat="3.17467e-11 -3.17467e-11 -0.707107 0.707107" contype="0" conaffinity="0" group="1" density="0" rgba="0 1 0 1"/>
<geom size="0.005" pos="-0.01212 -0.17655 0.07506" quat="3.17467e-11 -3.17467e-11 -0.707107 0.707107" rgba="0 1 0 1"/>
</body>
</body>
</body>
</body>
</body>
</body>
</body>
<light name="top_light"
mode="track"
directional="true"
diffuse="0.4 0.4 0.4"
specular="0.3 0.3 0.3"
pos="0 0 3"
dir="0 0 -1"/>
<geom name="floor"
type="plane"
pos="0 0 0"
size="0 0 0.05"
material="grid_floor"
condim="3"
friction="1 0.005 0.0001"/>
</worldbody>
<!-- 关节空间 PD 位置控制：ctrl = 目标关节角度 (rad) -->
<actuator>
<!-- 左臂：位置控制 -->
<position name="L_SHOULDER_P_pos" joint="L_SHOULDER_P"
kp="200" ctrlrange="-1.57 0.26"/>
<position name="L_SHOULDER_R_pos" joint="L_SHOULDER_R"
kp="200" ctrlrange="-1.57 0.78"/>
<position name="L_SHOULDER_Y_pos" joint="L_SHOULDER_Y"
kp="200" ctrlrange="-3.14 3.14"/>
<position name="L_ELBOW_R_pos" joint="L_ELBOW_R"
kp="150" ctrlrange="-2.05 0.00"/>
<position name="L_WRIST_P_pos" joint="L_WRIST_P"
kp="80" ctrlrange="-3.14 3.14"/>
<position name="L_WRIST_Y_pos" joint="L_WRIST_Y"
kp="80" ctrlrange="-0.78 0.78"/>
<position name="L_WRIST_R_pos" joint="L_WRIST_R"
kp="80" ctrlrange="-1.57 0.26"/>
<!-- 右臂：位置控制 -->
<position name="R_SHOULDER_P_pos" joint="R_SHOULDER_P"
kp="2000" ctrlrange="-3.14 3.14"/>
<position name="R_SHOULDER_R_pos" joint="R_SHOULDER_R"
kp="2000" ctrlrange="-0.78 1.57"/>
<position name="R_SHOULDER_Y_pos" joint="R_SHOULDER_Y"
kp="2000" ctrlrange="-3.14 3.14"/>
<position name="R_ELBOW_R_pos" joint="R_ELBOW_R"
kp="1500" ctrlrange="0 2.05"/>
<position name="R_WRIST_P_pos" joint="R_WRIST_P"
kp="800" ctrlrange="-3.14 3.14"/>
<position name="R_WRIST_Y_pos" joint="R_WRIST_Y"
kp="800" ctrlrange="-0.78 0.78"/>
<position name="R_WRIST_R_pos" joint="R_WRIST_R"
kp="800" ctrlrange="-0.26 1.57"/>
</actuator>
</mujoco>

347
robot_model/xiaoyan_description/dual_arm_temp.xml Normal file
View File

@ -0,0 +1,347 @@
<mujoco model="dual_arm">
<compiler angle="radian" meshdir="meshes/"/>
<!-- 全局默认：给所有关节一点阻尼和等效转子惯量 -->
<default>
<joint damping="10" armature="0.001"/>
</default>
<!-- 稳定一点的时间步和积分器 -->
<!-- <option timestep="0.001" integrator="RK4"/>-->
<asset>
<mesh name="PELVIS_S" file="PELVIS_S.STL"/>
<mesh name="L_SHOULDER_P_S" file="L_SHOULDER_P_S.STL"/>
<mesh name="L_SHOULDER_R_S" file="L_SHOULDER_R_S.STL"/>
<mesh name="L_SHOULDER_Y_S" file="L_SHOULDER_Y_S.STL"/>
<mesh name="L_ELBOW_R_S" file="L_ELBOW_R_S.STL"/>
<mesh name="L_WRIST_P_S" file="L_WRIST_P_S.STL"/>
<mesh name="L_WRIST_Y_S" file="L_WRIST_Y_S.STL"/>
<mesh name="L_WRIST_R_S" file="L_WRIST_R_S.STL"/>
<mesh name="R_SHOULDER_P_S" file="R_SHOULDER_P_S.STL"/>
<mesh name="R_SHOULDER_R_S" file="R_SHOULDER_R_S.STL"/>
<mesh name="R_SHOULDER_Y_S" file="R_SHOULDER_Y_S.STL"/>
<mesh name="R_ELBOW_R_S" file="R_ELBOW_R_S.STL"/>
<mesh name="R_WRIST_P_S" file="R_WRIST_P_S.STL"/>
<mesh name="R_WRIST_Y_S" file="R_WRIST_Y_S.STL"/>
<mesh name="R_WRIST_R_S" file="R_WRIST_R_S.STL"/>
<!-- 天空背景，可要可不要 -->
<texture name="skybox" type="skybox"
builtin="gradient"
rgb1="0.4 0.6 0.8"
rgb2="0 0 0"
width="512" height="512"/>
<!-- 蓝色方格地面纹理：builtin="checker" -->
<texture name="grid"
type="2d"
builtin="checker"
width="512" height="512"
rgb1="0.2 0.3 0.4"
rgb2="0.1 0.2 0.3"
mark="cross"
markrgb="0.8 0.8 0.8"/>
<!-- 地面材质，绑定上面的纹理 -->
<material name="grid_floor"
texture="grid"
texrepeat="5 5"
reflectance="0.2"/>
</asset>
<worldbody>
<geom type="mesh" contype="0" conaffinity="0" group="1" density="0"
rgba="0.698039 0.698039 0.698039 1" mesh="PELVIS_S"/>
<geom type="mesh" rgba="0.698039 0.698039 0.698039 1" mesh="PELVIS_S"/>
<body name="L_SHOULDER_P_S" pos="0 0.0945 0.042">
<inertial pos="-0.00982259 0.0704593 1.15262e-06"
quat="0.706163 0.705933 -0.0386962 -0.0386741"
mass="0.880738"
diaginertia="0.000584874 0.000465648 0.000443849"/>
<joint name="L_SHOULDER_P" pos="0 0 0" axis="0 1 0"
range="-1.57 1.57" actuatorfrcrange="-120 120"/>
<geom type="mesh" contype="0" conaffinity="0" group="1" density="0"
rgba="0.898039 0.917647 0.929412 1" mesh="L_SHOULDER_P_S"/>
<geom type="mesh" rgba="0.898039 0.917647 0.929412 1"
mesh="L_SHOULDER_P_S"/>
<body name="L_SHOULDER_R_S" pos="0.035 0.0765 0">
<inertial pos="-0.0346025 0.0917393 -1.67085e-08"
quat="0.358778 0.609303 -0.358823 0.609323"
mass="0.594788"
diaginertia="0.000414771 0.000407636 0.000294296"/>
<joint name="L_SHOULDER_R" pos="0 0 0" axis="1 0 0"
range="-2 2" actuatorfrcrange="-120 120"/>
<geom type="mesh" contype="0" conaffinity="0" group="1" density="0"
rgba="0.890196 0.890196 0.913725 1" mesh="L_SHOULDER_R_S"/>
<geom type="mesh" rgba="0.890196 0.890196 0.913725 1"
mesh="L_SHOULDER_R_S"/>
<body name="L_SHOULDER_Y_S" pos="-0.035 0.1475 0">
<inertial pos="-0.00440977 0.086362 9.50749e-09"
quat="0.705001 0.704998 -0.054559 -0.0545441"
mass="0.563406"
diaginertia="0.000329815 0.000297341 0.000211019"/>
<joint name="L_SHOULDER_Y" pos="0 0 0" axis="0 1 0"
range="-2.18 0" actuatorfrcrange="-80 80"/>
<geom type="mesh" contype="0" conaffinity="0" group="1" density="0"
rgba="0.890196 0.890196 0.913725 1" mesh="L_SHOULDER_Y_S"/>
<geom type="mesh" rgba="0.890196 0.890196 0.913725 1"
mesh="L_SHOULDER_Y_S"/>
<body name="L_ELBOW_R_S" pos="0.034 0.1025 0">
<inertial pos="-0.0335624 0.06032 2.99656e-07"
quat="0.674756 0.674714 0.211333 0.211667"
mass="0.393572"
diaginertia="0.000189078 0.000181042 0.000139034"/>
<joint name="L_ELBOW_R" pos="0 0 0" axis="1 0 0"
range="-2.05 0" actuatorfrcrange="-50 50"/>
<geom type="mesh" contype="0" conaffinity="0" group="1" density="0"
rgba="0.890196 0.890196 0.913725 1" mesh="L_ELBOW_R_S"/>
<geom type="mesh" rgba="0.890196 0.890196 0.913725 1"
mesh="L_ELBOW_R_S"/>
<body name="L_WRIST_P_S" pos="-0.034 0.0965 0">
<inertial pos="-1.39659e-10 0.0675973 0.0192006"
quat="0.467537 0.530481 -0.530481 0.467537"
mass="0.442332"
diaginertia="0.000489142 0.000476754 9.72811e-05"/>
<joint name="L_WRIST_P" pos="0 0 0" axis="0 1 0"
range="0 3.14" actuatorfrcrange="-50 50"/>
<geom type="mesh" contype="0" conaffinity="0" group="1" density="0"
rgba="0.698039 0.698039 0.698039 1" mesh="L_WRIST_P_S"/>
<geom type="mesh" rgba="0.698039 0.698039 0.698039 1"
mesh="L_WRIST_P_S"/>
<body name="L_WRIST_Y_S" pos="0 0.1525 0.039">
<inertial pos="-0.00464136 -5.06427e-10 -0.0341254"
quat="0.298107 0.641196 0.641196 0.298107"
mass="0.235738"
diaginertia="6.00636e-05 5.8497e-05 4.579e-05"/>
<joint name="L_WRIST_Y" pos="0 0 0" axis="0 0 1"
range="-0.78 0.78" actuatorfrcrange="-50 50"/>
<geom type="mesh" contype="0" conaffinity="0" group="1" density="0"
rgba="0.647059 0.619608 0.588235 1" mesh="L_WRIST_Y_S"/>
<geom type="mesh" rgba="0.647059 0.619608 0.588235 1"
mesh="L_WRIST_Y_S"/>
<body name="L_WRIST_R_S" pos="0.0258 0 -0.039">
<inertial pos="-0.0161477 0.0955047 -0.00499392"
quat="0.595488 0.369507 -0.591676 0.39847"
mass="0.504894"
diaginertia="0.000280921 0.000188575 0.000178055"/>
<joint name="L_WRIST_R" pos="0 0 0" axis="1 0 0"
range="-1.57 0.26" actuatorfrcrange="-50 50"/>
<geom type="mesh" contype="0" conaffinity="0" group="1" density="0"
rgba="0.890196 0.890196 0.913725 1" mesh="L_WRIST_R_S"/>
<geom type="mesh" rgba="0.890196 0.890196 0.913725 1"
mesh="L_WRIST_R_S"/>
</body>
</body>
</body>
</body>
</body>
</body>
</body>
<body name="R_SHOULDER_P_S" pos="0 -0.0945 0.042">
<inertial pos="-0.00982259 -0.0704593 -1.1507e-06"
quat="0.706163 0.705933 0.0386962 0.0386741"
mass="0.880738"
diaginertia="0.000584874 0.000465648 0.000443849"/>
<joint name="R_SHOULDER_P" pos="0 0 0" axis="0 -1 0"
range="-1.57 1.57" actuatorfrcrange="-120 120"/>
<geom type="mesh" contype="0" conaffinity="0" group="1" density="0"
rgba="0.890196 0.890196 0.913725 1" mesh="R_SHOULDER_P_S"/>
<geom type="mesh" rgba="0.890196 0.890196 0.913725 1"
mesh="R_SHOULDER_P_S"/>
<body name="R_SHOULDER_R_S" pos="0.035 -0.0765 0">
<inertial pos="-0.0346025 -0.0917393 1.86281e-08"
quat="0.609323 0.358823 -0.609303 0.358778"
mass="0.594788"
diaginertia="0.000414771 0.000407636 0.000294296"/>
<joint name="R_SHOULDER_R" pos="0 0 0" axis="1 0 0"
range="-2 2" actuatorfrcrange="-120 120"/>
<geom type="mesh" contype="0" conaffinity="0" group="1" density="0"
rgba="0.890196 0.890196 0.913725 1" mesh="R_SHOULDER_R_S"/>
<geom type="mesh" rgba="0.890196 0.890196 0.913725 1"
mesh="R_SHOULDER_R_S"/>
<body name="R_SHOULDER_Y_S" pos="-0.035 -0.1475 0">
<inertial pos="-0.00440977 -0.086362 -7.58792e-09"
quat="0.705001 0.704998 0.054559 0.0545441"
mass="0.563406"
diaginertia="0.000329815 0.000297341 0.000211019"/>
<joint name="R_SHOULDER_Y" pos="0 0 0" axis="0 -1 0"
range="0 3.14" actuatorfrcrange="-80 80"/>
<geom type="mesh" contype="0" conaffinity="0" group="1" density="0"
rgba="0.890196 0.890196 0.913725 1" mesh="R_SHOULDER_Y_S"/>
<geom type="mesh" rgba="0.890196 0.890196 0.913725 1"
mesh="R_SHOULDER_Y_S"/>
<body name="R_ELBOW_R_S" pos="0.034 -0.1025 0">
<inertial pos="-0.0335624 -0.06032 -2.97736e-07"
quat="0.674756 0.674714 -0.211333 -0.211667"
mass="0.393572"
diaginertia="0.000189078 0.000181042 0.000139034"/>
<joint name="R_ELBOW_R" pos="0 0 0" axis="1 0 0"
range="0 2.05" actuatorfrcrange="-80 80"/>
<geom type="mesh" contype="0" conaffinity="0" group="1" density="0"
rgba="0.890196 0.890196 0.913725 1" mesh="R_ELBOW_R_S"/>
<geom type="mesh" rgba="0.890196 0.890196 0.913725 1"
mesh="R_ELBOW_R_S"/>
<body name="R_WRIST_P_S" pos="-0.034 -0.0965 0">
<inertial pos="-1.39657e-10 -0.0675973 0.0192006"
quat="0.530481 0.467537 -0.467537 0.530481"
mass="0.442332"
diaginertia="0.000489142 0.000476754 9.72811e-05"/>
<joint name="R_WRIST_P" pos="0 0 0" axis="0 -1 0"
range="-3.14 0" actuatorfrcrange="-50 50"/>
<geom type="mesh" contype="0" conaffinity="0" group="1" density="0"
rgba="0.647059 0.619608 0.588235 1" mesh="R_WRIST_P_S"/>
<geom type="mesh" rgba="0.647059 0.619608 0.588235 1"
mesh="R_WRIST_P_S"/>
<body name="R_WRIST_Y_S" pos="0 -0.1525 0.039">
<inertial pos="-0.00464136 -5.06426e-10 -0.0341254"
quat="0.298107 0.641196 0.641196 0.298107"
mass="0.235738"
diaginertia="6.00636e-05 5.8497e-05 4.579e-05"/>
<joint name="R_WRIST_Y" pos="0 0 0" axis="0 0 1"
range="-0.78 0.78" actuatorfrcrange="-50 50"/>
<geom type="mesh" contype="0" conaffinity="0" group="1" density="0"
rgba="0.647059 0.619608 0.588235 1" mesh="R_WRIST_Y_S"/>
<geom type="mesh" rgba="0.647059 0.619608 0.588235 1"
mesh="R_WRIST_Y_S"/>
<body name="R_WRIST_R_S" pos="0.03 0 -0.039">
<inertial pos="-0.0201642 -0.11075 -0.00598955"
quat="0.483404 0.529786 -0.463203 0.520663"
mass="0.504366"
diaginertia="0.00027189 0.000186086 0.000130629"/>
<joint name="R_WRIST_R" pos="0 0 0" axis="1 0 0"
range="-0.26 1.57" actuatorfrcrange="-50 50"/>
<geom type="mesh" contype="0" conaffinity="0" group="1" density="0"
rgba="0.890196 0.890196 0.913725 1" mesh="R_WRIST_R_S"/>
<geom type="mesh" rgba="0.890196 0.890196 0.913725 1"
mesh="R_WRIST_R_S"/>
<!-- 小标记点 -->
<geom size="0.004"
pos="0.00684256 -0.284077 0.00801525"
contype="0" conaffinity="0" group="1"
density="0" rgba="0 1 1 1"/>
<geom size="0.005"
pos="0.00684256 -0.284077 0.00801525"
rgba="0 1 1 1"/>
<geom size="0.004"
pos="-0.01212 -0.17655 0.07506"
quat="3.17467e-11 -3.17467e-11 -0.707107 0.707107"
contype="0" conaffinity="0" group="1"
density="0" rgba="0 1 0 1"/>
<geom size="0.005"
pos="-0.01212 -0.17655 0.07506"
quat="3.17467e-11 -3.17467e-11 -0.707107 0.707107"
rgba="0 1 0 1"/>
</body>
</body>
</body>
</body>
</body>
</body>
</body>
<light name="top_light"
mode="track"
directional="true"
diffuse="1 1 1"
specular="0.3 0.3 0.3"
pos="0 0 3"
dir="0 0 -1"/>
<geom name="floor"
type="plane"
pos="0 0 0"
size="0 0 0.05"
material="grid_floor"
condim="3"
friction="1 0.005 0.0001"/>
</worldbody>
<!-- <actuator>-->
<!-- &lt;!&ndash; 左臂：力矩控制，ctrl = 关节力矩 (Nm) &ndash;&gt;-->
<!-- <motor name="L_SHOULDER_P_torque" joint="L_SHOULDER_P"-->
<!-- gear="1" ctrlrange="-200 200"/>-->
<!-- <motor name="L_SHOULDER_R_torque" joint="L_SHOULDER_R"-->
<!-- gear="1" ctrlrange="-200 200"/>-->
<!-- <motor name="L_SHOULDER_Y_torque" joint="L_SHOULDER_Y"-->
<!-- gear="1" ctrlrange="-150 150"/>-->
<!-- <motor name="L_ELBOW_R_torque" joint="L_ELBOW_R"-->
<!-- gear="1" ctrlrange="-120 120"/>-->
<!-- <motor name="L_WRIST_P_torque" joint="L_WRIST_P"-->
<!-- gear="1" ctrlrange="-80 80"/>-->
<!-- <motor name="L_WRIST_Y_torque" joint="L_WRIST_Y"-->
<!-- gear="1" ctrlrange="-80 80"/>-->
<!-- <motor name="L_WRIST_R_torque" joint="L_WRIST_R"-->
<!-- gear="1" ctrlrange="-80 80"/>-->
<!-- &lt;!&ndash; 右臂：力矩控制 &ndash;&gt;-->
<!-- <motor name="R_SHOULDER_P_torque" joint="R_SHOULDER_P"-->
<!-- gear="1" ctrlrange="-200 200"/>-->
<!-- <motor name="R_SHOULDER_R_torque" joint="R_SHOULDER_R"-->
<!-- gear="1" ctrlrange="-200 200"/>-->
<!-- <motor name="R_SHOULDER_Y_torque" joint="R_SHOULDER_Y"-->
<!-- gear="1" ctrlrange="-150 150"/>-->
<!-- <motor name="R_ELBOW_R_torque" joint="R_ELBOW_R"-->
<!-- gear="1" ctrlrange="-120 120"/>-->
<!-- <motor name="R_WRIST_P_torque" joint="R_WRIST_P"-->
<!-- gear="1" ctrlrange="-80 80"/>-->
<!-- <motor name="R_WRIST_Y_torque" joint="R_WRIST_Y"-->
<!-- gear="1" ctrlrange="-80 80"/>-->
<!-- <motor name="R_WRIST_R_torque" joint="R_WRIST_R"-->
<!-- gear="1" ctrlrange="-80 80"/>-->
<!-- </actuator>-->
<actuator>
<!-- 左臂：位置控制，ctrl = 关节目标角度 (rad) -->
<position name="L_SHOULDER_P_pos" joint="L_SHOULDER_P"
kp="40" ctrlrange="-1.57 1.57"/>
<position name="L_SHOULDER_R_pos" joint="L_SHOULDER_R"
kp="40" ctrlrange="-1.57 1.57"/>
<position name="L_SHOULDER_Y_pos" joint="L_SHOULDER_Y"
kp="30" ctrlrange="-1.57 1.57"/>
<position name="L_ELBOW_R_pos" joint="L_ELBOW_R"
kp="30" ctrlrange="-2.09 0.0"/>
<position name="L_WRIST_P_pos" joint="L_WRIST_P"
kp="20" ctrlrange="-1.57 1.57"/>
<position name="L_WRIST_Y_pos" joint="L_WRIST_Y"
kp="20" ctrlrange="-1.57 1.57"/>
<position name="L_WRIST_R_pos" joint="L_WRIST_R"
kp="20" ctrlrange="-1.57 1.57"/>
<!-- 右臂：位置控制 -->
<position name="R_SHOULDER_P_pos" joint="R_SHOULDER_P"
kp="40" ctrlrange="-1.57 1.57"/>
<position name="R_SHOULDER_R_pos" joint="R_SHOULDER_R"
kp="40" ctrlrange="-1.57 1.57"/>
<position name="R_SHOULDER_Y_pos" joint="R_SHOULDER_Y"
kp="30" ctrlrange="-1.57 1.57"/>
<position name="R_ELBOW_R_pos" joint="R_ELBOW_R"
kp="40" ctrlrange="0 2.05"/>
<position name="R_WRIST_P_pos" joint="R_WRIST_P"
kp="20" ctrlrange="-1.57 1.57"/>
<position name="R_WRIST_Y_pos" joint="R_WRIST_Y"
kp="20" ctrlrange="-1.57 1.57"/>
<position name="R_WRIST_R_pos" joint="R_WRIST_R"
kp="20" ctrlrange="-1.57 1.57"/>
</actuator>
</mujoco>

281
robot_model/xiaoyan_description/left_arm.urdf Normal file
View File

@ -0,0 +1,281 @@
<?xml version="1.0" encoding="utf-8"?>
<robot name="left_arm">
<mujoco>
<compiler
meshdir="meshes"
balanceinertia="true"
discardvisual="false" />
</mujoco>
<link name="base_link">
<visual>
<geometry>
<cylinder radius="0.05" length="1.2"/>
</geometry>
<material name="gray">
<color rgba="0.5 0.5 0.5 1.0"/>
</material>
</visual>
<collision>
<geometry>
<cylinder radius="0.05" length="1.2"/>
</geometry>
</collision>
<inertial>
<mass value="20.0"/>
<origin xyz="0 0 0" rpy="0 0 0"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<joint name="base_fixed" type="fixed">
<origin rpy="0 0 0.0" xyz="0 0 0.6"/>
<parent link="base_link"/>
<child link="PELVIS_S"/>
<axis xyz="0 0 0"/>
</joint>
<link name="PELVIS_S">
<inertial>
<origin xyz="3.78529087037144E-05 3.81781425684836E-07 0.0386396273530852" rpy="0 0 0" />
<mass value="2.10624590271277" />
<inertia ixx="0.00165706865324979" ixy="3.13663044821662E-09" ixz="6.84209533216046E-07" iyy="0.00136736758630241" iyz="-1.01014607878816E-10" izz="0.00168295663089359" />
</inertial>
<visual>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<mesh filename="meshes/PELVIS_S.STL" />
</geometry>
<material name="">
<color rgba="0.698039215686274 0.698039215686274 0.698039215686274 1" />
</material>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<mesh filename="meshes/PELVIS_S.STL" />
</geometry>
</collision>
</link>
<joint name="L_SHOULDER_P" type="revolute">
<origin xyz="0 0.0945 0.042" rpy="0 0 0" />
<parent link="PELVIS_S" />
<child link="L_SHOULDER_P_S" />
<axis xyz="0 1 0" />
<limit lower="-3.14" upper="3.14" effort="100" velocity="3.351" />
</joint>
<link name="L_SHOULDER_P_S">
<inertial>
<origin xyz="-0.00982258725282134 0.0704593083751867 1.15261874861217E-06" rpy="0 0 0" />
<mass value="0.880737519698403" />
<inertia ixx="0.000583190308378148" ixy="-1.53153074560473E-05" ixz="6.58466471754072E-09" iyy="0.000445532440067177" iyz="6.37001404038516E-09" izz="0.000465648071069952" />
</inertial>
<visual>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<mesh filename="meshes/L_SHOULDER_P_S.STL" />
</geometry>
<material
name="">
<color rgba="0.898039215686275 0.917647058823529 0.929411764705882 1" />
</material>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<mesh filename="meshes/L_SHOULDER_P_S.STL" />
</geometry>
</collision>
</link>
<joint name="L_SHOULDER_R" type="revolute">
<origin xyz="0.035 0.0765 0" rpy="0 0 0" />
<parent link="L_SHOULDER_P_S" />
<child link="L_SHOULDER_R_S" />
<axis xyz="1 0 0" />
<limit lower="-3.14" upper="3.14" effort="100" velocity="3.351" />
</joint>
<link name="L_SHOULDER_R_S">
<inertial>
<origin xyz="-0.0346025282975857 0.091739327988169 -1.67085072999562E-08" rpy="0 0 0" />
<mass value="0.594788424442483" />
<inertia ixx="0.000380970396712656" ixy="4.80751844573946E-05" ixz="-1.34913746586865E-11" iyy="0.000320962178597474" iyz="-1.00039763442409E-09" izz="0.000414771047901155" />
</inertial>
<visual>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<mesh filename="meshes/L_SHOULDER_R_S.STL" />
</geometry>
<material name="">
<color rgba="0.890196078431372 0.890196078431372 0.913725490196078 1" />
</material>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<mesh filename="meshes/L_SHOULDER_R_S.STL" />
</geometry>
</collision>
</link>
<joint name="L_SHOULDER_Y" type="revolute">
<origin xyz="-0.035 0.1475 0" rpy="0 0 0" />
<parent link="L_SHOULDER_R_S" />
<child link="L_SHOULDER_Y_S" />
<axis xyz="0 1 0" />
<limit lower="-3.14" upper="3.14" effort="49" velocity="3.8758" />
</joint>
<link name="L_SHOULDER_Y_S">
<inertial>
<origin xyz="-0.00440976862014801 0.0863620459174068 9.50748668682166E-09" rpy="0 0 0" />
<mass value="0.563406026626801" />
<inertia ixx="0.000327003834287552" ixy="-1.8057438966771E-05" ixz="7.28778136267901E-10" iyy="0.000213830709361531" iyz="1.49273668517817E-10" izz="0.000297341029639189" />
</inertial>
<visual>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<mesh filename="meshes/L_SHOULDER_Y_S.STL" />
</geometry>
<material name="">
<color rgba="0.890196078431372 0.890196078431372 0.913725490196078 1" />
</material>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<mesh filename="meshes/L_SHOULDER_Y_S.STL" />
</geometry>
</collision>
</link>
<joint name="L_ELBOW_R" type="revolute">
<origin xyz="0.034 0.1025 0" rpy="0 0 0" />
<parent link="L_SHOULDER_Y_S" />
<child link="L_ELBOW_R_S" />
<axis xyz="1 0 0" />
<limit lower="-3.14" upper="3.14" effort="49" velocity="3.8758" />
</joint>
<link name="L_ELBOW_R_S">
<inertial>
<origin xyz="-0.0335624237303443 0.0603199964106564 2.99655911639718E-07" rpy="0 0 0" />
<mass value="0.393571904406493" />
<inertia ixx="0.00017277119386253" ixy="2.34549801867355E-05" ixz="-1.90560556659271E-09" iyy="0.000155340245267897" iyz="8.82493073600007E-09" izz="0.00018104232734159" />
</inertial>
<visual>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<mesh filename="meshes/L_ELBOW_R_S.STL" />
</geometry>
<material name="">
<color rgba="0.890196078431372 0.890196078431372 0.913725490196078 1" />
</material>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<mesh filename="meshes/L_ELBOW_R_S.STL" />
</geometry>
</collision>
</link>
<joint name="L_WRIST_P" type="revolute">
<origin xyz="-0.034 0.0965 0" rpy="0 0 0" />
<parent link="L_ELBOW_R_S" />
<child link="L_WRIST_P_S" />
<axis xyz="0 1 0" />
<limit lower="-3.14" upper="3.14" effort="26" velocity="4.19" />
</joint>
<link name="L_WRIST_P_S">
<inertial>
<origin xyz="-1.39659173115092E-10 0.0675972604744393 0.019200551565574" rpy="0 0 0" />
<mass value="0.442332465815496" />
<inertia ixx="0.000476754055455895" ixy="-4.61505824713347E-15" ixz="6.03808112272229E-18" iyy="0.000103466585850499" iyz="-4.88426705501198E-05" izz="0.000482956345754213" />
</inertial>
<visual>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<mesh filename="meshes/L_WRIST_P_S.STL" />
</geometry>
<material name="">
<color rgba="0.698039215686274 0.698039215686274 0.698039215686274 1" />
</material>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<mesh filename="meshes/L_WRIST_P_S.STL" />
</geometry>
</collision>
</link>
<joint name="L_WRIST_Y" type="revolute">
<origin xyz="0 0.1525 0.039" rpy="0 0 0" />
<parent link="L_WRIST_P_S" />
<child link="L_WRIST_Y_S" />
<axis xyz="0 0 1" />
<limit lower="-3.14" upper="3.14" effort="26" velocity="4.71" />
</joint>
<link name="L_WRIST_Y_S">
<inertial>
<origin xyz="-0.00464135887331864 -5.06426789392833E-10 -0.0341253783666609" rpy="0 0 0" />
<mass value="0.23573847772002" />
<inertia ixx="5.10686940455785E-05" ixy="7.45706654358429E-16" ixz="6.2619568923368E-06" iyy="6.00636019624519E-05" iyz="1.27989654155383E-16" izz="5.32182903609189E-05" />
</inertial>
<visual>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<mesh filename="meshes/L_WRIST_Y_S.STL" />
</geometry>
<material name="">
<color rgba="0.647058823529412 0.619607843137255 0.588235294117647 1" />
</material>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<mesh filename="meshes/L_WRIST_Y_S.STL" />
</geometry>
</collision>
</link>
<joint name="L_WRIST_R" type="revolute">
<origin xyz="0.0258 0 -0.039" rpy="0 0 0" />
<parent link="L_WRIST_Y_S" />
<child link="L_WRIST_R_S" />
<axis xyz="1 0 0" />
<limit lower="-3.14" upper="3.14" effort="26" velocity="4.71" />
</joint>
<link name="L_WRIST_R_S">
<inertial>
<origin xyz="-0.0161477357620243 0.0955046558857351 -0.00499392489444117" rpy="0 0 0" />
<mass value="0.504894112043562" />
<inertia ixx="0.000186833711781125" ixy="-3.99488705622731E-06" ixz="-1.51920720398336E-06" iyy="0.000179960980467837" iyz="3.6992669535716E-06" izz="0.000280756101568308" />
</inertial>
<visual>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<mesh filename="meshes/L_WRIST_R_S.STL" />
</geometry>
<material name="">
<color rgba="0.890196078431372 0.890196078431372 0.913725490196078 1" />
</material>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<mesh filename="meshes/L_WRIST_R_S.STL" />
</geometry>
</collision>
</link>
</robot>

63
robot_model/xiaoyan_description/left_arm.xml Normal file
View File

@ -0,0 +1,63 @@
<mujoco model="left_arm">
<compiler angle="radian" meshdir="meshes/"/>
<asset>
<mesh name="PELVIS_S" file="PELVIS_S.STL"/>
<mesh name="L_SHOULDER_P_S" file="L_SHOULDER_P_S.STL"/>
<mesh name="L_SHOULDER_R_S" file="L_SHOULDER_R_S.STL"/>
<mesh name="L_SHOULDER_Y_S" file="L_SHOULDER_Y_S.STL"/>
<mesh name="L_ELBOW_R_S" file="L_ELBOW_R_S.STL"/>
<mesh name="L_WRIST_P_S" file="L_WRIST_P_S.STL"/>
<mesh name="L_WRIST_Y_S" file="L_WRIST_Y_S.STL"/>
<mesh name="L_WRIST_R_S" file="L_WRIST_R_S.STL"/>
</asset>
<worldbody>
<geom size="0.05 0.6" type="cylinder" contype="0" conaffinity="0" group="1" density="0"/>
<geom size="0.05 0.6" type="cylinder"/>
<geom pos="0 0 0.6" quat="1 0 0 0" type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.698039 0.698039 0.698039 1" mesh="PELVIS_S"/>
<geom pos="0 0 0.6" quat="1 0 0 0" type="mesh" rgba="0.698039 0.698039 0.698039 1" mesh="PELVIS_S"/>
<body name="L_SHOULDER_P_S" pos="0 0.0945 0.642">
<inertial pos="-0.00982259 0.0704593 1.15262e-06" quat="0.706163 0.705933 -0.0386962 -0.0386741" mass="0.880738" diaginertia="0.000584874 0.000465648 0.000443849"/>
<joint name="L_SHOULDER_P" pos="0 0 0" axis="0 1 0" range="-3.14 3.14" actuatorfrcrange="-100 100"/>
<geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.898039 0.917647 0.929412 1" mesh="L_SHOULDER_P_S"/>
<geom type="mesh" rgba="0.898039 0.917647 0.929412 1" mesh="L_SHOULDER_P_S"/>
<body name="L_SHOULDER_R_S" pos="0.035 0.0765 0">
<inertial pos="-0.0346025 0.0917393 -1.67085e-08" quat="0.358778 0.609303 -0.358823 0.609323" mass="0.594788" diaginertia="0.000414771 0.000407636 0.000294296"/>
<joint name="L_SHOULDER_R" pos="0 0 0" axis="1 0 0" range="-3.14 3.14" actuatorfrcrange="-100 100"/>
<geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.890196 0.890196 0.913725 1" mesh="L_SHOULDER_R_S"/>
<geom type="mesh" rgba="0.890196 0.890196 0.913725 1" mesh="L_SHOULDER_R_S"/>
<body name="L_SHOULDER_Y_S" pos="-0.035 0.1475 0">
<inertial pos="-0.00440977 0.086362 9.50749e-09" quat="0.705001 0.704998 -0.054559 -0.0545441" mass="0.563406" diaginertia="0.000329815 0.000297341 0.000211019"/>
<joint name="L_SHOULDER_Y" pos="0 0 0" axis="0 1 0" range="-3.14 3.14" actuatorfrcrange="-49 49"/>
<geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.890196 0.890196 0.913725 1" mesh="L_SHOULDER_Y_S"/>
<geom type="mesh" rgba="0.890196 0.890196 0.913725 1" mesh="L_SHOULDER_Y_S"/>
<body name="L_ELBOW_R_S" pos="0.034 0.1025 0">
<inertial pos="-0.0335624 0.06032 2.99656e-07" quat="0.674756 0.674714 0.211333 0.211667" mass="0.393572" diaginertia="0.000189078 0.000181042 0.000139034"/>
<joint name="L_ELBOW_R" pos="0 0 0" axis="1 0 0" range="-3.14 3.14" actuatorfrcrange="-49 49"/>
<geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.890196 0.890196 0.913725 1" mesh="L_ELBOW_R_S"/>
<geom type="mesh" rgba="0.890196 0.890196 0.913725 1" mesh="L_ELBOW_R_S"/>
<body name="L_WRIST_P_S" pos="-0.034 0.0965 0">
<inertial pos="-1.39659e-10 0.0675973 0.0192006" quat="0.467537 0.530481 -0.530481 0.467537" mass="0.442332" diaginertia="0.000489142 0.000476754 9.72811e-05"/>
<joint name="L_WRIST_P" pos="0 0 0" axis="0 1 0" range="-3.14 3.14" actuatorfrcrange="-26 26"/>
<geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.698039 0.698039 0.698039 1" mesh="L_WRIST_P_S"/>
<geom type="mesh" rgba="0.698039 0.698039 0.698039 1" mesh="L_WRIST_P_S"/>
<body name="L_WRIST_Y_S" pos="0 0.1525 0.039">
<inertial pos="-0.00464136 -5.06427e-10 -0.0341254" quat="0.298107 0.641196 0.641196 0.298107" mass="0.235738" diaginertia="6.00636e-05 5.8497e-05 4.579e-05"/>
<joint name="L_WRIST_Y" pos="0 0 0" axis="0 0 1" range="-3.14 3.14" actuatorfrcrange="-26 26"/>
<geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.647059 0.619608 0.588235 1" mesh="L_WRIST_Y_S"/>
<geom type="mesh" rgba="0.647059 0.619608 0.588235 1" mesh="L_WRIST_Y_S"/>
<body name="L_WRIST_R_S" pos="0.0258 0 -0.039">
<inertial pos="-0.0161477 0.0955047 -0.00499392" quat="0.595488 0.369507 -0.591676 0.39847" mass="0.504894" diaginertia="0.000280921 0.000188575 0.000178055"/>
<joint name="L_WRIST_R" pos="0 0 0" axis="1 0 0" range="-3.14 3.14" actuatorfrcrange="-26 26"/>
<geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.890196 0.890196 0.913725 1" mesh="L_WRIST_R_S"/>
<geom type="mesh" rgba="0.890196 0.890196 0.913725 1" mesh="L_WRIST_R_S"/>
</body>
</body>
</body>
</body>
</body>
</body>
</body>
</worldbody>
</mujoco>

BIN
robot_model/xiaoyan_description/meshes/L_ELBOW_R_S.STL Normal file
View File

Binary file not shown.

BIN
robot_model/xiaoyan_description/meshes/L_SHOULDER_P_S.STL Normal file
View File

Binary file not shown.

BIN
robot_model/xiaoyan_description/meshes/L_SHOULDER_R_S.STL Normal file
View File

Binary file not shown.

BIN
robot_model/xiaoyan_description/meshes/L_SHOULDER_Y_S.STL Normal file
View File

Binary file not shown.

BIN
robot_model/xiaoyan_description/meshes/L_WRIST_P_S.STL Normal file
View File

Binary file not shown.

BIN
robot_model/xiaoyan_description/meshes/L_WRIST_R_S.STL Normal file
View File

Binary file not shown.

BIN
robot_model/xiaoyan_description/meshes/L_WRIST_Y_S.STL Normal file
View File

Binary file not shown.

BIN
robot_model/xiaoyan_description/meshes/NECK_P_S.STL Normal file
View File

Binary file not shown.

BIN
robot_model/xiaoyan_description/meshes/NECK_R_S.STL Normal file
View File

Binary file not shown.

BIN
robot_model/xiaoyan_description/meshes/NECK_Y_S.STL Normal file
View File

Binary file not shown.

BIN
robot_model/xiaoyan_description/meshes/PELVIS_S.STL Normal file
View File

Binary file not shown.

BIN
robot_model/xiaoyan_description/meshes/R_ELBOW_R_S.STL Normal file
View File

Binary file not shown.

BIN
robot_model/xiaoyan_description/meshes/R_SHOULDER_P_S.STL Normal file
View File

Binary file not shown.

BIN
robot_model/xiaoyan_description/meshes/R_SHOULDER_R_S.STL Normal file
View File

Binary file not shown.

BIN
robot_model/xiaoyan_description/meshes/R_SHOULDER_Y_S.STL Normal file
View File

Binary file not shown.

BIN
robot_model/xiaoyan_description/meshes/R_WRIST_P_S.STL Normal file
View File

Binary file not shown.

BIN
robot_model/xiaoyan_description/meshes/R_WRIST_R_S.STL Normal file
View File

Binary file not shown.

BIN
robot_model/xiaoyan_description/meshes/R_WRIST_Y_S.STL Normal file
View File

Binary file not shown.

BIN
robot_model/xiaoyan_description/meshes/base_link.STL Normal file
View File

Binary file not shown.

BIN
robot_model/xiaoyan_description/meshes/index_force_sensor_1.STL Normal file
View File

Binary file not shown.

BIN
robot_model/xiaoyan_description/meshes/index_force_sensor_2.STL Normal file
View File

Binary file not shown.

BIN
robot_model/xiaoyan_description/meshes/index_force_sensor_3.STL Normal file
View File

Binary file not shown.

BIN
robot_model/xiaoyan_description/meshes/little_force_sensor_1.STL Normal file
View File

Binary file not shown.

BIN
robot_model/xiaoyan_description/meshes/little_force_sensor_2.STL Normal file
View File

Binary file not shown.

BIN
robot_model/xiaoyan_description/meshes/little_force_sensor_3.STL Normal file
View File

Binary file not shown.

BIN
robot_model/xiaoyan_description/meshes/middle_force_sensor_1.STL Normal file
View File

Binary file not shown.

BIN
robot_model/xiaoyan_description/meshes/middle_force_sensor_2.STL Normal file
View File

Binary file not shown.

BIN
robot_model/xiaoyan_description/meshes/middle_force_sensor_3.STL Normal file
View File

Binary file not shown.

BIN
robot_model/xiaoyan_description/meshes/palm_force_sensor.STL Normal file
View File

Binary file not shown.

BIN
robot_model/xiaoyan_description/meshes/right_index_1.STL Normal file
View File

Binary file not shown.

BIN
robot_model/xiaoyan_description/meshes/right_index_2.STL Normal file
View File

Binary file not shown.

BIN
robot_model/xiaoyan_description/meshes/right_little_1.STL Normal file
View File

Binary file not shown.

BIN
robot_model/xiaoyan_description/meshes/right_little_2.STL Normal file
View File

Binary file not shown.

BIN
robot_model/xiaoyan_description/meshes/right_middle_1.STL Normal file
View File

Binary file not shown.

BIN
robot_model/xiaoyan_description/meshes/right_middle_2.STL Normal file
View File

Binary file not shown.

BIN
robot_model/xiaoyan_description/meshes/right_ring_1.STL Normal file
View File

Binary file not shown.

BIN
robot_model/xiaoyan_description/meshes/right_ring_2.STL Normal file
View File

Binary file not shown.

BIN
robot_model/xiaoyan_description/meshes/right_thumb_1.STL Normal file
View File

Binary file not shown.

BIN
robot_model/xiaoyan_description/meshes/right_thumb_2.STL Normal file
View File

Binary file not shown.

BIN
robot_model/xiaoyan_description/meshes/right_thumb_3.STL Normal file
View File

Binary file not shown.

BIN
robot_model/xiaoyan_description/meshes/right_thumb_4.STL Normal file
View File

Binary file not shown.

BIN
robot_model/xiaoyan_description/meshes/ring_force_sensor_1.STL Normal file
View File

Binary file not shown.

BIN
robot_model/xiaoyan_description/meshes/ring_force_sensor_2.STL Normal file
View File

Binary file not shown.

BIN
robot_model/xiaoyan_description/meshes/ring_force_sensor_3.STL Normal file
View File

Binary file not shown.

BIN
robot_model/xiaoyan_description/meshes/thumb_force_sensor_1.STL Normal file
View File

Binary file not shown.

BIN
robot_model/xiaoyan_description/meshes/thumb_force_sensor_2.STL Normal file
View File

Binary file not shown.

BIN
robot_model/xiaoyan_description/meshes/thumb_force_sensor_3.STL Normal file
View File

Binary file not shown.

BIN
robot_model/xiaoyan_description/meshes/thumb_force_sensor_4.STL Normal file
View File

Binary file not shown.

322
robot_model/xiaoyan_description/right_arm.urdf Normal file
View File

@ -0,0 +1,322 @@
<?xml version="1.0" encoding="utf-8"?>
<robot name="right_arm">
<mujoco>
<compiler
meshdir="meshes"
balanceinertia="true"
discardvisual="false" />
</mujoco>
<link name="PELVIS_S">
<inertial>
<origin xyz="3.78529e-05 3.81781e-07 0.0386396" rpy="0 0 0"/>
<mass value="2.10624590271277"/>
<inertia
ixx="0.00165706865324979" ixy="3.13663e-09" ixz="6.84210e-07"
iyy="0.00136736758630241" iyz="-1.01015e-10" izz="0.00168295663089359"/>
</inertial>
<visual>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<mesh filename="meshes/PELVIS_S.STL"/>
</geometry>
<material name="light_gray">
<color rgba="0.698 0.698 0.698 1"/>
</material>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<mesh filename="meshes/PELVIS_S.STL"/>
</geometry>
</collision>
</link>
<link name="R_SHOULDER_P_S">
<inertial>
<origin xyz="-0.00982258725282141 -0.0704593093873431 -1.15069920925137E-06" rpy="0 0 0" />
<mass value="0.880737519698404" />
<inertia
ixx="0.000583190308378149"
ixy="1.53153074560471E-05"
ixz="-6.58466471736542E-09"
iyy="0.000445532440067178"
iyz="6.37001403998779E-09"
izz="0.000465648071069953" />
</inertial>
<visual>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<mesh filename="meshes/R_SHOULDER_P_S.STL" />
</geometry>
<material name="">
<color rgba="0.890196078431372 0.890196078431372 0.913725490196078 1" />
</material>
</visual>
<collision>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh filename="meshes/R_SHOULDER_P_S.STL" />
</geometry>
</collision>
</link>
<joint name="R_SHOULDER_P" type="revolute">
<origin xyz="0 -0.0945 0.042" rpy="0 0 0" />
<parent link="PELVIS_S" />
<child link="R_SHOULDER_P_S" />
<axis xyz="0 -1 0" />
<limit lower="-1.57" upper="1.57" effort="120" velocity="3.351" />
</joint>
<link name="R_SHOULDER_R_S">
<inertial>
<origin xyz="-0.0346025282975784 -0.09173932900033 1.86280643132974E-08" rpy="0 0 0" />
<mass value="0.59478842444248" />
<inertia
ixx="0.000380970396712653" ixy="-4.80751844573946E-05" ixz="1.34913746041655E-11"
iyy="0.000320962178597472" iyz="-1.00039763417375E-09" izz="0.000414771047901152" />
</inertial>
<visual>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<mesh filename="meshes/R_SHOULDER_R_S.STL" />
</geometry>
<material name="">
<color rgba="0.890196078431372 0.890196078431372 0.913725490196078 1" />
</material>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<mesh filename="meshes/R_SHOULDER_R_S.STL" />
</geometry>
</collision>
</link>
<joint name="R_SHOULDER_R" type="revolute">
<origin xyz="0.035 -0.0765 0" rpy="0 0 0" />
<parent link="R_SHOULDER_P_S" />
<child link="R_SHOULDER_R_S" />
<axis xyz="1 0 0" />
<limit lower="-2" upper="2" effort="120" velocity="3.351" />
</joint>
<link name="R_SHOULDER_Y_S">
<inertial>
<origin xyz="-0.00440976862014946 -0.0863620469295699 -7.58791862676134E-09" rpy="0 0 0" />
<mass value="0.563406026626801" />
<inertia
ixx="0.000327003834287551"
ixy="1.80574389667711E-05"
ixz="-7.28778136077729E-10"
iyy="0.000213830709361532"
iyz="1.49273668428957E-10"
izz="0.000297341029639189" />
</inertial>
<visual>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<mesh filename="meshes/R_SHOULDER_Y_S.STL" />
</geometry>
<material name="">
<color rgba="0.890196078431372 0.890196078431372 0.913725490196078 1" />
</material>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<mesh filename="meshes/R_SHOULDER_Y_S.STL" />
</geometry>
</collision>
</link>
<joint name="R_SHOULDER_Y" type="revolute">
<origin xyz="-0.035 -0.1475 0" rpy="0 0 0" />
<parent link="R_SHOULDER_R_S" />
<child link="R_SHOULDER_Y_S" />
<axis xyz="0 -1 0" />
<limit lower="0" upper="3.14" effort="80" velocity="3.8758" />
</joint>
<link name="R_ELBOW_R_S">
<inertial>
<origin xyz="-0.0335624237303424 -0.0603199974228191 -2.97736340082455E-07" rpy="0 0 0" />
<mass value="0.393571904406492" />
<inertia
ixx="0.000172771193862529"
ixy="-2.34549801867353E-05"
ixz="1.90560556668771E-09"
iyy="0.000155340245267897"
iyz="8.82493073602971E-09"
izz="0.00018104232734159" />
</inertial>
<visual>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<mesh filename="meshes/R_ELBOW_R_S.STL" />
</geometry>
<material name="">
<color rgba="0.890196078431372 0.890196078431372 0.913725490196078 1" />
</material>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<mesh filename="meshes/R_ELBOW_R_S.STL" />
</geometry>
</collision>
</link>
<joint name="R_ELBOW_R" type="revolute">
<origin xyz="0.034 -0.1025 0" rpy="0 0 0" />
<parent link="R_SHOULDER_Y_S" />
<child link="R_ELBOW_R_S" />
<axis xyz="1 0 0" />
<limit lower="0" upper="2.18" effort="80" velocity="3.8758" />
</joint>
<link name="R_WRIST_P_S">
<inertial>
<origin xyz="-1.39656577968772E-10 -0.0675972614865965 0.0192005515655728" rpy="0 0 0" />
<mass value="0.442332465815497" />
<inertia
ixx="0.000476754055455896"
ixy="-4.61462558956188E-15"
ixz="-5.91762926004267E-18"
iyy="0.0001034665858505"
iyz="4.88426705501212E-05"
izz="0.000482956345754214" />
</inertial>
<visual>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<mesh filename="meshes/R_WRIST_P_S.STL" />
</geometry>
<material name="">
<color rgba="0.647058823529412 0.619607843137255 0.588235294117647 1" />
</material>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<mesh filename="meshes/R_WRIST_P_S.STL" />
</geometry>
</collision>
</link>
<joint name="R_WRIST_P" type="revolute">
<origin xyz="-0.034 -0.0965 0" rpy="0 0 0" />
<parent link="R_ELBOW_R_S" />
<child link="R_WRIST_P_S" />
<axis xyz="0 -1 0" />
<limit lower="-3.14" upper="0" effort="50" velocity="4.71" />
</joint>
<link name="R_WRIST_Y_S">
<inertial>
<origin xyz="-0.00464135887330083 -5.06426456325926E-10 -0.0341253783666614" rpy="0 0 0" />
<mass value="0.235738477720019" />
<inertia
ixx="5.1068694045578E-05"
ixy="7.45771487459288E-16"
ixz="6.26195689233664E-06"
iyy="6.00636019624515E-05"
iyz="1.27828046342987E-16"
izz="5.32182903609188E-05" />
</inertial>
<visual>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<mesh filename="meshes/R_WRIST_Y_S.STL" />
</geometry>
<material name="">
<color rgba="0.647058823529412 0.619607843137255 0.588235294117647 1" />
</material>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<mesh filename="meshes/R_WRIST_Y_S.STL" />
</geometry>
</collision>
</link>
<joint name="R_WRIST_Y" type="revolute">
<origin xyz="0 -0.1525 0.039" rpy="0 0 0" />
<parent link="R_WRIST_P_S" />
<child link="R_WRIST_Y_S" />
<axis xyz="0 0 1" />
<limit lower="0" upper="0.78" effort="50" velocity="0.79" />
</joint>
<link name="R_WRIST_R_S">
<inertial>
<origin xyz="-0.0201642233698132 -0.11074968386657 -0.00598955339232021" rpy="0 0 0" />
<mass value="0.204366058218534" />
<inertia
ixx="0.000185559065855467"
ixy="5.75889766751509E-06"
ixz="2.40683898454438E-06"
iyy="0.000131246007872084"
iyz="1.60533277040148E-06"
izz="0.000271800951396149" />
</inertial>
<visual>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<mesh filename="meshes/R_WRIST_R_S.STL" />
</geometry>
<material name="">
<color rgba="0.890196078431372 0.890196078431372 0.913725490196078 1" />
</material>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<mesh filename="meshes/R_WRIST_R_S.STL" />
</geometry>
</collision>
</link>
<joint name="R_WRIST_R" type="revolute">
<origin xyz="0.03 0 -0.039" rpy="0 0 0" />
<parent link="R_WRIST_Y_S" />
<child link="R_WRIST_R_S" />
<axis xyz="1 0 0" />
<limit lower="-0.26" upper="1.57" effort="50" velocity="4.71" />
</joint>
<link name="R_FINGER_TIP">
<inertial>
<origin xyz="0 0 0" rpy="0 0 0"/>
<mass value="0"/>
<inertia ixx="1e-6" ixy="0" ixz="0" iyy="1e-6" iyz="0" izz="1e-6"/>
</inertial>
<visual>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<sphere radius="0.002"/>
</geometry>
<material name="">
<color rgba="0 1 0 1"/> <!-- 绿色 -->
</material>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<sphere radius="0.005"/>
</geometry>
</collision>
</link>
<joint name="R_FINGER_TIP_FIXED" type="fixed">
<origin xyz="-0.01212 -0.17655 0.07506" rpy="3.14159 0 -1.5708"/>
<parent link="R_WRIST_R_S"/>
<child link="R_FINGER_TIP"/>
</joint>
</robot>

63
robot_model/xiaoyan_description/right_arm.xml Normal file
View File

@ -0,0 +1,63 @@
<mujoco model="right_arm">
<compiler angle="radian" meshdir="meshes/"/>
<asset>
<mesh name="PELVIS_S" file="PELVIS_S.STL"/>
<mesh name="R_SHOULDER_P_S" file="R_SHOULDER_P_S.STL"/>
<mesh name="R_SHOULDER_R_S" file="R_SHOULDER_R_S.STL"/>
<mesh name="R_SHOULDER_Y_S" file="R_SHOULDER_Y_S.STL"/>
<mesh name="R_ELBOW_R_S" file="R_ELBOW_R_S.STL"/>
<mesh name="R_WRIST_P_S" file="R_WRIST_P_S.STL"/>
<mesh name="R_WRIST_Y_S" file="R_WRIST_Y_S.STL"/>
<mesh name="R_WRIST_R_S" file="R_WRIST_R_S.STL"/>
</asset>
<worldbody>
<geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.698 0.698 0.698 1" mesh="PELVIS_S"/>
<geom type="mesh" rgba="0.698 0.698 0.698 1" mesh="PELVIS_S"/>
<body name="R_SHOULDER_P_S" pos="0 -0.0945 0.042">
<inertial pos="-0.00982259 -0.0704593 -1.1507e-06" quat="0.706163 0.705933 0.0386962 0.0386741" mass="0.880738" diaginertia="0.000584874 0.000465648 0.000443849"/>
<joint name="R_SHOULDER_P" pos="0 0 0" axis="0 -1 0" range="-1.57 1.57" actuatorfrcrange="-120 120"/>
<geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.890196 0.890196 0.913725 1" mesh="R_SHOULDER_P_S"/>
<geom type="mesh" rgba="0.890196 0.890196 0.913725 1" mesh="R_SHOULDER_P_S"/>
<body name="R_SHOULDER_R_S" pos="0.035 -0.0765 0">
<inertial pos="-0.0346025 -0.0917393 1.86281e-08" quat="0.609323 0.358823 -0.609303 0.358778" mass="0.594788" diaginertia="0.000414771 0.000407636 0.000294296"/>
<joint name="R_SHOULDER_R" pos="0 0 0" axis="1 0 0" range="-2 2" actuatorfrcrange="-120 120"/>
<geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.890196 0.890196 0.913725 1" mesh="R_SHOULDER_R_S"/>
<geom type="mesh" rgba="0.890196 0.890196 0.913725 1" mesh="R_SHOULDER_R_S"/>
<body name="R_SHOULDER_Y_S" pos="-0.035 -0.1475 0">
<inertial pos="-0.00440977 -0.086362 -7.58792e-09" quat="0.705001 0.704998 0.054559 0.0545441" mass="0.563406" diaginertia="0.000329815 0.000297341 0.000211019"/>
<joint name="R_SHOULDER_Y" pos="0 0 0" axis="0 -1 0" range="0 3.14" actuatorfrcrange="-80 80"/>
<geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.890196 0.890196 0.913725 1" mesh="R_SHOULDER_Y_S"/>
<geom type="mesh" rgba="0.890196 0.890196 0.913725 1" mesh="R_SHOULDER_Y_S"/>
<body name="R_ELBOW_R_S" pos="0.034 -0.1025 0">
<inertial pos="-0.0335624 -0.06032 -2.97736e-07" quat="0.674756 0.674714 -0.211333 -0.211667" mass="0.393572" diaginertia="0.000189078 0.000181042 0.000139034"/>
<joint name="R_ELBOW_R" pos="0 0 0" axis="1 0 0" range="0 2.18" actuatorfrcrange="-80 80"/>
<geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.890196 0.890196 0.913725 1" mesh="R_ELBOW_R_S"/>
<geom type="mesh" rgba="0.890196 0.890196 0.913725 1" mesh="R_ELBOW_R_S"/>
<body name="R_WRIST_P_S" pos="-0.034 -0.0965 0">
<inertial pos="-1.39657e-10 -0.0675973 0.0192006" quat="0.530481 0.467537 -0.467537 0.530481" mass="0.442332" diaginertia="0.000489142 0.000476754 9.72811e-05"/>
<joint name="R_WRIST_P" pos="0 0 0" axis="0 -1 0" range="-3.14 0" actuatorfrcrange="-50 50"/>
<geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.647059 0.619608 0.588235 1" mesh="R_WRIST_P_S"/>
<geom type="mesh" rgba="0.647059 0.619608 0.588235 1" mesh="R_WRIST_P_S"/>
<body name="R_WRIST_Y_S" pos="0 -0.1525 0.039">
<inertial pos="-0.00464136 -5.06426e-10 -0.0341254" quat="0.298107 0.641196 0.641196 0.298107" mass="0.235738" diaginertia="6.00636e-05 5.8497e-05 4.579e-05"/>
<joint name="R_WRIST_Y" pos="0 0 0" axis="0 0 1" range="0 0.78" actuatorfrcrange="-50 50"/>
<geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.647059 0.619608 0.588235 1" mesh="R_WRIST_Y_S"/>
<geom type="mesh" rgba="0.647059 0.619608 0.588235 1" mesh="R_WRIST_Y_S"/>
<body name="R_WRIST_R_S" pos="0.03 0 -0.039">
<inertial pos="-0.0201642 -0.11075 -0.00598955" quat="0.483404 0.529786 -0.463203 0.520663" mass="0.204366" diaginertia="0.00027189 0.000186086 0.000130629"/>
<joint name="R_WRIST_R" pos="0 0 0" axis="1 0 0" range="-0.26 1.57" actuatorfrcrange="-50 50"/>
<geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.890196 0.890196 0.913725 1" mesh="R_WRIST_R_S"/>
<geom type="mesh" rgba="0.890196 0.890196 0.913725 1" mesh="R_WRIST_R_S"/>
<geom size="0.002" pos="-0.01212 -0.17655 0.07506" quat="9.38184e-07 0.707105 -0.707108 -9.38187e-07" contype="0" conaffinity="0" group="1" density="0" rgba="0 1 0 1"/>
<geom size="0.005" pos="-0.01212 -0.17655 0.07506" quat="9.38184e-07 0.707105 -0.707108 -9.38187e-07" rgba="0 1 0 1"/>
</body>
</body>
</body>
</body>
</body>
</body>
</body>
</worldbody>
</mujoco>

1
ui/__init__.py Normal file
View File

@ -0,0 +1 @@
# Package marker for UI modules.

BIN
ui/__pycache__/__init__.cpython-39.pyc Normal file
View File

Binary file not shown.

BIN
ui/__pycache__/app.cpython-39.pyc Normal file
View File

Binary file not shown.

BIN
ui/__pycache__/run_action.cpython-39.pyc Normal file
View File

Binary file not shown.

783
ui/app.py Normal file
View File

@ -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())

103
ui/run_action.py Normal file
View File

@ -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())