feat(ethercat): configure dual Eyou robot arms

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

@@ -160,9 +160,15 @@ arm {
 
       motor {
         motor_system_id: "ethercat_motors"
-        motor_group_ids: "right_arm_ethercat"
+        motor_group_ids: "dual_arm_ethercat"
         dof: 7
         joint_names: "R_SHOULDER_P"
+        joint_names: "R_SHOULDER_R"
+        joint_names: "R_SHOULDER_Y"
+        joint_names: "R_ELBOW_R"
+        joint_names: "R_WRIST_P"
+        joint_names: "R_WRIST_Y"
+        joint_names: "R_WRIST_R"
         upd_freq: 1000
         buffer_size: 50
         default_vel: 1.0

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"
+    id: "dual_arm_ethercat"
     bus_type: MOTOR_BUS_ETHERCAT
     vendor: MOTOR_VENDOR_EYOU
     protocol: MOTOR_PROTOCOL_ETHERCAT_CIA402
@@ -45,6 +45,13 @@ motor {
       slaves { motor_id: 5 alias: 0 position: 4 }
       slaves { motor_id: 6 alias: 0 position: 5 }
       slaves { motor_id: 7 alias: 0 position: 6 }
+      slaves { motor_id: 8 alias: 0 position: 7 }
+      slaves { motor_id: 9 alias: 0 position: 8 }
+      slaves { motor_id: 10 alias: 0 position: 9 }
+      slaves { motor_id: 11 alias: 0 position: 10 }
+      slaves { motor_id: 12 alias: 0 position: 11 }
+      slaves { motor_id: 13 alias: 0 position: 12 }
+      slaves { motor_id: 14 alias: 0 position: 13 }
     }
 
     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

@@ -91,7 +91,7 @@ device_manager {
     id: "ethercat_motors"
     type: DEVICE_TYPE_MOTOR_SYSTEM
     config_file: "devices/motor/ethercat_motors.pb.txt"
-    enable: false
+    enable: true
   }
 
   devices {
@@ -105,7 +105,14 @@ device_manager {
     id: "eyou_arm"
     type: DEVICE_TYPE_ROBOT_ARM
     config_file: "devices/arm/arm.pb.txt"
-    enable: false
+    enable: true
+  }
+
+  devices {
+    id: "eyou_left_arm"
+    type: DEVICE_TYPE_ROBOT_ARM
+    config_file: "devices/arm/arm_eyou_left.pb.txt"
+    enable: true
   }
 
   devices {

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" />