feat(ethercat): configure dual Eyou robot arms

cmvr-es/config/devices/arm/arm.pb.txt

@@ -3,8 +3,8 @@ arm {
     id: "right_arm"
 
     motor {
-      motor_system_id: "right_arm_can_motors"
+      motor_system_id: "ti5_motors"
-      motor_group_ids: "right_arm_can_motors"
+      motor_group_ids: "right_arm_can"
       dof: 7
       joint_names: "R_SHOULDER_P"
       joint_names: "R_SHOULDER_R"
@@ -51,6 +51,7 @@ arm {
             gain: 0.2
             margin_ratio: 0.15
             max_push: 0.25
+            weight: 0.05
           }
         }
       }
@@ -58,14 +59,6 @@ arm {
 
     motion {
       move_j {
-        # MoveJ 轨迹完成后等待关节稳定的最长时间，单位为秒。
-        settle_timeout_s: 2.0
-        # MoveJ 完成时允许的最大关节位置误差，单位为弧度。
-        settle_position_tolerance_rad: 0.002
-        # MoveJ 完成时允许的最大关节速度，单位为弧度/秒。
-        settle_velocity_tolerance_rad_s: 0.02
-        # 位置和速度连续满足条件的采样次数。
-        settle_stable_sample_count: 3
         toppra_joint_motion_planner {
           path_type: TOPPRA_PATH_TYPE_QUINTIC
           sample_period_s: 0.001
@@ -151,28 +144,27 @@ arm {
             stop_command_velocity_norm: 1e-3
             stop_measured_velocity_norm: 1e-2
             stop_acceleration: 10
-            # 等待 Cartesian 速度运动停止的最长时间，单位为秒。
-            stop_timeout_s: 2.0
           }
         }
       }
     }
   }
 
+
   robot_arms {
-        id: "left_arm"
+      id: "eyou_arm"
 
       motor {
-          motor_system_id: "left_arm_can_motors"
+        motor_system_id: "ethercat_motors"
-          motor_group_ids: "left_arm_can_motors"
+        motor_group_ids: "dual_arm_ethercat"
         dof: 7
-          joint_names: "L_SHOULDER_P"
+        joint_names: "R_SHOULDER_P"
-          joint_names: "L_SHOULDER_R"
+        joint_names: "R_SHOULDER_R"
-          joint_names: "L_SHOULDER_Y"
+        joint_names: "R_SHOULDER_Y"
-          joint_names: "L_ELBOW_R"
+        joint_names: "R_ELBOW_R"
-          joint_names: "L_WRIST_P"
+        joint_names: "R_WRIST_P"
-          joint_names: "L_WRIST_Y"
+        joint_names: "R_WRIST_Y"
-          joint_names: "L_WRIST_R"
+        joint_names: "R_WRIST_R"
         upd_freq: 1000
         buffer_size: 50
         default_vel: 1.0
@@ -183,8 +175,8 @@ arm {
         pinocchio_dls_ik_solver {
           urdf_path: "model/xiaoyan_description/dual_arm.urdf"
           base_frame_name: "PELVIS_S"
-            flange_frame_name: "L_WRIST_R_S"
+          flange_frame_name: "R_WRIST_R_S"
-            tcp_frame_name: "L_FINGER_TIP_FIXED"
+          tcp_frame_name: "R_FINGER_TIP_FIXED"
           max_iters: 100
           pos_eps: 1e-6
           rot_eps: 1e-6
@@ -193,13 +185,13 @@ arm {
             limits {
               enable: true
               source: JOINT_LIMIT_SOURCE_CUSTOM
-                joints { joint_name: "L_SHOULDER_P" q_lb: -3.14 q_ub: 3.14 qd: 5.0 qdd: 10.0 }
+              joints { joint_name: "R_SHOULDER_P" q_lb: -3.14 q_ub: 3.14 qd: 5.0 qdd: 10.0 }
-                joints { joint_name: "L_SHOULDER_R" q_lb: -0.78 q_ub: 1.57 qd: 5.0 qdd: 10.0 }
+              joints { joint_name: "R_SHOULDER_R" q_lb: -0.78 q_ub: 1.57 qd: 5.0 qdd: 10.0 }
-                joints { joint_name: "L_SHOULDER_Y" q_lb: -3.14 q_ub: 3.14 qd: 5.0 qdd: 10.0 }
+              joints { joint_name: "R_SHOULDER_Y" q_lb: -3.14 q_ub: 3.14 qd: 5.0 qdd: 10.0 }
-                joints { joint_name: "L_ELBOW_R" q_lb: 0 q_ub: 2.05 qd: 5.0 qdd: 10.0 }
+              joints { joint_name: "R_ELBOW_R" q_lb: 0 q_ub: 2.05 qd: 5.0 qdd: 10.0 }
-                joints { joint_name: "L_WRIST_P" q_lb: -3.14 q_ub: 3.14 qd: 5.0 qdd: 10.0 }
+              joints { joint_name: "R_WRIST_P" q_lb: -3.14 q_ub: 3.14 qd: 5.0 qdd: 10.0 }
-                joints { joint_name: "L_WRIST_Y" q_lb: -0.78 q_ub: 0.78 qd: 5.0 qdd: 10.0 }
+              joints { joint_name: "R_WRIST_Y" q_lb: -0.78 q_ub: 0.78 qd: 5.0 qdd: 10.0 }
-                joints { joint_name: "L_WRIST_R" q_lb: -0.57 q_ub: 1.57 qd: 5.0 qdd: 10.0 }
+              joints { joint_name: "R_WRIST_R" q_lb: -0.57 q_ub: 1.57 qd: 5.0 qdd: 10.0 }
             }
             soft_limit {
               enable: true

cmvr-es/config/devices/arm/arm_eyou_left.pb.txt

@@ -0,0 +1,152 @@
+arm {
+  robot_arms {
+    id: "eyou_left_arm"
+
+    motor {
+      motor_system_id: "ethercat_motors"
+      motor_group_ids: "dual_arm_ethercat"
+      dof: 7
+      joint_names: "L_SHOULDER_P"
+      joint_names: "L_SHOULDER_R"
+      joint_names: "L_SHOULDER_Y"
+      joint_names: "L_ELBOW_R"
+      joint_names: "L_WRIST_P"
+      joint_names: "L_WRIST_Y"
+      joint_names: "L_WRIST_R"
+      upd_freq: 1000
+      buffer_size: 50
+      default_vel: 1.0
+      default_acc: 2.0
+    }
+
+    kinematics {
+      pinocchio_dls_ik_solver {
+        urdf_path: "model/xiaoyan_description/dual_arm.urdf"
+        base_frame_name: "PELVIS_S"
+        flange_frame_name: "L_WRIST_R_S"
+        tcp_frame_name: "L_FINGER_TIP_FIXED"
+        max_iters: 100
+        pos_eps: 1e-6
+        rot_eps: 1e-6
+        damping: 1e-6
+        joint_limit_policy {
+          limits {
+            enable: true
+            source: JOINT_LIMIT_SOURCE_CUSTOM
+            joints { joint_name: "L_SHOULDER_P" q_lb: -3.14 q_ub: 3.14 qd: 5.0 qdd: 10.0 }
+            joints { joint_name: "L_SHOULDER_R" q_lb: -0.78 q_ub: 1.57 qd: 5.0 qdd: 10.0 }
+            joints { joint_name: "L_SHOULDER_Y" q_lb: -3.14 q_ub: 3.14 qd: 5.0 qdd: 10.0 }
+            joints { joint_name: "L_ELBOW_R" q_lb: 0 q_ub: 2.05 qd: 5.0 qdd: 10.0 }
+            joints { joint_name: "L_WRIST_P" q_lb: -3.14 q_ub: 3.14 qd: 5.0 qdd: 10.0 }
+            joints { joint_name: "L_WRIST_Y" q_lb: -0.78 q_ub: 0.78 qd: 5.0 qdd: 10.0 }
+            joints { joint_name: "L_WRIST_R" q_lb: -0.57 q_ub: 1.57 qd: 5.0 qdd: 10.0 }
+          }
+          soft_limit {
+            enable: true
+            margin_ratio: 0.01
+            min_margin_rad: 0.01
+          }
+          avoidance {
+            enable: false
+            gain: 0.2
+            margin_ratio: 0.15
+            max_push: 0.25
+            weight: 0.05
+          }
+        }
+      }
+    }
+
+    motion {
+      move_j {
+        toppra_joint_motion_planner {
+          path_type: TOPPRA_PATH_TYPE_QUINTIC
+          sample_period_s: 0.001
+          grid_size: 150
+          high_grid_size: 300
+        }
+      }
+
+      move_l {
+        pinocchio_cartesian_motion_planner {
+          sample_period_s: 0.001
+          position_gain: 4.0
+          rotation_gain: 4.0
+          line_deviation_check {
+            enable: true
+            line_deviation_warn_m: 0.01
+            line_deviation_stop_m: 0.03
+            line_direction_warn_deg: 20.0
+            line_direction_stop_deg: 45.0
+            line_direction_reset_deg: 10.0
+            line_check_min_distance_m: 0.01
+          }
+          joint_continuity_check {
+            enable: true
+            max_joint_delta_rad: 0.05
+            max_joint_velocity_rad_s: 10.0
+            max_joint_acceleration_rad_s2: 5000.0
+          }
+          cartesian_step_feasibility_check {
+            enable: true
+            min_linear_speed_ratio: 0.2
+            max_linear_direction_deviation_deg: 45.0
+            min_angular_speed_ratio: 0.2
+            max_angular_direction_deviation_deg: 45.0
+            min_desired_linear_speed: 1e-4
+            min_desired_angular_speed: 1e-4
+          }
+        }
+      }
+
+      speed_l {
+        pinocchio_cartesian_motion_planner {
+          linear_velocity_max: 0.55
+          linear_acceleration_max: 5.0
+          linear_jerk_max: 10.0
+          angular_velocity_max: 1.0
+          angular_acceleration_max: 5.0
+          angular_jerk_max: 12.0
+          linear_target_replan_threshold: 1e-4
+          angular_target_replan_threshold: 1e-4
+          linear_reverse_cos_threshold: -0.8660254037844386
+          linear_reverse_switch_speed_threshold: 1e-3
+          enforce_joint_acceleration_limits: true
+          line_deviation_check {
+            enable: true
+            line_deviation_warn_m: 0.01
+            line_deviation_stop_m: 0.03
+            line_direction_warn_deg: 20.0
+            line_direction_stop_deg: 45.0
+            line_direction_reset_deg: 10.0
+            line_check_min_distance_m: 0.01
+          }
+          joint_velocity_check {
+            enable: true
+            max_joint_velocity_rad_s: 30.0
+            max_joint_acceleration_rad_s2: 10000.0
+          }
+          cartesian_velocity_feasibility_check {
+            enable: true
+            min_linear_speed_ratio: 0.2
+            max_linear_direction_deviation_deg: 5.0
+            min_angular_speed_ratio: 0.2
+            max_angular_direction_deviation_deg: 5.0
+            min_desired_linear_speed: 1e-4
+            min_desired_angular_speed: 1e-4
+          }
+        }
+
+        speed_l_controller {
+          cartesian_velocity_controller {
+            control_period_s: 0.001
+            stop_twist_norm: 1e-9
+            stop_command_velocity_norm: 1e-3
+            stop_measured_velocity_norm: 1e-2
+            stop_acceleration: 10
+          }
+        }
+      }
+    }
+  }
+}

cmvr-es/config/devices/motor/ethercat_motors.pb.txt

@@ -2,7 +2,7 @@ motor {
   id: "ethercat_motors"
 
   motor_groups {
-    id: "right_arm_ethercat_motors"
+    id: "dual_arm_ethercat"
     bus_type: MOTOR_BUS_ETHERCAT
     vendor: MOTOR_VENDOR_EYOU
     protocol: MOTOR_PROTOCOL_ETHERCAT_CIA402
@@ -38,13 +38,20 @@ motor {
         sync_monitor_period_ms: 1000
       }
 
-      slaves { motor_id: 1 alias: 0 position: 0 }
+      slaves { motor_id: 1 alias: 0 position: 1 }
-      slaves { motor_id: 2 alias: 0 position: 1 }
+      slaves { motor_id: 2 alias: 0 position: 2 }
-      slaves { motor_id: 3 alias: 0 position: 2 }
+      slaves { motor_id: 3 alias: 0 position: 3 }
-      slaves { motor_id: 4 alias: 0 position: 3 }
+      slaves { motor_id: 4 alias: 0 position: 4 }
-      slaves { motor_id: 5 alias: 0 position: 4 }
+      slaves { motor_id: 5 alias: 0 position: 5 }
-      slaves { motor_id: 6 alias: 0 position: 5 }
+      slaves { motor_id: 6 alias: 0 position: 6 }
-      slaves { motor_id: 7 alias: 0 position: 6 }
+      slaves { motor_id: 7 alias: 0 position: 7 }
+      slaves { motor_id: 8 alias: 0 position: 8 }
+      slaves { motor_id: 9 alias: 0 position: 9 }
+      slaves { motor_id: 10 alias: 0 position: 10 }
+      slaves { motor_id: 11 alias: 0 position: 11 }
+      slaves { motor_id: 12 alias: 0 position: 12 }
+      slaves { motor_id: 13 alias: 0 position: 13 }
+      slaves { motor_id: 14 alias: 0 position: 14 }
     }
 
     joint_limits {
@@ -57,6 +64,13 @@ motor {
       joints { joint_name: "R_WRIST_P" q_lb: -3.14 q_ub: 3.14 qd: 5.0 qdd: 10.0 }
       joints { joint_name: "R_WRIST_Y" q_lb: -0.78 q_ub: 0.78 qd: 5.0 qdd: 10.0 }
       joints { joint_name: "R_WRIST_R" q_lb: -0.57 q_ub: 1.57 qd: 5.0 qdd: 10.0 }
+      joints { joint_name: "L_SHOULDER_P" q_lb: -3.14 q_ub: 3.14 qd: 5.0 qdd: 10.0 }
+      joints { joint_name: "L_SHOULDER_R" q_lb: -0.78 q_ub: 1.57 qd: 5.0 qdd: 10.0 }
+      joints { joint_name: "L_SHOULDER_Y" q_lb: -3.14 q_ub: 3.14 qd: 5.0 qdd: 10.0 }
+      joints { joint_name: "L_ELBOW_R" q_lb: 0 q_ub: 2.05 qd: 5.0 qdd: 10.0 }
+      joints { joint_name: "L_WRIST_P" q_lb: -3.14 q_ub: 3.14 qd: 5.0 qdd: 10.0 }
+      joints { joint_name: "L_WRIST_Y" q_lb: -0.78 q_ub: 0.78 qd: 5.0 qdd: 10.0 }
+      joints { joint_name: "L_WRIST_R" q_lb: -0.57 q_ub: 1.57 qd: 5.0 qdd: 10.0 }
     }
 
     motors {
@@ -67,6 +81,13 @@ motor {
       motors { id: 5 joint_name: "R_WRIST_P" encoder_counts_per_rev: 65536 gear_ratio: 101.0 }
       motors { id: 6 joint_name: "R_WRIST_Y" encoder_counts_per_rev: 65536 gear_ratio: 101.0 }
       motors { id: 7 joint_name: "R_WRIST_R" encoder_counts_per_rev: 65536 gear_ratio: 101.0 }
+      motors { id: 8 joint_name: "L_SHOULDER_P" encoder_counts_per_rev: 65536 gear_ratio: 101.0 }
+      motors { id: 9 joint_name: "L_SHOULDER_R" encoder_counts_per_rev: 65536 gear_ratio: 101.0 }
+      motors { id: 10 joint_name: "L_SHOULDER_Y" encoder_counts_per_rev: 65536 gear_ratio: 101.0 }
+      motors { id: 11 joint_name: "L_ELBOW_R" encoder_counts_per_rev: 65536 gear_ratio: 101.0 }
+      motors { id: 12 joint_name: "L_WRIST_P" encoder_counts_per_rev: 65536 gear_ratio: 101.0 }
+      motors { id: 13 joint_name: "L_WRIST_Y" encoder_counts_per_rev: 65536 gear_ratio: 101.0 }
+      motors { id: 14 joint_name: "L_WRIST_R" encoder_counts_per_rev: 65536 gear_ratio: 101.0 }
     }
   }
 }

cmvr-es/config/manager/device_manager.pb.txt

@@ -2,46 +2,41 @@ device_manager {
   name: "cmvr_es"
   version: "0.1"
   description: "cmvr edge system version 0.1"
+  init_all_motors_when_no_active_joints: true
+
   devices {
     id: "mujoco_world"
     type: DEVICE_TYPE_MUJOCO_WORLD
-    config_file: "devices/mujoco/right_arm_eye_to_hand_world.pb.txt"
+    config_file: "devices/mujoco/mujoco_world.pb.txt"
-    enable: true
+    enable: false
   }
 
   devices {
-    id: "right_arm_mujoco_motors"
+    id: "mujoco_motors"
     type: DEVICE_TYPE_MOTOR_SYSTEM
     config_file: "devices/motor/mujoco_motors.pb.txt"
-    enable: true
+    enable: false
   }
 
   devices {
     id: "mujoco_right_arm"
     type: DEVICE_TYPE_ROBOT_ARM
     config_file: "devices/arm/arm_mujoco_qp.pb.txt"
-    enable: true
+    enable: false
   }
 
   devices {
     id: "mujoco_viewer"
     type: DEVICE_TYPE_MUJOCO_VIEWER
     config_file: "devices/mujoco/mujoco_viewer.pb.txt"
-    enable: true
+    enable: false
   }
 
   devices {
     id: "mujoco_hand_cam"
     type: DEVICE_TYPE_CAMERA
     config_file: "devices/camera/camera.pb.txt"
-    enable: true
+    enable: false
-  }
-
-  devices {
-    id: "mujoco_external_touch_cam"
-    type: DEVICE_TYPE_CAMERA
-    config_file: "devices/camera/camera.pb.txt"
-    enable: true
   }
 
   devices {
@@ -74,42 +69,14 @@ device_manager {
   }
 
   devices {
-    id: "mujoco_zero_touch_dexhand"
+    id: "ti5_motors"
-    type: DEVICE_TYPE_DEXHAND
-    config_file: "devices/dexhand/dexhand.pb.txt"
-    enable: true
-  }
-
-  devices {
-    id: "left_arm_can_motors"
     type: DEVICE_TYPE_MOTOR_SYSTEM
     config_file: "devices/motor/ti5_motors.pb.txt"
     enable: false
   }
 
   devices {
-    id: "right_arm_can_motors"
+    id: "ethercat_motors"
-    type: DEVICE_TYPE_MOTOR_SYSTEM
-    config_file: "devices/motor/ti5_motors.pb.txt"
-    enable: false
-  }
-
-  devices {
-    id: "head_can_motors"
-    type: DEVICE_TYPE_MOTOR_SYSTEM
-    config_file: "devices/motor/ti5_motors.pb.txt"
-    enable: false
-  }
-
-  devices {
-    id: "waist_can_motors"
-    type: DEVICE_TYPE_MOTOR_SYSTEM
-    config_file: "devices/motor/ti5_motors.pb.txt"
-    enable: false
-  }
-
-  devices {
-    id: "right_arm_ethercat_motors"
     type: DEVICE_TYPE_MOTOR_SYSTEM
     config_file: "devices/motor/ethercat_motors.pb.txt"
     enable: false
@@ -118,7 +85,21 @@ device_manager {
   devices {
     id: "right_arm"
     type: DEVICE_TYPE_ROBOT_ARM
-    config_file: "devices/arm/arm_qp.pb.txt"
+    config_file: "devices/arm/arm.pb.txt"
+    enable: false
+  }
+
+  devices {
+    id: "eyou_arm"
+    type: DEVICE_TYPE_ROBOT_ARM
+    config_file: "devices/arm/arm.pb.txt"
+    enable: false
+  }
+
+  devices {
+    id: "eyou_left_arm"
+    type: DEVICE_TYPE_ROBOT_ARM
+    config_file: "devices/arm/arm_eyou_left.pb.txt"
     enable: false
   }
 
@@ -144,9 +125,56 @@ device_manager {
   }
   
   devices {
-    id: "agv_1"
+    id: "src1100"
     type: DEVICE_TYPE_AGV
-    config_file: "devices/agv/agv.pb.txt"
+    config_file: "devices/agv/src1100.pb.txt"
+    enable: false
+  }
+
+  devices {
+      id: "hikvision_cam"
+      type: DEVICE_TYPE_CAMERA
+      config_file: "devices/camera/camera.pb.txt"
+      # Host-development default: keep physical cameras disabled.
+      enable: false
+    }
+  devices {
+      id: "hikvision_thermal_cam"
+      type: DEVICE_TYPE_CAMERA
+      config_file: "devices/camera/camera.pb.txt"
+      # Host-development default: keep physical cameras disabled.
+      enable: false
+    }
+
+    devices {
+      id: "mic1"
+      type: DEVICE_TYPE_MICROPHONE
+      config_file: "devices/microphone/microphone.pb.txt"
+      # Host-development default: keep physical audio devices disabled.
+      enable: false
+    }
+
+    devices {
+      id: "spk1"
+      type: DEVICE_TYPE_SPEAKER
+      config_file: "devices/speaker/speaker.pb.txt"
+      # Host-development default: keep physical audio devices disabled.
+      enable: false
+    }
+
+
+    devices {
+          id: "real_cam1"
+          type: DEVICE_TYPE_CAMERA
+          config_file: "devices/camera/camera.pb.txt"
+          # Host-development default: keep physical cameras disabled.
+          enable: false
+        }
+    devices {
+          id: "usb_cam1"
+          type: DEVICE_TYPE_CAMERA
+          config_file: "devices/camera/camera.pb.txt"
+          # Host-development default: keep physical cameras disabled.
           enable: false
         }
 }

model/xiaoyan_description/dual_arm.urdf

@@ -274,6 +274,36 @@
     <limit lower="-1.57" upper="0.26" effort="50" velocity="4.71" />
   </joint>
 
+  <link name="L_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="L_FINGER_TIP_FIXED" type="fixed">
+    <origin xyz="0.00684256 0.284077 0.00801525" rpy="0 0 0"/>
+    <parent link="L_WRIST_R_S"/>
+    <child link="L_FINGER_TIP"/>
+    <axis xyz="0 0 1"/>
+  </joint>
+
   <link name="R_SHOULDER_P_S">
     <inertial>
       <origin xyz="-0.00982258725282141 -0.0704593093873431 -1.15069920925137E-06" rpy="0 0 0" />