Gamepad (#2145)

* add motion crate

* add motion crate

* --wip--

* add new remotecontrol behavior for new motion comman

* implement motioncommand changes

* move type definition

* implement get remote_motion_comman as separate node

* remove unneccesary variables and fmt

* remove unnecessary stuff

* fix rebase conflicts

* fix: last gait progress

* gamepad in thread

* add start to start gamepad back

* fix rebase

* fix format und konrads changes

* add robot and controler step

* review changes: connectionerror and step based loop

* fmt

* rename file

* fix controller reseting when joustick snaps

* dead zones

* round dead zones

* match label no data

Author: pejotejo

crates/control/src/motion/booster_walking.rs

@@ -3,12 +3,11 @@ use color_eyre::Result;
 use context_attribute::context;
 use framework::{AdditionalOutput, MainOutput};
 use hardware::PathsInterface;
-use linear_algebra::vector;
 use serde::{Deserialize, Serialize};
 use types::{
     cycle_time::CycleTime,
     joints::Joints,
-    motion_command::{HeadMotion, MotionCommand},
+    motion_command::MotionCommand,
     parameters::{MotorCommandParameters, RLWalkingParameters},
 };
 use walking_inference::{inference::WalkingInference, inputs::WalkingInferenceInputs};
@@ -36,6 +35,7 @@ pub struct CycleContext {
     imu_state: Input<ImuState, "imu_state">,
     serial_motor_states: Input<Joints<MotorState>, "serial_motor_states">,
     cycle_time: Input<CycleTime, "cycle_time">,
+    motion_command: Input<MotionCommand, "motion_command">,
 }
 
 #[context]
@@ -63,19 +63,10 @@ impl RLWalking {
     }
 
     pub fn cycle(&mut self, mut context: CycleContext) -> Result<MainOutputs> {
-        let motion_command = &MotionCommand::WalkWithVelocity {
-            velocity: vector!(
-                context.walking_parameters.walk_command[0],
-                context.walking_parameters.walk_command[1]
-            ),
-            angular_velocity: context.walking_parameters.walk_command[2],
-            head: HeadMotion::Center,
-        };
-
         let (walking_inference_inputs, inference_output_positions) =
             self.walking_inference.do_inference(
                 *context.cycle_time,
-                motion_command,
+                context.motion_command,
                 context.imu_state,
                 *context.serial_motor_states,
                 context.walking_parameters,

crates/control/src/motion/mod.rs

@@ -15,5 +15,6 @@ pub mod look_around;
 pub mod look_at;
 pub mod motion_selector;
 pub mod motor_commands_collector;
+pub mod remote_control;
 pub mod sit_down;
 pub mod wide_stance;

crates/control/src/motion/remote_control.rs

@@ -0,0 +1,48 @@
+use color_eyre::Result;
+use context_attribute::context;
+
+use framework::MainOutput;
+use linear_algebra::vector;
+use serde::{Deserialize, Serialize};
+use types::{
+    motion_command::{HeadMotion, MotionCommand},
+    parameters::RemoteControlParameters,
+};
+
+#[context]
+pub struct CreationContext {}
+
+#[context]
+pub struct CycleContext {
+    remote_control_parameters: Parameter<RemoteControlParameters, "remote_control_parameters">,
+}
+
+#[derive(Deserialize, Serialize)]
+pub struct RemoteControl {}
+
+#[context]
+#[derive(Default)]
+pub struct MainOutputs {
+    pub motion_command: MainOutput<MotionCommand>,
+}
+
+impl RemoteControl {
+    pub fn new(_context: CreationContext) -> Result<Self> {
+        Ok(RemoteControl {})
+    }
+
+    pub fn cycle(&mut self, context: CycleContext) -> Result<MainOutputs> {
+        let motion_command = MotionCommand::WalkWithVelocity {
+            head: HeadMotion::Center,
+            velocity: vector![
+                context.remote_control_parameters.walk.forward,
+                context.remote_control_parameters.walk.left,
+            ],
+            angular_velocity: context.remote_control_parameters.walk.turn,
+        };
+
+        Ok(MainOutputs {
+            motion_command: motion_command.into(),
+        })
+    }
+}

crates/hulk_manifest/src/lib.rs

@@ -72,6 +72,7 @@ pub fn collect_hulk_cyclers(root: impl AsRef<Path>) -> Result<Cyclers, Error> {
                     // "control::motion::center_jump",
                     "control::motion::command_sender",
                     "control::motion::booster_walking",
+                    "control::motion::remote_control",
                     // "control::motion::condition_input_provider",
                     // "control::motion::dispatching_interpolator",
                     // "control::motion::fall_protector",

crates/types/src/parameters.rs

@@ -15,8 +15,16 @@ use crate::{
     joints::Joints,
     motion_command::{KickVariant, MotionCommand},
     roles::Role,
+    step::Step,
 };
 
+#[derive(
+    Clone, Debug, Default, Deserialize, Serialize, PathSerialize, PathDeserialize, PathIntrospect,
+)]
+pub struct RemoteControlParameters {
+    pub walk: Step,
+}
+
 #[derive(
     Clone, Debug, Default, Deserialize, Serialize, PathSerialize, PathDeserialize, PathIntrospect,
 )]

crates/walking_inference/src/inputs.rs

@@ -74,11 +74,11 @@ impl WalkingInferenceInputs {
             _ => todo!(),
         };
 
-        let gait_frequency =
+        let (gait_frequency, last_gait_progress) =
             if linear_velocity_command.norm() < 1e-5 && angular_velocity_command.abs() < 1e-5 {
-                0.0
+                (0.0, 0.0)
             } else {
-                walking_parameters.gait_frequency
+                (walking_parameters.gait_frequency, last_gait_progress)
             };
         let gait_progress =
             last_gait_progress + gait_frequency * cycle_time.last_cycle_duration.as_secs_f32();

etc/parameters/default.json

@@ -266,5 +266,12 @@
         "ankle_down": 1
       }
     }
+  },
+  "remote_control_parameters": {
+    "walk": {
+      "forward": 0.0,
+      "left": 0.0,
+      "turn": 0.0
+    }
   }
 }

tools/twix/src/panels/mod.rs

@@ -12,7 +12,7 @@ mod map;
 mod mujoco_simulator;
 mod parameter;
 mod plot;
-mod remote;
+mod remote_control;
 mod text;
 mod vision_tuner;
 
@@ -32,6 +32,6 @@ pub use map::MapPanel;
 pub use mujoco_simulator::MujocoSimulatorPanel;
 pub use parameter::ParameterPanel;
 pub use plot::PlotPanel;
-pub use remote::RemotePanel;
+pub use remote_control::RemotePanel;
 pub use text::TextPanel;
 pub use vision_tuner::VisionTunerPanel;