Skip to content

Commit

Permalink
Add fast walking mode
Browse files Browse the repository at this point in the history
  • Loading branch information
@dmweis
dmweis committed Dec 14, 2023
1 parent b65ba68 commit 7bdcbec
Show file tree
Hide file tree
Showing 4 changed files with 88 additions and 32 deletions.
2 changes: 1 addition & 1 deletion Cargo.lock
View file

Some generated files are not rendered by default. Learn more about how customized files appear on GitHub.

23 changes: 10 additions & 13 deletions Cargo.toml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -8,7 +8,7 @@ name = "hopper_rust"
publish = false
readme = "README.md"
repository = "https://github.com/dmweis/hopper_rust"
version = "0.4.14"
version = "0.4.15"

[package.metadata.deb]
assets = [
Expand Down Expand Up @@ -50,22 +50,22 @@ visualizer = ["kiss3d", "gilrs"]
# Utilities
anyhow = "1.0"
bitflags = "2.3.3"
bytes = "1.4"
chrono = {version = "0.4", features = ["serde"]}
clap = {version = "4", features = ["derive"]}
config = "0.13.2"
ctrlc = "3.1"
lazy_static = "1.4"
rand = "0.8"
reqwest = {version = "0.11", features = ["json"]}
sha2 = "0.10"
tempdir = "0.3.7"
thiserror = "1.0"
walkdir = "2.3.3"
bytes = "1.4"
reqwest = {version = "0.11", features = ["json"]}
tempdir = "0.3.7"

# async
futures = "0.3"
async-trait = "0.1"
futures = "0.3"
tokio = {version = "1.6", features = [
"macros",
"rt-multi-thread",
Expand All @@ -83,13 +83,13 @@ tracing-subscriber = {version = "0.3", features = [
]}

# serialization
base64 = "0.21.0"
cobs-rs = "1.1.1"
schemars = "0.8.12"
serde = {version = "1.0", features = ["derive"]}
serde_json = "1.0"
serde_yaml = "0.9.25"
toml = "0.7.6"
schemars = "0.8.12"
cobs-rs = "1.1.1"
base64 = "0.21.0"

# other projects
azure_tts = {git = "https://github.com/dmweis/azure_tts", branch = "main", optional = true}
Expand All @@ -101,22 +101,19 @@ rplidar_driver = {git = "https://github.com/dmweis/rplidar_driver", branch = "ma
async-openai = {version = "0.17.1"}
# async-openai = {git = "https://github.com/dmweis/async-openai", branch = "main"}



serialport = {version = "4.0.1", default-features = false}
v4l = "0.14.0"
serialport = { version = "4.0.1", default-features = false }

gilrs = {version = "0.10", optional = true, features = ["serde-serialize"]}
kiss3d = {optional = true, version = "0.35"}
last-message-channel = {branch = "main", git = "https://github.com/dmweis/last-message-channel"}
mint = "0.5"
nalgebra = {version = "0.30", features = ["serde-serialize"]}

rodio = { version = "0.17", optional = true}
rodio = {version = "0.17", optional = true}
# fix weird bug with mp3 not playing from start
# rodio = {git = "https://github.com/RustAudio/rodio", rev = "55d957f8b40c59fccea4162c4b03f6dd87a7a4d9", optional = true}


# zenoh
zenoh = "0.7.2-rc"
zenoh-config = "0.7.2-rc"
Expand Down
1 change: 1 addition & 0 deletions src/motion_controller/walking.rs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -8,6 +8,7 @@ use tracing::*;

pub const DEFAULT_STEP_TIME: Duration = Duration::from_millis(600);
pub const DEFAULT_STEP_HEIGHT: f32 = 0.03;
pub const DEFAULT_STEP_DISTANCE: f32 = 0.025;

#[derive(Debug, Serialize, Deserialize, Copy, Clone, PartialEq)]
pub struct MoveCommand {
Expand Down
94 changes: 76 additions & 18 deletions src/zenoh_remote.rs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -4,8 +4,11 @@ use crate::hexapod::LegFlags;
use crate::high_five::HighFiveServiceController;
use crate::ioc_container::IocContainer;
use crate::lidar::LidarServiceController;
use crate::motion_controller::walking::{DEFAULT_STEP_HEIGHT, DEFAULT_STEP_TIME};
use crate::motion_controller::walking::{
DEFAULT_STEP_DISTANCE, DEFAULT_STEP_HEIGHT, DEFAULT_STEP_TIME,
};
use crate::motion_controller::{self, SingleLegCommand};
use crate::speech::SpeechService;
use crate::zenoh_consts::{
BODY_MOTOR_SPEED_SUBSCRIBER, COMPLIANCE_SLOPE_SUBSCRIBER, HOPPER_WALKING_CONFIG_PUBLISHER,
REMOTE_CONTROL_SUBSCRIBER, STANCE_SUBSCRIBER, WALKING_CONFIG_SUBSCRIBER,
Expand Down Expand Up @@ -275,13 +278,23 @@ impl GamepadController {
gamepad_message,
last_gamepad_message,
);
let lt_pressed_since_last = was_button_pressed_since_last_time(
let lt_pressed = was_button_pressed_since_last_time(
Button::LeftTrigger2,
gamepad_message,
last_gamepad_message,
);
let left_paddle_pressed = was_button_pressed_since_last_time(
Button::LeftPaddle,
gamepad_message,
last_gamepad_message,
);
let right_paddle_pressed = was_button_pressed_since_last_time(
Button::RightPaddle,
gamepad_message,
last_gamepad_message,
);

if lt_pressed_since_last {
if lt_pressed {
self.single_leg_foot = self.feet_iterator.next().unwrap();
info!("Switching to single leg foot {:?}", self.single_leg_foot);
}
Expand Down Expand Up @@ -327,32 +340,57 @@ impl GamepadController {
// clamp
self.height_offset = self.height_offset.max(-0.03).min(0.05);

let lb_pressed = is_button_down(Button::LeftTrigger, gamepad_message);
let rb_pressed = is_button_down(Button::RightTrigger, gamepad_message);
let lt_pressed = is_button_down(Button::LeftTrigger2, gamepad_message);
let rt_pressed = is_button_down(Button::RightTrigger2, gamepad_message);
let lb_down = is_button_down(Button::LeftTrigger, gamepad_message);
let rb_down = is_button_down(Button::RightTrigger, gamepad_message);
let lt_down = is_button_down(Button::LeftTrigger2, gamepad_message);
let rt_down = is_button_down(Button::RightTrigger2, gamepad_message);

if a_pressed {
info!("Setting stance to standing");
controller.set_body_state(motion_controller::BodyState::Standing);
} else if y_pressed {
}
if y_pressed {
info!("Starting happy dance");
controller.start_sequence(motion_controller::DanceMove::HappyDance);
} else if x_pressed {
}
if x_pressed {
info!("Starting wave");
controller.start_sequence(motion_controller::DanceMove::WaveHiWithSound);
} else if select_pressed {
}
if select_pressed {
info!("Folding");
controller.set_body_state(motion_controller::BodyState::Folded);
} else if start_pressed {
}
if start_pressed {
info!("Grounding");
controller.set_body_state(motion_controller::BodyState::Grounded);
} else if b_pressed {
}
if b_pressed {
info!("Starting random dance");
controller.start_sequence(motion_controller::DanceMove::Random);
}
if left_paddle_pressed {
self.walking_config.max_step_distance_m = 0.03;
self.walking_config.step_time = Duration::from_millis(400);
tokio::spawn(async move {
_ = IocContainer::global_instance()
.service::<SpeechService>()?
.say_eleven_with_default_voice("Fast walking mode!")
.await;
});
}
if right_paddle_pressed {
self.walking_config.max_step_distance_m = DEFAULT_STEP_DISTANCE;
self.walking_config.step_time = DEFAULT_STEP_TIME;
tokio::spawn(async move {
_ = IocContainer::global_instance()
.service::<SpeechService>()?
.say_eleven_with_default_voice("Regular walking mode!")
.await;
});
}

if lb_pressed {
if lb_down {
// translation mode
let x = -get_axis(Axis::LeftStickY, gamepad_message) * 0.05;
let y = get_axis(Axis::LeftStickX, gamepad_message) * 0.05;
Expand All @@ -362,7 +400,7 @@ impl GamepadController {
let translation = Vector3::new(x, y, 0.0);
let rotation = UnitQuaternion::from_euler_angles(0.0, pitch, yaw);
controller.set_transformation(translation, rotation);
} else if rb_pressed {
} else if rb_down {
// translation mode 2
let x = -get_axis(Axis::LeftStickY, gamepad_message) * 0.05;
let z = -get_axis(Axis::RightStickY, gamepad_message) * 0.05;
Expand All @@ -372,7 +410,7 @@ impl GamepadController {
let translation = Vector3::new(x, 0.0, z);
let rotation = UnitQuaternion::from_euler_angles(roll, 0.0, yaw);
controller.set_transformation(translation, rotation);
} else if rt_pressed {
} else if rt_down {
// walking mode
let x = get_axis(Axis::LeftStickY, gamepad_message)
* self.walking_config.max_step_distance_m;
Expand All @@ -394,7 +432,7 @@ impl GamepadController {
Default::default(),
);
controller.set_command(move_command);
} else if lt_pressed {
} else if lt_down {
let x = get_axis(Axis::LeftStickY, gamepad_message) * 0.07;
let y = -get_axis(Axis::LeftStickX, gamepad_message) * 0.07;
let z = get_axis(Axis::RightStickY, gamepad_message) * 0.07;
Expand Down Expand Up @@ -502,6 +540,8 @@ pub enum Button {
DPadLeft,
DPadRight,
Unknown,
LeftPaddle,
RightPaddle,
}

impl Button {
Expand Down Expand Up @@ -598,7 +638,7 @@ pub struct WalkingConfig {
impl Default for WalkingConfig {
fn default() -> Self {
WalkingConfig {
max_step_distance_m: 0.025,
max_step_distance_m: DEFAULT_STEP_DISTANCE,
step_time: DEFAULT_STEP_TIME,
step_height_m: DEFAULT_STEP_HEIGHT,
max_yaw_rate_deg: 15.0,
Expand Down Expand Up @@ -679,11 +719,29 @@ fn controller_reader(sender: &mut Sender<InputMessage>) -> HopperResult<()> {

gamepad_data.last_event_time = std::time::SystemTime::now().into();
match gilrs_event.event {
gilrs::EventType::ButtonPressed(button, _) => {
gilrs::EventType::ButtonPressed(button, code) => {
*gamepad_data
.button_down_event_counter
.entry(button.into())
.or_default() += 1;

// hacky hack to support paddles on the steam controller
match code.into_u32() {
65873_u32 => {
*gamepad_data
.button_down_event_counter
.entry(Button::RightPaddle)
.or_default() += 1;
}
65872_u32 => {
*gamepad_data
.button_down_event_counter
.entry(Button::LeftPaddle)
.or_default() += 1;
}
// ignore others
_ => (),
}
}
gilrs::EventType::ButtonReleased(button, _) => {
*gamepad_data
Expand Down

0 comments on commit 7bdcbec

Please sign in to comment.