feat: 调整AGV移动方式

public/upper_body/assets/arm_link_1.part

@@ -0,0 +1,13 @@
+{
+    "configuration": "default",
+    "documentId": "3fb6fc43af8407628b8a436e",
+    "documentMicroversion": "5f27bb53d2c263aa503ee4fb",
+    "elementId": "8c088a46f41b8780aa1cf914",
+    "fullConfiguration": "default",
+    "id": "M77c5kvnmFYX3+E1/",
+    "isStandardContent": false,
+    "name": "arm_link_1 <2>",
+    "partId": "RHDD",
+    "suppressed": false,
+    "type": "Part"
+}

public/upper_body/assets/arm_link_2.part

@@ -0,0 +1,13 @@
+{
+    "configuration": "default",
+    "documentId": "3fb6fc43af8407628b8a436e",
+    "documentMicroversion": "5f27bb53d2c263aa503ee4fb",
+    "elementId": "8c088a46f41b8780aa1cf914",
+    "fullConfiguration": "default",
+    "id": "MAagFsEi4vOqUYBnE",
+    "isStandardContent": false,
+    "name": "arm_link_2 <2>",
+    "partId": "JvD",
+    "suppressed": false,
+    "type": "Part"
+}

public/upper_body/assets/arm_link_3.part

@@ -0,0 +1,13 @@
+{
+    "configuration": "default",
+    "documentId": "3fb6fc43af8407628b8a436e",
+    "documentMicroversion": "5f27bb53d2c263aa503ee4fb",
+    "elementId": "8c088a46f41b8780aa1cf914",
+    "fullConfiguration": "default",
+    "id": "MODZ2heuMlDmDK6JN",
+    "isStandardContent": false,
+    "name": "arm_link_3 <2>",
+    "partId": "RRBD",
+    "suppressed": false,
+    "type": "Part"
+}

public/upper_body/assets/arm_link_4.part

@@ -0,0 +1,13 @@
+{
+    "configuration": "default",
+    "documentId": "3fb6fc43af8407628b8a436e",
+    "documentMicroversion": "5f27bb53d2c263aa503ee4fb",
+    "elementId": "8c088a46f41b8780aa1cf914",
+    "fullConfiguration": "default",
+    "id": "MmEi+ZosIdG9Wp+iP",
+    "isStandardContent": false,
+    "name": "arm_link_4 <2>",
+    "partId": "RwCD",
+    "suppressed": false,
+    "type": "Part"
+}

public/upper_body/assets/arm_link_5.part

@@ -0,0 +1,13 @@
+{
+    "configuration": "default",
+    "documentId": "3fb6fc43af8407628b8a436e",
+    "documentMicroversion": "5f27bb53d2c263aa503ee4fb",
+    "elementId": "8c088a46f41b8780aa1cf914",
+    "fullConfiguration": "default",
+    "id": "MBPP2k1l9Xc6L3QvC",
+    "isStandardContent": false,
+    "name": "arm_link_5 <2>",
+    "partId": "RxCD",
+    "suppressed": false,
+    "type": "Part"
+}

public/upper_body/assets/arm_link_6.part

@@ -0,0 +1,13 @@
+{
+    "configuration": "default",
+    "documentId": "3fb6fc43af8407628b8a436e",
+    "documentMicroversion": "5f27bb53d2c263aa503ee4fb",
+    "elementId": "8c088a46f41b8780aa1cf914",
+    "fullConfiguration": "default",
+    "id": "MIcvuqnQVxIZF1ql0",
+    "isStandardContent": false,
+    "name": "arm_link_6 <2>",
+    "partId": "RyCD",
+    "suppressed": false,
+    "type": "Part"
+}

public/upper_body/assets/arm_link_7.part

@@ -0,0 +1,13 @@
+{
+    "configuration": "default",
+    "documentId": "3fb6fc43af8407628b8a436e",
+    "documentMicroversion": "5f27bb53d2c263aa503ee4fb",
+    "elementId": "8c088a46f41b8780aa1cf914",
+    "fullConfiguration": "default",
+    "id": "Mv//Etw9ckikOO5Dz",
+    "isStandardContent": false,
+    "name": "arm_link_7 <2>",
+    "partId": "RrCD",
+    "suppressed": false,
+    "type": "Part"
+}

public/upper_body/assets/body_link.part

@@ -0,0 +1,13 @@
+{
+    "configuration": "default",
+    "documentId": "3fb6fc43af8407628b8a436e",
+    "documentMicroversion": "5f27bb53d2c263aa503ee4fb",
+    "elementId": "0384359606fafa70e61c4a9c",
+    "fullConfiguration": "default",
+    "id": "MdJXCUi9nHtYjnfh4",
+    "isStandardContent": false,
+    "name": "body_link <1>",
+    "partId": "RAQD",
+    "suppressed": false,
+    "type": "Part"
+}

public/upper_body/assets/waist_link_1.part

@@ -0,0 +1,13 @@
+{
+    "configuration": "default",
+    "documentId": "3fb6fc43af8407628b8a436e",
+    "documentMicroversion": "5f27bb53d2c263aa503ee4fb",
+    "elementId": "384bd645b0ca4a89ac420dd3",
+    "fullConfiguration": "default",
+    "id": "MaMIM7xMUAfPCsjBw",
+    "isStandardContent": false,
+    "name": "waist_link_1 <1>",
+    "partId": "R2ED",
+    "suppressed": false,
+    "type": "Part"
+}

public/upper_body/assets/waist_link_2.part

@@ -0,0 +1,13 @@
+{
+    "configuration": "default",
+    "documentId": "3fb6fc43af8407628b8a436e",
+    "documentMicroversion": "5f27bb53d2c263aa503ee4fb",
+    "elementId": "384bd645b0ca4a89ac420dd3",
+    "fullConfiguration": "default",
+    "id": "M751ldLqHrvZy5x87",
+    "isStandardContent": false,
+    "name": "waist_link_2 <1>",
+    "partId": "RWFD",
+    "suppressed": false,
+    "type": "Part"
+}

public/upper_body/config.json

@@ -0,0 +1,10 @@
+// config.json general options
+// for urdf or mujoco specific options, see documentation
+{
+    // Onshape assembly URL
+    "url": "https://cad.onshape.com/documents/3fb6fc43af8407628b8a436e/w/97510b712561d1cd4f61c300/e/926f5c272b7ea2919e153a0d",
+    // Output format: urdf or mujoco (required)
+    "output_format": "urdf",
+    "merge_stls": true,
+    "simplify_stls": "visual"
+}

public/upper_body/robot.urdf

@@ -0,0 +1,541 @@
+<?xml version="1.0" ?>
+<!-- Generated using onshape-to-robot -->
+<!-- Onshape https://cad.onshape.com/documents/3fb6fc43af8407628b8a436e/w/97510b712561d1cd4f61c300/e/926f5c272b7ea2919e153a0d -->
+<robot name="upper_body">
+  <!-- Link body_link -->
+  <link name="body_link">
+    <inertial>
+      <origin xyz="-0.00770062 -4.54985e-06 -0.105557" rpy="0 0 0"/>
+      <mass value="14.6288"/>
+      <inertia ixx="0.245371" ixy="-1.70664e-06" ixz="-0.00216387" iyy="0.195814" iyz="1.53223e-05" izz="0.131523"/>
+    </inertial>
+    <!-- Part body_link_parts -->
+    <visual>
+      <origin xyz="-0.00770062 -4.54985e-06 -0.105557" rpy="0 -0 0"/>
+      <geometry>
+        <mesh filename="package://assets/merged/body_link_visual.stl"/>
+      </geometry>
+      <material name="body_link_parts_material">
+        <color rgba="0.980392 0.713725 0.00392157 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="-0.00770062 -4.54985e-06 -0.105557" rpy="0 -0 0"/>
+      <geometry>
+        <mesh filename="package://assets/merged/body_link_collision.stl"/>
+      </geometry>
+    </collision>
+  </link>
+  <!-- Link arm_link_1 -->
+  <link name="arm_link_1">
+    <inertial>
+      <origin xyz="-1.06869e-05 -0.00369804 -0.0615825" rpy="0 0 0"/>
+      <mass value="2.2038"/>
+      <inertia ixx="0.00398711" ixy="-6.128e-09" ixz="-9.9572e-08" iyy="0.00332264" iyz="-4.52028e-05" izz="0.00327232"/>
+    </inertial>
+    <!-- Part arm_link_1_parts -->
+    <visual>
+      <origin xyz="-1.06869e-05 -0.00369804 -0.0615825" rpy="-1.5708 1e-11 -1.57077"/>
+      <geometry>
+        <mesh filename="package://assets/merged/arm_link_1_visual.stl"/>
+      </geometry>
+      <material name="arm_link_1_parts_material">
+        <color rgba="0.901961 0.901961 0.901961 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="-1.06869e-05 -0.00369804 -0.0615825" rpy="-1.5708 1e-11 -1.57077"/>
+      <geometry>
+        <mesh filename="package://assets/merged/arm_link_1_collision.stl"/>
+      </geometry>
+    </collision>
+  </link>
+  <!-- Link arm_link_2 -->
+  <link name="arm_link_2">
+    <inertial>
+      <origin xyz="0.0860616 3.31509e-05 0.0345088" rpy="0 0 0"/>
+      <mass value="1.9456"/>
+      <inertia ixx="0.00232848" ixy="8.86713e-07" ixz="-0.000218608" iyy="0.00607768" iyz="-4.37798e-07" izz="0.00629001"/>
+    </inertial>
+    <!-- Part arm_link_2_parts -->
+    <visual>
+      <origin xyz="0.0860616 3.31509e-05 0.0345088" rpy="4.54761e-07 1.57077 -1.5708"/>
+      <geometry>
+        <mesh filename="package://assets/merged/arm_link_2_visual.stl"/>
+      </geometry>
+      <material name="arm_link_2_parts_material">
+        <color rgba="0.901961 0.901961 0.901961 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0.0860616 3.31509e-05 0.0345088" rpy="4.54761e-07 1.57077 -1.5708"/>
+      <geometry>
+        <mesh filename="package://assets/merged/arm_link_2_collision.stl"/>
+      </geometry>
+    </collision>
+  </link>
+  <!-- Link arm_link_3 -->
+  <link name="arm_link_3">
+    <inertial>
+      <origin xyz="4.40212e-05 -0.00236525 -0.104181" rpy="0 0 0"/>
+      <mass value="1.94886"/>
+      <inertia ixx="0.00638928" ixy="-2.96534e-06" ixz="3.84694e-06" iyy="0.00598683" iyz="-0.000187472" izz="0.00215638"/>
+    </inertial>
+    <!-- Part arm_link_3_parts -->
+    <visual>
+      <origin xyz="4.40212e-05 -0.00236525 -0.104181" rpy="-1.5708 1e-11 -1.57077"/>
+      <geometry>
+        <mesh filename="package://assets/merged/arm_link_3_visual.stl"/>
+      </geometry>
+      <material name="arm_link_3_parts_material">
+        <color rgba="0.901961 0.901961 0.901961 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="4.40212e-05 -0.00236525 -0.104181" rpy="-1.5708 1e-11 -1.57077"/>
+      <geometry>
+        <mesh filename="package://assets/merged/arm_link_3_collision.stl"/>
+      </geometry>
+    </collision>
+  </link>
+  <!-- Link arm_link_4 -->
+  <link name="arm_link_4">
+    <inertial>
+      <origin xyz="0.0678998 -1.73447e-05 0.0344772" rpy="0 0 0"/>
+      <mass value="0.916518"/>
+      <inertia ixx="0.000809181" ixy="5.12238e-07" ixz="-8.56762e-05" iyy="0.00204112" iyz="-3.3225e-08" izz="0.00203466"/>
+    </inertial>
+    <!-- Part arm_link_4_parts -->
+    <visual>
+      <origin xyz="0.0678998 -1.73447e-05 0.0344772" rpy="4.54761e-07 1.57077 -1.5708"/>
+      <geometry>
+        <mesh filename="package://assets/merged/arm_link_4_visual.stl"/>
+      </geometry>
+      <material name="arm_link_4_parts_material">
+        <color rgba="0.901961 0.901961 0.901961 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0.0678998 -1.73447e-05 0.0344772" rpy="4.54761e-07 1.57077 -1.5708"/>
+      <geometry>
+        <mesh filename="package://assets/merged/arm_link_4_collision.stl"/>
+      </geometry>
+    </collision>
+  </link>
+  <!-- Link arm_link_5 -->
+  <link name="arm_link_5">
+    <inertial>
+      <origin xyz="0.00175699 -4.98862e-07 -0.0504805" rpy="0 0 0"/>
+      <mass value="0.843691"/>
+      <inertia ixx="0.00115484" ixy="-4.1705e-08" ixz="2.59664e-05" iyy="0.00131635" iyz="1.16286e-07" izz="0.000742072"/>
+    </inertial>
+    <!-- Part arm_link_5_parts -->
+    <visual>
+      <origin xyz="0.00175699 -4.98862e-07 -0.0504805" rpy="-1.5708 1e-11 -1.57077"/>
+      <geometry>
+        <mesh filename="package://assets/merged/arm_link_5_visual.stl"/>
+      </geometry>
+      <material name="arm_link_5_parts_material">
+        <color rgba="0.901961 0.901961 0.901961 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0.00175699 -4.98862e-07 -0.0504805" rpy="-1.5708 1e-11 -1.57077"/>
+      <geometry>
+        <mesh filename="package://assets/merged/arm_link_5_collision.stl"/>
+      </geometry>
+    </collision>
+  </link>
+  <!-- Link arm_link_6 -->
+  <link name="arm_link_6">
+    <inertial>
+      <origin xyz="9.27263e-05 -0.004905 -0.0280094" rpy="0 0 0"/>
+      <mass value="0.525446"/>
+      <inertia ixx="0.000275345" ixy="-1.06372e-06" ixz="-5.7647e-08" iyy="0.000233524" iyz="-3.51729e-06" izz="0.000268736"/>
+    </inertial>
+    <!-- Part arm_link_6_parts -->
+    <visual>
+      <origin xyz="9.27263e-05 -0.004905 -0.0280094" rpy="0 -2.25524e-05 -1.5708"/>
+      <geometry>
+        <mesh filename="package://assets/merged/arm_link_6_visual.stl"/>
+      </geometry>
+      <material name="arm_link_6_parts_material">
+        <color rgba="0.901961 0.901961 0.901961 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="9.27263e-05 -0.004905 -0.0280094" rpy="0 -2.25524e-05 -1.5708"/>
+      <geometry>
+        <mesh filename="package://assets/merged/arm_link_6_collision.stl"/>
+      </geometry>
+    </collision>
+  </link>
+  <!-- Link arm_link_7 -->
+  <link name="arm_link_7">
+    <inertial>
+      <origin xyz="0.0288794 -1.4907e-07 0.0339252" rpy="0 0 0"/>
+      <mass value="0.13521"/>
+      <inertia ixx="0.000149861" ixy="1.66e-10" ixz="-8.09464e-06" iyy="0.000202616" iyz="-2.81e-10" izz="0.000110973"/>
+    </inertial>
+    <!-- Part arm_link_7_parts -->
+    <visual>
+      <origin xyz="0.0288794 -1.4907e-07 0.0339252" rpy="3.96342e-07 1.57077 -1.5708"/>
+      <geometry>
+        <mesh filename="package://assets/merged/arm_link_7_visual.stl"/>
+      </geometry>
+      <material name="arm_link_7_parts_material">
+        <color rgba="0.901961 0.901961 0.901961 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0.0288794 -1.4907e-07 0.0339252" rpy="3.96342e-07 1.57077 -1.5708"/>
+      <geometry>
+        <mesh filename="package://assets/merged/arm_link_7_collision.stl"/>
+      </geometry>
+    </collision>
+  </link>
+  <!-- Joint from arm_link_6 to arm_link_7 -->
+  <joint name="left_arm_J7" type="revolute">
+    <origin xyz="0 -0.036 -0.028" rpy="-1.5708 -0 0"/>
+    <parent link="arm_link_6"/>
+    <child link="arm_link_7"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="10" velocity="10" lower="-3.141592653589793" upper="3.141592653589793"/>
+  </joint>
+  <!-- Joint from arm_link_5 to arm_link_6 -->
+  <joint name="left_arm_J6" type="revolute">
+    <origin xyz="0.0295 0 -0.131" rpy="0 1.5708 0"/>
+    <parent link="arm_link_5"/>
+    <child link="arm_link_6"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="10" velocity="10" lower="-3.141592653589793" upper="3.141592653589793"/>
+  </joint>
+  <!-- Joint from arm_link_4 to arm_link_5 -->
+  <joint name="left_arm_J5" type="revolute">
+    <origin xyz="0.1215 -0 0.036" rpy="1.5708 -0 -1.5708"/>
+    <parent link="arm_link_4"/>
+    <child link="arm_link_5"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="10" velocity="10" lower="-3.141592653589793" upper="3.141592653589793"/>
+  </joint>
+  <!-- Joint from arm_link_3 to arm_link_4 -->
+  <joint name="left_arm_J4" type="revolute">
+    <origin xyz="-7.46e-10 -0.036 -0.146" rpy="-1.5708 1.5708 0"/>
+    <parent link="arm_link_3"/>
+    <child link="arm_link_4"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="10" velocity="10" lower="-3.141592653589793" upper="3.141592653589793"/>
+  </joint>
+  <!-- Joint from arm_link_2 to arm_link_3 -->
+  <joint name="left_arm_J3" type="revolute">
+    <origin xyz="0.1565 -2.41e-10 0.036" rpy="1.5708 0 -1.5708"/>
+    <parent link="arm_link_2"/>
+    <child link="arm_link_3"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="10" velocity="10" lower="-3.141592653589793" upper="3.141592653589793"/>
+  </joint>
+  <!-- Joint from arm_link_1 to arm_link_2 -->
+  <joint name="left_arm_J2" type="revolute">
+    <origin xyz="-0 -0.036 -0.071" rpy="-1.5708 1.5708 0"/>
+    <parent link="arm_link_1"/>
+    <child link="arm_link_2"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="10" velocity="10" lower="-3.141592653589793" upper="3.141592653589793"/>
+  </joint>
+  <!-- Joint from body_link to arm_link_1 -->
+  <joint name="left_arm_J1" type="revolute">
+    <origin xyz="-0 0.1425 -0.004" rpy="1.5708 -1.57077 -0"/>
+    <parent link="body_link"/>
+    <child link="arm_link_1"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="10" velocity="10" lower="-3.141592653589793" upper="3.141592653589793"/>
+  </joint>
+  <!-- Link arm_link_1_2 -->
+  <link name="arm_link_1_2">
+    <inertial>
+      <origin xyz="-1.06869e-05 -0.00369804 -0.0615825" rpy="0 0 0"/>
+      <mass value="2.2038"/>
+      <inertia ixx="0.00398711" ixy="-6.128e-09" ixz="-9.9572e-08" iyy="0.00332264" iyz="-4.52028e-05" izz="0.00327232"/>
+    </inertial>
+    <!-- Part arm_link_1_2_parts -->
+    <visual>
+      <origin xyz="-1.06869e-05 -0.00369804 -0.0615825" rpy="1.5708 -0 -1.5708"/>
+      <geometry>
+        <mesh filename="package://assets/merged/arm_link_1_2_visual.stl"/>
+      </geometry>
+      <material name="arm_link_1_2_parts_material">
+        <color rgba="0.901961 0.901961 0.901961 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="-1.06869e-05 -0.00369804 -0.0615825" rpy="1.5708 -0 -1.5708"/>
+      <geometry>
+        <mesh filename="package://assets/merged/arm_link_1_2_collision.stl"/>
+      </geometry>
+    </collision>
+  </link>
+  <!-- Link arm_link_2_2 -->
+  <link name="arm_link_2_2">
+    <inertial>
+      <origin xyz="0.0860616 3.31509e-05 0.0345088" rpy="0 0 0"/>
+      <mass value="1.9456"/>
+      <inertia ixx="0.00232848" ixy="8.86713e-07" ixz="-0.000218608" iyy="0.00607768" iyz="-4.37798e-07" izz="0.00629001"/>
+    </inertial>
+    <!-- Part arm_link_2_2_parts -->
+    <visual>
+      <origin xyz="0.0860616 3.31509e-05 0.0345088" rpy="-1.5708 1.5708 0"/>
+      <geometry>
+        <mesh filename="package://assets/merged/arm_link_2_2_visual.stl"/>
+      </geometry>
+      <material name="arm_link_2_2_parts_material">
+        <color rgba="0.901961 0.901961 0.901961 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0.0860616 3.31509e-05 0.0345088" rpy="-1.5708 1.5708 0"/>
+      <geometry>
+        <mesh filename="package://assets/merged/arm_link_2_2_collision.stl"/>
+      </geometry>
+    </collision>
+  </link>
+  <!-- Link arm_link_3_2 -->
+  <link name="arm_link_3_2">
+    <inertial>
+      <origin xyz="4.40212e-05 -0.00236525 -0.104181" rpy="0 0 0"/>
+      <mass value="1.94886"/>
+      <inertia ixx="0.00638928" ixy="-2.96534e-06" ixz="3.84694e-06" iyy="0.00598683" iyz="-0.000187472" izz="0.00215638"/>
+    </inertial>
+    <!-- Part arm_link_3_2_parts -->
+    <visual>
+      <origin xyz="4.40212e-05 -0.00236525 -0.104181" rpy="1.5708 -0 -1.5708"/>
+      <geometry>
+        <mesh filename="package://assets/merged/arm_link_3_2_visual.stl"/>
+      </geometry>
+      <material name="arm_link_3_2_parts_material">
+        <color rgba="0.901961 0.901961 0.901961 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="4.40212e-05 -0.00236525 -0.104181" rpy="1.5708 -0 -1.5708"/>
+      <geometry>
+        <mesh filename="package://assets/merged/arm_link_3_2_collision.stl"/>
+      </geometry>
+    </collision>
+  </link>
+  <!-- Link arm_link_4_2 -->
+  <link name="arm_link_4_2">
+    <inertial>
+      <origin xyz="0.0678998 -1.73447e-05 0.0344772" rpy="0 0 0"/>
+      <mass value="0.916518"/>
+      <inertia ixx="0.000809181" ixy="5.12238e-07" ixz="-8.56762e-05" iyy="0.00204112" iyz="-3.3225e-08" izz="0.00203466"/>
+    </inertial>
+    <!-- Part arm_link_4_2_parts -->
+    <visual>
+      <origin xyz="0.0678998 -1.73447e-05 0.0344772" rpy="-1.5708 1.5708 0"/>
+      <geometry>
+        <mesh filename="package://assets/merged/arm_link_4_2_visual.stl"/>
+      </geometry>
+      <material name="arm_link_4_2_parts_material">
+        <color rgba="0.901961 0.901961 0.901961 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0.0678998 -1.73447e-05 0.0344772" rpy="-1.5708 1.5708 0"/>
+      <geometry>
+        <mesh filename="package://assets/merged/arm_link_4_2_collision.stl"/>
+      </geometry>
+    </collision>
+  </link>
+  <!-- Link arm_link_5_2 -->
+  <link name="arm_link_5_2">
+    <inertial>
+      <origin xyz="0.00175699 -4.98862e-07 -0.0504805" rpy="0 0 0"/>
+      <mass value="0.843691"/>
+      <inertia ixx="0.00115484" ixy="-4.1705e-08" ixz="2.59664e-05" iyy="0.00131635" iyz="1.16286e-07" izz="0.000742072"/>
+    </inertial>
+    <!-- Part arm_link_5_2_parts -->
+    <visual>
+      <origin xyz="0.00175699 -4.98862e-07 -0.0504805" rpy="1.5708 -0 -1.5708"/>
+      <geometry>
+        <mesh filename="package://assets/merged/arm_link_5_2_visual.stl"/>
+      </geometry>
+      <material name="arm_link_5_2_parts_material">
+        <color rgba="0.901961 0.901961 0.901961 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0.00175699 -4.98862e-07 -0.0504805" rpy="1.5708 -0 -1.5708"/>
+      <geometry>
+        <mesh filename="package://assets/merged/arm_link_5_2_collision.stl"/>
+      </geometry>
+    </collision>
+  </link>
+  <!-- Link arm_link_6_2 -->
+  <link name="arm_link_6_2">
+    <inertial>
+      <origin xyz="9.27263e-05 -0.004905 -0.0280094" rpy="0 0 0"/>
+      <mass value="0.525446"/>
+      <inertia ixx="0.000275345" ixy="-1.06372e-06" ixz="-5.7647e-08" iyy="0.000233524" iyz="-3.51729e-06" izz="0.000268736"/>
+    </inertial>
+    <!-- Part arm_link_6_2_parts -->
+    <visual>
+      <origin xyz="9.27263e-05 -0.004905 -0.0280094" rpy="3.14159 -0 -1.5708"/>
+      <geometry>
+        <mesh filename="package://assets/merged/arm_link_6_2_visual.stl"/>
+      </geometry>
+      <material name="arm_link_6_2_parts_material">
+        <color rgba="0.901961 0.901961 0.901961 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="9.27263e-05 -0.004905 -0.0280094" rpy="3.14159 -0 -1.5708"/>
+      <geometry>
+        <mesh filename="package://assets/merged/arm_link_6_2_collision.stl"/>
+      </geometry>
+    </collision>
+  </link>
+  <!-- Link arm_link_7_2 -->
+  <link name="arm_link_7_2">
+    <inertial>
+      <origin xyz="0.0288794 -1.4907e-07 0.0339252" rpy="0 0 0"/>
+      <mass value="0.13521"/>
+      <inertia ixx="0.000149861" ixy="1.66e-10" ixz="-8.09464e-06" iyy="0.000202616" iyz="-2.81e-10" izz="0.000110973"/>
+    </inertial>
+    <!-- Part arm_link_7_2_parts -->
+    <visual>
+      <origin xyz="0.0288794 -1.4907e-07 0.0339252" rpy="-1.5708 1.5708 0"/>
+      <geometry>
+        <mesh filename="package://assets/merged/arm_link_7_2_visual.stl"/>
+      </geometry>
+      <material name="arm_link_7_2_parts_material">
+        <color rgba="0.901961 0.901961 0.901961 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0.0288794 -1.4907e-07 0.0339252" rpy="-1.5708 1.5708 0"/>
+      <geometry>
+        <mesh filename="package://assets/merged/arm_link_7_2_collision.stl"/>
+      </geometry>
+    </collision>
+  </link>
+  <!-- Joint from arm_link_6_2 to arm_link_7_2 -->
+  <joint name="right_arm_J7" type="revolute">
+    <origin xyz="0 -0.036 -0.028" rpy="-1.5708 -0 0"/>
+    <parent link="arm_link_6_2"/>
+    <child link="arm_link_7_2"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="10" velocity="10" lower="-3.141592653589793" upper="3.141592653589793"/>
+  </joint>
+  <!-- Joint from arm_link_5_2 to arm_link_6_2 -->
+  <joint name="right_arm_J6" type="revolute">
+    <origin xyz="0.0295 0 -0.131" rpy="-0 1.5708 0"/>
+    <parent link="arm_link_5_2"/>
+    <child link="arm_link_6_2"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="10" velocity="10" lower="-3.141592653589793" upper="3.141592653589793"/>
+  </joint>
+  <!-- Joint from arm_link_4_2 to arm_link_5_2 -->
+  <joint name="right_arm_J5" type="revolute">
+    <origin xyz="0.1215 -0 0.036" rpy="1.5708 -0 -1.5708"/>
+    <parent link="arm_link_4_2"/>
+    <child link="arm_link_5_2"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="10" velocity="10" lower="-3.141592653589793" upper="3.141592653589793"/>
+  </joint>
+  <!-- Joint from arm_link_3_2 to arm_link_4_2 -->
+  <joint name="right_arm_J4" type="revolute">
+    <origin xyz="-7.46e-10 -0.036 -0.146" rpy="-1.5708 1.5708 0"/>
+    <parent link="arm_link_3_2"/>
+    <child link="arm_link_4_2"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="10" velocity="10" lower="-3.141592653589793" upper="3.141592653589793"/>
+  </joint>
+  <!-- Joint from arm_link_2_2 to arm_link_3_2 -->
+  <joint name="right_arm_J3" type="revolute">
+    <origin xyz="0.1565 -2.41e-10 0.036" rpy="1.5708 -0 -1.5708"/>
+    <parent link="arm_link_2_2"/>
+    <child link="arm_link_3_2"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="10" velocity="10" lower="-3.141592653589793" upper="3.141592653589793"/>
+  </joint>
+  <!-- Joint from arm_link_1_2 to arm_link_2_2 -->
+  <joint name="right_arm_J2" type="revolute">
+    <origin xyz="-0 -0.036 -0.071" rpy="-1.5708 1.5708 0"/>
+    <parent link="arm_link_1_2"/>
+    <child link="arm_link_2_2"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="10" velocity="10" lower="-3.141592653589793" upper="3.141592653589793"/>
+  </joint>
+  <!-- Joint from body_link to arm_link_1_2 -->
+  <joint name="right_arm_J1" type="revolute">
+    <origin xyz="1.9742e-08 -0.1425 -0.00399998" rpy="-1.5708 1.5708 0"/>
+    <parent link="body_link"/>
+    <child link="arm_link_1_2"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="10" velocity="10" lower="-3.141592653589793" upper="3.141592653589793"/>
+  </joint>
+  <!-- Link waist_link_1 -->
+  <link name="waist_link_1">
+    <inertial>
+      <origin xyz="3.11467e-06 0.00674712 -0.0457153" rpy="0 0 0"/>
+      <mass value="2.07988"/>
+      <inertia ixx="0.00276958" ixy="-8.367e-08" ixz="8.1856e-07" iyy="0.00250298" iyz="0.000161404" izz="0.00275977"/>
+    </inertial>
+    <!-- Part waist_link_1_parts -->
+    <visual>
+      <origin xyz="3.11467e-06 0.00674712 -0.0457153" rpy="-0 -0 -1.5708"/>
+      <geometry>
+        <mesh filename="package://assets/merged/waist_link_1_visual.stl"/>
+      </geometry>
+      <material name="waist_link_1_parts_material">
+        <color rgba="0.972549 0.529412 0.00392157 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="3.11467e-06 0.00674712 -0.0457153" rpy="-0 -0 -1.5708"/>
+      <geometry>
+        <mesh filename="package://assets/merged/waist_link_1_collision.stl"/>
+      </geometry>
+    </collision>
+  </link>
+  <!-- Link waist_link_2 -->
+  <link name="waist_link_2">
+    <inertial>
+      <origin xyz="1.34175e-07 -0.0201253 -0.0130557" rpy="0 0 0"/>
+      <mass value="4.6278"/>
+      <inertia ixx="0.0212779" ixy="-2.77841e-07" ixz="1.15998e-06" iyy="0.0217788" iyz="0.00445321" izz="0.0141532"/>
+    </inertial>
+    <!-- Part waist_link_2_parts -->
+    <visual>
+      <origin xyz="1.34175e-07 -0.0201253 -0.0130557" rpy="-1.5708 -1.5708 0"/>
+      <geometry>
+        <mesh filename="package://assets/merged/waist_link_2_visual.stl"/>
+      </geometry>
+      <material name="waist_link_2_parts_material">
+        <color rgba="0.498039 0.498039 0.498039 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="1.34175e-07 -0.0201253 -0.0130557" rpy="-1.5708 -1.5708 0"/>
+      <geometry>
+        <mesh filename="package://assets/merged/waist_link_2_collision.stl"/>
+      </geometry>
+    </collision>
+  </link>
+  <!-- Joint from waist_link_1 to waist_link_2 -->
+  <joint name="waist_J2" type="revolute">
+    <origin xyz="0 0.058 -0.058" rpy="1.5708 2.1e-11 -0"/>
+    <parent link="waist_link_1"/>
+    <child link="waist_link_2"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="10" velocity="10" lower="-3.141592653589793" upper="3.141592653589793"/>
+  </joint>
+  <!-- Joint from body_link to waist_link_1 -->
+  <joint name="waist_J1" type="revolute">
+    <origin xyz="0.045 0 -0.367" rpy="0 -0 1.5708"/>
+    <parent link="body_link"/>
+    <child link="waist_link_1"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="10" velocity="10" lower="-3.141592653589793" upper="3.141592653589793"/>
+  </joint>
+</robot>

server/index.js

@@ -234,7 +234,7 @@ app.post('/api/agv/moveRobot', async (req, res) => {
     velocity: {
       vx: req.body.vx,
       vy: req.body.vy,
-      vz: req.body.vz
+      wz: req.body.wz
     }
   };
 

src/views/Address.vue

@@ -92,7 +92,7 @@ const testConnect = async () => {
         robotStore.setRobotType(ruleForm.value.robotType)
         if (ruleForm.value.robotType === 'inspection') {
             robotStore.setAgvDeviceId('src1100')
-            robotStore.setAlmDeviceId('huayan_arm')
+            robotStore.setAlmDeviceId('aubo_arm')
             robotStore.setSpkDeviceId('spk1')
             robotStore.setMicDeviceId('mic1')
             robotStore.setCameraDeviceId('real_cam1')
@@ -104,24 +104,24 @@ const testConnect = async () => {
             robotStore.setCameraDeviceId('real_cam1')
         }
         
-        // const result = await getRuntimeState({
-        //     ip: robotStore.ip,
-        //     deviceId: ruleForm.value.agvDeviceId
-        // })
+        const result = await getRuntimeState({
+            ip: robotStore.ip,
+            deviceId: ruleForm.value.agvDeviceId
+        })
         loading.value = false
-        // if (result.code === 200) {
-        //     robotStore.setPosition(result.data.pose)
-        //     robotStore.setBattery(result.data.battery)
-        //     robotStore.setCurrentMap(result.data.current_map)
-        //     router.push({
-        //         path: '/home'
-        //     })
-        // } else {
-        //     ElMessage.error(result?.message || '连接服务错误')
-        // }
-        router.push({
+        if (result.code === 200) {
+            robotStore.setPosition(result.data.pose)
+            robotStore.setBattery(result.data.battery)
+            robotStore.setCurrentMap(result.data.current_map)
+            router.push({
                 path: '/home'
             })
+        } else {
+            ElMessage.error(result?.message || '连接服务错误')
+        }
+        router.push({
+            path: '/home'
+        })
     }
 }
 

src/views/arm/UrdfView.vue

@@ -7,26 +7,45 @@
 </template>
 
 <script setup>
-import { ref, onMounted, onBeforeUnmount, nextTick, reactive } from 'vue';
+import { computed, ref, onMounted, onBeforeUnmount, nextTick, reactive } from 'vue';
 import * as THREE from 'three';
 import { OrbitControls } from 'three/examples/jsm/controls/OrbitControls.js';
-import { STLLoader } from 'three/examples/jsm/loaders/STLLoader.js';
+import URDFLoader from 'urdf-loader';
 
 import { XacroAdapter } from '@/adapters/XacroAdapter.js'
-import { fileNameMap } from './config'
+import { URDFAdapter } from '@/adapters/URDFAdapter.js'
 import { getThemeColor } from '@/utils/themeColors.js'
 
 const props = defineProps({
+  modelPath: {
+    type: String,
+    default: '/inspection/elfin10/elfin10.urdf.xacro'
+  },
   filePath: {
     type: String,
-    default: '/inspection/elfin10/'
+    default: ''
   },
   fileName: {
     type: String,
-    default: 'elfin10.urdf.xacro'
+    default: ''
   }
 })
 
+const normalizePublicPath = (path) => {
+  let normalized = path.trim().replace(/\\/g, '/')
+  normalized = normalized.replace(/^[a-zA-Z]:.*?\/public\//, '/')
+  normalized = normalized.replace(/^\.?\/?public\//, '/')
+  return normalized.startsWith('/') ? normalized : `/${normalized}`
+}
+
+const modelUrl = computed(() => normalizePublicPath(
+  props.filePath && props.fileName
+    ? `${props.filePath.replace(/[\\/]?$/, '/')}${props.fileName}`
+    : props.modelPath
+))
+
+const modelDirectory = computed(() => modelUrl.value.slice(0, modelUrl.value.lastIndexOf('/') + 1))
+
 const viewerContainer = ref(null);
 
 let scene, camera, renderer, controls;
@@ -97,33 +116,76 @@ const resetJoints = () => {
 };
 
 const fileMap = new Map()
+const resolveReferencedUrl = (reference, parentUrl) => {
+  const normalized = reference.replace(/\\/g, '/')
+  const rosPath = normalized.match(/^(?:file:\/\/)?\$\(find\s+[^)]+\)\/(.+)$/)
+  if (rosPath) return `${modelDirectory.value}${rosPath[1]}`
+
+  const packagePath = normalized.match(/^package:\/\/[^/]+\/(.+)$/)
+  if (packagePath) return `${modelDirectory.value}${packagePath[1]}`
+
+  if (/^(?:https?:|blob:|data:)/i.test(normalized)) return normalized
+  return new URL(normalized, new URL(parentUrl, window.location.origin)).pathname
+}
+
 const loadFiles = async () => {
-  const fileNames = fileNameMap[props.filePath]
-  const fetchPromises = fileNames.map(async (name) => {
-    const url = `${props.filePath}${name}`
+  fileMap.clear()
+  if (!modelUrl.value.toLowerCase().endsWith('.xacro')) return
+
+  const pendingUrls = [modelUrl.value]
+  const visitedUrls = new Set()
+  while (pendingUrls.length) {
+    const url = pendingUrls.shift()
+    if (visitedUrls.has(url)) continue
+    visitedUrls.add(url)
+
     const response = await fetch(url)
+    if (!response.ok) throw new Error(`加载模型依赖失败：${url} (HTTP ${response.status})`)
     const blob = await response.blob()
-    return new File([blob], name, { type: blob.type })
-  })
-  const fileList = await Promise.all(fetchPromises)
-  fileList.forEach(item => {
-    fileMap.set(props.filePath + item.name, item);
-  })
+    const fileName = url.split('/').pop()
+    const file = new File([blob], fileName, { type: blob.type })
+    fileMap.set(url, file)
+
+    if (!/\.(?:xacro|urdf|xml)$/i.test(fileName)) continue
+    const content = await blob.text()
+    const references = [...content.matchAll(/\b(?:filename|url)=["']([^"']+)["']/gi)]
+    references.forEach(([, reference]) => {
+      const referencedUrl = resolveReferencedUrl(reference, url)
+      if (!visitedUrls.has(referencedUrl)) pendingUrls.push(referencedUrl)
+    })
+  }
 }
 
 const currentModel = ref()
 let robotInstance = null; // 保存 URDFRobot 对象
 
-const loadRobot = async () => {
-  const xacroUrl = `${props.filePath}${props.fileName}`;
+const parseUrdf = async (urdfContent) => {
+  const loader = new URDFLoader()
+  loader.parseCollision = true
 
-  const response = await fetch(xacroUrl);
-  if (!response.ok) throw new Error(`HTTP ${response.status}`);
-  const xacroContent = await response.text();
-  const file = fileMap.get(xacroUrl)
+  const packageNames = [...urdfContent.matchAll(/package:\/\/([^/]+)/g)].map(match => match[1])
+  loader.packages = Object.fromEntries(
+    [...new Set(packageNames)].map(packageName => [packageName, `${modelDirectory.value}${packageName}`])
+  )
+
+  const robot = loader.parse(urdfContent, modelDirectory.value)
+  return URDFAdapter.convert(robot, urdfContent)
+}
+
+const loadRobot = async () => {
+  const response = await fetch(modelUrl.value);
+  if (!response.ok) throw new Error(`加载模型失败：${modelUrl.value} (HTTP ${response.status})`);
+  const content = await response.text();
+  const fileName = modelUrl.value.split('/').pop()
+  const lowerCaseFileName = fileName.toLowerCase()
   let model
-  if (props.fileName.split('.').pop() === 'xacro') {
-    model = await XacroAdapter.parse(xacroContent, file.name, fileMap, file);
+  if (lowerCaseFileName.endsWith('.xacro')) {
+    const file = fileMap.get(modelUrl.value) || new File([content], fileName, { type: 'text/xml' })
+    model = await XacroAdapter.parse(content, file.name, fileMap, file);
+  } else if (lowerCaseFileName.endsWith('.urdf')) {
+    model = await parseUrdf(content)
+  } else {
+    throw new Error(`不支持的模型格式：${fileName}，仅支持 .xacro 和 .urdf`)
   }
 
   const robot = model.threeObject; 

src/views/chassis/Chassis.vue

@@ -89,7 +89,7 @@
             />
           </div>
         </div>
-        <div class="card">
+        <!-- <div class="card">
             <el-switch
                 v-model="isDragging"
                 size="large"
@@ -114,7 +114,7 @@
                     ⬣ 紧急停止
                 </el-button>
             </div>
-        </div>
+        </div> -->
       </aside>
     </div>
 
@@ -150,7 +150,7 @@ const rotationAngle = ref(0)
 
 const paramList = reactive([
   { key: 'speed', label: '速度', model: maxSpeed, min: 0, max: 1, step: 0.1, unit: 'm/s', digits: 1 },
-  { key: 'rot', label: '旋转角度', model: rotationAngle, min: -1, max: 1, step: 0.1, unit: 'm/s', digits: 0 },
+  { key: 'rot', label: '旋转角度', model: rotationAngle, min: -1, max: 1, step: 0.1, unit: 'm/s', digits: 1 },
 ])
 
 const statusItems = [
@@ -171,8 +171,11 @@ const moveRobotData = ref({
     vz: 0
 })
 
+const running = ref(false)
+
 /* long-press stubs */
 const onPress = async (_dir) => {
+    running.value = true
     if (_dir === 'forward') {
         moveRobotData.value.vx = maxSpeed.value
     } else if (_dir === 'backward') {
@@ -182,22 +185,35 @@ const onPress = async (_dir) => {
     } else {
         moveRobotData.value.vy = -maxSpeed.value
     }
-    moveRobotData.value.vz = rotationAngle.value
+    moveRobotData.value.wz = rotationAngle.value
+    run(moveRobotData.value)
+}
+
+const run = async (data) => {
+    if (!running.value) {
+        return
+    }
     const res = await moveRobot({
         ip: robotStore.ip,
         deviceId: robotStore.agvDeviceId,
-        ...moveRobotData.value 
+        ...data
     })
+    console.log('res', res)
+    if (res.code === 200) {
+        run(moveRobotData.value)
+    } else {
+        running.value = false
+    }
 }
 
 function onRelease() { 
+    running.value = false
     moveRobotData.value.vx = 0;
     moveRobotData.value.vy = 0;
-    stopRobot({
-        ip: robotStore.ip,
-        // deviceId: VITE_AGV_DEVICE_ID
-        deviceId: robotStore.agvDeviceId,
-    })
+    // stopRobot({
+    //     ip: robotStore.ip,
+    //     deviceId: robotStore.agvDeviceId,
+    // })
 }
 
 const activePointerIds = new Set()