lgv
ffccdea26d
Preserve acceleration through same-axis velocity reversal and submit retract commands immediately after zero-dwell tactile contact. Capture the applied command's TCP reference and measure signed retract progress, with logging of the maximum sampled forward displacement after reversal starts. Support per-command linear jerk in both touch.speed_l and retract, and cap requested acceleration and jerk at the configured arm limits. Protect command snapshots and stop completion against newer speedL submissions. Include the current QP arm and retract parameter tuning and remove an obsolete config field. Add headless actuator-driven MuJoCo reversal measurement and focused regression coverage for reversal continuity, controller concurrency, limit enforcement, and touch jerk configuration. Validation: cmvr_es and touch_screen_task_test build successfully; 18 focused regression tests pass, along with headless MuJoCo reversal checks.
312 lines
10 KiB
Plaintext
312 lines
10 KiB
Plaintext
arm {
|
|
robot_arms {
|
|
id: "right_arm"
|
|
|
|
motor {
|
|
motor_system_id: "right_arm_can_motors"
|
|
motor_group_ids: "right_arm_can_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_arm"
|
|
|
|
motor {
|
|
motor_system_id: "left_arm_can_motors"
|
|
motor_group_ids: "left_arm_can_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
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|