Skip to content

Commit 32bb48b

Browse files
ZGZ713912ZGZ713912
committed
feat: remove suspension mode and torque parameters from joint controllers and add auto aim for deformable infantry
1 parent 78c6d39 commit 32bb48b

7 files changed

Lines changed: 19 additions & 101 deletions

File tree

‎rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml‎

Lines changed: 0 additions & 15 deletions
Original file line numberDiff line numberDiff line change
@@ -181,8 +181,6 @@ lf_joint_controller:
181181
measurement_velocity: /chassis/left_front_joint/physical_velocity
182182
setpoint_angle: /chassis/left_front_joint/target_physical_angle
183183
setpoint_velocity: /chassis/left_front_joint/target_physical_velocity
184-
mode_input: /chassis/left_front_joint/suspension_mode
185-
suspension_torque: /chassis/left_front_joint/suspension_torque
186184
control: /chassis/left_front_joint/control_torque
187185

188186
# Normal ADRC servo mode
@@ -219,17 +217,13 @@ lf_joint_controller:
219217

220218
# Joint-local feedforward / limit shaping
221219
torque_feedforward_gain: 0.0
222-
suspension_torque_feedforward_gain: -1.0
223-
224220
lb_joint_controller:
225221
ros__parameters:
226222
# Same joint-servo layout as lf_joint_controller
227223
measurement_angle: /chassis/left_back_joint/physical_angle
228224
measurement_velocity: /chassis/left_back_joint/physical_velocity
229225
setpoint_angle: /chassis/left_back_joint/target_physical_angle
230226
setpoint_velocity: /chassis/left_back_joint/target_physical_velocity
231-
mode_input: /chassis/left_back_joint/suspension_mode
232-
suspension_torque: /chassis/left_back_joint/suspension_torque
233227
control: /chassis/left_back_joint/control_torque
234228
dt: 0.001
235229
b0: -1.0
@@ -260,17 +254,13 @@ lb_joint_controller:
260254
suspension_output_min: -35.0
261255
suspension_output_max: 35.0
262256
torque_feedforward_gain: 0.0
263-
suspension_torque_feedforward_gain: -1.0
264-
265257
rb_joint_controller:
266258
ros__parameters:
267259
# Same joint-servo layout as lf_joint_controller
268260
measurement_angle: /chassis/right_back_joint/physical_angle
269261
measurement_velocity: /chassis/right_back_joint/physical_velocity
270262
setpoint_angle: /chassis/right_back_joint/target_physical_angle
271263
setpoint_velocity: /chassis/right_back_joint/target_physical_velocity
272-
mode_input: /chassis/right_back_joint/suspension_mode
273-
suspension_torque: /chassis/right_back_joint/suspension_torque
274264
control: /chassis/right_back_joint/control_torque
275265
dt: 0.001
276266
b0: -1.0
@@ -301,17 +291,13 @@ rb_joint_controller:
301291
suspension_output_min: -35.0
302292
suspension_output_max: 35.0
303293
torque_feedforward_gain: 0.0
304-
suspension_torque_feedforward_gain: -1.0
305-
306294
rf_joint_controller:
307295
ros__parameters:
308296
# Same joint-servo layout as lf_joint_controller
309297
measurement_angle: /chassis/right_front_joint/physical_angle
310298
measurement_velocity: /chassis/right_front_joint/physical_velocity
311299
setpoint_angle: /chassis/right_front_joint/target_physical_angle
312300
setpoint_velocity: /chassis/right_front_joint/target_physical_velocity
313-
mode_input: /chassis/right_front_joint/suspension_mode
314-
suspension_torque: /chassis/right_front_joint/suspension_torque
315301
control: /chassis/right_front_joint/control_torque
316302
dt: 0.001
317303
b0: -1.0
@@ -342,4 +328,3 @@ rf_joint_controller:
342328
suspension_output_min: -35.0
343329
suspension_output_max: 35.0
344330
torque_feedforward_gain: 0.0
345-
suspension_torque_feedforward_gain: -1.0

‎rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml‎

Lines changed: 1 addition & 16 deletions
Original file line numberDiff line numberDiff line change
@@ -30,7 +30,7 @@ rmcs_executor:
3030
- rmcs_core::controller::chassis::DeformableJointController -> rf_joint_controller
3131

3232
- rmcs_core::broadcaster::ValueBroadcaster -> value_broadcaster
33-
# - rmcs::AutoAimComponent -> auto_aim_component
33+
- rmcs::AutoAimComponent -> auto_aim_component
3434

3535
value_broadcaster:
3636
ros__parameters:
@@ -200,8 +200,6 @@ lf_joint_controller:
200200
measurement_velocity: /chassis/left_front_joint/physical_velocity
201201
setpoint_angle: /chassis/left_front_joint/target_physical_angle
202202
setpoint_velocity: /chassis/left_front_joint/target_physical_velocity
203-
mode_input: /chassis/left_front_joint/suspension_mode
204-
suspension_torque: /chassis/left_front_joint/suspension_torque
205203
control: /chassis/left_front_joint/control_torque
206204

207205
# Normal ADRC servo mode
@@ -238,17 +236,13 @@ lf_joint_controller:
238236

239237
# Joint-local feedforward / limit shaping
240238
torque_feedforward_gain: 0.0
241-
suspension_torque_feedforward_gain: -1.0
242-
243239
lb_joint_controller:
244240
ros__parameters:
245241
# Same joint-servo layout as lf_joint_controller
246242
measurement_angle: /chassis/left_back_joint/physical_angle
247243
measurement_velocity: /chassis/left_back_joint/physical_velocity
248244
setpoint_angle: /chassis/left_back_joint/target_physical_angle
249245
setpoint_velocity: /chassis/left_back_joint/target_physical_velocity
250-
mode_input: /chassis/left_back_joint/suspension_mode
251-
suspension_torque: /chassis/left_back_joint/suspension_torque
252246
control: /chassis/left_back_joint/control_torque
253247
dt: 0.001
254248
b0: -0.60
@@ -279,17 +273,13 @@ lb_joint_controller:
279273
suspension_output_min: -35.0
280274
suspension_output_max: 35.0
281275
torque_feedforward_gain: 0.0
282-
suspension_torque_feedforward_gain: -1.0
283-
284276
rb_joint_controller:
285277
ros__parameters:
286278
# Same joint-servo layout as lf_joint_controller
287279
measurement_angle: /chassis/right_back_joint/physical_angle
288280
measurement_velocity: /chassis/right_back_joint/physical_velocity
289281
setpoint_angle: /chassis/right_back_joint/target_physical_angle
290282
setpoint_velocity: /chassis/right_back_joint/target_physical_velocity
291-
mode_input: /chassis/right_back_joint/suspension_mode
292-
suspension_torque: /chassis/right_back_joint/suspension_torque
293283
control: /chassis/right_back_joint/control_torque
294284
dt: 0.001
295285
b0: -0.60
@@ -320,17 +310,13 @@ rb_joint_controller:
320310
suspension_output_min: -35.0
321311
suspension_output_max: 35.0
322312
torque_feedforward_gain: 0.0
323-
suspension_torque_feedforward_gain: -1.0
324-
325313
rf_joint_controller:
326314
ros__parameters:
327315
# Same joint-servo layout as lf_joint_controller
328316
measurement_angle: /chassis/right_front_joint/physical_angle
329317
measurement_velocity: /chassis/right_front_joint/physical_velocity
330318
setpoint_angle: /chassis/right_front_joint/target_physical_angle
331319
setpoint_velocity: /chassis/right_front_joint/target_physical_velocity
332-
mode_input: /chassis/right_front_joint/suspension_mode
333-
suspension_torque: /chassis/right_front_joint/suspension_torque
334320
control: /chassis/right_front_joint/control_torque
335321
dt: 0.001
336322
b0: -0.60
@@ -361,4 +347,3 @@ rf_joint_controller:
361347
suspension_output_min: -35.0
362348
suspension_output_max: 35.0
363349
torque_feedforward_gain: 0.0
364-
suspension_torque_feedforward_gain: -1.0

‎rmcs_ws/src/rmcs_bringup/config/deformable-infantry-steering.yaml‎

Lines changed: 0 additions & 15 deletions
Original file line numberDiff line numberDiff line change
@@ -181,8 +181,6 @@ lf_joint_controller:
181181
measurement_velocity: /chassis/left_front_joint/physical_velocity
182182
setpoint_angle: /chassis/left_front_joint/target_physical_angle
183183
setpoint_velocity: /chassis/left_front_joint/target_physical_velocity
184-
mode_input: /chassis/left_front_joint/suspension_mode
185-
suspension_torque: /chassis/left_front_joint/suspension_torque
186184
control: /chassis/left_front_joint/control_torque
187185

188186
# Normal ADRC servo mode
@@ -219,17 +217,13 @@ lf_joint_controller:
219217

220218
# Joint-local feedforward / limit shaping
221219
torque_feedforward_gain: 0.0
222-
suspension_torque_feedforward_gain: -1.0
223-
224220
lb_joint_controller:
225221
ros__parameters:
226222
# Same joint-servo layout as lf_joint_controller
227223
measurement_angle: /chassis/left_back_joint/physical_angle
228224
measurement_velocity: /chassis/left_back_joint/physical_velocity
229225
setpoint_angle: /chassis/left_back_joint/target_physical_angle
230226
setpoint_velocity: /chassis/left_back_joint/target_physical_velocity
231-
mode_input: /chassis/left_back_joint/suspension_mode
232-
suspension_torque: /chassis/left_back_joint/suspension_torque
233227
control: /chassis/left_back_joint/control_torque
234228
dt: 0.001
235229
b0: -1.0
@@ -260,17 +254,13 @@ lb_joint_controller:
260254
suspension_output_min: -35.0
261255
suspension_output_max: 35.0
262256
torque_feedforward_gain: 0.0
263-
suspension_torque_feedforward_gain: -1.0
264-
265257
rb_joint_controller:
266258
ros__parameters:
267259
# Same joint-servo layout as lf_joint_controller
268260
measurement_angle: /chassis/right_back_joint/physical_angle
269261
measurement_velocity: /chassis/right_back_joint/physical_velocity
270262
setpoint_angle: /chassis/right_back_joint/target_physical_angle
271263
setpoint_velocity: /chassis/right_back_joint/target_physical_velocity
272-
mode_input: /chassis/right_back_joint/suspension_mode
273-
suspension_torque: /chassis/right_back_joint/suspension_torque
274264
control: /chassis/right_back_joint/control_torque
275265
dt: 0.001
276266
b0: -1.0
@@ -301,17 +291,13 @@ rb_joint_controller:
301291
suspension_output_min: -35.0
302292
suspension_output_max: 35.0
303293
torque_feedforward_gain: 0.0
304-
suspension_torque_feedforward_gain: -1.0
305-
306294
rf_joint_controller:
307295
ros__parameters:
308296
# Same joint-servo layout as lf_joint_controller
309297
measurement_angle: /chassis/right_front_joint/physical_angle
310298
measurement_velocity: /chassis/right_front_joint/physical_velocity
311299
setpoint_angle: /chassis/right_front_joint/target_physical_angle
312300
setpoint_velocity: /chassis/right_front_joint/target_physical_velocity
313-
mode_input: /chassis/right_front_joint/suspension_mode
314-
suspension_torque: /chassis/right_front_joint/suspension_torque
315301
control: /chassis/right_front_joint/control_torque
316302
dt: 0.001
317303
b0: -1.0
@@ -342,4 +328,3 @@ rf_joint_controller:
342328
suspension_output_min: -35.0
343329
suspension_output_max: 35.0
344330
torque_feedforward_gain: 0.0
345-
suspension_torque_feedforward_gain: -1.0

‎rmcs_ws/src/rmcs_bringup/launch/rmcs.launch.py‎

Lines changed: 12 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -6,7 +6,8 @@
66
LaunchDescription,
77
LaunchDescriptionEntity,
88
)
9-
from launch.actions import LogInfo
9+
from launch.actions import IncludeLaunchDescription,LogInfo
10+
from launch.launch_description_sources import PythonLaunchDescriptionSource
1011
from launch.substitutions import LaunchConfiguration
1112

1213
from launch_ros.actions import Node
@@ -52,6 +53,16 @@ def visit(
5253

5354
if is_automatic:
5455
pass
56+
57+
entities.append(
58+
IncludeLaunchDescription(
59+
PythonLaunchDescriptionSource([
60+
FindPackageShare('rmcs_auto_aim_v2'), '/launch.py'
61+
])
62+
)
63+
)
64+
65+
5566

5667
return entities
5768

‎rmcs_ws/src/rmcs_bringup/package.xml‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -11,6 +11,7 @@
1111
<buildtool_depend>ament_cmake</buildtool_depend>
1212

1313
<exec_depend>joint_state_broadcaster</exec_depend>
14+
<exec_depend>rmcs_auto_aim_v2</exec_depend>
1415
<exec_depend>robot_state_publisher</exec_depend>
1516
<exec_depend>rviz2</exec_depend>
1617
<exec_depend>xacro</exec_depend>

‎rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_chassis.cpp‎

Lines changed: 0 additions & 50 deletions
Original file line numberDiff line numberDiff line change
@@ -255,27 +255,6 @@ class DeformableChassis
255255
right_front_joint_target_physical_acceleration_, nan_);
256256
register_output("/chassis/processed_encoder/angle", processed_encoder_angle_, nan_);
257257

258-
register_output(
259-
"/chassis/left_front_joint/suspension_mode", left_front_joint_suspension_mode_, false);
260-
register_output(
261-
"/chassis/left_back_joint/suspension_mode", left_back_joint_suspension_mode_, false);
262-
register_output(
263-
"/chassis/right_front_joint/suspension_mode", right_front_joint_suspension_mode_, false);
264-
register_output(
265-
"/chassis/right_back_joint/suspension_mode", right_back_joint_suspension_mode_, false);
266-
267-
register_output(
268-
"/chassis/left_front_joint/suspension_torque", left_front_joint_suspension_torque_,
269-
nan_);
270-
register_output(
271-
"/chassis/left_back_joint/suspension_torque", left_back_joint_suspension_torque_, nan_);
272-
register_output(
273-
"/chassis/right_back_joint/suspension_torque", right_back_joint_suspension_torque_,
274-
nan_);
275-
register_output(
276-
"/chassis/right_front_joint/suspension_torque", right_front_joint_suspension_torque_,
277-
nan_);
278-
279258
*mode_ = rmcs_msgs::ChassisMode::AUTO;
280259
chassis_control_velocity_->vector << nan_, nan_, nan_;
281260

@@ -403,18 +382,6 @@ class DeformableChassis
403382
return wrap_deg(joint_offset) - wrap_deg(*joint_encoder_angle) + legacy_fixed_compensation;
404383
}
405384

406-
void clear_suspension_output_interfaces_() {
407-
*left_front_joint_suspension_mode_ = false;
408-
*left_back_joint_suspension_mode_ = false;
409-
*right_back_joint_suspension_mode_ = false;
410-
*right_front_joint_suspension_mode_ = false;
411-
412-
*left_front_joint_suspension_torque_ = 0.0;
413-
*left_back_joint_suspension_torque_ = 0.0;
414-
*right_back_joint_suspension_torque_ = 0.0;
415-
*right_front_joint_suspension_torque_ = 0.0;
416-
}
417-
418385
void update_mode_from_inputs_(
419386
rmcs_msgs::Switch switch_left, rmcs_msgs::Switch switch_right,
420387
const rmcs_msgs::Keyboard& keyboard) {
@@ -621,7 +588,6 @@ class DeformableChassis
621588
}
622589

623590
void update_active_suspension_(const JointFeedbackFrame&) {
624-
clear_suspension_output_interfaces_();
625591
if (!suspension_requested_by_input_()) {
626592
reset_attitude_correction_state_();
627593
return;
@@ -716,7 +682,6 @@ class DeformableChassis
716682

717683
*processed_encoder_angle_ = nan_;
718684

719-
clear_suspension_output_interfaces_();
720685
}
721686

722687
void update_velocity_control() {
@@ -860,11 +825,6 @@ class DeformableChassis
860825
scope_motor_control(prone_override);
861826
update_active_suspension_(joint_feedback);
862827

863-
*left_front_joint_suspension_mode_ = joint_suspension_active_[kLeftFront];
864-
*left_back_joint_suspension_mode_ = joint_suspension_active_[kLeftBack];
865-
*right_front_joint_suspension_mode_ = joint_suspension_active_[kRightFront];
866-
*right_back_joint_suspension_mode_ = joint_suspension_active_[kRightBack];
867-
868828
update_joint_target_trajectory();
869829
publish_joint_target_angles(joint_feedback.physical_angles);
870830
}
@@ -1065,7 +1025,6 @@ class DeformableChassis
10651025
*rb_angle_error_ = nan_;
10661026
*rf_angle_error_ = nan_;
10671027

1068-
clear_suspension_output_interfaces_();
10691028
}
10701029

10711030
private:
@@ -1149,15 +1108,6 @@ class DeformableChassis
11491108

11501109
OutputInterface<double> processed_encoder_angle_;
11511110

1152-
OutputInterface<bool> left_front_joint_suspension_mode_;
1153-
OutputInterface<bool> left_back_joint_suspension_mode_;
1154-
OutputInterface<bool> right_front_joint_suspension_mode_;
1155-
OutputInterface<bool> right_back_joint_suspension_mode_;
1156-
OutputInterface<double> left_front_joint_suspension_torque_;
1157-
OutputInterface<double> left_back_joint_suspension_torque_;
1158-
OutputInterface<double> right_front_joint_suspension_torque_;
1159-
OutputInterface<double> right_back_joint_suspension_torque_;
1160-
11611111
double min_angle_;
11621112
double max_angle_;
11631113
double left_front_joint_offset_;

‎rmcs_ws/src/rmcs_core/src/controller/shooting/bullet_feeder_controller_17mm.cpp‎

Lines changed: 5 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -53,7 +53,7 @@ class BulletFeederController17mm
5353
register_input("/remote/mouse", mouse_);
5454
register_input("/remote/keyboard", keyboard_);
5555

56-
register_input("/gimbal/auto_aim/fire_control", fire_control_, false);
56+
register_input("/auto_aim/should_shoot", fire_control_, false);
5757

5858
register_input("/gimbal/bullet_feeder/velocity", bullet_feeder_velocity_);
5959
register_output(
@@ -102,8 +102,9 @@ class BulletFeederController17mm
102102

103103
if (*friction_ready_) {
104104
if (shoot_mode == ShootMode::AUTOMATIC) {
105-
bool triggered = mouse.left || switch_left == Switch::DOWN
106-
|| (switch_right == Switch::UP && *fire_control_);
105+
const bool auto_aim_enabled = mouse.right || switch_right == Switch::UP;
106+
bool triggered = (mouse.left && !mouse.right) || switch_left == Switch::DOWN
107+
|| (mouse.left && auto_aim_enabled && *fire_control_);
107108
bullet_allowance =
108109
triggered ? *control_bullet_allowance_limited_by_heat_ : 0;
109110
} else {
@@ -232,4 +233,4 @@ class BulletFeederController17mm
232233
#include <pluginlib/class_list_macros.hpp>
233234

234235
PLUGINLIB_EXPORT_CLASS(
235-
rmcs_core::controller::shooting::BulletFeederController17mm, rmcs_executor::Component)
236+
rmcs_core::controller::shooting::BulletFeederController17mm, rmcs_executor::Component)

0 commit comments

Comments
 (0)