From a7d084cf6b8501ebc9c194c62baf1fff33c35172 Mon Sep 17 00:00:00 2001 From: Gijs de Jong Date: Sun, 22 Feb 2026 19:16:30 +0100 Subject: [PATCH 01/15] add missing high level apis --- booster_sdk/src/client/ai_client.rs | 271 +++++++++ .../src/client/light_control_client.rs | 109 ++++ booster_sdk/src/client/loco_client.rs | 341 +++++++++-- booster_sdk/src/client/mod.rs | 9 + booster_sdk/src/client/util.rs | 12 + booster_sdk/src/client/vision_client.rs | 151 +++++ booster_sdk/src/client/x5_camera_client.rs | 160 ++++++ booster_sdk/src/dds/messages.rs | 10 +- booster_sdk/src/dds/rpc.rs | 55 +- booster_sdk/src/dds/topics.rs | 70 ++- booster_sdk/src/types/b1.rs | 543 ++++++++++++++++++ booster_sdk/src/types/error.rs | 8 + booster_sdk/src/types/mod.rs | 2 + booster_sdk/src/types/robot.rs | 19 +- 14 files changed, 1697 insertions(+), 63 deletions(-) create mode 100644 booster_sdk/src/client/ai_client.rs create mode 100644 booster_sdk/src/client/light_control_client.rs create mode 100644 booster_sdk/src/client/util.rs create mode 100644 booster_sdk/src/client/vision_client.rs create mode 100644 booster_sdk/src/client/x5_camera_client.rs create mode 100644 booster_sdk/src/types/b1.rs diff --git a/booster_sdk/src/client/ai_client.rs b/booster_sdk/src/client/ai_client.rs new file mode 100644 index 0000000..84d3b39 --- /dev/null +++ b/booster_sdk/src/client/ai_client.rs @@ -0,0 +1,271 @@ +//! AI and LUI high-level RPC clients. + +use serde::{Deserialize, Serialize, de::DeserializeOwned}; + +use crate::dds::{ + AI_API_TOPIC, DdsNode, DdsSubscription, LUI_API_TOPIC, RpcClient, RpcClientOptions, + ai_subtitle_topic, lui_asr_chunk_topic, +}; +use crate::types::Result; + +use super::util::{EmptyResponse, serialize_param}; + +#[derive(Debug, Clone, Copy, PartialEq, Eq, Hash, Serialize, Deserialize)] +#[serde(into = "i32", try_from = "i32")] +#[repr(i32)] +pub enum AiApiId { + StartAiChat = 2000, + StopAiChat = 2001, + Speak = 2002, + StartFaceTracking = 2003, + StopFaceTracking = 2004, +} + +impl From for i32 { + fn from(value: AiApiId) -> Self { + value as i32 + } +} + +impl TryFrom for AiApiId { + type Error = &'static str; + + fn try_from(value: i32) -> std::result::Result { + match value { + 2000 => Ok(Self::StartAiChat), + 2001 => Ok(Self::StopAiChat), + 2002 => Ok(Self::Speak), + 2003 => Ok(Self::StartFaceTracking), + 2004 => Ok(Self::StopFaceTracking), + _ => Err("invalid value"), + } + } +} + +#[derive(Debug, Clone, Copy, PartialEq, Eq, Hash, Serialize, Deserialize)] +#[serde(into = "i32", try_from = "i32")] +#[repr(i32)] +pub enum LuiApiId { + StartAsr = 1000, + StopAsr = 1001, + StartTts = 1050, + StopTts = 1051, + SendTtsText = 1052, +} + +impl From for i32 { + fn from(value: LuiApiId) -> Self { + value as i32 + } +} + +impl TryFrom for LuiApiId { + type Error = &'static str; + + fn try_from(value: i32) -> std::result::Result { + match value { + 1000 => Ok(Self::StartAsr), + 1001 => Ok(Self::StopAsr), + 1050 => Ok(Self::StartTts), + 1051 => Ok(Self::StopTts), + 1052 => Ok(Self::SendTtsText), + _ => Err("invalid value"), + } + } +} + +#[derive(Debug, Clone, PartialEq, Eq, Serialize, Deserialize)] +pub struct TtsConfig { + pub voice_type: String, + pub ignore_bracket_text: Vec, +} + +#[derive(Debug, Clone, PartialEq, Eq, Serialize, Deserialize)] +pub struct LlmConfig { + pub system_prompt: String, + pub welcome_msg: String, + pub prompt_name: String, +} + +#[derive(Debug, Clone, PartialEq, Eq, Serialize, Deserialize)] +pub struct AsrConfig { + pub interrupt_speech_duration: i32, + pub interrupt_keywords: Vec, +} + +#[derive(Debug, Clone, PartialEq, Eq, Serialize, Deserialize)] +pub struct StartAiChatParameter { + pub interrupt_mode: bool, + pub asr_config: AsrConfig, + pub llm_config: LlmConfig, + pub tts_config: TtsConfig, + pub enable_face_tracking: bool, +} + +#[derive(Debug, Clone, PartialEq, Eq, Serialize, Deserialize)] +pub struct SpeakParameter { + pub msg: String, +} + +#[derive(Debug, Clone, PartialEq, Eq, Serialize, Deserialize)] +pub struct LuiTtsConfig { + pub voice_type: String, +} + +#[derive(Debug, Clone, PartialEq, Eq, Serialize, Deserialize)] +pub struct LuiTtsParameter { + pub text: String, +} + +/// AI subtitle topic payload. +#[derive(Debug, Clone, PartialEq, Eq, Serialize, Deserialize)] +pub struct Subtitle { + pub magic_number: String, + pub text: String, + pub language: String, + pub user_id: String, + pub seq: i32, + pub definite: bool, + pub paragraph: bool, + pub round_id: i32, +} + +/// LUI ASR chunk topic payload. +#[derive(Debug, Clone, PartialEq, Eq, Serialize, Deserialize)] +pub struct AsrChunk { + pub text: String, +} + +pub const BOOSTER_ROBOT_USER_ID: &str = "BoosterRobot"; + +/// High-level RPC client for AI chat features. +pub struct AiClient { + rpc: RpcClient, + node: DdsNode, +} + +impl AiClient { + pub fn new() -> Result { + Self::with_options(RpcClientOptions::for_service(AI_API_TOPIC)) + } + + pub fn with_options(options: RpcClientOptions) -> Result { + let rpc = RpcClient::new(options.with_service_topic(AI_API_TOPIC))?; + let node = rpc.node().clone(); + Ok(Self { rpc, node }) + } + + pub fn node(&self) -> &DdsNode { + &self.node + } + + pub async fn send_api_request(&self, api_id: AiApiId, param: &str) -> Result<()> { + self.rpc + .call_with_body::(i32::from(api_id), param.to_owned(), None) + .await?; + Ok(()) + } + + pub async fn send_api_request_with_response(&self, api_id: AiApiId, param: &str) -> Result + where + R: DeserializeOwned + Send + 'static, + { + self.rpc + .call_with_body(i32::from(api_id), param.to_owned(), None) + .await + } + + pub async fn start_ai_chat(&self, param: &StartAiChatParameter) -> Result<()> { + self.send_api_request(AiApiId::StartAiChat, &serialize_param(param)?) + .await + } + + pub async fn stop_ai_chat(&self) -> Result<()> { + self.send_api_request(AiApiId::StopAiChat, "").await + } + + pub async fn speak(&self, param: &SpeakParameter) -> Result<()> { + self.send_api_request(AiApiId::Speak, &serialize_param(param)?) + .await + } + + pub async fn start_face_tracking(&self) -> Result<()> { + self.send_api_request(AiApiId::StartFaceTracking, "").await + } + + pub async fn stop_face_tracking(&self) -> Result<()> { + self.send_api_request(AiApiId::StopFaceTracking, "").await + } + + pub fn subscribe_subtitle(&self) -> Result> { + self.node.subscribe(&ai_subtitle_topic(), 16) + } +} + +/// High-level RPC client for LUI ASR/TTS features. +pub struct LuiClient { + rpc: RpcClient, + node: DdsNode, +} + +impl LuiClient { + pub fn new() -> Result { + Self::with_options(RpcClientOptions::for_service(LUI_API_TOPIC)) + } + + pub fn with_options(options: RpcClientOptions) -> Result { + let rpc = RpcClient::new(options.with_service_topic(LUI_API_TOPIC))?; + let node = rpc.node().clone(); + Ok(Self { rpc, node }) + } + + pub fn node(&self) -> &DdsNode { + &self.node + } + + pub async fn send_api_request(&self, api_id: LuiApiId, param: &str) -> Result<()> { + self.rpc + .call_with_body::(i32::from(api_id), param.to_owned(), None) + .await?; + Ok(()) + } + + pub async fn send_api_request_with_response( + &self, + api_id: LuiApiId, + param: &str, + ) -> Result + where + R: DeserializeOwned + Send + 'static, + { + self.rpc + .call_with_body(i32::from(api_id), param.to_owned(), None) + .await + } + + pub async fn start_asr(&self) -> Result<()> { + self.send_api_request(LuiApiId::StartAsr, "").await + } + + pub async fn stop_asr(&self) -> Result<()> { + self.send_api_request(LuiApiId::StopAsr, "").await + } + + pub async fn start_tts(&self, config: &LuiTtsConfig) -> Result<()> { + self.send_api_request(LuiApiId::StartTts, &serialize_param(config)?) + .await + } + + pub async fn stop_tts(&self) -> Result<()> { + self.send_api_request(LuiApiId::StopTts, "").await + } + + pub async fn send_tts_text(&self, param: &LuiTtsParameter) -> Result<()> { + self.send_api_request(LuiApiId::SendTtsText, &serialize_param(param)?) + .await + } + + pub fn subscribe_asr_chunk(&self) -> Result> { + self.node.subscribe(&lui_asr_chunk_topic(), 16) + } +} diff --git a/booster_sdk/src/client/light_control_client.rs b/booster_sdk/src/client/light_control_client.rs new file mode 100644 index 0000000..23bddf3 --- /dev/null +++ b/booster_sdk/src/client/light_control_client.rs @@ -0,0 +1,109 @@ +//! LED light control RPC client. + +use serde::{Deserialize, Serialize, de::DeserializeOwned}; + +use crate::dds::{LIGHT_CONTROL_API_TOPIC, RpcClient, RpcClientOptions}; +use crate::types::Result; + +use super::util::{EmptyResponse, serialize_param}; + +#[derive(Debug, Clone, Copy, PartialEq, Eq, Hash, Serialize, Deserialize)] +#[serde(into = "i32", try_from = "i32")] +#[repr(i32)] +pub enum LightApiId { + SetLedLightColor = 2000, + StopLedLightControl = 2001, +} + +impl From for i32 { + fn from(value: LightApiId) -> Self { + value as i32 + } +} + +impl TryFrom for LightApiId { + type Error = &'static str; + + fn try_from(value: i32) -> std::result::Result { + match value { + 2000 => Ok(Self::SetLedLightColor), + 2001 => Ok(Self::StopLedLightControl), + _ => Err("invalid value"), + } + } +} + +#[derive(Debug, Clone, Copy, PartialEq, Eq, Serialize, Deserialize)] +pub struct SetLedLightColorParameter { + pub r: u8, + pub g: u8, + pub b: u8, +} + +impl SetLedLightColorParameter { + #[must_use] + pub fn from_hex(color: &str) -> Option { + let color = color.trim(); + if color.len() != 7 || !color.starts_with('#') { + return None; + } + + let r = u8::from_str_radix(&color[1..3], 16).ok()?; + let g = u8::from_str_radix(&color[3..5], 16).ok()?; + let b = u8::from_str_radix(&color[5..7], 16).ok()?; + + Some(Self { r, g, b }) + } +} + +/// High-level RPC client for LED light control APIs. +pub struct LightControlClient { + rpc: RpcClient, +} + +impl LightControlClient { + pub fn new() -> Result { + Self::with_options(RpcClientOptions::for_service(LIGHT_CONTROL_API_TOPIC)) + } + + pub fn with_options(options: RpcClientOptions) -> Result { + let rpc = RpcClient::new(options.with_service_topic(LIGHT_CONTROL_API_TOPIC))?; + Ok(Self { rpc }) + } + + pub async fn send_api_request(&self, api_id: LightApiId, param: &str) -> Result<()> { + self.rpc + .call_with_body::(i32::from(api_id), param.to_owned(), None) + .await?; + Ok(()) + } + + pub async fn send_api_request_with_response( + &self, + api_id: LightApiId, + param: &str, + ) -> Result + where + R: DeserializeOwned + Send + 'static, + { + self.rpc + .call_with_body(i32::from(api_id), param.to_owned(), None) + .await + } + + pub async fn set_led_light_color(&self, r: u8, g: u8, b: u8) -> Result<()> { + let param = SetLedLightColorParameter { r, g, b }; + self.send_api_request(LightApiId::SetLedLightColor, &serialize_param(¶m)?) + .await + } + + pub async fn set_led_light_color_param(&self, param: &SetLedLightColorParameter) -> Result<()> { + self.send_api_request(LightApiId::SetLedLightColor, &serialize_param(param)?) + .await + } + + pub async fn stop_led_light_control(&self) -> Result<()> { + self.send_api_request(LightApiId::StopLedLightControl, "") + .await + } +} diff --git a/booster_sdk/src/client/loco_client.rs b/booster_sdk/src/client/loco_client.rs index 73dd1eb..7cbf31a 100644 --- a/booster_sdk/src/client/loco_client.rs +++ b/booster_sdk/src/client/loco_client.rs @@ -1,5 +1,6 @@ -//! High-level Booster robot client built on DDS. +//! High-level B1 locomotion client built on DDS RPC and topic I/O. +use super::util::{EmptyResponse, serialize_param}; use crate::dds::{ BatteryState, BinaryData, ButtonEventMsg, DdsNode, DdsPublisher, DdsSubscription, GripperControl, LightControlMsg, MotionState, RemoteControllerState, RobotProcessStateMsg, @@ -8,21 +9,20 @@ use crate::dds::{ motion_state_topic, process_state_topic, remote_controller_topic, safe_mode_topic, video_stream_topic, }; -use crate::types::{Result, RobotMode}; -use serde::{Deserialize, Serialize}; - -const CHANGE_MODE_API_ID: i32 = 2000; -const MOVE_API_ID: i32 = 2001; - +use crate::types::{ + BoosterHandType, CustomTrainedTraj, DanceId, DexterousFingerParameter, Frame, GetModeResponse, + GetRobotInfoResponse, GetStatusResponse, GripperControlMode, GripperMotionParameter, + HandAction, HandIndex, LoadCustomTrainedTrajResponse, LocoApiId, Result, RobotMode, Transform, + WholeBodyDanceId, +}; +use serde::Deserialize; +use serde_json::json; // The controller may send an intermediate pending status (-1) before the // final success response. Mode transitions (especially PREPARE) can take // several seconds. const CHANGE_MODE_TIMEOUT: std::time::Duration = std::time::Duration::from_secs(30); -#[derive(Deserialize)] -struct EmptyResponse {} - -/// High-level client for Booster robot control and telemetry. +/// High-level client for B1 locomotion control and telemetry. pub struct BoosterClient { rpc: RpcClient, node: DdsNode, @@ -52,38 +52,312 @@ impl BoosterClient { }) } - pub async fn change_mode(&self, mode: RobotMode) -> Result<()> { - #[derive(Serialize)] - struct Params { - mode: i32, - } + pub fn node(&self) -> &DdsNode { + &self.node + } + + pub async fn send_api_request(&self, api_id: LocoApiId, param: &str) -> Result<()> { + self.rpc + .call_with_body::(i32::from(api_id), param.to_owned(), None) + .await?; + Ok(()) + } + + pub async fn send_api_request_with_response( + &self, + api_id: LocoApiId, + param: &str, + ) -> Result + where + R: for<'de> Deserialize<'de> + Send + 'static, + { + self.rpc + .call_with_body(i32::from(api_id), param.to_owned(), None) + .await + } + pub async fn change_mode(&self, mode: RobotMode) -> Result<()> { + let param = json!({ "mode": i32::from(mode) }).to_string(); self.rpc - .call::( - CHANGE_MODE_API_ID, - &Params { - mode: i32::from(mode), - }, + .call_with_body::( + i32::from(LocoApiId::ChangeMode), + param, Some(CHANGE_MODE_TIMEOUT), ) .await?; - Ok(()) } + pub async fn get_mode(&self) -> Result { + self.send_api_request_with_response(LocoApiId::GetMode, "") + .await + } + + pub async fn get_status(&self) -> Result { + self.send_api_request_with_response(LocoApiId::GetStatus, "") + .await + } + + pub async fn get_robot_info(&self) -> Result { + self.send_api_request_with_response(LocoApiId::GetRobotInfo, "") + .await + } + pub async fn move_robot(&self, vx: f32, vy: f32, vyaw: f32) -> Result<()> { - #[derive(Serialize)] - struct Params { - vx: f32, - vy: f32, - vyaw: f32, - } + let param = json!({ "vx": vx, "vy": vy, "vyaw": vyaw }).to_string(); + self.send_api_request(LocoApiId::Move, ¶m).await + } - self.rpc - .call::(MOVE_API_ID, &Params { vx, vy, vyaw }, None) - .await?; + pub async fn rotate_head(&self, pitch: f32, yaw: f32) -> Result<()> { + let param = json!({ "pitch": pitch, "yaw": yaw }).to_string(); + self.send_api_request(LocoApiId::RotateHead, ¶m).await + } - Ok(()) + pub async fn wave_hand(&self, action: HandAction) -> Result<()> { + let param = json!({ + "hand_index": i32::from(HandIndex::Right), + "hand_action": i32::from(action), + }) + .to_string(); + self.send_api_request(LocoApiId::WaveHand, ¶m).await + } + + pub async fn rotate_head_with_direction( + &self, + pitch_direction: i32, + yaw_direction: i32, + ) -> Result<()> { + let param = json!({ + "pitch_direction": pitch_direction, + "yaw_direction": yaw_direction, + }) + .to_string(); + self.send_api_request(LocoApiId::RotateHeadWithDirection, ¶m) + .await + } + + pub async fn lie_down(&self) -> Result<()> { + self.send_api_request(LocoApiId::LieDown, "").await + } + + pub async fn get_up(&self) -> Result<()> { + self.send_api_request(LocoApiId::GetUp, "").await + } + + pub async fn get_up_with_mode(&self, mode: RobotMode) -> Result<()> { + let param = json!({ "mode": i32::from(mode) }).to_string(); + self.send_api_request(LocoApiId::GetUpWithMode, ¶m) + .await + } + + pub async fn shoot(&self) -> Result<()> { + self.send_api_request(LocoApiId::Shoot, "").await + } + + pub async fn push_up(&self) -> Result<()> { + self.send_api_request(LocoApiId::PushUp, "").await + } + + pub async fn move_hand_end_effector_with_aux( + &self, + target_posture: &crate::types::Posture, + aux_posture: &crate::types::Posture, + time_millis: i32, + hand_index: HandIndex, + ) -> Result<()> { + let param = json!({ + "target_posture": target_posture, + "aux_posture": aux_posture, + "time_millis": time_millis, + "hand_index": i32::from(hand_index), + "has_aux": true, + "new_version": false, + }) + .to_string(); + self.send_api_request(LocoApiId::MoveHandEndEffector, ¶m) + .await + } + + pub async fn move_hand_end_effector( + &self, + target_posture: &crate::types::Posture, + time_millis: i32, + hand_index: HandIndex, + ) -> Result<()> { + let param = json!({ + "target_posture": target_posture, + "time_millis": time_millis, + "hand_index": i32::from(hand_index), + "has_aux": false, + "new_version": false, + }) + .to_string(); + self.send_api_request(LocoApiId::MoveHandEndEffector, ¶m) + .await + } + + pub async fn move_hand_end_effector_v2( + &self, + target_posture: &crate::types::Posture, + time_millis: i32, + hand_index: HandIndex, + ) -> Result<()> { + let param = json!({ + "target_posture": target_posture, + "time_millis": time_millis, + "hand_index": i32::from(hand_index), + "has_aux": false, + "new_version": true, + }) + .to_string(); + self.send_api_request(LocoApiId::MoveHandEndEffector, ¶m) + .await + } + + pub async fn stop_hand_end_effector(&self) -> Result<()> { + self.send_api_request(LocoApiId::StopHandEndEffector, "") + .await + } + + pub async fn control_gripper( + &self, + motion_param: GripperMotionParameter, + mode: GripperControlMode, + hand_index: HandIndex, + ) -> Result<()> { + let param = json!({ + "motion_param": motion_param, + "mode": i32::from(mode), + "hand_index": i32::from(hand_index), + }) + .to_string(); + self.send_api_request(LocoApiId::ControlGripper, ¶m) + .await + } + + pub async fn get_frame_transform(&self, src: Frame, dst: Frame) -> Result { + let param = json!({ + "src": i32::from(src), + "dst": i32::from(dst), + }) + .to_string(); + self.send_api_request_with_response(LocoApiId::GetFrameTransform, ¶m) + .await + } + + pub async fn switch_hand_end_effector_control_mode(&self, switch_on: bool) -> Result<()> { + let param = json!({ "switch_on": switch_on }).to_string(); + self.send_api_request(LocoApiId::SwitchHandEndEffectorControlMode, ¶m) + .await + } + + pub async fn handshake(&self, action: HandAction) -> Result<()> { + let param = json!({ "hand_action": i32::from(action) }).to_string(); + self.send_api_request(LocoApiId::Handshake, ¶m).await + } + + pub async fn control_dexterous_hand( + &self, + finger_params: &[DexterousFingerParameter], + hand_index: HandIndex, + hand_type: BoosterHandType, + ) -> Result<()> { + let param = json!({ + "finger_params": finger_params, + "hand_index": i32::from(hand_index), + "hand_type": i32::from(hand_type), + }) + .to_string(); + self.send_api_request(LocoApiId::ControlDexterousHand, ¶m) + .await + } + + pub async fn control_dexterous_hand_default( + &self, + finger_params: &[DexterousFingerParameter], + hand_index: HandIndex, + ) -> Result<()> { + self.control_dexterous_hand(finger_params, hand_index, BoosterHandType::InspireHand) + .await + } + + pub async fn dance(&self, dance_id: DanceId) -> Result<()> { + let param = json!({ "dance_id": i32::from(dance_id) }).to_string(); + self.send_api_request(LocoApiId::Dance, ¶m).await + } + + pub async fn play_sound(&self, sound_file_path: impl Into) -> Result<()> { + let param = json!({ "sound_file_path": sound_file_path.into() }).to_string(); + self.send_api_request(LocoApiId::PlaySound, ¶m).await + } + + pub async fn stop_sound(&self) -> Result<()> { + self.send_api_request(LocoApiId::StopSound, "").await + } + + pub async fn zero_torque_drag(&self, active: bool) -> Result<()> { + let param = json!({ "enable": active }).to_string(); + self.send_api_request(LocoApiId::ZeroTorqueDrag, ¶m) + .await + } + + pub async fn record_trajectory(&self, active: bool) -> Result<()> { + let param = json!({ "enable": active }).to_string(); + self.send_api_request(LocoApiId::RecordTrajectory, ¶m) + .await + } + + pub async fn replay_trajectory(&self, traj_file_path: impl Into) -> Result<()> { + let param = json!({ "traj_file_path": traj_file_path.into() }).to_string(); + self.send_api_request(LocoApiId::ReplayTrajectory, ¶m) + .await + } + + pub async fn whole_body_dance(&self, dance_id: WholeBodyDanceId) -> Result<()> { + let param = json!({ "dance_id": i32::from(dance_id) }).to_string(); + self.send_api_request(LocoApiId::WholeBodyDance, ¶m) + .await + } + + pub async fn upper_body_custom_control(&self, start: bool) -> Result<()> { + let param = json!({ "start": start }).to_string(); + self.send_api_request(LocoApiId::UpperBodyCustomControl, ¶m) + .await + } + + pub async fn reset_odometry(&self) -> Result<()> { + self.send_api_request(LocoApiId::ResetOdometry, "").await + } + + pub async fn load_custom_trained_traj( + &self, + traj: &CustomTrainedTraj, + ) -> Result { + self.send_api_request_with_response( + LocoApiId::LoadCustomTrainedTraj, + &serialize_param(traj)?, + ) + .await + } + + pub async fn activate_custom_trained_traj(&self, tid: impl Into) -> Result<()> { + let param = json!({ "tid": tid.into() }).to_string(); + self.send_api_request(LocoApiId::ActivateCustomTrainedTraj, ¶m) + .await + } + + pub async fn unload_custom_trained_traj(&self, tid: impl Into) -> Result<()> { + let param = json!({ "tid": tid.into() }).to_string(); + self.send_api_request(LocoApiId::UnloadCustomTrainedTraj, ¶m) + .await + } + + pub async fn enter_wbc_gait(&self) -> Result<()> { + self.send_api_request(LocoApiId::EnterWbcGait, "").await + } + + pub async fn exit_wbc_gait(&self) -> Result<()> { + self.send_api_request(LocoApiId::ExitWbcGait, "").await } pub fn publish_gripper(&self, control: GripperControl) -> Result<()> { @@ -130,3 +404,6 @@ impl BoosterClient { self.node.subscribe(&video_stream_topic(), 4) } } + +/// Alias matching the C++ class naming. +pub type B1LocoClient = BoosterClient; diff --git a/booster_sdk/src/client/mod.rs b/booster_sdk/src/client/mod.rs index 576fcaf..f880f29 100644 --- a/booster_sdk/src/client/mod.rs +++ b/booster_sdk/src/client/mod.rs @@ -1,7 +1,16 @@ //! High-level client APIs for the Booster Robotics SDK. +pub mod ai_client; pub mod commands; +pub mod light_control_client; pub mod loco_client; +mod util; +pub mod vision_client; +pub mod x5_camera_client; +pub use ai_client::*; pub use commands::*; +pub use light_control_client::*; pub use loco_client::*; +pub use vision_client::*; +pub use x5_camera_client::*; diff --git a/booster_sdk/src/client/util.rs b/booster_sdk/src/client/util.rs new file mode 100644 index 0000000..ea3e1d9 --- /dev/null +++ b/booster_sdk/src/client/util.rs @@ -0,0 +1,12 @@ +//! Internal client utilities shared across high-level client modules. + +use serde::Serialize; + +use crate::types::Result; + +#[derive(Debug, Clone, serde::Deserialize, Default)] +pub(crate) struct EmptyResponse {} + +pub(crate) fn serialize_param(value: &T) -> Result { + Ok(serde_json::to_string(value)?) +} diff --git a/booster_sdk/src/client/vision_client.rs b/booster_sdk/src/client/vision_client.rs new file mode 100644 index 0000000..6803e80 --- /dev/null +++ b/booster_sdk/src/client/vision_client.rs @@ -0,0 +1,151 @@ +//! Vision service RPC client. + +use serde::{Deserialize, Serialize, de::DeserializeOwned}; +use serde_json::Value; + +use crate::dds::{RpcClient, RpcClientOptions, VISION_API_TOPIC}; +use crate::types::Result; + +use super::util::{EmptyResponse, serialize_param}; + +#[derive(Debug, Clone, Copy, PartialEq, Eq, Hash, Serialize, Deserialize)] +#[serde(into = "i32", try_from = "i32")] +#[repr(i32)] +pub enum VisionApiId { + StartVisionService = 3000, + StopVisionService = 3001, + GetDetectionObject = 3002, +} + +impl From for i32 { + fn from(value: VisionApiId) -> Self { + value as i32 + } +} + +impl TryFrom for VisionApiId { + type Error = &'static str; + + fn try_from(value: i32) -> std::result::Result { + match value { + 3000 => Ok(Self::StartVisionService), + 3001 => Ok(Self::StopVisionService), + 3002 => Ok(Self::GetDetectionObject), + _ => Err("invalid value"), + } + } +} + +#[derive(Debug, Clone, Copy, PartialEq, Eq, Serialize, Deserialize)] +pub struct StartVisionServiceParameter { + pub enable_position: bool, + pub enable_color: bool, + pub enable_face_detection: bool, +} + +#[derive(Debug, Clone, Copy, PartialEq, Serialize, Deserialize)] +pub struct GetDetectionObjectParameter { + pub focus_ratio: f32, +} + +impl Default for GetDetectionObjectParameter { + fn default() -> Self { + Self { focus_ratio: 0.33 } + } +} + +#[derive(Debug, Clone, PartialEq, Serialize, Deserialize)] +pub struct DetectResults { + pub xmin: i64, + pub ymin: i64, + pub xmax: i64, + pub ymax: i64, + pub position: Vec, + pub tag: String, + pub conf: f32, + pub rgb_mean: Vec, +} + +/// High-level RPC client for vision inference APIs. +pub struct VisionClient { + rpc: RpcClient, +} + +impl VisionClient { + pub fn new() -> Result { + Self::with_options(RpcClientOptions::for_service(VISION_API_TOPIC)) + } + + pub fn with_options(options: RpcClientOptions) -> Result { + let rpc = RpcClient::new(options.with_service_topic(VISION_API_TOPIC))?; + Ok(Self { rpc }) + } + + pub async fn send_api_request(&self, api_id: VisionApiId, param: &str) -> Result<()> { + self.rpc + .call_with_body::(i32::from(api_id), param.to_owned(), None) + .await?; + Ok(()) + } + + pub async fn send_api_request_with_response( + &self, + api_id: VisionApiId, + param: &str, + ) -> Result + where + R: DeserializeOwned + Send + 'static, + { + self.rpc + .call_with_body(i32::from(api_id), param.to_owned(), None) + .await + } + + pub async fn start_vision_service( + &self, + enable_position: bool, + enable_color: bool, + enable_face_detection: bool, + ) -> Result<()> { + let param = StartVisionServiceParameter { + enable_position, + enable_color, + enable_face_detection, + }; + self.send_api_request(VisionApiId::StartVisionService, &serialize_param(¶m)?) + .await + } + + pub async fn stop_vision_service(&self) -> Result<()> { + self.send_api_request(VisionApiId::StopVisionService, "{}") + .await + } + + pub async fn get_detection_object_with_ratio( + &self, + focus_ratio: f32, + ) -> Result> { + let param = GetDetectionObjectParameter { focus_ratio }; + let value: Value = self + .send_api_request_with_response( + VisionApiId::GetDetectionObject, + &serialize_param(¶m)?, + ) + .await?; + + if value.is_array() { + return Ok(serde_json::from_value(value)?); + } + + if let Some(objects) = value.get("objects") { + return Ok(serde_json::from_value(objects.clone())?); + } + + Ok(Vec::new()) + } + + pub async fn get_detection_object(&self) -> Result> { + self.get_detection_object_with_ratio(GetDetectionObjectParameter::default().focus_ratio) + .await + } +} diff --git a/booster_sdk/src/client/x5_camera_client.rs b/booster_sdk/src/client/x5_camera_client.rs new file mode 100644 index 0000000..709cd43 --- /dev/null +++ b/booster_sdk/src/client/x5_camera_client.rs @@ -0,0 +1,160 @@ +//! X5 camera control RPC client. + +use serde::{Deserialize, Serialize, de::DeserializeOwned}; + +use crate::dds::{RpcClient, RpcClientOptions, X5_CAMERA_CONTROL_API_TOPIC}; +use crate::types::Result; + +use super::util::{EmptyResponse, serialize_param}; + +#[derive(Debug, Clone, Copy, PartialEq, Eq, Hash, Serialize, Deserialize)] +#[serde(into = "i32", try_from = "i32")] +#[repr(i32)] +pub enum X5CameraApiId { + ChangeMode = 5001, + GetStatus = 5002, +} + +impl From for i32 { + fn from(value: X5CameraApiId) -> Self { + value as i32 + } +} + +impl TryFrom for X5CameraApiId { + type Error = &'static str; + + fn try_from(value: i32) -> std::result::Result { + match value { + 5001 => Ok(Self::ChangeMode), + 5002 => Ok(Self::GetStatus), + _ => Err("invalid value"), + } + } +} + +#[derive(Debug, Clone, Copy, PartialEq, Eq, Hash, Serialize, Deserialize)] +#[serde(into = "i32", try_from = "i32")] +#[repr(i32)] +pub enum CameraSetMode { + CameraModeNormal = 0, + CameraModeHighResolution = 1, + CameraModeNormalEnable = 2, + CameraModeHighResolutionEnable = 3, +} + +impl From for i32 { + fn from(value: CameraSetMode) -> Self { + value as i32 + } +} + +impl TryFrom for CameraSetMode { + type Error = &'static str; + + fn try_from(value: i32) -> std::result::Result { + match value { + 0 => Ok(Self::CameraModeNormal), + 1 => Ok(Self::CameraModeHighResolution), + 2 => Ok(Self::CameraModeNormalEnable), + 3 => Ok(Self::CameraModeHighResolutionEnable), + _ => Err("invalid value"), + } + } +} + +#[derive(Debug, Clone, Copy, PartialEq, Eq, Hash, Serialize, Deserialize)] +#[serde(into = "i32", try_from = "i32")] +#[repr(i32)] +pub enum CameraControlStatus { + CameraStatusNormal = 0, + CameraStatusHighResolution = 1, + CameraStatusError = 2, + CameraStatusNull = 3, +} + +impl From for i32 { + fn from(value: CameraControlStatus) -> Self { + value as i32 + } +} + +impl TryFrom for CameraControlStatus { + type Error = &'static str; + + fn try_from(value: i32) -> std::result::Result { + match value { + 0 => Ok(Self::CameraStatusNormal), + 1 => Ok(Self::CameraStatusHighResolution), + 2 => Ok(Self::CameraStatusError), + 3 => Ok(Self::CameraStatusNull), + _ => Err("invalid value"), + } + } +} + +#[derive(Debug, Clone, Copy, PartialEq, Eq, Serialize, Deserialize)] +pub struct ChangeModeParameter { + pub mode: i32, +} + +#[derive(Debug, Clone, Copy, PartialEq, Eq, Serialize, Deserialize)] +pub struct GetStatusResponse { + pub status: i32, +} + +impl GetStatusResponse { + #[must_use] + pub fn status_enum(&self) -> Option { + CameraControlStatus::try_from(self.status).ok() + } +} + +/// High-level RPC client for X5 camera control APIs. +pub struct X5CameraClient { + rpc: RpcClient, +} + +impl X5CameraClient { + pub fn new() -> Result { + Self::with_options(RpcClientOptions::for_service(X5_CAMERA_CONTROL_API_TOPIC)) + } + + pub fn with_options(options: RpcClientOptions) -> Result { + let rpc = RpcClient::new(options.with_service_topic(X5_CAMERA_CONTROL_API_TOPIC))?; + Ok(Self { rpc }) + } + + pub async fn send_api_request(&self, api_id: X5CameraApiId, param: &str) -> Result<()> { + self.rpc + .call_with_body::(i32::from(api_id), param.to_owned(), None) + .await?; + Ok(()) + } + + pub async fn send_api_request_with_response( + &self, + api_id: X5CameraApiId, + param: &str, + ) -> Result + where + R: DeserializeOwned + Send + 'static, + { + self.rpc + .call_with_body(i32::from(api_id), param.to_owned(), None) + .await + } + + pub async fn change_mode(&self, mode: CameraSetMode) -> Result<()> { + let param = ChangeModeParameter { + mode: i32::from(mode), + }; + self.send_api_request(X5CameraApiId::ChangeMode, &serialize_param(¶m)?) + .await + } + + pub async fn get_status(&self) -> Result { + self.send_api_request_with_response(X5CameraApiId::GetStatus, "") + .await + } +} diff --git a/booster_sdk/src/dds/messages.rs b/booster_sdk/src/dds/messages.rs index 5792a17..c48d8df 100644 --- a/booster_sdk/src/dds/messages.rs +++ b/booster_sdk/src/dds/messages.rs @@ -138,10 +138,16 @@ pub struct GripperControl { pub speed: i32, } +#[derive(Debug, Clone, Copy, PartialEq, Eq, Serialize, Deserialize)] +pub struct LightPixel { + pub r: u8, + pub g: u8, + pub b: u8, +} + #[derive(Debug, Clone, Serialize, Deserialize)] pub struct LightControlMsg { - /// Raw payload for light control (schema not documented in DDS reference). - pub data: Vec, + pub pixels: Vec, } #[derive(Debug, Clone, Serialize, Deserialize)] diff --git a/booster_sdk/src/dds/rpc.rs b/booster_sdk/src/dds/rpc.rs index 3f1fe73..b035fca 100644 --- a/booster_sdk/src/dds/rpc.rs +++ b/booster_sdk/src/dds/rpc.rs @@ -12,12 +12,13 @@ use crate::types::{DdsError, Result, RpcError}; use super::DdsNode; use super::messages::{RpcReqMsg, RpcRespMsg}; -use super::topics::{loco_request_topic, loco_response_topic}; +use super::topics::{LOCO_API_TOPIC, rpc_request_topic, rpc_response_topic}; #[derive(Debug)] pub struct RpcClientOptions { pub domain_id: u16, pub default_timeout: Duration, + pub service_topic: String, } impl Default for RpcClientOptions { @@ -27,10 +28,27 @@ impl Default for RpcClientOptions { // 5 s is a safe default for most commands. Mode changes are slow, // so change_mode passes its own longer timeout. default_timeout: Duration::from_secs(5), + service_topic: LOCO_API_TOPIC.to_owned(), } } } +impl RpcClientOptions { + #[must_use] + pub fn for_service(service_topic: impl Into) -> Self { + Self { + service_topic: service_topic.into(), + ..Self::default() + } + } + + #[must_use] + pub fn with_service_topic(mut self, service_topic: impl Into) -> Self { + self.service_topic = service_topic.into(); + self + } +} + pub struct RpcClient { node: DdsNode, request_writer: rustdds::no_key::DataWriter, @@ -64,14 +82,29 @@ where serde_json::from_str(trimmed) } +fn normalize_service_topic(service_topic: &str) -> String { + let trimmed = service_topic.trim(); + if trimmed.is_empty() { + return LOCO_API_TOPIC.to_owned(); + } + if let Some(base) = trimmed.strip_suffix("Req") { + return base.to_owned(); + } + if let Some(base) = trimmed.strip_suffix("Resp") { + return base.to_owned(); + } + trimmed.to_owned() +} + impl RpcClient { pub fn new(options: RpcClientOptions) -> Result { let node = DdsNode::new(super::DdsConfig { domain_id: options.domain_id, })?; - let request_topic = loco_request_topic(); - let response_topic = loco_response_topic(); + let service_topic = normalize_service_topic(&options.service_topic); + let request_topic = rpc_request_topic(&service_topic); + let response_topic = rpc_response_topic(&service_topic); let request_writer = node.publisher::(&request_topic)?; let response_reader = node.subscribe_reader::(&response_topic)?; @@ -92,12 +125,24 @@ impl RpcClient { P: Serialize, R: DeserializeOwned + Send + 'static, { - let request_id = Uuid::new_v4().to_string(); - let body = serde_json::to_string(params).map_err(|e| { RpcError::BadRequest(format!("Failed to serialize request parameters: {e}")) })?; + self.call_with_body(api_id, body, timeout).await + } + + pub async fn call_with_body( + &self, + api_id: i32, + body: impl Into, + timeout: Option, + ) -> Result + where + R: DeserializeOwned + Send + 'static, + { + let request_id = Uuid::new_v4().to_string(); + let body = body.into(); let header = serde_json::json!({ "api_id": api_id }).to_string(); let request = RpcReqMsg { diff --git a/booster_sdk/src/dds/topics.rs b/booster_sdk/src/dds/topics.rs index 87e9916..8c9785b 100644 --- a/booster_sdk/src/dds/topics.rs +++ b/booster_sdk/src/dds/topics.rs @@ -1,15 +1,14 @@ //! DDS topic specifications for Booster robot communication. -use rustdds::{Topic, TopicKind}; +use rustdds::{QosPolicies, Topic, TopicKind}; use crate::types::{DdsError, Result}; use super::qos::{qos_best_effort_keep_last, qos_reliable_keep_all, qos_reliable_keep_last}; -use rustdds::QosPolicies; #[derive(Debug, Clone)] pub struct TopicSpec { - pub name: &'static str, + pub name: String, pub type_name: &'static str, pub qos: QosPolicies, pub kind: TopicKind, @@ -19,7 +18,7 @@ impl TopicSpec { pub fn create_topic(&self, participant: &rustdds::DomainParticipant) -> Result { participant .create_topic( - self.name.to_string(), + self.name.clone(), self.type_name.to_string(), &self.qos, self.kind, @@ -40,28 +39,45 @@ pub const TYPE_BINARY_DATA: &str = "booster_msgs::msg::dds_::BinaryData_"; pub const TYPE_GRIPPER_CONTROL: &str = "booster_interface::msg::dds_::GripperControl_"; pub const TYPE_LIGHT_CONTROL: &str = "booster_interface::msg::dds_::LightControlMsg_"; pub const TYPE_SAFE_MODE: &str = "booster_msgs::msg::dds_::BinaryData_"; +pub const TYPE_SUBTITLE: &str = "booster_interface::msg::dds_::Subtitle_"; +pub const TYPE_ASR_CHUNK: &str = "booster_interface::msg::dds_::AsrChunk_"; -pub fn loco_request_topic() -> TopicSpec { +pub const LOCO_API_TOPIC: &str = "rt/LocoApiTopic"; +pub const AI_API_TOPIC: &str = "rt/AiApiTopic"; +pub const LUI_API_TOPIC: &str = "rt/LuiApiTopic"; +pub const LIGHT_CONTROL_API_TOPIC: &str = "rt/LightControlApiTopic"; +pub const VISION_API_TOPIC: &str = "rt/VisionApiTopic"; +pub const X5_CAMERA_CONTROL_API_TOPIC: &str = "rt/X5CameraControl"; + +pub fn rpc_request_topic(service_topic: &str) -> TopicSpec { TopicSpec { - name: "rt/LocoApiTopicReq", + name: format!("{service_topic}Req"), type_name: TYPE_RPC_REQ, qos: qos_reliable_keep_last(10), kind: TopicKind::NoKey, } } -pub fn loco_response_topic() -> TopicSpec { +pub fn rpc_response_topic(service_topic: &str) -> TopicSpec { TopicSpec { - name: "rt/LocoApiTopicResp", + name: format!("{service_topic}Resp"), type_name: TYPE_RPC_RESP, qos: qos_reliable_keep_last(10), kind: TopicKind::NoKey, } } +pub fn loco_request_topic() -> TopicSpec { + rpc_request_topic(LOCO_API_TOPIC) +} + +pub fn loco_response_topic() -> TopicSpec { + rpc_response_topic(LOCO_API_TOPIC) +} + pub fn device_gateway_topic() -> TopicSpec { TopicSpec { - name: "rt/device_gateway", + name: "rt/device_gateway".to_owned(), type_name: TYPE_ROBOT_STATUS, qos: qos_best_effort_keep_last(1), kind: TopicKind::NoKey, @@ -70,7 +86,7 @@ pub fn device_gateway_topic() -> TopicSpec { pub fn motion_state_topic() -> TopicSpec { TopicSpec { - name: "rt/motion_state", + name: "rt/motion_state".to_owned(), type_name: TYPE_MOTION_STATE, qos: qos_best_effort_keep_last(1), kind: TopicKind::NoKey, @@ -79,7 +95,7 @@ pub fn motion_state_topic() -> TopicSpec { pub fn battery_state_topic() -> TopicSpec { TopicSpec { - name: "rt/battery_state", + name: "rt/battery_state".to_owned(), type_name: TYPE_BATTERY_STATE, qos: qos_reliable_keep_last(1), kind: TopicKind::NoKey, @@ -88,7 +104,7 @@ pub fn battery_state_topic() -> TopicSpec { pub fn button_event_topic() -> TopicSpec { TopicSpec { - name: "rt/button_event", + name: "rt/button_event".to_owned(), type_name: TYPE_BUTTON_EVENT, qos: qos_reliable_keep_all(), kind: TopicKind::NoKey, @@ -97,7 +113,7 @@ pub fn button_event_topic() -> TopicSpec { pub fn remote_controller_topic() -> TopicSpec { TopicSpec { - name: "rt/remote_controller_state", + name: "rt/remote_controller_state".to_owned(), type_name: TYPE_REMOTE_CONTROLLER, qos: qos_best_effort_keep_last(1), kind: TopicKind::NoKey, @@ -106,7 +122,7 @@ pub fn remote_controller_topic() -> TopicSpec { pub fn process_state_topic() -> TopicSpec { TopicSpec { - name: "rt/booster_process_state", + name: "rt/booster_process_state".to_owned(), type_name: TYPE_PROCESS_STATE, qos: qos_reliable_keep_last(1), kind: TopicKind::NoKey, @@ -115,7 +131,7 @@ pub fn process_state_topic() -> TopicSpec { pub fn video_stream_topic() -> TopicSpec { TopicSpec { - name: "rt/booster/video_stream", + name: "rt/booster/video_stream".to_owned(), type_name: TYPE_BINARY_DATA, qos: qos_best_effort_keep_last(1), kind: TopicKind::NoKey, @@ -124,7 +140,7 @@ pub fn video_stream_topic() -> TopicSpec { pub fn gripper_control_topic() -> TopicSpec { TopicSpec { - name: "rt/gripper_control", + name: "rt/gripper_control".to_owned(), type_name: TYPE_GRIPPER_CONTROL, qos: qos_reliable_keep_last(10), kind: TopicKind::NoKey, @@ -133,7 +149,7 @@ pub fn gripper_control_topic() -> TopicSpec { pub fn light_control_topic() -> TopicSpec { TopicSpec { - name: "rt/light_control", + name: "rt/light_control".to_owned(), type_name: TYPE_LIGHT_CONTROL, qos: qos_reliable_keep_last(10), kind: TopicKind::NoKey, @@ -142,9 +158,27 @@ pub fn light_control_topic() -> TopicSpec { pub fn safe_mode_topic() -> TopicSpec { TopicSpec { - name: "rt/enter_safe_mode", + name: "rt/enter_safe_mode".to_owned(), type_name: TYPE_SAFE_MODE, qos: qos_reliable_keep_all(), kind: TopicKind::NoKey, } } + +pub fn ai_subtitle_topic() -> TopicSpec { + TopicSpec { + name: "rt/ai_subtitle".to_owned(), + type_name: TYPE_SUBTITLE, + qos: qos_reliable_keep_last(16), + kind: TopicKind::NoKey, + } +} + +pub fn lui_asr_chunk_topic() -> TopicSpec { + TopicSpec { + name: "rt/lui_asr_chunk".to_owned(), + type_name: TYPE_ASR_CHUNK, + qos: qos_reliable_keep_last(16), + kind: TopicKind::NoKey, + } +} diff --git a/booster_sdk/src/types/b1.rs b/booster_sdk/src/types/b1.rs new file mode 100644 index 0000000..9f15def --- /dev/null +++ b/booster_sdk/src/types/b1.rs @@ -0,0 +1,543 @@ +//! B1 locomotion, kinematics, and high-level API types. + +use serde::{Deserialize, Serialize}; + +use super::{Hand, RobotMode}; + +#[derive(Debug, Clone, Copy, PartialEq, Eq, Hash, Serialize, Deserialize)] +#[serde(into = "i32", try_from = "i32")] +#[repr(i32)] +pub enum LocoApiId { + ChangeMode = 2000, + Move = 2001, + RotateHead = 2004, + WaveHand = 2005, + RotateHeadWithDirection = 2006, + LieDown = 2007, + GetUp = 2008, + MoveHandEndEffector = 2009, + ControlGripper = 2010, + GetFrameTransform = 2011, + SwitchHandEndEffectorControlMode = 2012, + ControlDexterousHand = 2013, + Handshake = 2015, + Dance = 2016, + GetMode = 2017, + GetStatus = 2018, + PushUp = 2019, + PlaySound = 2020, + StopSound = 2021, + GetRobotInfo = 2022, + StopHandEndEffector = 2023, + Shoot = 2024, + GetUpWithMode = 2025, + ZeroTorqueDrag = 2026, + RecordTrajectory = 2027, + ReplayTrajectory = 2028, + WholeBodyDance = 2029, + UpperBodyCustomControl = 2030, + ResetOdometry = 2031, + LoadCustomTrainedTraj = 2032, + ActivateCustomTrainedTraj = 2033, + UnloadCustomTrainedTraj = 2034, + EnterWbcGait = 2035, + ExitWbcGait = 2036, +} + +impl From for i32 { + fn from(value: LocoApiId) -> Self { + value as i32 + } +} + +impl TryFrom for LocoApiId { + type Error = &'static str; + + fn try_from(value: i32) -> Result { + match value { + 2000 => Ok(Self::ChangeMode), + 2001 => Ok(Self::Move), + 2004 => Ok(Self::RotateHead), + 2005 => Ok(Self::WaveHand), + 2006 => Ok(Self::RotateHeadWithDirection), + 2007 => Ok(Self::LieDown), + 2008 => Ok(Self::GetUp), + 2009 => Ok(Self::MoveHandEndEffector), + 2010 => Ok(Self::ControlGripper), + 2011 => Ok(Self::GetFrameTransform), + 2012 => Ok(Self::SwitchHandEndEffectorControlMode), + 2013 => Ok(Self::ControlDexterousHand), + 2015 => Ok(Self::Handshake), + 2016 => Ok(Self::Dance), + 2017 => Ok(Self::GetMode), + 2018 => Ok(Self::GetStatus), + 2019 => Ok(Self::PushUp), + 2020 => Ok(Self::PlaySound), + 2021 => Ok(Self::StopSound), + 2022 => Ok(Self::GetRobotInfo), + 2023 => Ok(Self::StopHandEndEffector), + 2024 => Ok(Self::Shoot), + 2025 => Ok(Self::GetUpWithMode), + 2026 => Ok(Self::ZeroTorqueDrag), + 2027 => Ok(Self::RecordTrajectory), + 2028 => Ok(Self::ReplayTrajectory), + 2029 => Ok(Self::WholeBodyDance), + 2030 => Ok(Self::UpperBodyCustomControl), + 2031 => Ok(Self::ResetOdometry), + 2032 => Ok(Self::LoadCustomTrainedTraj), + 2033 => Ok(Self::ActivateCustomTrainedTraj), + 2034 => Ok(Self::UnloadCustomTrainedTraj), + 2035 => Ok(Self::EnterWbcGait), + 2036 => Ok(Self::ExitWbcGait), + _ => Err("invalid value"), + } + } +} + +#[derive(Debug, Clone, Copy, PartialEq, Eq, Hash, Serialize, Deserialize)] +#[serde(into = "i32", try_from = "i32")] +#[repr(i32)] +pub enum BodyControl { + Unknown = 0, + Damping = 1, + Prepare = 2, + HumanlikeGait = 3, + ProneBody = 4, + SoccerGait = 5, + Custom = 6, + GetUp = 7, + WholeBodyDance = 8, + Shoot = 9, + InsideFoot = 10, + Goalie = 11, + WbcGait = 12, +} + +impl From for i32 { + fn from(value: BodyControl) -> Self { + value as i32 + } +} + +impl TryFrom for BodyControl { + type Error = &'static str; + + fn try_from(value: i32) -> Result { + match value { + 0 => Ok(Self::Unknown), + 1 => Ok(Self::Damping), + 2 => Ok(Self::Prepare), + 3 => Ok(Self::HumanlikeGait), + 4 => Ok(Self::ProneBody), + 5 => Ok(Self::SoccerGait), + 6 => Ok(Self::Custom), + 7 => Ok(Self::GetUp), + 8 => Ok(Self::WholeBodyDance), + 9 => Ok(Self::Shoot), + 10 => Ok(Self::InsideFoot), + 11 => Ok(Self::Goalie), + 12 => Ok(Self::WbcGait), + _ => Err("invalid value"), + } + } +} + +#[derive(Debug, Clone, Copy, PartialEq, Eq, Hash, Serialize, Deserialize)] +#[serde(into = "i32", try_from = "i32")] +#[repr(i32)] +pub enum Action { + Unknown = 0, + HandShake = 1, + HandWave = 2, + HandControl = 3, + DanceNewYear = 4, + DanceNezha = 5, + DanceTowardsFuture = 6, + GestureDabbing = 7, + GestureUltraman = 8, + GestureRespect = 9, + GestureCheer = 10, + GestureLuckyCat = 11, + GestureBoxing = 12, + ZeroTorqueDrag = 13, + RecordTraj = 14, + RunRecordedTraj = 15, +} + +impl From for i32 { + fn from(value: Action) -> Self { + value as i32 + } +} + +impl TryFrom for Action { + type Error = &'static str; + + fn try_from(value: i32) -> Result { + match value { + 0 => Ok(Self::Unknown), + 1 => Ok(Self::HandShake), + 2 => Ok(Self::HandWave), + 3 => Ok(Self::HandControl), + 4 => Ok(Self::DanceNewYear), + 5 => Ok(Self::DanceNezha), + 6 => Ok(Self::DanceTowardsFuture), + 7 => Ok(Self::GestureDabbing), + 8 => Ok(Self::GestureUltraman), + 9 => Ok(Self::GestureRespect), + 10 => Ok(Self::GestureCheer), + 11 => Ok(Self::GestureLuckyCat), + 12 => Ok(Self::GestureBoxing), + 13 => Ok(Self::ZeroTorqueDrag), + 14 => Ok(Self::RecordTraj), + 15 => Ok(Self::RunRecordedTraj), + _ => Err("invalid value"), + } + } +} + +#[derive(Debug, Clone, Copy, PartialEq, Eq, Hash, Serialize, Deserialize)] +#[serde(into = "i32", try_from = "i32")] +#[repr(i32)] +pub enum Frame { + Unknown = -1, + Body = 0, + Head = 1, + LeftHand = 2, + RightHand = 3, + LeftFoot = 4, + RightFoot = 5, +} + +impl From for i32 { + fn from(value: Frame) -> Self { + value as i32 + } +} + +impl TryFrom for Frame { + type Error = &'static str; + + fn try_from(value: i32) -> Result { + match value { + -1 => Ok(Self::Unknown), + 0 => Ok(Self::Body), + 1 => Ok(Self::Head), + 2 => Ok(Self::LeftHand), + 3 => Ok(Self::RightHand), + 4 => Ok(Self::LeftFoot), + 5 => Ok(Self::RightFoot), + _ => Err("invalid value"), + } + } +} + +#[derive(Debug, Clone, Copy, PartialEq, Eq, Hash, Serialize, Deserialize)] +#[serde(into = "i32", try_from = "i32")] +#[repr(i32)] +pub enum HandAction { + Open = 0, + Close = 1, +} + +impl From for i32 { + fn from(value: HandAction) -> Self { + value as i32 + } +} + +impl TryFrom for HandAction { + type Error = &'static str; + + fn try_from(value: i32) -> Result { + match value { + 0 => Ok(Self::Open), + 1 => Ok(Self::Close), + _ => Err("invalid value"), + } + } +} + +#[derive(Debug, Clone, Copy, PartialEq, Eq, Hash, Serialize, Deserialize)] +#[serde(into = "i32", try_from = "i32")] +#[repr(i32)] +pub enum BoosterHandType { + InspireHand = 0, + InspireTouchHand = 2, + RevoHand = 3, + Unknown = -1, +} + +impl From for i32 { + fn from(value: BoosterHandType) -> Self { + value as i32 + } +} + +impl TryFrom for BoosterHandType { + type Error = &'static str; + + fn try_from(value: i32) -> Result { + match value { + 0 => Ok(Self::InspireHand), + 2 => Ok(Self::InspireTouchHand), + 3 => Ok(Self::RevoHand), + -1 => Ok(Self::Unknown), + _ => Err("invalid value"), + } + } +} + +#[derive(Debug, Clone, Copy, PartialEq, Eq, Hash, Serialize, Deserialize)] +#[serde(into = "i32", try_from = "i32")] +#[repr(i32)] +pub enum DanceId { + NewYear = 0, + Nezha = 1, + TowardsFuture = 2, + DabbingGesture = 3, + UltramanGesture = 4, + RespectGesture = 5, + CheeringGesture = 6, + LuckyCatGesture = 7, + Stop = 1000, +} + +impl From for i32 { + fn from(value: DanceId) -> Self { + value as i32 + } +} + +impl TryFrom for DanceId { + type Error = &'static str; + + fn try_from(value: i32) -> Result { + match value { + 0 => Ok(Self::NewYear), + 1 => Ok(Self::Nezha), + 2 => Ok(Self::TowardsFuture), + 3 => Ok(Self::DabbingGesture), + 4 => Ok(Self::UltramanGesture), + 5 => Ok(Self::RespectGesture), + 6 => Ok(Self::CheeringGesture), + 7 => Ok(Self::LuckyCatGesture), + 1000 => Ok(Self::Stop), + _ => Err("invalid value"), + } + } +} + +#[derive(Debug, Clone, Copy, PartialEq, Eq, Hash, Serialize, Deserialize)] +#[serde(into = "i32", try_from = "i32")] +#[repr(i32)] +pub enum WholeBodyDanceId { + ArbicDance = 0, + MichaelDance1 = 1, + MichaelDance2 = 2, + MichaelDance3 = 3, + MoonWalk = 4, + BoxingStyleKick = 5, + RoundhouseKick = 6, +} + +impl From for i32 { + fn from(value: WholeBodyDanceId) -> Self { + value as i32 + } +} + +impl TryFrom for WholeBodyDanceId { + type Error = &'static str; + + fn try_from(value: i32) -> Result { + match value { + 0 => Ok(Self::ArbicDance), + 1 => Ok(Self::MichaelDance1), + 2 => Ok(Self::MichaelDance2), + 3 => Ok(Self::MichaelDance3), + 4 => Ok(Self::MoonWalk), + 5 => Ok(Self::BoxingStyleKick), + 6 => Ok(Self::RoundhouseKick), + _ => Err("invalid value"), + } + } +} + +#[derive(Debug, Clone, Copy, PartialEq, Eq, Hash, Serialize, Deserialize)] +#[serde(into = "i32", try_from = "i32")] +#[repr(i32)] +pub enum JointOrder { + MuJoCo = 0, + IsaacLab = 1, +} + +impl From for i32 { + fn from(value: JointOrder) -> Self { + value as i32 + } +} + +impl TryFrom for JointOrder { + type Error = &'static str; + + fn try_from(value: i32) -> Result { + match value { + 0 => Ok(Self::MuJoCo), + 1 => Ok(Self::IsaacLab), + _ => Err("invalid value"), + } + } +} + +#[derive(Debug, Clone, Copy, PartialEq, Eq, Hash, Serialize, Deserialize)] +#[serde(into = "i32", try_from = "i32")] +#[repr(i32)] +pub enum GripperControlMode { + Position = 0, + Force = 1, +} + +impl From for i32 { + fn from(value: GripperControlMode) -> Self { + value as i32 + } +} + +impl TryFrom for GripperControlMode { + type Error = &'static str; + + fn try_from(value: i32) -> Result { + match value { + 0 => Ok(Self::Position), + 1 => Ok(Self::Force), + _ => Err("invalid value"), + } + } +} + +#[derive(Debug, Clone, Copy, PartialEq, Serialize, Deserialize)] +pub struct Position { + pub x: f32, + pub y: f32, + pub z: f32, +} + +#[derive(Debug, Clone, Copy, PartialEq, Serialize, Deserialize)] +pub struct Orientation { + pub roll: f32, + pub pitch: f32, + pub yaw: f32, +} + +#[derive(Debug, Clone, Copy, PartialEq, Serialize, Deserialize)] +pub struct Posture { + pub position: Position, + pub orientation: Orientation, +} + +#[derive(Debug, Clone, Copy, PartialEq, Serialize, Deserialize)] +pub struct Quaternion { + pub x: f32, + pub y: f32, + pub z: f32, + pub w: f32, +} + +#[derive(Debug, Clone, Copy, PartialEq, Serialize, Deserialize)] +pub struct Transform { + pub position: Position, + pub orientation: Quaternion, +} + +#[derive(Debug, Clone, Copy, PartialEq, Eq, Serialize, Deserialize)] +pub struct GripperMotionParameter { + pub position: i32, + pub force: i32, + pub speed: i32, +} + +#[derive(Debug, Clone, Copy, PartialEq, Eq, Serialize, Deserialize)] +pub struct DexterousFingerParameter { + pub seq: i32, + pub angle: i32, + pub force: i32, + pub speed: i32, +} + +#[derive(Debug, Clone, PartialEq, Serialize, Deserialize)] +pub struct GetModeResponse { + pub mode: i32, +} + +impl GetModeResponse { + #[must_use] + pub fn mode_enum(&self) -> Option { + RobotMode::try_from(self.mode).ok() + } +} + +#[derive(Debug, Clone, PartialEq, Serialize, Deserialize)] +pub struct GetStatusResponse { + pub current_mode: i32, + pub current_body_control: i32, + pub current_actions: Vec, +} + +impl GetStatusResponse { + #[must_use] + pub fn current_mode_enum(&self) -> Option { + RobotMode::try_from(self.current_mode).ok() + } + + #[must_use] + pub fn current_body_control_enum(&self) -> Option { + BodyControl::try_from(self.current_body_control).ok() + } + + #[must_use] + pub fn current_actions_enum(&self) -> Vec { + self.current_actions + .iter() + .copied() + .filter_map(|value| Action::try_from(value).ok()) + .collect() + } +} + +#[derive(Debug, Clone, PartialEq, Eq, Serialize, Deserialize)] +pub struct GetRobotInfoResponse { + pub name: String, + pub nickname: String, + pub version: String, + pub model: String, + pub serial_number: String, +} + +#[derive(Debug, Clone, PartialEq, Serialize, Deserialize)] +pub struct CustomModelParams { + pub action_scale: Vec, + pub kp: Vec, + pub kd: Vec, +} + +#[derive(Debug, Clone, PartialEq, Serialize, Deserialize)] +pub struct CustomModel { + pub file_path: String, + pub params: Vec, + pub joint_order: JointOrder, +} + +#[derive(Debug, Clone, PartialEq, Serialize, Deserialize)] +pub struct CustomTrainedTraj { + pub traj_file_path: String, + pub model: CustomModel, +} + +#[derive(Debug, Clone, PartialEq, Eq, Serialize, Deserialize)] +pub struct LoadCustomTrainedTrajResponse { + pub tid: String, +} + +/// Convenience alias matching the C++ naming. +pub type HandIndex = Hand; diff --git a/booster_sdk/src/types/error.rs b/booster_sdk/src/types/error.rs index 709bbeb..e91bc93 100644 --- a/booster_sdk/src/types/error.rs +++ b/booster_sdk/src/types/error.rs @@ -72,6 +72,12 @@ pub enum RpcError { #[error("Server refused request: {0}")] ServerRefused(String), + #[error("Request conflicts with current robot state: {0}")] + Conflict(String), + + #[error("Request rejected because it is too frequent: {0}")] + RequestTooFrequent(String), + #[error("State transition failed: {0}")] StateTransitionFailed(String), @@ -92,6 +98,8 @@ impl RpcError { timeout: Duration::ZERO, }, 400 => RpcError::BadRequest(message), + 409 => RpcError::Conflict(message), + 429 => RpcError::RequestTooFrequent(message), 500 => RpcError::InternalServerError(message), 501 => RpcError::ServerRefused(message), 502 => RpcError::StateTransitionFailed(message), diff --git a/booster_sdk/src/types/mod.rs b/booster_sdk/src/types/mod.rs index c347248..fe57d19 100644 --- a/booster_sdk/src/types/mod.rs +++ b/booster_sdk/src/types/mod.rs @@ -1,7 +1,9 @@ //! Core domain types shared across the Booster Robotics SDK. +mod b1; mod error; mod robot; +pub use b1::*; pub use error::*; pub use robot::*; diff --git a/booster_sdk/src/types/robot.rs b/booster_sdk/src/types/robot.rs index 45b5ba8..0caa7c7 100644 --- a/booster_sdk/src/types/robot.rs +++ b/booster_sdk/src/types/robot.rs @@ -4,9 +4,13 @@ use serde::{Deserialize, Serialize}; /// Robot operational mode #[derive(Debug, Clone, Copy, PartialEq, Eq, Hash, Serialize, Deserialize)] +#[serde(into = "i32", try_from = "i32")] #[repr(i32)] #[non_exhaustive] pub enum RobotMode { + /// Unknown mode, typically used for error handling. + Unknown = -1, + /// Damping mode, motors are compliant Damping = 0, @@ -24,16 +28,17 @@ pub enum RobotMode { } impl TryFrom for RobotMode { - type Error = (); + type Error = &'static str; fn try_from(value: i32) -> Result { match value { + -1 => Ok(RobotMode::Unknown), 0 => Ok(RobotMode::Damping), 1 => Ok(RobotMode::Prepare), 2 => Ok(RobotMode::Walking), 3 => Ok(RobotMode::Custom), 4 => Ok(RobotMode::Soccer), - _ => Err(()), + _ => Err("invalid value"), } } } @@ -102,6 +107,7 @@ impl TryFrom for Hand { /// Gripper control mode #[derive(Debug, Clone, Copy, PartialEq, Eq, Hash, Serialize, Deserialize)] +#[serde(into = "i32", try_from = "i32")] #[repr(i32)] pub enum GripperMode { /// Position-based control @@ -118,13 +124,13 @@ impl From for i32 { } impl TryFrom for GripperMode { - type Error = (); + type Error = &'static str; fn try_from(value: i32) -> Result { match value { 0 => Ok(GripperMode::Position), 1 => Ok(GripperMode::Force), - _ => Err(()), + _ => Err("invalid value"), } } } @@ -135,9 +141,10 @@ mod tests { #[test] fn test_robot_mode_conversion() { + assert_eq!(RobotMode::try_from(-1), Ok(RobotMode::Unknown)); assert_eq!(RobotMode::try_from(0), Ok(RobotMode::Damping)); assert_eq!(RobotMode::try_from(2), Ok(RobotMode::Walking)); - assert_eq!(RobotMode::try_from(99), Err(())); + assert_eq!(RobotMode::try_from(99), Err("invalid value")); assert_eq!(i32::from(RobotMode::Walking), 2); } @@ -146,6 +153,6 @@ mod tests { fn test_gripper_mode_conversion() { assert_eq!(GripperMode::try_from(0), Ok(GripperMode::Position)); assert_eq!(GripperMode::try_from(1), Ok(GripperMode::Force)); - assert_eq!(GripperMode::try_from(2), Err(())); + assert_eq!(GripperMode::try_from(2), Err("invalid value")); } } From d2d925d485efb0d88208dddc6d39ee3ae7664512 Mon Sep 17 00:00:00 2001 From: Gijs de Jong Date: Sun, 22 Feb 2026 19:29:13 +0100 Subject: [PATCH 02/15] Simplify client definitions --- booster_sdk/src/client/ai_client.rs | 84 +++------- .../src/client/light_control_client.rs | 35 +--- booster_sdk/src/client/loco_client.rs | 153 +++++++----------- booster_sdk/src/client/mod.rs | 1 - booster_sdk/src/client/util.rs | 12 -- booster_sdk/src/client/vision_client.rs | 38 +---- booster_sdk/src/client/x5_camera_client.rs | 32 +--- booster_sdk/src/dds/rpc.rs | 60 ++++++- 8 files changed, 160 insertions(+), 255 deletions(-) delete mode 100644 booster_sdk/src/client/util.rs diff --git a/booster_sdk/src/client/ai_client.rs b/booster_sdk/src/client/ai_client.rs index 84d3b39..08c6490 100644 --- a/booster_sdk/src/client/ai_client.rs +++ b/booster_sdk/src/client/ai_client.rs @@ -1,6 +1,6 @@ //! AI and LUI high-level RPC clients. -use serde::{Deserialize, Serialize, de::DeserializeOwned}; +use serde::{Deserialize, Serialize}; use crate::dds::{ AI_API_TOPIC, DdsNode, DdsSubscription, LUI_API_TOPIC, RpcClient, RpcClientOptions, @@ -8,8 +8,6 @@ use crate::dds::{ }; use crate::types::Result; -use super::util::{EmptyResponse, serialize_param}; - #[derive(Debug, Clone, Copy, PartialEq, Eq, Hash, Serialize, Deserialize)] #[serde(into = "i32", try_from = "i32")] #[repr(i32)] @@ -141,7 +139,6 @@ pub const BOOSTER_ROBOT_USER_ID: &str = "BoosterRobot"; /// High-level RPC client for AI chat features. pub struct AiClient { rpc: RpcClient, - node: DdsNode, } impl AiClient { @@ -150,62 +147,42 @@ impl AiClient { } pub fn with_options(options: RpcClientOptions) -> Result { - let rpc = RpcClient::new(options.with_service_topic(AI_API_TOPIC))?; - let node = rpc.node().clone(); - Ok(Self { rpc, node }) + let rpc = RpcClient::for_topic(options, AI_API_TOPIC)?; + Ok(Self { rpc }) } pub fn node(&self) -> &DdsNode { - &self.node - } - - pub async fn send_api_request(&self, api_id: AiApiId, param: &str) -> Result<()> { - self.rpc - .call_with_body::(i32::from(api_id), param.to_owned(), None) - .await?; - Ok(()) - } - - pub async fn send_api_request_with_response(&self, api_id: AiApiId, param: &str) -> Result - where - R: DeserializeOwned + Send + 'static, - { - self.rpc - .call_with_body(i32::from(api_id), param.to_owned(), None) - .await + self.rpc.node() } pub async fn start_ai_chat(&self, param: &StartAiChatParameter) -> Result<()> { - self.send_api_request(AiApiId::StartAiChat, &serialize_param(param)?) - .await + self.rpc.call_serialized(AiApiId::StartAiChat, param).await } pub async fn stop_ai_chat(&self) -> Result<()> { - self.send_api_request(AiApiId::StopAiChat, "").await + self.rpc.call_void(AiApiId::StopAiChat, "").await } pub async fn speak(&self, param: &SpeakParameter) -> Result<()> { - self.send_api_request(AiApiId::Speak, &serialize_param(param)?) - .await + self.rpc.call_serialized(AiApiId::Speak, param).await } pub async fn start_face_tracking(&self) -> Result<()> { - self.send_api_request(AiApiId::StartFaceTracking, "").await + self.rpc.call_void(AiApiId::StartFaceTracking, "").await } pub async fn stop_face_tracking(&self) -> Result<()> { - self.send_api_request(AiApiId::StopFaceTracking, "").await + self.rpc.call_void(AiApiId::StopFaceTracking, "").await } pub fn subscribe_subtitle(&self) -> Result> { - self.node.subscribe(&ai_subtitle_topic(), 16) + self.rpc.node().subscribe(&ai_subtitle_topic(), 16) } } /// High-level RPC client for LUI ASR/TTS features. pub struct LuiClient { rpc: RpcClient, - node: DdsNode, } impl LuiClient { @@ -214,58 +191,35 @@ impl LuiClient { } pub fn with_options(options: RpcClientOptions) -> Result { - let rpc = RpcClient::new(options.with_service_topic(LUI_API_TOPIC))?; - let node = rpc.node().clone(); - Ok(Self { rpc, node }) + let rpc = RpcClient::for_topic(options, LUI_API_TOPIC)?; + Ok(Self { rpc }) } pub fn node(&self) -> &DdsNode { - &self.node - } - - pub async fn send_api_request(&self, api_id: LuiApiId, param: &str) -> Result<()> { - self.rpc - .call_with_body::(i32::from(api_id), param.to_owned(), None) - .await?; - Ok(()) - } - - pub async fn send_api_request_with_response( - &self, - api_id: LuiApiId, - param: &str, - ) -> Result - where - R: DeserializeOwned + Send + 'static, - { - self.rpc - .call_with_body(i32::from(api_id), param.to_owned(), None) - .await + self.rpc.node() } pub async fn start_asr(&self) -> Result<()> { - self.send_api_request(LuiApiId::StartAsr, "").await + self.rpc.call_void(LuiApiId::StartAsr, "").await } pub async fn stop_asr(&self) -> Result<()> { - self.send_api_request(LuiApiId::StopAsr, "").await + self.rpc.call_void(LuiApiId::StopAsr, "").await } pub async fn start_tts(&self, config: &LuiTtsConfig) -> Result<()> { - self.send_api_request(LuiApiId::StartTts, &serialize_param(config)?) - .await + self.rpc.call_serialized(LuiApiId::StartTts, config).await } pub async fn stop_tts(&self) -> Result<()> { - self.send_api_request(LuiApiId::StopTts, "").await + self.rpc.call_void(LuiApiId::StopTts, "").await } pub async fn send_tts_text(&self, param: &LuiTtsParameter) -> Result<()> { - self.send_api_request(LuiApiId::SendTtsText, &serialize_param(param)?) - .await + self.rpc.call_serialized(LuiApiId::SendTtsText, param).await } pub fn subscribe_asr_chunk(&self) -> Result> { - self.node.subscribe(&lui_asr_chunk_topic(), 16) + self.rpc.node().subscribe(&lui_asr_chunk_topic(), 16) } } diff --git a/booster_sdk/src/client/light_control_client.rs b/booster_sdk/src/client/light_control_client.rs index 23bddf3..94911e1 100644 --- a/booster_sdk/src/client/light_control_client.rs +++ b/booster_sdk/src/client/light_control_client.rs @@ -1,12 +1,10 @@ //! LED light control RPC client. -use serde::{Deserialize, Serialize, de::DeserializeOwned}; +use serde::{Deserialize, Serialize}; use crate::dds::{LIGHT_CONTROL_API_TOPIC, RpcClient, RpcClientOptions}; use crate::types::Result; -use super::util::{EmptyResponse, serialize_param}; - #[derive(Debug, Clone, Copy, PartialEq, Eq, Hash, Serialize, Deserialize)] #[serde(into = "i32", try_from = "i32")] #[repr(i32)] @@ -67,43 +65,26 @@ impl LightControlClient { } pub fn with_options(options: RpcClientOptions) -> Result { - let rpc = RpcClient::new(options.with_service_topic(LIGHT_CONTROL_API_TOPIC))?; + let rpc = RpcClient::for_topic(options, LIGHT_CONTROL_API_TOPIC)?; Ok(Self { rpc }) } - pub async fn send_api_request(&self, api_id: LightApiId, param: &str) -> Result<()> { - self.rpc - .call_with_body::(i32::from(api_id), param.to_owned(), None) - .await?; - Ok(()) - } - - pub async fn send_api_request_with_response( - &self, - api_id: LightApiId, - param: &str, - ) -> Result - where - R: DeserializeOwned + Send + 'static, - { - self.rpc - .call_with_body(i32::from(api_id), param.to_owned(), None) - .await - } - pub async fn set_led_light_color(&self, r: u8, g: u8, b: u8) -> Result<()> { let param = SetLedLightColorParameter { r, g, b }; - self.send_api_request(LightApiId::SetLedLightColor, &serialize_param(¶m)?) + self.rpc + .call_serialized(LightApiId::SetLedLightColor, ¶m) .await } pub async fn set_led_light_color_param(&self, param: &SetLedLightColorParameter) -> Result<()> { - self.send_api_request(LightApiId::SetLedLightColor, &serialize_param(param)?) + self.rpc + .call_serialized(LightApiId::SetLedLightColor, param) .await } pub async fn stop_led_light_control(&self) -> Result<()> { - self.send_api_request(LightApiId::StopLedLightControl, "") + self.rpc + .call_void(LightApiId::StopLedLightControl, "") .await } } diff --git a/booster_sdk/src/client/loco_client.rs b/booster_sdk/src/client/loco_client.rs index 7cbf31a..3b96c98 100644 --- a/booster_sdk/src/client/loco_client.rs +++ b/booster_sdk/src/client/loco_client.rs @@ -1,13 +1,12 @@ //! High-level B1 locomotion client built on DDS RPC and topic I/O. -use super::util::{EmptyResponse, serialize_param}; +use crate::dds::RpcClient; use crate::dds::{ BatteryState, BinaryData, ButtonEventMsg, DdsNode, DdsPublisher, DdsSubscription, GripperControl, LightControlMsg, MotionState, RemoteControllerState, RobotProcessStateMsg, - RobotStatusDdsMsg, RpcClient, RpcClientOptions, SafeMode, battery_state_topic, - button_event_topic, device_gateway_topic, gripper_control_topic, light_control_topic, - motion_state_topic, process_state_topic, remote_controller_topic, safe_mode_topic, - video_stream_topic, + RobotStatusDdsMsg, RpcClientOptions, SafeMode, battery_state_topic, button_event_topic, + device_gateway_topic, gripper_control_topic, light_control_topic, motion_state_topic, + process_state_topic, remote_controller_topic, safe_mode_topic, video_stream_topic, }; use crate::types::{ BoosterHandType, CustomTrainedTraj, DanceId, DexterousFingerParameter, Frame, GetModeResponse, @@ -15,7 +14,6 @@ use crate::types::{ HandAction, HandIndex, LoadCustomTrainedTrajResponse, LocoApiId, Result, RobotMode, Transform, WholeBodyDanceId, }; -use serde::Deserialize; use serde_json::json; // The controller may send an intermediate pending status (-1) before the // final success response. Mode transitions (especially PREPARE) can take @@ -25,7 +23,6 @@ const CHANGE_MODE_TIMEOUT: std::time::Duration = std::time::Duration::from_secs( /// High-level client for B1 locomotion control and telemetry. pub struct BoosterClient { rpc: RpcClient, - node: DdsNode, gripper_publisher: DdsPublisher, light_publisher: DdsPublisher, safe_mode_publisher: DdsPublisher, @@ -45,7 +42,6 @@ impl BoosterClient { Ok(Self { rpc, - node, gripper_publisher, light_publisher, safe_mode_publisher, @@ -53,64 +49,36 @@ impl BoosterClient { } pub fn node(&self) -> &DdsNode { - &self.node - } - - pub async fn send_api_request(&self, api_id: LocoApiId, param: &str) -> Result<()> { - self.rpc - .call_with_body::(i32::from(api_id), param.to_owned(), None) - .await?; - Ok(()) - } - - pub async fn send_api_request_with_response( - &self, - api_id: LocoApiId, - param: &str, - ) -> Result - where - R: for<'de> Deserialize<'de> + Send + 'static, - { - self.rpc - .call_with_body(i32::from(api_id), param.to_owned(), None) - .await + self.rpc.node() } pub async fn change_mode(&self, mode: RobotMode) -> Result<()> { let param = json!({ "mode": i32::from(mode) }).to_string(); self.rpc - .call_with_body::( - i32::from(LocoApiId::ChangeMode), - param, - Some(CHANGE_MODE_TIMEOUT), - ) - .await?; - Ok(()) + .call_void_with_timeout(LocoApiId::ChangeMode, param, Some(CHANGE_MODE_TIMEOUT)) + .await } pub async fn get_mode(&self) -> Result { - self.send_api_request_with_response(LocoApiId::GetMode, "") - .await + self.rpc.call_response(LocoApiId::GetMode, "").await } pub async fn get_status(&self) -> Result { - self.send_api_request_with_response(LocoApiId::GetStatus, "") - .await + self.rpc.call_response(LocoApiId::GetStatus, "").await } pub async fn get_robot_info(&self) -> Result { - self.send_api_request_with_response(LocoApiId::GetRobotInfo, "") - .await + self.rpc.call_response(LocoApiId::GetRobotInfo, "").await } pub async fn move_robot(&self, vx: f32, vy: f32, vyaw: f32) -> Result<()> { let param = json!({ "vx": vx, "vy": vy, "vyaw": vyaw }).to_string(); - self.send_api_request(LocoApiId::Move, ¶m).await + self.rpc.call_void(LocoApiId::Move, param).await } pub async fn rotate_head(&self, pitch: f32, yaw: f32) -> Result<()> { let param = json!({ "pitch": pitch, "yaw": yaw }).to_string(); - self.send_api_request(LocoApiId::RotateHead, ¶m).await + self.rpc.call_void(LocoApiId::RotateHead, param).await } pub async fn wave_hand(&self, action: HandAction) -> Result<()> { @@ -119,7 +87,7 @@ impl BoosterClient { "hand_action": i32::from(action), }) .to_string(); - self.send_api_request(LocoApiId::WaveHand, ¶m).await + self.rpc.call_void(LocoApiId::WaveHand, param).await } pub async fn rotate_head_with_direction( @@ -132,30 +100,30 @@ impl BoosterClient { "yaw_direction": yaw_direction, }) .to_string(); - self.send_api_request(LocoApiId::RotateHeadWithDirection, ¶m) + self.rpc + .call_void(LocoApiId::RotateHeadWithDirection, param) .await } pub async fn lie_down(&self) -> Result<()> { - self.send_api_request(LocoApiId::LieDown, "").await + self.rpc.call_void(LocoApiId::LieDown, "").await } pub async fn get_up(&self) -> Result<()> { - self.send_api_request(LocoApiId::GetUp, "").await + self.rpc.call_void(LocoApiId::GetUp, "").await } pub async fn get_up_with_mode(&self, mode: RobotMode) -> Result<()> { let param = json!({ "mode": i32::from(mode) }).to_string(); - self.send_api_request(LocoApiId::GetUpWithMode, ¶m) - .await + self.rpc.call_void(LocoApiId::GetUpWithMode, param).await } pub async fn shoot(&self) -> Result<()> { - self.send_api_request(LocoApiId::Shoot, "").await + self.rpc.call_void(LocoApiId::Shoot, "").await } pub async fn push_up(&self) -> Result<()> { - self.send_api_request(LocoApiId::PushUp, "").await + self.rpc.call_void(LocoApiId::PushUp, "").await } pub async fn move_hand_end_effector_with_aux( @@ -174,7 +142,8 @@ impl BoosterClient { "new_version": false, }) .to_string(); - self.send_api_request(LocoApiId::MoveHandEndEffector, ¶m) + self.rpc + .call_void(LocoApiId::MoveHandEndEffector, param) .await } @@ -192,7 +161,8 @@ impl BoosterClient { "new_version": false, }) .to_string(); - self.send_api_request(LocoApiId::MoveHandEndEffector, ¶m) + self.rpc + .call_void(LocoApiId::MoveHandEndEffector, param) .await } @@ -210,13 +180,13 @@ impl BoosterClient { "new_version": true, }) .to_string(); - self.send_api_request(LocoApiId::MoveHandEndEffector, ¶m) + self.rpc + .call_void(LocoApiId::MoveHandEndEffector, param) .await } pub async fn stop_hand_end_effector(&self) -> Result<()> { - self.send_api_request(LocoApiId::StopHandEndEffector, "") - .await + self.rpc.call_void(LocoApiId::StopHandEndEffector, "").await } pub async fn control_gripper( @@ -231,8 +201,7 @@ impl BoosterClient { "hand_index": i32::from(hand_index), }) .to_string(); - self.send_api_request(LocoApiId::ControlGripper, ¶m) - .await + self.rpc.call_void(LocoApiId::ControlGripper, param).await } pub async fn get_frame_transform(&self, src: Frame, dst: Frame) -> Result { @@ -241,19 +210,21 @@ impl BoosterClient { "dst": i32::from(dst), }) .to_string(); - self.send_api_request_with_response(LocoApiId::GetFrameTransform, ¶m) + self.rpc + .call_response(LocoApiId::GetFrameTransform, param) .await } pub async fn switch_hand_end_effector_control_mode(&self, switch_on: bool) -> Result<()> { let param = json!({ "switch_on": switch_on }).to_string(); - self.send_api_request(LocoApiId::SwitchHandEndEffectorControlMode, ¶m) + self.rpc + .call_void(LocoApiId::SwitchHandEndEffectorControlMode, param) .await } pub async fn handshake(&self, action: HandAction) -> Result<()> { let param = json!({ "hand_action": i32::from(action) }).to_string(); - self.send_api_request(LocoApiId::Handshake, ¶m).await + self.rpc.call_void(LocoApiId::Handshake, param).await } pub async fn control_dexterous_hand( @@ -268,7 +239,8 @@ impl BoosterClient { "hand_type": i32::from(hand_type), }) .to_string(); - self.send_api_request(LocoApiId::ControlDexterousHand, ¶m) + self.rpc + .call_void(LocoApiId::ControlDexterousHand, param) .await } @@ -283,81 +255,78 @@ impl BoosterClient { pub async fn dance(&self, dance_id: DanceId) -> Result<()> { let param = json!({ "dance_id": i32::from(dance_id) }).to_string(); - self.send_api_request(LocoApiId::Dance, ¶m).await + self.rpc.call_void(LocoApiId::Dance, param).await } pub async fn play_sound(&self, sound_file_path: impl Into) -> Result<()> { let param = json!({ "sound_file_path": sound_file_path.into() }).to_string(); - self.send_api_request(LocoApiId::PlaySound, ¶m).await + self.rpc.call_void(LocoApiId::PlaySound, param).await } pub async fn stop_sound(&self) -> Result<()> { - self.send_api_request(LocoApiId::StopSound, "").await + self.rpc.call_void(LocoApiId::StopSound, "").await } pub async fn zero_torque_drag(&self, active: bool) -> Result<()> { let param = json!({ "enable": active }).to_string(); - self.send_api_request(LocoApiId::ZeroTorqueDrag, ¶m) - .await + self.rpc.call_void(LocoApiId::ZeroTorqueDrag, param).await } pub async fn record_trajectory(&self, active: bool) -> Result<()> { let param = json!({ "enable": active }).to_string(); - self.send_api_request(LocoApiId::RecordTrajectory, ¶m) - .await + self.rpc.call_void(LocoApiId::RecordTrajectory, param).await } pub async fn replay_trajectory(&self, traj_file_path: impl Into) -> Result<()> { let param = json!({ "traj_file_path": traj_file_path.into() }).to_string(); - self.send_api_request(LocoApiId::ReplayTrajectory, ¶m) - .await + self.rpc.call_void(LocoApiId::ReplayTrajectory, param).await } pub async fn whole_body_dance(&self, dance_id: WholeBodyDanceId) -> Result<()> { let param = json!({ "dance_id": i32::from(dance_id) }).to_string(); - self.send_api_request(LocoApiId::WholeBodyDance, ¶m) - .await + self.rpc.call_void(LocoApiId::WholeBodyDance, param).await } pub async fn upper_body_custom_control(&self, start: bool) -> Result<()> { let param = json!({ "start": start }).to_string(); - self.send_api_request(LocoApiId::UpperBodyCustomControl, ¶m) + self.rpc + .call_void(LocoApiId::UpperBodyCustomControl, param) .await } pub async fn reset_odometry(&self) -> Result<()> { - self.send_api_request(LocoApiId::ResetOdometry, "").await + self.rpc.call_void(LocoApiId::ResetOdometry, "").await } pub async fn load_custom_trained_traj( &self, traj: &CustomTrainedTraj, ) -> Result { - self.send_api_request_with_response( - LocoApiId::LoadCustomTrainedTraj, - &serialize_param(traj)?, - ) - .await + self.rpc + .call_serialized_response(LocoApiId::LoadCustomTrainedTraj, traj) + .await } pub async fn activate_custom_trained_traj(&self, tid: impl Into) -> Result<()> { let param = json!({ "tid": tid.into() }).to_string(); - self.send_api_request(LocoApiId::ActivateCustomTrainedTraj, ¶m) + self.rpc + .call_void(LocoApiId::ActivateCustomTrainedTraj, param) .await } pub async fn unload_custom_trained_traj(&self, tid: impl Into) -> Result<()> { let param = json!({ "tid": tid.into() }).to_string(); - self.send_api_request(LocoApiId::UnloadCustomTrainedTraj, ¶m) + self.rpc + .call_void(LocoApiId::UnloadCustomTrainedTraj, param) .await } pub async fn enter_wbc_gait(&self) -> Result<()> { - self.send_api_request(LocoApiId::EnterWbcGait, "").await + self.rpc.call_void(LocoApiId::EnterWbcGait, "").await } pub async fn exit_wbc_gait(&self) -> Result<()> { - self.send_api_request(LocoApiId::ExitWbcGait, "").await + self.rpc.call_void(LocoApiId::ExitWbcGait, "").await } pub fn publish_gripper(&self, control: GripperControl) -> Result<()> { @@ -377,31 +346,31 @@ impl BoosterClient { } pub fn subscribe_device_gateway(&self) -> Result> { - self.node.subscribe(&device_gateway_topic(), 32) + self.rpc.node().subscribe(&device_gateway_topic(), 32) } pub fn subscribe_motion_state(&self) -> Result> { - self.node.subscribe(&motion_state_topic(), 16) + self.rpc.node().subscribe(&motion_state_topic(), 16) } pub fn subscribe_battery_state(&self) -> Result> { - self.node.subscribe(&battery_state_topic(), 8) + self.rpc.node().subscribe(&battery_state_topic(), 8) } pub fn subscribe_button_events(&self) -> Result> { - self.node.subscribe(&button_event_topic(), 32) + self.rpc.node().subscribe(&button_event_topic(), 32) } pub fn subscribe_remote_controller(&self) -> Result> { - self.node.subscribe(&remote_controller_topic(), 32) + self.rpc.node().subscribe(&remote_controller_topic(), 32) } pub fn subscribe_process_state(&self) -> Result> { - self.node.subscribe(&process_state_topic(), 8) + self.rpc.node().subscribe(&process_state_topic(), 8) } pub fn subscribe_video_stream(&self) -> Result> { - self.node.subscribe(&video_stream_topic(), 4) + self.rpc.node().subscribe(&video_stream_topic(), 4) } } diff --git a/booster_sdk/src/client/mod.rs b/booster_sdk/src/client/mod.rs index f880f29..29789b9 100644 --- a/booster_sdk/src/client/mod.rs +++ b/booster_sdk/src/client/mod.rs @@ -4,7 +4,6 @@ pub mod ai_client; pub mod commands; pub mod light_control_client; pub mod loco_client; -mod util; pub mod vision_client; pub mod x5_camera_client; diff --git a/booster_sdk/src/client/util.rs b/booster_sdk/src/client/util.rs deleted file mode 100644 index ea3e1d9..0000000 --- a/booster_sdk/src/client/util.rs +++ /dev/null @@ -1,12 +0,0 @@ -//! Internal client utilities shared across high-level client modules. - -use serde::Serialize; - -use crate::types::Result; - -#[derive(Debug, Clone, serde::Deserialize, Default)] -pub(crate) struct EmptyResponse {} - -pub(crate) fn serialize_param(value: &T) -> Result { - Ok(serde_json::to_string(value)?) -} diff --git a/booster_sdk/src/client/vision_client.rs b/booster_sdk/src/client/vision_client.rs index 6803e80..94d0f3b 100644 --- a/booster_sdk/src/client/vision_client.rs +++ b/booster_sdk/src/client/vision_client.rs @@ -1,13 +1,11 @@ //! Vision service RPC client. -use serde::{Deserialize, Serialize, de::DeserializeOwned}; +use serde::{Deserialize, Serialize}; use serde_json::Value; use crate::dds::{RpcClient, RpcClientOptions, VISION_API_TOPIC}; use crate::types::Result; -use super::util::{EmptyResponse, serialize_param}; - #[derive(Debug, Clone, Copy, PartialEq, Eq, Hash, Serialize, Deserialize)] #[serde(into = "i32", try_from = "i32")] #[repr(i32)] @@ -77,30 +75,10 @@ impl VisionClient { } pub fn with_options(options: RpcClientOptions) -> Result { - let rpc = RpcClient::new(options.with_service_topic(VISION_API_TOPIC))?; + let rpc = RpcClient::for_topic(options, VISION_API_TOPIC)?; Ok(Self { rpc }) } - pub async fn send_api_request(&self, api_id: VisionApiId, param: &str) -> Result<()> { - self.rpc - .call_with_body::(i32::from(api_id), param.to_owned(), None) - .await?; - Ok(()) - } - - pub async fn send_api_request_with_response( - &self, - api_id: VisionApiId, - param: &str, - ) -> Result - where - R: DeserializeOwned + Send + 'static, - { - self.rpc - .call_with_body(i32::from(api_id), param.to_owned(), None) - .await - } - pub async fn start_vision_service( &self, enable_position: bool, @@ -112,12 +90,14 @@ impl VisionClient { enable_color, enable_face_detection, }; - self.send_api_request(VisionApiId::StartVisionService, &serialize_param(¶m)?) + self.rpc + .call_serialized(VisionApiId::StartVisionService, ¶m) .await } pub async fn stop_vision_service(&self) -> Result<()> { - self.send_api_request(VisionApiId::StopVisionService, "{}") + self.rpc + .call_void(VisionApiId::StopVisionService, "{}") .await } @@ -127,10 +107,8 @@ impl VisionClient { ) -> Result> { let param = GetDetectionObjectParameter { focus_ratio }; let value: Value = self - .send_api_request_with_response( - VisionApiId::GetDetectionObject, - &serialize_param(¶m)?, - ) + .rpc + .call_serialized_response(VisionApiId::GetDetectionObject, ¶m) .await?; if value.is_array() { diff --git a/booster_sdk/src/client/x5_camera_client.rs b/booster_sdk/src/client/x5_camera_client.rs index 709cd43..c7cd36a 100644 --- a/booster_sdk/src/client/x5_camera_client.rs +++ b/booster_sdk/src/client/x5_camera_client.rs @@ -1,12 +1,10 @@ //! X5 camera control RPC client. -use serde::{Deserialize, Serialize, de::DeserializeOwned}; +use serde::{Deserialize, Serialize}; use crate::dds::{RpcClient, RpcClientOptions, X5_CAMERA_CONTROL_API_TOPIC}; use crate::types::Result; -use super::util::{EmptyResponse, serialize_param}; - #[derive(Debug, Clone, Copy, PartialEq, Eq, Hash, Serialize, Deserialize)] #[serde(into = "i32", try_from = "i32")] #[repr(i32)] @@ -121,40 +119,20 @@ impl X5CameraClient { } pub fn with_options(options: RpcClientOptions) -> Result { - let rpc = RpcClient::new(options.with_service_topic(X5_CAMERA_CONTROL_API_TOPIC))?; + let rpc = RpcClient::for_topic(options, X5_CAMERA_CONTROL_API_TOPIC)?; Ok(Self { rpc }) } - pub async fn send_api_request(&self, api_id: X5CameraApiId, param: &str) -> Result<()> { - self.rpc - .call_with_body::(i32::from(api_id), param.to_owned(), None) - .await?; - Ok(()) - } - - pub async fn send_api_request_with_response( - &self, - api_id: X5CameraApiId, - param: &str, - ) -> Result - where - R: DeserializeOwned + Send + 'static, - { - self.rpc - .call_with_body(i32::from(api_id), param.to_owned(), None) - .await - } - pub async fn change_mode(&self, mode: CameraSetMode) -> Result<()> { let param = ChangeModeParameter { mode: i32::from(mode), }; - self.send_api_request(X5CameraApiId::ChangeMode, &serialize_param(¶m)?) + self.rpc + .call_serialized(X5CameraApiId::ChangeMode, ¶m) .await } pub async fn get_status(&self) -> Result { - self.send_api_request_with_response(X5CameraApiId::GetStatus, "") - .await + self.rpc.call_response(X5CameraApiId::GetStatus, "").await } } diff --git a/booster_sdk/src/dds/rpc.rs b/booster_sdk/src/dds/rpc.rs index b035fca..f90a4da 100644 --- a/booster_sdk/src/dds/rpc.rs +++ b/booster_sdk/src/dds/rpc.rs @@ -1,6 +1,6 @@ //! RPC client for high-level API requests over DDS. -use serde::{Serialize, de::DeserializeOwned}; +use serde::{Deserialize, Serialize, de::DeserializeOwned}; use serde_json::Value; use std::time::{Duration, Instant}; use uuid::Uuid; @@ -56,6 +56,9 @@ pub struct RpcClient { default_timeout: Duration, } +#[derive(Debug, Deserialize, Default)] +struct EmptyResponse {} + fn parse_status_value(value: &Value) -> Option { match value { Value::Number(n) => n.as_i64().and_then(|v| i32::try_from(v).ok()), @@ -97,6 +100,10 @@ fn normalize_service_topic(service_topic: &str) -> String { } impl RpcClient { + pub fn for_topic(options: RpcClientOptions, service_topic: impl Into) -> Result { + Self::new(options.with_service_topic(service_topic)) + } + pub fn new(options: RpcClientOptions) -> Result { let node = DdsNode::new(super::DdsConfig { domain_id: options.domain_id, @@ -120,6 +127,57 @@ impl RpcClient { &self.node } + pub async fn call_void(&self, api_id: ApiId, body: impl Into) -> Result<()> + where + ApiId: Into + Copy, + { + self.call_void_with_timeout(api_id, body, None).await + } + + pub async fn call_void_with_timeout( + &self, + api_id: ApiId, + body: impl Into, + timeout: Option, + ) -> Result<()> + where + ApiId: Into + Copy, + { + self.call_with_body::(api_id.into(), body.into(), timeout) + .await?; + Ok(()) + } + + pub async fn call_response(&self, api_id: ApiId, body: impl Into) -> Result + where + ApiId: Into + Copy, + R: DeserializeOwned + Send + 'static, + { + self.call_with_body(api_id.into(), body.into(), None).await + } + + pub async fn call_serialized(&self, api_id: ApiId, params: &P) -> Result<()> + where + ApiId: Into + Copy, + P: Serialize, + { + self.call_void(api_id, serde_json::to_string(params)?).await + } + + pub async fn call_serialized_response( + &self, + api_id: ApiId, + params: &P, + ) -> Result + where + ApiId: Into + Copy, + P: Serialize, + R: DeserializeOwned + Send + 'static, + { + self.call_response(api_id, serde_json::to_string(params)?) + .await + } + pub async fn call(&self, api_id: i32, params: &P, timeout: Option) -> Result where P: Serialize, From 63c021cb42ab40126d4849a0913b3e17395cf7fe Mon Sep 17 00:00:00 2001 From: Gijs de Jong Date: Sun, 22 Feb 2026 19:35:59 +0100 Subject: [PATCH 03/15] add macro for api definitions --- booster_sdk/src/client/ai_client.rs | 78 +++-------- .../src/client/light_control_client.rs | 27 +--- booster_sdk/src/client/mod.rs | 43 ++++++ booster_sdk/src/client/vision_client.rs | 30 +---- booster_sdk/src/client/x5_camera_client.rs | 27 +--- booster_sdk/src/types/b1.rs | 123 +++++------------- 6 files changed, 109 insertions(+), 219 deletions(-) diff --git a/booster_sdk/src/client/ai_client.rs b/booster_sdk/src/client/ai_client.rs index 08c6490..2ec207b 100644 --- a/booster_sdk/src/client/ai_client.rs +++ b/booster_sdk/src/client/ai_client.rs @@ -8,67 +8,23 @@ use crate::dds::{ }; use crate::types::Result; -#[derive(Debug, Clone, Copy, PartialEq, Eq, Hash, Serialize, Deserialize)] -#[serde(into = "i32", try_from = "i32")] -#[repr(i32)] -pub enum AiApiId { - StartAiChat = 2000, - StopAiChat = 2001, - Speak = 2002, - StartFaceTracking = 2003, - StopFaceTracking = 2004, -} - -impl From for i32 { - fn from(value: AiApiId) -> Self { - value as i32 - } -} - -impl TryFrom for AiApiId { - type Error = &'static str; - - fn try_from(value: i32) -> std::result::Result { - match value { - 2000 => Ok(Self::StartAiChat), - 2001 => Ok(Self::StopAiChat), - 2002 => Ok(Self::Speak), - 2003 => Ok(Self::StartFaceTracking), - 2004 => Ok(Self::StopFaceTracking), - _ => Err("invalid value"), - } - } -} - -#[derive(Debug, Clone, Copy, PartialEq, Eq, Hash, Serialize, Deserialize)] -#[serde(into = "i32", try_from = "i32")] -#[repr(i32)] -pub enum LuiApiId { - StartAsr = 1000, - StopAsr = 1001, - StartTts = 1050, - StopTts = 1051, - SendTtsText = 1052, -} - -impl From for i32 { - fn from(value: LuiApiId) -> Self { - value as i32 - } -} - -impl TryFrom for LuiApiId { - type Error = &'static str; - - fn try_from(value: i32) -> std::result::Result { - match value { - 1000 => Ok(Self::StartAsr), - 1001 => Ok(Self::StopAsr), - 1050 => Ok(Self::StartTts), - 1051 => Ok(Self::StopTts), - 1052 => Ok(Self::SendTtsText), - _ => Err("invalid value"), - } +crate::api_id_enum! { + pub enum AiApiId { + StartAiChat = 2000, + StopAiChat = 2001, + Speak = 2002, + StartFaceTracking = 2003, + StopFaceTracking = 2004, + } +} + +crate::api_id_enum! { + pub enum LuiApiId { + StartAsr = 1000, + StopAsr = 1001, + StartTts = 1050, + StopTts = 1051, + SendTtsText = 1052, } } diff --git a/booster_sdk/src/client/light_control_client.rs b/booster_sdk/src/client/light_control_client.rs index 94911e1..8b833c1 100644 --- a/booster_sdk/src/client/light_control_client.rs +++ b/booster_sdk/src/client/light_control_client.rs @@ -5,29 +5,10 @@ use serde::{Deserialize, Serialize}; use crate::dds::{LIGHT_CONTROL_API_TOPIC, RpcClient, RpcClientOptions}; use crate::types::Result; -#[derive(Debug, Clone, Copy, PartialEq, Eq, Hash, Serialize, Deserialize)] -#[serde(into = "i32", try_from = "i32")] -#[repr(i32)] -pub enum LightApiId { - SetLedLightColor = 2000, - StopLedLightControl = 2001, -} - -impl From for i32 { - fn from(value: LightApiId) -> Self { - value as i32 - } -} - -impl TryFrom for LightApiId { - type Error = &'static str; - - fn try_from(value: i32) -> std::result::Result { - match value { - 2000 => Ok(Self::SetLedLightColor), - 2001 => Ok(Self::StopLedLightControl), - _ => Err("invalid value"), - } +crate::api_id_enum! { + pub enum LightApiId { + SetLedLightColor = 2000, + StopLedLightControl = 2001, } } diff --git a/booster_sdk/src/client/mod.rs b/booster_sdk/src/client/mod.rs index 29789b9..8cd27d9 100644 --- a/booster_sdk/src/client/mod.rs +++ b/booster_sdk/src/client/mod.rs @@ -13,3 +13,46 @@ pub use light_control_client::*; pub use loco_client::*; pub use vision_client::*; pub use x5_camera_client::*; + +#[macro_export] +macro_rules! api_id_enum { + ( + $(#[$meta:meta])* + $vis:vis enum $name:ident { + $( + $(#[$variant_meta:meta])* + $variant:ident = $value:literal + ),+ $(,)? + } + ) => { + #[derive(Debug, Clone, Copy, PartialEq, Eq, Hash, serde::Serialize, serde::Deserialize)] + #[serde(into = "i32", try_from = "i32")] + #[repr(i32)] + $(#[$meta])* + $vis enum $name { + $( + $(#[$variant_meta])* + $variant = $value, + )+ + } + + impl From<$name> for i32 { + fn from(value: $name) -> Self { + value as i32 + } + } + + impl TryFrom for $name { + type Error = &'static str; + + fn try_from(value: i32) -> std::result::Result { + match value { + $( + $value => Ok(Self::$variant), + )+ + _ => Err("invalid value"), + } + } + } + }; +} diff --git a/booster_sdk/src/client/vision_client.rs b/booster_sdk/src/client/vision_client.rs index 94d0f3b..d7da085 100644 --- a/booster_sdk/src/client/vision_client.rs +++ b/booster_sdk/src/client/vision_client.rs @@ -6,31 +6,11 @@ use serde_json::Value; use crate::dds::{RpcClient, RpcClientOptions, VISION_API_TOPIC}; use crate::types::Result; -#[derive(Debug, Clone, Copy, PartialEq, Eq, Hash, Serialize, Deserialize)] -#[serde(into = "i32", try_from = "i32")] -#[repr(i32)] -pub enum VisionApiId { - StartVisionService = 3000, - StopVisionService = 3001, - GetDetectionObject = 3002, -} - -impl From for i32 { - fn from(value: VisionApiId) -> Self { - value as i32 - } -} - -impl TryFrom for VisionApiId { - type Error = &'static str; - - fn try_from(value: i32) -> std::result::Result { - match value { - 3000 => Ok(Self::StartVisionService), - 3001 => Ok(Self::StopVisionService), - 3002 => Ok(Self::GetDetectionObject), - _ => Err("invalid value"), - } +crate::api_id_enum! { + pub enum VisionApiId { + StartVisionService = 3000, + StopVisionService = 3001, + GetDetectionObject = 3002, } } diff --git a/booster_sdk/src/client/x5_camera_client.rs b/booster_sdk/src/client/x5_camera_client.rs index c7cd36a..74f8e0e 100644 --- a/booster_sdk/src/client/x5_camera_client.rs +++ b/booster_sdk/src/client/x5_camera_client.rs @@ -5,29 +5,10 @@ use serde::{Deserialize, Serialize}; use crate::dds::{RpcClient, RpcClientOptions, X5_CAMERA_CONTROL_API_TOPIC}; use crate::types::Result; -#[derive(Debug, Clone, Copy, PartialEq, Eq, Hash, Serialize, Deserialize)] -#[serde(into = "i32", try_from = "i32")] -#[repr(i32)] -pub enum X5CameraApiId { - ChangeMode = 5001, - GetStatus = 5002, -} - -impl From for i32 { - fn from(value: X5CameraApiId) -> Self { - value as i32 - } -} - -impl TryFrom for X5CameraApiId { - type Error = &'static str; - - fn try_from(value: i32) -> std::result::Result { - match value { - 5001 => Ok(Self::ChangeMode), - 5002 => Ok(Self::GetStatus), - _ => Err("invalid value"), - } +crate::api_id_enum! { + pub enum X5CameraApiId { + ChangeMode = 5001, + GetStatus = 5002, } } diff --git a/booster_sdk/src/types/b1.rs b/booster_sdk/src/types/b1.rs index 9f15def..7d4c2ef 100644 --- a/booster_sdk/src/types/b1.rs +++ b/booster_sdk/src/types/b1.rs @@ -4,93 +4,42 @@ use serde::{Deserialize, Serialize}; use super::{Hand, RobotMode}; -#[derive(Debug, Clone, Copy, PartialEq, Eq, Hash, Serialize, Deserialize)] -#[serde(into = "i32", try_from = "i32")] -#[repr(i32)] -pub enum LocoApiId { - ChangeMode = 2000, - Move = 2001, - RotateHead = 2004, - WaveHand = 2005, - RotateHeadWithDirection = 2006, - LieDown = 2007, - GetUp = 2008, - MoveHandEndEffector = 2009, - ControlGripper = 2010, - GetFrameTransform = 2011, - SwitchHandEndEffectorControlMode = 2012, - ControlDexterousHand = 2013, - Handshake = 2015, - Dance = 2016, - GetMode = 2017, - GetStatus = 2018, - PushUp = 2019, - PlaySound = 2020, - StopSound = 2021, - GetRobotInfo = 2022, - StopHandEndEffector = 2023, - Shoot = 2024, - GetUpWithMode = 2025, - ZeroTorqueDrag = 2026, - RecordTrajectory = 2027, - ReplayTrajectory = 2028, - WholeBodyDance = 2029, - UpperBodyCustomControl = 2030, - ResetOdometry = 2031, - LoadCustomTrainedTraj = 2032, - ActivateCustomTrainedTraj = 2033, - UnloadCustomTrainedTraj = 2034, - EnterWbcGait = 2035, - ExitWbcGait = 2036, -} - -impl From for i32 { - fn from(value: LocoApiId) -> Self { - value as i32 - } -} - -impl TryFrom for LocoApiId { - type Error = &'static str; - - fn try_from(value: i32) -> Result { - match value { - 2000 => Ok(Self::ChangeMode), - 2001 => Ok(Self::Move), - 2004 => Ok(Self::RotateHead), - 2005 => Ok(Self::WaveHand), - 2006 => Ok(Self::RotateHeadWithDirection), - 2007 => Ok(Self::LieDown), - 2008 => Ok(Self::GetUp), - 2009 => Ok(Self::MoveHandEndEffector), - 2010 => Ok(Self::ControlGripper), - 2011 => Ok(Self::GetFrameTransform), - 2012 => Ok(Self::SwitchHandEndEffectorControlMode), - 2013 => Ok(Self::ControlDexterousHand), - 2015 => Ok(Self::Handshake), - 2016 => Ok(Self::Dance), - 2017 => Ok(Self::GetMode), - 2018 => Ok(Self::GetStatus), - 2019 => Ok(Self::PushUp), - 2020 => Ok(Self::PlaySound), - 2021 => Ok(Self::StopSound), - 2022 => Ok(Self::GetRobotInfo), - 2023 => Ok(Self::StopHandEndEffector), - 2024 => Ok(Self::Shoot), - 2025 => Ok(Self::GetUpWithMode), - 2026 => Ok(Self::ZeroTorqueDrag), - 2027 => Ok(Self::RecordTrajectory), - 2028 => Ok(Self::ReplayTrajectory), - 2029 => Ok(Self::WholeBodyDance), - 2030 => Ok(Self::UpperBodyCustomControl), - 2031 => Ok(Self::ResetOdometry), - 2032 => Ok(Self::LoadCustomTrainedTraj), - 2033 => Ok(Self::ActivateCustomTrainedTraj), - 2034 => Ok(Self::UnloadCustomTrainedTraj), - 2035 => Ok(Self::EnterWbcGait), - 2036 => Ok(Self::ExitWbcGait), - _ => Err("invalid value"), - } +crate::api_id_enum! { + pub enum LocoApiId { + ChangeMode = 2000, + Move = 2001, + RotateHead = 2004, + WaveHand = 2005, + RotateHeadWithDirection = 2006, + LieDown = 2007, + GetUp = 2008, + MoveHandEndEffector = 2009, + ControlGripper = 2010, + GetFrameTransform = 2011, + SwitchHandEndEffectorControlMode = 2012, + ControlDexterousHand = 2013, + Handshake = 2015, + Dance = 2016, + GetMode = 2017, + GetStatus = 2018, + PushUp = 2019, + PlaySound = 2020, + StopSound = 2021, + GetRobotInfo = 2022, + StopHandEndEffector = 2023, + Shoot = 2024, + GetUpWithMode = 2025, + ZeroTorqueDrag = 2026, + RecordTrajectory = 2027, + ReplayTrajectory = 2028, + WholeBodyDance = 2029, + UpperBodyCustomControl = 2030, + ResetOdometry = 2031, + LoadCustomTrainedTraj = 2032, + ActivateCustomTrainedTraj = 2033, + UnloadCustomTrainedTraj = 2034, + EnterWbcGait = 2035, + ExitWbcGait = 2036, } } From cd64d2e2fe2a1756aeff84fa9cbab3f15d123982 Mon Sep 17 00:00:00 2001 From: Gijs de Jong Date: Sun, 22 Feb 2026 19:39:57 +0100 Subject: [PATCH 04/15] clean up macro definitions --- booster_sdk/src/client/ai_client.rs | 4 +- .../src/client/light_control_client.rs | 2 +- booster_sdk/src/client/mod.rs | 41 ++++++++++++++++++- booster_sdk/src/client/vision_client.rs | 2 +- booster_sdk/src/client/x5_camera_client.rs | 2 +- booster_sdk/src/types/b1.rs | 2 +- 6 files changed, 46 insertions(+), 7 deletions(-) diff --git a/booster_sdk/src/client/ai_client.rs b/booster_sdk/src/client/ai_client.rs index 2ec207b..ca20394 100644 --- a/booster_sdk/src/client/ai_client.rs +++ b/booster_sdk/src/client/ai_client.rs @@ -9,7 +9,7 @@ use crate::dds::{ use crate::types::Result; crate::api_id_enum! { - pub enum AiApiId { + AiApiId { StartAiChat = 2000, StopAiChat = 2001, Speak = 2002, @@ -19,7 +19,7 @@ crate::api_id_enum! { } crate::api_id_enum! { - pub enum LuiApiId { + LuiApiId { StartAsr = 1000, StopAsr = 1001, StartTts = 1050, diff --git a/booster_sdk/src/client/light_control_client.rs b/booster_sdk/src/client/light_control_client.rs index 8b833c1..728b352 100644 --- a/booster_sdk/src/client/light_control_client.rs +++ b/booster_sdk/src/client/light_control_client.rs @@ -6,7 +6,7 @@ use crate::dds::{LIGHT_CONTROL_API_TOPIC, RpcClient, RpcClientOptions}; use crate::types::Result; crate::api_id_enum! { - pub enum LightApiId { + LightApiId { SetLedLightColor = 2000, StopLedLightControl = 2001, } diff --git a/booster_sdk/src/client/mod.rs b/booster_sdk/src/client/mod.rs index 8cd27d9..c46c2f3 100644 --- a/booster_sdk/src/client/mod.rs +++ b/booster_sdk/src/client/mod.rs @@ -17,8 +17,47 @@ pub use x5_camera_client::*; #[macro_export] macro_rules! api_id_enum { ( + $name:ident { + $( + $(#[$variant_meta:meta])* + $variant:ident = $value:literal + ),+ $(,)? + } + ) => { + $crate::api_id_enum! { + @impl + pub $name { + $( + $(#[$variant_meta])* + $variant = $value + ),+ + } + } + }; + ( + $(#[$meta:meta])* + $vis:vis $name:ident { + $( + $(#[$variant_meta:meta])* + $variant:ident = $value:literal + ),+ $(,)? + } + ) => { + $crate::api_id_enum! { + @impl + $(#[$meta])* + $vis $name { + $( + $(#[$variant_meta])* + $variant = $value + ),+ + } + } + }; + ( + @impl $(#[$meta:meta])* - $vis:vis enum $name:ident { + $vis:vis $name:ident { $( $(#[$variant_meta:meta])* $variant:ident = $value:literal diff --git a/booster_sdk/src/client/vision_client.rs b/booster_sdk/src/client/vision_client.rs index d7da085..69c7316 100644 --- a/booster_sdk/src/client/vision_client.rs +++ b/booster_sdk/src/client/vision_client.rs @@ -7,7 +7,7 @@ use crate::dds::{RpcClient, RpcClientOptions, VISION_API_TOPIC}; use crate::types::Result; crate::api_id_enum! { - pub enum VisionApiId { + VisionApiId { StartVisionService = 3000, StopVisionService = 3001, GetDetectionObject = 3002, diff --git a/booster_sdk/src/client/x5_camera_client.rs b/booster_sdk/src/client/x5_camera_client.rs index 74f8e0e..7c06a40 100644 --- a/booster_sdk/src/client/x5_camera_client.rs +++ b/booster_sdk/src/client/x5_camera_client.rs @@ -6,7 +6,7 @@ use crate::dds::{RpcClient, RpcClientOptions, X5_CAMERA_CONTROL_API_TOPIC}; use crate::types::Result; crate::api_id_enum! { - pub enum X5CameraApiId { + X5CameraApiId { ChangeMode = 5001, GetStatus = 5002, } diff --git a/booster_sdk/src/types/b1.rs b/booster_sdk/src/types/b1.rs index 7d4c2ef..af9576f 100644 --- a/booster_sdk/src/types/b1.rs +++ b/booster_sdk/src/types/b1.rs @@ -5,7 +5,7 @@ use serde::{Deserialize, Serialize}; use super::{Hand, RobotMode}; crate::api_id_enum! { - pub enum LocoApiId { + LocoApiId { ChangeMode = 2000, Move = 2001, RotateHead = 2004, From 9a611e3d55f66417ef11e4b139dbda3281cec48c Mon Sep 17 00:00:00 2001 From: Gijs de Jong Date: Sun, 22 Feb 2026 19:44:40 +0100 Subject: [PATCH 05/15] less duplicated type definitions --- booster_sdk/src/client/loco_client.rs | 1 + booster_sdk/src/client/mod.rs | 20 ++ booster_sdk/src/client/x5_camera_client.rs | 66 +--- booster_sdk/src/types/b1.rs | 375 +++++---------------- booster_sdk/src/types/robot.rs | 88 ++--- 5 files changed, 136 insertions(+), 414 deletions(-) diff --git a/booster_sdk/src/client/loco_client.rs b/booster_sdk/src/client/loco_client.rs index 3b96c98..f3e2cd9 100644 --- a/booster_sdk/src/client/loco_client.rs +++ b/booster_sdk/src/client/loco_client.rs @@ -15,6 +15,7 @@ use crate::types::{ WholeBodyDanceId, }; use serde_json::json; + // The controller may send an intermediate pending status (-1) before the // final success response. Mode transitions (especially PREPARE) can take // several seconds. diff --git a/booster_sdk/src/client/mod.rs b/booster_sdk/src/client/mod.rs index c46c2f3..07fb797 100644 --- a/booster_sdk/src/client/mod.rs +++ b/booster_sdk/src/client/mod.rs @@ -34,6 +34,26 @@ macro_rules! api_id_enum { } } }; + ( + $(#[$meta:meta])* + $name:ident { + $( + $(#[$variant_meta:meta])* + $variant:ident = $value:literal + ),+ $(,)? + } + ) => { + $crate::api_id_enum! { + @impl + $(#[$meta])* + pub $name { + $( + $(#[$variant_meta])* + $variant = $value + ),+ + } + } + }; ( $(#[$meta:meta])* $vis:vis $name:ident { diff --git a/booster_sdk/src/client/x5_camera_client.rs b/booster_sdk/src/client/x5_camera_client.rs index 7c06a40..7c62277 100644 --- a/booster_sdk/src/client/x5_camera_client.rs +++ b/booster_sdk/src/client/x5_camera_client.rs @@ -12,63 +12,21 @@ crate::api_id_enum! { } } -#[derive(Debug, Clone, Copy, PartialEq, Eq, Hash, Serialize, Deserialize)] -#[serde(into = "i32", try_from = "i32")] -#[repr(i32)] -pub enum CameraSetMode { - CameraModeNormal = 0, - CameraModeHighResolution = 1, - CameraModeNormalEnable = 2, - CameraModeHighResolutionEnable = 3, -} - -impl From for i32 { - fn from(value: CameraSetMode) -> Self { - value as i32 - } -} - -impl TryFrom for CameraSetMode { - type Error = &'static str; - - fn try_from(value: i32) -> std::result::Result { - match value { - 0 => Ok(Self::CameraModeNormal), - 1 => Ok(Self::CameraModeHighResolution), - 2 => Ok(Self::CameraModeNormalEnable), - 3 => Ok(Self::CameraModeHighResolutionEnable), - _ => Err("invalid value"), - } - } -} - -#[derive(Debug, Clone, Copy, PartialEq, Eq, Hash, Serialize, Deserialize)] -#[serde(into = "i32", try_from = "i32")] -#[repr(i32)] -pub enum CameraControlStatus { - CameraStatusNormal = 0, - CameraStatusHighResolution = 1, - CameraStatusError = 2, - CameraStatusNull = 3, -} - -impl From for i32 { - fn from(value: CameraControlStatus) -> Self { - value as i32 +crate::api_id_enum! { + CameraSetMode { + CameraModeNormal = 0, + CameraModeHighResolution = 1, + CameraModeNormalEnable = 2, + CameraModeHighResolutionEnable = 3, } } -impl TryFrom for CameraControlStatus { - type Error = &'static str; - - fn try_from(value: i32) -> std::result::Result { - match value { - 0 => Ok(Self::CameraStatusNormal), - 1 => Ok(Self::CameraStatusHighResolution), - 2 => Ok(Self::CameraStatusError), - 3 => Ok(Self::CameraStatusNull), - _ => Err("invalid value"), - } +crate::api_id_enum! { + CameraControlStatus { + CameraStatusNormal = 0, + CameraStatusHighResolution = 1, + CameraStatusError = 2, + CameraStatusNull = 3, } } diff --git a/booster_sdk/src/types/b1.rs b/booster_sdk/src/types/b1.rs index af9576f..642913b 100644 --- a/booster_sdk/src/types/b1.rs +++ b/booster_sdk/src/types/b1.rs @@ -43,325 +43,110 @@ crate::api_id_enum! { } } -#[derive(Debug, Clone, Copy, PartialEq, Eq, Hash, Serialize, Deserialize)] -#[serde(into = "i32", try_from = "i32")] -#[repr(i32)] -pub enum BodyControl { - Unknown = 0, - Damping = 1, - Prepare = 2, - HumanlikeGait = 3, - ProneBody = 4, - SoccerGait = 5, - Custom = 6, - GetUp = 7, - WholeBodyDance = 8, - Shoot = 9, - InsideFoot = 10, - Goalie = 11, - WbcGait = 12, -} - -impl From for i32 { - fn from(value: BodyControl) -> Self { - value as i32 - } -} - -impl TryFrom for BodyControl { - type Error = &'static str; - - fn try_from(value: i32) -> Result { - match value { - 0 => Ok(Self::Unknown), - 1 => Ok(Self::Damping), - 2 => Ok(Self::Prepare), - 3 => Ok(Self::HumanlikeGait), - 4 => Ok(Self::ProneBody), - 5 => Ok(Self::SoccerGait), - 6 => Ok(Self::Custom), - 7 => Ok(Self::GetUp), - 8 => Ok(Self::WholeBodyDance), - 9 => Ok(Self::Shoot), - 10 => Ok(Self::InsideFoot), - 11 => Ok(Self::Goalie), - 12 => Ok(Self::WbcGait), - _ => Err("invalid value"), - } - } -} - -#[derive(Debug, Clone, Copy, PartialEq, Eq, Hash, Serialize, Deserialize)] -#[serde(into = "i32", try_from = "i32")] -#[repr(i32)] -pub enum Action { - Unknown = 0, - HandShake = 1, - HandWave = 2, - HandControl = 3, - DanceNewYear = 4, - DanceNezha = 5, - DanceTowardsFuture = 6, - GestureDabbing = 7, - GestureUltraman = 8, - GestureRespect = 9, - GestureCheer = 10, - GestureLuckyCat = 11, - GestureBoxing = 12, - ZeroTorqueDrag = 13, - RecordTraj = 14, - RunRecordedTraj = 15, -} - -impl From for i32 { - fn from(value: Action) -> Self { - value as i32 - } -} - -impl TryFrom for Action { - type Error = &'static str; - - fn try_from(value: i32) -> Result { - match value { - 0 => Ok(Self::Unknown), - 1 => Ok(Self::HandShake), - 2 => Ok(Self::HandWave), - 3 => Ok(Self::HandControl), - 4 => Ok(Self::DanceNewYear), - 5 => Ok(Self::DanceNezha), - 6 => Ok(Self::DanceTowardsFuture), - 7 => Ok(Self::GestureDabbing), - 8 => Ok(Self::GestureUltraman), - 9 => Ok(Self::GestureRespect), - 10 => Ok(Self::GestureCheer), - 11 => Ok(Self::GestureLuckyCat), - 12 => Ok(Self::GestureBoxing), - 13 => Ok(Self::ZeroTorqueDrag), - 14 => Ok(Self::RecordTraj), - 15 => Ok(Self::RunRecordedTraj), - _ => Err("invalid value"), - } - } -} - -#[derive(Debug, Clone, Copy, PartialEq, Eq, Hash, Serialize, Deserialize)] -#[serde(into = "i32", try_from = "i32")] -#[repr(i32)] -pub enum Frame { - Unknown = -1, - Body = 0, - Head = 1, - LeftHand = 2, - RightHand = 3, - LeftFoot = 4, - RightFoot = 5, -} - -impl From for i32 { - fn from(value: Frame) -> Self { - value as i32 - } -} - -impl TryFrom for Frame { - type Error = &'static str; - - fn try_from(value: i32) -> Result { - match value { - -1 => Ok(Self::Unknown), - 0 => Ok(Self::Body), - 1 => Ok(Self::Head), - 2 => Ok(Self::LeftHand), - 3 => Ok(Self::RightHand), - 4 => Ok(Self::LeftFoot), - 5 => Ok(Self::RightFoot), - _ => Err("invalid value"), - } - } -} - -#[derive(Debug, Clone, Copy, PartialEq, Eq, Hash, Serialize, Deserialize)] -#[serde(into = "i32", try_from = "i32")] -#[repr(i32)] -pub enum HandAction { - Open = 0, - Close = 1, -} - -impl From for i32 { - fn from(value: HandAction) -> Self { - value as i32 - } -} - -impl TryFrom for HandAction { - type Error = &'static str; - - fn try_from(value: i32) -> Result { - match value { - 0 => Ok(Self::Open), - 1 => Ok(Self::Close), - _ => Err("invalid value"), - } - } -} - -#[derive(Debug, Clone, Copy, PartialEq, Eq, Hash, Serialize, Deserialize)] -#[serde(into = "i32", try_from = "i32")] -#[repr(i32)] -pub enum BoosterHandType { - InspireHand = 0, - InspireTouchHand = 2, - RevoHand = 3, - Unknown = -1, -} - -impl From for i32 { - fn from(value: BoosterHandType) -> Self { - value as i32 - } -} - -impl TryFrom for BoosterHandType { - type Error = &'static str; - - fn try_from(value: i32) -> Result { - match value { - 0 => Ok(Self::InspireHand), - 2 => Ok(Self::InspireTouchHand), - 3 => Ok(Self::RevoHand), - -1 => Ok(Self::Unknown), - _ => Err("invalid value"), - } +crate::api_id_enum! { + BodyControl { + Unknown = 0, + Damping = 1, + Prepare = 2, + HumanlikeGait = 3, + ProneBody = 4, + SoccerGait = 5, + Custom = 6, + GetUp = 7, + WholeBodyDance = 8, + Shoot = 9, + InsideFoot = 10, + Goalie = 11, + WbcGait = 12, } } -#[derive(Debug, Clone, Copy, PartialEq, Eq, Hash, Serialize, Deserialize)] -#[serde(into = "i32", try_from = "i32")] -#[repr(i32)] -pub enum DanceId { - NewYear = 0, - Nezha = 1, - TowardsFuture = 2, - DabbingGesture = 3, - UltramanGesture = 4, - RespectGesture = 5, - CheeringGesture = 6, - LuckyCatGesture = 7, - Stop = 1000, -} - -impl From for i32 { - fn from(value: DanceId) -> Self { - value as i32 +crate::api_id_enum! { + Action { + Unknown = 0, + HandShake = 1, + HandWave = 2, + HandControl = 3, + DanceNewYear = 4, + DanceNezha = 5, + DanceTowardsFuture = 6, + GestureDabbing = 7, + GestureUltraman = 8, + GestureRespect = 9, + GestureCheer = 10, + GestureLuckyCat = 11, + GestureBoxing = 12, + ZeroTorqueDrag = 13, + RecordTraj = 14, + RunRecordedTraj = 15, } } -impl TryFrom for DanceId { - type Error = &'static str; - - fn try_from(value: i32) -> Result { - match value { - 0 => Ok(Self::NewYear), - 1 => Ok(Self::Nezha), - 2 => Ok(Self::TowardsFuture), - 3 => Ok(Self::DabbingGesture), - 4 => Ok(Self::UltramanGesture), - 5 => Ok(Self::RespectGesture), - 6 => Ok(Self::CheeringGesture), - 7 => Ok(Self::LuckyCatGesture), - 1000 => Ok(Self::Stop), - _ => Err("invalid value"), - } +crate::api_id_enum! { + Frame { + Unknown = -1, + Body = 0, + Head = 1, + LeftHand = 2, + RightHand = 3, + LeftFoot = 4, + RightFoot = 5, } } -#[derive(Debug, Clone, Copy, PartialEq, Eq, Hash, Serialize, Deserialize)] -#[serde(into = "i32", try_from = "i32")] -#[repr(i32)] -pub enum WholeBodyDanceId { - ArbicDance = 0, - MichaelDance1 = 1, - MichaelDance2 = 2, - MichaelDance3 = 3, - MoonWalk = 4, - BoxingStyleKick = 5, - RoundhouseKick = 6, -} - -impl From for i32 { - fn from(value: WholeBodyDanceId) -> Self { - value as i32 +crate::api_id_enum! { + HandAction { + Open = 0, + Close = 1, } } -impl TryFrom for WholeBodyDanceId { - type Error = &'static str; - - fn try_from(value: i32) -> Result { - match value { - 0 => Ok(Self::ArbicDance), - 1 => Ok(Self::MichaelDance1), - 2 => Ok(Self::MichaelDance2), - 3 => Ok(Self::MichaelDance3), - 4 => Ok(Self::MoonWalk), - 5 => Ok(Self::BoxingStyleKick), - 6 => Ok(Self::RoundhouseKick), - _ => Err("invalid value"), - } +crate::api_id_enum! { + BoosterHandType { + InspireHand = 0, + InspireTouchHand = 2, + RevoHand = 3, + Unknown = -1, } } -#[derive(Debug, Clone, Copy, PartialEq, Eq, Hash, Serialize, Deserialize)] -#[serde(into = "i32", try_from = "i32")] -#[repr(i32)] -pub enum JointOrder { - MuJoCo = 0, - IsaacLab = 1, -} - -impl From for i32 { - fn from(value: JointOrder) -> Self { - value as i32 +crate::api_id_enum! { + DanceId { + NewYear = 0, + Nezha = 1, + TowardsFuture = 2, + DabbingGesture = 3, + UltramanGesture = 4, + RespectGesture = 5, + CheeringGesture = 6, + LuckyCatGesture = 7, + Stop = 1000, } } -impl TryFrom for JointOrder { - type Error = &'static str; - - fn try_from(value: i32) -> Result { - match value { - 0 => Ok(Self::MuJoCo), - 1 => Ok(Self::IsaacLab), - _ => Err("invalid value"), - } +crate::api_id_enum! { + WholeBodyDanceId { + ArbicDance = 0, + MichaelDance1 = 1, + MichaelDance2 = 2, + MichaelDance3 = 3, + MoonWalk = 4, + BoxingStyleKick = 5, + RoundhouseKick = 6, } } -#[derive(Debug, Clone, Copy, PartialEq, Eq, Hash, Serialize, Deserialize)] -#[serde(into = "i32", try_from = "i32")] -#[repr(i32)] -pub enum GripperControlMode { - Position = 0, - Force = 1, -} - -impl From for i32 { - fn from(value: GripperControlMode) -> Self { - value as i32 +crate::api_id_enum! { + JointOrder { + MuJoCo = 0, + IsaacLab = 1, } } -impl TryFrom for GripperControlMode { - type Error = &'static str; - - fn try_from(value: i32) -> Result { - match value { - 0 => Ok(Self::Position), - 1 => Ok(Self::Force), - _ => Err("invalid value"), - } +crate::api_id_enum! { + GripperControlMode { + Position = 0, + Force = 1, } } diff --git a/booster_sdk/src/types/robot.rs b/booster_sdk/src/types/robot.rs index 0caa7c7..daddb7e 100644 --- a/booster_sdk/src/types/robot.rs +++ b/booster_sdk/src/types/robot.rs @@ -2,50 +2,27 @@ use serde::{Deserialize, Serialize}; -/// Robot operational mode -#[derive(Debug, Clone, Copy, PartialEq, Eq, Hash, Serialize, Deserialize)] -#[serde(into = "i32", try_from = "i32")] -#[repr(i32)] -#[non_exhaustive] -pub enum RobotMode { - /// Unknown mode, typically used for error handling. - Unknown = -1, - - /// Damping mode, motors are compliant - Damping = 0, - - /// Prepare mode, standing pose - Prepare = 1, +crate::api_id_enum! { + /// Robot operational mode + #[non_exhaustive] + pub RobotMode { + /// Unknown mode, typically used for error handling. + Unknown = -1, - /// Walking mode, active locomotion - Walking = 2, + /// Damping mode, motors are compliant + Damping = 0, - /// Custom mode, user-defined behavior - Custom = 3, - - /// Soccer mode - Soccer = 4, -} + /// Prepare mode, standing pose + Prepare = 1, -impl TryFrom for RobotMode { - type Error = &'static str; + /// Walking mode, active locomotion + Walking = 2, - fn try_from(value: i32) -> Result { - match value { - -1 => Ok(RobotMode::Unknown), - 0 => Ok(RobotMode::Damping), - 1 => Ok(RobotMode::Prepare), - 2 => Ok(RobotMode::Walking), - 3 => Ok(RobotMode::Custom), - 4 => Ok(RobotMode::Soccer), - _ => Err("invalid value"), - } - } -} + /// Custom mode, user-defined behavior + Custom = 3, -impl From for i32 { - fn from(mode: RobotMode) -> Self { - mode as i32 + /// Soccer mode + Soccer = 4, } } @@ -105,33 +82,14 @@ impl TryFrom for Hand { } } -/// Gripper control mode -#[derive(Debug, Clone, Copy, PartialEq, Eq, Hash, Serialize, Deserialize)] -#[serde(into = "i32", try_from = "i32")] -#[repr(i32)] -pub enum GripperMode { - /// Position-based control - Position = 0, - - /// Force-based control - Force = 1, -} - -impl From for i32 { - fn from(mode: GripperMode) -> Self { - mode as i32 - } -} +crate::api_id_enum! { + /// Gripper control mode + GripperMode { + /// Position-based control + Position = 0, -impl TryFrom for GripperMode { - type Error = &'static str; - - fn try_from(value: i32) -> Result { - match value { - 0 => Ok(GripperMode::Position), - 1 => Ok(GripperMode::Force), - _ => Err("invalid value"), - } + /// Force-based control + Force = 1, } } From 8bae78e0fd6db84defffed4a06f61b8949feadf7 Mon Sep 17 00:00:00 2001 From: Gijs de Jong Date: Sun, 22 Feb 2026 19:50:22 +0100 Subject: [PATCH 06/15] add rust doc --- booster_sdk/src/client/ai_client.rs | 28 ++++++++++ .../src/client/light_control_client.rs | 8 +++ booster_sdk/src/client/loco_client.rs | 51 +++++++++++++++++++ booster_sdk/src/client/mod.rs | 7 +++ booster_sdk/src/client/vision_client.rs | 10 ++++ booster_sdk/src/client/x5_camera_client.rs | 10 ++++ booster_sdk/src/types/b1.rs | 28 ++++++++++ 7 files changed, 142 insertions(+) diff --git a/booster_sdk/src/client/ai_client.rs b/booster_sdk/src/client/ai_client.rs index ca20394..410de65 100644 --- a/booster_sdk/src/client/ai_client.rs +++ b/booster_sdk/src/client/ai_client.rs @@ -9,6 +9,7 @@ use crate::dds::{ use crate::types::Result; crate::api_id_enum! { + /// AI chat RPC API identifiers. AiApiId { StartAiChat = 2000, StopAiChat = 2001, @@ -19,6 +20,7 @@ crate::api_id_enum! { } crate::api_id_enum! { + /// LUI speech RPC API identifiers. LuiApiId { StartAsr = 1000, StopAsr = 1001, @@ -28,12 +30,14 @@ crate::api_id_enum! { } } +/// TTS configuration for AI chat. #[derive(Debug, Clone, PartialEq, Eq, Serialize, Deserialize)] pub struct TtsConfig { pub voice_type: String, pub ignore_bracket_text: Vec, } +/// LLM prompt configuration for AI chat. #[derive(Debug, Clone, PartialEq, Eq, Serialize, Deserialize)] pub struct LlmConfig { pub system_prompt: String, @@ -41,12 +45,14 @@ pub struct LlmConfig { pub prompt_name: String, } +/// ASR interruption configuration. #[derive(Debug, Clone, PartialEq, Eq, Serialize, Deserialize)] pub struct AsrConfig { pub interrupt_speech_duration: i32, pub interrupt_keywords: Vec, } +/// Parameters for starting AI chat. #[derive(Debug, Clone, PartialEq, Eq, Serialize, Deserialize)] pub struct StartAiChatParameter { pub interrupt_mode: bool, @@ -56,16 +62,19 @@ pub struct StartAiChatParameter { pub enable_face_tracking: bool, } +/// Parameters for AI speech output. #[derive(Debug, Clone, PartialEq, Eq, Serialize, Deserialize)] pub struct SpeakParameter { pub msg: String, } +/// LUI TTS startup configuration. #[derive(Debug, Clone, PartialEq, Eq, Serialize, Deserialize)] pub struct LuiTtsConfig { pub voice_type: String, } +/// Parameters for sending TTS text to LUI. #[derive(Debug, Clone, PartialEq, Eq, Serialize, Deserialize)] pub struct LuiTtsParameter { pub text: String, @@ -90,6 +99,7 @@ pub struct AsrChunk { pub text: String, } +/// User identifier used by robot-generated subtitle entries. pub const BOOSTER_ROBOT_USER_ID: &str = "BoosterRobot"; /// High-level RPC client for AI chat features. @@ -98,39 +108,48 @@ pub struct AiClient { } impl AiClient { + /// Create an AI client with default options. pub fn new() -> Result { Self::with_options(RpcClientOptions::for_service(AI_API_TOPIC)) } + /// Create an AI client with custom RPC options. pub fn with_options(options: RpcClientOptions) -> Result { let rpc = RpcClient::for_topic(options, AI_API_TOPIC)?; Ok(Self { rpc }) } + /// Access the underlying DDS node. pub fn node(&self) -> &DdsNode { self.rpc.node() } + /// Start AI chat with the provided configuration. pub async fn start_ai_chat(&self, param: &StartAiChatParameter) -> Result<()> { self.rpc.call_serialized(AiApiId::StartAiChat, param).await } + /// Stop the active AI chat session. pub async fn stop_ai_chat(&self) -> Result<()> { self.rpc.call_void(AiApiId::StopAiChat, "").await } + /// Request the AI service to speak a message. pub async fn speak(&self, param: &SpeakParameter) -> Result<()> { self.rpc.call_serialized(AiApiId::Speak, param).await } + /// Enable face tracking in the AI service. pub async fn start_face_tracking(&self) -> Result<()> { self.rpc.call_void(AiApiId::StartFaceTracking, "").await } + /// Disable face tracking in the AI service. pub async fn stop_face_tracking(&self) -> Result<()> { self.rpc.call_void(AiApiId::StopFaceTracking, "").await } + /// Subscribe to AI subtitle messages. pub fn subscribe_subtitle(&self) -> Result> { self.rpc.node().subscribe(&ai_subtitle_topic(), 16) } @@ -142,39 +161,48 @@ pub struct LuiClient { } impl LuiClient { + /// Create a LUI client with default options. pub fn new() -> Result { Self::with_options(RpcClientOptions::for_service(LUI_API_TOPIC)) } + /// Create a LUI client with custom RPC options. pub fn with_options(options: RpcClientOptions) -> Result { let rpc = RpcClient::for_topic(options, LUI_API_TOPIC)?; Ok(Self { rpc }) } + /// Access the underlying DDS node. pub fn node(&self) -> &DdsNode { self.rpc.node() } + /// Start ASR. pub async fn start_asr(&self) -> Result<()> { self.rpc.call_void(LuiApiId::StartAsr, "").await } + /// Stop ASR. pub async fn stop_asr(&self) -> Result<()> { self.rpc.call_void(LuiApiId::StopAsr, "").await } + /// Start TTS with the given configuration. pub async fn start_tts(&self, config: &LuiTtsConfig) -> Result<()> { self.rpc.call_serialized(LuiApiId::StartTts, config).await } + /// Stop TTS. pub async fn stop_tts(&self) -> Result<()> { self.rpc.call_void(LuiApiId::StopTts, "").await } + /// Send text to TTS. pub async fn send_tts_text(&self, param: &LuiTtsParameter) -> Result<()> { self.rpc.call_serialized(LuiApiId::SendTtsText, param).await } + /// Subscribe to ASR chunk messages. pub fn subscribe_asr_chunk(&self) -> Result> { self.rpc.node().subscribe(&lui_asr_chunk_topic(), 16) } diff --git a/booster_sdk/src/client/light_control_client.rs b/booster_sdk/src/client/light_control_client.rs index 728b352..5c77364 100644 --- a/booster_sdk/src/client/light_control_client.rs +++ b/booster_sdk/src/client/light_control_client.rs @@ -6,12 +6,14 @@ use crate::dds::{LIGHT_CONTROL_API_TOPIC, RpcClient, RpcClientOptions}; use crate::types::Result; crate::api_id_enum! { + /// LED light control RPC API identifiers. LightApiId { SetLedLightColor = 2000, StopLedLightControl = 2001, } } +/// RGB color payload for LED control. #[derive(Debug, Clone, Copy, PartialEq, Eq, Serialize, Deserialize)] pub struct SetLedLightColorParameter { pub r: u8, @@ -20,6 +22,7 @@ pub struct SetLedLightColorParameter { } impl SetLedLightColorParameter { + /// Parse a `#RRGGBB` color string. #[must_use] pub fn from_hex(color: &str) -> Option { let color = color.trim(); @@ -41,15 +44,18 @@ pub struct LightControlClient { } impl LightControlClient { + /// Create a light control client with default options. pub fn new() -> Result { Self::with_options(RpcClientOptions::for_service(LIGHT_CONTROL_API_TOPIC)) } + /// Create a light control client with custom RPC options. pub fn with_options(options: RpcClientOptions) -> Result { let rpc = RpcClient::for_topic(options, LIGHT_CONTROL_API_TOPIC)?; Ok(Self { rpc }) } + /// Set LED light color from RGB values. pub async fn set_led_light_color(&self, r: u8, g: u8, b: u8) -> Result<()> { let param = SetLedLightColorParameter { r, g, b }; self.rpc @@ -57,12 +63,14 @@ impl LightControlClient { .await } + /// Set LED light color using a parameter struct. pub async fn set_led_light_color_param(&self, param: &SetLedLightColorParameter) -> Result<()> { self.rpc .call_serialized(LightApiId::SetLedLightColor, param) .await } + /// Stop LED light control. pub async fn stop_led_light_control(&self) -> Result<()> { self.rpc .call_void(LightApiId::StopLedLightControl, "") diff --git a/booster_sdk/src/client/loco_client.rs b/booster_sdk/src/client/loco_client.rs index f3e2cd9..1630e5c 100644 --- a/booster_sdk/src/client/loco_client.rs +++ b/booster_sdk/src/client/loco_client.rs @@ -30,10 +30,12 @@ pub struct BoosterClient { } impl BoosterClient { + /// Create a locomotion client with default options. pub fn new() -> Result { Self::with_options(RpcClientOptions::default()) } + /// Create a locomotion client with custom RPC options. pub fn with_options(options: RpcClientOptions) -> Result { let rpc = RpcClient::new(options)?; let node = rpc.node().clone(); @@ -49,10 +51,12 @@ impl BoosterClient { }) } + /// Access the underlying DDS node. pub fn node(&self) -> &DdsNode { self.rpc.node() } + /// Change the robot mode. pub async fn change_mode(&self, mode: RobotMode) -> Result<()> { let param = json!({ "mode": i32::from(mode) }).to_string(); self.rpc @@ -60,28 +64,34 @@ impl BoosterClient { .await } + /// Get the current robot mode. pub async fn get_mode(&self) -> Result { self.rpc.call_response(LocoApiId::GetMode, "").await } + /// Get the current robot status. pub async fn get_status(&self) -> Result { self.rpc.call_response(LocoApiId::GetStatus, "").await } + /// Get robot identity and version information. pub async fn get_robot_info(&self) -> Result { self.rpc.call_response(LocoApiId::GetRobotInfo, "").await } + /// Move the robot base in body frame. pub async fn move_robot(&self, vx: f32, vy: f32, vyaw: f32) -> Result<()> { let param = json!({ "vx": vx, "vy": vy, "vyaw": vyaw }).to_string(); self.rpc.call_void(LocoApiId::Move, param).await } + /// Rotate the head to absolute pitch/yaw angles. pub async fn rotate_head(&self, pitch: f32, yaw: f32) -> Result<()> { let param = json!({ "pitch": pitch, "yaw": yaw }).to_string(); self.rpc.call_void(LocoApiId::RotateHead, param).await } + /// Trigger a right-hand wave action. pub async fn wave_hand(&self, action: HandAction) -> Result<()> { let param = json!({ "hand_index": i32::from(HandIndex::Right), @@ -91,6 +101,7 @@ impl BoosterClient { self.rpc.call_void(LocoApiId::WaveHand, param).await } + /// Rotate the head with direction steps. pub async fn rotate_head_with_direction( &self, pitch_direction: i32, @@ -106,27 +117,33 @@ impl BoosterClient { .await } + /// Command the robot to lie down. pub async fn lie_down(&self) -> Result<()> { self.rpc.call_void(LocoApiId::LieDown, "").await } + /// Command the robot to get up. pub async fn get_up(&self) -> Result<()> { self.rpc.call_void(LocoApiId::GetUp, "").await } + /// Command the robot to get up into a specific mode. pub async fn get_up_with_mode(&self, mode: RobotMode) -> Result<()> { let param = json!({ "mode": i32::from(mode) }).to_string(); self.rpc.call_void(LocoApiId::GetUpWithMode, param).await } + /// Trigger a shoot action. pub async fn shoot(&self) -> Result<()> { self.rpc.call_void(LocoApiId::Shoot, "").await } + /// Trigger a push-up action. pub async fn push_up(&self) -> Result<()> { self.rpc.call_void(LocoApiId::PushUp, "").await } + /// Move a hand end effector with auxiliary posture input. pub async fn move_hand_end_effector_with_aux( &self, target_posture: &crate::types::Posture, @@ -148,6 +165,7 @@ impl BoosterClient { .await } + /// Move a hand end effector. pub async fn move_hand_end_effector( &self, target_posture: &crate::types::Posture, @@ -167,6 +185,7 @@ impl BoosterClient { .await } + /// Move a hand end effector using the v2 behavior flag. pub async fn move_hand_end_effector_v2( &self, target_posture: &crate::types::Posture, @@ -186,10 +205,12 @@ impl BoosterClient { .await } + /// Stop hand end-effector motion. pub async fn stop_hand_end_effector(&self) -> Result<()> { self.rpc.call_void(LocoApiId::StopHandEndEffector, "").await } + /// Control a gripper. pub async fn control_gripper( &self, motion_param: GripperMotionParameter, @@ -205,6 +226,7 @@ impl BoosterClient { self.rpc.call_void(LocoApiId::ControlGripper, param).await } + /// Query the transform from `src` frame to `dst` frame. pub async fn get_frame_transform(&self, src: Frame, dst: Frame) -> Result { let param = json!({ "src": i32::from(src), @@ -216,6 +238,7 @@ impl BoosterClient { .await } + /// Enable or disable hand end-effector control mode. pub async fn switch_hand_end_effector_control_mode(&self, switch_on: bool) -> Result<()> { let param = json!({ "switch_on": switch_on }).to_string(); self.rpc @@ -223,11 +246,13 @@ impl BoosterClient { .await } + /// Trigger a handshake action. pub async fn handshake(&self, action: HandAction) -> Result<()> { let param = json!({ "hand_action": i32::from(action) }).to_string(); self.rpc.call_void(LocoApiId::Handshake, param).await } + /// Control a dexterous hand with explicit hand type. pub async fn control_dexterous_hand( &self, finger_params: &[DexterousFingerParameter], @@ -245,6 +270,7 @@ impl BoosterClient { .await } + /// Control a dexterous hand using the default hand type. pub async fn control_dexterous_hand_default( &self, finger_params: &[DexterousFingerParameter], @@ -254,40 +280,48 @@ impl BoosterClient { .await } + /// Trigger an upper-body dance or gesture action. pub async fn dance(&self, dance_id: DanceId) -> Result<()> { let param = json!({ "dance_id": i32::from(dance_id) }).to_string(); self.rpc.call_void(LocoApiId::Dance, param).await } + /// Play a sound file on the robot. pub async fn play_sound(&self, sound_file_path: impl Into) -> Result<()> { let param = json!({ "sound_file_path": sound_file_path.into() }).to_string(); self.rpc.call_void(LocoApiId::PlaySound, param).await } + /// Stop active sound playback. pub async fn stop_sound(&self) -> Result<()> { self.rpc.call_void(LocoApiId::StopSound, "").await } + /// Enable or disable zero-torque drag mode. pub async fn zero_torque_drag(&self, active: bool) -> Result<()> { let param = json!({ "enable": active }).to_string(); self.rpc.call_void(LocoApiId::ZeroTorqueDrag, param).await } + /// Start or stop trajectory recording. pub async fn record_trajectory(&self, active: bool) -> Result<()> { let param = json!({ "enable": active }).to_string(); self.rpc.call_void(LocoApiId::RecordTrajectory, param).await } + /// Replay a recorded trajectory from file. pub async fn replay_trajectory(&self, traj_file_path: impl Into) -> Result<()> { let param = json!({ "traj_file_path": traj_file_path.into() }).to_string(); self.rpc.call_void(LocoApiId::ReplayTrajectory, param).await } + /// Trigger a whole-body dance action. pub async fn whole_body_dance(&self, dance_id: WholeBodyDanceId) -> Result<()> { let param = json!({ "dance_id": i32::from(dance_id) }).to_string(); self.rpc.call_void(LocoApiId::WholeBodyDance, param).await } + /// Enable or disable upper-body custom control. pub async fn upper_body_custom_control(&self, start: bool) -> Result<()> { let param = json!({ "start": start }).to_string(); self.rpc @@ -295,10 +329,12 @@ impl BoosterClient { .await } + /// Reset odometry state. pub async fn reset_odometry(&self) -> Result<()> { self.rpc.call_void(LocoApiId::ResetOdometry, "").await } + /// Load a custom trained trajectory. pub async fn load_custom_trained_traj( &self, traj: &CustomTrainedTraj, @@ -308,6 +344,7 @@ impl BoosterClient { .await } + /// Activate a loaded custom trained trajectory by id. pub async fn activate_custom_trained_traj(&self, tid: impl Into) -> Result<()> { let param = json!({ "tid": tid.into() }).to_string(); self.rpc @@ -315,6 +352,7 @@ impl BoosterClient { .await } + /// Unload a custom trained trajectory by id. pub async fn unload_custom_trained_traj(&self, tid: impl Into) -> Result<()> { let param = json!({ "tid": tid.into() }).to_string(); self.rpc @@ -322,54 +360,67 @@ impl BoosterClient { .await } + /// Enter WBC gait mode. pub async fn enter_wbc_gait(&self) -> Result<()> { self.rpc.call_void(LocoApiId::EnterWbcGait, "").await } + /// Exit WBC gait mode. pub async fn exit_wbc_gait(&self) -> Result<()> { self.rpc.call_void(LocoApiId::ExitWbcGait, "").await } + /// Publish a raw gripper control topic message. pub fn publish_gripper(&self, control: GripperControl) -> Result<()> { self.gripper_publisher.write(control) } + /// Publish a high-level gripper command. pub fn publish_gripper_command(&self, command: &crate::client::GripperCommand) -> Result<()> { self.gripper_publisher.write(command.to_dds_control()) } + /// Publish a light control topic message. pub fn publish_light_control(&self, message: LightControlMsg) -> Result<()> { self.light_publisher.write(message) } + /// Publish a safe mode topic message. pub fn enter_safe_mode(&self, message: SafeMode) -> Result<()> { self.safe_mode_publisher.write(message) } + /// Subscribe to device gateway robot status messages. pub fn subscribe_device_gateway(&self) -> Result> { self.rpc.node().subscribe(&device_gateway_topic(), 32) } + /// Subscribe to motion state messages. pub fn subscribe_motion_state(&self) -> Result> { self.rpc.node().subscribe(&motion_state_topic(), 16) } + /// Subscribe to battery state messages. pub fn subscribe_battery_state(&self) -> Result> { self.rpc.node().subscribe(&battery_state_topic(), 8) } + /// Subscribe to button event messages. pub fn subscribe_button_events(&self) -> Result> { self.rpc.node().subscribe(&button_event_topic(), 32) } + /// Subscribe to remote controller state messages. pub fn subscribe_remote_controller(&self) -> Result> { self.rpc.node().subscribe(&remote_controller_topic(), 32) } + /// Subscribe to robot process state messages. pub fn subscribe_process_state(&self) -> Result> { self.rpc.node().subscribe(&process_state_topic(), 8) } + /// Subscribe to video stream messages. pub fn subscribe_video_stream(&self) -> Result> { self.rpc.node().subscribe(&video_stream_topic(), 4) } diff --git a/booster_sdk/src/client/mod.rs b/booster_sdk/src/client/mod.rs index 07fb797..1137adb 100644 --- a/booster_sdk/src/client/mod.rs +++ b/booster_sdk/src/client/mod.rs @@ -14,6 +14,13 @@ pub use loco_client::*; pub use vision_client::*; pub use x5_camera_client::*; +/// Declare an i32-backed enum with serde, `From`, and `TryFrom`. +/// +/// Default form makes the enum `pub`: +/// `api_id_enum! { Name { A = 1, B = 2 } }` +/// +/// You can also pass attributes and visibility: +/// `api_id_enum! { #[non_exhaustive] pub(crate) Name { A = 1 } }` #[macro_export] macro_rules! api_id_enum { ( diff --git a/booster_sdk/src/client/vision_client.rs b/booster_sdk/src/client/vision_client.rs index 69c7316..76c4c95 100644 --- a/booster_sdk/src/client/vision_client.rs +++ b/booster_sdk/src/client/vision_client.rs @@ -7,6 +7,7 @@ use crate::dds::{RpcClient, RpcClientOptions, VISION_API_TOPIC}; use crate::types::Result; crate::api_id_enum! { + /// Vision service RPC API identifiers. VisionApiId { StartVisionService = 3000, StopVisionService = 3001, @@ -14,6 +15,7 @@ crate::api_id_enum! { } } +/// Parameters for starting the vision service. #[derive(Debug, Clone, Copy, PartialEq, Eq, Serialize, Deserialize)] pub struct StartVisionServiceParameter { pub enable_position: bool, @@ -21,6 +23,7 @@ pub struct StartVisionServiceParameter { pub enable_face_detection: bool, } +/// Parameters for object detection requests. #[derive(Debug, Clone, Copy, PartialEq, Serialize, Deserialize)] pub struct GetDetectionObjectParameter { pub focus_ratio: f32, @@ -32,6 +35,7 @@ impl Default for GetDetectionObjectParameter { } } +/// Single vision detection result. #[derive(Debug, Clone, PartialEq, Serialize, Deserialize)] pub struct DetectResults { pub xmin: i64, @@ -50,15 +54,18 @@ pub struct VisionClient { } impl VisionClient { + /// Create a vision client with default options. pub fn new() -> Result { Self::with_options(RpcClientOptions::for_service(VISION_API_TOPIC)) } + /// Create a vision client with custom RPC options. pub fn with_options(options: RpcClientOptions) -> Result { let rpc = RpcClient::for_topic(options, VISION_API_TOPIC)?; Ok(Self { rpc }) } + /// Start the vision service with selected features. pub async fn start_vision_service( &self, enable_position: bool, @@ -75,12 +82,14 @@ impl VisionClient { .await } + /// Stop the vision service. pub async fn stop_vision_service(&self) -> Result<()> { self.rpc .call_void(VisionApiId::StopVisionService, "{}") .await } + /// Fetch detected objects with a custom focus ratio. pub async fn get_detection_object_with_ratio( &self, focus_ratio: f32, @@ -102,6 +111,7 @@ impl VisionClient { Ok(Vec::new()) } + /// Fetch detected objects with the default focus ratio. pub async fn get_detection_object(&self) -> Result> { self.get_detection_object_with_ratio(GetDetectionObjectParameter::default().focus_ratio) .await diff --git a/booster_sdk/src/client/x5_camera_client.rs b/booster_sdk/src/client/x5_camera_client.rs index 7c62277..294a818 100644 --- a/booster_sdk/src/client/x5_camera_client.rs +++ b/booster_sdk/src/client/x5_camera_client.rs @@ -6,6 +6,7 @@ use crate::dds::{RpcClient, RpcClientOptions, X5_CAMERA_CONTROL_API_TOPIC}; use crate::types::Result; crate::api_id_enum! { + /// X5 camera RPC API identifiers. X5CameraApiId { ChangeMode = 5001, GetStatus = 5002, @@ -13,6 +14,7 @@ crate::api_id_enum! { } crate::api_id_enum! { + /// Requested X5 camera mode. CameraSetMode { CameraModeNormal = 0, CameraModeHighResolution = 1, @@ -22,6 +24,7 @@ crate::api_id_enum! { } crate::api_id_enum! { + /// Reported X5 camera status values. CameraControlStatus { CameraStatusNormal = 0, CameraStatusHighResolution = 1, @@ -30,17 +33,20 @@ crate::api_id_enum! { } } +/// Parameters for camera mode changes. #[derive(Debug, Clone, Copy, PartialEq, Eq, Serialize, Deserialize)] pub struct ChangeModeParameter { pub mode: i32, } +/// Response payload for camera status requests. #[derive(Debug, Clone, Copy, PartialEq, Eq, Serialize, Deserialize)] pub struct GetStatusResponse { pub status: i32, } impl GetStatusResponse { + /// Convert the raw integer status into the enum form. #[must_use] pub fn status_enum(&self) -> Option { CameraControlStatus::try_from(self.status).ok() @@ -53,15 +59,18 @@ pub struct X5CameraClient { } impl X5CameraClient { + /// Create an X5 camera client with default options. pub fn new() -> Result { Self::with_options(RpcClientOptions::for_service(X5_CAMERA_CONTROL_API_TOPIC)) } + /// Create an X5 camera client with custom RPC options. pub fn with_options(options: RpcClientOptions) -> Result { let rpc = RpcClient::for_topic(options, X5_CAMERA_CONTROL_API_TOPIC)?; Ok(Self { rpc }) } + /// Change the camera mode. pub async fn change_mode(&self, mode: CameraSetMode) -> Result<()> { let param = ChangeModeParameter { mode: i32::from(mode), @@ -71,6 +80,7 @@ impl X5CameraClient { .await } + /// Read the current camera status. pub async fn get_status(&self) -> Result { self.rpc.call_response(X5CameraApiId::GetStatus, "").await } diff --git a/booster_sdk/src/types/b1.rs b/booster_sdk/src/types/b1.rs index 642913b..b8bdeac 100644 --- a/booster_sdk/src/types/b1.rs +++ b/booster_sdk/src/types/b1.rs @@ -5,6 +5,7 @@ use serde::{Deserialize, Serialize}; use super::{Hand, RobotMode}; crate::api_id_enum! { + /// Locomotion RPC API identifiers. LocoApiId { ChangeMode = 2000, Move = 2001, @@ -44,6 +45,7 @@ crate::api_id_enum! { } crate::api_id_enum! { + /// High-level body behavior identifiers. BodyControl { Unknown = 0, Damping = 1, @@ -62,6 +64,7 @@ crate::api_id_enum! { } crate::api_id_enum! { + /// High-level action identifiers. Action { Unknown = 0, HandShake = 1, @@ -83,6 +86,7 @@ crate::api_id_enum! { } crate::api_id_enum! { + /// Reference frame identifiers used in transforms. Frame { Unknown = -1, Body = 0, @@ -95,6 +99,7 @@ crate::api_id_enum! { } crate::api_id_enum! { + /// Hand open/close action identifiers. HandAction { Open = 0, Close = 1, @@ -102,6 +107,7 @@ crate::api_id_enum! { } crate::api_id_enum! { + /// Supported dexterous hand hardware identifiers. BoosterHandType { InspireHand = 0, InspireTouchHand = 2, @@ -111,6 +117,7 @@ crate::api_id_enum! { } crate::api_id_enum! { + /// Upper-body dance and gesture identifiers. DanceId { NewYear = 0, Nezha = 1, @@ -125,6 +132,7 @@ crate::api_id_enum! { } crate::api_id_enum! { + /// Whole-body dance identifiers. WholeBodyDanceId { ArbicDance = 0, MichaelDance1 = 1, @@ -137,6 +145,7 @@ crate::api_id_enum! { } crate::api_id_enum! { + /// Joint ordering identifiers for model compatibility. JointOrder { MuJoCo = 0, IsaacLab = 1, @@ -144,12 +153,14 @@ crate::api_id_enum! { } crate::api_id_enum! { + /// Gripper command mode identifiers. GripperControlMode { Position = 0, Force = 1, } } +/// Cartesian position. #[derive(Debug, Clone, Copy, PartialEq, Serialize, Deserialize)] pub struct Position { pub x: f32, @@ -157,6 +168,7 @@ pub struct Position { pub z: f32, } +/// Euler orientation in radians. #[derive(Debug, Clone, Copy, PartialEq, Serialize, Deserialize)] pub struct Orientation { pub roll: f32, @@ -164,12 +176,14 @@ pub struct Orientation { pub yaw: f32, } +/// Position and orientation pair. #[derive(Debug, Clone, Copy, PartialEq, Serialize, Deserialize)] pub struct Posture { pub position: Position, pub orientation: Orientation, } +/// Quaternion orientation. #[derive(Debug, Clone, Copy, PartialEq, Serialize, Deserialize)] pub struct Quaternion { pub x: f32, @@ -178,12 +192,14 @@ pub struct Quaternion { pub w: f32, } +/// Transform with position and quaternion orientation. #[derive(Debug, Clone, Copy, PartialEq, Serialize, Deserialize)] pub struct Transform { pub position: Position, pub orientation: Quaternion, } +/// Gripper motion command values. #[derive(Debug, Clone, Copy, PartialEq, Eq, Serialize, Deserialize)] pub struct GripperMotionParameter { pub position: i32, @@ -191,6 +207,7 @@ pub struct GripperMotionParameter { pub speed: i32, } +/// Single dexterous finger control value set. #[derive(Debug, Clone, Copy, PartialEq, Eq, Serialize, Deserialize)] pub struct DexterousFingerParameter { pub seq: i32, @@ -199,18 +216,21 @@ pub struct DexterousFingerParameter { pub speed: i32, } +/// Response payload for `GetMode`. #[derive(Debug, Clone, PartialEq, Serialize, Deserialize)] pub struct GetModeResponse { pub mode: i32, } impl GetModeResponse { + /// Convert `mode` to [`RobotMode`], if valid. #[must_use] pub fn mode_enum(&self) -> Option { RobotMode::try_from(self.mode).ok() } } +/// Response payload for `GetStatus`. #[derive(Debug, Clone, PartialEq, Serialize, Deserialize)] pub struct GetStatusResponse { pub current_mode: i32, @@ -219,16 +239,19 @@ pub struct GetStatusResponse { } impl GetStatusResponse { + /// Convert `current_mode` to [`RobotMode`], if valid. #[must_use] pub fn current_mode_enum(&self) -> Option { RobotMode::try_from(self.current_mode).ok() } + /// Convert `current_body_control` to [`BodyControl`], if valid. #[must_use] pub fn current_body_control_enum(&self) -> Option { BodyControl::try_from(self.current_body_control).ok() } + /// Convert valid entries in `current_actions` to [`Action`]. #[must_use] pub fn current_actions_enum(&self) -> Vec { self.current_actions @@ -239,6 +262,7 @@ impl GetStatusResponse { } } +/// Basic robot identity and version information. #[derive(Debug, Clone, PartialEq, Eq, Serialize, Deserialize)] pub struct GetRobotInfoResponse { pub name: String, @@ -248,6 +272,7 @@ pub struct GetRobotInfoResponse { pub serial_number: String, } +/// Model parameters used by custom trajectories. #[derive(Debug, Clone, PartialEq, Serialize, Deserialize)] pub struct CustomModelParams { pub action_scale: Vec, @@ -255,6 +280,7 @@ pub struct CustomModelParams { pub kd: Vec, } +/// Model metadata for custom trajectories. #[derive(Debug, Clone, PartialEq, Serialize, Deserialize)] pub struct CustomModel { pub file_path: String, @@ -262,12 +288,14 @@ pub struct CustomModel { pub joint_order: JointOrder, } +/// Payload used to load a custom trained trajectory. #[derive(Debug, Clone, PartialEq, Serialize, Deserialize)] pub struct CustomTrainedTraj { pub traj_file_path: String, pub model: CustomModel, } +/// Response payload for `LoadCustomTrainedTraj`. #[derive(Debug, Clone, PartialEq, Eq, Serialize, Deserialize)] pub struct LoadCustomTrainedTrajResponse { pub tid: String, From 5696cc030e732ab1e2d001c5bce2ddea490f4eec Mon Sep 17 00:00:00 2001 From: Gijs de Jong Date: Sun, 22 Feb 2026 20:06:31 +0100 Subject: [PATCH 07/15] python sdk parity --- .github/workflows/release.yml | 13 + booster_sdk_py/booster_sdk/__init__.py | 95 +- booster_sdk_py/booster_sdk/client.py | 45 +- booster_sdk_py/booster_sdk/types.py | 76 +- .../booster_sdk_bindings.pyi | 527 +++- booster_sdk_py/src/lib.rs | 2264 ++++++++++++++++- 6 files changed, 2900 insertions(+), 120 deletions(-) diff --git a/.github/workflows/release.yml b/.github/workflows/release.yml index 575f682..9975f0a 100644 --- a/.github/workflows/release.yml +++ b/.github/workflows/release.yml @@ -89,6 +89,19 @@ jobs: name: wheels-${{ matrix.platform.id }} path: dist/*.whl + publish-crate: + runs-on: ubuntu-22.04 + environment: release + steps: + - uses: actions/checkout@v4 + - uses: prefix-dev/setup-pixi@v0.9.1 + with: + pixi-version: v0.63.2 + - name: Publish to crates.io + run: pixi run cargo publish + env: + CARGO_REGISTRY_TOKEN: ${{ secrets.CARGO_REGISTRY_TOKEN }} + publish: runs-on: ubuntu-22.04 needs: build diff --git a/booster_sdk_py/booster_sdk/__init__.py b/booster_sdk_py/booster_sdk/__init__.py index 57e6036..be94a06 100644 --- a/booster_sdk_py/booster_sdk/__init__.py +++ b/booster_sdk_py/booster_sdk/__init__.py @@ -1,16 +1,103 @@ -"""Booster SDK - Python bindings for controlling the Booster robot""" +"""Booster SDK Python package.""" from __future__ import annotations -from .client import BoosterClient -from .types import BoosterSdkError, GripperCommand, GripperMode, Hand, RobotMode +from .client import ( + AiClient, + BoosterClient, + LightControlClient, + LuiClient, + VisionClient, + X5CameraClient, +) +from .types import ( + BOOSTER_ROBOT_USER_ID, + Action, + AsrConfig, + BoosterHandType, + BoosterSdkError, + BodyControl, + CameraControlStatus, + CameraSetMode, + CustomModel, + CustomModelParams, + CustomTrainedTraj, + DanceId, + DetectResults, + DexterousFingerParameter, + Frame, + GetModeResponse, + GetRobotInfoResponse, + GetStatusResponse, + GripperCommand, + GripperControlMode, + GripperMode, + GripperMotionParameter, + Hand, + HandAction, + JointOrder, + LlmConfig, + LoadCustomTrainedTrajResponse, + LuiTtsConfig, + LuiTtsParameter, + Orientation, + Position, + Posture, + Quaternion, + RobotMode, + SpeakParameter, + StartAiChatParameter, + Transform, + TtsConfig, + WholeBodyDanceId, + X5CameraGetStatusResponse, +) __all__ = [ - # Client "BoosterClient", + "AiClient", + "LuiClient", + "LightControlClient", + "VisionClient", + "X5CameraClient", + "BOOSTER_ROBOT_USER_ID", "BoosterSdkError", "RobotMode", "Hand", "GripperMode", "GripperCommand", + "HandAction", + "Frame", + "GripperControlMode", + "BoosterHandType", + "DanceId", + "WholeBodyDanceId", + "JointOrder", + "BodyControl", + "Action", + "CameraSetMode", + "CameraControlStatus", + "Position", + "Orientation", + "Posture", + "Quaternion", + "Transform", + "GripperMotionParameter", + "DexterousFingerParameter", + "CustomModelParams", + "CustomModel", + "CustomTrainedTraj", + "TtsConfig", + "LlmConfig", + "AsrConfig", + "StartAiChatParameter", + "SpeakParameter", + "LuiTtsConfig", + "LuiTtsParameter", + "GetModeResponse", + "GetStatusResponse", + "GetRobotInfoResponse", + "LoadCustomTrainedTrajResponse", + "X5CameraGetStatusResponse", + "DetectResults", ] diff --git a/booster_sdk_py/booster_sdk/client.py b/booster_sdk_py/booster_sdk/client.py index f01b4db..5e59471 100644 --- a/booster_sdk_py/booster_sdk/client.py +++ b/booster_sdk_py/booster_sdk/client.py @@ -1,34 +1,21 @@ -"""High-level client for controlling the Booster robot.""" +"""High-level client classes for the Booster SDK.""" from __future__ import annotations import booster_sdk_bindings as bindings -from .types import GripperCommand, GripperMode, Hand, RobotMode - -__all__ = ["BoosterClient"] - - -class BoosterClient: - """Minimal Python wrapper for the BoosterClient bindings.""" - - def __init__(self) -> None: - self._inner = bindings.BoosterClient() - - def change_mode(self, mode: RobotMode) -> None: - self._inner.change_mode(mode) - - def move_robot(self, vx: float, vy: float, vyaw: float) -> None: - self._inner.move_robot(vx, vy, vyaw) - - def publish_gripper_command(self, command: GripperCommand) -> None: - self._inner.publish_gripper_command(command) - - def publish_gripper( - self, - hand: Hand, - mode: GripperMode, - motion_param: int, - speed: int | None = None, - ) -> None: - self._inner.publish_gripper(hand, mode, motion_param, speed) +BoosterClient = bindings.BoosterClient +AiClient = bindings.AiClient +LuiClient = bindings.LuiClient +LightControlClient = bindings.LightControlClient +VisionClient = bindings.VisionClient +X5CameraClient = bindings.X5CameraClient + +__all__ = [ + "BoosterClient", + "AiClient", + "LuiClient", + "LightControlClient", + "VisionClient", + "X5CameraClient", +] diff --git a/booster_sdk_py/booster_sdk/types.py b/booster_sdk_py/booster_sdk/types.py index 03cd910..d8fea7f 100644 --- a/booster_sdk_py/booster_sdk/types.py +++ b/booster_sdk_py/booster_sdk/types.py @@ -3,17 +3,87 @@ from __future__ import annotations from booster_sdk_bindings import ( + BOOSTER_ROBOT_USER_ID, + Action, + AsrConfig, + BoosterHandType, BoosterSdkError, + BodyControl, + CameraControlStatus, + CameraSetMode, + CustomModel, + CustomModelParams, + CustomTrainedTraj, + DanceId, + DetectResults, + DexterousFingerParameter, + Frame, + GetModeResponse, + GetRobotInfoResponse, + GetStatusResponse, GripperCommand, + GripperControlMode, GripperMode, + GripperMotionParameter, Hand, + HandAction, + JointOrder, + LlmConfig, + LoadCustomTrainedTrajResponse, + LuiTtsConfig, + LuiTtsParameter, + Orientation, + Position, + Posture, + Quaternion, RobotMode, + SpeakParameter, + StartAiChatParameter, + Transform, + TtsConfig, + WholeBodyDanceId, + X5CameraGetStatusResponse, ) __all__ = [ + "BOOSTER_ROBOT_USER_ID", "BoosterSdkError", - "GripperCommand", - "GripperMode", - "Hand", "RobotMode", + "Hand", + "GripperMode", + "GripperCommand", + "HandAction", + "Frame", + "GripperControlMode", + "BoosterHandType", + "DanceId", + "WholeBodyDanceId", + "JointOrder", + "BodyControl", + "Action", + "CameraSetMode", + "CameraControlStatus", + "Position", + "Orientation", + "Posture", + "Quaternion", + "Transform", + "GripperMotionParameter", + "DexterousFingerParameter", + "CustomModelParams", + "CustomModel", + "CustomTrainedTraj", + "TtsConfig", + "LlmConfig", + "AsrConfig", + "StartAiChatParameter", + "SpeakParameter", + "LuiTtsConfig", + "LuiTtsParameter", + "GetModeResponse", + "GetStatusResponse", + "GetRobotInfoResponse", + "LoadCustomTrainedTrajResponse", + "X5CameraGetStatusResponse", + "DetectResults", ] diff --git a/booster_sdk_py/booster_sdk_bindings/booster_sdk_bindings.pyi b/booster_sdk_py/booster_sdk_bindings/booster_sdk_bindings.pyi index 948ddd2..321b6c5 100644 --- a/booster_sdk_py/booster_sdk_bindings/booster_sdk_bindings.pyi +++ b/booster_sdk_py/booster_sdk_bindings/booster_sdk_bindings.pyi @@ -1,13 +1,16 @@ """Type stubs for booster_sdk_bindings.""" +from __future__ import annotations + +BOOSTER_ROBOT_USER_ID: str + class BoosterSdkError(Exception): """Exception raised by the Booster SDK.""" ... class RobotMode: - """Robot operating mode.""" - + UNKNOWN: RobotMode DAMPING: RobotMode PREPARE: RobotMode WALKING: RobotMode @@ -19,8 +22,6 @@ class RobotMode: def __eq__(self, other: object) -> bool: ... class Hand: - """Robot hand identifier.""" - LEFT: Hand RIGHT: Hand @@ -29,8 +30,6 @@ class Hand: def __eq__(self, other: object) -> bool: ... class GripperMode: - """Gripper control mode.""" - POSITION: GripperMode FORCE: GripperMode @@ -38,9 +37,143 @@ class GripperMode: def __int__(self) -> int: ... def __eq__(self, other: object) -> bool: ... -class GripperCommand: - """Gripper control command.""" +class HandAction: + OPEN: HandAction + CLOSE: HandAction + + def __repr__(self) -> str: ... + def __int__(self) -> int: ... + def __eq__(self, other: object) -> bool: ... + +class Frame: + UNKNOWN: Frame + BODY: Frame + HEAD: Frame + LEFT_HAND: Frame + RIGHT_HAND: Frame + LEFT_FOOT: Frame + RIGHT_FOOT: Frame + + def __repr__(self) -> str: ... + def __int__(self) -> int: ... + def __eq__(self, other: object) -> bool: ... + +class GripperControlMode: + POSITION: GripperControlMode + FORCE: GripperControlMode + + def __repr__(self) -> str: ... + def __int__(self) -> int: ... + def __eq__(self, other: object) -> bool: ... + +class BoosterHandType: + INSPIRE_HAND: BoosterHandType + INSPIRE_TOUCH_HAND: BoosterHandType + REVO_HAND: BoosterHandType + UNKNOWN: BoosterHandType + + def __repr__(self) -> str: ... + def __int__(self) -> int: ... + def __eq__(self, other: object) -> bool: ... + +class DanceId: + NEW_YEAR: DanceId + NEZHA: DanceId + TOWARDS_FUTURE: DanceId + DABBING_GESTURE: DanceId + ULTRAMAN_GESTURE: DanceId + RESPECT_GESTURE: DanceId + CHEERING_GESTURE: DanceId + LUCKY_CAT_GESTURE: DanceId + STOP: DanceId + + def __repr__(self) -> str: ... + def __int__(self) -> int: ... + def __eq__(self, other: object) -> bool: ... + +class WholeBodyDanceId: + ARBIC_DANCE: WholeBodyDanceId + MICHAEL_DANCE_1: WholeBodyDanceId + MICHAEL_DANCE_2: WholeBodyDanceId + MICHAEL_DANCE_3: WholeBodyDanceId + MOON_WALK: WholeBodyDanceId + BOXING_STYLE_KICK: WholeBodyDanceId + ROUNDHOUSE_KICK: WholeBodyDanceId + + def __repr__(self) -> str: ... + def __int__(self) -> int: ... + def __eq__(self, other: object) -> bool: ... + +class JointOrder: + MUJOCO: JointOrder + ISAAC_LAB: JointOrder + + def __repr__(self) -> str: ... + def __int__(self) -> int: ... + def __eq__(self, other: object) -> bool: ... + +class BodyControl: + UNKNOWN: BodyControl + DAMPING: BodyControl + PREPARE: BodyControl + HUMANLIKE_GAIT: BodyControl + PRONE_BODY: BodyControl + SOCCER_GAIT: BodyControl + CUSTOM: BodyControl + GET_UP: BodyControl + WHOLE_BODY_DANCE: BodyControl + SHOOT: BodyControl + INSIDE_FOOT: BodyControl + GOALIE: BodyControl + WBC_GAIT: BodyControl + + def __repr__(self) -> str: ... + def __int__(self) -> int: ... + def __eq__(self, other: object) -> bool: ... + +class Action: + UNKNOWN: Action + HAND_SHAKE: Action + HAND_WAVE: Action + HAND_CONTROL: Action + DANCE_NEW_YEAR: Action + DANCE_NEZHA: Action + DANCE_TOWARDS_FUTURE: Action + GESTURE_DABBING: Action + GESTURE_ULTRAMAN: Action + GESTURE_RESPECT: Action + GESTURE_CHEER: Action + GESTURE_LUCKY_CAT: Action + GESTURE_BOXING: Action + ZERO_TORQUE_DRAG: Action + RECORD_TRAJ: Action + RUN_RECORDED_TRAJ: Action + + def __repr__(self) -> str: ... + def __int__(self) -> int: ... + def __eq__(self, other: object) -> bool: ... +class CameraSetMode: + CAMERA_MODE_NORMAL: CameraSetMode + CAMERA_MODE_HIGH_RESOLUTION: CameraSetMode + CAMERA_MODE_NORMAL_ENABLE: CameraSetMode + CAMERA_MODE_HIGH_RESOLUTION_ENABLE: CameraSetMode + + def __repr__(self) -> str: ... + def __int__(self) -> int: ... + def __eq__(self, other: object) -> bool: ... + +class CameraControlStatus: + CAMERA_STATUS_NORMAL: CameraControlStatus + CAMERA_STATUS_HIGH_RESOLUTION: CameraControlStatus + CAMERA_STATUS_ERROR: CameraControlStatus + CAMERA_STATUS_NULL: CameraControlStatus + + def __repr__(self) -> str: ... + def __int__(self) -> int: ... + def __eq__(self, other: object) -> bool: ... + +class GripperCommand: def __init__( self, hand: Hand, @@ -54,15 +187,346 @@ class GripperCommand: def close(hand: Hand) -> GripperCommand: ... @staticmethod def grasp(hand: Hand, force: int) -> GripperCommand: ... + @property + def hand(self) -> Hand: ... + @property + def mode(self) -> GripperMode: ... + @property + def motion_param(self) -> int: ... + @property + def speed(self) -> int: ... def __repr__(self) -> str: ... -class BoosterClient: - """High-level robot client.""" +class Position: + def __init__(self, x: float, y: float, z: float) -> None: ... + @property + def x(self) -> float: ... + @x.setter + def x(self, value: float) -> None: ... + @property + def y(self) -> float: ... + @y.setter + def y(self, value: float) -> None: ... + @property + def z(self) -> float: ... + @z.setter + def z(self, value: float) -> None: ... + +class Orientation: + def __init__(self, roll: float, pitch: float, yaw: float) -> None: ... + @property + def roll(self) -> float: ... + @roll.setter + def roll(self, value: float) -> None: ... + @property + def pitch(self) -> float: ... + @pitch.setter + def pitch(self, value: float) -> None: ... + @property + def yaw(self) -> float: ... + @yaw.setter + def yaw(self, value: float) -> None: ... + +class Posture: + def __init__(self, position: Position, orientation: Orientation) -> None: ... + @property + def position(self) -> Position: ... + @position.setter + def position(self, value: Position) -> None: ... + @property + def orientation(self) -> Orientation: ... + @orientation.setter + def orientation(self, value: Orientation) -> None: ... + +class Quaternion: + def __init__(self, x: float, y: float, z: float, w: float) -> None: ... + @property + def x(self) -> float: ... + @property + def y(self) -> float: ... + @property + def z(self) -> float: ... + @property + def w(self) -> float: ... + +class Transform: + def __init__(self, position: Position, orientation: Quaternion) -> None: ... + @property + def position(self) -> Position: ... + @property + def orientation(self) -> Quaternion: ... +class GripperMotionParameter: + def __init__(self, position: int, force: int, speed: int) -> None: ... + @property + def position(self) -> int: ... + @property + def force(self) -> int: ... + @property + def speed(self) -> int: ... + +class DexterousFingerParameter: + def __init__(self, seq: int, angle: int, force: int, speed: int) -> None: ... + @property + def seq(self) -> int: ... + @property + def angle(self) -> int: ... + @property + def force(self) -> int: ... + @property + def speed(self) -> int: ... + +class CustomModelParams: + def __init__( + self, action_scale: list[float], kp: list[float], kd: list[float] + ) -> None: ... + @property + def action_scale(self) -> list[float]: ... + @property + def kp(self) -> list[float]: ... + @property + def kd(self) -> list[float]: ... + +class CustomModel: + def __init__( + self, + file_path: str, + params: list[CustomModelParams], + joint_order: JointOrder, + ) -> None: ... + @property + def file_path(self) -> str: ... + @property + def params(self) -> list[CustomModelParams]: ... + @property + def joint_order(self) -> JointOrder: ... + +class CustomTrainedTraj: + def __init__(self, traj_file_path: str, model: CustomModel) -> None: ... + @property + def traj_file_path(self) -> str: ... + @property + def model(self) -> CustomModel: ... + +class TtsConfig: + def __init__(self, voice_type: str, ignore_bracket_text: list[int]) -> None: ... + @property + def voice_type(self) -> str: ... + @property + def ignore_bracket_text(self) -> list[int]: ... + +class LlmConfig: + def __init__( + self, system_prompt: str, welcome_msg: str, prompt_name: str + ) -> None: ... + @property + def system_prompt(self) -> str: ... + @property + def welcome_msg(self) -> str: ... + @property + def prompt_name(self) -> str: ... + +class AsrConfig: + def __init__( + self, interrupt_speech_duration: int, interrupt_keywords: list[str] + ) -> None: ... + @property + def interrupt_speech_duration(self) -> int: ... + @property + def interrupt_keywords(self) -> list[str]: ... + +class StartAiChatParameter: + def __init__( + self, + interrupt_mode: bool, + asr_config: AsrConfig, + llm_config: LlmConfig, + tts_config: TtsConfig, + enable_face_tracking: bool, + ) -> None: ... + @property + def interrupt_mode(self) -> bool: ... + @property + def asr_config(self) -> AsrConfig: ... + @property + def llm_config(self) -> LlmConfig: ... + @property + def tts_config(self) -> TtsConfig: ... + @property + def enable_face_tracking(self) -> bool: ... + +class SpeakParameter: + def __init__(self, msg: str) -> None: ... + @property + def msg(self) -> str: ... + +class LuiTtsConfig: + def __init__(self, voice_type: str) -> None: ... + @property + def voice_type(self) -> str: ... + +class LuiTtsParameter: + def __init__(self, text: str) -> None: ... + @property + def text(self) -> str: ... + +class GetModeResponse: + def __init__(self, mode: int) -> None: ... + @property + def mode(self) -> int: ... + def mode_enum(self) -> RobotMode | None: ... + +class GetStatusResponse: + def __init__( + self, + current_mode: int, + current_body_control: int, + current_actions: list[int], + ) -> None: ... + @property + def current_mode(self) -> int: ... + @property + def current_body_control(self) -> int: ... + @property + def current_actions(self) -> list[int]: ... + def current_mode_enum(self) -> RobotMode | None: ... + def current_body_control_enum(self) -> BodyControl | None: ... + def current_actions_enum(self) -> list[Action]: ... + +class GetRobotInfoResponse: + def __init__( + self, + name: str, + nickname: str, + version: str, + model: str, + serial_number: str, + ) -> None: ... + @property + def name(self) -> str: ... + @property + def nickname(self) -> str: ... + @property + def version(self) -> str: ... + @property + def model(self) -> str: ... + @property + def serial_number(self) -> str: ... + +class LoadCustomTrainedTrajResponse: + def __init__(self, tid: str) -> None: ... + @property + def tid(self) -> str: ... + +class X5CameraGetStatusResponse: + def __init__(self, status: int) -> None: ... + @property + def status(self) -> int: ... + def status_enum(self) -> CameraControlStatus | None: ... + +class DetectResults: + def __init__( + self, + xmin: int, + ymin: int, + xmax: int, + ymax: int, + position: list[float], + tag: str, + conf: float, + rgb_mean: list[int], + ) -> None: ... + @property + def xmin(self) -> int: ... + @property + def ymin(self) -> int: ... + @property + def xmax(self) -> int: ... + @property + def ymax(self) -> int: ... + @property + def position(self) -> list[float]: ... + @property + def tag(self) -> str: ... + @property + def conf(self) -> float: ... + @property + def rgb_mean(self) -> list[int]: ... + +class BoosterClient: def __init__(self) -> None: ... - def wait_for_discovery(self, timeout_secs: float) -> None: ... def change_mode(self, mode: RobotMode) -> None: ... + def get_mode(self) -> GetModeResponse: ... + def get_status(self) -> GetStatusResponse: ... + def get_robot_info(self) -> GetRobotInfoResponse: ... def move_robot(self, vx: float, vy: float, vyaw: float) -> None: ... + def rotate_head(self, pitch: float, yaw: float) -> None: ... + def wave_hand(self, action: HandAction) -> None: ... + def rotate_head_with_direction( + self, pitch_direction: int, yaw_direction: int + ) -> None: ... + def lie_down(self) -> None: ... + def get_up(self) -> None: ... + def get_up_with_mode(self, mode: RobotMode) -> None: ... + def shoot(self) -> None: ... + def push_up(self) -> None: ... + def move_hand_end_effector( + self, + target_posture: Posture, + time_millis: int, + hand_index: Hand, + ) -> None: ... + def move_hand_end_effector_with_aux( + self, + target_posture: Posture, + aux_posture: Posture, + time_millis: int, + hand_index: Hand, + ) -> None: ... + def move_hand_end_effector_v2( + self, + target_posture: Posture, + time_millis: int, + hand_index: Hand, + ) -> None: ... + def stop_hand_end_effector(self) -> None: ... + def control_gripper( + self, + motion_param: GripperMotionParameter, + mode: GripperControlMode, + hand_index: Hand, + ) -> None: ... + def get_frame_transform(self, src: Frame, dst: Frame) -> Transform: ... + def switch_hand_end_effector_control_mode(self, switch_on: bool) -> None: ... + def handshake(self, action: HandAction) -> None: ... + def control_dexterous_hand( + self, + finger_params: list[DexterousFingerParameter], + hand_index: Hand, + hand_type: BoosterHandType, + ) -> None: ... + def control_dexterous_hand_default( + self, + finger_params: list[DexterousFingerParameter], + hand_index: Hand, + ) -> None: ... + def dance(self, dance_id: DanceId) -> None: ... + def play_sound(self, sound_file_path: str) -> None: ... + def stop_sound(self) -> None: ... + def zero_torque_drag(self, active: bool) -> None: ... + def record_trajectory(self, active: bool) -> None: ... + def replay_trajectory(self, traj_file_path: str) -> None: ... + def whole_body_dance(self, dance_id: WholeBodyDanceId) -> None: ... + def upper_body_custom_control(self, start: bool) -> None: ... + def reset_odometry(self) -> None: ... + def load_custom_trained_traj( + self, + traj: CustomTrainedTraj, + ) -> LoadCustomTrainedTrajResponse: ... + def activate_custom_trained_traj(self, tid: str) -> None: ... + def unload_custom_trained_traj(self, tid: str) -> None: ... + def enter_wbc_gait(self) -> None: ... + def exit_wbc_gait(self) -> None: ... def publish_gripper_command(self, command: GripperCommand) -> None: ... def publish_gripper( self, @@ -71,3 +535,44 @@ class BoosterClient: motion_param: int, speed: int | None = ..., ) -> None: ... + +class AiClient: + def __init__(self) -> None: ... + def start_ai_chat(self, param: StartAiChatParameter) -> None: ... + def stop_ai_chat(self) -> None: ... + def speak(self, param: SpeakParameter) -> None: ... + def start_face_tracking(self) -> None: ... + def stop_face_tracking(self) -> None: ... + +class LuiClient: + def __init__(self) -> None: ... + def start_asr(self) -> None: ... + def stop_asr(self) -> None: ... + def start_tts(self, config: LuiTtsConfig) -> None: ... + def stop_tts(self) -> None: ... + def send_tts_text(self, param: LuiTtsParameter) -> None: ... + +class LightControlClient: + def __init__(self) -> None: ... + def set_led_light_color(self, r: int, g: int, b: int) -> None: ... + def set_led_light_color_param(self, r: int, g: int, b: int) -> None: ... + def stop_led_light_control(self) -> None: ... + +class VisionClient: + def __init__(self) -> None: ... + def start_vision_service( + self, + enable_position: bool, + enable_color: bool, + enable_face_detection: bool, + ) -> None: ... + def stop_vision_service(self) -> None: ... + def get_detection_object_with_ratio( + self, focus_ratio: float + ) -> list[DetectResults]: ... + def get_detection_object(self) -> list[DetectResults]: ... + +class X5CameraClient: + def __init__(self) -> None: ... + def change_mode(self, mode: CameraSetMode) -> None: ... + def get_status(self) -> X5CameraGetStatusResponse: ... diff --git a/booster_sdk_py/src/lib.rs b/booster_sdk_py/src/lib.rs index 91488b1..91619a3 100644 --- a/booster_sdk_py/src/lib.rs +++ b/booster_sdk_py/src/lib.rs @@ -4,11 +4,24 @@ use crate::runtime::wait_for_future; use std::sync::Arc; use ::booster_sdk::{ - client::{BoosterClient, GripperCommand}, - types::{BoosterError, GripperMode, Hand, RobotMode}, + client::{ + AiClient, AsrConfig, BOOSTER_ROBOT_USER_ID, CameraControlStatus, CameraSetMode, + DetectResults, GetStatusResponse as X5CameraStatusResponse, LightControlClient, LlmConfig, + LuiClient, LuiTtsConfig, LuiTtsParameter, SpeakParameter, StartAiChatParameter, TtsConfig, + VisionClient, X5CameraClient, + }, + types::{ + Action, BodyControl, BoosterError, BoosterHandType, CustomModel, CustomModelParams, + CustomTrainedTraj, DanceId, DexterousFingerParameter, Frame, GetModeResponse, + GetRobotInfoResponse, GetStatusResponse, GripperControlMode, GripperMode, + GripperMotionParameter, Hand, HandAction, JointOrder, LoadCustomTrainedTrajResponse, + Orientation, Position, Posture, Quaternion, RobotMode, Transform, WholeBodyDanceId, + }, }; use pyo3::{exceptions::PyException, prelude::*, types::PyModule}; +use ::booster_sdk::client::{BoosterClient, GripperCommand}; + pyo3::create_exception!(booster_sdk_bindings, BoosterSdkError, PyException); fn to_py_err(err: BoosterError) -> PyErr { @@ -21,6 +34,8 @@ pub struct PyRobotMode(RobotMode); #[pymethods] impl PyRobotMode { + #[classattr] + const UNKNOWN: Self = Self(RobotMode::Unknown); #[classattr] const DAMPING: Self = Self(RobotMode::Damping); #[classattr] @@ -34,6 +49,7 @@ impl PyRobotMode { fn __repr__(&self) -> String { match self.0 { + RobotMode::Unknown => "RobotMode.UNKNOWN".to_string(), RobotMode::Damping => "RobotMode.DAMPING".to_string(), RobotMode::Prepare => "RobotMode.PREPARE".to_string(), RobotMode::Walking => "RobotMode.WALKING".to_string(), @@ -54,6 +70,12 @@ impl From for RobotMode { } } +impl From for PyRobotMode { + fn from(mode: RobotMode) -> Self { + Self(mode) + } +} + #[pyclass(module = "booster_sdk_bindings", name = "Hand", eq)] #[derive(Clone, Copy, PartialEq, Eq)] pub struct PyHand(Hand); @@ -83,6 +105,12 @@ impl From for Hand { } } +impl From for PyHand { + fn from(hand: Hand) -> Self { + Self(hand) + } +} + #[pyclass(module = "booster_sdk_bindings", name = "GripperMode", eq)] #[derive(Clone, Copy, PartialEq, Eq)] pub struct PyGripperMode(GripperMode); @@ -112,104 +140,2194 @@ impl From for GripperMode { } } -#[pyclass(module = "booster_sdk_bindings", name = "GripperCommand")] -#[derive(Clone)] -pub struct PyGripperCommand(GripperCommand); +#[pyclass(module = "booster_sdk_bindings", name = "HandAction", eq)] +#[derive(Clone, Copy, PartialEq, Eq)] +pub struct PyHandAction(HandAction); #[pymethods] -impl PyGripperCommand { - #[new] - fn new(hand: PyHand, mode: PyGripperMode, motion_param: u16, speed: Option) -> Self { - Self(GripperCommand { - hand: hand.into(), - mode: mode.into(), - motion_param, - speed: speed.unwrap_or(500), - }) +impl PyHandAction { + #[classattr] + const OPEN: Self = Self(HandAction::Open); + #[classattr] + const CLOSE: Self = Self(HandAction::Close); + + fn __repr__(&self) -> String { + match self.0 { + HandAction::Open => "HandAction.OPEN".to_string(), + HandAction::Close => "HandAction.CLOSE".to_string(), + } } - #[staticmethod] - fn open(hand: PyHand) -> Self { - Self(GripperCommand::open(hand.into())) + fn __int__(&self) -> i32 { + i32::from(self.0) } +} - #[staticmethod] - fn close(hand: PyHand) -> Self { - Self(GripperCommand::close(hand.into())) +impl From for HandAction { + fn from(py_action: PyHandAction) -> Self { + py_action.0 } +} - #[staticmethod] - fn grasp(hand: PyHand, force: u16) -> Self { - Self(GripperCommand::grasp(hand.into(), force)) +#[pyclass(module = "booster_sdk_bindings", name = "Frame", eq)] +#[derive(Clone, Copy, PartialEq, Eq)] +pub struct PyFrame(Frame); + +#[pymethods] +impl PyFrame { + #[classattr] + const UNKNOWN: Self = Self(Frame::Unknown); + #[classattr] + const BODY: Self = Self(Frame::Body); + #[classattr] + const HEAD: Self = Self(Frame::Head); + #[classattr] + const LEFT_HAND: Self = Self(Frame::LeftHand); + #[classattr] + const RIGHT_HAND: Self = Self(Frame::RightHand); + #[classattr] + const LEFT_FOOT: Self = Self(Frame::LeftFoot); + #[classattr] + const RIGHT_FOOT: Self = Self(Frame::RightFoot); + + fn __repr__(&self) -> String { + match self.0 { + Frame::Unknown => "Frame.UNKNOWN".to_string(), + Frame::Body => "Frame.BODY".to_string(), + Frame::Head => "Frame.HEAD".to_string(), + Frame::LeftHand => "Frame.LEFT_HAND".to_string(), + Frame::RightHand => "Frame.RIGHT_HAND".to_string(), + Frame::LeftFoot => "Frame.LEFT_FOOT".to_string(), + Frame::RightFoot => "Frame.RIGHT_FOOT".to_string(), + } + } + + fn __int__(&self) -> i32 { + i32::from(self.0) + } +} + +impl From for Frame { + fn from(value: PyFrame) -> Self { + value.0 } +} + +#[pyclass(module = "booster_sdk_bindings", name = "GripperControlMode", eq)] +#[derive(Clone, Copy, PartialEq, Eq)] +pub struct PyGripperControlMode(GripperControlMode); + +#[pymethods] +impl PyGripperControlMode { + #[classattr] + const POSITION: Self = Self(GripperControlMode::Position); + #[classattr] + const FORCE: Self = Self(GripperControlMode::Force); fn __repr__(&self) -> String { - format!( - "GripperCommand(hand={}, mode={}, motion_param={}, speed={})", - u8::from(self.0.hand), - i32::from(self.0.mode), - self.0.motion_param, - self.0.speed - ) + match self.0 { + GripperControlMode::Position => "GripperControlMode.POSITION".to_string(), + GripperControlMode::Force => "GripperControlMode.FORCE".to_string(), + } + } + + fn __int__(&self) -> i32 { + i32::from(self.0) } } -#[pyclass(module = "booster_sdk_bindings", name = "BoosterClient", unsendable)] -pub struct PyBoosterClient { - client: Arc, +impl From for GripperControlMode { + fn from(value: PyGripperControlMode) -> Self { + value.0 + } } +#[pyclass(module = "booster_sdk_bindings", name = "BoosterHandType", eq)] +#[derive(Clone, Copy, PartialEq, Eq)] +pub struct PyBoosterHandType(BoosterHandType); + #[pymethods] -impl PyBoosterClient { - #[new] - fn new() -> PyResult { - Ok(Self { - client: Arc::new(BoosterClient::new().map_err(to_py_err)?), - }) +impl PyBoosterHandType { + #[classattr] + const INSPIRE_HAND: Self = Self(BoosterHandType::InspireHand); + #[classattr] + const INSPIRE_TOUCH_HAND: Self = Self(BoosterHandType::InspireTouchHand); + #[classattr] + const REVO_HAND: Self = Self(BoosterHandType::RevoHand); + #[classattr] + const UNKNOWN: Self = Self(BoosterHandType::Unknown); + + fn __repr__(&self) -> String { + match self.0 { + BoosterHandType::InspireHand => "BoosterHandType.INSPIRE_HAND".to_string(), + BoosterHandType::InspireTouchHand => "BoosterHandType.INSPIRE_TOUCH_HAND".to_string(), + BoosterHandType::RevoHand => "BoosterHandType.REVO_HAND".to_string(), + BoosterHandType::Unknown => "BoosterHandType.UNKNOWN".to_string(), + } } - fn change_mode(&self, py: Python<'_>, mode: PyRobotMode) -> PyResult<()> { - let client = Arc::clone(&self.client); - wait_for_future(py, async move { client.change_mode(mode.into()).await }).map_err(to_py_err) + fn __int__(&self) -> i32 { + i32::from(self.0) } +} - fn move_robot(&self, py: Python<'_>, vx: f32, vy: f32, vyaw: f32) -> PyResult<()> { - let client = Arc::clone(&self.client); - wait_for_future(py, async move { client.move_robot(vx, vy, vyaw).await }).map_err(to_py_err) +impl From for BoosterHandType { + fn from(value: PyBoosterHandType) -> Self { + value.0 } +} - fn publish_gripper_command(&self, command: PyGripperCommand) -> PyResult<()> { - self.client - .publish_gripper_command(&command.0) - .map_err(to_py_err) +#[pyclass(module = "booster_sdk_bindings", name = "DanceId", eq)] +#[derive(Clone, Copy, PartialEq, Eq)] +pub struct PyDanceId(DanceId); + +#[pymethods] +impl PyDanceId { + #[classattr] + const NEW_YEAR: Self = Self(DanceId::NewYear); + #[classattr] + const NEZHA: Self = Self(DanceId::Nezha); + #[classattr] + const TOWARDS_FUTURE: Self = Self(DanceId::TowardsFuture); + #[classattr] + const DABBING_GESTURE: Self = Self(DanceId::DabbingGesture); + #[classattr] + const ULTRAMAN_GESTURE: Self = Self(DanceId::UltramanGesture); + #[classattr] + const RESPECT_GESTURE: Self = Self(DanceId::RespectGesture); + #[classattr] + const CHEERING_GESTURE: Self = Self(DanceId::CheeringGesture); + #[classattr] + const LUCKY_CAT_GESTURE: Self = Self(DanceId::LuckyCatGesture); + #[classattr] + const STOP: Self = Self(DanceId::Stop); + + fn __repr__(&self) -> String { + match self.0 { + DanceId::NewYear => "DanceId.NEW_YEAR".to_string(), + DanceId::Nezha => "DanceId.NEZHA".to_string(), + DanceId::TowardsFuture => "DanceId.TOWARDS_FUTURE".to_string(), + DanceId::DabbingGesture => "DanceId.DABBING_GESTURE".to_string(), + DanceId::UltramanGesture => "DanceId.ULTRAMAN_GESTURE".to_string(), + DanceId::RespectGesture => "DanceId.RESPECT_GESTURE".to_string(), + DanceId::CheeringGesture => "DanceId.CHEERING_GESTURE".to_string(), + DanceId::LuckyCatGesture => "DanceId.LUCKY_CAT_GESTURE".to_string(), + DanceId::Stop => "DanceId.STOP".to_string(), + } } - fn publish_gripper( - &self, - hand: PyHand, - mode: PyGripperMode, - motion_param: u16, - speed: Option, - ) -> PyResult<()> { - let command = GripperCommand { - hand: hand.into(), - mode: mode.into(), - motion_param, - speed: speed.unwrap_or(500), - }; - self.client - .publish_gripper_command(&command) - .map_err(to_py_err) + fn __int__(&self) -> i32 { + i32::from(self.0) } } -#[pymodule] -fn booster_sdk_bindings(py: Python<'_>, m: &Bound<'_, PyModule>) -> PyResult<()> { - m.add("BoosterSdkError", py.get_type::())?; - m.add_class::()?; - m.add_class::()?; - m.add_class::()?; - m.add_class::()?; - m.add_class::()?; +impl From for DanceId { + fn from(value: PyDanceId) -> Self { + value.0 + } +} + +#[pyclass(module = "booster_sdk_bindings", name = "WholeBodyDanceId", eq)] +#[derive(Clone, Copy, PartialEq, Eq)] +pub struct PyWholeBodyDanceId(WholeBodyDanceId); + +#[pymethods] +impl PyWholeBodyDanceId { + #[classattr] + const ARBIC_DANCE: Self = Self(WholeBodyDanceId::ArbicDance); + #[classattr] + const MICHAEL_DANCE_1: Self = Self(WholeBodyDanceId::MichaelDance1); + #[classattr] + const MICHAEL_DANCE_2: Self = Self(WholeBodyDanceId::MichaelDance2); + #[classattr] + const MICHAEL_DANCE_3: Self = Self(WholeBodyDanceId::MichaelDance3); + #[classattr] + const MOON_WALK: Self = Self(WholeBodyDanceId::MoonWalk); + #[classattr] + const BOXING_STYLE_KICK: Self = Self(WholeBodyDanceId::BoxingStyleKick); + #[classattr] + const ROUNDHOUSE_KICK: Self = Self(WholeBodyDanceId::RoundhouseKick); + + fn __repr__(&self) -> String { + match self.0 { + WholeBodyDanceId::ArbicDance => "WholeBodyDanceId.ARBIC_DANCE".to_string(), + WholeBodyDanceId::MichaelDance1 => "WholeBodyDanceId.MICHAEL_DANCE_1".to_string(), + WholeBodyDanceId::MichaelDance2 => "WholeBodyDanceId.MICHAEL_DANCE_2".to_string(), + WholeBodyDanceId::MichaelDance3 => "WholeBodyDanceId.MICHAEL_DANCE_3".to_string(), + WholeBodyDanceId::MoonWalk => "WholeBodyDanceId.MOON_WALK".to_string(), + WholeBodyDanceId::BoxingStyleKick => "WholeBodyDanceId.BOXING_STYLE_KICK".to_string(), + WholeBodyDanceId::RoundhouseKick => "WholeBodyDanceId.ROUNDHOUSE_KICK".to_string(), + } + } + + fn __int__(&self) -> i32 { + i32::from(self.0) + } +} + +impl From for WholeBodyDanceId { + fn from(value: PyWholeBodyDanceId) -> Self { + value.0 + } +} + +#[pyclass(module = "booster_sdk_bindings", name = "JointOrder", eq)] +#[derive(Clone, Copy, PartialEq, Eq)] +pub struct PyJointOrder(JointOrder); + +#[pymethods] +impl PyJointOrder { + #[classattr] + const MUJOCO: Self = Self(JointOrder::MuJoCo); + #[classattr] + const ISAAC_LAB: Self = Self(JointOrder::IsaacLab); + + fn __repr__(&self) -> String { + match self.0 { + JointOrder::MuJoCo => "JointOrder.MUJOCO".to_string(), + JointOrder::IsaacLab => "JointOrder.ISAAC_LAB".to_string(), + } + } + + fn __int__(&self) -> i32 { + i32::from(self.0) + } +} + +impl From for JointOrder { + fn from(value: PyJointOrder) -> Self { + value.0 + } +} + +#[pyclass(module = "booster_sdk_bindings", name = "BodyControl", eq)] +#[derive(Clone, Copy, PartialEq, Eq)] +pub struct PyBodyControl(BodyControl); + +#[pymethods] +impl PyBodyControl { + #[classattr] + const UNKNOWN: Self = Self(BodyControl::Unknown); + #[classattr] + const DAMPING: Self = Self(BodyControl::Damping); + #[classattr] + const PREPARE: Self = Self(BodyControl::Prepare); + #[classattr] + const HUMANLIKE_GAIT: Self = Self(BodyControl::HumanlikeGait); + #[classattr] + const PRONE_BODY: Self = Self(BodyControl::ProneBody); + #[classattr] + const SOCCER_GAIT: Self = Self(BodyControl::SoccerGait); + #[classattr] + const CUSTOM: Self = Self(BodyControl::Custom); + #[classattr] + const GET_UP: Self = Self(BodyControl::GetUp); + #[classattr] + const WHOLE_BODY_DANCE: Self = Self(BodyControl::WholeBodyDance); + #[classattr] + const SHOOT: Self = Self(BodyControl::Shoot); + #[classattr] + const INSIDE_FOOT: Self = Self(BodyControl::InsideFoot); + #[classattr] + const GOALIE: Self = Self(BodyControl::Goalie); + #[classattr] + const WBC_GAIT: Self = Self(BodyControl::WbcGait); + + fn __repr__(&self) -> String { + match self.0 { + BodyControl::Unknown => "BodyControl.UNKNOWN".to_string(), + BodyControl::Damping => "BodyControl.DAMPING".to_string(), + BodyControl::Prepare => "BodyControl.PREPARE".to_string(), + BodyControl::HumanlikeGait => "BodyControl.HUMANLIKE_GAIT".to_string(), + BodyControl::ProneBody => "BodyControl.PRONE_BODY".to_string(), + BodyControl::SoccerGait => "BodyControl.SOCCER_GAIT".to_string(), + BodyControl::Custom => "BodyControl.CUSTOM".to_string(), + BodyControl::GetUp => "BodyControl.GET_UP".to_string(), + BodyControl::WholeBodyDance => "BodyControl.WHOLE_BODY_DANCE".to_string(), + BodyControl::Shoot => "BodyControl.SHOOT".to_string(), + BodyControl::InsideFoot => "BodyControl.INSIDE_FOOT".to_string(), + BodyControl::Goalie => "BodyControl.GOALIE".to_string(), + BodyControl::WbcGait => "BodyControl.WBC_GAIT".to_string(), + } + } + + fn __int__(&self) -> i32 { + i32::from(self.0) + } +} + +impl From for PyBodyControl { + fn from(value: BodyControl) -> Self { + Self(value) + } +} + +#[pyclass(module = "booster_sdk_bindings", name = "Action", eq)] +#[derive(Clone, Copy, PartialEq, Eq)] +pub struct PyAction(Action); + +#[pymethods] +impl PyAction { + #[classattr] + const UNKNOWN: Self = Self(Action::Unknown); + #[classattr] + const HAND_SHAKE: Self = Self(Action::HandShake); + #[classattr] + const HAND_WAVE: Self = Self(Action::HandWave); + #[classattr] + const HAND_CONTROL: Self = Self(Action::HandControl); + #[classattr] + const DANCE_NEW_YEAR: Self = Self(Action::DanceNewYear); + #[classattr] + const DANCE_NEZHA: Self = Self(Action::DanceNezha); + #[classattr] + const DANCE_TOWARDS_FUTURE: Self = Self(Action::DanceTowardsFuture); + #[classattr] + const GESTURE_DABBING: Self = Self(Action::GestureDabbing); + #[classattr] + const GESTURE_ULTRAMAN: Self = Self(Action::GestureUltraman); + #[classattr] + const GESTURE_RESPECT: Self = Self(Action::GestureRespect); + #[classattr] + const GESTURE_CHEER: Self = Self(Action::GestureCheer); + #[classattr] + const GESTURE_LUCKY_CAT: Self = Self(Action::GestureLuckyCat); + #[classattr] + const GESTURE_BOXING: Self = Self(Action::GestureBoxing); + #[classattr] + const ZERO_TORQUE_DRAG: Self = Self(Action::ZeroTorqueDrag); + #[classattr] + const RECORD_TRAJ: Self = Self(Action::RecordTraj); + #[classattr] + const RUN_RECORDED_TRAJ: Self = Self(Action::RunRecordedTraj); + + fn __repr__(&self) -> String { + match self.0 { + Action::Unknown => "Action.UNKNOWN".to_string(), + Action::HandShake => "Action.HAND_SHAKE".to_string(), + Action::HandWave => "Action.HAND_WAVE".to_string(), + Action::HandControl => "Action.HAND_CONTROL".to_string(), + Action::DanceNewYear => "Action.DANCE_NEW_YEAR".to_string(), + Action::DanceNezha => "Action.DANCE_NEZHA".to_string(), + Action::DanceTowardsFuture => "Action.DANCE_TOWARDS_FUTURE".to_string(), + Action::GestureDabbing => "Action.GESTURE_DABBING".to_string(), + Action::GestureUltraman => "Action.GESTURE_ULTRAMAN".to_string(), + Action::GestureRespect => "Action.GESTURE_RESPECT".to_string(), + Action::GestureCheer => "Action.GESTURE_CHEER".to_string(), + Action::GestureLuckyCat => "Action.GESTURE_LUCKY_CAT".to_string(), + Action::GestureBoxing => "Action.GESTURE_BOXING".to_string(), + Action::ZeroTorqueDrag => "Action.ZERO_TORQUE_DRAG".to_string(), + Action::RecordTraj => "Action.RECORD_TRAJ".to_string(), + Action::RunRecordedTraj => "Action.RUN_RECORDED_TRAJ".to_string(), + } + } + + fn __int__(&self) -> i32 { + i32::from(self.0) + } +} + +impl From for PyAction { + fn from(value: Action) -> Self { + Self(value) + } +} + +#[pyclass(module = "booster_sdk_bindings", name = "CameraSetMode", eq)] +#[derive(Clone, Copy, PartialEq, Eq)] +pub struct PyCameraSetMode(CameraSetMode); + +#[pymethods] +impl PyCameraSetMode { + #[classattr] + const CAMERA_MODE_NORMAL: Self = Self(CameraSetMode::CameraModeNormal); + #[classattr] + const CAMERA_MODE_HIGH_RESOLUTION: Self = Self(CameraSetMode::CameraModeHighResolution); + #[classattr] + const CAMERA_MODE_NORMAL_ENABLE: Self = Self(CameraSetMode::CameraModeNormalEnable); + #[classattr] + const CAMERA_MODE_HIGH_RESOLUTION_ENABLE: Self = + Self(CameraSetMode::CameraModeHighResolutionEnable); + + fn __repr__(&self) -> String { + match self.0 { + CameraSetMode::CameraModeNormal => "CameraSetMode.CAMERA_MODE_NORMAL".to_string(), + CameraSetMode::CameraModeHighResolution => { + "CameraSetMode.CAMERA_MODE_HIGH_RESOLUTION".to_string() + } + CameraSetMode::CameraModeNormalEnable => { + "CameraSetMode.CAMERA_MODE_NORMAL_ENABLE".to_string() + } + CameraSetMode::CameraModeHighResolutionEnable => { + "CameraSetMode.CAMERA_MODE_HIGH_RESOLUTION_ENABLE".to_string() + } + } + } + + fn __int__(&self) -> i32 { + i32::from(self.0) + } +} + +impl From for CameraSetMode { + fn from(value: PyCameraSetMode) -> Self { + value.0 + } +} + +#[pyclass(module = "booster_sdk_bindings", name = "CameraControlStatus", eq)] +#[derive(Clone, Copy, PartialEq, Eq)] +pub struct PyCameraControlStatus(CameraControlStatus); + +#[pymethods] +impl PyCameraControlStatus { + #[classattr] + const CAMERA_STATUS_NORMAL: Self = Self(CameraControlStatus::CameraStatusNormal); + #[classattr] + const CAMERA_STATUS_HIGH_RESOLUTION: Self = + Self(CameraControlStatus::CameraStatusHighResolution); + #[classattr] + const CAMERA_STATUS_ERROR: Self = Self(CameraControlStatus::CameraStatusError); + #[classattr] + const CAMERA_STATUS_NULL: Self = Self(CameraControlStatus::CameraStatusNull); + + fn __repr__(&self) -> String { + match self.0 { + CameraControlStatus::CameraStatusNormal => { + "CameraControlStatus.CAMERA_STATUS_NORMAL".to_string() + } + CameraControlStatus::CameraStatusHighResolution => { + "CameraControlStatus.CAMERA_STATUS_HIGH_RESOLUTION".to_string() + } + CameraControlStatus::CameraStatusError => { + "CameraControlStatus.CAMERA_STATUS_ERROR".to_string() + } + CameraControlStatus::CameraStatusNull => { + "CameraControlStatus.CAMERA_STATUS_NULL".to_string() + } + } + } + + fn __int__(&self) -> i32 { + i32::from(self.0) + } +} + +impl From for PyCameraControlStatus { + fn from(value: CameraControlStatus) -> Self { + Self(value) + } +} + +#[pyclass(module = "booster_sdk_bindings", name = "GripperCommand")] +#[derive(Clone)] +pub struct PyGripperCommand(GripperCommand); + +#[pymethods] +impl PyGripperCommand { + #[new] + fn new(hand: PyHand, mode: PyGripperMode, motion_param: u16, speed: Option) -> Self { + Self(GripperCommand { + hand: hand.into(), + mode: mode.into(), + motion_param, + speed: speed.unwrap_or(500), + }) + } + + #[staticmethod] + fn open(hand: PyHand) -> Self { + Self(GripperCommand::open(hand.into())) + } + + #[staticmethod] + fn close(hand: PyHand) -> Self { + Self(GripperCommand::close(hand.into())) + } + + #[staticmethod] + fn grasp(hand: PyHand, force: u16) -> Self { + Self(GripperCommand::grasp(hand.into(), force)) + } + + #[getter] + fn hand(&self) -> PyHand { + self.0.hand.into() + } + + #[getter] + fn mode(&self) -> PyGripperMode { + PyGripperMode(self.0.mode) + } + + #[getter] + fn motion_param(&self) -> u16 { + self.0.motion_param + } + + #[getter] + fn speed(&self) -> u16 { + self.0.speed + } + + fn __repr__(&self) -> String { + format!( + "GripperCommand(hand={}, mode={}, motion_param={}, speed={})", + u8::from(self.0.hand), + i32::from(self.0.mode), + self.0.motion_param, + self.0.speed + ) + } +} + +#[pyclass(module = "booster_sdk_bindings", name = "Position")] +#[derive(Clone, Copy)] +pub struct PyPosition(Position); + +#[pymethods] +impl PyPosition { + #[new] + fn new(x: f32, y: f32, z: f32) -> Self { + Self(Position { x, y, z }) + } + + #[getter] + fn x(&self) -> f32 { + self.0.x + } + + #[setter] + fn set_x(&mut self, value: f32) { + self.0.x = value; + } + + #[getter] + fn y(&self) -> f32 { + self.0.y + } + + #[setter] + fn set_y(&mut self, value: f32) { + self.0.y = value; + } + + #[getter] + fn z(&self) -> f32 { + self.0.z + } + + #[setter] + fn set_z(&mut self, value: f32) { + self.0.z = value; + } + + fn __repr__(&self) -> String { + format!("Position(x={}, y={}, z={})", self.0.x, self.0.y, self.0.z) + } +} + +impl From for Position { + fn from(value: PyPosition) -> Self { + value.0 + } +} + +impl From for PyPosition { + fn from(value: Position) -> Self { + Self(value) + } +} + +#[pyclass(module = "booster_sdk_bindings", name = "Orientation")] +#[derive(Clone, Copy)] +pub struct PyOrientation(Orientation); + +#[pymethods] +impl PyOrientation { + #[new] + fn new(roll: f32, pitch: f32, yaw: f32) -> Self { + Self(Orientation { roll, pitch, yaw }) + } + + #[getter] + fn roll(&self) -> f32 { + self.0.roll + } + + #[setter] + fn set_roll(&mut self, value: f32) { + self.0.roll = value; + } + + #[getter] + fn pitch(&self) -> f32 { + self.0.pitch + } + + #[setter] + fn set_pitch(&mut self, value: f32) { + self.0.pitch = value; + } + + #[getter] + fn yaw(&self) -> f32 { + self.0.yaw + } + + #[setter] + fn set_yaw(&mut self, value: f32) { + self.0.yaw = value; + } + + fn __repr__(&self) -> String { + format!( + "Orientation(roll={}, pitch={}, yaw={})", + self.0.roll, self.0.pitch, self.0.yaw + ) + } +} + +impl From for Orientation { + fn from(value: PyOrientation) -> Self { + value.0 + } +} + +impl From for PyOrientation { + fn from(value: Orientation) -> Self { + Self(value) + } +} + +#[pyclass(module = "booster_sdk_bindings", name = "Posture")] +#[derive(Clone, Copy)] +pub struct PyPosture(Posture); + +#[pymethods] +impl PyPosture { + #[new] + fn new(position: PyPosition, orientation: PyOrientation) -> Self { + Self(Posture { + position: position.into(), + orientation: orientation.into(), + }) + } + + #[getter] + fn position(&self) -> PyPosition { + self.0.position.into() + } + + #[setter] + fn set_position(&mut self, value: PyPosition) { + self.0.position = value.into(); + } + + #[getter] + fn orientation(&self) -> PyOrientation { + self.0.orientation.into() + } + + #[setter] + fn set_orientation(&mut self, value: PyOrientation) { + self.0.orientation = value.into(); + } + + fn __repr__(&self) -> String { + format!( + "Posture(position={}, orientation={})", + self.position().__repr__(), + self.orientation().__repr__() + ) + } +} + +impl From for Posture { + fn from(value: PyPosture) -> Self { + value.0 + } +} + +impl From for PyPosture { + fn from(value: Posture) -> Self { + Self(value) + } +} + +#[pyclass(module = "booster_sdk_bindings", name = "Quaternion")] +#[derive(Clone, Copy)] +pub struct PyQuaternion(Quaternion); + +#[pymethods] +impl PyQuaternion { + #[new] + fn new(x: f32, y: f32, z: f32, w: f32) -> Self { + Self(Quaternion { x, y, z, w }) + } + + #[getter] + fn x(&self) -> f32 { + self.0.x + } + + #[getter] + fn y(&self) -> f32 { + self.0.y + } + + #[getter] + fn z(&self) -> f32 { + self.0.z + } + + #[getter] + fn w(&self) -> f32 { + self.0.w + } + + fn __repr__(&self) -> String { + format!( + "Quaternion(x={}, y={}, z={}, w={})", + self.0.x, self.0.y, self.0.z, self.0.w + ) + } +} + +impl From for PyQuaternion { + fn from(value: Quaternion) -> Self { + Self(value) + } +} + +#[pyclass(module = "booster_sdk_bindings", name = "Transform")] +#[derive(Clone, Copy)] +pub struct PyTransform(Transform); + +#[pymethods] +impl PyTransform { + #[new] + fn new(position: PyPosition, orientation: PyQuaternion) -> Self { + Self(Transform { + position: position.into(), + orientation: orientation.0, + }) + } + + #[getter] + fn position(&self) -> PyPosition { + self.0.position.into() + } + + #[getter] + fn orientation(&self) -> PyQuaternion { + self.0.orientation.into() + } + + fn __repr__(&self) -> String { + format!( + "Transform(position={}, orientation={})", + self.position().__repr__(), + self.orientation().__repr__() + ) + } +} + +impl From for PyTransform { + fn from(value: Transform) -> Self { + Self(value) + } +} + +#[pyclass(module = "booster_sdk_bindings", name = "GripperMotionParameter")] +#[derive(Clone, Copy)] +pub struct PyGripperMotionParameter(GripperMotionParameter); + +#[pymethods] +impl PyGripperMotionParameter { + #[new] + fn new(position: i32, force: i32, speed: i32) -> Self { + Self(GripperMotionParameter { + position, + force, + speed, + }) + } + + #[getter] + fn position(&self) -> i32 { + self.0.position + } + + #[getter] + fn force(&self) -> i32 { + self.0.force + } + + #[getter] + fn speed(&self) -> i32 { + self.0.speed + } + + fn __repr__(&self) -> String { + format!( + "GripperMotionParameter(position={}, force={}, speed={})", + self.0.position, self.0.force, self.0.speed + ) + } +} + +impl From for GripperMotionParameter { + fn from(value: PyGripperMotionParameter) -> Self { + value.0 + } +} + +#[pyclass(module = "booster_sdk_bindings", name = "DexterousFingerParameter")] +#[derive(Clone, Copy)] +pub struct PyDexterousFingerParameter(DexterousFingerParameter); + +#[pymethods] +impl PyDexterousFingerParameter { + #[new] + fn new(seq: i32, angle: i32, force: i32, speed: i32) -> Self { + Self(DexterousFingerParameter { + seq, + angle, + force, + speed, + }) + } + + #[getter] + fn seq(&self) -> i32 { + self.0.seq + } + + #[getter] + fn angle(&self) -> i32 { + self.0.angle + } + + #[getter] + fn force(&self) -> i32 { + self.0.force + } + + #[getter] + fn speed(&self) -> i32 { + self.0.speed + } + + fn __repr__(&self) -> String { + format!( + "DexterousFingerParameter(seq={}, angle={}, force={}, speed={})", + self.0.seq, self.0.angle, self.0.force, self.0.speed + ) + } +} + +impl From for DexterousFingerParameter { + fn from(value: PyDexterousFingerParameter) -> Self { + value.0 + } +} + +impl From for PyDexterousFingerParameter { + fn from(value: DexterousFingerParameter) -> Self { + Self(value) + } +} + +#[pyclass(module = "booster_sdk_bindings", name = "CustomModelParams")] +#[derive(Clone)] +pub struct PyCustomModelParams(CustomModelParams); + +#[pymethods] +impl PyCustomModelParams { + #[new] + fn new(action_scale: Vec, kp: Vec, kd: Vec) -> Self { + Self(CustomModelParams { + action_scale, + kp, + kd, + }) + } + + #[getter] + fn action_scale(&self) -> Vec { + self.0.action_scale.clone() + } + + #[getter] + fn kp(&self) -> Vec { + self.0.kp.clone() + } + + #[getter] + fn kd(&self) -> Vec { + self.0.kd.clone() + } + + fn __repr__(&self) -> String { + "CustomModelParams(...)".to_string() + } +} + +impl From for CustomModelParams { + fn from(value: PyCustomModelParams) -> Self { + value.0 + } +} + +impl From for PyCustomModelParams { + fn from(value: CustomModelParams) -> Self { + Self(value) + } +} + +#[pyclass(module = "booster_sdk_bindings", name = "CustomModel")] +#[derive(Clone)] +pub struct PyCustomModel(CustomModel); + +#[pymethods] +impl PyCustomModel { + #[new] + fn new(file_path: String, params: Vec, joint_order: PyJointOrder) -> Self { + Self(CustomModel { + file_path, + params: params.into_iter().map(Into::into).collect(), + joint_order: joint_order.into(), + }) + } + + #[getter] + fn file_path(&self) -> String { + self.0.file_path.clone() + } + + #[getter] + fn params(&self) -> Vec { + self.0.params.clone().into_iter().map(Into::into).collect() + } + + #[getter] + fn joint_order(&self) -> PyJointOrder { + PyJointOrder(self.0.joint_order) + } + + fn __repr__(&self) -> String { + format!("CustomModel(file_path='{}', params=...)", self.0.file_path) + } +} + +impl From for CustomModel { + fn from(value: PyCustomModel) -> Self { + value.0 + } +} + +impl From for PyCustomModel { + fn from(value: CustomModel) -> Self { + Self(value) + } +} + +#[pyclass(module = "booster_sdk_bindings", name = "CustomTrainedTraj")] +#[derive(Clone)] +pub struct PyCustomTrainedTraj(CustomTrainedTraj); + +#[pymethods] +impl PyCustomTrainedTraj { + #[new] + fn new(traj_file_path: String, model: PyCustomModel) -> Self { + Self(CustomTrainedTraj { + traj_file_path, + model: model.into(), + }) + } + + #[getter] + fn traj_file_path(&self) -> String { + self.0.traj_file_path.clone() + } + + #[getter] + fn model(&self) -> PyCustomModel { + self.0.model.clone().into() + } + + fn __repr__(&self) -> String { + format!( + "CustomTrainedTraj(traj_file_path='{}', model=...)", + self.0.traj_file_path + ) + } +} + +impl From for CustomTrainedTraj { + fn from(value: PyCustomTrainedTraj) -> Self { + value.0 + } +} + +#[pyclass(module = "booster_sdk_bindings", name = "TtsConfig")] +#[derive(Clone)] +pub struct PyTtsConfig(TtsConfig); + +#[pymethods] +impl PyTtsConfig { + #[new] + fn new(voice_type: String, ignore_bracket_text: Vec) -> Self { + Self(TtsConfig { + voice_type, + ignore_bracket_text, + }) + } + + #[getter] + fn voice_type(&self) -> String { + self.0.voice_type.clone() + } + + #[getter] + fn ignore_bracket_text(&self) -> Vec { + self.0.ignore_bracket_text.clone() + } +} + +impl From for TtsConfig { + fn from(value: PyTtsConfig) -> Self { + value.0 + } +} + +#[pyclass(module = "booster_sdk_bindings", name = "LlmConfig")] +#[derive(Clone)] +pub struct PyLlmConfig(LlmConfig); + +#[pymethods] +impl PyLlmConfig { + #[new] + fn new(system_prompt: String, welcome_msg: String, prompt_name: String) -> Self { + Self(LlmConfig { + system_prompt, + welcome_msg, + prompt_name, + }) + } + + #[getter] + fn system_prompt(&self) -> String { + self.0.system_prompt.clone() + } + + #[getter] + fn welcome_msg(&self) -> String { + self.0.welcome_msg.clone() + } + + #[getter] + fn prompt_name(&self) -> String { + self.0.prompt_name.clone() + } +} + +impl From for LlmConfig { + fn from(value: PyLlmConfig) -> Self { + value.0 + } +} + +#[pyclass(module = "booster_sdk_bindings", name = "AsrConfig")] +#[derive(Clone)] +pub struct PyAsrConfig(AsrConfig); + +#[pymethods] +impl PyAsrConfig { + #[new] + fn new(interrupt_speech_duration: i32, interrupt_keywords: Vec) -> Self { + Self(AsrConfig { + interrupt_speech_duration, + interrupt_keywords, + }) + } + + #[getter] + fn interrupt_speech_duration(&self) -> i32 { + self.0.interrupt_speech_duration + } + + #[getter] + fn interrupt_keywords(&self) -> Vec { + self.0.interrupt_keywords.clone() + } +} + +impl From for AsrConfig { + fn from(value: PyAsrConfig) -> Self { + value.0 + } +} + +#[pyclass(module = "booster_sdk_bindings", name = "StartAiChatParameter")] +#[derive(Clone)] +pub struct PyStartAiChatParameter(StartAiChatParameter); + +#[pymethods] +impl PyStartAiChatParameter { + #[new] + fn new( + interrupt_mode: bool, + asr_config: PyAsrConfig, + llm_config: PyLlmConfig, + tts_config: PyTtsConfig, + enable_face_tracking: bool, + ) -> Self { + Self(StartAiChatParameter { + interrupt_mode, + asr_config: asr_config.into(), + llm_config: llm_config.into(), + tts_config: tts_config.into(), + enable_face_tracking, + }) + } + + #[getter] + fn interrupt_mode(&self) -> bool { + self.0.interrupt_mode + } + + #[getter] + fn asr_config(&self) -> PyAsrConfig { + PyAsrConfig(self.0.asr_config.clone()) + } + + #[getter] + fn llm_config(&self) -> PyLlmConfig { + PyLlmConfig(self.0.llm_config.clone()) + } + + #[getter] + fn tts_config(&self) -> PyTtsConfig { + PyTtsConfig(self.0.tts_config.clone()) + } + + #[getter] + fn enable_face_tracking(&self) -> bool { + self.0.enable_face_tracking + } +} + +impl From for StartAiChatParameter { + fn from(value: PyStartAiChatParameter) -> Self { + value.0 + } +} + +#[pyclass(module = "booster_sdk_bindings", name = "SpeakParameter")] +#[derive(Clone)] +pub struct PySpeakParameter(SpeakParameter); + +#[pymethods] +impl PySpeakParameter { + #[new] + fn new(msg: String) -> Self { + Self(SpeakParameter { msg }) + } + + #[getter] + fn msg(&self) -> String { + self.0.msg.clone() + } +} + +impl From for SpeakParameter { + fn from(value: PySpeakParameter) -> Self { + value.0 + } +} + +#[pyclass(module = "booster_sdk_bindings", name = "LuiTtsConfig")] +#[derive(Clone)] +pub struct PyLuiTtsConfig(LuiTtsConfig); + +#[pymethods] +impl PyLuiTtsConfig { + #[new] + fn new(voice_type: String) -> Self { + Self(LuiTtsConfig { voice_type }) + } + + #[getter] + fn voice_type(&self) -> String { + self.0.voice_type.clone() + } +} + +impl From for LuiTtsConfig { + fn from(value: PyLuiTtsConfig) -> Self { + value.0 + } +} + +#[pyclass(module = "booster_sdk_bindings", name = "LuiTtsParameter")] +#[derive(Clone)] +pub struct PyLuiTtsParameter(LuiTtsParameter); + +#[pymethods] +impl PyLuiTtsParameter { + #[new] + fn new(text: String) -> Self { + Self(LuiTtsParameter { text }) + } + + #[getter] + fn text(&self) -> String { + self.0.text.clone() + } +} + +impl From for LuiTtsParameter { + fn from(value: PyLuiTtsParameter) -> Self { + value.0 + } +} + +#[pyclass(module = "booster_sdk_bindings", name = "GetModeResponse")] +#[derive(Clone)] +pub struct PyGetModeResponse(GetModeResponse); + +#[pymethods] +impl PyGetModeResponse { + #[new] + fn new(mode: i32) -> Self { + Self(GetModeResponse { mode }) + } + + #[getter] + fn mode(&self) -> i32 { + self.0.mode + } + + fn mode_enum(&self) -> Option { + self.0.mode_enum().map(Into::into) + } + + fn __repr__(&self) -> String { + format!("GetModeResponse(mode={})", self.0.mode) + } +} + +impl From for PyGetModeResponse { + fn from(value: GetModeResponse) -> Self { + Self(value) + } +} + +#[pyclass(module = "booster_sdk_bindings", name = "GetStatusResponse")] +#[derive(Clone)] +pub struct PyGetStatusResponse(GetStatusResponse); + +#[pymethods] +impl PyGetStatusResponse { + #[new] + fn new(current_mode: i32, current_body_control: i32, current_actions: Vec) -> Self { + Self(GetStatusResponse { + current_mode, + current_body_control, + current_actions, + }) + } + + #[getter] + fn current_mode(&self) -> i32 { + self.0.current_mode + } + + #[getter] + fn current_body_control(&self) -> i32 { + self.0.current_body_control + } + + #[getter] + fn current_actions(&self) -> Vec { + self.0.current_actions.clone() + } + + fn current_mode_enum(&self) -> Option { + self.0.current_mode_enum().map(Into::into) + } + + fn current_body_control_enum(&self) -> Option { + self.0.current_body_control_enum().map(Into::into) + } + + fn current_actions_enum(&self) -> Vec { + self.0 + .current_actions_enum() + .into_iter() + .map(Into::into) + .collect() + } + + fn __repr__(&self) -> String { + format!( + "GetStatusResponse(current_mode={}, current_body_control={}, current_actions={:?})", + self.0.current_mode, self.0.current_body_control, self.0.current_actions + ) + } +} + +impl From for PyGetStatusResponse { + fn from(value: GetStatusResponse) -> Self { + Self(value) + } +} + +#[pyclass(module = "booster_sdk_bindings", name = "GetRobotInfoResponse")] +#[derive(Clone)] +pub struct PyGetRobotInfoResponse(GetRobotInfoResponse); + +#[pymethods] +impl PyGetRobotInfoResponse { + #[new] + fn new( + name: String, + nickname: String, + version: String, + model: String, + serial_number: String, + ) -> Self { + Self(GetRobotInfoResponse { + name, + nickname, + version, + model, + serial_number, + }) + } + + #[getter] + fn name(&self) -> String { + self.0.name.clone() + } + + #[getter] + fn nickname(&self) -> String { + self.0.nickname.clone() + } + + #[getter] + fn version(&self) -> String { + self.0.version.clone() + } + + #[getter] + fn model(&self) -> String { + self.0.model.clone() + } + + #[getter] + fn serial_number(&self) -> String { + self.0.serial_number.clone() + } + + fn __repr__(&self) -> String { + format!( + "GetRobotInfoResponse(name='{}', nickname='{}', version='{}', model='{}', serial_number='{}')", + self.0.name, self.0.nickname, self.0.version, self.0.model, self.0.serial_number + ) + } +} + +impl From for PyGetRobotInfoResponse { + fn from(value: GetRobotInfoResponse) -> Self { + Self(value) + } +} + +#[pyclass( + module = "booster_sdk_bindings", + name = "LoadCustomTrainedTrajResponse" +)] +#[derive(Clone)] +pub struct PyLoadCustomTrainedTrajResponse(LoadCustomTrainedTrajResponse); + +#[pymethods] +impl PyLoadCustomTrainedTrajResponse { + #[new] + fn new(tid: String) -> Self { + Self(LoadCustomTrainedTrajResponse { tid }) + } + + #[getter] + fn tid(&self) -> String { + self.0.tid.clone() + } + + fn __repr__(&self) -> String { + format!("LoadCustomTrainedTrajResponse(tid='{}')", self.0.tid) + } +} + +impl From for PyLoadCustomTrainedTrajResponse { + fn from(value: LoadCustomTrainedTrajResponse) -> Self { + Self(value) + } +} + +#[pyclass(module = "booster_sdk_bindings", name = "X5CameraGetStatusResponse")] +#[derive(Clone)] +pub struct PyX5CameraGetStatusResponse(X5CameraStatusResponse); + +#[pymethods] +impl PyX5CameraGetStatusResponse { + #[new] + fn new(status: i32) -> Self { + Self(X5CameraStatusResponse { status }) + } + + #[getter] + fn status(&self) -> i32 { + self.0.status + } + + fn status_enum(&self) -> Option { + self.0.status_enum().map(Into::into) + } + + fn __repr__(&self) -> String { + format!("X5CameraGetStatusResponse(status={})", self.0.status) + } +} + +impl From for PyX5CameraGetStatusResponse { + fn from(value: X5CameraStatusResponse) -> Self { + Self(value) + } +} + +#[pyclass(module = "booster_sdk_bindings", name = "DetectResults")] +#[derive(Clone)] +pub struct PyDetectResults(DetectResults); + +#[pymethods] +impl PyDetectResults { + #[new] + fn new( + xmin: i64, + ymin: i64, + xmax: i64, + ymax: i64, + position: Vec, + tag: String, + conf: f32, + rgb_mean: Vec, + ) -> Self { + Self(DetectResults { + xmin, + ymin, + xmax, + ymax, + position, + tag, + conf, + rgb_mean, + }) + } + + #[getter] + fn xmin(&self) -> i64 { + self.0.xmin + } + + #[getter] + fn ymin(&self) -> i64 { + self.0.ymin + } + + #[getter] + fn xmax(&self) -> i64 { + self.0.xmax + } + + #[getter] + fn ymax(&self) -> i64 { + self.0.ymax + } + + #[getter] + fn position(&self) -> Vec { + self.0.position.clone() + } + + #[getter] + fn tag(&self) -> String { + self.0.tag.clone() + } + + #[getter] + fn conf(&self) -> f32 { + self.0.conf + } + + #[getter] + fn rgb_mean(&self) -> Vec { + self.0.rgb_mean.clone() + } + + fn __repr__(&self) -> String { + format!( + "DetectResults(tag='{}', conf={}, bbox=({}, {}, {}, {}))", + self.0.tag, self.0.conf, self.0.xmin, self.0.ymin, self.0.xmax, self.0.ymax + ) + } +} + +impl From for PyDetectResults { + fn from(value: DetectResults) -> Self { + Self(value) + } +} + +#[pyclass(module = "booster_sdk_bindings", name = "BoosterClient", unsendable)] +pub struct PyBoosterClient { + client: Arc, +} + +#[pymethods] +impl PyBoosterClient { + #[new] + fn new() -> PyResult { + Ok(Self { + client: Arc::new(BoosterClient::new().map_err(to_py_err)?), + }) + } + + fn change_mode(&self, py: Python<'_>, mode: PyRobotMode) -> PyResult<()> { + let client = Arc::clone(&self.client); + wait_for_future(py, async move { client.change_mode(mode.into()).await }).map_err(to_py_err) + } + + fn get_mode(&self, py: Python<'_>) -> PyResult { + let client = Arc::clone(&self.client); + wait_for_future(py, async move { client.get_mode().await }) + .map(Into::into) + .map_err(to_py_err) + } + + fn get_status(&self, py: Python<'_>) -> PyResult { + let client = Arc::clone(&self.client); + wait_for_future(py, async move { client.get_status().await }) + .map(Into::into) + .map_err(to_py_err) + } + + fn get_robot_info(&self, py: Python<'_>) -> PyResult { + let client = Arc::clone(&self.client); + wait_for_future(py, async move { client.get_robot_info().await }) + .map(Into::into) + .map_err(to_py_err) + } + + fn move_robot(&self, py: Python<'_>, vx: f32, vy: f32, vyaw: f32) -> PyResult<()> { + let client = Arc::clone(&self.client); + wait_for_future(py, async move { client.move_robot(vx, vy, vyaw).await }).map_err(to_py_err) + } + + fn rotate_head(&self, py: Python<'_>, pitch: f32, yaw: f32) -> PyResult<()> { + let client = Arc::clone(&self.client); + wait_for_future(py, async move { client.rotate_head(pitch, yaw).await }).map_err(to_py_err) + } + + fn wave_hand(&self, py: Python<'_>, action: PyHandAction) -> PyResult<()> { + let client = Arc::clone(&self.client); + wait_for_future(py, async move { client.wave_hand(action.into()).await }).map_err(to_py_err) + } + + fn rotate_head_with_direction( + &self, + py: Python<'_>, + pitch_direction: i32, + yaw_direction: i32, + ) -> PyResult<()> { + let client = Arc::clone(&self.client); + wait_for_future(py, async move { + client + .rotate_head_with_direction(pitch_direction, yaw_direction) + .await + }) + .map_err(to_py_err) + } + + fn lie_down(&self, py: Python<'_>) -> PyResult<()> { + let client = Arc::clone(&self.client); + wait_for_future(py, async move { client.lie_down().await }).map_err(to_py_err) + } + + fn get_up(&self, py: Python<'_>) -> PyResult<()> { + let client = Arc::clone(&self.client); + wait_for_future(py, async move { client.get_up().await }).map_err(to_py_err) + } + + fn get_up_with_mode(&self, py: Python<'_>, mode: PyRobotMode) -> PyResult<()> { + let client = Arc::clone(&self.client); + wait_for_future( + py, + async move { client.get_up_with_mode(mode.into()).await }, + ) + .map_err(to_py_err) + } + + fn shoot(&self, py: Python<'_>) -> PyResult<()> { + let client = Arc::clone(&self.client); + wait_for_future(py, async move { client.shoot().await }).map_err(to_py_err) + } + + fn push_up(&self, py: Python<'_>) -> PyResult<()> { + let client = Arc::clone(&self.client); + wait_for_future(py, async move { client.push_up().await }).map_err(to_py_err) + } + + fn move_hand_end_effector_with_aux( + &self, + py: Python<'_>, + target_posture: PyPosture, + aux_posture: PyPosture, + time_millis: i32, + hand_index: PyHand, + ) -> PyResult<()> { + let client = Arc::clone(&self.client); + let target_posture: Posture = target_posture.into(); + let aux_posture: Posture = aux_posture.into(); + let hand_index: Hand = hand_index.into(); + wait_for_future(py, async move { + client + .move_hand_end_effector_with_aux( + &target_posture, + &aux_posture, + time_millis, + hand_index, + ) + .await + }) + .map_err(to_py_err) + } + + fn move_hand_end_effector( + &self, + py: Python<'_>, + target_posture: PyPosture, + time_millis: i32, + hand_index: PyHand, + ) -> PyResult<()> { + let client = Arc::clone(&self.client); + let target_posture: Posture = target_posture.into(); + let hand_index: Hand = hand_index.into(); + wait_for_future(py, async move { + client + .move_hand_end_effector(&target_posture, time_millis, hand_index) + .await + }) + .map_err(to_py_err) + } + + fn move_hand_end_effector_v2( + &self, + py: Python<'_>, + target_posture: PyPosture, + time_millis: i32, + hand_index: PyHand, + ) -> PyResult<()> { + let client = Arc::clone(&self.client); + let target_posture: Posture = target_posture.into(); + let hand_index: Hand = hand_index.into(); + wait_for_future(py, async move { + client + .move_hand_end_effector_v2(&target_posture, time_millis, hand_index) + .await + }) + .map_err(to_py_err) + } + + fn stop_hand_end_effector(&self, py: Python<'_>) -> PyResult<()> { + let client = Arc::clone(&self.client); + wait_for_future(py, async move { client.stop_hand_end_effector().await }).map_err(to_py_err) + } + + fn control_gripper( + &self, + py: Python<'_>, + motion_param: PyGripperMotionParameter, + mode: PyGripperControlMode, + hand_index: PyHand, + ) -> PyResult<()> { + let client = Arc::clone(&self.client); + let motion_param: GripperMotionParameter = motion_param.into(); + let mode: GripperControlMode = mode.into(); + let hand_index: Hand = hand_index.into(); + wait_for_future(py, async move { + client.control_gripper(motion_param, mode, hand_index).await + }) + .map_err(to_py_err) + } + + fn get_frame_transform( + &self, + py: Python<'_>, + src: PyFrame, + dst: PyFrame, + ) -> PyResult { + let client = Arc::clone(&self.client); + let src: Frame = src.into(); + let dst: Frame = dst.into(); + wait_for_future( + py, + async move { client.get_frame_transform(src, dst).await }, + ) + .map(Into::into) + .map_err(to_py_err) + } + + fn switch_hand_end_effector_control_mode( + &self, + py: Python<'_>, + switch_on: bool, + ) -> PyResult<()> { + let client = Arc::clone(&self.client); + wait_for_future(py, async move { + client + .switch_hand_end_effector_control_mode(switch_on) + .await + }) + .map_err(to_py_err) + } + + fn handshake(&self, py: Python<'_>, action: PyHandAction) -> PyResult<()> { + let client = Arc::clone(&self.client); + wait_for_future(py, async move { client.handshake(action.into()).await }).map_err(to_py_err) + } + + fn control_dexterous_hand( + &self, + py: Python<'_>, + finger_params: Vec, + hand_index: PyHand, + hand_type: PyBoosterHandType, + ) -> PyResult<()> { + let client = Arc::clone(&self.client); + let finger_params: Vec = + finger_params.into_iter().map(Into::into).collect(); + let hand_index: Hand = hand_index.into(); + let hand_type: BoosterHandType = hand_type.into(); + wait_for_future(py, async move { + client + .control_dexterous_hand(&finger_params, hand_index, hand_type) + .await + }) + .map_err(to_py_err) + } + + fn control_dexterous_hand_default( + &self, + py: Python<'_>, + finger_params: Vec, + hand_index: PyHand, + ) -> PyResult<()> { + let client = Arc::clone(&self.client); + let finger_params: Vec = + finger_params.into_iter().map(Into::into).collect(); + let hand_index: Hand = hand_index.into(); + wait_for_future(py, async move { + client + .control_dexterous_hand_default(&finger_params, hand_index) + .await + }) + .map_err(to_py_err) + } + + fn dance(&self, py: Python<'_>, dance_id: PyDanceId) -> PyResult<()> { + let client = Arc::clone(&self.client); + wait_for_future(py, async move { client.dance(dance_id.into()).await }).map_err(to_py_err) + } + + fn play_sound(&self, py: Python<'_>, sound_file_path: String) -> PyResult<()> { + let client = Arc::clone(&self.client); + wait_for_future(py, async move { client.play_sound(sound_file_path).await }) + .map_err(to_py_err) + } + + fn stop_sound(&self, py: Python<'_>) -> PyResult<()> { + let client = Arc::clone(&self.client); + wait_for_future(py, async move { client.stop_sound().await }).map_err(to_py_err) + } + + fn zero_torque_drag(&self, py: Python<'_>, active: bool) -> PyResult<()> { + let client = Arc::clone(&self.client); + wait_for_future(py, async move { client.zero_torque_drag(active).await }).map_err(to_py_err) + } + + fn record_trajectory(&self, py: Python<'_>, active: bool) -> PyResult<()> { + let client = Arc::clone(&self.client); + wait_for_future(py, async move { client.record_trajectory(active).await }) + .map_err(to_py_err) + } + + fn replay_trajectory(&self, py: Python<'_>, traj_file_path: String) -> PyResult<()> { + let client = Arc::clone(&self.client); + wait_for_future( + py, + async move { client.replay_trajectory(traj_file_path).await }, + ) + .map_err(to_py_err) + } + + fn whole_body_dance(&self, py: Python<'_>, dance_id: PyWholeBodyDanceId) -> PyResult<()> { + let client = Arc::clone(&self.client); + wait_for_future( + py, + async move { client.whole_body_dance(dance_id.into()).await }, + ) + .map_err(to_py_err) + } + + fn upper_body_custom_control(&self, py: Python<'_>, start: bool) -> PyResult<()> { + let client = Arc::clone(&self.client); + wait_for_future( + py, + async move { client.upper_body_custom_control(start).await }, + ) + .map_err(to_py_err) + } + + fn reset_odometry(&self, py: Python<'_>) -> PyResult<()> { + let client = Arc::clone(&self.client); + wait_for_future(py, async move { client.reset_odometry().await }).map_err(to_py_err) + } + + fn load_custom_trained_traj( + &self, + py: Python<'_>, + traj: PyCustomTrainedTraj, + ) -> PyResult { + let client = Arc::clone(&self.client); + let traj: CustomTrainedTraj = traj.into(); + wait_for_future( + py, + async move { client.load_custom_trained_traj(&traj).await }, + ) + .map(Into::into) + .map_err(to_py_err) + } + + fn activate_custom_trained_traj(&self, py: Python<'_>, tid: String) -> PyResult<()> { + let client = Arc::clone(&self.client); + wait_for_future( + py, + async move { client.activate_custom_trained_traj(tid).await }, + ) + .map_err(to_py_err) + } + + fn unload_custom_trained_traj(&self, py: Python<'_>, tid: String) -> PyResult<()> { + let client = Arc::clone(&self.client); + wait_for_future( + py, + async move { client.unload_custom_trained_traj(tid).await }, + ) + .map_err(to_py_err) + } + + fn enter_wbc_gait(&self, py: Python<'_>) -> PyResult<()> { + let client = Arc::clone(&self.client); + wait_for_future(py, async move { client.enter_wbc_gait().await }).map_err(to_py_err) + } + + fn exit_wbc_gait(&self, py: Python<'_>) -> PyResult<()> { + let client = Arc::clone(&self.client); + wait_for_future(py, async move { client.exit_wbc_gait().await }).map_err(to_py_err) + } + + fn publish_gripper_command(&self, command: PyGripperCommand) -> PyResult<()> { + self.client + .publish_gripper_command(&command.0) + .map_err(to_py_err) + } + + fn publish_gripper( + &self, + hand: PyHand, + mode: PyGripperMode, + motion_param: u16, + speed: Option, + ) -> PyResult<()> { + let command = GripperCommand { + hand: hand.into(), + mode: mode.into(), + motion_param, + speed: speed.unwrap_or(500), + }; + self.client + .publish_gripper_command(&command) + .map_err(to_py_err) + } +} + +#[pyclass(module = "booster_sdk_bindings", name = "AiClient", unsendable)] +pub struct PyAiClient { + client: Arc, +} + +#[pymethods] +impl PyAiClient { + #[new] + fn new() -> PyResult { + Ok(Self { + client: Arc::new(AiClient::new().map_err(to_py_err)?), + }) + } + + fn start_ai_chat(&self, py: Python<'_>, param: PyStartAiChatParameter) -> PyResult<()> { + let client = Arc::clone(&self.client); + let param: StartAiChatParameter = param.into(); + wait_for_future(py, async move { client.start_ai_chat(¶m).await }).map_err(to_py_err) + } + + fn stop_ai_chat(&self, py: Python<'_>) -> PyResult<()> { + let client = Arc::clone(&self.client); + wait_for_future(py, async move { client.stop_ai_chat().await }).map_err(to_py_err) + } + + fn speak(&self, py: Python<'_>, param: PySpeakParameter) -> PyResult<()> { + let client = Arc::clone(&self.client); + let param: SpeakParameter = param.into(); + wait_for_future(py, async move { client.speak(¶m).await }).map_err(to_py_err) + } + + fn start_face_tracking(&self, py: Python<'_>) -> PyResult<()> { + let client = Arc::clone(&self.client); + wait_for_future(py, async move { client.start_face_tracking().await }).map_err(to_py_err) + } + + fn stop_face_tracking(&self, py: Python<'_>) -> PyResult<()> { + let client = Arc::clone(&self.client); + wait_for_future(py, async move { client.stop_face_tracking().await }).map_err(to_py_err) + } +} + +#[pyclass(module = "booster_sdk_bindings", name = "LuiClient", unsendable)] +pub struct PyLuiClient { + client: Arc, +} + +#[pymethods] +impl PyLuiClient { + #[new] + fn new() -> PyResult { + Ok(Self { + client: Arc::new(LuiClient::new().map_err(to_py_err)?), + }) + } + + fn start_asr(&self, py: Python<'_>) -> PyResult<()> { + let client = Arc::clone(&self.client); + wait_for_future(py, async move { client.start_asr().await }).map_err(to_py_err) + } + + fn stop_asr(&self, py: Python<'_>) -> PyResult<()> { + let client = Arc::clone(&self.client); + wait_for_future(py, async move { client.stop_asr().await }).map_err(to_py_err) + } + + fn start_tts(&self, py: Python<'_>, config: PyLuiTtsConfig) -> PyResult<()> { + let client = Arc::clone(&self.client); + let config: LuiTtsConfig = config.into(); + wait_for_future(py, async move { client.start_tts(&config).await }).map_err(to_py_err) + } + + fn stop_tts(&self, py: Python<'_>) -> PyResult<()> { + let client = Arc::clone(&self.client); + wait_for_future(py, async move { client.stop_tts().await }).map_err(to_py_err) + } + + fn send_tts_text(&self, py: Python<'_>, param: PyLuiTtsParameter) -> PyResult<()> { + let client = Arc::clone(&self.client); + let param: LuiTtsParameter = param.into(); + wait_for_future(py, async move { client.send_tts_text(¶m).await }).map_err(to_py_err) + } +} + +#[pyclass( + module = "booster_sdk_bindings", + name = "LightControlClient", + unsendable +)] +pub struct PyLightControlClient { + client: Arc, +} + +#[pymethods] +impl PyLightControlClient { + #[new] + fn new() -> PyResult { + Ok(Self { + client: Arc::new(LightControlClient::new().map_err(to_py_err)?), + }) + } + + fn set_led_light_color(&self, py: Python<'_>, r: u8, g: u8, b: u8) -> PyResult<()> { + let client = Arc::clone(&self.client); + wait_for_future(py, async move { client.set_led_light_color(r, g, b).await }) + .map_err(to_py_err) + } + + fn set_led_light_color_param(&self, py: Python<'_>, r: u8, g: u8, b: u8) -> PyResult<()> { + self.set_led_light_color(py, r, g, b) + } + + fn stop_led_light_control(&self, py: Python<'_>) -> PyResult<()> { + let client = Arc::clone(&self.client); + wait_for_future(py, async move { client.stop_led_light_control().await }).map_err(to_py_err) + } +} + +#[pyclass(module = "booster_sdk_bindings", name = "VisionClient", unsendable)] +pub struct PyVisionClient { + client: Arc, +} + +#[pymethods] +impl PyVisionClient { + #[new] + fn new() -> PyResult { + Ok(Self { + client: Arc::new(VisionClient::new().map_err(to_py_err)?), + }) + } + + fn start_vision_service( + &self, + py: Python<'_>, + enable_position: bool, + enable_color: bool, + enable_face_detection: bool, + ) -> PyResult<()> { + let client = Arc::clone(&self.client); + wait_for_future(py, async move { + client + .start_vision_service(enable_position, enable_color, enable_face_detection) + .await + }) + .map_err(to_py_err) + } + + fn stop_vision_service(&self, py: Python<'_>) -> PyResult<()> { + let client = Arc::clone(&self.client); + wait_for_future(py, async move { client.stop_vision_service().await }).map_err(to_py_err) + } + + fn get_detection_object_with_ratio( + &self, + py: Python<'_>, + focus_ratio: f32, + ) -> PyResult> { + let client = Arc::clone(&self.client); + wait_for_future(py, async move { + client.get_detection_object_with_ratio(focus_ratio).await + }) + .map(|results| results.into_iter().map(Into::into).collect()) + .map_err(to_py_err) + } + + fn get_detection_object(&self, py: Python<'_>) -> PyResult> { + let client = Arc::clone(&self.client); + wait_for_future(py, async move { client.get_detection_object().await }) + .map(|results| results.into_iter().map(Into::into).collect()) + .map_err(to_py_err) + } +} + +#[pyclass(module = "booster_sdk_bindings", name = "X5CameraClient", unsendable)] +pub struct PyX5CameraClient { + client: Arc, +} + +#[pymethods] +impl PyX5CameraClient { + #[new] + fn new() -> PyResult { + Ok(Self { + client: Arc::new(X5CameraClient::new().map_err(to_py_err)?), + }) + } + + fn change_mode(&self, py: Python<'_>, mode: PyCameraSetMode) -> PyResult<()> { + let client = Arc::clone(&self.client); + wait_for_future(py, async move { client.change_mode(mode.into()).await }).map_err(to_py_err) + } + + fn get_status(&self, py: Python<'_>) -> PyResult { + let client = Arc::clone(&self.client); + wait_for_future(py, async move { client.get_status().await }) + .map(Into::into) + .map_err(to_py_err) + } +} + +#[pymodule] +fn booster_sdk_bindings(py: Python<'_>, m: &Bound<'_, PyModule>) -> PyResult<()> { + m.add("BoosterSdkError", py.get_type::())?; + m.add("BOOSTER_ROBOT_USER_ID", BOOSTER_ROBOT_USER_ID)?; + + m.add_class::()?; + m.add_class::()?; + m.add_class::()?; + m.add_class::()?; + m.add_class::()?; + m.add_class::()?; + + m.add_class::()?; + m.add_class::()?; + m.add_class::()?; + m.add_class::()?; + m.add_class::()?; + m.add_class::()?; + m.add_class::()?; + m.add_class::()?; + m.add_class::()?; + m.add_class::()?; + m.add_class::()?; + m.add_class::()?; + m.add_class::()?; + m.add_class::()?; + + m.add_class::()?; + m.add_class::()?; + m.add_class::()?; + m.add_class::()?; + m.add_class::()?; + m.add_class::()?; + m.add_class::()?; + m.add_class::()?; + m.add_class::()?; + m.add_class::()?; + m.add_class::()?; + + m.add_class::()?; + m.add_class::()?; + m.add_class::()?; + m.add_class::()?; + m.add_class::()?; + m.add_class::()?; + m.add_class::()?; + + m.add_class::()?; + m.add_class::()?; + m.add_class::()?; + m.add_class::()?; + m.add_class::()?; + m.add_class::()?; + Ok(()) } From 64a6ef2aa7d08b3d0cc95ab345c796c246b99d9e Mon Sep 17 00:00:00 2001 From: Gijs de Jong Date: Sun, 22 Feb 2026 20:14:15 +0100 Subject: [PATCH 08/15] clean up module layout --- .../src/client/{ai_client.rs => ai.rs} | 0 booster_sdk/src/client/commands.rs | 96 ------------------- ...ght_control_client.rs => light_control.rs} | 0 .../src/client/{loco_client.rs => loco.rs} | 89 +++++++++++++++-- booster_sdk/src/client/mod.rs | 18 ++-- .../client/{vision_client.rs => vision.rs} | 0 .../{x5_camera_client.rs => x5_camera.rs} | 0 7 files changed, 85 insertions(+), 118 deletions(-) rename booster_sdk/src/client/{ai_client.rs => ai.rs} (100%) delete mode 100644 booster_sdk/src/client/commands.rs rename booster_sdk/src/client/{light_control_client.rs => light_control.rs} (100%) rename booster_sdk/src/client/{loco_client.rs => loco.rs} (86%) rename booster_sdk/src/client/{vision_client.rs => vision.rs} (100%) rename booster_sdk/src/client/{x5_camera_client.rs => x5_camera.rs} (100%) diff --git a/booster_sdk/src/client/ai_client.rs b/booster_sdk/src/client/ai.rs similarity index 100% rename from booster_sdk/src/client/ai_client.rs rename to booster_sdk/src/client/ai.rs diff --git a/booster_sdk/src/client/commands.rs b/booster_sdk/src/client/commands.rs deleted file mode 100644 index c00bf8d..0000000 --- a/booster_sdk/src/client/commands.rs +++ /dev/null @@ -1,96 +0,0 @@ -//! Command parameter types with builders. -//! -//! This module provides ergonomic builder types for constructing robot control commands. - -use crate::types::{GripperMode, Hand}; -use serde::{Deserialize, Serialize}; -use typed_builder::TypedBuilder; - -/// Gripper control command -#[derive(Debug, Clone, Copy, TypedBuilder, Serialize, Deserialize)] -pub struct GripperCommand { - /// Target hand - pub hand: Hand, - - /// Control mode (position or force) - pub mode: GripperMode, - - /// Motion parameter value - /// - Position mode: 0-1000 (0 = fully open, 1000 = fully closed) - /// - Force mode: 50-1000 (grasping force) - pub motion_param: u16, - - /// Movement speed (1-1000) - #[builder(default = 500)] - pub speed: u16, -} - -impl GripperCommand { - /// Create a command to open the gripper - #[must_use] - pub fn open(hand: Hand) -> Self { - Self { - hand, - mode: GripperMode::Position, - motion_param: 0, - speed: 500, - } - } - - /// Create a command to close the gripper - #[must_use] - pub fn close(hand: Hand) -> Self { - Self { - hand, - mode: GripperMode::Position, - motion_param: 1000, - speed: 500, - } - } - - /// Create a force-based grasp command - #[must_use] - pub fn grasp(hand: Hand, force: u16) -> Self { - Self { - hand, - mode: GripperMode::Force, - motion_param: force.clamp(50, 1000), - speed: 500, - } - } - - /// Convert to DDS gripper control message. - #[must_use] - pub fn to_dds_control(&self) -> crate::dds::GripperControl { - let (position, force) = match self.mode { - GripperMode::Position => (self.motion_param as i32, 0), - GripperMode::Force => (0, self.motion_param as i32), - }; - - crate::dds::GripperControl { - hand_index: u8::from(self.hand), - position, - force, - speed: self.speed as i32, - } - } -} - -#[cfg(test)] -mod tests { - use super::*; - - #[test] - fn test_gripper_command_builders() { - let open = GripperCommand::open(Hand::Left); - assert_eq!(open.motion_param, 0); - assert_eq!(open.mode, GripperMode::Position); - - let close = GripperCommand::close(Hand::Right); - assert_eq!(close.motion_param, 1000); - - let grasp = GripperCommand::grasp(Hand::Left, 600); - assert_eq!(grasp.mode, GripperMode::Force); - assert_eq!(grasp.motion_param, 600); - } -} diff --git a/booster_sdk/src/client/light_control_client.rs b/booster_sdk/src/client/light_control.rs similarity index 100% rename from booster_sdk/src/client/light_control_client.rs rename to booster_sdk/src/client/light_control.rs diff --git a/booster_sdk/src/client/loco_client.rs b/booster_sdk/src/client/loco.rs similarity index 86% rename from booster_sdk/src/client/loco_client.rs rename to booster_sdk/src/client/loco.rs index 1630e5c..de3a922 100644 --- a/booster_sdk/src/client/loco_client.rs +++ b/booster_sdk/src/client/loco.rs @@ -1,20 +1,22 @@ //! High-level B1 locomotion client built on DDS RPC and topic I/O. -use crate::dds::RpcClient; use crate::dds::{ BatteryState, BinaryData, ButtonEventMsg, DdsNode, DdsPublisher, DdsSubscription, GripperControl, LightControlMsg, MotionState, RemoteControllerState, RobotProcessStateMsg, - RobotStatusDdsMsg, RpcClientOptions, SafeMode, battery_state_topic, button_event_topic, - device_gateway_topic, gripper_control_topic, light_control_topic, motion_state_topic, - process_state_topic, remote_controller_topic, safe_mode_topic, video_stream_topic, + RobotStatusDdsMsg, RpcClient, RpcClientOptions, SafeMode, battery_state_topic, + button_event_topic, device_gateway_topic, gripper_control_topic, light_control_topic, + motion_state_topic, process_state_topic, remote_controller_topic, safe_mode_topic, + video_stream_topic, }; use crate::types::{ BoosterHandType, CustomTrainedTraj, DanceId, DexterousFingerParameter, Frame, GetModeResponse, - GetRobotInfoResponse, GetStatusResponse, GripperControlMode, GripperMotionParameter, - HandAction, HandIndex, LoadCustomTrainedTrajResponse, LocoApiId, Result, RobotMode, Transform, - WholeBodyDanceId, + GetRobotInfoResponse, GetStatusResponse, GripperControlMode, GripperMode, + GripperMotionParameter, Hand, HandAction, HandIndex, LoadCustomTrainedTrajResponse, LocoApiId, + Result, RobotMode, Transform, WholeBodyDanceId, }; +use serde::{Deserialize, Serialize}; use serde_json::json; +use typed_builder::TypedBuilder; // The controller may send an intermediate pending status (-1) before the // final success response. Mode transitions (especially PREPARE) can take @@ -376,7 +378,7 @@ impl BoosterClient { } /// Publish a high-level gripper command. - pub fn publish_gripper_command(&self, command: &crate::client::GripperCommand) -> Result<()> { + pub fn publish_gripper_command(&self, command: &GripperCommand) -> Result<()> { self.gripper_publisher.write(command.to_dds_control()) } @@ -426,5 +428,72 @@ impl BoosterClient { } } -/// Alias matching the C++ class naming. -pub type B1LocoClient = BoosterClient; +/// Gripper control command +#[derive(Debug, Clone, Copy, TypedBuilder, Serialize, Deserialize)] +pub struct GripperCommand { + /// Target hand + pub hand: Hand, + + /// Control mode (position or force) + pub mode: GripperMode, + + /// Motion parameter value + /// - Position mode: 0-1000 (0 = fully open, 1000 = fully closed) + /// - Force mode: 50-1000 (grasping force) + pub motion_param: u16, + + /// Movement speed (1-1000) + #[builder(default = 500)] + pub speed: u16, +} + +impl GripperCommand { + /// Create a command to open the gripper + #[must_use] + pub fn open(hand: Hand) -> Self { + Self { + hand, + mode: GripperMode::Position, + motion_param: 0, + speed: 500, + } + } + + /// Create a command to close the gripper + #[must_use] + pub fn close(hand: Hand) -> Self { + Self { + hand, + mode: GripperMode::Position, + motion_param: 1000, + speed: 500, + } + } + + /// Create a force-based grasp command + #[must_use] + pub fn grasp(hand: Hand, force: u16) -> Self { + Self { + hand, + mode: GripperMode::Force, + motion_param: force.clamp(50, 1000), + speed: 500, + } + } + + /// Convert to DDS gripper control message. + #[must_use] + pub fn to_dds_control(&self) -> crate::dds::GripperControl { + let (position, force) = match self.mode { + GripperMode::Position => (self.motion_param as i32, 0), + GripperMode::Force => (0, self.motion_param as i32), + }; + + crate::dds::GripperControl { + hand_index: u8::from(self.hand), + position, + force, + speed: self.speed as i32, + } + } +} diff --git a/booster_sdk/src/client/mod.rs b/booster_sdk/src/client/mod.rs index 1137adb..e4f615f 100644 --- a/booster_sdk/src/client/mod.rs +++ b/booster_sdk/src/client/mod.rs @@ -1,18 +1,12 @@ //! High-level client APIs for the Booster Robotics SDK. -pub mod ai_client; -pub mod commands; -pub mod light_control_client; -pub mod loco_client; -pub mod vision_client; -pub mod x5_camera_client; +pub mod ai; +pub mod light_control; +pub mod loco; +pub mod vision; +pub mod x5_camera; -pub use ai_client::*; -pub use commands::*; -pub use light_control_client::*; -pub use loco_client::*; -pub use vision_client::*; -pub use x5_camera_client::*; +pub use loco::BoosterClient; /// Declare an i32-backed enum with serde, `From`, and `TryFrom`. /// diff --git a/booster_sdk/src/client/vision_client.rs b/booster_sdk/src/client/vision.rs similarity index 100% rename from booster_sdk/src/client/vision_client.rs rename to booster_sdk/src/client/vision.rs diff --git a/booster_sdk/src/client/x5_camera_client.rs b/booster_sdk/src/client/x5_camera.rs similarity index 100% rename from booster_sdk/src/client/x5_camera_client.rs rename to booster_sdk/src/client/x5_camera.rs From 755ecb410bd18a6bc6ee42ef0477ac736e3d54bb Mon Sep 17 00:00:00 2001 From: Gijs de Jong Date: Sun, 22 Feb 2026 20:24:39 +0100 Subject: [PATCH 09/15] clean up python module layout --- booster_sdk_py/booster_sdk/client.py | 21 - booster_sdk_py/booster_sdk/client/__init__.py | 19 + booster_sdk_py/booster_sdk/client/ai.py | 9 + booster_sdk_py/booster_sdk/client/booster.py | 9 + .../booster_sdk/client/light_control.py | 9 + booster_sdk_py/booster_sdk/client/lui.py | 9 + booster_sdk_py/booster_sdk/client/vision.py | 9 + .../booster_sdk/client/x5_camera.py | 9 + booster_sdk_py/src/client/ai.rs | 53 ++ booster_sdk_py/src/client/booster.rs | 407 ++++++++++++ booster_sdk_py/src/client/light_control.rs | 45 ++ booster_sdk_py/src/client/lui.rs | 53 ++ booster_sdk_py/src/client/mod.rs | 18 + booster_sdk_py/src/client/vision.rs | 67 ++ booster_sdk_py/src/client/x5_camera.rs | 38 ++ booster_sdk_py/src/lib.rs | 620 +----------------- 16 files changed, 773 insertions(+), 622 deletions(-) delete mode 100644 booster_sdk_py/booster_sdk/client.py create mode 100644 booster_sdk_py/booster_sdk/client/__init__.py create mode 100644 booster_sdk_py/booster_sdk/client/ai.py create mode 100644 booster_sdk_py/booster_sdk/client/booster.py create mode 100644 booster_sdk_py/booster_sdk/client/light_control.py create mode 100644 booster_sdk_py/booster_sdk/client/lui.py create mode 100644 booster_sdk_py/booster_sdk/client/vision.py create mode 100644 booster_sdk_py/booster_sdk/client/x5_camera.py create mode 100644 booster_sdk_py/src/client/ai.rs create mode 100644 booster_sdk_py/src/client/booster.rs create mode 100644 booster_sdk_py/src/client/light_control.rs create mode 100644 booster_sdk_py/src/client/lui.rs create mode 100644 booster_sdk_py/src/client/mod.rs create mode 100644 booster_sdk_py/src/client/vision.rs create mode 100644 booster_sdk_py/src/client/x5_camera.rs diff --git a/booster_sdk_py/booster_sdk/client.py b/booster_sdk_py/booster_sdk/client.py deleted file mode 100644 index 5e59471..0000000 --- a/booster_sdk_py/booster_sdk/client.py +++ /dev/null @@ -1,21 +0,0 @@ -"""High-level client classes for the Booster SDK.""" - -from __future__ import annotations - -import booster_sdk_bindings as bindings - -BoosterClient = bindings.BoosterClient -AiClient = bindings.AiClient -LuiClient = bindings.LuiClient -LightControlClient = bindings.LightControlClient -VisionClient = bindings.VisionClient -X5CameraClient = bindings.X5CameraClient - -__all__ = [ - "BoosterClient", - "AiClient", - "LuiClient", - "LightControlClient", - "VisionClient", - "X5CameraClient", -] diff --git a/booster_sdk_py/booster_sdk/client/__init__.py b/booster_sdk_py/booster_sdk/client/__init__.py new file mode 100644 index 0000000..31856cd --- /dev/null +++ b/booster_sdk_py/booster_sdk/client/__init__.py @@ -0,0 +1,19 @@ +"""High-level client classes for the Booster SDK.""" + +from __future__ import annotations + +from .ai import AiClient +from .booster import BoosterClient +from .light_control import LightControlClient +from .lui import LuiClient +from .vision import VisionClient +from .x5_camera import X5CameraClient + +__all__ = [ + "BoosterClient", + "AiClient", + "LuiClient", + "LightControlClient", + "VisionClient", + "X5CameraClient", +] diff --git a/booster_sdk_py/booster_sdk/client/ai.py b/booster_sdk_py/booster_sdk/client/ai.py new file mode 100644 index 0000000..56ccd12 --- /dev/null +++ b/booster_sdk_py/booster_sdk/client/ai.py @@ -0,0 +1,9 @@ +"""AI client bindings.""" + +from __future__ import annotations + +import booster_sdk_bindings as bindings + +AiClient = bindings.AiClient + +__all__ = ["AiClient"] diff --git a/booster_sdk_py/booster_sdk/client/booster.py b/booster_sdk_py/booster_sdk/client/booster.py new file mode 100644 index 0000000..7080c63 --- /dev/null +++ b/booster_sdk_py/booster_sdk/client/booster.py @@ -0,0 +1,9 @@ +"""Booster locomotion and control client bindings.""" + +from __future__ import annotations + +import booster_sdk_bindings as bindings + +BoosterClient = bindings.BoosterClient + +__all__ = ["BoosterClient"] diff --git a/booster_sdk_py/booster_sdk/client/light_control.py b/booster_sdk_py/booster_sdk/client/light_control.py new file mode 100644 index 0000000..3f06aa7 --- /dev/null +++ b/booster_sdk_py/booster_sdk/client/light_control.py @@ -0,0 +1,9 @@ +"""Light control client bindings.""" + +from __future__ import annotations + +import booster_sdk_bindings as bindings + +LightControlClient = bindings.LightControlClient + +__all__ = ["LightControlClient"] diff --git a/booster_sdk_py/booster_sdk/client/lui.py b/booster_sdk_py/booster_sdk/client/lui.py new file mode 100644 index 0000000..e3caf59 --- /dev/null +++ b/booster_sdk_py/booster_sdk/client/lui.py @@ -0,0 +1,9 @@ +"""LUI speech client bindings.""" + +from __future__ import annotations + +import booster_sdk_bindings as bindings + +LuiClient = bindings.LuiClient + +__all__ = ["LuiClient"] diff --git a/booster_sdk_py/booster_sdk/client/vision.py b/booster_sdk_py/booster_sdk/client/vision.py new file mode 100644 index 0000000..8830150 --- /dev/null +++ b/booster_sdk_py/booster_sdk/client/vision.py @@ -0,0 +1,9 @@ +"""Vision client bindings.""" + +from __future__ import annotations + +import booster_sdk_bindings as bindings + +VisionClient = bindings.VisionClient + +__all__ = ["VisionClient"] diff --git a/booster_sdk_py/booster_sdk/client/x5_camera.py b/booster_sdk_py/booster_sdk/client/x5_camera.py new file mode 100644 index 0000000..463f7e2 --- /dev/null +++ b/booster_sdk_py/booster_sdk/client/x5_camera.py @@ -0,0 +1,9 @@ +"""X5 camera client bindings.""" + +from __future__ import annotations + +import booster_sdk_bindings as bindings + +X5CameraClient = bindings.X5CameraClient + +__all__ = ["X5CameraClient"] diff --git a/booster_sdk_py/src/client/ai.rs b/booster_sdk_py/src/client/ai.rs new file mode 100644 index 0000000..fd30213 --- /dev/null +++ b/booster_sdk_py/src/client/ai.rs @@ -0,0 +1,53 @@ +use std::sync::Arc; + +use booster_sdk::client::ai::AiClient; +use pyo3::{Bound, prelude::*, types::PyModule}; + +use crate::{PySpeakParameter, PyStartAiChatParameter, runtime::wait_for_future, to_py_err}; + +#[pyclass(module = "booster_sdk_bindings", name = "AiClient", unsendable)] +pub struct PyAiClient { + client: Arc, +} + +#[pymethods] +impl PyAiClient { + #[new] + fn new() -> PyResult { + Ok(Self { + client: Arc::new(AiClient::new().map_err(to_py_err)?), + }) + } + + fn start_ai_chat(&self, py: Python<'_>, param: PyStartAiChatParameter) -> PyResult<()> { + let client = Arc::clone(&self.client); + let param = param.into(); + wait_for_future(py, async move { client.start_ai_chat(¶m).await }).map_err(to_py_err) + } + + fn stop_ai_chat(&self, py: Python<'_>) -> PyResult<()> { + let client = Arc::clone(&self.client); + wait_for_future(py, async move { client.stop_ai_chat().await }).map_err(to_py_err) + } + + fn speak(&self, py: Python<'_>, param: PySpeakParameter) -> PyResult<()> { + let client = Arc::clone(&self.client); + let param = param.into(); + wait_for_future(py, async move { client.speak(¶m).await }).map_err(to_py_err) + } + + fn start_face_tracking(&self, py: Python<'_>) -> PyResult<()> { + let client = Arc::clone(&self.client); + wait_for_future(py, async move { client.start_face_tracking().await }).map_err(to_py_err) + } + + fn stop_face_tracking(&self, py: Python<'_>) -> PyResult<()> { + let client = Arc::clone(&self.client); + wait_for_future(py, async move { client.stop_face_tracking().await }).map_err(to_py_err) + } +} + +pub(crate) fn register(m: &Bound<'_, PyModule>) -> PyResult<()> { + m.add_class::()?; + Ok(()) +} diff --git a/booster_sdk_py/src/client/booster.rs b/booster_sdk_py/src/client/booster.rs new file mode 100644 index 0000000..3f9023f --- /dev/null +++ b/booster_sdk_py/src/client/booster.rs @@ -0,0 +1,407 @@ +use std::sync::Arc; + +use booster_sdk::{ + client::loco::{BoosterClient, GripperCommand}, + types::{ + BoosterHandType, CustomTrainedTraj, DexterousFingerParameter, Frame, GripperControlMode, + GripperMotionParameter, Hand, Posture, + }, +}; +use pyo3::{Bound, prelude::*, types::PyModule}; + +use crate::{ + PyBoosterHandType, PyCustomTrainedTraj, PyDanceId, PyDexterousFingerParameter, PyFrame, + PyGetModeResponse, PyGetRobotInfoResponse, PyGetStatusResponse, PyGripperCommand, + PyGripperControlMode, PyGripperMode, PyGripperMotionParameter, PyHand, PyHandAction, + PyLoadCustomTrainedTrajResponse, PyPosture, PyRobotMode, PyTransform, PyWholeBodyDanceId, + runtime::wait_for_future, to_py_err, +}; + +#[pyclass(module = "booster_sdk_bindings", name = "BoosterClient", unsendable)] +pub struct PyBoosterClient { + client: Arc, +} + +#[pymethods] +impl PyBoosterClient { + #[new] + fn new() -> PyResult { + Ok(Self { + client: Arc::new(BoosterClient::new().map_err(to_py_err)?), + }) + } + + fn change_mode(&self, py: Python<'_>, mode: PyRobotMode) -> PyResult<()> { + let client = Arc::clone(&self.client); + wait_for_future(py, async move { client.change_mode(mode.into()).await }).map_err(to_py_err) + } + + fn get_mode(&self, py: Python<'_>) -> PyResult { + let client = Arc::clone(&self.client); + wait_for_future(py, async move { client.get_mode().await }) + .map(Into::into) + .map_err(to_py_err) + } + + fn get_status(&self, py: Python<'_>) -> PyResult { + let client = Arc::clone(&self.client); + wait_for_future(py, async move { client.get_status().await }) + .map(Into::into) + .map_err(to_py_err) + } + + fn get_robot_info(&self, py: Python<'_>) -> PyResult { + let client = Arc::clone(&self.client); + wait_for_future(py, async move { client.get_robot_info().await }) + .map(Into::into) + .map_err(to_py_err) + } + + fn move_robot(&self, py: Python<'_>, vx: f32, vy: f32, vyaw: f32) -> PyResult<()> { + let client = Arc::clone(&self.client); + wait_for_future(py, async move { client.move_robot(vx, vy, vyaw).await }).map_err(to_py_err) + } + + fn rotate_head(&self, py: Python<'_>, pitch: f32, yaw: f32) -> PyResult<()> { + let client = Arc::clone(&self.client); + wait_for_future(py, async move { client.rotate_head(pitch, yaw).await }).map_err(to_py_err) + } + + fn wave_hand(&self, py: Python<'_>, action: PyHandAction) -> PyResult<()> { + let client = Arc::clone(&self.client); + wait_for_future(py, async move { client.wave_hand(action.into()).await }).map_err(to_py_err) + } + + fn rotate_head_with_direction( + &self, + py: Python<'_>, + pitch_direction: i32, + yaw_direction: i32, + ) -> PyResult<()> { + let client = Arc::clone(&self.client); + wait_for_future(py, async move { + client + .rotate_head_with_direction(pitch_direction, yaw_direction) + .await + }) + .map_err(to_py_err) + } + + fn lie_down(&self, py: Python<'_>) -> PyResult<()> { + let client = Arc::clone(&self.client); + wait_for_future(py, async move { client.lie_down().await }).map_err(to_py_err) + } + + fn get_up(&self, py: Python<'_>) -> PyResult<()> { + let client = Arc::clone(&self.client); + wait_for_future(py, async move { client.get_up().await }).map_err(to_py_err) + } + + fn get_up_with_mode(&self, py: Python<'_>, mode: PyRobotMode) -> PyResult<()> { + let client = Arc::clone(&self.client); + wait_for_future( + py, + async move { client.get_up_with_mode(mode.into()).await }, + ) + .map_err(to_py_err) + } + + fn shoot(&self, py: Python<'_>) -> PyResult<()> { + let client = Arc::clone(&self.client); + wait_for_future(py, async move { client.shoot().await }).map_err(to_py_err) + } + + fn push_up(&self, py: Python<'_>) -> PyResult<()> { + let client = Arc::clone(&self.client); + wait_for_future(py, async move { client.push_up().await }).map_err(to_py_err) + } + + fn move_hand_end_effector_with_aux( + &self, + py: Python<'_>, + target_posture: PyPosture, + aux_posture: PyPosture, + time_millis: i32, + hand_index: PyHand, + ) -> PyResult<()> { + let client = Arc::clone(&self.client); + let target_posture: Posture = target_posture.into(); + let aux_posture: Posture = aux_posture.into(); + let hand_index: Hand = hand_index.into(); + wait_for_future(py, async move { + client + .move_hand_end_effector_with_aux( + &target_posture, + &aux_posture, + time_millis, + hand_index, + ) + .await + }) + .map_err(to_py_err) + } + + fn move_hand_end_effector( + &self, + py: Python<'_>, + target_posture: PyPosture, + time_millis: i32, + hand_index: PyHand, + ) -> PyResult<()> { + let client = Arc::clone(&self.client); + let target_posture: Posture = target_posture.into(); + let hand_index: Hand = hand_index.into(); + wait_for_future(py, async move { + client + .move_hand_end_effector(&target_posture, time_millis, hand_index) + .await + }) + .map_err(to_py_err) + } + + fn move_hand_end_effector_v2( + &self, + py: Python<'_>, + target_posture: PyPosture, + time_millis: i32, + hand_index: PyHand, + ) -> PyResult<()> { + let client = Arc::clone(&self.client); + let target_posture: Posture = target_posture.into(); + let hand_index: Hand = hand_index.into(); + wait_for_future(py, async move { + client + .move_hand_end_effector_v2(&target_posture, time_millis, hand_index) + .await + }) + .map_err(to_py_err) + } + + fn stop_hand_end_effector(&self, py: Python<'_>) -> PyResult<()> { + let client = Arc::clone(&self.client); + wait_for_future(py, async move { client.stop_hand_end_effector().await }).map_err(to_py_err) + } + + fn control_gripper( + &self, + py: Python<'_>, + motion_param: PyGripperMotionParameter, + mode: PyGripperControlMode, + hand_index: PyHand, + ) -> PyResult<()> { + let client = Arc::clone(&self.client); + let motion_param: GripperMotionParameter = motion_param.into(); + let mode: GripperControlMode = mode.into(); + let hand_index: Hand = hand_index.into(); + wait_for_future(py, async move { + client.control_gripper(motion_param, mode, hand_index).await + }) + .map_err(to_py_err) + } + + fn get_frame_transform( + &self, + py: Python<'_>, + src: PyFrame, + dst: PyFrame, + ) -> PyResult { + let client = Arc::clone(&self.client); + let src: Frame = src.into(); + let dst: Frame = dst.into(); + wait_for_future( + py, + async move { client.get_frame_transform(src, dst).await }, + ) + .map(Into::into) + .map_err(to_py_err) + } + + fn switch_hand_end_effector_control_mode( + &self, + py: Python<'_>, + switch_on: bool, + ) -> PyResult<()> { + let client = Arc::clone(&self.client); + wait_for_future(py, async move { + client + .switch_hand_end_effector_control_mode(switch_on) + .await + }) + .map_err(to_py_err) + } + + fn handshake(&self, py: Python<'_>, action: PyHandAction) -> PyResult<()> { + let client = Arc::clone(&self.client); + wait_for_future(py, async move { client.handshake(action.into()).await }).map_err(to_py_err) + } + + fn control_dexterous_hand( + &self, + py: Python<'_>, + finger_params: Vec, + hand_index: PyHand, + hand_type: PyBoosterHandType, + ) -> PyResult<()> { + let client = Arc::clone(&self.client); + let finger_params: Vec = + finger_params.into_iter().map(Into::into).collect(); + let hand_index: Hand = hand_index.into(); + let hand_type: BoosterHandType = hand_type.into(); + wait_for_future(py, async move { + client + .control_dexterous_hand(&finger_params, hand_index, hand_type) + .await + }) + .map_err(to_py_err) + } + + fn control_dexterous_hand_default( + &self, + py: Python<'_>, + finger_params: Vec, + hand_index: PyHand, + ) -> PyResult<()> { + let client = Arc::clone(&self.client); + let finger_params: Vec = + finger_params.into_iter().map(Into::into).collect(); + let hand_index: Hand = hand_index.into(); + wait_for_future(py, async move { + client + .control_dexterous_hand_default(&finger_params, hand_index) + .await + }) + .map_err(to_py_err) + } + + fn dance(&self, py: Python<'_>, dance_id: PyDanceId) -> PyResult<()> { + let client = Arc::clone(&self.client); + wait_for_future(py, async move { client.dance(dance_id.into()).await }).map_err(to_py_err) + } + + fn play_sound(&self, py: Python<'_>, sound_file_path: String) -> PyResult<()> { + let client = Arc::clone(&self.client); + wait_for_future(py, async move { client.play_sound(sound_file_path).await }) + .map_err(to_py_err) + } + + fn stop_sound(&self, py: Python<'_>) -> PyResult<()> { + let client = Arc::clone(&self.client); + wait_for_future(py, async move { client.stop_sound().await }).map_err(to_py_err) + } + + fn zero_torque_drag(&self, py: Python<'_>, active: bool) -> PyResult<()> { + let client = Arc::clone(&self.client); + wait_for_future(py, async move { client.zero_torque_drag(active).await }).map_err(to_py_err) + } + + fn record_trajectory(&self, py: Python<'_>, active: bool) -> PyResult<()> { + let client = Arc::clone(&self.client); + wait_for_future(py, async move { client.record_trajectory(active).await }) + .map_err(to_py_err) + } + + fn replay_trajectory(&self, py: Python<'_>, traj_file_path: String) -> PyResult<()> { + let client = Arc::clone(&self.client); + wait_for_future( + py, + async move { client.replay_trajectory(traj_file_path).await }, + ) + .map_err(to_py_err) + } + + fn whole_body_dance(&self, py: Python<'_>, dance_id: PyWholeBodyDanceId) -> PyResult<()> { + let client = Arc::clone(&self.client); + wait_for_future( + py, + async move { client.whole_body_dance(dance_id.into()).await }, + ) + .map_err(to_py_err) + } + + fn upper_body_custom_control(&self, py: Python<'_>, start: bool) -> PyResult<()> { + let client = Arc::clone(&self.client); + wait_for_future( + py, + async move { client.upper_body_custom_control(start).await }, + ) + .map_err(to_py_err) + } + + fn reset_odometry(&self, py: Python<'_>) -> PyResult<()> { + let client = Arc::clone(&self.client); + wait_for_future(py, async move { client.reset_odometry().await }).map_err(to_py_err) + } + + fn load_custom_trained_traj( + &self, + py: Python<'_>, + traj: PyCustomTrainedTraj, + ) -> PyResult { + let client = Arc::clone(&self.client); + let traj: CustomTrainedTraj = traj.into(); + wait_for_future( + py, + async move { client.load_custom_trained_traj(&traj).await }, + ) + .map(Into::into) + .map_err(to_py_err) + } + + fn activate_custom_trained_traj(&self, py: Python<'_>, tid: String) -> PyResult<()> { + let client = Arc::clone(&self.client); + wait_for_future( + py, + async move { client.activate_custom_trained_traj(tid).await }, + ) + .map_err(to_py_err) + } + + fn unload_custom_trained_traj(&self, py: Python<'_>, tid: String) -> PyResult<()> { + let client = Arc::clone(&self.client); + wait_for_future( + py, + async move { client.unload_custom_trained_traj(tid).await }, + ) + .map_err(to_py_err) + } + + fn enter_wbc_gait(&self, py: Python<'_>) -> PyResult<()> { + let client = Arc::clone(&self.client); + wait_for_future(py, async move { client.enter_wbc_gait().await }).map_err(to_py_err) + } + + fn exit_wbc_gait(&self, py: Python<'_>) -> PyResult<()> { + let client = Arc::clone(&self.client); + wait_for_future(py, async move { client.exit_wbc_gait().await }).map_err(to_py_err) + } + + fn publish_gripper_command(&self, command: PyGripperCommand) -> PyResult<()> { + let command: GripperCommand = command.into(); + self.client + .publish_gripper_command(&command) + .map_err(to_py_err) + } + + fn publish_gripper( + &self, + hand: PyHand, + mode: PyGripperMode, + motion_param: u16, + speed: Option, + ) -> PyResult<()> { + let command = GripperCommand { + hand: hand.into(), + mode: mode.into(), + motion_param, + speed: speed.unwrap_or(500), + }; + self.client + .publish_gripper_command(&command) + .map_err(to_py_err) + } +} + +pub(crate) fn register(m: &Bound<'_, PyModule>) -> PyResult<()> { + m.add_class::()?; + Ok(()) +} diff --git a/booster_sdk_py/src/client/light_control.rs b/booster_sdk_py/src/client/light_control.rs new file mode 100644 index 0000000..34b7fe0 --- /dev/null +++ b/booster_sdk_py/src/client/light_control.rs @@ -0,0 +1,45 @@ +use std::sync::Arc; + +use booster_sdk::client::light_control::LightControlClient; +use pyo3::{Bound, prelude::*, types::PyModule}; + +use crate::{runtime::wait_for_future, to_py_err}; + +#[pyclass( + module = "booster_sdk_bindings", + name = "LightControlClient", + unsendable +)] +pub struct PyLightControlClient { + client: Arc, +} + +#[pymethods] +impl PyLightControlClient { + #[new] + fn new() -> PyResult { + Ok(Self { + client: Arc::new(LightControlClient::new().map_err(to_py_err)?), + }) + } + + fn set_led_light_color(&self, py: Python<'_>, r: u8, g: u8, b: u8) -> PyResult<()> { + let client = Arc::clone(&self.client); + wait_for_future(py, async move { client.set_led_light_color(r, g, b).await }) + .map_err(to_py_err) + } + + fn set_led_light_color_param(&self, py: Python<'_>, r: u8, g: u8, b: u8) -> PyResult<()> { + self.set_led_light_color(py, r, g, b) + } + + fn stop_led_light_control(&self, py: Python<'_>) -> PyResult<()> { + let client = Arc::clone(&self.client); + wait_for_future(py, async move { client.stop_led_light_control().await }).map_err(to_py_err) + } +} + +pub(crate) fn register(m: &Bound<'_, PyModule>) -> PyResult<()> { + m.add_class::()?; + Ok(()) +} diff --git a/booster_sdk_py/src/client/lui.rs b/booster_sdk_py/src/client/lui.rs new file mode 100644 index 0000000..146b2e2 --- /dev/null +++ b/booster_sdk_py/src/client/lui.rs @@ -0,0 +1,53 @@ +use std::sync::Arc; + +use booster_sdk::client::ai::LuiClient; +use pyo3::{Bound, prelude::*, types::PyModule}; + +use crate::{PyLuiTtsConfig, PyLuiTtsParameter, runtime::wait_for_future, to_py_err}; + +#[pyclass(module = "booster_sdk_bindings", name = "LuiClient", unsendable)] +pub struct PyLuiClient { + client: Arc, +} + +#[pymethods] +impl PyLuiClient { + #[new] + fn new() -> PyResult { + Ok(Self { + client: Arc::new(LuiClient::new().map_err(to_py_err)?), + }) + } + + fn start_asr(&self, py: Python<'_>) -> PyResult<()> { + let client = Arc::clone(&self.client); + wait_for_future(py, async move { client.start_asr().await }).map_err(to_py_err) + } + + fn stop_asr(&self, py: Python<'_>) -> PyResult<()> { + let client = Arc::clone(&self.client); + wait_for_future(py, async move { client.stop_asr().await }).map_err(to_py_err) + } + + fn start_tts(&self, py: Python<'_>, config: PyLuiTtsConfig) -> PyResult<()> { + let client = Arc::clone(&self.client); + let config = config.into(); + wait_for_future(py, async move { client.start_tts(&config).await }).map_err(to_py_err) + } + + fn stop_tts(&self, py: Python<'_>) -> PyResult<()> { + let client = Arc::clone(&self.client); + wait_for_future(py, async move { client.stop_tts().await }).map_err(to_py_err) + } + + fn send_tts_text(&self, py: Python<'_>, param: PyLuiTtsParameter) -> PyResult<()> { + let client = Arc::clone(&self.client); + let param = param.into(); + wait_for_future(py, async move { client.send_tts_text(¶m).await }).map_err(to_py_err) + } +} + +pub(crate) fn register(m: &Bound<'_, PyModule>) -> PyResult<()> { + m.add_class::()?; + Ok(()) +} diff --git a/booster_sdk_py/src/client/mod.rs b/booster_sdk_py/src/client/mod.rs new file mode 100644 index 0000000..51189f8 --- /dev/null +++ b/booster_sdk_py/src/client/mod.rs @@ -0,0 +1,18 @@ +mod ai; +mod booster; +mod light_control; +mod lui; +mod vision; +mod x5_camera; + +use pyo3::{Bound, PyResult, types::PyModule}; + +pub(crate) fn register_classes(m: &Bound<'_, PyModule>) -> PyResult<()> { + booster::register(m)?; + ai::register(m)?; + lui::register(m)?; + light_control::register(m)?; + vision::register(m)?; + x5_camera::register(m)?; + Ok(()) +} diff --git a/booster_sdk_py/src/client/vision.rs b/booster_sdk_py/src/client/vision.rs new file mode 100644 index 0000000..ea3136b --- /dev/null +++ b/booster_sdk_py/src/client/vision.rs @@ -0,0 +1,67 @@ +use std::sync::Arc; + +use booster_sdk::client::vision::VisionClient; +use pyo3::{Bound, prelude::*, types::PyModule}; + +use crate::{PyDetectResults, runtime::wait_for_future, to_py_err}; + +#[pyclass(module = "booster_sdk_bindings", name = "VisionClient", unsendable)] +pub struct PyVisionClient { + client: Arc, +} + +#[pymethods] +impl PyVisionClient { + #[new] + fn new() -> PyResult { + Ok(Self { + client: Arc::new(VisionClient::new().map_err(to_py_err)?), + }) + } + + fn start_vision_service( + &self, + py: Python<'_>, + enable_position: bool, + enable_color: bool, + enable_face_detection: bool, + ) -> PyResult<()> { + let client = Arc::clone(&self.client); + wait_for_future(py, async move { + client + .start_vision_service(enable_position, enable_color, enable_face_detection) + .await + }) + .map_err(to_py_err) + } + + fn stop_vision_service(&self, py: Python<'_>) -> PyResult<()> { + let client = Arc::clone(&self.client); + wait_for_future(py, async move { client.stop_vision_service().await }).map_err(to_py_err) + } + + fn get_detection_object_with_ratio( + &self, + py: Python<'_>, + focus_ratio: f32, + ) -> PyResult> { + let client = Arc::clone(&self.client); + wait_for_future(py, async move { + client.get_detection_object_with_ratio(focus_ratio).await + }) + .map(|results| results.into_iter().map(Into::into).collect()) + .map_err(to_py_err) + } + + fn get_detection_object(&self, py: Python<'_>) -> PyResult> { + let client = Arc::clone(&self.client); + wait_for_future(py, async move { client.get_detection_object().await }) + .map(|results| results.into_iter().map(Into::into).collect()) + .map_err(to_py_err) + } +} + +pub(crate) fn register(m: &Bound<'_, PyModule>) -> PyResult<()> { + m.add_class::()?; + Ok(()) +} diff --git a/booster_sdk_py/src/client/x5_camera.rs b/booster_sdk_py/src/client/x5_camera.rs new file mode 100644 index 0000000..09db5a9 --- /dev/null +++ b/booster_sdk_py/src/client/x5_camera.rs @@ -0,0 +1,38 @@ +use std::sync::Arc; + +use booster_sdk::client::x5_camera::X5CameraClient; +use pyo3::{Bound, prelude::*, types::PyModule}; + +use crate::{PyCameraSetMode, PyX5CameraGetStatusResponse, runtime::wait_for_future, to_py_err}; + +#[pyclass(module = "booster_sdk_bindings", name = "X5CameraClient", unsendable)] +pub struct PyX5CameraClient { + client: Arc, +} + +#[pymethods] +impl PyX5CameraClient { + #[new] + fn new() -> PyResult { + Ok(Self { + client: Arc::new(X5CameraClient::new().map_err(to_py_err)?), + }) + } + + fn change_mode(&self, py: Python<'_>, mode: PyCameraSetMode) -> PyResult<()> { + let client = Arc::clone(&self.client); + wait_for_future(py, async move { client.change_mode(mode.into()).await }).map_err(to_py_err) + } + + fn get_status(&self, py: Python<'_>) -> PyResult { + let client = Arc::clone(&self.client); + wait_for_future(py, async move { client.get_status().await }) + .map(Into::into) + .map_err(to_py_err) + } +} + +pub(crate) fn register(m: &Bound<'_, PyModule>) -> PyResult<()> { + m.add_class::()?; + Ok(()) +} diff --git a/booster_sdk_py/src/lib.rs b/booster_sdk_py/src/lib.rs index 91619a3..2977c3c 100644 --- a/booster_sdk_py/src/lib.rs +++ b/booster_sdk_py/src/lib.rs @@ -1,14 +1,17 @@ +mod client; mod runtime; -use crate::runtime::wait_for_future; -use std::sync::Arc; - -use ::booster_sdk::{ +use booster_sdk::{ client::{ - AiClient, AsrConfig, BOOSTER_ROBOT_USER_ID, CameraControlStatus, CameraSetMode, - DetectResults, GetStatusResponse as X5CameraStatusResponse, LightControlClient, LlmConfig, - LuiClient, LuiTtsConfig, LuiTtsParameter, SpeakParameter, StartAiChatParameter, TtsConfig, - VisionClient, X5CameraClient, + ai::{ + AsrConfig, BOOSTER_ROBOT_USER_ID, LlmConfig, LuiTtsConfig, LuiTtsParameter, + SpeakParameter, StartAiChatParameter, TtsConfig, + }, + loco::GripperCommand, + vision::DetectResults, + x5_camera::{ + CameraControlStatus, CameraSetMode, GetStatusResponse as X5CameraStatusResponse, + }, }, types::{ Action, BodyControl, BoosterError, BoosterHandType, CustomModel, CustomModelParams, @@ -20,11 +23,9 @@ use ::booster_sdk::{ }; use pyo3::{exceptions::PyException, prelude::*, types::PyModule}; -use ::booster_sdk::client::{BoosterClient, GripperCommand}; - pyo3::create_exception!(booster_sdk_bindings, BoosterSdkError, PyException); -fn to_py_err(err: BoosterError) -> PyErr { +pub(crate) fn to_py_err(err: BoosterError) -> PyErr { BoosterSdkError::new_err(err.to_string()) } @@ -681,6 +682,12 @@ impl PyGripperCommand { } } +impl From for GripperCommand { + fn from(value: PyGripperCommand) -> Self { + value.0 + } +} + #[pyclass(module = "booster_sdk_bindings", name = "Position")] #[derive(Clone, Copy)] pub struct PyPosition(Position); @@ -1691,601 +1698,12 @@ impl From for PyDetectResults { } } -#[pyclass(module = "booster_sdk_bindings", name = "BoosterClient", unsendable)] -pub struct PyBoosterClient { - client: Arc, -} - -#[pymethods] -impl PyBoosterClient { - #[new] - fn new() -> PyResult { - Ok(Self { - client: Arc::new(BoosterClient::new().map_err(to_py_err)?), - }) - } - - fn change_mode(&self, py: Python<'_>, mode: PyRobotMode) -> PyResult<()> { - let client = Arc::clone(&self.client); - wait_for_future(py, async move { client.change_mode(mode.into()).await }).map_err(to_py_err) - } - - fn get_mode(&self, py: Python<'_>) -> PyResult { - let client = Arc::clone(&self.client); - wait_for_future(py, async move { client.get_mode().await }) - .map(Into::into) - .map_err(to_py_err) - } - - fn get_status(&self, py: Python<'_>) -> PyResult { - let client = Arc::clone(&self.client); - wait_for_future(py, async move { client.get_status().await }) - .map(Into::into) - .map_err(to_py_err) - } - - fn get_robot_info(&self, py: Python<'_>) -> PyResult { - let client = Arc::clone(&self.client); - wait_for_future(py, async move { client.get_robot_info().await }) - .map(Into::into) - .map_err(to_py_err) - } - - fn move_robot(&self, py: Python<'_>, vx: f32, vy: f32, vyaw: f32) -> PyResult<()> { - let client = Arc::clone(&self.client); - wait_for_future(py, async move { client.move_robot(vx, vy, vyaw).await }).map_err(to_py_err) - } - - fn rotate_head(&self, py: Python<'_>, pitch: f32, yaw: f32) -> PyResult<()> { - let client = Arc::clone(&self.client); - wait_for_future(py, async move { client.rotate_head(pitch, yaw).await }).map_err(to_py_err) - } - - fn wave_hand(&self, py: Python<'_>, action: PyHandAction) -> PyResult<()> { - let client = Arc::clone(&self.client); - wait_for_future(py, async move { client.wave_hand(action.into()).await }).map_err(to_py_err) - } - - fn rotate_head_with_direction( - &self, - py: Python<'_>, - pitch_direction: i32, - yaw_direction: i32, - ) -> PyResult<()> { - let client = Arc::clone(&self.client); - wait_for_future(py, async move { - client - .rotate_head_with_direction(pitch_direction, yaw_direction) - .await - }) - .map_err(to_py_err) - } - - fn lie_down(&self, py: Python<'_>) -> PyResult<()> { - let client = Arc::clone(&self.client); - wait_for_future(py, async move { client.lie_down().await }).map_err(to_py_err) - } - - fn get_up(&self, py: Python<'_>) -> PyResult<()> { - let client = Arc::clone(&self.client); - wait_for_future(py, async move { client.get_up().await }).map_err(to_py_err) - } - - fn get_up_with_mode(&self, py: Python<'_>, mode: PyRobotMode) -> PyResult<()> { - let client = Arc::clone(&self.client); - wait_for_future( - py, - async move { client.get_up_with_mode(mode.into()).await }, - ) - .map_err(to_py_err) - } - - fn shoot(&self, py: Python<'_>) -> PyResult<()> { - let client = Arc::clone(&self.client); - wait_for_future(py, async move { client.shoot().await }).map_err(to_py_err) - } - - fn push_up(&self, py: Python<'_>) -> PyResult<()> { - let client = Arc::clone(&self.client); - wait_for_future(py, async move { client.push_up().await }).map_err(to_py_err) - } - - fn move_hand_end_effector_with_aux( - &self, - py: Python<'_>, - target_posture: PyPosture, - aux_posture: PyPosture, - time_millis: i32, - hand_index: PyHand, - ) -> PyResult<()> { - let client = Arc::clone(&self.client); - let target_posture: Posture = target_posture.into(); - let aux_posture: Posture = aux_posture.into(); - let hand_index: Hand = hand_index.into(); - wait_for_future(py, async move { - client - .move_hand_end_effector_with_aux( - &target_posture, - &aux_posture, - time_millis, - hand_index, - ) - .await - }) - .map_err(to_py_err) - } - - fn move_hand_end_effector( - &self, - py: Python<'_>, - target_posture: PyPosture, - time_millis: i32, - hand_index: PyHand, - ) -> PyResult<()> { - let client = Arc::clone(&self.client); - let target_posture: Posture = target_posture.into(); - let hand_index: Hand = hand_index.into(); - wait_for_future(py, async move { - client - .move_hand_end_effector(&target_posture, time_millis, hand_index) - .await - }) - .map_err(to_py_err) - } - - fn move_hand_end_effector_v2( - &self, - py: Python<'_>, - target_posture: PyPosture, - time_millis: i32, - hand_index: PyHand, - ) -> PyResult<()> { - let client = Arc::clone(&self.client); - let target_posture: Posture = target_posture.into(); - let hand_index: Hand = hand_index.into(); - wait_for_future(py, async move { - client - .move_hand_end_effector_v2(&target_posture, time_millis, hand_index) - .await - }) - .map_err(to_py_err) - } - - fn stop_hand_end_effector(&self, py: Python<'_>) -> PyResult<()> { - let client = Arc::clone(&self.client); - wait_for_future(py, async move { client.stop_hand_end_effector().await }).map_err(to_py_err) - } - - fn control_gripper( - &self, - py: Python<'_>, - motion_param: PyGripperMotionParameter, - mode: PyGripperControlMode, - hand_index: PyHand, - ) -> PyResult<()> { - let client = Arc::clone(&self.client); - let motion_param: GripperMotionParameter = motion_param.into(); - let mode: GripperControlMode = mode.into(); - let hand_index: Hand = hand_index.into(); - wait_for_future(py, async move { - client.control_gripper(motion_param, mode, hand_index).await - }) - .map_err(to_py_err) - } - - fn get_frame_transform( - &self, - py: Python<'_>, - src: PyFrame, - dst: PyFrame, - ) -> PyResult { - let client = Arc::clone(&self.client); - let src: Frame = src.into(); - let dst: Frame = dst.into(); - wait_for_future( - py, - async move { client.get_frame_transform(src, dst).await }, - ) - .map(Into::into) - .map_err(to_py_err) - } - - fn switch_hand_end_effector_control_mode( - &self, - py: Python<'_>, - switch_on: bool, - ) -> PyResult<()> { - let client = Arc::clone(&self.client); - wait_for_future(py, async move { - client - .switch_hand_end_effector_control_mode(switch_on) - .await - }) - .map_err(to_py_err) - } - - fn handshake(&self, py: Python<'_>, action: PyHandAction) -> PyResult<()> { - let client = Arc::clone(&self.client); - wait_for_future(py, async move { client.handshake(action.into()).await }).map_err(to_py_err) - } - - fn control_dexterous_hand( - &self, - py: Python<'_>, - finger_params: Vec, - hand_index: PyHand, - hand_type: PyBoosterHandType, - ) -> PyResult<()> { - let client = Arc::clone(&self.client); - let finger_params: Vec = - finger_params.into_iter().map(Into::into).collect(); - let hand_index: Hand = hand_index.into(); - let hand_type: BoosterHandType = hand_type.into(); - wait_for_future(py, async move { - client - .control_dexterous_hand(&finger_params, hand_index, hand_type) - .await - }) - .map_err(to_py_err) - } - - fn control_dexterous_hand_default( - &self, - py: Python<'_>, - finger_params: Vec, - hand_index: PyHand, - ) -> PyResult<()> { - let client = Arc::clone(&self.client); - let finger_params: Vec = - finger_params.into_iter().map(Into::into).collect(); - let hand_index: Hand = hand_index.into(); - wait_for_future(py, async move { - client - .control_dexterous_hand_default(&finger_params, hand_index) - .await - }) - .map_err(to_py_err) - } - - fn dance(&self, py: Python<'_>, dance_id: PyDanceId) -> PyResult<()> { - let client = Arc::clone(&self.client); - wait_for_future(py, async move { client.dance(dance_id.into()).await }).map_err(to_py_err) - } - - fn play_sound(&self, py: Python<'_>, sound_file_path: String) -> PyResult<()> { - let client = Arc::clone(&self.client); - wait_for_future(py, async move { client.play_sound(sound_file_path).await }) - .map_err(to_py_err) - } - - fn stop_sound(&self, py: Python<'_>) -> PyResult<()> { - let client = Arc::clone(&self.client); - wait_for_future(py, async move { client.stop_sound().await }).map_err(to_py_err) - } - - fn zero_torque_drag(&self, py: Python<'_>, active: bool) -> PyResult<()> { - let client = Arc::clone(&self.client); - wait_for_future(py, async move { client.zero_torque_drag(active).await }).map_err(to_py_err) - } - - fn record_trajectory(&self, py: Python<'_>, active: bool) -> PyResult<()> { - let client = Arc::clone(&self.client); - wait_for_future(py, async move { client.record_trajectory(active).await }) - .map_err(to_py_err) - } - - fn replay_trajectory(&self, py: Python<'_>, traj_file_path: String) -> PyResult<()> { - let client = Arc::clone(&self.client); - wait_for_future( - py, - async move { client.replay_trajectory(traj_file_path).await }, - ) - .map_err(to_py_err) - } - - fn whole_body_dance(&self, py: Python<'_>, dance_id: PyWholeBodyDanceId) -> PyResult<()> { - let client = Arc::clone(&self.client); - wait_for_future( - py, - async move { client.whole_body_dance(dance_id.into()).await }, - ) - .map_err(to_py_err) - } - - fn upper_body_custom_control(&self, py: Python<'_>, start: bool) -> PyResult<()> { - let client = Arc::clone(&self.client); - wait_for_future( - py, - async move { client.upper_body_custom_control(start).await }, - ) - .map_err(to_py_err) - } - - fn reset_odometry(&self, py: Python<'_>) -> PyResult<()> { - let client = Arc::clone(&self.client); - wait_for_future(py, async move { client.reset_odometry().await }).map_err(to_py_err) - } - - fn load_custom_trained_traj( - &self, - py: Python<'_>, - traj: PyCustomTrainedTraj, - ) -> PyResult { - let client = Arc::clone(&self.client); - let traj: CustomTrainedTraj = traj.into(); - wait_for_future( - py, - async move { client.load_custom_trained_traj(&traj).await }, - ) - .map(Into::into) - .map_err(to_py_err) - } - - fn activate_custom_trained_traj(&self, py: Python<'_>, tid: String) -> PyResult<()> { - let client = Arc::clone(&self.client); - wait_for_future( - py, - async move { client.activate_custom_trained_traj(tid).await }, - ) - .map_err(to_py_err) - } - - fn unload_custom_trained_traj(&self, py: Python<'_>, tid: String) -> PyResult<()> { - let client = Arc::clone(&self.client); - wait_for_future( - py, - async move { client.unload_custom_trained_traj(tid).await }, - ) - .map_err(to_py_err) - } - - fn enter_wbc_gait(&self, py: Python<'_>) -> PyResult<()> { - let client = Arc::clone(&self.client); - wait_for_future(py, async move { client.enter_wbc_gait().await }).map_err(to_py_err) - } - - fn exit_wbc_gait(&self, py: Python<'_>) -> PyResult<()> { - let client = Arc::clone(&self.client); - wait_for_future(py, async move { client.exit_wbc_gait().await }).map_err(to_py_err) - } - - fn publish_gripper_command(&self, command: PyGripperCommand) -> PyResult<()> { - self.client - .publish_gripper_command(&command.0) - .map_err(to_py_err) - } - - fn publish_gripper( - &self, - hand: PyHand, - mode: PyGripperMode, - motion_param: u16, - speed: Option, - ) -> PyResult<()> { - let command = GripperCommand { - hand: hand.into(), - mode: mode.into(), - motion_param, - speed: speed.unwrap_or(500), - }; - self.client - .publish_gripper_command(&command) - .map_err(to_py_err) - } -} - -#[pyclass(module = "booster_sdk_bindings", name = "AiClient", unsendable)] -pub struct PyAiClient { - client: Arc, -} - -#[pymethods] -impl PyAiClient { - #[new] - fn new() -> PyResult { - Ok(Self { - client: Arc::new(AiClient::new().map_err(to_py_err)?), - }) - } - - fn start_ai_chat(&self, py: Python<'_>, param: PyStartAiChatParameter) -> PyResult<()> { - let client = Arc::clone(&self.client); - let param: StartAiChatParameter = param.into(); - wait_for_future(py, async move { client.start_ai_chat(¶m).await }).map_err(to_py_err) - } - - fn stop_ai_chat(&self, py: Python<'_>) -> PyResult<()> { - let client = Arc::clone(&self.client); - wait_for_future(py, async move { client.stop_ai_chat().await }).map_err(to_py_err) - } - - fn speak(&self, py: Python<'_>, param: PySpeakParameter) -> PyResult<()> { - let client = Arc::clone(&self.client); - let param: SpeakParameter = param.into(); - wait_for_future(py, async move { client.speak(¶m).await }).map_err(to_py_err) - } - - fn start_face_tracking(&self, py: Python<'_>) -> PyResult<()> { - let client = Arc::clone(&self.client); - wait_for_future(py, async move { client.start_face_tracking().await }).map_err(to_py_err) - } - - fn stop_face_tracking(&self, py: Python<'_>) -> PyResult<()> { - let client = Arc::clone(&self.client); - wait_for_future(py, async move { client.stop_face_tracking().await }).map_err(to_py_err) - } -} - -#[pyclass(module = "booster_sdk_bindings", name = "LuiClient", unsendable)] -pub struct PyLuiClient { - client: Arc, -} - -#[pymethods] -impl PyLuiClient { - #[new] - fn new() -> PyResult { - Ok(Self { - client: Arc::new(LuiClient::new().map_err(to_py_err)?), - }) - } - - fn start_asr(&self, py: Python<'_>) -> PyResult<()> { - let client = Arc::clone(&self.client); - wait_for_future(py, async move { client.start_asr().await }).map_err(to_py_err) - } - - fn stop_asr(&self, py: Python<'_>) -> PyResult<()> { - let client = Arc::clone(&self.client); - wait_for_future(py, async move { client.stop_asr().await }).map_err(to_py_err) - } - - fn start_tts(&self, py: Python<'_>, config: PyLuiTtsConfig) -> PyResult<()> { - let client = Arc::clone(&self.client); - let config: LuiTtsConfig = config.into(); - wait_for_future(py, async move { client.start_tts(&config).await }).map_err(to_py_err) - } - - fn stop_tts(&self, py: Python<'_>) -> PyResult<()> { - let client = Arc::clone(&self.client); - wait_for_future(py, async move { client.stop_tts().await }).map_err(to_py_err) - } - - fn send_tts_text(&self, py: Python<'_>, param: PyLuiTtsParameter) -> PyResult<()> { - let client = Arc::clone(&self.client); - let param: LuiTtsParameter = param.into(); - wait_for_future(py, async move { client.send_tts_text(¶m).await }).map_err(to_py_err) - } -} - -#[pyclass( - module = "booster_sdk_bindings", - name = "LightControlClient", - unsendable -)] -pub struct PyLightControlClient { - client: Arc, -} - -#[pymethods] -impl PyLightControlClient { - #[new] - fn new() -> PyResult { - Ok(Self { - client: Arc::new(LightControlClient::new().map_err(to_py_err)?), - }) - } - - fn set_led_light_color(&self, py: Python<'_>, r: u8, g: u8, b: u8) -> PyResult<()> { - let client = Arc::clone(&self.client); - wait_for_future(py, async move { client.set_led_light_color(r, g, b).await }) - .map_err(to_py_err) - } - - fn set_led_light_color_param(&self, py: Python<'_>, r: u8, g: u8, b: u8) -> PyResult<()> { - self.set_led_light_color(py, r, g, b) - } - - fn stop_led_light_control(&self, py: Python<'_>) -> PyResult<()> { - let client = Arc::clone(&self.client); - wait_for_future(py, async move { client.stop_led_light_control().await }).map_err(to_py_err) - } -} - -#[pyclass(module = "booster_sdk_bindings", name = "VisionClient", unsendable)] -pub struct PyVisionClient { - client: Arc, -} - -#[pymethods] -impl PyVisionClient { - #[new] - fn new() -> PyResult { - Ok(Self { - client: Arc::new(VisionClient::new().map_err(to_py_err)?), - }) - } - - fn start_vision_service( - &self, - py: Python<'_>, - enable_position: bool, - enable_color: bool, - enable_face_detection: bool, - ) -> PyResult<()> { - let client = Arc::clone(&self.client); - wait_for_future(py, async move { - client - .start_vision_service(enable_position, enable_color, enable_face_detection) - .await - }) - .map_err(to_py_err) - } - - fn stop_vision_service(&self, py: Python<'_>) -> PyResult<()> { - let client = Arc::clone(&self.client); - wait_for_future(py, async move { client.stop_vision_service().await }).map_err(to_py_err) - } - - fn get_detection_object_with_ratio( - &self, - py: Python<'_>, - focus_ratio: f32, - ) -> PyResult> { - let client = Arc::clone(&self.client); - wait_for_future(py, async move { - client.get_detection_object_with_ratio(focus_ratio).await - }) - .map(|results| results.into_iter().map(Into::into).collect()) - .map_err(to_py_err) - } - - fn get_detection_object(&self, py: Python<'_>) -> PyResult> { - let client = Arc::clone(&self.client); - wait_for_future(py, async move { client.get_detection_object().await }) - .map(|results| results.into_iter().map(Into::into).collect()) - .map_err(to_py_err) - } -} - -#[pyclass(module = "booster_sdk_bindings", name = "X5CameraClient", unsendable)] -pub struct PyX5CameraClient { - client: Arc, -} - -#[pymethods] -impl PyX5CameraClient { - #[new] - fn new() -> PyResult { - Ok(Self { - client: Arc::new(X5CameraClient::new().map_err(to_py_err)?), - }) - } - - fn change_mode(&self, py: Python<'_>, mode: PyCameraSetMode) -> PyResult<()> { - let client = Arc::clone(&self.client); - wait_for_future(py, async move { client.change_mode(mode.into()).await }).map_err(to_py_err) - } - - fn get_status(&self, py: Python<'_>) -> PyResult { - let client = Arc::clone(&self.client); - wait_for_future(py, async move { client.get_status().await }) - .map(Into::into) - .map_err(to_py_err) - } -} - #[pymodule] fn booster_sdk_bindings(py: Python<'_>, m: &Bound<'_, PyModule>) -> PyResult<()> { m.add("BoosterSdkError", py.get_type::())?; m.add("BOOSTER_ROBOT_USER_ID", BOOSTER_ROBOT_USER_ID)?; - m.add_class::()?; - m.add_class::()?; - m.add_class::()?; - m.add_class::()?; - m.add_class::()?; - m.add_class::()?; + client::register_classes(m)?; m.add_class::()?; m.add_class::()?; From caca31253adbe428099d1f26489a92de88e61185 Mon Sep 17 00:00:00 2001 From: Gijs de Jong Date: Sun, 22 Feb 2026 20:33:18 +0100 Subject: [PATCH 10/15] clean up import scheme for python sdk --- README.md | 3 +- booster_sdk/examples/gripper_control.rs | 2 +- booster_sdk/examples/locomotion_control.rs | 2 +- booster_sdk/examples/look_around.rs | 2 +- booster_sdk_py/booster_sdk/__init__.py | 107 ++---------------- booster_sdk_py/booster_sdk/client/__init__.py | 18 +-- 6 files changed, 14 insertions(+), 120 deletions(-) diff --git a/README.md b/README.md index c1558a1..25cb366 100644 --- a/README.md +++ b/README.md @@ -67,7 +67,8 @@ This will create a wheel file in `booster_sdk_py/dist/` that can be installed wi Note: Python bindings are intentionally minimal and expose a subset of the Rust API. ```python -from booster_sdk import BoosterClient, GripperCommand, Hand, RobotMode +from booster_sdk.client.booster import BoosterClient +from booster_sdk.types import GripperCommand, Hand, RobotMode client = BoosterClient() diff --git a/booster_sdk/examples/gripper_control.rs b/booster_sdk/examples/gripper_control.rs index 324ccbd..03ce7a5 100644 --- a/booster_sdk/examples/gripper_control.rs +++ b/booster_sdk/examples/gripper_control.rs @@ -4,7 +4,7 @@ //! //! Run with: cargo run --example `gripper_control` -use booster_sdk::client::{BoosterClient, commands::GripperCommand}; +use booster_sdk::client::loco::{BoosterClient, GripperCommand}; use booster_sdk::types::{Hand, RobotMode}; #[tokio::main] diff --git a/booster_sdk/examples/locomotion_control.rs b/booster_sdk/examples/locomotion_control.rs index 1871a76..607e6a1 100644 --- a/booster_sdk/examples/locomotion_control.rs +++ b/booster_sdk/examples/locomotion_control.rs @@ -4,7 +4,7 @@ //! //! Run with: cargo run --example `locomotion_control` -use booster_sdk::client::BoosterClient; +use booster_sdk::client::loco::BoosterClient; use booster_sdk::types::RobotMode; use tokio::time::Duration; diff --git a/booster_sdk/examples/look_around.rs b/booster_sdk/examples/look_around.rs index a45bc8d..6b73b6e 100644 --- a/booster_sdk/examples/look_around.rs +++ b/booster_sdk/examples/look_around.rs @@ -4,7 +4,7 @@ //! //! Run with: cargo run --example `look_around` -use booster_sdk::client::BoosterClient; +use booster_sdk::client::loco::BoosterClient; use tokio::time::Duration; #[tokio::main] diff --git a/booster_sdk_py/booster_sdk/__init__.py b/booster_sdk_py/booster_sdk/__init__.py index be94a06..84ca893 100644 --- a/booster_sdk_py/booster_sdk/__init__.py +++ b/booster_sdk_py/booster_sdk/__init__.py @@ -1,103 +1,10 @@ -"""Booster SDK Python package.""" +"""Booster SDK Python package. -from __future__ import annotations +Use module-scoped imports: +- `booster_sdk.client.` +- `booster_sdk.types` +""" -from .client import ( - AiClient, - BoosterClient, - LightControlClient, - LuiClient, - VisionClient, - X5CameraClient, -) -from .types import ( - BOOSTER_ROBOT_USER_ID, - Action, - AsrConfig, - BoosterHandType, - BoosterSdkError, - BodyControl, - CameraControlStatus, - CameraSetMode, - CustomModel, - CustomModelParams, - CustomTrainedTraj, - DanceId, - DetectResults, - DexterousFingerParameter, - Frame, - GetModeResponse, - GetRobotInfoResponse, - GetStatusResponse, - GripperCommand, - GripperControlMode, - GripperMode, - GripperMotionParameter, - Hand, - HandAction, - JointOrder, - LlmConfig, - LoadCustomTrainedTrajResponse, - LuiTtsConfig, - LuiTtsParameter, - Orientation, - Position, - Posture, - Quaternion, - RobotMode, - SpeakParameter, - StartAiChatParameter, - Transform, - TtsConfig, - WholeBodyDanceId, - X5CameraGetStatusResponse, -) +from __future__ import annotations -__all__ = [ - "BoosterClient", - "AiClient", - "LuiClient", - "LightControlClient", - "VisionClient", - "X5CameraClient", - "BOOSTER_ROBOT_USER_ID", - "BoosterSdkError", - "RobotMode", - "Hand", - "GripperMode", - "GripperCommand", - "HandAction", - "Frame", - "GripperControlMode", - "BoosterHandType", - "DanceId", - "WholeBodyDanceId", - "JointOrder", - "BodyControl", - "Action", - "CameraSetMode", - "CameraControlStatus", - "Position", - "Orientation", - "Posture", - "Quaternion", - "Transform", - "GripperMotionParameter", - "DexterousFingerParameter", - "CustomModelParams", - "CustomModel", - "CustomTrainedTraj", - "TtsConfig", - "LlmConfig", - "AsrConfig", - "StartAiChatParameter", - "SpeakParameter", - "LuiTtsConfig", - "LuiTtsParameter", - "GetModeResponse", - "GetStatusResponse", - "GetRobotInfoResponse", - "LoadCustomTrainedTrajResponse", - "X5CameraGetStatusResponse", - "DetectResults", -] +__all__ = ["client", "types"] diff --git a/booster_sdk_py/booster_sdk/client/__init__.py b/booster_sdk_py/booster_sdk/client/__init__.py index 31856cd..6e49639 100644 --- a/booster_sdk_py/booster_sdk/client/__init__.py +++ b/booster_sdk_py/booster_sdk/client/__init__.py @@ -1,19 +1,5 @@ -"""High-level client classes for the Booster SDK.""" +"""Client modules for the Booster SDK.""" from __future__ import annotations -from .ai import AiClient -from .booster import BoosterClient -from .light_control import LightControlClient -from .lui import LuiClient -from .vision import VisionClient -from .x5_camera import X5CameraClient - -__all__ = [ - "BoosterClient", - "AiClient", - "LuiClient", - "LightControlClient", - "VisionClient", - "X5CameraClient", -] +__all__ = ["ai", "booster", "light_control", "lui", "vision", "x5_camera"] From 4e77d83774f388aab922d8433044a905659ebb17 Mon Sep 17 00:00:00 2001 From: Gijs de Jong Date: Sun, 22 Feb 2026 20:44:22 +0100 Subject: [PATCH 11/15] update readme --- README.md | 34 ++++++++++------------------------ 1 file changed, 10 insertions(+), 24 deletions(-) diff --git a/README.md b/README.md index 25cb366..8ab8bdf 100644 --- a/README.md +++ b/README.md @@ -1,4 +1,4 @@ -# Booster Robotics Rust SDK +# Booster Robotics SDK [![License](https://img.shields.io/badge/license-MIT%2FApache-blue.svg)](https://github.com/IntelligentRoboticsLab/booster_sdk#license) [![Crates.io](https://img.shields.io/crates/v/booster_sdk.svg)](https://crates.io/crates/booster_sdk) @@ -6,12 +6,13 @@ [![Docs](https://docs.rs/booster_sdk/badge.svg)](https://docs.rs/booster_sdk/latest/booster_sdk/) [![PyPI](https://img.shields.io/pypi/v/booster_sdk.svg)](https://pypi.org/project/booster-sdk/) -A Rust SDK for controlling Booster robots based on [Booster Robotics C++ SDK](https://github.com/BoosterRobotics/booster_robotics_sdk). +This project is a Rust reimplementation of the original [Booster Robotics C++ SDK (`booster_robotics_sdk`)](https://github.com/BoosterRobotics/booster_robotics_sdk) for controlling Booster robots. + +In addition to the Rust crate, this repository also provides Python bindings built on top of the Rust implementation. ## 🚧 Project Status -This library is currently in early development. The core architecture and types are defined, but none of it has been tested on -an actual robot yet. The DDS transport layer is implemented using RustDDS. +This library is currently in active development and has been tested on a real robot. ## API Examples @@ -45,26 +46,15 @@ async fn main() -> Result<(), Box> { ## Experimental Python Bindings -Python bindings for the SDK are available using [PyO3](https://github.com/PyO3/pyo3). These bindings are very experimental! - -### Requirements - -- Python 3.10 or higher -- Rust toolchain (for building from source) - -### Installation - -The Python package can be built using pixi: +Python wheels are available on [PyPI](https://pypi.org/project/booster-sdk/): ```bash -pixi run py-build-wheel +pip install booster-sdk ``` -This will create a wheel file in `booster_sdk_py/dist/` that can be installed with `pip install booster_sdk_py/dist/*.whl`. - ### Python API Example -Note: Python bindings are intentionally minimal and expose a subset of the Rust API. +Note: Python bindings are experimental. ```python from booster_sdk.client.booster import BoosterClient @@ -82,12 +72,8 @@ client.move_robot(0.5, 0.0, 0.0) client.publish_gripper_command(GripperCommand.open(Hand.RIGHT)) ``` -The Python bindings currently cover basic mode changes, locomotion, and gripper control. - -# DDS Setup - -The Rust SDK communicates directly over DDS (RustDDS). Please refer to the [DDS Setup Guide](docs/dds_setup.md) for detailed instructions. +The Python bindings currently cover core control flows, including locomotion, gripper control, AI/LUI RPC calls, vision RPC calls, and X5 camera RPC calls. ## Contributing -This SDK is currently in early development. Contributions are welcome! Please open issues or pull requests for bug fixes, features, or documentation improvements. +This SDK is in active development. Contributions are welcome! Please open issues or pull requests for bug fixes, features, or documentation improvements. From 4803708e5024ca0698d73dd0ded55f7b7808e907 Mon Sep 17 00:00:00 2001 From: Gijs de Jong Date: Sun, 22 Feb 2026 20:45:51 +0100 Subject: [PATCH 12/15] version 0.1.0-alpha.7 --- Cargo.lock | 4 ++-- Cargo.toml | 2 +- pixi.toml | 2 +- 3 files changed, 4 insertions(+), 4 deletions(-) diff --git a/Cargo.lock b/Cargo.lock index 55be2fa..63f46bc 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -46,7 +46,7 @@ checksum = "812e12b5285cc515a9c72a5c1d3b6d46a19dac5acfef5265968c166106e31dd3" [[package]] name = "booster_sdk" -version = "0.1.0-alpha.6" +version = "0.1.0-alpha.7" dependencies = [ "rustdds", "serde", @@ -61,7 +61,7 @@ dependencies = [ [[package]] name = "booster_sdk_py" -version = "0.1.0-alpha.6" +version = "0.1.0-alpha.7" dependencies = [ "booster_sdk", "pyo3", diff --git a/Cargo.toml b/Cargo.toml index 00bc78b..fc8d7f0 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -3,7 +3,7 @@ members = ["booster_sdk", "booster_sdk_py"] resolver = "2" [workspace.package] -version = "0.1.0-alpha.6" +version = "0.1.0-alpha.7" edition = "2024" authors = ["Team whIRLwind"] license = "MIT OR Apache-2.0" diff --git a/pixi.toml b/pixi.toml index a717c4e..9450f03 100644 --- a/pixi.toml +++ b/pixi.toml @@ -3,7 +3,7 @@ authors = ["Team whIRLwind"] channels = ["conda-forge"] name = "booster-sdk" platforms = ["osx-arm64", "linux-64", "linux-aarch64"] -version = "0.1.0-alpha.6" +version = "0.1.0-alpha.7" [environments] py = ["wheel-build", "python-tasks"] From fd82bea742e75059a873f0cc95b46ae0152f87b1 Mon Sep 17 00:00:00 2001 From: Gijs de Jong Date: Sun, 22 Feb 2026 20:54:38 +0100 Subject: [PATCH 13/15] clean up rust sdk bindings --- booster_sdk_py/src/client/ai.rs | 188 ++- booster_sdk_py/src/client/booster.rs | 1288 +++++++++++++++++- booster_sdk_py/src/client/lui.rs | 52 +- booster_sdk_py/src/client/vision.rs | 89 +- booster_sdk_py/src/client/x5_camera.rs | 126 +- booster_sdk_py/src/lib.rs | 1732 +----------------------- 6 files changed, 1727 insertions(+), 1748 deletions(-) diff --git a/booster_sdk_py/src/client/ai.rs b/booster_sdk_py/src/client/ai.rs index fd30213..62086c2 100644 --- a/booster_sdk_py/src/client/ai.rs +++ b/booster_sdk_py/src/client/ai.rs @@ -1,9 +1,188 @@ use std::sync::Arc; -use booster_sdk::client::ai::AiClient; +use booster_sdk::client::ai::{ + AiClient, AsrConfig, LlmConfig, SpeakParameter, StartAiChatParameter, TtsConfig, +}; use pyo3::{Bound, prelude::*, types::PyModule}; -use crate::{PySpeakParameter, PyStartAiChatParameter, runtime::wait_for_future, to_py_err}; +use crate::{runtime::wait_for_future, to_py_err}; + +#[pyclass(module = "booster_sdk_bindings", name = "TtsConfig")] +#[derive(Clone)] +pub struct PyTtsConfig(TtsConfig); + +#[pymethods] +impl PyTtsConfig { + #[new] + fn new(voice_type: String, ignore_bracket_text: Vec) -> Self { + Self(TtsConfig { + voice_type, + ignore_bracket_text, + }) + } + + #[getter] + fn voice_type(&self) -> String { + self.0.voice_type.clone() + } + + #[getter] + fn ignore_bracket_text(&self) -> Vec { + self.0.ignore_bracket_text.clone() + } +} + +impl From for TtsConfig { + fn from(value: PyTtsConfig) -> Self { + value.0 + } +} + +#[pyclass(module = "booster_sdk_bindings", name = "LlmConfig")] +#[derive(Clone)] +pub struct PyLlmConfig(LlmConfig); + +#[pymethods] +impl PyLlmConfig { + #[new] + fn new(system_prompt: String, welcome_msg: String, prompt_name: String) -> Self { + Self(LlmConfig { + system_prompt, + welcome_msg, + prompt_name, + }) + } + + #[getter] + fn system_prompt(&self) -> String { + self.0.system_prompt.clone() + } + + #[getter] + fn welcome_msg(&self) -> String { + self.0.welcome_msg.clone() + } + + #[getter] + fn prompt_name(&self) -> String { + self.0.prompt_name.clone() + } +} + +impl From for LlmConfig { + fn from(value: PyLlmConfig) -> Self { + value.0 + } +} + +#[pyclass(module = "booster_sdk_bindings", name = "AsrConfig")] +#[derive(Clone)] +pub struct PyAsrConfig(AsrConfig); + +#[pymethods] +impl PyAsrConfig { + #[new] + fn new(interrupt_speech_duration: i32, interrupt_keywords: Vec) -> Self { + Self(AsrConfig { + interrupt_speech_duration, + interrupt_keywords, + }) + } + + #[getter] + fn interrupt_speech_duration(&self) -> i32 { + self.0.interrupt_speech_duration + } + + #[getter] + fn interrupt_keywords(&self) -> Vec { + self.0.interrupt_keywords.clone() + } +} + +impl From for AsrConfig { + fn from(value: PyAsrConfig) -> Self { + value.0 + } +} + +#[pyclass(module = "booster_sdk_bindings", name = "StartAiChatParameter")] +#[derive(Clone)] +pub struct PyStartAiChatParameter(StartAiChatParameter); + +#[pymethods] +impl PyStartAiChatParameter { + #[new] + fn new( + interrupt_mode: bool, + asr_config: PyAsrConfig, + llm_config: PyLlmConfig, + tts_config: PyTtsConfig, + enable_face_tracking: bool, + ) -> Self { + Self(StartAiChatParameter { + interrupt_mode, + asr_config: asr_config.into(), + llm_config: llm_config.into(), + tts_config: tts_config.into(), + enable_face_tracking, + }) + } + + #[getter] + fn interrupt_mode(&self) -> bool { + self.0.interrupt_mode + } + + #[getter] + fn asr_config(&self) -> PyAsrConfig { + PyAsrConfig(self.0.asr_config.clone()) + } + + #[getter] + fn llm_config(&self) -> PyLlmConfig { + PyLlmConfig(self.0.llm_config.clone()) + } + + #[getter] + fn tts_config(&self) -> PyTtsConfig { + PyTtsConfig(self.0.tts_config.clone()) + } + + #[getter] + fn enable_face_tracking(&self) -> bool { + self.0.enable_face_tracking + } +} + +impl From for StartAiChatParameter { + fn from(value: PyStartAiChatParameter) -> Self { + value.0 + } +} + +#[pyclass(module = "booster_sdk_bindings", name = "SpeakParameter")] +#[derive(Clone)] +pub struct PySpeakParameter(SpeakParameter); + +#[pymethods] +impl PySpeakParameter { + #[new] + fn new(msg: String) -> Self { + Self(SpeakParameter { msg }) + } + + #[getter] + fn msg(&self) -> String { + self.0.msg.clone() + } +} + +impl From for SpeakParameter { + fn from(value: PySpeakParameter) -> Self { + value.0 + } +} #[pyclass(module = "booster_sdk_bindings", name = "AiClient", unsendable)] pub struct PyAiClient { @@ -48,6 +227,11 @@ impl PyAiClient { } pub(crate) fn register(m: &Bound<'_, PyModule>) -> PyResult<()> { + m.add_class::()?; + m.add_class::()?; + m.add_class::()?; + m.add_class::()?; + m.add_class::()?; m.add_class::()?; Ok(()) } diff --git a/booster_sdk_py/src/client/booster.rs b/booster_sdk_py/src/client/booster.rs index 3f9023f..001799c 100644 --- a/booster_sdk_py/src/client/booster.rs +++ b/booster_sdk_py/src/client/booster.rs @@ -3,19 +3,1262 @@ use std::sync::Arc; use booster_sdk::{ client::loco::{BoosterClient, GripperCommand}, types::{ - BoosterHandType, CustomTrainedTraj, DexterousFingerParameter, Frame, GripperControlMode, - GripperMotionParameter, Hand, Posture, + Action, BodyControl, BoosterHandType, CustomModel, CustomModelParams, CustomTrainedTraj, + DanceId, DexterousFingerParameter, Frame, GetModeResponse, GetRobotInfoResponse, + GetStatusResponse, GripperControlMode, GripperMode, GripperMotionParameter, Hand, + HandAction, JointOrder, LoadCustomTrainedTrajResponse, Orientation, Position, Posture, + Quaternion, RobotMode, Transform, WholeBodyDanceId, }, }; use pyo3::{Bound, prelude::*, types::PyModule}; -use crate::{ - PyBoosterHandType, PyCustomTrainedTraj, PyDanceId, PyDexterousFingerParameter, PyFrame, - PyGetModeResponse, PyGetRobotInfoResponse, PyGetStatusResponse, PyGripperCommand, - PyGripperControlMode, PyGripperMode, PyGripperMotionParameter, PyHand, PyHandAction, - PyLoadCustomTrainedTrajResponse, PyPosture, PyRobotMode, PyTransform, PyWholeBodyDanceId, - runtime::wait_for_future, to_py_err, -}; +use crate::{runtime::wait_for_future, to_py_err}; + +#[pyclass(module = "booster_sdk_bindings", name = "RobotMode", eq)] +#[derive(Clone, Copy, PartialEq, Eq)] +pub struct PyRobotMode(RobotMode); + +#[pymethods] +impl PyRobotMode { + #[classattr] + const UNKNOWN: Self = Self(RobotMode::Unknown); + #[classattr] + const DAMPING: Self = Self(RobotMode::Damping); + #[classattr] + const PREPARE: Self = Self(RobotMode::Prepare); + #[classattr] + const WALKING: Self = Self(RobotMode::Walking); + #[classattr] + const CUSTOM: Self = Self(RobotMode::Custom); + #[classattr] + const SOCCER: Self = Self(RobotMode::Soccer); + + fn __repr__(&self) -> String { + match self.0 { + RobotMode::Unknown => "RobotMode.UNKNOWN".to_string(), + RobotMode::Damping => "RobotMode.DAMPING".to_string(), + RobotMode::Prepare => "RobotMode.PREPARE".to_string(), + RobotMode::Walking => "RobotMode.WALKING".to_string(), + RobotMode::Custom => "RobotMode.CUSTOM".to_string(), + RobotMode::Soccer => "RobotMode.SOCCER".to_string(), + _ => format!("RobotMode({})", i32::from(self.0)), + } + } + + fn __int__(&self) -> i32 { + i32::from(self.0) + } +} + +impl From for RobotMode { + fn from(py_mode: PyRobotMode) -> Self { + py_mode.0 + } +} + +impl From for PyRobotMode { + fn from(mode: RobotMode) -> Self { + Self(mode) + } +} + +#[pyclass(module = "booster_sdk_bindings", name = "Hand", eq)] +#[derive(Clone, Copy, PartialEq, Eq)] +pub struct PyHand(Hand); + +#[pymethods] +impl PyHand { + #[classattr] + const LEFT: Self = Self(Hand::Left); + #[classattr] + const RIGHT: Self = Self(Hand::Right); + + fn __repr__(&self) -> String { + match self.0 { + Hand::Left => "Hand.LEFT".to_string(), + Hand::Right => "Hand.RIGHT".to_string(), + } + } + + fn __int__(&self) -> i32 { + i32::from(self.0) + } +} + +impl From for Hand { + fn from(py_hand: PyHand) -> Self { + py_hand.0 + } +} + +impl From for PyHand { + fn from(hand: Hand) -> Self { + Self(hand) + } +} + +#[pyclass(module = "booster_sdk_bindings", name = "GripperMode", eq)] +#[derive(Clone, Copy, PartialEq, Eq)] +pub struct PyGripperMode(GripperMode); + +#[pymethods] +impl PyGripperMode { + #[classattr] + const POSITION: Self = Self(GripperMode::Position); + #[classattr] + const FORCE: Self = Self(GripperMode::Force); + + fn __repr__(&self) -> String { + match self.0 { + GripperMode::Position => "GripperMode.POSITION".to_string(), + GripperMode::Force => "GripperMode.FORCE".to_string(), + } + } + + fn __int__(&self) -> i32 { + i32::from(self.0) + } +} + +impl From for GripperMode { + fn from(py_mode: PyGripperMode) -> Self { + py_mode.0 + } +} + +#[pyclass(module = "booster_sdk_bindings", name = "HandAction", eq)] +#[derive(Clone, Copy, PartialEq, Eq)] +pub struct PyHandAction(HandAction); + +#[pymethods] +impl PyHandAction { + #[classattr] + const OPEN: Self = Self(HandAction::Open); + #[classattr] + const CLOSE: Self = Self(HandAction::Close); + + fn __repr__(&self) -> String { + match self.0 { + HandAction::Open => "HandAction.OPEN".to_string(), + HandAction::Close => "HandAction.CLOSE".to_string(), + } + } + + fn __int__(&self) -> i32 { + i32::from(self.0) + } +} + +impl From for HandAction { + fn from(py_action: PyHandAction) -> Self { + py_action.0 + } +} + +#[pyclass(module = "booster_sdk_bindings", name = "Frame", eq)] +#[derive(Clone, Copy, PartialEq, Eq)] +pub struct PyFrame(Frame); + +#[pymethods] +impl PyFrame { + #[classattr] + const UNKNOWN: Self = Self(Frame::Unknown); + #[classattr] + const BODY: Self = Self(Frame::Body); + #[classattr] + const HEAD: Self = Self(Frame::Head); + #[classattr] + const LEFT_HAND: Self = Self(Frame::LeftHand); + #[classattr] + const RIGHT_HAND: Self = Self(Frame::RightHand); + #[classattr] + const LEFT_FOOT: Self = Self(Frame::LeftFoot); + #[classattr] + const RIGHT_FOOT: Self = Self(Frame::RightFoot); + + fn __repr__(&self) -> String { + match self.0 { + Frame::Unknown => "Frame.UNKNOWN".to_string(), + Frame::Body => "Frame.BODY".to_string(), + Frame::Head => "Frame.HEAD".to_string(), + Frame::LeftHand => "Frame.LEFT_HAND".to_string(), + Frame::RightHand => "Frame.RIGHT_HAND".to_string(), + Frame::LeftFoot => "Frame.LEFT_FOOT".to_string(), + Frame::RightFoot => "Frame.RIGHT_FOOT".to_string(), + } + } + + fn __int__(&self) -> i32 { + i32::from(self.0) + } +} + +impl From for Frame { + fn from(value: PyFrame) -> Self { + value.0 + } +} + +#[pyclass(module = "booster_sdk_bindings", name = "GripperControlMode", eq)] +#[derive(Clone, Copy, PartialEq, Eq)] +pub struct PyGripperControlMode(GripperControlMode); + +#[pymethods] +impl PyGripperControlMode { + #[classattr] + const POSITION: Self = Self(GripperControlMode::Position); + #[classattr] + const FORCE: Self = Self(GripperControlMode::Force); + + fn __repr__(&self) -> String { + match self.0 { + GripperControlMode::Position => "GripperControlMode.POSITION".to_string(), + GripperControlMode::Force => "GripperControlMode.FORCE".to_string(), + } + } + + fn __int__(&self) -> i32 { + i32::from(self.0) + } +} + +impl From for GripperControlMode { + fn from(value: PyGripperControlMode) -> Self { + value.0 + } +} + +#[pyclass(module = "booster_sdk_bindings", name = "BoosterHandType", eq)] +#[derive(Clone, Copy, PartialEq, Eq)] +pub struct PyBoosterHandType(BoosterHandType); + +#[pymethods] +impl PyBoosterHandType { + #[classattr] + const INSPIRE_HAND: Self = Self(BoosterHandType::InspireHand); + #[classattr] + const INSPIRE_TOUCH_HAND: Self = Self(BoosterHandType::InspireTouchHand); + #[classattr] + const REVO_HAND: Self = Self(BoosterHandType::RevoHand); + #[classattr] + const UNKNOWN: Self = Self(BoosterHandType::Unknown); + + fn __repr__(&self) -> String { + match self.0 { + BoosterHandType::InspireHand => "BoosterHandType.INSPIRE_HAND".to_string(), + BoosterHandType::InspireTouchHand => "BoosterHandType.INSPIRE_TOUCH_HAND".to_string(), + BoosterHandType::RevoHand => "BoosterHandType.REVO_HAND".to_string(), + BoosterHandType::Unknown => "BoosterHandType.UNKNOWN".to_string(), + } + } + + fn __int__(&self) -> i32 { + i32::from(self.0) + } +} + +impl From for BoosterHandType { + fn from(value: PyBoosterHandType) -> Self { + value.0 + } +} + +#[pyclass(module = "booster_sdk_bindings", name = "DanceId", eq)] +#[derive(Clone, Copy, PartialEq, Eq)] +pub struct PyDanceId(DanceId); + +#[pymethods] +impl PyDanceId { + #[classattr] + const NEW_YEAR: Self = Self(DanceId::NewYear); + #[classattr] + const NEZHA: Self = Self(DanceId::Nezha); + #[classattr] + const TOWARDS_FUTURE: Self = Self(DanceId::TowardsFuture); + #[classattr] + const DABBING_GESTURE: Self = Self(DanceId::DabbingGesture); + #[classattr] + const ULTRAMAN_GESTURE: Self = Self(DanceId::UltramanGesture); + #[classattr] + const RESPECT_GESTURE: Self = Self(DanceId::RespectGesture); + #[classattr] + const CHEERING_GESTURE: Self = Self(DanceId::CheeringGesture); + #[classattr] + const LUCKY_CAT_GESTURE: Self = Self(DanceId::LuckyCatGesture); + #[classattr] + const STOP: Self = Self(DanceId::Stop); + + fn __repr__(&self) -> String { + match self.0 { + DanceId::NewYear => "DanceId.NEW_YEAR".to_string(), + DanceId::Nezha => "DanceId.NEZHA".to_string(), + DanceId::TowardsFuture => "DanceId.TOWARDS_FUTURE".to_string(), + DanceId::DabbingGesture => "DanceId.DABBING_GESTURE".to_string(), + DanceId::UltramanGesture => "DanceId.ULTRAMAN_GESTURE".to_string(), + DanceId::RespectGesture => "DanceId.RESPECT_GESTURE".to_string(), + DanceId::CheeringGesture => "DanceId.CHEERING_GESTURE".to_string(), + DanceId::LuckyCatGesture => "DanceId.LUCKY_CAT_GESTURE".to_string(), + DanceId::Stop => "DanceId.STOP".to_string(), + } + } + + fn __int__(&self) -> i32 { + i32::from(self.0) + } +} + +impl From for DanceId { + fn from(value: PyDanceId) -> Self { + value.0 + } +} + +#[pyclass(module = "booster_sdk_bindings", name = "WholeBodyDanceId", eq)] +#[derive(Clone, Copy, PartialEq, Eq)] +pub struct PyWholeBodyDanceId(WholeBodyDanceId); + +#[pymethods] +impl PyWholeBodyDanceId { + #[classattr] + const ARBIC_DANCE: Self = Self(WholeBodyDanceId::ArbicDance); + #[classattr] + const MICHAEL_DANCE_1: Self = Self(WholeBodyDanceId::MichaelDance1); + #[classattr] + const MICHAEL_DANCE_2: Self = Self(WholeBodyDanceId::MichaelDance2); + #[classattr] + const MICHAEL_DANCE_3: Self = Self(WholeBodyDanceId::MichaelDance3); + #[classattr] + const MOON_WALK: Self = Self(WholeBodyDanceId::MoonWalk); + #[classattr] + const BOXING_STYLE_KICK: Self = Self(WholeBodyDanceId::BoxingStyleKick); + #[classattr] + const ROUNDHOUSE_KICK: Self = Self(WholeBodyDanceId::RoundhouseKick); + + fn __repr__(&self) -> String { + match self.0 { + WholeBodyDanceId::ArbicDance => "WholeBodyDanceId.ARBIC_DANCE".to_string(), + WholeBodyDanceId::MichaelDance1 => "WholeBodyDanceId.MICHAEL_DANCE_1".to_string(), + WholeBodyDanceId::MichaelDance2 => "WholeBodyDanceId.MICHAEL_DANCE_2".to_string(), + WholeBodyDanceId::MichaelDance3 => "WholeBodyDanceId.MICHAEL_DANCE_3".to_string(), + WholeBodyDanceId::MoonWalk => "WholeBodyDanceId.MOON_WALK".to_string(), + WholeBodyDanceId::BoxingStyleKick => "WholeBodyDanceId.BOXING_STYLE_KICK".to_string(), + WholeBodyDanceId::RoundhouseKick => "WholeBodyDanceId.ROUNDHOUSE_KICK".to_string(), + } + } + + fn __int__(&self) -> i32 { + i32::from(self.0) + } +} + +impl From for WholeBodyDanceId { + fn from(value: PyWholeBodyDanceId) -> Self { + value.0 + } +} + +#[pyclass(module = "booster_sdk_bindings", name = "JointOrder", eq)] +#[derive(Clone, Copy, PartialEq, Eq)] +pub struct PyJointOrder(JointOrder); + +#[pymethods] +impl PyJointOrder { + #[classattr] + const MUJOCO: Self = Self(JointOrder::MuJoCo); + #[classattr] + const ISAAC_LAB: Self = Self(JointOrder::IsaacLab); + + fn __repr__(&self) -> String { + match self.0 { + JointOrder::MuJoCo => "JointOrder.MUJOCO".to_string(), + JointOrder::IsaacLab => "JointOrder.ISAAC_LAB".to_string(), + } + } + + fn __int__(&self) -> i32 { + i32::from(self.0) + } +} + +impl From for JointOrder { + fn from(value: PyJointOrder) -> Self { + value.0 + } +} + +#[pyclass(module = "booster_sdk_bindings", name = "BodyControl", eq)] +#[derive(Clone, Copy, PartialEq, Eq)] +pub struct PyBodyControl(BodyControl); + +#[pymethods] +impl PyBodyControl { + #[classattr] + const UNKNOWN: Self = Self(BodyControl::Unknown); + #[classattr] + const DAMPING: Self = Self(BodyControl::Damping); + #[classattr] + const PREPARE: Self = Self(BodyControl::Prepare); + #[classattr] + const HUMANLIKE_GAIT: Self = Self(BodyControl::HumanlikeGait); + #[classattr] + const PRONE_BODY: Self = Self(BodyControl::ProneBody); + #[classattr] + const SOCCER_GAIT: Self = Self(BodyControl::SoccerGait); + #[classattr] + const CUSTOM: Self = Self(BodyControl::Custom); + #[classattr] + const GET_UP: Self = Self(BodyControl::GetUp); + #[classattr] + const WHOLE_BODY_DANCE: Self = Self(BodyControl::WholeBodyDance); + #[classattr] + const SHOOT: Self = Self(BodyControl::Shoot); + #[classattr] + const INSIDE_FOOT: Self = Self(BodyControl::InsideFoot); + #[classattr] + const GOALIE: Self = Self(BodyControl::Goalie); + #[classattr] + const WBC_GAIT: Self = Self(BodyControl::WbcGait); + + fn __repr__(&self) -> String { + match self.0 { + BodyControl::Unknown => "BodyControl.UNKNOWN".to_string(), + BodyControl::Damping => "BodyControl.DAMPING".to_string(), + BodyControl::Prepare => "BodyControl.PREPARE".to_string(), + BodyControl::HumanlikeGait => "BodyControl.HUMANLIKE_GAIT".to_string(), + BodyControl::ProneBody => "BodyControl.PRONE_BODY".to_string(), + BodyControl::SoccerGait => "BodyControl.SOCCER_GAIT".to_string(), + BodyControl::Custom => "BodyControl.CUSTOM".to_string(), + BodyControl::GetUp => "BodyControl.GET_UP".to_string(), + BodyControl::WholeBodyDance => "BodyControl.WHOLE_BODY_DANCE".to_string(), + BodyControl::Shoot => "BodyControl.SHOOT".to_string(), + BodyControl::InsideFoot => "BodyControl.INSIDE_FOOT".to_string(), + BodyControl::Goalie => "BodyControl.GOALIE".to_string(), + BodyControl::WbcGait => "BodyControl.WBC_GAIT".to_string(), + } + } + + fn __int__(&self) -> i32 { + i32::from(self.0) + } +} + +impl From for PyBodyControl { + fn from(value: BodyControl) -> Self { + Self(value) + } +} + +#[pyclass(module = "booster_sdk_bindings", name = "Action", eq)] +#[derive(Clone, Copy, PartialEq, Eq)] +pub struct PyAction(Action); + +#[pymethods] +impl PyAction { + #[classattr] + const UNKNOWN: Self = Self(Action::Unknown); + #[classattr] + const HAND_SHAKE: Self = Self(Action::HandShake); + #[classattr] + const HAND_WAVE: Self = Self(Action::HandWave); + #[classattr] + const HAND_CONTROL: Self = Self(Action::HandControl); + #[classattr] + const DANCE_NEW_YEAR: Self = Self(Action::DanceNewYear); + #[classattr] + const DANCE_NEZHA: Self = Self(Action::DanceNezha); + #[classattr] + const DANCE_TOWARDS_FUTURE: Self = Self(Action::DanceTowardsFuture); + #[classattr] + const GESTURE_DABBING: Self = Self(Action::GestureDabbing); + #[classattr] + const GESTURE_ULTRAMAN: Self = Self(Action::GestureUltraman); + #[classattr] + const GESTURE_RESPECT: Self = Self(Action::GestureRespect); + #[classattr] + const GESTURE_CHEER: Self = Self(Action::GestureCheer); + #[classattr] + const GESTURE_LUCKY_CAT: Self = Self(Action::GestureLuckyCat); + #[classattr] + const GESTURE_BOXING: Self = Self(Action::GestureBoxing); + #[classattr] + const ZERO_TORQUE_DRAG: Self = Self(Action::ZeroTorqueDrag); + #[classattr] + const RECORD_TRAJ: Self = Self(Action::RecordTraj); + #[classattr] + const RUN_RECORDED_TRAJ: Self = Self(Action::RunRecordedTraj); + + fn __repr__(&self) -> String { + match self.0 { + Action::Unknown => "Action.UNKNOWN".to_string(), + Action::HandShake => "Action.HAND_SHAKE".to_string(), + Action::HandWave => "Action.HAND_WAVE".to_string(), + Action::HandControl => "Action.HAND_CONTROL".to_string(), + Action::DanceNewYear => "Action.DANCE_NEW_YEAR".to_string(), + Action::DanceNezha => "Action.DANCE_NEZHA".to_string(), + Action::DanceTowardsFuture => "Action.DANCE_TOWARDS_FUTURE".to_string(), + Action::GestureDabbing => "Action.GESTURE_DABBING".to_string(), + Action::GestureUltraman => "Action.GESTURE_ULTRAMAN".to_string(), + Action::GestureRespect => "Action.GESTURE_RESPECT".to_string(), + Action::GestureCheer => "Action.GESTURE_CHEER".to_string(), + Action::GestureLuckyCat => "Action.GESTURE_LUCKY_CAT".to_string(), + Action::GestureBoxing => "Action.GESTURE_BOXING".to_string(), + Action::ZeroTorqueDrag => "Action.ZERO_TORQUE_DRAG".to_string(), + Action::RecordTraj => "Action.RECORD_TRAJ".to_string(), + Action::RunRecordedTraj => "Action.RUN_RECORDED_TRAJ".to_string(), + } + } + + fn __int__(&self) -> i32 { + i32::from(self.0) + } +} + +impl From for PyAction { + fn from(value: Action) -> Self { + Self(value) + } +} + +#[pyclass(module = "booster_sdk_bindings", name = "GripperCommand")] +#[derive(Clone)] +pub struct PyGripperCommand(GripperCommand); + +#[pymethods] +impl PyGripperCommand { + #[new] + fn new(hand: PyHand, mode: PyGripperMode, motion_param: u16, speed: Option) -> Self { + Self(GripperCommand { + hand: hand.into(), + mode: mode.into(), + motion_param, + speed: speed.unwrap_or(500), + }) + } + + #[staticmethod] + fn open(hand: PyHand) -> Self { + Self(GripperCommand::open(hand.into())) + } + + #[staticmethod] + fn close(hand: PyHand) -> Self { + Self(GripperCommand::close(hand.into())) + } + + #[staticmethod] + fn grasp(hand: PyHand, force: u16) -> Self { + Self(GripperCommand::grasp(hand.into(), force)) + } + + #[getter] + fn hand(&self) -> PyHand { + self.0.hand.into() + } + + #[getter] + fn mode(&self) -> PyGripperMode { + PyGripperMode(self.0.mode) + } + + #[getter] + fn motion_param(&self) -> u16 { + self.0.motion_param + } + + #[getter] + fn speed(&self) -> u16 { + self.0.speed + } + + fn __repr__(&self) -> String { + format!( + "GripperCommand(hand={}, mode={}, motion_param={}, speed={})", + u8::from(self.0.hand), + i32::from(self.0.mode), + self.0.motion_param, + self.0.speed + ) + } +} + +impl From for GripperCommand { + fn from(value: PyGripperCommand) -> Self { + value.0 + } +} + +#[pyclass(module = "booster_sdk_bindings", name = "Position")] +#[derive(Clone, Copy)] +pub struct PyPosition(Position); + +#[pymethods] +impl PyPosition { + #[new] + fn new(x: f32, y: f32, z: f32) -> Self { + Self(Position { x, y, z }) + } + + #[getter] + fn x(&self) -> f32 { + self.0.x + } + + #[setter] + fn set_x(&mut self, value: f32) { + self.0.x = value; + } + + #[getter] + fn y(&self) -> f32 { + self.0.y + } + + #[setter] + fn set_y(&mut self, value: f32) { + self.0.y = value; + } + + #[getter] + fn z(&self) -> f32 { + self.0.z + } + + #[setter] + fn set_z(&mut self, value: f32) { + self.0.z = value; + } + + fn __repr__(&self) -> String { + format!("Position(x={}, y={}, z={})", self.0.x, self.0.y, self.0.z) + } +} + +impl From for Position { + fn from(value: PyPosition) -> Self { + value.0 + } +} + +impl From for PyPosition { + fn from(value: Position) -> Self { + Self(value) + } +} + +#[pyclass(module = "booster_sdk_bindings", name = "Orientation")] +#[derive(Clone, Copy)] +pub struct PyOrientation(Orientation); + +#[pymethods] +impl PyOrientation { + #[new] + fn new(roll: f32, pitch: f32, yaw: f32) -> Self { + Self(Orientation { roll, pitch, yaw }) + } + + #[getter] + fn roll(&self) -> f32 { + self.0.roll + } + + #[setter] + fn set_roll(&mut self, value: f32) { + self.0.roll = value; + } + + #[getter] + fn pitch(&self) -> f32 { + self.0.pitch + } + + #[setter] + fn set_pitch(&mut self, value: f32) { + self.0.pitch = value; + } + + #[getter] + fn yaw(&self) -> f32 { + self.0.yaw + } + + #[setter] + fn set_yaw(&mut self, value: f32) { + self.0.yaw = value; + } + + fn __repr__(&self) -> String { + format!( + "Orientation(roll={}, pitch={}, yaw={})", + self.0.roll, self.0.pitch, self.0.yaw + ) + } +} + +impl From for Orientation { + fn from(value: PyOrientation) -> Self { + value.0 + } +} + +impl From for PyOrientation { + fn from(value: Orientation) -> Self { + Self(value) + } +} + +#[pyclass(module = "booster_sdk_bindings", name = "Posture")] +#[derive(Clone, Copy)] +pub struct PyPosture(Posture); + +#[pymethods] +impl PyPosture { + #[new] + fn new(position: PyPosition, orientation: PyOrientation) -> Self { + Self(Posture { + position: position.into(), + orientation: orientation.into(), + }) + } + + #[getter] + fn position(&self) -> PyPosition { + self.0.position.into() + } + + #[setter] + fn set_position(&mut self, value: PyPosition) { + self.0.position = value.into(); + } + + #[getter] + fn orientation(&self) -> PyOrientation { + self.0.orientation.into() + } + + #[setter] + fn set_orientation(&mut self, value: PyOrientation) { + self.0.orientation = value.into(); + } + + fn __repr__(&self) -> String { + format!( + "Posture(position={}, orientation={})", + self.position().__repr__(), + self.orientation().__repr__() + ) + } +} + +impl From for Posture { + fn from(value: PyPosture) -> Self { + value.0 + } +} + +impl From for PyPosture { + fn from(value: Posture) -> Self { + Self(value) + } +} + +#[pyclass(module = "booster_sdk_bindings", name = "Quaternion")] +#[derive(Clone, Copy)] +pub struct PyQuaternion(Quaternion); + +#[pymethods] +impl PyQuaternion { + #[new] + fn new(x: f32, y: f32, z: f32, w: f32) -> Self { + Self(Quaternion { x, y, z, w }) + } + + #[getter] + fn x(&self) -> f32 { + self.0.x + } + + #[getter] + fn y(&self) -> f32 { + self.0.y + } + + #[getter] + fn z(&self) -> f32 { + self.0.z + } + + #[getter] + fn w(&self) -> f32 { + self.0.w + } + + fn __repr__(&self) -> String { + format!( + "Quaternion(x={}, y={}, z={}, w={})", + self.0.x, self.0.y, self.0.z, self.0.w + ) + } +} + +impl From for PyQuaternion { + fn from(value: Quaternion) -> Self { + Self(value) + } +} + +#[pyclass(module = "booster_sdk_bindings", name = "Transform")] +#[derive(Clone, Copy)] +pub struct PyTransform(Transform); + +#[pymethods] +impl PyTransform { + #[new] + fn new(position: PyPosition, orientation: PyQuaternion) -> Self { + Self(Transform { + position: position.into(), + orientation: orientation.0, + }) + } + + #[getter] + fn position(&self) -> PyPosition { + self.0.position.into() + } + + #[getter] + fn orientation(&self) -> PyQuaternion { + self.0.orientation.into() + } + + fn __repr__(&self) -> String { + format!( + "Transform(position={}, orientation={})", + self.position().__repr__(), + self.orientation().__repr__() + ) + } +} + +impl From for PyTransform { + fn from(value: Transform) -> Self { + Self(value) + } +} + +#[pyclass(module = "booster_sdk_bindings", name = "GripperMotionParameter")] +#[derive(Clone, Copy)] +pub struct PyGripperMotionParameter(GripperMotionParameter); + +#[pymethods] +impl PyGripperMotionParameter { + #[new] + fn new(position: i32, force: i32, speed: i32) -> Self { + Self(GripperMotionParameter { + position, + force, + speed, + }) + } + + #[getter] + fn position(&self) -> i32 { + self.0.position + } + + #[getter] + fn force(&self) -> i32 { + self.0.force + } + + #[getter] + fn speed(&self) -> i32 { + self.0.speed + } + + fn __repr__(&self) -> String { + format!( + "GripperMotionParameter(position={}, force={}, speed={})", + self.0.position, self.0.force, self.0.speed + ) + } +} + +impl From for GripperMotionParameter { + fn from(value: PyGripperMotionParameter) -> Self { + value.0 + } +} + +#[pyclass(module = "booster_sdk_bindings", name = "DexterousFingerParameter")] +#[derive(Clone, Copy)] +pub struct PyDexterousFingerParameter(DexterousFingerParameter); + +#[pymethods] +impl PyDexterousFingerParameter { + #[new] + fn new(seq: i32, angle: i32, force: i32, speed: i32) -> Self { + Self(DexterousFingerParameter { + seq, + angle, + force, + speed, + }) + } + + #[getter] + fn seq(&self) -> i32 { + self.0.seq + } + + #[getter] + fn angle(&self) -> i32 { + self.0.angle + } + + #[getter] + fn force(&self) -> i32 { + self.0.force + } + + #[getter] + fn speed(&self) -> i32 { + self.0.speed + } + + fn __repr__(&self) -> String { + format!( + "DexterousFingerParameter(seq={}, angle={}, force={}, speed={})", + self.0.seq, self.0.angle, self.0.force, self.0.speed + ) + } +} + +impl From for DexterousFingerParameter { + fn from(value: PyDexterousFingerParameter) -> Self { + value.0 + } +} + +impl From for PyDexterousFingerParameter { + fn from(value: DexterousFingerParameter) -> Self { + Self(value) + } +} + +#[pyclass(module = "booster_sdk_bindings", name = "CustomModelParams")] +#[derive(Clone)] +pub struct PyCustomModelParams(CustomModelParams); + +#[pymethods] +impl PyCustomModelParams { + #[new] + fn new(action_scale: Vec, kp: Vec, kd: Vec) -> Self { + Self(CustomModelParams { + action_scale, + kp, + kd, + }) + } + + #[getter] + fn action_scale(&self) -> Vec { + self.0.action_scale.clone() + } + + #[getter] + fn kp(&self) -> Vec { + self.0.kp.clone() + } + + #[getter] + fn kd(&self) -> Vec { + self.0.kd.clone() + } + + fn __repr__(&self) -> String { + "CustomModelParams(...)".to_string() + } +} + +impl From for CustomModelParams { + fn from(value: PyCustomModelParams) -> Self { + value.0 + } +} + +impl From for PyCustomModelParams { + fn from(value: CustomModelParams) -> Self { + Self(value) + } +} + +#[pyclass(module = "booster_sdk_bindings", name = "CustomModel")] +#[derive(Clone)] +pub struct PyCustomModel(CustomModel); + +#[pymethods] +impl PyCustomModel { + #[new] + fn new(file_path: String, params: Vec, joint_order: PyJointOrder) -> Self { + Self(CustomModel { + file_path, + params: params.into_iter().map(Into::into).collect(), + joint_order: joint_order.into(), + }) + } + + #[getter] + fn file_path(&self) -> String { + self.0.file_path.clone() + } + + #[getter] + fn params(&self) -> Vec { + self.0.params.clone().into_iter().map(Into::into).collect() + } + + #[getter] + fn joint_order(&self) -> PyJointOrder { + PyJointOrder(self.0.joint_order) + } + + fn __repr__(&self) -> String { + format!("CustomModel(file_path='{}', params=...)", self.0.file_path) + } +} + +impl From for CustomModel { + fn from(value: PyCustomModel) -> Self { + value.0 + } +} + +impl From for PyCustomModel { + fn from(value: CustomModel) -> Self { + Self(value) + } +} + +#[pyclass(module = "booster_sdk_bindings", name = "CustomTrainedTraj")] +#[derive(Clone)] +pub struct PyCustomTrainedTraj(CustomTrainedTraj); + +#[pymethods] +impl PyCustomTrainedTraj { + #[new] + fn new(traj_file_path: String, model: PyCustomModel) -> Self { + Self(CustomTrainedTraj { + traj_file_path, + model: model.into(), + }) + } + + #[getter] + fn traj_file_path(&self) -> String { + self.0.traj_file_path.clone() + } + + #[getter] + fn model(&self) -> PyCustomModel { + self.0.model.clone().into() + } + + fn __repr__(&self) -> String { + format!( + "CustomTrainedTraj(traj_file_path='{}', model=...)", + self.0.traj_file_path + ) + } +} + +impl From for CustomTrainedTraj { + fn from(value: PyCustomTrainedTraj) -> Self { + value.0 + } +} + +#[pyclass(module = "booster_sdk_bindings", name = "GetModeResponse")] +#[derive(Clone)] +pub struct PyGetModeResponse(GetModeResponse); + +#[pymethods] +impl PyGetModeResponse { + #[new] + fn new(mode: i32) -> Self { + Self(GetModeResponse { mode }) + } + + #[getter] + fn mode(&self) -> i32 { + self.0.mode + } + + fn mode_enum(&self) -> Option { + self.0.mode_enum().map(Into::into) + } + + fn __repr__(&self) -> String { + format!("GetModeResponse(mode={})", self.0.mode) + } +} + +impl From for PyGetModeResponse { + fn from(value: GetModeResponse) -> Self { + Self(value) + } +} + +#[pyclass(module = "booster_sdk_bindings", name = "GetStatusResponse")] +#[derive(Clone)] +pub struct PyGetStatusResponse(GetStatusResponse); + +#[pymethods] +impl PyGetStatusResponse { + #[new] + fn new(current_mode: i32, current_body_control: i32, current_actions: Vec) -> Self { + Self(GetStatusResponse { + current_mode, + current_body_control, + current_actions, + }) + } + + #[getter] + fn current_mode(&self) -> i32 { + self.0.current_mode + } + + #[getter] + fn current_body_control(&self) -> i32 { + self.0.current_body_control + } + + #[getter] + fn current_actions(&self) -> Vec { + self.0.current_actions.clone() + } + + fn current_mode_enum(&self) -> Option { + self.0.current_mode_enum().map(Into::into) + } + + fn current_body_control_enum(&self) -> Option { + self.0.current_body_control_enum().map(Into::into) + } + + fn current_actions_enum(&self) -> Vec { + self.0 + .current_actions_enum() + .into_iter() + .map(Into::into) + .collect() + } + + fn __repr__(&self) -> String { + format!( + "GetStatusResponse(current_mode={}, current_body_control={}, current_actions={:?})", + self.0.current_mode, self.0.current_body_control, self.0.current_actions + ) + } +} + +impl From for PyGetStatusResponse { + fn from(value: GetStatusResponse) -> Self { + Self(value) + } +} + +#[pyclass(module = "booster_sdk_bindings", name = "GetRobotInfoResponse")] +#[derive(Clone)] +pub struct PyGetRobotInfoResponse(GetRobotInfoResponse); + +#[pymethods] +impl PyGetRobotInfoResponse { + #[new] + fn new( + name: String, + nickname: String, + version: String, + model: String, + serial_number: String, + ) -> Self { + Self(GetRobotInfoResponse { + name, + nickname, + version, + model, + serial_number, + }) + } + + #[getter] + fn name(&self) -> String { + self.0.name.clone() + } + + #[getter] + fn nickname(&self) -> String { + self.0.nickname.clone() + } + + #[getter] + fn version(&self) -> String { + self.0.version.clone() + } + + #[getter] + fn model(&self) -> String { + self.0.model.clone() + } + + #[getter] + fn serial_number(&self) -> String { + self.0.serial_number.clone() + } + + fn __repr__(&self) -> String { + format!( + "GetRobotInfoResponse(name='{}', nickname='{}', version='{}', model='{}', serial_number='{}')", + self.0.name, self.0.nickname, self.0.version, self.0.model, self.0.serial_number + ) + } +} + +impl From for PyGetRobotInfoResponse { + fn from(value: GetRobotInfoResponse) -> Self { + Self(value) + } +} + +#[pyclass( + module = "booster_sdk_bindings", + name = "LoadCustomTrainedTrajResponse" +)] +#[derive(Clone)] +pub struct PyLoadCustomTrainedTrajResponse(LoadCustomTrainedTrajResponse); + +#[pymethods] +impl PyLoadCustomTrainedTrajResponse { + #[new] + fn new(tid: String) -> Self { + Self(LoadCustomTrainedTrajResponse { tid }) + } + + #[getter] + fn tid(&self) -> String { + self.0.tid.clone() + } + + fn __repr__(&self) -> String { + format!("LoadCustomTrainedTrajResponse(tid='{}')", self.0.tid) + } +} + +impl From for PyLoadCustomTrainedTrajResponse { + fn from(value: LoadCustomTrainedTrajResponse) -> Self { + Self(value) + } +} #[pyclass(module = "booster_sdk_bindings", name = "BoosterClient", unsendable)] pub struct PyBoosterClient { @@ -402,6 +1645,33 @@ impl PyBoosterClient { } pub(crate) fn register(m: &Bound<'_, PyModule>) -> PyResult<()> { + m.add_class::()?; + m.add_class::()?; + m.add_class::()?; + m.add_class::()?; + m.add_class::()?; + m.add_class::()?; + m.add_class::()?; + m.add_class::()?; + m.add_class::()?; + m.add_class::()?; + m.add_class::()?; + m.add_class::()?; + m.add_class::()?; + m.add_class::()?; + m.add_class::()?; + m.add_class::()?; + m.add_class::()?; + m.add_class::()?; + m.add_class::()?; + m.add_class::()?; + m.add_class::()?; + m.add_class::()?; + m.add_class::()?; + m.add_class::()?; + m.add_class::()?; + m.add_class::()?; + m.add_class::()?; m.add_class::()?; Ok(()) } diff --git a/booster_sdk_py/src/client/lui.rs b/booster_sdk_py/src/client/lui.rs index 146b2e2..d00504c 100644 --- a/booster_sdk_py/src/client/lui.rs +++ b/booster_sdk_py/src/client/lui.rs @@ -1,9 +1,55 @@ use std::sync::Arc; -use booster_sdk::client::ai::LuiClient; +use booster_sdk::client::ai::{LuiClient, LuiTtsConfig, LuiTtsParameter}; use pyo3::{Bound, prelude::*, types::PyModule}; -use crate::{PyLuiTtsConfig, PyLuiTtsParameter, runtime::wait_for_future, to_py_err}; +use crate::{runtime::wait_for_future, to_py_err}; + +#[pyclass(module = "booster_sdk_bindings", name = "LuiTtsConfig")] +#[derive(Clone)] +pub struct PyLuiTtsConfig(LuiTtsConfig); + +#[pymethods] +impl PyLuiTtsConfig { + #[new] + fn new(voice_type: String) -> Self { + Self(LuiTtsConfig { voice_type }) + } + + #[getter] + fn voice_type(&self) -> String { + self.0.voice_type.clone() + } +} + +impl From for LuiTtsConfig { + fn from(value: PyLuiTtsConfig) -> Self { + value.0 + } +} + +#[pyclass(module = "booster_sdk_bindings", name = "LuiTtsParameter")] +#[derive(Clone)] +pub struct PyLuiTtsParameter(LuiTtsParameter); + +#[pymethods] +impl PyLuiTtsParameter { + #[new] + fn new(text: String) -> Self { + Self(LuiTtsParameter { text }) + } + + #[getter] + fn text(&self) -> String { + self.0.text.clone() + } +} + +impl From for LuiTtsParameter { + fn from(value: PyLuiTtsParameter) -> Self { + value.0 + } +} #[pyclass(module = "booster_sdk_bindings", name = "LuiClient", unsendable)] pub struct PyLuiClient { @@ -48,6 +94,8 @@ impl PyLuiClient { } pub(crate) fn register(m: &Bound<'_, PyModule>) -> PyResult<()> { + m.add_class::()?; + m.add_class::()?; m.add_class::()?; Ok(()) } diff --git a/booster_sdk_py/src/client/vision.rs b/booster_sdk_py/src/client/vision.rs index ea3136b..691776c 100644 --- a/booster_sdk_py/src/client/vision.rs +++ b/booster_sdk_py/src/client/vision.rs @@ -1,9 +1,93 @@ use std::sync::Arc; -use booster_sdk::client::vision::VisionClient; +use booster_sdk::client::vision::{DetectResults, VisionClient}; use pyo3::{Bound, prelude::*, types::PyModule}; -use crate::{PyDetectResults, runtime::wait_for_future, to_py_err}; +use crate::{runtime::wait_for_future, to_py_err}; + +#[pyclass(module = "booster_sdk_bindings", name = "DetectResults")] +#[derive(Clone)] +pub struct PyDetectResults(DetectResults); + +#[pymethods] +impl PyDetectResults { + #[new] + #[expect(clippy::too_many_arguments)] + fn new( + xmin: i64, + ymin: i64, + xmax: i64, + ymax: i64, + position: Vec, + tag: String, + conf: f32, + rgb_mean: Vec, + ) -> Self { + Self(DetectResults { + xmin, + ymin, + xmax, + ymax, + position, + tag, + conf, + rgb_mean, + }) + } + + #[getter] + fn xmin(&self) -> i64 { + self.0.xmin + } + + #[getter] + fn ymin(&self) -> i64 { + self.0.ymin + } + + #[getter] + fn xmax(&self) -> i64 { + self.0.xmax + } + + #[getter] + fn ymax(&self) -> i64 { + self.0.ymax + } + + #[getter] + fn position(&self) -> Vec { + self.0.position.clone() + } + + #[getter] + fn tag(&self) -> String { + self.0.tag.clone() + } + + #[getter] + fn conf(&self) -> f32 { + self.0.conf + } + + #[getter] + fn rgb_mean(&self) -> Vec { + self.0.rgb_mean.clone() + } + + fn __repr__(&self) -> String { + format!( + "DetectResults(tag='{}', conf={}, bbox=({}, {}, {}, {}))", + self.0.tag, self.0.conf, self.0.xmin, self.0.ymin, self.0.xmax, self.0.ymax + ) + } +} + +impl From for PyDetectResults { + fn from(value: DetectResults) -> Self { + Self(value) + } +} #[pyclass(module = "booster_sdk_bindings", name = "VisionClient", unsendable)] pub struct PyVisionClient { @@ -62,6 +146,7 @@ impl PyVisionClient { } pub(crate) fn register(m: &Bound<'_, PyModule>) -> PyResult<()> { + m.add_class::()?; m.add_class::()?; Ok(()) } diff --git a/booster_sdk_py/src/client/x5_camera.rs b/booster_sdk_py/src/client/x5_camera.rs index 09db5a9..8f4b091 100644 --- a/booster_sdk_py/src/client/x5_camera.rs +++ b/booster_sdk_py/src/client/x5_camera.rs @@ -1,9 +1,128 @@ use std::sync::Arc; -use booster_sdk::client::x5_camera::X5CameraClient; +use booster_sdk::client::x5_camera::{ + CameraControlStatus, CameraSetMode, GetStatusResponse as X5CameraStatusResponse, X5CameraClient, +}; use pyo3::{Bound, prelude::*, types::PyModule}; -use crate::{PyCameraSetMode, PyX5CameraGetStatusResponse, runtime::wait_for_future, to_py_err}; +use crate::{runtime::wait_for_future, to_py_err}; + +#[pyclass(module = "booster_sdk_bindings", name = "CameraSetMode", eq)] +#[derive(Clone, Copy, PartialEq, Eq)] +pub struct PyCameraSetMode(CameraSetMode); + +#[pymethods] +impl PyCameraSetMode { + #[classattr] + const CAMERA_MODE_NORMAL: Self = Self(CameraSetMode::CameraModeNormal); + #[classattr] + const CAMERA_MODE_HIGH_RESOLUTION: Self = Self(CameraSetMode::CameraModeHighResolution); + #[classattr] + const CAMERA_MODE_NORMAL_ENABLE: Self = Self(CameraSetMode::CameraModeNormalEnable); + #[classattr] + const CAMERA_MODE_HIGH_RESOLUTION_ENABLE: Self = + Self(CameraSetMode::CameraModeHighResolutionEnable); + + fn __repr__(&self) -> String { + match self.0 { + CameraSetMode::CameraModeNormal => "CameraSetMode.CAMERA_MODE_NORMAL".to_string(), + CameraSetMode::CameraModeHighResolution => { + "CameraSetMode.CAMERA_MODE_HIGH_RESOLUTION".to_string() + } + CameraSetMode::CameraModeNormalEnable => { + "CameraSetMode.CAMERA_MODE_NORMAL_ENABLE".to_string() + } + CameraSetMode::CameraModeHighResolutionEnable => { + "CameraSetMode.CAMERA_MODE_HIGH_RESOLUTION_ENABLE".to_string() + } + } + } + + fn __int__(&self) -> i32 { + i32::from(self.0) + } +} + +impl From for CameraSetMode { + fn from(value: PyCameraSetMode) -> Self { + value.0 + } +} + +#[pyclass(module = "booster_sdk_bindings", name = "CameraControlStatus", eq)] +#[derive(Clone, Copy, PartialEq, Eq)] +pub struct PyCameraControlStatus(CameraControlStatus); + +#[pymethods] +impl PyCameraControlStatus { + #[classattr] + const CAMERA_STATUS_NORMAL: Self = Self(CameraControlStatus::CameraStatusNormal); + #[classattr] + const CAMERA_STATUS_HIGH_RESOLUTION: Self = + Self(CameraControlStatus::CameraStatusHighResolution); + #[classattr] + const CAMERA_STATUS_ERROR: Self = Self(CameraControlStatus::CameraStatusError); + #[classattr] + const CAMERA_STATUS_NULL: Self = Self(CameraControlStatus::CameraStatusNull); + + fn __repr__(&self) -> String { + match self.0 { + CameraControlStatus::CameraStatusNormal => { + "CameraControlStatus.CAMERA_STATUS_NORMAL".to_string() + } + CameraControlStatus::CameraStatusHighResolution => { + "CameraControlStatus.CAMERA_STATUS_HIGH_RESOLUTION".to_string() + } + CameraControlStatus::CameraStatusError => { + "CameraControlStatus.CAMERA_STATUS_ERROR".to_string() + } + CameraControlStatus::CameraStatusNull => { + "CameraControlStatus.CAMERA_STATUS_NULL".to_string() + } + } + } + + fn __int__(&self) -> i32 { + i32::from(self.0) + } +} + +impl From for PyCameraControlStatus { + fn from(value: CameraControlStatus) -> Self { + Self(value) + } +} + +#[pyclass(module = "booster_sdk_bindings", name = "X5CameraGetStatusResponse")] +#[derive(Clone)] +pub struct PyX5CameraGetStatusResponse(X5CameraStatusResponse); + +#[pymethods] +impl PyX5CameraGetStatusResponse { + #[new] + fn new(status: i32) -> Self { + Self(X5CameraStatusResponse { status }) + } + + #[getter] + fn status(&self) -> i32 { + self.0.status + } + + fn status_enum(&self) -> Option { + self.0.status_enum().map(Into::into) + } + + fn __repr__(&self) -> String { + format!("X5CameraGetStatusResponse(status={})", self.0.status) + } +} + +impl From for PyX5CameraGetStatusResponse { + fn from(value: X5CameraStatusResponse) -> Self { + Self(value) + } +} #[pyclass(module = "booster_sdk_bindings", name = "X5CameraClient", unsendable)] pub struct PyX5CameraClient { @@ -33,6 +152,9 @@ impl PyX5CameraClient { } pub(crate) fn register(m: &Bound<'_, PyModule>) -> PyResult<()> { + m.add_class::()?; + m.add_class::()?; + m.add_class::()?; m.add_class::()?; Ok(()) } diff --git a/booster_sdk_py/src/lib.rs b/booster_sdk_py/src/lib.rs index 2977c3c..9c9218f 100644 --- a/booster_sdk_py/src/lib.rs +++ b/booster_sdk_py/src/lib.rs @@ -1,26 +1,7 @@ mod client; mod runtime; -use booster_sdk::{ - client::{ - ai::{ - AsrConfig, BOOSTER_ROBOT_USER_ID, LlmConfig, LuiTtsConfig, LuiTtsParameter, - SpeakParameter, StartAiChatParameter, TtsConfig, - }, - loco::GripperCommand, - vision::DetectResults, - x5_camera::{ - CameraControlStatus, CameraSetMode, GetStatusResponse as X5CameraStatusResponse, - }, - }, - types::{ - Action, BodyControl, BoosterError, BoosterHandType, CustomModel, CustomModelParams, - CustomTrainedTraj, DanceId, DexterousFingerParameter, Frame, GetModeResponse, - GetRobotInfoResponse, GetStatusResponse, GripperControlMode, GripperMode, - GripperMotionParameter, Hand, HandAction, JointOrder, LoadCustomTrainedTrajResponse, - Orientation, Position, Posture, Quaternion, RobotMode, Transform, WholeBodyDanceId, - }, -}; +use booster_sdk::{client::ai::BOOSTER_ROBOT_USER_ID, types::BoosterError}; use pyo3::{exceptions::PyException, prelude::*, types::PyModule}; pyo3::create_exception!(booster_sdk_bindings, BoosterSdkError, PyException); @@ -29,1675 +10,6 @@ pub(crate) fn to_py_err(err: BoosterError) -> PyErr { BoosterSdkError::new_err(err.to_string()) } -#[pyclass(module = "booster_sdk_bindings", name = "RobotMode", eq)] -#[derive(Clone, Copy, PartialEq, Eq)] -pub struct PyRobotMode(RobotMode); - -#[pymethods] -impl PyRobotMode { - #[classattr] - const UNKNOWN: Self = Self(RobotMode::Unknown); - #[classattr] - const DAMPING: Self = Self(RobotMode::Damping); - #[classattr] - const PREPARE: Self = Self(RobotMode::Prepare); - #[classattr] - const WALKING: Self = Self(RobotMode::Walking); - #[classattr] - const CUSTOM: Self = Self(RobotMode::Custom); - #[classattr] - const SOCCER: Self = Self(RobotMode::Soccer); - - fn __repr__(&self) -> String { - match self.0 { - RobotMode::Unknown => "RobotMode.UNKNOWN".to_string(), - RobotMode::Damping => "RobotMode.DAMPING".to_string(), - RobotMode::Prepare => "RobotMode.PREPARE".to_string(), - RobotMode::Walking => "RobotMode.WALKING".to_string(), - RobotMode::Custom => "RobotMode.CUSTOM".to_string(), - RobotMode::Soccer => "RobotMode.SOCCER".to_string(), - _ => format!("RobotMode({})", i32::from(self.0)), - } - } - - fn __int__(&self) -> i32 { - i32::from(self.0) - } -} - -impl From for RobotMode { - fn from(py_mode: PyRobotMode) -> Self { - py_mode.0 - } -} - -impl From for PyRobotMode { - fn from(mode: RobotMode) -> Self { - Self(mode) - } -} - -#[pyclass(module = "booster_sdk_bindings", name = "Hand", eq)] -#[derive(Clone, Copy, PartialEq, Eq)] -pub struct PyHand(Hand); - -#[pymethods] -impl PyHand { - #[classattr] - const LEFT: Self = Self(Hand::Left); - #[classattr] - const RIGHT: Self = Self(Hand::Right); - - fn __repr__(&self) -> String { - match self.0 { - Hand::Left => "Hand.LEFT".to_string(), - Hand::Right => "Hand.RIGHT".to_string(), - } - } - - fn __int__(&self) -> i32 { - i32::from(self.0) - } -} - -impl From for Hand { - fn from(py_hand: PyHand) -> Self { - py_hand.0 - } -} - -impl From for PyHand { - fn from(hand: Hand) -> Self { - Self(hand) - } -} - -#[pyclass(module = "booster_sdk_bindings", name = "GripperMode", eq)] -#[derive(Clone, Copy, PartialEq, Eq)] -pub struct PyGripperMode(GripperMode); - -#[pymethods] -impl PyGripperMode { - #[classattr] - const POSITION: Self = Self(GripperMode::Position); - #[classattr] - const FORCE: Self = Self(GripperMode::Force); - - fn __repr__(&self) -> String { - match self.0 { - GripperMode::Position => "GripperMode.POSITION".to_string(), - GripperMode::Force => "GripperMode.FORCE".to_string(), - } - } - - fn __int__(&self) -> i32 { - i32::from(self.0) - } -} - -impl From for GripperMode { - fn from(py_mode: PyGripperMode) -> Self { - py_mode.0 - } -} - -#[pyclass(module = "booster_sdk_bindings", name = "HandAction", eq)] -#[derive(Clone, Copy, PartialEq, Eq)] -pub struct PyHandAction(HandAction); - -#[pymethods] -impl PyHandAction { - #[classattr] - const OPEN: Self = Self(HandAction::Open); - #[classattr] - const CLOSE: Self = Self(HandAction::Close); - - fn __repr__(&self) -> String { - match self.0 { - HandAction::Open => "HandAction.OPEN".to_string(), - HandAction::Close => "HandAction.CLOSE".to_string(), - } - } - - fn __int__(&self) -> i32 { - i32::from(self.0) - } -} - -impl From for HandAction { - fn from(py_action: PyHandAction) -> Self { - py_action.0 - } -} - -#[pyclass(module = "booster_sdk_bindings", name = "Frame", eq)] -#[derive(Clone, Copy, PartialEq, Eq)] -pub struct PyFrame(Frame); - -#[pymethods] -impl PyFrame { - #[classattr] - const UNKNOWN: Self = Self(Frame::Unknown); - #[classattr] - const BODY: Self = Self(Frame::Body); - #[classattr] - const HEAD: Self = Self(Frame::Head); - #[classattr] - const LEFT_HAND: Self = Self(Frame::LeftHand); - #[classattr] - const RIGHT_HAND: Self = Self(Frame::RightHand); - #[classattr] - const LEFT_FOOT: Self = Self(Frame::LeftFoot); - #[classattr] - const RIGHT_FOOT: Self = Self(Frame::RightFoot); - - fn __repr__(&self) -> String { - match self.0 { - Frame::Unknown => "Frame.UNKNOWN".to_string(), - Frame::Body => "Frame.BODY".to_string(), - Frame::Head => "Frame.HEAD".to_string(), - Frame::LeftHand => "Frame.LEFT_HAND".to_string(), - Frame::RightHand => "Frame.RIGHT_HAND".to_string(), - Frame::LeftFoot => "Frame.LEFT_FOOT".to_string(), - Frame::RightFoot => "Frame.RIGHT_FOOT".to_string(), - } - } - - fn __int__(&self) -> i32 { - i32::from(self.0) - } -} - -impl From for Frame { - fn from(value: PyFrame) -> Self { - value.0 - } -} - -#[pyclass(module = "booster_sdk_bindings", name = "GripperControlMode", eq)] -#[derive(Clone, Copy, PartialEq, Eq)] -pub struct PyGripperControlMode(GripperControlMode); - -#[pymethods] -impl PyGripperControlMode { - #[classattr] - const POSITION: Self = Self(GripperControlMode::Position); - #[classattr] - const FORCE: Self = Self(GripperControlMode::Force); - - fn __repr__(&self) -> String { - match self.0 { - GripperControlMode::Position => "GripperControlMode.POSITION".to_string(), - GripperControlMode::Force => "GripperControlMode.FORCE".to_string(), - } - } - - fn __int__(&self) -> i32 { - i32::from(self.0) - } -} - -impl From for GripperControlMode { - fn from(value: PyGripperControlMode) -> Self { - value.0 - } -} - -#[pyclass(module = "booster_sdk_bindings", name = "BoosterHandType", eq)] -#[derive(Clone, Copy, PartialEq, Eq)] -pub struct PyBoosterHandType(BoosterHandType); - -#[pymethods] -impl PyBoosterHandType { - #[classattr] - const INSPIRE_HAND: Self = Self(BoosterHandType::InspireHand); - #[classattr] - const INSPIRE_TOUCH_HAND: Self = Self(BoosterHandType::InspireTouchHand); - #[classattr] - const REVO_HAND: Self = Self(BoosterHandType::RevoHand); - #[classattr] - const UNKNOWN: Self = Self(BoosterHandType::Unknown); - - fn __repr__(&self) -> String { - match self.0 { - BoosterHandType::InspireHand => "BoosterHandType.INSPIRE_HAND".to_string(), - BoosterHandType::InspireTouchHand => "BoosterHandType.INSPIRE_TOUCH_HAND".to_string(), - BoosterHandType::RevoHand => "BoosterHandType.REVO_HAND".to_string(), - BoosterHandType::Unknown => "BoosterHandType.UNKNOWN".to_string(), - } - } - - fn __int__(&self) -> i32 { - i32::from(self.0) - } -} - -impl From for BoosterHandType { - fn from(value: PyBoosterHandType) -> Self { - value.0 - } -} - -#[pyclass(module = "booster_sdk_bindings", name = "DanceId", eq)] -#[derive(Clone, Copy, PartialEq, Eq)] -pub struct PyDanceId(DanceId); - -#[pymethods] -impl PyDanceId { - #[classattr] - const NEW_YEAR: Self = Self(DanceId::NewYear); - #[classattr] - const NEZHA: Self = Self(DanceId::Nezha); - #[classattr] - const TOWARDS_FUTURE: Self = Self(DanceId::TowardsFuture); - #[classattr] - const DABBING_GESTURE: Self = Self(DanceId::DabbingGesture); - #[classattr] - const ULTRAMAN_GESTURE: Self = Self(DanceId::UltramanGesture); - #[classattr] - const RESPECT_GESTURE: Self = Self(DanceId::RespectGesture); - #[classattr] - const CHEERING_GESTURE: Self = Self(DanceId::CheeringGesture); - #[classattr] - const LUCKY_CAT_GESTURE: Self = Self(DanceId::LuckyCatGesture); - #[classattr] - const STOP: Self = Self(DanceId::Stop); - - fn __repr__(&self) -> String { - match self.0 { - DanceId::NewYear => "DanceId.NEW_YEAR".to_string(), - DanceId::Nezha => "DanceId.NEZHA".to_string(), - DanceId::TowardsFuture => "DanceId.TOWARDS_FUTURE".to_string(), - DanceId::DabbingGesture => "DanceId.DABBING_GESTURE".to_string(), - DanceId::UltramanGesture => "DanceId.ULTRAMAN_GESTURE".to_string(), - DanceId::RespectGesture => "DanceId.RESPECT_GESTURE".to_string(), - DanceId::CheeringGesture => "DanceId.CHEERING_GESTURE".to_string(), - DanceId::LuckyCatGesture => "DanceId.LUCKY_CAT_GESTURE".to_string(), - DanceId::Stop => "DanceId.STOP".to_string(), - } - } - - fn __int__(&self) -> i32 { - i32::from(self.0) - } -} - -impl From for DanceId { - fn from(value: PyDanceId) -> Self { - value.0 - } -} - -#[pyclass(module = "booster_sdk_bindings", name = "WholeBodyDanceId", eq)] -#[derive(Clone, Copy, PartialEq, Eq)] -pub struct PyWholeBodyDanceId(WholeBodyDanceId); - -#[pymethods] -impl PyWholeBodyDanceId { - #[classattr] - const ARBIC_DANCE: Self = Self(WholeBodyDanceId::ArbicDance); - #[classattr] - const MICHAEL_DANCE_1: Self = Self(WholeBodyDanceId::MichaelDance1); - #[classattr] - const MICHAEL_DANCE_2: Self = Self(WholeBodyDanceId::MichaelDance2); - #[classattr] - const MICHAEL_DANCE_3: Self = Self(WholeBodyDanceId::MichaelDance3); - #[classattr] - const MOON_WALK: Self = Self(WholeBodyDanceId::MoonWalk); - #[classattr] - const BOXING_STYLE_KICK: Self = Self(WholeBodyDanceId::BoxingStyleKick); - #[classattr] - const ROUNDHOUSE_KICK: Self = Self(WholeBodyDanceId::RoundhouseKick); - - fn __repr__(&self) -> String { - match self.0 { - WholeBodyDanceId::ArbicDance => "WholeBodyDanceId.ARBIC_DANCE".to_string(), - WholeBodyDanceId::MichaelDance1 => "WholeBodyDanceId.MICHAEL_DANCE_1".to_string(), - WholeBodyDanceId::MichaelDance2 => "WholeBodyDanceId.MICHAEL_DANCE_2".to_string(), - WholeBodyDanceId::MichaelDance3 => "WholeBodyDanceId.MICHAEL_DANCE_3".to_string(), - WholeBodyDanceId::MoonWalk => "WholeBodyDanceId.MOON_WALK".to_string(), - WholeBodyDanceId::BoxingStyleKick => "WholeBodyDanceId.BOXING_STYLE_KICK".to_string(), - WholeBodyDanceId::RoundhouseKick => "WholeBodyDanceId.ROUNDHOUSE_KICK".to_string(), - } - } - - fn __int__(&self) -> i32 { - i32::from(self.0) - } -} - -impl From for WholeBodyDanceId { - fn from(value: PyWholeBodyDanceId) -> Self { - value.0 - } -} - -#[pyclass(module = "booster_sdk_bindings", name = "JointOrder", eq)] -#[derive(Clone, Copy, PartialEq, Eq)] -pub struct PyJointOrder(JointOrder); - -#[pymethods] -impl PyJointOrder { - #[classattr] - const MUJOCO: Self = Self(JointOrder::MuJoCo); - #[classattr] - const ISAAC_LAB: Self = Self(JointOrder::IsaacLab); - - fn __repr__(&self) -> String { - match self.0 { - JointOrder::MuJoCo => "JointOrder.MUJOCO".to_string(), - JointOrder::IsaacLab => "JointOrder.ISAAC_LAB".to_string(), - } - } - - fn __int__(&self) -> i32 { - i32::from(self.0) - } -} - -impl From for JointOrder { - fn from(value: PyJointOrder) -> Self { - value.0 - } -} - -#[pyclass(module = "booster_sdk_bindings", name = "BodyControl", eq)] -#[derive(Clone, Copy, PartialEq, Eq)] -pub struct PyBodyControl(BodyControl); - -#[pymethods] -impl PyBodyControl { - #[classattr] - const UNKNOWN: Self = Self(BodyControl::Unknown); - #[classattr] - const DAMPING: Self = Self(BodyControl::Damping); - #[classattr] - const PREPARE: Self = Self(BodyControl::Prepare); - #[classattr] - const HUMANLIKE_GAIT: Self = Self(BodyControl::HumanlikeGait); - #[classattr] - const PRONE_BODY: Self = Self(BodyControl::ProneBody); - #[classattr] - const SOCCER_GAIT: Self = Self(BodyControl::SoccerGait); - #[classattr] - const CUSTOM: Self = Self(BodyControl::Custom); - #[classattr] - const GET_UP: Self = Self(BodyControl::GetUp); - #[classattr] - const WHOLE_BODY_DANCE: Self = Self(BodyControl::WholeBodyDance); - #[classattr] - const SHOOT: Self = Self(BodyControl::Shoot); - #[classattr] - const INSIDE_FOOT: Self = Self(BodyControl::InsideFoot); - #[classattr] - const GOALIE: Self = Self(BodyControl::Goalie); - #[classattr] - const WBC_GAIT: Self = Self(BodyControl::WbcGait); - - fn __repr__(&self) -> String { - match self.0 { - BodyControl::Unknown => "BodyControl.UNKNOWN".to_string(), - BodyControl::Damping => "BodyControl.DAMPING".to_string(), - BodyControl::Prepare => "BodyControl.PREPARE".to_string(), - BodyControl::HumanlikeGait => "BodyControl.HUMANLIKE_GAIT".to_string(), - BodyControl::ProneBody => "BodyControl.PRONE_BODY".to_string(), - BodyControl::SoccerGait => "BodyControl.SOCCER_GAIT".to_string(), - BodyControl::Custom => "BodyControl.CUSTOM".to_string(), - BodyControl::GetUp => "BodyControl.GET_UP".to_string(), - BodyControl::WholeBodyDance => "BodyControl.WHOLE_BODY_DANCE".to_string(), - BodyControl::Shoot => "BodyControl.SHOOT".to_string(), - BodyControl::InsideFoot => "BodyControl.INSIDE_FOOT".to_string(), - BodyControl::Goalie => "BodyControl.GOALIE".to_string(), - BodyControl::WbcGait => "BodyControl.WBC_GAIT".to_string(), - } - } - - fn __int__(&self) -> i32 { - i32::from(self.0) - } -} - -impl From for PyBodyControl { - fn from(value: BodyControl) -> Self { - Self(value) - } -} - -#[pyclass(module = "booster_sdk_bindings", name = "Action", eq)] -#[derive(Clone, Copy, PartialEq, Eq)] -pub struct PyAction(Action); - -#[pymethods] -impl PyAction { - #[classattr] - const UNKNOWN: Self = Self(Action::Unknown); - #[classattr] - const HAND_SHAKE: Self = Self(Action::HandShake); - #[classattr] - const HAND_WAVE: Self = Self(Action::HandWave); - #[classattr] - const HAND_CONTROL: Self = Self(Action::HandControl); - #[classattr] - const DANCE_NEW_YEAR: Self = Self(Action::DanceNewYear); - #[classattr] - const DANCE_NEZHA: Self = Self(Action::DanceNezha); - #[classattr] - const DANCE_TOWARDS_FUTURE: Self = Self(Action::DanceTowardsFuture); - #[classattr] - const GESTURE_DABBING: Self = Self(Action::GestureDabbing); - #[classattr] - const GESTURE_ULTRAMAN: Self = Self(Action::GestureUltraman); - #[classattr] - const GESTURE_RESPECT: Self = Self(Action::GestureRespect); - #[classattr] - const GESTURE_CHEER: Self = Self(Action::GestureCheer); - #[classattr] - const GESTURE_LUCKY_CAT: Self = Self(Action::GestureLuckyCat); - #[classattr] - const GESTURE_BOXING: Self = Self(Action::GestureBoxing); - #[classattr] - const ZERO_TORQUE_DRAG: Self = Self(Action::ZeroTorqueDrag); - #[classattr] - const RECORD_TRAJ: Self = Self(Action::RecordTraj); - #[classattr] - const RUN_RECORDED_TRAJ: Self = Self(Action::RunRecordedTraj); - - fn __repr__(&self) -> String { - match self.0 { - Action::Unknown => "Action.UNKNOWN".to_string(), - Action::HandShake => "Action.HAND_SHAKE".to_string(), - Action::HandWave => "Action.HAND_WAVE".to_string(), - Action::HandControl => "Action.HAND_CONTROL".to_string(), - Action::DanceNewYear => "Action.DANCE_NEW_YEAR".to_string(), - Action::DanceNezha => "Action.DANCE_NEZHA".to_string(), - Action::DanceTowardsFuture => "Action.DANCE_TOWARDS_FUTURE".to_string(), - Action::GestureDabbing => "Action.GESTURE_DABBING".to_string(), - Action::GestureUltraman => "Action.GESTURE_ULTRAMAN".to_string(), - Action::GestureRespect => "Action.GESTURE_RESPECT".to_string(), - Action::GestureCheer => "Action.GESTURE_CHEER".to_string(), - Action::GestureLuckyCat => "Action.GESTURE_LUCKY_CAT".to_string(), - Action::GestureBoxing => "Action.GESTURE_BOXING".to_string(), - Action::ZeroTorqueDrag => "Action.ZERO_TORQUE_DRAG".to_string(), - Action::RecordTraj => "Action.RECORD_TRAJ".to_string(), - Action::RunRecordedTraj => "Action.RUN_RECORDED_TRAJ".to_string(), - } - } - - fn __int__(&self) -> i32 { - i32::from(self.0) - } -} - -impl From for PyAction { - fn from(value: Action) -> Self { - Self(value) - } -} - -#[pyclass(module = "booster_sdk_bindings", name = "CameraSetMode", eq)] -#[derive(Clone, Copy, PartialEq, Eq)] -pub struct PyCameraSetMode(CameraSetMode); - -#[pymethods] -impl PyCameraSetMode { - #[classattr] - const CAMERA_MODE_NORMAL: Self = Self(CameraSetMode::CameraModeNormal); - #[classattr] - const CAMERA_MODE_HIGH_RESOLUTION: Self = Self(CameraSetMode::CameraModeHighResolution); - #[classattr] - const CAMERA_MODE_NORMAL_ENABLE: Self = Self(CameraSetMode::CameraModeNormalEnable); - #[classattr] - const CAMERA_MODE_HIGH_RESOLUTION_ENABLE: Self = - Self(CameraSetMode::CameraModeHighResolutionEnable); - - fn __repr__(&self) -> String { - match self.0 { - CameraSetMode::CameraModeNormal => "CameraSetMode.CAMERA_MODE_NORMAL".to_string(), - CameraSetMode::CameraModeHighResolution => { - "CameraSetMode.CAMERA_MODE_HIGH_RESOLUTION".to_string() - } - CameraSetMode::CameraModeNormalEnable => { - "CameraSetMode.CAMERA_MODE_NORMAL_ENABLE".to_string() - } - CameraSetMode::CameraModeHighResolutionEnable => { - "CameraSetMode.CAMERA_MODE_HIGH_RESOLUTION_ENABLE".to_string() - } - } - } - - fn __int__(&self) -> i32 { - i32::from(self.0) - } -} - -impl From for CameraSetMode { - fn from(value: PyCameraSetMode) -> Self { - value.0 - } -} - -#[pyclass(module = "booster_sdk_bindings", name = "CameraControlStatus", eq)] -#[derive(Clone, Copy, PartialEq, Eq)] -pub struct PyCameraControlStatus(CameraControlStatus); - -#[pymethods] -impl PyCameraControlStatus { - #[classattr] - const CAMERA_STATUS_NORMAL: Self = Self(CameraControlStatus::CameraStatusNormal); - #[classattr] - const CAMERA_STATUS_HIGH_RESOLUTION: Self = - Self(CameraControlStatus::CameraStatusHighResolution); - #[classattr] - const CAMERA_STATUS_ERROR: Self = Self(CameraControlStatus::CameraStatusError); - #[classattr] - const CAMERA_STATUS_NULL: Self = Self(CameraControlStatus::CameraStatusNull); - - fn __repr__(&self) -> String { - match self.0 { - CameraControlStatus::CameraStatusNormal => { - "CameraControlStatus.CAMERA_STATUS_NORMAL".to_string() - } - CameraControlStatus::CameraStatusHighResolution => { - "CameraControlStatus.CAMERA_STATUS_HIGH_RESOLUTION".to_string() - } - CameraControlStatus::CameraStatusError => { - "CameraControlStatus.CAMERA_STATUS_ERROR".to_string() - } - CameraControlStatus::CameraStatusNull => { - "CameraControlStatus.CAMERA_STATUS_NULL".to_string() - } - } - } - - fn __int__(&self) -> i32 { - i32::from(self.0) - } -} - -impl From for PyCameraControlStatus { - fn from(value: CameraControlStatus) -> Self { - Self(value) - } -} - -#[pyclass(module = "booster_sdk_bindings", name = "GripperCommand")] -#[derive(Clone)] -pub struct PyGripperCommand(GripperCommand); - -#[pymethods] -impl PyGripperCommand { - #[new] - fn new(hand: PyHand, mode: PyGripperMode, motion_param: u16, speed: Option) -> Self { - Self(GripperCommand { - hand: hand.into(), - mode: mode.into(), - motion_param, - speed: speed.unwrap_or(500), - }) - } - - #[staticmethod] - fn open(hand: PyHand) -> Self { - Self(GripperCommand::open(hand.into())) - } - - #[staticmethod] - fn close(hand: PyHand) -> Self { - Self(GripperCommand::close(hand.into())) - } - - #[staticmethod] - fn grasp(hand: PyHand, force: u16) -> Self { - Self(GripperCommand::grasp(hand.into(), force)) - } - - #[getter] - fn hand(&self) -> PyHand { - self.0.hand.into() - } - - #[getter] - fn mode(&self) -> PyGripperMode { - PyGripperMode(self.0.mode) - } - - #[getter] - fn motion_param(&self) -> u16 { - self.0.motion_param - } - - #[getter] - fn speed(&self) -> u16 { - self.0.speed - } - - fn __repr__(&self) -> String { - format!( - "GripperCommand(hand={}, mode={}, motion_param={}, speed={})", - u8::from(self.0.hand), - i32::from(self.0.mode), - self.0.motion_param, - self.0.speed - ) - } -} - -impl From for GripperCommand { - fn from(value: PyGripperCommand) -> Self { - value.0 - } -} - -#[pyclass(module = "booster_sdk_bindings", name = "Position")] -#[derive(Clone, Copy)] -pub struct PyPosition(Position); - -#[pymethods] -impl PyPosition { - #[new] - fn new(x: f32, y: f32, z: f32) -> Self { - Self(Position { x, y, z }) - } - - #[getter] - fn x(&self) -> f32 { - self.0.x - } - - #[setter] - fn set_x(&mut self, value: f32) { - self.0.x = value; - } - - #[getter] - fn y(&self) -> f32 { - self.0.y - } - - #[setter] - fn set_y(&mut self, value: f32) { - self.0.y = value; - } - - #[getter] - fn z(&self) -> f32 { - self.0.z - } - - #[setter] - fn set_z(&mut self, value: f32) { - self.0.z = value; - } - - fn __repr__(&self) -> String { - format!("Position(x={}, y={}, z={})", self.0.x, self.0.y, self.0.z) - } -} - -impl From for Position { - fn from(value: PyPosition) -> Self { - value.0 - } -} - -impl From for PyPosition { - fn from(value: Position) -> Self { - Self(value) - } -} - -#[pyclass(module = "booster_sdk_bindings", name = "Orientation")] -#[derive(Clone, Copy)] -pub struct PyOrientation(Orientation); - -#[pymethods] -impl PyOrientation { - #[new] - fn new(roll: f32, pitch: f32, yaw: f32) -> Self { - Self(Orientation { roll, pitch, yaw }) - } - - #[getter] - fn roll(&self) -> f32 { - self.0.roll - } - - #[setter] - fn set_roll(&mut self, value: f32) { - self.0.roll = value; - } - - #[getter] - fn pitch(&self) -> f32 { - self.0.pitch - } - - #[setter] - fn set_pitch(&mut self, value: f32) { - self.0.pitch = value; - } - - #[getter] - fn yaw(&self) -> f32 { - self.0.yaw - } - - #[setter] - fn set_yaw(&mut self, value: f32) { - self.0.yaw = value; - } - - fn __repr__(&self) -> String { - format!( - "Orientation(roll={}, pitch={}, yaw={})", - self.0.roll, self.0.pitch, self.0.yaw - ) - } -} - -impl From for Orientation { - fn from(value: PyOrientation) -> Self { - value.0 - } -} - -impl From for PyOrientation { - fn from(value: Orientation) -> Self { - Self(value) - } -} - -#[pyclass(module = "booster_sdk_bindings", name = "Posture")] -#[derive(Clone, Copy)] -pub struct PyPosture(Posture); - -#[pymethods] -impl PyPosture { - #[new] - fn new(position: PyPosition, orientation: PyOrientation) -> Self { - Self(Posture { - position: position.into(), - orientation: orientation.into(), - }) - } - - #[getter] - fn position(&self) -> PyPosition { - self.0.position.into() - } - - #[setter] - fn set_position(&mut self, value: PyPosition) { - self.0.position = value.into(); - } - - #[getter] - fn orientation(&self) -> PyOrientation { - self.0.orientation.into() - } - - #[setter] - fn set_orientation(&mut self, value: PyOrientation) { - self.0.orientation = value.into(); - } - - fn __repr__(&self) -> String { - format!( - "Posture(position={}, orientation={})", - self.position().__repr__(), - self.orientation().__repr__() - ) - } -} - -impl From for Posture { - fn from(value: PyPosture) -> Self { - value.0 - } -} - -impl From for PyPosture { - fn from(value: Posture) -> Self { - Self(value) - } -} - -#[pyclass(module = "booster_sdk_bindings", name = "Quaternion")] -#[derive(Clone, Copy)] -pub struct PyQuaternion(Quaternion); - -#[pymethods] -impl PyQuaternion { - #[new] - fn new(x: f32, y: f32, z: f32, w: f32) -> Self { - Self(Quaternion { x, y, z, w }) - } - - #[getter] - fn x(&self) -> f32 { - self.0.x - } - - #[getter] - fn y(&self) -> f32 { - self.0.y - } - - #[getter] - fn z(&self) -> f32 { - self.0.z - } - - #[getter] - fn w(&self) -> f32 { - self.0.w - } - - fn __repr__(&self) -> String { - format!( - "Quaternion(x={}, y={}, z={}, w={})", - self.0.x, self.0.y, self.0.z, self.0.w - ) - } -} - -impl From for PyQuaternion { - fn from(value: Quaternion) -> Self { - Self(value) - } -} - -#[pyclass(module = "booster_sdk_bindings", name = "Transform")] -#[derive(Clone, Copy)] -pub struct PyTransform(Transform); - -#[pymethods] -impl PyTransform { - #[new] - fn new(position: PyPosition, orientation: PyQuaternion) -> Self { - Self(Transform { - position: position.into(), - orientation: orientation.0, - }) - } - - #[getter] - fn position(&self) -> PyPosition { - self.0.position.into() - } - - #[getter] - fn orientation(&self) -> PyQuaternion { - self.0.orientation.into() - } - - fn __repr__(&self) -> String { - format!( - "Transform(position={}, orientation={})", - self.position().__repr__(), - self.orientation().__repr__() - ) - } -} - -impl From for PyTransform { - fn from(value: Transform) -> Self { - Self(value) - } -} - -#[pyclass(module = "booster_sdk_bindings", name = "GripperMotionParameter")] -#[derive(Clone, Copy)] -pub struct PyGripperMotionParameter(GripperMotionParameter); - -#[pymethods] -impl PyGripperMotionParameter { - #[new] - fn new(position: i32, force: i32, speed: i32) -> Self { - Self(GripperMotionParameter { - position, - force, - speed, - }) - } - - #[getter] - fn position(&self) -> i32 { - self.0.position - } - - #[getter] - fn force(&self) -> i32 { - self.0.force - } - - #[getter] - fn speed(&self) -> i32 { - self.0.speed - } - - fn __repr__(&self) -> String { - format!( - "GripperMotionParameter(position={}, force={}, speed={})", - self.0.position, self.0.force, self.0.speed - ) - } -} - -impl From for GripperMotionParameter { - fn from(value: PyGripperMotionParameter) -> Self { - value.0 - } -} - -#[pyclass(module = "booster_sdk_bindings", name = "DexterousFingerParameter")] -#[derive(Clone, Copy)] -pub struct PyDexterousFingerParameter(DexterousFingerParameter); - -#[pymethods] -impl PyDexterousFingerParameter { - #[new] - fn new(seq: i32, angle: i32, force: i32, speed: i32) -> Self { - Self(DexterousFingerParameter { - seq, - angle, - force, - speed, - }) - } - - #[getter] - fn seq(&self) -> i32 { - self.0.seq - } - - #[getter] - fn angle(&self) -> i32 { - self.0.angle - } - - #[getter] - fn force(&self) -> i32 { - self.0.force - } - - #[getter] - fn speed(&self) -> i32 { - self.0.speed - } - - fn __repr__(&self) -> String { - format!( - "DexterousFingerParameter(seq={}, angle={}, force={}, speed={})", - self.0.seq, self.0.angle, self.0.force, self.0.speed - ) - } -} - -impl From for DexterousFingerParameter { - fn from(value: PyDexterousFingerParameter) -> Self { - value.0 - } -} - -impl From for PyDexterousFingerParameter { - fn from(value: DexterousFingerParameter) -> Self { - Self(value) - } -} - -#[pyclass(module = "booster_sdk_bindings", name = "CustomModelParams")] -#[derive(Clone)] -pub struct PyCustomModelParams(CustomModelParams); - -#[pymethods] -impl PyCustomModelParams { - #[new] - fn new(action_scale: Vec, kp: Vec, kd: Vec) -> Self { - Self(CustomModelParams { - action_scale, - kp, - kd, - }) - } - - #[getter] - fn action_scale(&self) -> Vec { - self.0.action_scale.clone() - } - - #[getter] - fn kp(&self) -> Vec { - self.0.kp.clone() - } - - #[getter] - fn kd(&self) -> Vec { - self.0.kd.clone() - } - - fn __repr__(&self) -> String { - "CustomModelParams(...)".to_string() - } -} - -impl From for CustomModelParams { - fn from(value: PyCustomModelParams) -> Self { - value.0 - } -} - -impl From for PyCustomModelParams { - fn from(value: CustomModelParams) -> Self { - Self(value) - } -} - -#[pyclass(module = "booster_sdk_bindings", name = "CustomModel")] -#[derive(Clone)] -pub struct PyCustomModel(CustomModel); - -#[pymethods] -impl PyCustomModel { - #[new] - fn new(file_path: String, params: Vec, joint_order: PyJointOrder) -> Self { - Self(CustomModel { - file_path, - params: params.into_iter().map(Into::into).collect(), - joint_order: joint_order.into(), - }) - } - - #[getter] - fn file_path(&self) -> String { - self.0.file_path.clone() - } - - #[getter] - fn params(&self) -> Vec { - self.0.params.clone().into_iter().map(Into::into).collect() - } - - #[getter] - fn joint_order(&self) -> PyJointOrder { - PyJointOrder(self.0.joint_order) - } - - fn __repr__(&self) -> String { - format!("CustomModel(file_path='{}', params=...)", self.0.file_path) - } -} - -impl From for CustomModel { - fn from(value: PyCustomModel) -> Self { - value.0 - } -} - -impl From for PyCustomModel { - fn from(value: CustomModel) -> Self { - Self(value) - } -} - -#[pyclass(module = "booster_sdk_bindings", name = "CustomTrainedTraj")] -#[derive(Clone)] -pub struct PyCustomTrainedTraj(CustomTrainedTraj); - -#[pymethods] -impl PyCustomTrainedTraj { - #[new] - fn new(traj_file_path: String, model: PyCustomModel) -> Self { - Self(CustomTrainedTraj { - traj_file_path, - model: model.into(), - }) - } - - #[getter] - fn traj_file_path(&self) -> String { - self.0.traj_file_path.clone() - } - - #[getter] - fn model(&self) -> PyCustomModel { - self.0.model.clone().into() - } - - fn __repr__(&self) -> String { - format!( - "CustomTrainedTraj(traj_file_path='{}', model=...)", - self.0.traj_file_path - ) - } -} - -impl From for CustomTrainedTraj { - fn from(value: PyCustomTrainedTraj) -> Self { - value.0 - } -} - -#[pyclass(module = "booster_sdk_bindings", name = "TtsConfig")] -#[derive(Clone)] -pub struct PyTtsConfig(TtsConfig); - -#[pymethods] -impl PyTtsConfig { - #[new] - fn new(voice_type: String, ignore_bracket_text: Vec) -> Self { - Self(TtsConfig { - voice_type, - ignore_bracket_text, - }) - } - - #[getter] - fn voice_type(&self) -> String { - self.0.voice_type.clone() - } - - #[getter] - fn ignore_bracket_text(&self) -> Vec { - self.0.ignore_bracket_text.clone() - } -} - -impl From for TtsConfig { - fn from(value: PyTtsConfig) -> Self { - value.0 - } -} - -#[pyclass(module = "booster_sdk_bindings", name = "LlmConfig")] -#[derive(Clone)] -pub struct PyLlmConfig(LlmConfig); - -#[pymethods] -impl PyLlmConfig { - #[new] - fn new(system_prompt: String, welcome_msg: String, prompt_name: String) -> Self { - Self(LlmConfig { - system_prompt, - welcome_msg, - prompt_name, - }) - } - - #[getter] - fn system_prompt(&self) -> String { - self.0.system_prompt.clone() - } - - #[getter] - fn welcome_msg(&self) -> String { - self.0.welcome_msg.clone() - } - - #[getter] - fn prompt_name(&self) -> String { - self.0.prompt_name.clone() - } -} - -impl From for LlmConfig { - fn from(value: PyLlmConfig) -> Self { - value.0 - } -} - -#[pyclass(module = "booster_sdk_bindings", name = "AsrConfig")] -#[derive(Clone)] -pub struct PyAsrConfig(AsrConfig); - -#[pymethods] -impl PyAsrConfig { - #[new] - fn new(interrupt_speech_duration: i32, interrupt_keywords: Vec) -> Self { - Self(AsrConfig { - interrupt_speech_duration, - interrupt_keywords, - }) - } - - #[getter] - fn interrupt_speech_duration(&self) -> i32 { - self.0.interrupt_speech_duration - } - - #[getter] - fn interrupt_keywords(&self) -> Vec { - self.0.interrupt_keywords.clone() - } -} - -impl From for AsrConfig { - fn from(value: PyAsrConfig) -> Self { - value.0 - } -} - -#[pyclass(module = "booster_sdk_bindings", name = "StartAiChatParameter")] -#[derive(Clone)] -pub struct PyStartAiChatParameter(StartAiChatParameter); - -#[pymethods] -impl PyStartAiChatParameter { - #[new] - fn new( - interrupt_mode: bool, - asr_config: PyAsrConfig, - llm_config: PyLlmConfig, - tts_config: PyTtsConfig, - enable_face_tracking: bool, - ) -> Self { - Self(StartAiChatParameter { - interrupt_mode, - asr_config: asr_config.into(), - llm_config: llm_config.into(), - tts_config: tts_config.into(), - enable_face_tracking, - }) - } - - #[getter] - fn interrupt_mode(&self) -> bool { - self.0.interrupt_mode - } - - #[getter] - fn asr_config(&self) -> PyAsrConfig { - PyAsrConfig(self.0.asr_config.clone()) - } - - #[getter] - fn llm_config(&self) -> PyLlmConfig { - PyLlmConfig(self.0.llm_config.clone()) - } - - #[getter] - fn tts_config(&self) -> PyTtsConfig { - PyTtsConfig(self.0.tts_config.clone()) - } - - #[getter] - fn enable_face_tracking(&self) -> bool { - self.0.enable_face_tracking - } -} - -impl From for StartAiChatParameter { - fn from(value: PyStartAiChatParameter) -> Self { - value.0 - } -} - -#[pyclass(module = "booster_sdk_bindings", name = "SpeakParameter")] -#[derive(Clone)] -pub struct PySpeakParameter(SpeakParameter); - -#[pymethods] -impl PySpeakParameter { - #[new] - fn new(msg: String) -> Self { - Self(SpeakParameter { msg }) - } - - #[getter] - fn msg(&self) -> String { - self.0.msg.clone() - } -} - -impl From for SpeakParameter { - fn from(value: PySpeakParameter) -> Self { - value.0 - } -} - -#[pyclass(module = "booster_sdk_bindings", name = "LuiTtsConfig")] -#[derive(Clone)] -pub struct PyLuiTtsConfig(LuiTtsConfig); - -#[pymethods] -impl PyLuiTtsConfig { - #[new] - fn new(voice_type: String) -> Self { - Self(LuiTtsConfig { voice_type }) - } - - #[getter] - fn voice_type(&self) -> String { - self.0.voice_type.clone() - } -} - -impl From for LuiTtsConfig { - fn from(value: PyLuiTtsConfig) -> Self { - value.0 - } -} - -#[pyclass(module = "booster_sdk_bindings", name = "LuiTtsParameter")] -#[derive(Clone)] -pub struct PyLuiTtsParameter(LuiTtsParameter); - -#[pymethods] -impl PyLuiTtsParameter { - #[new] - fn new(text: String) -> Self { - Self(LuiTtsParameter { text }) - } - - #[getter] - fn text(&self) -> String { - self.0.text.clone() - } -} - -impl From for LuiTtsParameter { - fn from(value: PyLuiTtsParameter) -> Self { - value.0 - } -} - -#[pyclass(module = "booster_sdk_bindings", name = "GetModeResponse")] -#[derive(Clone)] -pub struct PyGetModeResponse(GetModeResponse); - -#[pymethods] -impl PyGetModeResponse { - #[new] - fn new(mode: i32) -> Self { - Self(GetModeResponse { mode }) - } - - #[getter] - fn mode(&self) -> i32 { - self.0.mode - } - - fn mode_enum(&self) -> Option { - self.0.mode_enum().map(Into::into) - } - - fn __repr__(&self) -> String { - format!("GetModeResponse(mode={})", self.0.mode) - } -} - -impl From for PyGetModeResponse { - fn from(value: GetModeResponse) -> Self { - Self(value) - } -} - -#[pyclass(module = "booster_sdk_bindings", name = "GetStatusResponse")] -#[derive(Clone)] -pub struct PyGetStatusResponse(GetStatusResponse); - -#[pymethods] -impl PyGetStatusResponse { - #[new] - fn new(current_mode: i32, current_body_control: i32, current_actions: Vec) -> Self { - Self(GetStatusResponse { - current_mode, - current_body_control, - current_actions, - }) - } - - #[getter] - fn current_mode(&self) -> i32 { - self.0.current_mode - } - - #[getter] - fn current_body_control(&self) -> i32 { - self.0.current_body_control - } - - #[getter] - fn current_actions(&self) -> Vec { - self.0.current_actions.clone() - } - - fn current_mode_enum(&self) -> Option { - self.0.current_mode_enum().map(Into::into) - } - - fn current_body_control_enum(&self) -> Option { - self.0.current_body_control_enum().map(Into::into) - } - - fn current_actions_enum(&self) -> Vec { - self.0 - .current_actions_enum() - .into_iter() - .map(Into::into) - .collect() - } - - fn __repr__(&self) -> String { - format!( - "GetStatusResponse(current_mode={}, current_body_control={}, current_actions={:?})", - self.0.current_mode, self.0.current_body_control, self.0.current_actions - ) - } -} - -impl From for PyGetStatusResponse { - fn from(value: GetStatusResponse) -> Self { - Self(value) - } -} - -#[pyclass(module = "booster_sdk_bindings", name = "GetRobotInfoResponse")] -#[derive(Clone)] -pub struct PyGetRobotInfoResponse(GetRobotInfoResponse); - -#[pymethods] -impl PyGetRobotInfoResponse { - #[new] - fn new( - name: String, - nickname: String, - version: String, - model: String, - serial_number: String, - ) -> Self { - Self(GetRobotInfoResponse { - name, - nickname, - version, - model, - serial_number, - }) - } - - #[getter] - fn name(&self) -> String { - self.0.name.clone() - } - - #[getter] - fn nickname(&self) -> String { - self.0.nickname.clone() - } - - #[getter] - fn version(&self) -> String { - self.0.version.clone() - } - - #[getter] - fn model(&self) -> String { - self.0.model.clone() - } - - #[getter] - fn serial_number(&self) -> String { - self.0.serial_number.clone() - } - - fn __repr__(&self) -> String { - format!( - "GetRobotInfoResponse(name='{}', nickname='{}', version='{}', model='{}', serial_number='{}')", - self.0.name, self.0.nickname, self.0.version, self.0.model, self.0.serial_number - ) - } -} - -impl From for PyGetRobotInfoResponse { - fn from(value: GetRobotInfoResponse) -> Self { - Self(value) - } -} - -#[pyclass( - module = "booster_sdk_bindings", - name = "LoadCustomTrainedTrajResponse" -)] -#[derive(Clone)] -pub struct PyLoadCustomTrainedTrajResponse(LoadCustomTrainedTrajResponse); - -#[pymethods] -impl PyLoadCustomTrainedTrajResponse { - #[new] - fn new(tid: String) -> Self { - Self(LoadCustomTrainedTrajResponse { tid }) - } - - #[getter] - fn tid(&self) -> String { - self.0.tid.clone() - } - - fn __repr__(&self) -> String { - format!("LoadCustomTrainedTrajResponse(tid='{}')", self.0.tid) - } -} - -impl From for PyLoadCustomTrainedTrajResponse { - fn from(value: LoadCustomTrainedTrajResponse) -> Self { - Self(value) - } -} - -#[pyclass(module = "booster_sdk_bindings", name = "X5CameraGetStatusResponse")] -#[derive(Clone)] -pub struct PyX5CameraGetStatusResponse(X5CameraStatusResponse); - -#[pymethods] -impl PyX5CameraGetStatusResponse { - #[new] - fn new(status: i32) -> Self { - Self(X5CameraStatusResponse { status }) - } - - #[getter] - fn status(&self) -> i32 { - self.0.status - } - - fn status_enum(&self) -> Option { - self.0.status_enum().map(Into::into) - } - - fn __repr__(&self) -> String { - format!("X5CameraGetStatusResponse(status={})", self.0.status) - } -} - -impl From for PyX5CameraGetStatusResponse { - fn from(value: X5CameraStatusResponse) -> Self { - Self(value) - } -} - -#[pyclass(module = "booster_sdk_bindings", name = "DetectResults")] -#[derive(Clone)] -pub struct PyDetectResults(DetectResults); - -#[pymethods] -impl PyDetectResults { - #[new] - fn new( - xmin: i64, - ymin: i64, - xmax: i64, - ymax: i64, - position: Vec, - tag: String, - conf: f32, - rgb_mean: Vec, - ) -> Self { - Self(DetectResults { - xmin, - ymin, - xmax, - ymax, - position, - tag, - conf, - rgb_mean, - }) - } - - #[getter] - fn xmin(&self) -> i64 { - self.0.xmin - } - - #[getter] - fn ymin(&self) -> i64 { - self.0.ymin - } - - #[getter] - fn xmax(&self) -> i64 { - self.0.xmax - } - - #[getter] - fn ymax(&self) -> i64 { - self.0.ymax - } - - #[getter] - fn position(&self) -> Vec { - self.0.position.clone() - } - - #[getter] - fn tag(&self) -> String { - self.0.tag.clone() - } - - #[getter] - fn conf(&self) -> f32 { - self.0.conf - } - - #[getter] - fn rgb_mean(&self) -> Vec { - self.0.rgb_mean.clone() - } - - fn __repr__(&self) -> String { - format!( - "DetectResults(tag='{}', conf={}, bbox=({}, {}, {}, {}))", - self.0.tag, self.0.conf, self.0.xmin, self.0.ymin, self.0.xmax, self.0.ymax - ) - } -} - -impl From for PyDetectResults { - fn from(value: DetectResults) -> Self { - Self(value) - } -} - #[pymodule] fn booster_sdk_bindings(py: Python<'_>, m: &Bound<'_, PyModule>) -> PyResult<()> { m.add("BoosterSdkError", py.get_type::())?; @@ -1705,47 +17,5 @@ fn booster_sdk_bindings(py: Python<'_>, m: &Bound<'_, PyModule>) -> PyResult<()> client::register_classes(m)?; - m.add_class::()?; - m.add_class::()?; - m.add_class::()?; - m.add_class::()?; - m.add_class::()?; - m.add_class::()?; - m.add_class::()?; - m.add_class::()?; - m.add_class::()?; - m.add_class::()?; - m.add_class::()?; - m.add_class::()?; - m.add_class::()?; - m.add_class::()?; - - m.add_class::()?; - m.add_class::()?; - m.add_class::()?; - m.add_class::()?; - m.add_class::()?; - m.add_class::()?; - m.add_class::()?; - m.add_class::()?; - m.add_class::()?; - m.add_class::()?; - m.add_class::()?; - - m.add_class::()?; - m.add_class::()?; - m.add_class::()?; - m.add_class::()?; - m.add_class::()?; - m.add_class::()?; - m.add_class::()?; - - m.add_class::()?; - m.add_class::()?; - m.add_class::()?; - m.add_class::()?; - m.add_class::()?; - m.add_class::()?; - Ok(()) } From 658a8ff07ecc693795d6af6d741209b401eb68ad Mon Sep 17 00:00:00 2001 From: Gijs de Jong Date: Sun, 22 Feb 2026 21:26:11 +0100 Subject: [PATCH 14/15] fix publish to crates io --- .github/workflows/release.yml | 231 +++++++++++++++++----------------- 1 file changed, 116 insertions(+), 115 deletions(-) diff --git a/.github/workflows/release.yml b/.github/workflows/release.yml index 9975f0a..fabc046 100644 --- a/.github/workflows/release.yml +++ b/.github/workflows/release.yml @@ -1,122 +1,123 @@ name: Release on: - workflow_dispatch: + workflow_dispatch: permissions: - contents: read + contents: read jobs: - build: - name: Build wheels (${{ matrix.platform.id }}) - runs-on: ${{ matrix.platform.runner }} - strategy: - fail-fast: false - matrix: - platform: - # manylinux_2_28 x86_64 (cp310-cp313) - - id: manylinux_2_28-x86_64 - runner: ubuntu-22.04 - target: x86_64-unknown-linux-gnu - manylinux: "2_28" - maturin_args: >- - --release --manifest-path booster_sdk_py/Cargo.toml --out dist --compatibility pypi - -i python3.10 -i python3.11 -i python3.12 -i python3.13 - - # manylinux_2_28 aarch64 (cp310-cp313) - - id: manylinux_2_28-aarch64 - runner: ubuntu-22.04 - target: aarch64-unknown-linux-gnu - manylinux: "2_28" - maturin_args: >- - --release --manifest-path booster_sdk_py/Cargo.toml --out dist --compatibility pypi - -i python3.10 -i python3.11 -i python3.12 -i python3.13 - - # macOS arm64 (cp310) - - id: macos-arm64-cp310 - runner: macos-14 - target: aarch64-apple-darwin - python: "3.10" - manylinux: "" - maturin_args: --release --manifest-path booster_sdk_py/Cargo.toml --out dist --compatibility pypi -i python - - # macOS arm64 (cp311) - - id: macos-arm64-cp311 - runner: macos-14 - target: aarch64-apple-darwin - python: "3.11" - manylinux: "" - maturin_args: --release --manifest-path booster_sdk_py/Cargo.toml --out dist --compatibility pypi -i python - - # macOS arm64 (cp312) - - id: macos-arm64-cp312 - runner: macos-14 - target: aarch64-apple-darwin - python: "3.12" - manylinux: "" - maturin_args: --release --manifest-path booster_sdk_py/Cargo.toml --out dist --compatibility pypi -i python - - # macOS arm64 (cp313) - - id: macos-arm64-cp313 - runner: macos-14 - target: aarch64-apple-darwin - python: "3.13" - manylinux: "" - maturin_args: --release --manifest-path booster_sdk_py/Cargo.toml --out dist --compatibility pypi -i python - - steps: - - uses: actions/checkout@v4 - - # Only needed for macOS builds (Linux manylinux builds run inside the container and - # actions/setup-python won't affect those). - - name: Setup Python (macOS only) - if: startsWith(matrix.platform.runner, 'macos') - uses: actions/setup-python@v5 - with: - python-version: ${{ matrix.platform.python }} - - - name: Build wheels - uses: PyO3/maturin-action@v1 - with: - command: build - target: ${{ matrix.platform.target }} - manylinux: ${{ matrix.platform.manylinux }} - args: ${{ matrix.platform.maturin_args }} - - - name: Upload wheels - uses: actions/upload-artifact@v4 - with: - name: wheels-${{ matrix.platform.id }} - path: dist/*.whl - - publish-crate: - runs-on: ubuntu-22.04 - environment: release - steps: - - uses: actions/checkout@v4 - - uses: prefix-dev/setup-pixi@v0.9.1 - with: - pixi-version: v0.63.2 - - name: Publish to crates.io - run: pixi run cargo publish - env: - CARGO_REGISTRY_TOKEN: ${{ secrets.CARGO_REGISTRY_TOKEN }} - - publish: - runs-on: ubuntu-22.04 - needs: build - environment: release - permissions: - id-token: write - steps: - - uses: actions/download-artifact@v4 - with: - pattern: wheels-* - merge-multiple: true - path: dist/ - - - name: Install uv - uses: astral-sh/setup-uv@v5 - - - name: Publish to PyPI - run: uv publish dist/* + build: + name: Build wheels (${{ matrix.platform.id }}) + runs-on: ${{ matrix.platform.runner }} + strategy: + fail-fast: false + matrix: + platform: + # manylinux_2_28 x86_64 (cp310-cp313) + - id: manylinux_2_28-x86_64 + runner: ubuntu-22.04 + target: x86_64-unknown-linux-gnu + manylinux: "2_28" + maturin_args: >- + --release --manifest-path booster_sdk_py/Cargo.toml --out dist --compatibility pypi + -i python3.10 -i python3.11 -i python3.12 -i python3.13 + + # manylinux_2_28 aarch64 (cp310-cp313) + - id: manylinux_2_28-aarch64 + runner: ubuntu-22.04 + target: aarch64-unknown-linux-gnu + manylinux: "2_28" + maturin_args: >- + --release --manifest-path booster_sdk_py/Cargo.toml --out dist --compatibility pypi + -i python3.10 -i python3.11 -i python3.12 -i python3.13 + + # macOS arm64 (cp310) + - id: macos-arm64-cp310 + runner: macos-14 + target: aarch64-apple-darwin + python: "3.10" + manylinux: "" + maturin_args: --release --manifest-path booster_sdk_py/Cargo.toml --out dist --compatibility pypi -i python + + # macOS arm64 (cp311) + - id: macos-arm64-cp311 + runner: macos-14 + target: aarch64-apple-darwin + python: "3.11" + manylinux: "" + maturin_args: --release --manifest-path booster_sdk_py/Cargo.toml --out dist --compatibility pypi -i python + + # macOS arm64 (cp312) + - id: macos-arm64-cp312 + runner: macos-14 + target: aarch64-apple-darwin + python: "3.12" + manylinux: "" + maturin_args: --release --manifest-path booster_sdk_py/Cargo.toml --out dist --compatibility pypi -i python + + # macOS arm64 (cp313) + - id: macos-arm64-cp313 + runner: macos-14 + target: aarch64-apple-darwin + python: "3.13" + manylinux: "" + maturin_args: --release --manifest-path booster_sdk_py/Cargo.toml --out dist --compatibility pypi -i python + + steps: + - uses: actions/checkout@v4 + + # Only needed for macOS builds (Linux manylinux builds run inside the container and + # actions/setup-python won't affect those). + - name: Setup Python (macOS only) + if: startsWith(matrix.platform.runner, 'macos') + uses: actions/setup-python@v5 + with: + python-version: ${{ matrix.platform.python }} + + - name: Build wheels + uses: PyO3/maturin-action@v1 + with: + command: build + target: ${{ matrix.platform.target }} + manylinux: ${{ matrix.platform.manylinux }} + args: ${{ matrix.platform.maturin_args }} + + - name: Upload wheels + uses: actions/upload-artifact@v4 + with: + name: wheels-${{ matrix.platform.id }} + path: dist/*.whl + + publish-crate: + runs-on: ubuntu-22.04 + environment: release + needs: build + steps: + - uses: actions/checkout@v4 + - uses: prefix-dev/setup-pixi@v0.9.1 + with: + pixi-version: v0.63.2 + - name: Publish to crates.io + run: pixi run cargo publish -p booster_sdk + env: + CARGO_REGISTRY_TOKEN: ${{ secrets.CARGO_REGISTRY_TOKEN }} + + publish-pypi: + runs-on: ubuntu-22.04 + needs: build + environment: release + permissions: + id-token: write + steps: + - uses: actions/download-artifact@v4 + with: + pattern: wheels-* + merge-multiple: true + path: dist/ + + - name: Install uv + uses: astral-sh/setup-uv@v5 + + - name: Publish to PyPI + run: uv publish dist/* From 789d1b3220912dd1ac1b1bee25e7d2b0d07b4ac1 Mon Sep 17 00:00:00 2001 From: Gijs de Jong Date: Sun, 22 Feb 2026 21:28:16 +0100 Subject: [PATCH 15/15] remove redundant python fn --- booster_sdk/src/client/light_control.rs | 4 +--- booster_sdk_py/booster_sdk_bindings/booster_sdk_bindings.pyi | 1 - booster_sdk_py/src/client/light_control.rs | 4 ---- 3 files changed, 1 insertion(+), 8 deletions(-) diff --git a/booster_sdk/src/client/light_control.rs b/booster_sdk/src/client/light_control.rs index 5c77364..eebe612 100644 --- a/booster_sdk/src/client/light_control.rs +++ b/booster_sdk/src/client/light_control.rs @@ -57,9 +57,7 @@ impl LightControlClient { /// Set LED light color from RGB values. pub async fn set_led_light_color(&self, r: u8, g: u8, b: u8) -> Result<()> { - let param = SetLedLightColorParameter { r, g, b }; - self.rpc - .call_serialized(LightApiId::SetLedLightColor, ¶m) + self.set_led_light_color_param(&SetLedLightColorParameter { r, g, b }) .await } diff --git a/booster_sdk_py/booster_sdk_bindings/booster_sdk_bindings.pyi b/booster_sdk_py/booster_sdk_bindings/booster_sdk_bindings.pyi index 321b6c5..db202c9 100644 --- a/booster_sdk_py/booster_sdk_bindings/booster_sdk_bindings.pyi +++ b/booster_sdk_py/booster_sdk_bindings/booster_sdk_bindings.pyi @@ -555,7 +555,6 @@ class LuiClient: class LightControlClient: def __init__(self) -> None: ... def set_led_light_color(self, r: int, g: int, b: int) -> None: ... - def set_led_light_color_param(self, r: int, g: int, b: int) -> None: ... def stop_led_light_control(self) -> None: ... class VisionClient: diff --git a/booster_sdk_py/src/client/light_control.rs b/booster_sdk_py/src/client/light_control.rs index 34b7fe0..5517a3e 100644 --- a/booster_sdk_py/src/client/light_control.rs +++ b/booster_sdk_py/src/client/light_control.rs @@ -29,10 +29,6 @@ impl PyLightControlClient { .map_err(to_py_err) } - fn set_led_light_color_param(&self, py: Python<'_>, r: u8, g: u8, b: u8) -> PyResult<()> { - self.set_led_light_color(py, r, g, b) - } - fn stop_led_light_control(&self, py: Python<'_>) -> PyResult<()> { let client = Arc::clone(&self.client); wait_for_future(py, async move { client.stop_led_light_control().await }).map_err(to_py_err)