kuavo-humanoid-sdk 0.1.1__py3-none-any.whl → 1.1.2a692__py3-none-any.whl

kuavo_humanoid_sdk/kuavo/core/ros_env.py

@@ -52,37 +52,54 @@ class KuavoROSEnv:
         Raises:
             Exception: If the modelPath parameter is not found or the modelPath parameter is not a valid path.
         """
+        # if generate docs, skip init.
+        if 'GEN_KUAVO_HUMANOID_SDK_DOCS' in os.environ:
+            return True
+
         # Return directly if already initialized successfully
         if self._init_success:
             return True
-
+        
         # Check if ROS master is running
         try:
             rospy.get_master().getPid()
         except Exception as e:
-            raise Exception(f"ROS master is not running. Please start roscore first or check ROS_MASTER_URI, {e}")
-        
+            print(f"\033[31m\nError:Can't connect to ros master, Please start roscore first or check ROS_MASTER_URI.\nException:{e}\033[0m"
+                  "\nMaybe manually launch the app first?"
+                  "\n - for example(sim): roslaunch humanoid_controller load_kuavo_mujoco_sim.launch, "
+                  "\n - for example(real): roslaunch humanoid_controller load_kuavo_real.launch\n")
+            exit(1)
+
+        """
+        # NOTE: We add kuavo_msgs/motion_capture_ik package in SDK integration.
         kuavo_ws_root = self._get_kuavo_ws_root()
-        
         # Add kuavo_msgs package to Python path
         dist_path = os.path.join(kuavo_ws_root, 'devel/lib/python3/dist-packages')
         if not os.path.exists(dist_path):
             dist_path = os.path.join(kuavo_ws_root, 'install/lib/python3/dist-packages')
             if not os.path.exists(dist_path):
                 raise Exception(f"{dist_path} package not found in Python path")
-        
         if dist_path not in os.sys.path:
             os.sys.path.append(dist_path)
-
+        """
+        
         # Initialize the ROS node
         if not rospy.get_node_uri():
             rospy.init_node(f'kuavo_sdk_node', anonymous=True, disable_signals=True)
         
-        self.launch_ik_node()
-        self.launch_gait_switch_node()
-        
+        # Only check nodes exist when Init SDK, if not, tips user manually launch nodes.
+        # self.launch_ik_node()
+        # self.launch_gait_switch_node()
+        deps_nodes = ['/humanoid_gait_switch_by_name']
+        for node in deps_nodes:
+            if not KuavoROSEnv.check_rosnode_exists(node):
+                print(f"\033[31m\nError:Node {node} not found. Please launch it manuallly.\033[0m")
+                exit(1)
+                
         self._init_success = True  # Set flag after successful initialization
+
         return True
+    
     def _launch_ros_node(self, node_name, launch_cmd, log_name):
         """Launch a ROS node with the given command and log the output.
         
@@ -193,4 +210,25 @@ class KuavoROSEnv:
                 self._launch_ros_node('Gait switch node', full_cmd, 'launch_gait_switch')
 
         except Exception as e:
-            raise Exception(f"Failed to launch gait_switch node: {e}")
+            raise Exception(f"Failed to launch gait_switch node: {e}")
+        
+    
+    @staticmethod
+    def check_rosnode_exists(node_name):
+        """Check if a ROS node is running.
+        
+        Args:
+            node_name (str): Name of the node to check
+            
+        Returns:
+            bool: True if node is running, False otherwise
+        """
+        try:
+            nodes = subprocess.check_output(['rosnode', 'list']).decode('utf-8').split('\n')
+            return node_name in nodes
+        except subprocess.CalledProcessError as e:
+            SDKLogger.error(f"Error checking if node {node_name} exists: {e}")
+            return False
+        except Exception as e:
+            SDKLogger.error(f"Error checking if node {node_name} exists: {e}")
+            return False

kuavo_humanoid_sdk/kuavo/dexterous_hand.py

@@ -2,7 +2,7 @@
 # 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
+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
 
@@ -29,8 +29,8 @@ class DexterousHand(EndEffector):
         Note:
             target_velocities and target_torques are not supported.
         """
-        if len(target_positions) != len(self.joint_names):
-            raise ValueError(f"Target positions must have the same length as joint names {len(target_positions)} != {len(self.joint_names)}")
+        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]
 
@@ -54,8 +54,8 @@ class DexterousHand(EndEffector):
         Note:
             target_velocities and target_torques are not supported.
         """
-        if len(target_positions) != (len(self.joint_names)/2):
-                raise ValueError(f"Target positions must have the same length as joint names {len(target_positions)} != {len(self.joint_names)/2}.")
+        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]
 
@@ -78,8 +78,8 @@ class DexterousHand(EndEffector):
         Note:
             target_velocities and target_torques are not supported.
         """
-        if len(target_positions) != (len(self.joint_names)/2):
-            raise ValueError(f"Target positions must have the same length as joint names {len(target_positions)} != {len(self.joint_names)/2}.")
+        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]
 
@@ -95,7 +95,7 @@ class DexterousHand(EndEffector):
         Returns:
             bool: True if open command sent successfully, False otherwise.
         """
-        zero_pos = [0]*len(self.joint_names)
+        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:
@@ -119,10 +119,11 @@ class DexterousHand(EndEffector):
         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)
-
+            self.dex_hand_control.make_gestures(gesture)
+        return True
     def get_gesture_names(self)->list:
         """Get the names of all gestures.
 
@@ -134,7 +135,6 @@ class DexterousHand(EndEffector):
         return self.dex_hand_control.get_gesture_names()
     
     def get_state(self)->Tuple[EndEffectorState, EndEffectorState]:
-        # TODO(kuavo): Not implemented yet
         """Get the state of the dexterous hand.
 
         Returns:
@@ -142,42 +142,57 @@ class DexterousHand(EndEffector):
         """
         return self._rb_state.eef_state
 
-    def get_position(self)->Tuple[float, float]:
-        # TODO(kuavo): Not implemented yet
+    def get_position(self)->Tuple[list, list]:
         """Get the position of the dexterous hand.
 
         Returns:
-            Tuple[float, float]: The position of the dexterous hand.
+            Tuple[list, list]: The position of the dexterous hand.
         """
-        return self._rb_state.eef_state.position
+        state = self._rb_state.eef_state
+        return (state[0].position, state[1].position)
     
-    def get_velocity(self)->Tuple[float, float]:
-        # TODO(kuavo): Not implemented yet
+    def get_velocity(self)->Tuple[list, list]:
         """Get the velocity of the dexterous hand.
 
         Returns:
-            Tuple[float, float]: The velocity of the dexterous hand.
+            Tuple[list, list]: The velocity of the dexterous hand.
         """
-        return self._rb_state.eef_state.velocity
+        state = self._rb_state.eef_state
+        return (state[0].velocity, state[1].velocity)
 
-    def get_effort(self)->Tuple[float, float]:
-        # TODO(kuavo): Not implemented yet
+    def get_effort(self)->Tuple[list, list]:
         """Get the effort of the dexterous hand.
 
         Returns:
-            Tuple[float, float]: The effort of the dexterous hand.
+            Tuple[list, list]: The effort of the dexterous hand.
 
         Note:
             0 ~ 100 for each finger. Fraction of max motor current, absolute number.
             The max motor current is 600mA, in a word, 100.
         """
-        return self._rb_state.eef_state.effort
+        state = self._rb_state.eef_state
+        return (state[0].effort, state[1].effort)
 
     def get_grasping_state(self)->Tuple[EndEffectorState.GraspingState, EndEffectorState.GraspingState]:
-        # TODO(kuavo): Not implemented yet
         """Get the grasping state of the dexterous hand.
 
+        Note:
+            The grasping state is not implemented yet.
+
         Returns:
             Tuple[EndEffectorState.GraspingState, EndEffectorState.GraspingState]: The grasping state of the dexterous hand.
         """
-        return self._rb_state.eef_state.state
+        raise NotImplementedError("This function is not implemented yet")
+
+
+class TouchDexterousHand(DexterousHand):
+    def __init__(self):
+        super().__init__()
+
+    def get_touch_state(self)-> Tuple[KuavoDexHandTouchState, KuavoDexHandTouchState]:
+        """Get the touch state of the dexterous hand.
+
+        Returns:
+            Tuple[KuavoDexHandTouchState, KuavoDexHandTouchState]
+        """
+        return self._rb_state.dexhand_touch_state

kuavo_humanoid_sdk/kuavo/leju_claw.py

@@ -22,7 +22,7 @@ class LejuClaw(EndEffector):
             target_torques (list, optional): The target torques of the claws. If None, default value [1.0, 1.0] will be used.
 
         Note:
-            The target_positions, target_velocities,  target_torques must be a list of length 2.
+            The target_positions, target_velocities,  target_torques must be a list of length `self.joint_count()`.
             After calling this function, you can call wait_for_finish() to wait until the claws reach the target position.
 
         Warning:
@@ -31,18 +31,22 @@ class LejuClaw(EndEffector):
         Returns:
             bool: True if the claws are successfully gripped, False otherwise
         """
-        if target_positions is None:
+        if target_positions is None or len(target_positions) != self.joint_count():
             raise ValueError("Target positions must be provided.")
         
         target_positions = [max(0.0, min(100.0, pos)) for pos in target_positions]
         if target_velocities is None:
             target_velocities = [90, 90]
         else:
+           if len(target_velocities) != self.joint_count():
+               raise ValueError("Target velocities must be a list of length 2.")
            target_velocities = [max(0.0, min(100.0, vel)) for vel in target_velocities]
         
         if target_torques is None:
             target_torques = [1.0, 1.0]
         else:
+            if len(target_torques) != self.joint_count():
+                raise ValueError("Target torques must be a list of length 2.")
             target_torques = [max(0.0, min(10.0, torque)) for torque in target_torques]
             
         return self.leju_claw_control.control(target_positions, target_velocities, target_torques, EndEffectorSide.BOTH)
@@ -56,7 +60,7 @@ class LejuClaw(EndEffector):
             target_torques (list, optional): The target torque of the left claw. If None, default value 1.0 will be used.
 
         Note:
-            The target_positions, target_velocities, target_torques must be a list of length 1
+            The target_positions, target_velocities, target_torques must be a list of length `self.joint_count()/2`.
             After calling this function, you can call wait_for_finish() to wait until the claws reach the target position.
 
         Warning:
@@ -69,8 +73,8 @@ class LejuClaw(EndEffector):
             raise ValueError("Target positions must be provided.")
         
         for data in [target_positions, target_velocities, target_torques]:
-            if data is not None and len(data) != 1:
-                raise ValueError("Target positions must be a list of length 1.")
+            if data is not None and len(data) != self.joint_count()/2:
+                raise ValueError(f"Target data must be a list of length {self.joint_count()/2}.")
         
         q = max(0.0, min(100.0, target_positions[0]))
 
@@ -98,7 +102,7 @@ class LejuClaw(EndEffector):
             bool: True if the claw is successfully gripped, False otherwise.
         
         Note:
-            The target_positions, target_velocities, target_torques must be a list of length 1
+            The target_positions, target_velocities, target_torques must be a list of length `self.joint_count()/2`.
             After calling this function, you can call wait_for_finish() to wait until the claws reach the target position.
 
         Warning:
@@ -108,8 +112,8 @@ class LejuClaw(EndEffector):
             raise ValueError("Target positions must be provided.")
         
         for data in [target_positions, target_velocities, target_torques]:
-            if data is not None and len(data) != 1:
-                raise ValueError("Target positions must be a list of length 1.")
+            if data is not None and len(data) != self.joint_count()/2:
+                raise ValueError(f"Target data must be a list of length {self.joint_count()/2}.")
         
         q = max(0.0, min(100.0, target_positions[0]))
 
@@ -163,29 +167,32 @@ class LejuClaw(EndEffector):
         """
         return self._rb_state.eef_state.state
     
-    def get_position(self)->Tuple[float, float]:
+    def get_position(self)->Tuple[list, list]:
         """Get the position of the claws.
 
         Returns:
-            Tuple[float, float]: The position of the claws, range [0.0, 100.0].
+            Tuple[list, list]: The position of the claws, range [0.0, 100.0].
         """
-        return self._rb_state.eef_state.position
+        claw_state = self._rb_state.eef_state
+        return (claw_state[0].position, claw_state[1].position)
     
-    def get_velocity(self)->Tuple[float, float]:
+    def get_velocity(self)->Tuple[list, list]:
         """Get the velocity of the claws.
 
         Returns:
-            Tuple[float, float]: The velocity of the claws.
+            Tuple[list, list]: The velocity of the claws.
         """
-        return self._rb_state.eef_state.velocity
+        claw_state = self._rb_state.eef_state
+        return (claw_state[0].velocity, claw_state[1].velocity)
     
-    def get_effort(self)->Tuple[float, float]:
+    def get_effort(self)->Tuple[list, list]:
         """Get the effort of the claws.
 
         Returns:
-            Tuple[float, float]: The effort of the claws.
+            Tuple[list, list]: The effort of the claws.
         """
-        return self._rb_state.eef_state.effort
+        claw_state = self._rb_state.eef_state
+        return (claw_state[0].effort, claw_state[1].effort)
 
     def get_state(self)->Tuple[EndEffectorState, EndEffectorState]:
         """Get the state of the claws.

kuavo_humanoid_sdk/kuavo/robot.py

@@ -5,7 +5,6 @@ kuavo_ros_env = KuavoROSEnv()
 if not kuavo_ros_env.Init():
     raise RuntimeError("Failed to initialize ROS environment")
 else:
-    import time
     from typing import Tuple
     from kuavo_humanoid_sdk.kuavo.robot_info import KuavoRobotInfo
     from kuavo_humanoid_sdk.kuavo.robot_arm import KuavoRobotArm 
@@ -14,6 +13,7 @@ else:
     from kuavo_humanoid_sdk.interfaces.robot import RobotBase
     from kuavo_humanoid_sdk.interfaces.data_types import KuavoPose, KuavoIKParams
     from kuavo_humanoid_sdk.common.logger import SDKLogger, disable_sdk_logging
+
 """
 Kuavo SDK - Python Interface for Kuavo Robot Control
 
@@ -35,12 +35,17 @@ The module requires a properly configured ROS environment to function.
 """
 __all__ = ["KuavoSDK", "KuavoRobot"]
 
+
 class KuavoSDK:
+    class Options:
+        Normal = 0x01
+        WithIK = 0x02
+
     def __init__(self):
        pass
 
     @staticmethod   
-    def Init(log_level: str = "INFO")-> bool:
+    def Init(options:int=Options.Normal, log_level: str = "INFO")-> bool:
         """Initialize the SDK.
         
         Args:
@@ -59,7 +64,14 @@ class KuavoSDK:
         if log_level.upper() == 'DEBUG':
             debug = True
         else:
-            debug = False    
+            debug = False   
+
+        # Check if IK option is enabled
+        if options & KuavoSDK.Options.WithIK:
+            if not KuavoROSEnv.check_rosnode_exists('/arms_ik_node'):
+                print("\033[31m\nError:WithIK option is enabled but ik_node is not running, please run `roslaunch motion_capture_ik ik_node.launch`\033[0m")      
+                exit(1)
+
         if not kuavo_core.initialize(debug=debug):
             SDKLogger.error("[SDK] Failed to initialize core components.")
             return False
@@ -86,7 +98,7 @@ class KuavoRobot(RobotBase):
             bool: True if the robot is put into stance mode successfully, False otherwise.
             
         Note:
-            You can call KuavoRobotState.wait_for_stance() to wait until the robot enters stance mode.
+            You can call :meth:`KuavoRobotState.wait_for_stance` to wait until the robot enters stance mode.
         """
         return self._kuavo_core.to_stance()
         
@@ -97,7 +109,7 @@ class KuavoRobot(RobotBase):
             bool: True if the robot is put into trot mode successfully, False otherwise.
             
         Note:
-            You can call KuavoRobotState.wait_for_walk() to wait until the robot enters trot mode.
+            You can call :meth:`KuavoRobotState.wait_for_walk` to wait until the robot enters trot mode.
         """
         return self._kuavo_core.to_trot()
 
@@ -113,7 +125,7 @@ class KuavoRobot(RobotBase):
             bool: True if the motion is controlled successfully, False otherwise.
             
         Note:
-            You can call KuavoRobotState.wait_for_walk() to wait until the robot enters walk mode.
+            You can call :meth:`KuavoRobotState.wait_for_walk` to wait until the robot enters walk mode.
         """
         # Limit velocity ranges
         limited_linear_x = min(0.4, max(-0.4, linear_x))
@@ -136,24 +148,29 @@ class KuavoRobot(RobotBase):
     def squat(self, height: float, pitch: float=0.0)->bool:
         """Control the robot's squat height and pitch.
         Args:
-                height (float): The height offset from normal standing height in meters, range [-0.3, 0.0],Negative values indicate squatting down.
+                height (float): The height offset from normal standing height in meters, range [-0.35, 0.0],Negative values indicate squatting down.
                 pitch (float): The pitch angle of the robot's torso in radians, range [-0.4, 0.4].
             
         Returns:
             bool: True if the squat is controlled successfully, False otherwise.
         """
         # Limit height range
-        limited_height = min(0.0, max(-0.3, height))
+        MAX_HEIGHT = 0.0
+        MIN_HEIGHT = -0.35
+        MAX_PITCH = 0.4
+        MIN_PITCH = -0.4
+        
+        limited_height = min(MAX_HEIGHT, max(MIN_HEIGHT, height))
         
         # Check if height exceeds limits
-        if height > 0.0 or height < -0.3:
-            SDKLogger.warn(f"[Robot] height {height} exceeds limit [-0.3, 0.0], will be limited")
+        if height > MAX_HEIGHT or height < MIN_HEIGHT:
+            SDKLogger.warn(f"[Robot] height {height} exceeds limit [{MIN_HEIGHT}, {MAX_HEIGHT}], will be limited")
         # Limit pitch range
-        limited_pitch = min(0.4, max(-0.4, pitch))
+        limited_pitch = min(MAX_PITCH, max(MIN_PITCH, pitch))
         
         # Check if pitch exceeds limits
-        if abs(pitch) > 0.4:
-            SDKLogger.warn(f"[Robot] pitch {pitch} exceeds limit [-0.4, 0.4], will be limited")
+        if abs(pitch) > MAX_PITCH:
+            SDKLogger.warn(f"[Robot] pitch {pitch} exceeds limit [{MIN_PITCH}, {MAX_PITCH}], will be limited")
         
         return self._kuavo_core.squat(limited_height, limited_pitch)
      
@@ -172,7 +189,11 @@ class KuavoRobot(RobotBase):
         Raises:
             RuntimeError: If the robot is not in stance state when trying to control step motion.
             ValueError: If target_pose length is not 4.
-        """
+
+        Note:
+            You can call :meth:`KuavoRobotState.wait_for_step_control` to wait until the robot enters step-control mode.
+            You can call :meth:`KuavoRobotState.wait_for_stance` to wait the step-control finish.
+        """    
         if len(target_pose) != 4:
             raise ValueError(f"[Robot] target_pose length must be 4 (x, y, z, yaw), but got {len(target_pose)}")
 
@@ -235,7 +256,7 @@ class KuavoRobot(RobotBase):
             ValueError: If the joint position is outside the range of [-π, π].
             RuntimeError: If the robot is not in stance state when trying to control the arm.
             
-        Note:
+        Warning:
             This is an asynchronous interface. The function returns immediately after sending the command.
             Users need to wait for the motion to complete on their own.
         """
@@ -293,6 +314,9 @@ class KuavoRobot(RobotBase):
                 
         Returns:
             list: List of joint positions in radians, or None if inverse kinematics failed.
+
+        Warning:
+            This function requires initializing the SDK with the :attr:`KuavoSDK.Options.WithIK`.
         """
         return self._robot_arm.arm_ik(left_pose, right_pose, left_elbow_pos_xyz, right_elbow_pos_xyz, arm_q0, params)
 
@@ -305,5 +329,8 @@ class KuavoRobot(RobotBase):
         Returns:
             Tuple[KuavoPose, KuavoPose]: Tuple of poses for the robot left arm and right arm,
                 or (None, None) if forward kinematics failed.
+
+        Warning:
+            This function requires initializing the SDK with the :attr:`KuavoSDK.Options.WithIK`.        
         """
         return self._robot_arm.arm_fk(q)

kuavo_humanoid_sdk/kuavo/robot_arm.py

@@ -118,6 +118,9 @@ class KuavoRobotArm:
                 
         Returns:
             list: List of joint positions in radians, or None if inverse kinematics failed.
+
+        Warning:
+            This function requires initializing the SDK with the :attr:`KuavoSDK.Options.WithIK`.        
         """
         return self._kuavo_core.arm_ik(left_pose, right_pose, left_elbow_pos_xyz, right_elbow_pos_xyz, arm_q0, params)
 
@@ -130,6 +133,9 @@ class KuavoRobotArm:
         Returns:
             Tuple[KuavoPose, KuavoPose]: Tuple of poses for the robot left arm and right arm,
                 or (None, None) if forward kinematics failed.
+        
+        Warning:
+            This function requires initializing the SDK with the :attr:`KuavoSDK.Options.WithIK`.        
         """
         if len(q) != self._robot_info.arm_joint_dof:
             raise ValueError("Invalid position length. Expected {}, got {}".format(self._robot_info.arm_joint_dof, len(q)))

kuavo_humanoid_sdk/kuavo/robot_info.py

@@ -37,6 +37,7 @@ class KuavoRobotInfo(RobotInfoBase):
             str: The end effector type, where:
                 - "qiangnao" means "dexteroushand"
                 - "lejuclaw" means "lejuclaw"
+                - "qiangnao_touch" means "touchdexteroushand"
                 - ...
         """
         return self._end_effector_type