kuavo-humanoid-sdk 1.2.1__tar.gz → 1.2.1b1675__tar.gz

{kuavo_humanoid_sdk-1.2.1 → kuavo_humanoid_sdk-1.2.1b1675}/PKG-INFO

@@ -1,6 +1,6 @@
 Metadata-Version: 2.1
 Name: kuavo_humanoid_sdk
-Version: 1.2.1
+Version: 1.2.1b1675
 Summary: A Python SDK for kuavo humanoid robot.
 Home-page: https://gitee.com/leju-robot/kuavo-ros-opensource/
 Author: ['lejurobot']

{kuavo_humanoid_sdk-1.2.1 → kuavo_humanoid_sdk-1.2.1b1675}/README.md

@@ -77,49 +77,8 @@ chmod +x ./gen_docs.sh
 ![](docs/images/image.png)
 
 ## 搬箱子案例
-**编译**：
-```
-catkin build humanoid_controllers kuavo_msgs gazebo_sim ar_control
-```
-
-**运行**：
-
-⚠️ 在运行之前, 需要确认机器人版本`ROBOT_VERSION=45`，否则会机器人末端控制会有问题
-```
-# 启动gazebo场景
-roslaunch humanoid_controllers load_kuavo_gazebo_manipulate.launch joystick_type:=bt2pro
-
-# 启动ar_tag转换码操作和virtual操作
-roslaunch ar_control robot_strategies.launch  
-
-# 运行搬箱子案例
-python3 grasp_box_example.py 
-```
-
-🚨 第一次启动gazebo场景前需要修改tag尺寸：
-
-在`/opt/ros/noetic/share/apriltag_ros/config/tags.yaml`文件中将tag的size尺寸修改为和立方体tag码的尺寸一致（只需做一次）
-```
-standalone_tags:
-    [
-        {id: 0, size: 0.088, name: 'tag_0'},
-        {id: 1, size: 0.088, name: 'tag_1'},
-        {id: 2, size: 0.088, name: 'tag_2'},
-        {id: 3, size: 0.088, name: 'tag_3'},
-        {id: 4, size: 0.088, name: 'tag_4'},
-        {id: 5, size: 0.088, name: 'tag_5'},
-        {id: 6, size: 0.088, name: 'tag_6'},
-        {id: 7, size: 0.088, name: 'tag_7'},
-        {id: 8, size: 0.088, name: 'tag_8'},
-        {id: 9, size: 0.088, name: 'tag_9'},
-    ]
-```
-🚨 每次启动gazebo场景后需要手动打光：
-
-需要在机器人腰部位置附近给个点光源，否则会找不到tag
-
-![](docs/images/gazebo.jpg)
 
+请参阅 html 或 markdown 文档中，**策略模块/搬箱子示例**章节内容，获取更多信息。
 
 **测试**
 ```

{kuavo_humanoid_sdk-1.2.1 → kuavo_humanoid_sdk-1.2.1b1675}/kuavo_humanoid_sdk/interfaces/data_types.py

@@ -129,6 +129,18 @@ class KuavoPose:
     position: Tuple[float, float, float] # x, y, z
     orientation: Tuple[float, float, float, float] # x, y, z, w
 
+    def __str__(self) -> str:
+        """Returns a formatted string representation of the pose.
+        
+        Returns:
+            str: Formatted pose string with position and orientation
+        """
+        return (
+            f"Position (x,y,z): ({self.position[0]:.3f}, {self.position[1]:.3f}, {self.position[2]:.3f})\n"
+            f"Orientation (x,y,z,w): ({self.orientation[0]:.3f}, {self.orientation[1]:.3f}, "
+            f"{self.orientation[2]:.3f}, {self.orientation[3]:.3f})"
+        )
+
 @dataclass
 class KuavoIKParams:
     """表示IK节点参数的数据类。"""

{kuavo_humanoid_sdk-1.2.1 → kuavo_humanoid_sdk-1.2.1b1675}/kuavo_humanoid_sdk/kuavo/core/core.py

@@ -410,6 +410,8 @@ class KuavoRobotCore:
         if self.state != 'stance':
             raise RuntimeError(f"[Core] control_robot_arm_joint_positions failed: robot must be in stance state, current state: {self.state}")
         
+        if self._control.is_arm_collision_mode() and self._control.is_arm_collision():
+            raise RuntimeError(f"Arm collision detected, cannot publish arm trajectory")
         # change to external control mode  
         if self._arm_ctrl_mode != KuavoArmCtrlMode.ExternalControl:
             SDKLogger.debug("[Core] control_robot_arm_joint_positions, current arm mode != ExternalControl, change it.")
@@ -421,6 +423,9 @@ class KuavoRobotCore:
     def control_robot_arm_joint_trajectory(self, times:list, joint_q:list)->bool:
         if self.state != 'stance':
             raise RuntimeError("[Core] control_robot_arm_joint_trajectory failed: robot must be in stance state")
+            
+        if self._control.is_arm_collision_mode() and self._control.is_arm_collision():
+            raise RuntimeError(f"Arm collision detected, cannot publish arm trajectory")
         
         if self._arm_ctrl_mode != KuavoArmCtrlMode.ExternalControl:
             SDKLogger.debug("[Core] control_robot_arm_joint_trajectory, current arm mode != ExternalControl, change it.")
@@ -445,6 +450,9 @@ class KuavoRobotCore:
         
         return self._control.control_robot_end_effector_pose(left_pose, right_pose, frame)
 
+    def control_hand_wrench(self, left_wrench: list, right_wrench: list) -> bool:
+        return self._control.control_hand_wrench(left_wrench, right_wrench)
+    
     def change_manipulation_mpc_frame(self, frame: KuavoManipulationMpcFrame)->bool:
         timeout = 1.0
         count = 0
@@ -506,19 +514,15 @@ class KuavoRobotCore:
         return True
     
     def change_robot_arm_ctrl_mode(self, mode:KuavoArmCtrlMode)->bool:
-        timeout = 1.0
+
+        if self._control.is_arm_collision_mode() and self.is_arm_collision():
+            SDKLogger.warn("[Core] change_robot_arm_ctrl_mode failed, arm collision detected!")
+            return False
+
         count = 0
-        while self._rb_state.arm_control_mode != mode:
+        if self._rb_state.arm_control_mode != mode:
             SDKLogger.debug(f"[Core] Change robot arm control  from {self._rb_state.arm_control_mode} to {mode}, retry: {count}")
             self._control.change_robot_arm_ctrl_mode(mode)
-            if self._rb_state.arm_control_mode == mode:
-                break
-            if timeout <= 0:
-                SDKLogger.warn("[Core] Change robot arm control mode timeout!")
-                return False
-            timeout -= 0.1
-            time.sleep(0.1)
-            count += 1
         
         if not hasattr(self, '_arm_ctrl_mode_lock'):
             self._arm_ctrl_mode_lock = threading.Lock()
@@ -576,6 +580,11 @@ class KuavoRobotCore:
     def arm_fk(self, q: list) -> Tuple[KuavoPose, KuavoPose]:
         return self._control.arm_fk(q)
     
+    """ ------------------------------------------------------------------------"""
+    """ Base Pitch Limit Control """
+    def enable_base_pitch_limit(self, enable: bool) -> Tuple[bool, str]:
+        return self._control.enable_base_pitch_limit(enable)
+    """ ------------------------------------------------------------------------"""
     """ Callbacks """
     def _humanoid_gait_changed(self, current_time: float, gait_name: str):
         if self.state != gait_name:
@@ -586,6 +595,19 @@ class KuavoRobotCore:
                 # Call the transition method if it exists
                 getattr(self, to_method)()
 
+    def is_arm_collision(self)->bool:
+        return self._control.is_arm_collision()
+    
+    def release_arm_collision_mode(self):
+
+        self._control.release_arm_collision_mode()
+        
+
+    def wait_arm_collision_complete(self):
+        self._control.wait_arm_collision_complete()
+
+    def set_arm_collision_mode(self, enable: bool):
+        self._control.set_arm_collision_mode(enable)
 
 if __name__ == "__main__":
     DEBUG_MODE = 0

{kuavo_humanoid_sdk-1.2.1 → kuavo_humanoid_sdk-1.2.1b1675}/kuavo_humanoid_sdk/kuavo/core/ros/control.py

@@ -18,9 +18,10 @@ from kuavo_humanoid_sdk.msg.kuavo_msgs.srv import (gestureExecute, gestureExecut
                         changeTorsoCtrlMode, changeTorsoCtrlModeRequest, setMmCtrlFrame, setMmCtrlFrameRequest,
                         setTagId, setTagIdRequest, getMotorParam, getMotorParamRequest,
                         changeMotorParam, changeMotorParamRequest)
-from kuavo_humanoid_sdk.msg.kuavo_msgs.msg import twoArmHandPoseCmd, ikSolveParam, armHandPose
+from kuavo_humanoid_sdk.msg.kuavo_msgs.msg import twoArmHandPoseCmd, ikSolveParam, armHandPose, armCollisionCheckInfo
 from kuavo_humanoid_sdk.msg.kuavo_msgs.srv import twoArmHandPoseCmdSrv, fkSrv
 from std_srvs.srv import SetBool, SetBoolRequest
+from std_msgs.msg import Float64MultiArray
 
 
 
@@ -187,34 +188,87 @@ class ControlEndEffector:
 
 class ControlRobotArm:
     def __init__(self):
+
+        # 带有碰撞检查的轨迹发布
+        self._pub_ctrl_arm_traj_arm_collision = rospy.Publisher('/arm_collision/kuavo_arm_traj', JointState, queue_size=10)
+        self._pub_ctrl_arm_target_poses_arm_collision = rospy.Publisher('/arm_collision/kuavo_arm_target_poses', armTargetPoses, queue_size=10)
+
+        # 判断当前是否发生碰撞
+        self._sub_arm_collision_info = rospy.Subscriber('/arm_collision/info', armCollisionCheckInfo, self.callback_arm_collision_info, queue_size=10)
+        self._is_collision = False
+        self.arm_collision_enable = False
+
+        # 正常轨迹发布
         self._pub_ctrl_arm_traj = rospy.Publisher('/kuavo_arm_traj', JointState, queue_size=10)
         self._pub_ctrl_arm_target_poses = rospy.Publisher('/kuavo_arm_target_poses', armTargetPoses, queue_size=10)
         self._pub_ctrl_hand_pose_cmd = rospy.Publisher('/mm/two_arm_hand_pose_cmd', twoArmHandPoseCmd, queue_size=10)
+        self._pub_hand_wrench = rospy.Publisher('/hand_wrench_cmd', Float64MultiArray, queue_size=10)
+
+    def is_arm_collision(self)->bool:
+        return self._is_collision
+    
+    def is_arm_collision_mode(self)->bool:
+        return self.arm_collision_enable
+    
+    def callback_arm_collision_info(self, msg: armCollisionCheckInfo):
+        self._is_collision = True
+        SDKLogger.info(f"Arm collision detected")
+
+    def set_arm_collision_mode(self, enable: bool):
+        """
+            Set arm collision mode
+        """
+        self.arm_collision_enable = enable
+        srv_set_arm_collision_mode_srv = rospy.ServiceProxy('/arm_collision/set_arm_moving_enable', SetBool)
+        req = SetBoolRequest()
+        req.data = enable
+        resp = srv_set_arm_collision_mode_srv(req)
+        if not resp.success:
+            SDKLogger.error(f"Failed to wait arm collision complete: {resp.message}")
+    
+        
+
+    def release_arm_collision_mode(self):
+        self._is_collision = False
+    
+    def wait_arm_collision_complete(self):
+        if self._is_collision:
+            srv_wait_arm_collision_complete_srv = rospy.ServiceProxy('/arm_collision/wait_complete', SetBool)
+            req = SetBoolRequest()
+            req.data = True
+            resp = srv_wait_arm_collision_complete_srv(req)
+            if not resp.success:
+                SDKLogger.error(f"Failed to wait arm collision complete: {resp.message}")
 
     def connect(self, timeout:float=1.0)-> bool:
         start_time = rospy.Time.now()
         publishers = [
-            (self._pub_ctrl_arm_traj, "arm trajectory publisher"),
-            (self._pub_ctrl_arm_target_poses, "arm target poses publisher")
+            (self._pub_ctrl_arm_traj, "arm trajectory publisher", False),
+            (self._pub_ctrl_arm_target_poses, "arm target poses publisher", False)
         ]
         
         success = True
-        for pub, name in publishers:
+        for pub, name, required in publishers:
             while pub.get_num_connections() == 0:
                 if (rospy.Time.now() - start_time).to_sec() > timeout:
                     SDKLogger.error(f"Timeout waiting for {name} connection, '{pub.name}'")
-                    success = False
+                    if required:
+                        success = False
                     break
                 rospy.sleep(0.1)
         return success
 
     def pub_control_robot_arm_traj(self, joint_q: list)->bool:
+        
         try:
             msg = JointState()
             msg.name = ["arm_joint_" + str(i) for i in range(0, 14)]
             msg.header.stamp = rospy.Time.now()
             msg.position = 180.0 / np.pi * np.array(joint_q) # convert to degree
-            self._pub_ctrl_arm_traj.publish(msg)
+            if self.arm_collision_enable:
+                self._pub_ctrl_arm_traj_arm_collision.publish(msg)
+            else:
+                self._pub_ctrl_arm_traj.publish(msg)
             return True
         except Exception as e:
             SDKLogger.error(f"publish robot arm traj: {e}")
@@ -227,7 +281,10 @@ class ControlRobotArm:
             for i in range(len(joint_q)):
                 degs = [q for q in joint_q[i]]
                 msg.values.extend(degs)
-            self._pub_ctrl_arm_target_poses.publish(msg)
+            if self.arm_collision_enable:
+                self._pub_ctrl_arm_target_poses_arm_collision.publish(msg)
+            else:
+                self._pub_ctrl_arm_target_poses.publish(msg)
             return True
         except Exception as e:
             SDKLogger.error(f"publish arm target poses: {e}")
@@ -382,8 +439,8 @@ class ControlRobotArm:
 
     def srv_change_arm_ctrl_mode(self, mode: KuavoArmCtrlMode)->bool:
         try:
-            rospy.wait_for_service('/arm_traj_change_mode', timeout=2.0)
-            change_arm_ctrl_mode_srv = rospy.ServiceProxy('/arm_traj_change_mode', changeArmCtrlMode)
+            rospy.wait_for_service('/change_arm_ctrl_mode', timeout=2.0)
+            change_arm_ctrl_mode_srv = rospy.ServiceProxy('/change_arm_ctrl_mode', changeArmCtrlMode)
             req = changeArmCtrlModeRequest()
             req.control_mode = mode.value
             resp = change_arm_ctrl_mode_srv(req)
@@ -406,6 +463,32 @@ class ControlRobotArm:
         except Exception as e:
             SDKLogger.error(f"[Error] get arm ctrl mode: {e}")
         return None
+    
+    def pub_hand_wrench_cmd(self, left_wrench, right_wrench):
+        """
+        发布末端力控制命令
+        参数:
+            left_wrench: 左手6维力控指令 [Fx, Fy, Fz, Tx, Ty, Tz]
+            right_wrench: 右手6维力控指令 [Fx, Fy, Fz, Tx, Ty, Tz]
+            Fx: 沿X轴的线性力
+            Fy: 沿Y轴的线性力
+            Fz: 沿Z轴的线性力
+            Tx: 绕X轴的力矩
+            Ty: 绕Y轴的力矩
+            Tz: 绕Z轴的力矩
+        """
+        if len(left_wrench) != 6 or len(right_wrench) != 6:
+            SDKLogger.error("Wrench data must be 6-dimensional")
+            return False
+            
+        try:
+            msg = Float64MultiArray()
+            msg.data = list(left_wrench) + list(right_wrench)
+            self._pub_hand_wrench.publish(msg)
+            return True
+        except Exception as e:
+            SDKLogger.error(f"Publish hand wrench failed: {e}")
+            return False
         
 """ Control Robot Head """
 class ControlRobotHead:
@@ -415,13 +498,15 @@ class ControlRobotHead:
     def connect(self, timeout:float=1.0)->bool:
         start_time = rospy.Time.now()
         publishers = [
-            # (self._pub_ctrl_robot_head, "robot head publisher") # not need check!
+            (self._pub_ctrl_robot_head, "robot head publisher", False) # not need check!
         ]
-        for pub, name in publishers:
+        for pub, name, required in publishers:
             while pub.get_num_connections() == 0:
                 if (rospy.Time.now() - start_time).to_sec() > timeout:
                     SDKLogger.error(f"Timeout waiting for {name} connection, '{pub.name}'")
-                    return False
+                    if required:
+                        return False
+                    break
                 rospy.sleep(0.1)
         return True
     def pub_control_robot_head(self, yaw:float, pitch:float)->bool:
@@ -629,23 +714,25 @@ class ControlRobotMotion:
         self._pub_switch_gait = rospy.Publisher('/humanoid_switch_gait_by_name', switchGaitByName, queue_size=10)
         self._pub_step_ctrl = rospy.Publisher('/humanoid_mpc_foot_pose_target_trajectories', footPoseTargetTrajectories, queue_size=10)
 
-    def connect(self, timeout:float=2.0)-> bool:
+    def connect(self, timeout:float=3.0)-> bool:
         start_time = rospy.Time.now()
         publishers = [
             # (self._pub_joy, "joy publisher"),
-            (self._pub_cmd_vel, "cmd_vel publisher"),
-            (self._pub_cmd_pose, "cmd_pose publisher"),
-            (self._pub_step_ctrl, "step_ctrl publisher"),
-            (self._pub_switch_gait, "switch_gait publisher"),
-            (self._pub_cmd_pose_world, "cmd_pose_world publisher"),
+            # pub name required
+            (self._pub_cmd_vel, "cmd_vel publisher", False),
+            (self._pub_cmd_pose, "cmd_pose publisher", False),
+            (self._pub_step_ctrl, "step_ctrl publisher", False),
+            (self._pub_switch_gait, "switch_gait publisher", False),
+            (self._pub_cmd_pose_world, "cmd_pose_world publisher", False),
         ]
         
         success = True
-        for pub, name in publishers:
+        for pub, name, required in publishers:
             while pub.get_num_connections() == 0:
                 if (rospy.Time.now() - start_time).to_sec() > timeout:
                     SDKLogger.error(f"Timeout waiting for {name} connection, '{pub.name}'")
-                    success = False
+                    if required:
+                        success = False
                     break
                 rospy.sleep(0.1)
         return success
@@ -974,6 +1061,40 @@ class KuavoRobotControl:
         # SDKLogger.debug(f"[ROS] Control robot arm trajectory: {joint_data}")
         return self.kuavo_arm_control.pub_control_robot_arm_traj(joint_data)
     
+    def is_arm_collision(self)->bool:
+        """
+            Check if arm collision is happening
+            Returns:
+                bool: True if collision is happening, False otherwise
+        """
+        return self.kuavo_arm_control.is_arm_collision()
+    
+    def is_arm_collision_mode(self)->bool:
+        """
+            Check if arm collision mode is enabled
+            Returns:
+                bool: True if collision mode is enabled, False otherwise
+        """
+        return self.kuavo_arm_control.is_arm_collision_mode()
+
+    def release_arm_collision_mode(self):
+        """
+            Release arm collision mode
+        """
+        return self.kuavo_arm_control.release_arm_collision_mode()
+    
+    def wait_arm_collision_complete(self):
+        """
+            Wait for arm collision to complete
+        """
+        return self.kuavo_arm_control.wait_arm_collision_complete()
+    
+    def set_arm_collision_mode(self, enable: bool):
+        """
+            Set arm collision mode
+        """
+        return self.kuavo_arm_control.set_arm_collision_mode(enable)
+    
     def control_robot_arm_joint_trajectory(self, times:list, joint_q:list)->bool:
         """
             Control robot arm joint trajectory
@@ -1175,11 +1296,34 @@ class KuavoRobotControl:
             SDKLogger.error(f"Service call failed: {e}")
         return False, None, 'failed'
 
+    def control_hand_wrench(self, left_wrench: list, right_wrench: list) -> bool:
+        return self.kuavo_arm_control.pub_hand_wrench_cmd(left_wrench, right_wrench)
     
-""" ------------------------------------------------------------------------------"""    
-
-
-
+    def enable_base_pitch_limit(self, enable: bool) -> Tuple[bool, str]:
+        res_msg = 'failed'
+        try:
+            service_name = '/humanoid/mpc/enable_base_pitch_limit'
+            rospy.wait_for_service(service=service_name, timeout=2.0)
+            pitch_limit_service = rospy.ServiceProxy(service_name, SetBool)
+            
+            # request
+            request = SetBoolRequest()
+            request.data = enable
+            response = pitch_limit_service(request)
+            if not response.success:
+                SDKLogger.error(f"Failed to enable pitch limit: {response.message}")
+                return False, response.message
+            return True, 'success'
+        except rospy.ServiceException as e:
+            SDKLogger.error(f"Service call failed: {e}")
+            res_msg = str(e)
+        except rospy.ROSException as e:
+            SDKLogger.error(f"Service call failed: {e}")
+            res_msg = str(e)
+        except Exception as e:
+            SDKLogger.error(f"Service call failed: {e}")
+            res_msg = str(e)
+        return False, res_msg
 
 def euler_to_rotation_matrix(yaw, pitch, roll):
     # 计算各轴的旋转矩阵

{kuavo_humanoid_sdk-1.2.1 → kuavo_humanoid_sdk-1.2.1b1675}/kuavo_humanoid_sdk/kuavo/core/ros/state.py

@@ -4,6 +4,7 @@ import copy
 import rospy
 from std_msgs.msg import Float64
 from nav_msgs.msg import Odometry
+from std_srvs.srv import SetBool, SetBoolRequest
 from sensor_msgs.msg import JointState
 from kuavo_humanoid_sdk.msg.kuavo_msgs.msg import sensorsData, lejuClawState, gaitTimeName, dexhandTouchState
 from kuavo_msgs.srv import changeArmCtrlMode, changeArmCtrlModeRequest, getCurrentGaitName, getCurrentGaitNameRequest
@@ -201,6 +202,14 @@ class KuavoRobotStateCore:
             self._manipulation_mpc_control_flow = flow
         return self._manipulation_mpc_control_flow
     
+    @property
+    def pitch_limit_enabled(self)->bool:
+        success, status = self._srv_get_pitch_limit_status()
+        if success:
+            return not ('disabled' in status)
+        else:
+            return False
+    
     @property
     def eef_state(self)->Tuple[EndEffectorState, EndEffectorState]:
         return self._eef_state
@@ -596,7 +605,27 @@ class KuavoRobotStateCore:
             SDKLogger.error(f"Failed to get manipulation mpc wbc arm trajectory control mode: {e}")
         return KuavoManipulationMpcControlFlow.Error
 
-
+    def _srv_get_pitch_limit_status(self, )->Tuple[bool, str]:
+        try:
+            service_name = '/humanoid/mpc/pitch_limit_status'
+            rospy.wait_for_service(service_name, timeout=2.0)
+            get_pitch_limit_status_srv = rospy.ServiceProxy(service_name, SetBool)
+            
+            req = SetBoolRequest()
+            
+            resp = get_pitch_limit_status_srv(req)
+            if not resp.success:
+                SDKLogger.error(f"Failed to get pitch limit status: {resp.message}")
+                return False, 'unknown'
+            return resp.success, resp.message
+        except rospy.ServiceException as e:
+            SDKLogger.error(f"Service call to {service_name} failed: {e}")
+        except rospy.ROSException as e: # For timeout from wait_for_service
+            SDKLogger.error(f"Failed to connect to service {service_name}: {e}")
+        except Exception as e:
+            SDKLogger.error(f"Failed to get pitch limit status: {e}")
+        return False, 'unknown'
+    
 if __name__ == "__main__":
     state = KuavoRobotStateCore()
     print(state.manipulation_mpc_frame)

{kuavo_humanoid_sdk-1.2.1 → kuavo_humanoid_sdk-1.2.1b1675}/kuavo_humanoid_sdk/kuavo/core/ros/tools.py

@@ -38,8 +38,9 @@ class KuavoRobotToolsCore:
                 self._transform_cache = {}
                 self._initialized = True
             except Exception as e:
-                SDKLogger.error(f"Failed to initialize tf2_web_republisher: {str(e)}")
-                SDKLogger.error("Please make sure tf2_web_republisher node is running")
+                SDKLogger.error(f"Failed to initialize kuavo_tf2_web_republisher: {str(e)}")
+                SDKLogger.error(f"kuavo_tf2_web_republisher 节点未运行")
+                SDKLogger.error("请运行 `cd <kuavo_ros_application> && source devel/setup.bash && rosrun kuavo_tf2_web_republisher kuavo_tf2_web_republisher` 启动 kuavo_tf2_web_republisher 节点")
                 raise
 
     def _get_tf_tree_transform(self, target_frame: str, source_frame: str, 

{kuavo_humanoid_sdk-1.2.1 → kuavo_humanoid_sdk-1.2.1b1675}/kuavo_humanoid_sdk/kuavo/core/ros_env.py

@@ -228,6 +228,7 @@ class KuavoROSEnv:
         """
         try:
             nodes = subprocess.check_output(['rosnode', 'list']).decode('utf-8').split('\n')
+            # print(f"Debug: nodes: {nodes}")
             return node_name in nodes
         except subprocess.CalledProcessError as e:
             SDKLogger.error(f"Error checking if node {node_name} exists: {e}")

{kuavo_humanoid_sdk-1.2.1 → kuavo_humanoid_sdk-1.2.1b1675}/kuavo_humanoid_sdk/kuavo/robot.py

@@ -270,6 +270,21 @@ class KuavoRobot(RobotBase):
         return self._robot_head.disable_head_tracking()
     
     """ Robot Arm Control """
+    def control_hand_wrench(self, left_wrench: list, right_wrench: list) -> bool:
+        """控制机器人末端力/力矩
+        
+        Args:
+            left_wrench (list): 左手臂6维力控指令 [Fx, Fy, Fz, Tx, Ty, Tz]
+            right_wrench (list): 右手臂6维力控指令 [Fx, Fy, Fz, Tx, Ty, Tz]
+                单位:
+                Fx,Fy,Fz: 牛顿(N)
+                Tx,Ty,Tz: 牛·米(N·m)
+        
+        Returns:
+            bool: 控制成功返回True, 否则返回False
+        """
+        return self._robot_arm.control_hand_wrench(left_wrench, right_wrench)
+
     def arm_reset(self)->bool:
         """手臂归位
         
@@ -456,7 +471,41 @@ class KuavoRobot(RobotBase):
             Tuple[bool, list]: 成功标志和 :class:`kuavo_humanoid_sdk.interfaces.data_types.KuavoMotorParam` 对象列表的元组
         """
         return self._kuavo_core.get_motor_param()
+
+    def enable_base_pitch_limit(self, enable: bool) -> Tuple[bool, str]:
+        """开启/关闭机器人 basePitch 限制
+        
+        Note:
+             该接口用于关闭或开启机器人 basePitch 保护功能，关闭状态下可以进行比较大幅度的前后倾动作而不会触发保护导致摔倒。
+
+        Args:
+            enable (bool): 开启/关闭
+        """
+        return self._kuavo_core.enable_base_pitch_limit(enable)
+    
+    def is_arm_collision(self)->bool:
+        """判断当前是否发生碰撞
         
+        Returns:
+            bool: 发生碰撞返回True,否则返回False
+        """
+        return self._robot_arm.is_arm_collision()
+    
+    def wait_arm_collision_complete(self):
+        """等待碰撞完成
+        """
+        self._robot_arm.wait_arm_collision_complete()
+
+    def release_arm_collision_mode(self):
+        """释放碰撞模式
+        """
+        self._robot_arm.release_arm_collision_mode()
+
+    def set_arm_collision_mode(self, enable: bool):
+        """设置碰撞模式
+        """
+        self._robot_arm.set_arm_collision_mode(enable)
+
 if __name__ == "__main__":
     robot = KuavoRobot()
     robot.set_manipulation_mpc_mode(KuavoManipulationMpcCtrlMode.ArmOnly)

{kuavo_humanoid_sdk-1.2.1 → kuavo_humanoid_sdk-1.2.1b1675}/kuavo_humanoid_sdk/kuavo/robot_arm.py

@@ -202,6 +202,44 @@ class KuavoRobotArm:
             return None, None
         return result
 
+    def control_hand_wrench(self, left_wrench: list, right_wrench: list) -> bool:
+        """控制机器人末端力/力矩
+        
+        Args:
+            left_wrench (list): 左手臂6维力控指令 [Fx, Fy, Fz, Tx, Ty, Tz]
+            right_wrench (list): 右手臂6维力控指令 [Fx, Fy, Fz, Tx, Ty, Tz]
+                单位:
+                Fx,Fy,Fz: 牛顿(N)
+                Tx,Ty,Tz: 牛·米(N·m)
+        
+        Returns:
+            bool: 控制成功返回True, 否则返回False
+        """
+        return self._kuavo_core.control_hand_wrench(left_wrench, right_wrench)
+
+    def is_arm_collision(self)->bool:
+        """判断当前是否发生碰撞
+        
+        Returns:
+            bool: 发生碰撞返回True,否则返回False
+        """
+        return self._kuavo_core.is_arm_collision()
+    
+    def release_arm_collision_mode(self):
+        """释放碰撞模式
+        """
+        self._kuavo_core.release_arm_collision_mode()
+
+    def wait_arm_collision_complete(self):
+        """等待碰撞完成
+        """
+        self._kuavo_core.wait_arm_collision_complete()
+
+    def set_arm_collision_mode(self, enable: bool):
+        """设置碰撞模式
+        """
+        self._kuavo_core.set_arm_collision_mode(enable)
+
 # if __name__ == "__main__":
 #     arm = KuavoRobotArm()
 #     arm.set_manipulation_mpc_mode(KuavoManipulationMpcCtrlMode.ArmOnly)

{kuavo_humanoid_sdk-1.2.1 → kuavo_humanoid_sdk-1.2.1b1675}/kuavo_humanoid_sdk/kuavo/robot_head.py

@@ -25,7 +25,7 @@ class KuavoRobotHead:
         limited_yaw = min(math.pi*4/9, max(-math.pi*4/9, yaw))
 
         # Check pitch limits (-25 to 25 degrees)
-        if pitch < -math.pi/7.2 or pitch > math.pi/7.2:  # -25 to 25 degrees in radians
+        if pitch < -math.pi/7.2 - 0.001 or pitch > math.pi/7.2 + 0.001:  # -25 to 25 degrees in radians
             SDKLogger.warn(f"[Robot] pitch {pitch} exceeds limit [-{math.pi/7.2:.3f}, {math.pi/7.2:.3f}] radians (-25 to 25 degrees), will be limited")
         limited_pitch = min(math.pi/7.2, max(-math.pi/7.2, pitch))
         return self._kuavo_core.control_robot_head(yaw=limited_yaw, pitch=limited_pitch)

{kuavo_humanoid_sdk-1.2.1 → kuavo_humanoid_sdk-1.2.1b1675}/kuavo_humanoid_sdk/kuavo/robot_state.py

@@ -167,6 +167,14 @@ class KuavoRobotState:
         """
         return self._rs_core.manipulation_mpc_frame
     
+    def pitch_limit_enabled(self) -> bool:
+        """获取机器人 basePitch 限制状态, 如果开启则返回True，否则返回False。
+
+        Returns:
+            bool: 如果机器人 basePitch 限制开启返回True，否则返回False。
+        """
+        return self._rs_core.pitch_limit_enabled
+
     def head_joint_state(self) -> KuavoJointData:
         """获取机器人头部关节的当前状态。