kuavo-humanoid-sdk 1.2.1b3269__20250911204115-py3-none-any.whl

kuavo_humanoid_sdk/kuavo/demo_climbstair.py

@@ -0,0 +1,249 @@
+#!/usr/bin/env python3
+# coding: utf-8
+
+import argparse
+import sys
+import rospy
+from robot_climbstair import KuavoRobotClimbStair
+
+
+def parse_args():
+    """Parse command line arguments."""
+    parser = argparse.ArgumentParser(
+        description="Stair climbing demo using KuavoRobotClimbStair"
+    )
+    parser.add_argument(
+        "--plot", action="store_true", help="Enable trajectory plotting"
+    )
+    parser.add_argument(
+        "--initH", type=float, default=0.0, help="Stand height offset (default: 0.0)"
+    )
+    return parser.parse_args()
+
+
+def wait_for_user_confirmation(message: str) -> bool:
+    """Wait for user confirmation before proceeding."""
+    try:
+        response = input(f"{message} (y/n): ").lower().strip()
+        return response in ["y", "yes"]
+    except KeyboardInterrupt:
+        print("\nDemo interrupted by user")
+        return False
+
+
+def main():
+    """Main demo function replicating original stairClimbPlanner.py sequence."""
+    try:
+        # Parse command line arguments
+        args = parse_args()
+        plot_enabled = args.plot
+        stand_height = args.initH
+
+        rospy.loginfo(
+            f"[Demo] Starting stair climbing demo with plot={plot_enabled}, stand_height={stand_height}"
+        )
+
+        # Initialize robot stair climbing system
+        climb_stair = KuavoRobotClimbStair()
+
+        # Set stair parameters matching the original script
+        success = climb_stair.set_stair_parameters(
+            step_height=0.13,  # 台阶高度
+            step_length=0.28,  # 台阶长度
+            foot_width=0.10,  # 宽
+            stand_height=stand_height,
+            dt=0.6,  # 步态周期
+            ss_time=0.5,  # 支撑相时间
+        )
+
+        if not success:
+            rospy.logerr("[Demo] Failed to set stair parameters")
+            return False
+
+        # Display current parameters
+        params = climb_stair.get_parameters()
+        rospy.loginfo(
+            f"[Demo] Current parameters: step_height={params['step_height']:.3f}m, "
+            f"step_length={params['step_length']:.3f}m, dt={params['dt']:.3f}s"
+        )
+
+        # Ask user confirmation before starting
+        if not wait_for_user_confirmation("Start stair climbing demo sequence?"):
+            rospy.loginfo("[Demo] Demo cancelled by user")
+            return False
+
+        # Use the new API: plan all trajectories, then publish them
+        rospy.loginfo("[Demo] Planning complete demo sequence...")
+
+        # Disable pitch limit for stair climbing
+        from robot_climbstair import set_pitch_limit
+
+        set_pitch_limit(False)
+
+        # Clear any existing trajectory
+        climb_stair.clear_trajectory()
+
+        # Phase 1: Plan up stairs
+        success = climb_stair.climb_up_stairs(5)
+        if not success:
+            rospy.logerr("[Demo] Failed to plan up stairs")
+            set_pitch_limit(True)
+            return False
+        rospy.loginfo("[Demo] Up stairs plan done.")
+
+        # Phase 2: Plan move forward
+        success = climb_stair.move_to_position(0.35, 0, 0)
+        if not success:
+            rospy.logerr("[Demo] Failed to plan move forward")
+            set_pitch_limit(True)
+            return False
+        rospy.loginfo("[Demo] Move forward plan done.")
+
+        # Phase 3: Plan down stairs (TEMPORARILY DISABLED)
+        rospy.loginfo(
+            "[Demo] Skipping down stairs phase (functionality under development)"
+        )
+        # success = climb_stair.climb_down_stairs(5)
+        # if not success:
+        #     rospy.logerr("[Demo] Failed to plan down stairs")
+        #     set_pitch_limit(True)
+        #     return False
+        rospy.loginfo("[Demo] Down stairs phase skipped.")
+
+        # Execute the complete accumulated trajectory
+        rospy.loginfo("[Demo] Executing complete trajectory sequence...")
+        success = climb_stair.execute_trajectory()
+
+        # Re-enable pitch limit
+        set_pitch_limit(True)
+
+        if not success:
+            rospy.logerr("[Demo] Failed to execute demo sequence")
+            return False
+
+        rospy.loginfo("[Demo] ✓ Complete demo sequence executed successfully!")
+
+        # Print final statistics
+        total_steps = climb_stair.get_step_count()
+        rospy.loginfo(
+            f"[Demo] Demo completed successfully! Total steps taken: {total_steps}"
+        )
+        print(f"\n=== DEMO COMPLETED ===")
+        print(f"Total steps taken: {total_steps}")
+        print("All phases completed successfully!")
+
+        return True
+
+    except KeyboardInterrupt:
+        rospy.logwarn("[Demo] Demo interrupted by user (Ctrl+C)")
+        return False
+    except Exception as e:
+        rospy.logerr(f"[Demo] Demo failed with exception: {e}")
+        return False
+
+
+def run_detailed_demo():
+    """Run demo with detailed trajectory logging (similar to original script)."""
+    try:
+        args = parse_args()
+        stand_height = args.initH
+
+        rospy.loginfo("[Demo] Starting detailed stair climbing demo")
+
+        # Initialize robot
+        climb_stair = KuavoRobotClimbStair()
+
+        # Set parameters
+        climb_stair.set_stair_parameters(
+            step_height=0.13,
+            step_length=0.28,
+            foot_width=0.10,
+            stand_height=stand_height,
+        )
+
+        print("\n=== DETAILED STAIR CLIMBING DEMO ===")
+        print("This demo replicates the exact sequence from stairClimbPlanner.py:")
+        print("1. Up stairs (5 steps)")
+        print("2. Move forward (0.35m)")
+        print("3. Down stairs (5 steps)")
+        print()
+
+        # Disable pitch limit (matching original behavior)
+        climb_stair.set_pitch_limit(False)
+
+        print("=== EXECUTING COMPLETE TRAJECTORY SEQUENCE ===")
+
+        # Use the new API: plan all trajectories, then publish them
+        climb_stair.clear_trajectory()
+
+        # Phase 1: Plan up stairs
+        success = climb_stair.climb_up_stairs(5)
+        if not success:
+            print("✗ Failed to plan up stairs")
+            return False
+        print("✓ Up stairs plan done.")
+
+        # Phase 2: Plan move forward
+        success = climb_stair.move_to_position(0.35, 0, 0)
+        if not success:
+            print("✗ Failed to plan move forward")
+            return False
+        print("✓ Move forward plan done.")
+
+        # Phase 3: Plan down stairs (TEMPORARILY DISABLED)
+        print("⚠ Skipping down stairs phase (functionality under development)")
+        # success = climb_stair.climb_down_stairs(5)
+        # if not success:
+        #     print("✗ Failed to plan down stairs")
+        #     return False
+        print("✓ Down stairs phase skipped.")
+
+        # Execute the complete accumulated trajectory
+        print("Executing complete trajectory sequence...")
+        success = climb_stair.execute_trajectory()
+        if success:
+            print("✓ Complete trajectory sequence executed successfully")
+            print(f"Total step count: {climb_stair.get_step_count()}")
+        else:
+            print("✗ Complete trajectory sequence failed")
+            return False
+
+        # Re-enable pitch limit (safety)
+        climb_stair.set_pitch_limit(True)
+
+        total_steps = climb_stair.get_step_count()
+        print(f"\n=== DEMO COMPLETED ===")
+        print(f"Total steps taken: {total_steps}")
+        print("All phases completed successfully!")
+        print("\nTrajectory published to: /humanoid_mpc_foot_pose_target_trajectories")
+
+        return True
+
+    except Exception as e:
+        # Ensure pitch limit is restored on error
+        try:
+            climb_stair.set_pitch_limit(True)
+        except:
+            pass
+        rospy.logerr(f"[Demo] Detailed demo failed: {e}")
+        return False
+
+
+if __name__ == "__main__":
+    try:
+        rospy.init_node("stair_climbing_demo", anonymous=True)
+        success = main()
+
+        if success:
+            rospy.loginfo("[Demo] Demo completed successfully!")
+            sys.exit(0)
+        else:
+            rospy.logerr("[Demo] Demo failed!")
+            sys.exit(1)
+
+    except KeyboardInterrupt:
+        rospy.logwarn("[Demo] Demo interrupted by user")
+        sys.exit(130)
+    except Exception as e:
+        rospy.logerr(f"[Demo] Unexpected error: {e}")
+        sys.exit(1)

kuavo_humanoid_sdk/kuavo/dexterous_hand.py

@@ -0,0 +1,238 @@
+#!/usr/bin/env python3
+# coding: utf-8
+from typing import Tuple
+from kuavo_humanoid_sdk.interfaces.end_effector import EndEffector
+from kuavo_humanoid_sdk.interfaces.data_types import EndEffectorSide, EndEffectorState, KuavoDexHandTouchState
+from kuavo_humanoid_sdk.kuavo.core.dex_hand_control import DexHandControl
+from kuavo_humanoid_sdk.kuavo.core.ros.state import KuavoRobotStateCore
+import time
+
+class DexterousHand(EndEffector):
+    """普通灵巧手控制类"""
+    def __init__(self):
+        joint_names = ['l_thumb', 'l_thumb_aux', 'l_index', 'l_middle', 'l_ring', 'l_pinky',
+                       'r_thumb', 'r_thumb_aux', 'r_index', 'r_middle', 'r_ring', 'r_pinky',]
+        super().__init__(joint_names=joint_names)
+        self.dex_hand_control = DexHandControl()
+        self._rb_state = KuavoRobotStateCore()
+
+    def control(self, target_positions:list, target_velocities:list=None, target_torques:list=None)->bool:
+        """控制灵巧手的位置。
+
+        Args:
+            target_positions (list): 所有手指关节的目标位置列表，长度必须为12（每只手6个手指关节），范围 => [0.0 ~ 100.0] 
+            target_velocities (list, optional): 不支持。默认为None。
+            target_torques (list, optional): 不支持。默认为None。
+
+        Returns:
+            bool: 如果控制成功返回 True，否则返回 False。
+
+        Note:
+            target_velocities 和 target_torques 参数暂不支持。
+        """
+        if len(target_positions) != self.joint_count():
+            raise ValueError(f"Target positions must have the same length as joint names {len(target_positions)} != {self.joint_count()}")
+        
+        q = [max(0, min(100, pos if isinstance(pos, int) else int(pos))) for pos in target_positions]
+
+        # control the hand
+        return self.dex_hand_control.control(target_positions=q, side=EndEffectorSide.BOTH)
+
+    def control_right(self, target_positions:list, target_velocities:list=None, target_torques:list=None)->bool:
+        """控制右手灵巧手。
+
+        Args:
+            target_positions (list): 右手关节的目标位置 [0 ~ 100]，长度必须为6
+            target_velocities (list, optional): 不支持。默认为None。
+            target_torques (list, optional): 不支持。默认为None。
+
+        Returns:
+            bool: 如果控制成功返回True，否则返回False。
+
+        Raises:
+            ValueError: 如果目标位置长度与关节数不匹配或值超出[0,100]范围
+
+        Note:
+            target_velocities 和 target_torques 参数暂不支持。
+        """
+        if len(target_positions) != (self.joint_count()/2):
+                raise ValueError(f"Target positions must have the same length as joint names {len(target_positions)} != {self.joint_count()/2}.")
+        
+        q = [max(0, min(100, pos if isinstance(pos, int) else int(pos))) for pos in target_positions]
+
+        return self.dex_hand_control.control(target_positions=q, side=EndEffectorSide.RIGHT)
+
+    def control_left(self, target_positions:list, target_velocities:list=None, target_torques:list=None)->bool:
+        """控制左手灵巧手。
+
+        Args:
+            target_positions (list): 左手关节的目标位置 [0 ~ 100]，长度必须为6
+            target_velocities (list, optional): 不支持。默认为None。
+            target_torques (list, optional): 不支持。默认为None。
+
+        Returns:
+            bool: 如果控制成功返回True，否则返回False。
+
+        Raises:
+            ValueError: 如果目标位置长度与关节数不匹配或值超出[0,100]范围
+
+        Note:
+            target_velocities 和 target_torques 参数不支持。
+        """
+        if len(target_positions) != (self.joint_count()/2):
+            raise ValueError(f"Target positions must have the same length as joint names {len(target_positions)} != {self.joint_count()/2}.")
+        
+        q = [max(0, min(100, pos if isinstance(pos, int) else int(pos))) for pos in target_positions]
+
+        return self.dex_hand_control.control(target_positions=q, side=EndEffectorSide.LEFT)
+
+    def open(self, side: EndEffectorSide=EndEffectorSide.BOTH)->bool:
+        """通过将所有关节位置设置为 0 来张开灵巧手。
+
+        Args:
+            side (EndEffectorSide, optional): 要打开的手。默认为 :attr:`EndEffectorSide.BOTH`。 \n
+                可以是 :attr:`EndEffectorSide.LEFT`、:attr:`EndEffectorSide.RIGHT` 或 :attr:`EndEffectorSide.BOTH`。
+
+        Returns:
+            bool: 如果打开命令发送成功返回True，否则返回False。
+        """
+        zero_pos = [0]*self.joint_count()
+        if side == EndEffectorSide.LEFT:
+            return self.dex_hand_control.control(target_positions=zero_pos[:len(zero_pos)//2], side=EndEffectorSide.LEFT)
+        elif side == EndEffectorSide.RIGHT:
+            return self.dex_hand_control.control(target_positions=zero_pos[len(zero_pos)//2:], side=EndEffectorSide.RIGHT)
+        else:
+            return self.dex_hand_control.control(target_positions=zero_pos, side=EndEffectorSide.BOTH)     
+
+    def make_gesture(self, l_gesture_name: str, r_gesture_name: str)->bool:
+        """为双手做预定义的手势。
+
+        Args:
+            l_gesture_name (str): 左手手势的名称。None表示跳过左手。
+            r_gesture_name (str): 右手手势的名称。None表示跳过右手。
+
+        Returns:
+            bool: 如果手势命令发送成功返回True，否则返回False。
+
+        Note:
+            手势示例：'fist'、'ok'、'thumbs_up'、'666'等...
+        """
+        gesture = []
+        if l_gesture_name is not None:
+            gesture.append({'gesture_name':l_gesture_name, 'hand_side':EndEffectorSide.LEFT})
+            self.dex_hand_control.make_gestures(gesture)
+        if r_gesture_name is not None:
+            gesture.append({'gesture_name':r_gesture_name, 'hand_side':EndEffectorSide.RIGHT})    
+            self.dex_hand_control.make_gestures(gesture)
+        return True
+    def get_gesture_names(self)->list:
+        """获取所有手势的名称。
+
+        Returns:
+            list: 手势名称列表。
+                例如：['fist', 'ok', 'thumbs_up', '666', 'number_1', 'number_2', 'number_3', ... ], 如果没有手势则返回 None。
+        """
+        return self.dex_hand_control.get_gesture_names()
+    
+    def get_state(self)->Tuple[EndEffectorState, EndEffectorState]:
+        """获取灵巧手的状态。
+
+        Returns:
+            Tuple[EndEffectorState, EndEffectorState]: 灵巧手的状态。
+        """
+        return self._rb_state.eef_state
+
+    def get_position(self)->Tuple[list, list]:
+        """获取灵巧手的位置。
+
+        Returns:
+            Tuple[list, list]: 灵巧手的位置。
+        """
+        state = self._rb_state.eef_state
+        return (state[0].position, state[1].position)
+    
+    def get_velocity(self)->Tuple[list, list]:
+        """获取灵巧手的速度。
+
+        Returns:
+            Tuple[list, list]: 灵巧手的速度。
+        """
+        state = self._rb_state.eef_state
+        return (state[0].velocity, state[1].velocity)
+
+    def get_effort(self)->Tuple[list, list]:
+        """获取灵巧手的力。
+
+        Returns:
+            Tuple[list, list]: 灵巧手的力。
+
+        Note:
+            每个手指的范围为0 ~ 100。表示最大电机电流的分数，绝对数值。
+            最大电机电流为600mA，换句话说，100。
+        """
+        state = self._rb_state.eef_state
+        return (state[0].effort, state[1].effort)
+
+    def get_grasping_state(self)->Tuple[EndEffectorState.GraspingState, EndEffectorState.GraspingState]:
+        """获取灵巧手的抓取状态。
+
+        Note:
+            该功能尚未实现。
+
+        Returns:
+            Tuple[EndEffectorState.GraspingState, EndEffectorState.GraspingState]: 灵巧手的抓取状态。
+        """
+        raise NotImplementedError("This function is not implemented yet")
+
+
+class TouchDexterousHand(DexterousHand):
+    """触觉灵巧手控制类，继承自普通灵巧手控制类，可调用普通灵巧手控制类中的所有方法"""
+    def __init__(self):
+        super().__init__()
+
+    def get_touch_state(self)-> Tuple[KuavoDexHandTouchState, KuavoDexHandTouchState]:
+        """获取灵巧手的触觉状态。
+
+        Warning:
+            该功能仅在触觉灵巧手上可用。
+
+        Returns:
+            Tuple[KuavoDexHandTouchState, KuavoDexHandTouchState]
+        """
+        return self._rb_state.eef_state.state
+
+    def get_dexhand_gesture_state(self)->bool:
+        """获取机器人灵巧手势的当前状态。
+
+        Returns:
+            bool: 如果机器人灵巧手势正在执行返回True，否则返回False。
+        """
+        return self._rb_state._srv_get_dexhand_gesture_state()
+
+    def make_gesture_sync(self, l_gesture_name: str, r_gesture_name: str, timeout:float=5.0)->bool:
+        """为双手做预定义的手势（同步等待完成）。
+
+        Args:
+            l_gesture_name (str): 左手手势的名称。None表示跳过左手。
+            r_gesture_name (str): 右手手势的名称。None表示跳过右手。
+            timeout (float, optional): 手势超时时间。默认为5.0秒。
+
+        Returns:
+            bool: 如果手势执行成功返回True，否则返回False。
+        """
+        gesture = []
+        if l_gesture_name is not None:
+            gesture.append({'gesture_name':l_gesture_name, 'hand_side':EndEffectorSide.LEFT})
+        if r_gesture_name is not None:
+            gesture.append({'gesture_name':r_gesture_name, 'hand_side':EndEffectorSide.RIGHT})    
+        self.dex_hand_control.make_gestures(gesture)
+        start_time = time.time()
+        while time.time() - start_time < timeout:
+            if self.get_dexhand_gesture_state() == True:
+                break;
+            time.sleep(0.1)
+        while time.time() - start_time < timeout:
+            if self.get_dexhand_gesture_state() == False:
+                return True
+            time.sleep(0.1)
+        return False