From ed9df8fd31d8467a6119e2267edaefc43193b91e Mon Sep 17 00:00:00 2001 From: linbo <1034003879@qq.com> Date: Tue, 22 Sep 2026 10:27:44 +0800 Subject: [PATCH] config: add right EtherCAT arm configuration --- .../devices/arm/ethercat_dual_arm.pb.txt | 311 ++++++++++++++++++ .../devices/motor/ethercat_motors.pb.txt | 10 +- cmvr-es/config/manager/device_manager.pb.txt | 33 +- 3 files changed, 343 insertions(+), 11 deletions(-) create mode 100644 cmvr-es/config/devices/arm/ethercat_dual_arm.pb.txt diff --git a/cmvr-es/config/devices/arm/ethercat_dual_arm.pb.txt b/cmvr-es/config/devices/arm/ethercat_dual_arm.pb.txt new file mode 100644 index 00000000..ef6171c4 --- /dev/null +++ b/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 + } + } + } + } + } +} diff --git a/cmvr-es/config/devices/motor/ethercat_motors.pb.txt b/cmvr-es/config/devices/motor/ethercat_motors.pb.txt index 0d9432ec..6de03360 100644 --- a/cmvr-es/config/devices/motor/ethercat_motors.pb.txt +++ b/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_R" q_lb: -0.57 q_ub: 1.57 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.61 q_ub: 0.58 qd: 5.0 qdd: 10.0 } } motors { diff --git a/cmvr-es/config/manager/device_manager.pb.txt b/cmvr-es/config/manager/device_manager.pb.txt index 1b8c3f6d..49439ea8 100644 --- a/cmvr-es/config/manager/device_manager.pb.txt +++ b/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 + } + }