|
| 1 | +use bevy::prelude::*; |
| 2 | +use nidhogg::{types::JointArray, NaoControlMessage, NaoState}; |
| 3 | + |
| 4 | +use crate::{nao::CycleTime, prelude::PreWrite}; |
| 5 | + |
| 6 | +const MIN_CURRENT: f32 = 0.09; |
| 7 | +const MAX_CURRENT: f32 = 0.12; |
| 8 | + |
| 9 | +/// Joint position encoders have a resolution of 1/4096, but we apply a smaller |
| 10 | +/// incremental adjustment of 0.01/4096, scaled by cycle time. |
| 11 | +/// |
| 12 | +/// During longer cycles, smaller adjustments are enough, as the motor controllers run as part of HAL, not yggdrasil. |
| 13 | +const REDUCTION: f32 = 0.01 / 4096.0; |
| 14 | + |
| 15 | +pub struct EnergyOptimizerPlugin; |
| 16 | + |
| 17 | +impl Plugin for EnergyOptimizerPlugin { |
| 18 | + fn build(&self, app: &mut App) { |
| 19 | + app.init_resource::<JointCurrentOptimizer>(); |
| 20 | + app.add_systems( |
| 21 | + PreWrite, |
| 22 | + optimize_joint_currents |
| 23 | + .after(crate::nao::finalize) |
| 24 | + .run_if(should_optimize_joint_current), |
| 25 | + ); |
| 26 | + } |
| 27 | +} |
| 28 | + |
| 29 | +#[derive(Debug, Resource, Default)] |
| 30 | +pub struct JointCurrentOptimizer { |
| 31 | + enabled: bool, |
| 32 | + has_reach_minimum_current: bool, |
| 33 | + joint_offsets: JointArray<f32>, |
| 34 | +} |
| 35 | + |
| 36 | +/// Run condition that returns `true` when the [`JointCurrentOptimizer`] is enabled. |
| 37 | +fn should_optimize_joint_current(state: Res<JointCurrentOptimizer>) -> bool { |
| 38 | + state.enabled |
| 39 | +} |
| 40 | + |
| 41 | +/// Extension trait for toggling joint energy optimization. |
| 42 | +pub trait EnergyOptimizerExt<'w, 's> { |
| 43 | + /// Enables joint current optimization, using [`JointCurrentOptimizer`]. |
| 44 | + fn optimize_joint_currents(&mut self); |
| 45 | + |
| 46 | + /// Resets the [`JointCurrentOptimizer`] state. |
| 47 | + fn reset_joint_current_optimizer(&mut self); |
| 48 | +} |
| 49 | + |
| 50 | +impl<'w, 's> EnergyOptimizerExt<'w, 's> for Commands<'w, 's> { |
| 51 | + fn optimize_joint_currents(&mut self) { |
| 52 | + self.queue(|world: &mut World| { |
| 53 | + world.resource_mut::<JointCurrentOptimizer>().enabled = true; |
| 54 | + }); |
| 55 | + } |
| 56 | + |
| 57 | + fn reset_joint_current_optimizer(&mut self) { |
| 58 | + self.queue(|world: &mut World| { |
| 59 | + world.init_resource::<JointCurrentOptimizer>(); |
| 60 | + }); |
| 61 | + } |
| 62 | +} |
| 63 | + |
| 64 | +/// System that resets the joint current optimizer state. |
| 65 | +pub fn reset_joint_current_optimizer(mut state: ResMut<JointCurrentOptimizer>) { |
| 66 | + *state = JointCurrentOptimizer::default(); |
| 67 | +} |
| 68 | + |
| 69 | +/// System that optimises the requested joint positions based on the maximum current. |
| 70 | +/// |
| 71 | +/// Based on the implementation as described in the Berlin United 2019 Tech Report. |
| 72 | +fn optimize_joint_currents( |
| 73 | + nao_state: Res<NaoState>, |
| 74 | + mut control_msg: ResMut<NaoControlMessage>, |
| 75 | + mut state: ResMut<JointCurrentOptimizer>, |
| 76 | + cycle_time: Res<CycleTime>, |
| 77 | +) { |
| 78 | + let (joint_idx, max_current) = nao_state |
| 79 | + .current |
| 80 | + .as_array_ref() |
| 81 | + .into_iter() |
| 82 | + .enumerate() |
| 83 | + .max_by(|(_, a), (_, b)| a.total_cmp(b)) |
| 84 | + .unwrap(); |
| 85 | + |
| 86 | + state.has_reach_minimum_current = if *max_current < MIN_CURRENT { |
| 87 | + true |
| 88 | + } else if *max_current > MAX_CURRENT { |
| 89 | + false |
| 90 | + } else { |
| 91 | + state.has_reach_minimum_current |
| 92 | + }; |
| 93 | + |
| 94 | + if !state.has_reach_minimum_current { |
| 95 | + let max_adjustment = REDUCTION / cycle_time.duration.as_secs_f32(); |
| 96 | + |
| 97 | + if let Some(joint_offset) = state.joint_offsets.get_mut(joint_idx) { |
| 98 | + let requested_joints = control_msg.position.as_array_ref(); |
| 99 | + let measured_joints = nao_state.position.as_array_ref(); |
| 100 | + |
| 101 | + *joint_offset += (requested_joints[joint_idx] - measured_joints[joint_idx]) |
| 102 | + .clamp(-max_adjustment, max_adjustment); |
| 103 | + } |
| 104 | + } |
| 105 | + |
| 106 | + control_msg.position = control_msg |
| 107 | + .position |
| 108 | + .clone() |
| 109 | + .zip(state.joint_offsets.clone()) |
| 110 | + .map(|(position, offset)| position + offset); |
| 111 | + |
| 112 | + // reset the enabled state, so it can be enabled again next cycle if needed. |
| 113 | + state.enabled = false; |
| 114 | +} |
0 commit comments