config: add right EtherCAT arm configuration

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

@@ -0,0 +1,311 @@
+arm {
+  robot_arms {
+    id: "right_ethercat_arm"
+
+    motor {
+      motor_system_id: "right_arm_ethercat_motors"
+      motor_group_ids: "right_arm_ethercat_motors"
+      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
+      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: "R_WRIST_R_S"
+        tcp_frame_name: "R_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: "R_SHOULDER_P" q_lb: -3.14 q_ub: 3.14 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: "R_SHOULDER_Y" q_lb: -3.14 q_ub: 3.14 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: "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 }
+          }
+          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
+          }
+        }
+      }
+    }
+
+    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
+          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
+            # 等待 Cartesian 速度运动停止的最长时间，单位为秒。
+            stop_timeout_s: 2.0
+          }
+        }
+      }
+    }
+  }
+
+  robot_arms {
+        id: "left_ethercat_arm"
+
+        motor {
+          motor_system_id: "dual_arm_ethercat_motors"
+          motor_group_ids: "dual_arm_ethercat_motors"
+          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
+              }
+            }
+          }
+        }
+
+        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

@@ -29,7 +29,7 @@ motor {
       }
 
       dc {
-        enable: true
+        enable: false
         reference_motor_id: 1
         sync0_cycle_us: 1000
         sync0_shift_us: 0
@@ -51,12 +51,12 @@ motor {
       enable: true
       source: JOINT_LIMIT_SOURCE_CUSTOM
       joints { joint_name: "R_SHOULDER_P" q_lb: -3.14 q_ub: 3.14 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: "R_SHOULDER_R" q_lb: -0.64 q_ub: 1.64 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: "R_ELBOW_R" q_lb: 0 q_ub: 2.05 qd: 5.0 qdd: 10.0 }
+      joints { joint_name: "R_ELBOW_R" q_lb: -1.60 q_ub: 1.60 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: "R_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.37 q_ub: 0.79 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: "R_WRIST_R" q_lb: -0.61 q_ub: 0.58 qd: 5.0 qdd: 10.0 }
     }
 
     motors {

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

@@ -48,14 +48,14 @@ device_manager {
     id: "right_hand_cam"
     type: DEVICE_TYPE_CAMERA
     config_file: "devices/camera/camera.pb.txt"
-    enable: true
+    enable: false
   }
 
     devices {
       id: "left_hand_cam"
       type: DEVICE_TYPE_CAMERA
       config_file: "devices/camera/camera.pb.txt"
-      enable: true
+      enable: false
     }
 
 
@@ -63,14 +63,14 @@ device_manager {
     id: "hand2"
     type: DEVICE_TYPE_DEXHAND
     config_file: "devices/dexhand/dexhand.pb.txt"
-    enable: true
+    enable: false
   }
 
   devices {
     id: "paxini_tip_1"
     type: DEVICE_TYPE_DEXHAND
     config_file: "devices/dexhand/dexhand.pb.txt"
-    enable: true
+    enable: false
   }
 
   devices {
@@ -91,7 +91,7 @@ device_manager {
     id: "right_arm_can_motors"
     type: DEVICE_TYPE_MOTOR_SYSTEM
     config_file: "devices/motor/ti5_motors.pb.txt"
-    enable: true
+    enable: false
   }
 
   devices {
@@ -119,7 +119,7 @@ device_manager {
     id: "right_arm"
     type: DEVICE_TYPE_ROBOT_ARM
     config_file: "devices/arm/arm_qp.pb.txt"
-    enable: true
+    enable: false
   }
 
   devices {
@@ -192,4 +192,25 @@ device_manager {
     enable: false
   }
 
+  devices {
+    id: "dual_arm_ethercat_motors"
+    type: DEVICE_TYPE_MOTOR_SYSTEM
+    config_file: "devices/motor/dual_arm_ethercat_motors.pb.txt"
+    enable: false
+  }
+
+  devices {
+    id: "right_ethercat_arm"
+    type: DEVICE_TYPE_ROBOT_ARM
+    config_file: "devices/arm/ethercat_dual_arm.pb.txt"
+    enable: false
+  }
+
+  devices {
+    id: "left_ethercat_arm"
+    type: DEVICE_TYPE_ROBOT_ARM
+    config_file: "devices/arm/ethercat_dual_arm.pb.txt"
+    enable: false
+  }
+
 }