kuavo-humanoid-sdk 1.1.5__tar.gz → 1.1.6__tar.gz

{kuavo_humanoid_sdk-1.1.5 → kuavo_humanoid_sdk-1.1.6}/PKG-INFO

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

{kuavo_humanoid_sdk-1.1.5 → kuavo_humanoid_sdk-1.1.6}/kuavo_humanoid_sdk/__init__.py

@@ -1,6 +1,4 @@
 from .msg import *
 from .interfaces import *
 from .kuavo import *
-from .kuavo_strategy import *   
-from .kuavo_strategy.grasp_box import *
 from kuavo_humanoid_sdk.common.logger import SDKLogger, disable_sdk_logging

{kuavo_humanoid_sdk-1.1.5 → kuavo_humanoid_sdk-1.1.6}/kuavo_humanoid_sdk/interfaces/data_types.py

@@ -1,7 +1,6 @@
 from typing import Tuple
 from enum import Enum
 from dataclasses import dataclass
-import numpy as np
 
 @dataclass
 class KuavoJointData:
@@ -60,50 +59,6 @@ class KuavoArmCtrlMode(Enum):
     AutoSwing = 1 
     ExternalControl = 2
 
-class KuavoManipulationMpcFrame(Enum):
-    """Enum class representing the manipulation mpc frame for the Kuavo robot end effector.
-
-    Attributes:
-        KeepCurrentFrame: Keep the current frame (value: 0)
-        WorldFrame: World frame (value: 1)
-        LocalFrame: Local frame (value: 2)
-        VRFrame: VR frame (value: 3)
-        ManipulationWorldFrame: Manipulation world frame (value: 4)
-    """
-    KeepCurrentFrame = 0
-    WorldFrame = 1
-    LocalFrame = 2
-    VRFrame = 3
-    ManipulationWorldFrame = 4
-    ERROR = -1
-
-class KuavoManipulationMpcCtrlMode(Enum):
-    """Enum class representing the control mode for the Kuavo robot manipulation MPC.
-
-    Attributes:
-        NoControl: No control (value: 0)
-        ArmOnly: Only control the arm (value: 1)
-        BaseOnly: Only control the base (value: 2)
-        BaseArm: Control both the base and the arm (value: 3)
-        ERROR: Error state (value: -1)
-    """
-    NoControl = 0
-    ArmOnly = 1
-    BaseOnly = 2
-    BaseArm = 3
-    ERROR = -1
-
-class KuavoManipulationMpcControlFlow(Enum):
-    """Enum class representing the control data flow for the Kuavo robot manipulation.
-
-    Attributes:
-        ThroughFullBodyMpc: Control data flows through full-body MPC before entering WBC (value: 0)
-        DirectToWbc: Control data flows directly to WBC without full-body MPC (value: 1)
-        Error: Invalid control path (value: -1)
-    """
-    ThroughFullBodyMpc = 0
-    DirectToWbc = 1
-    Error = -1
 
 @dataclass
 class EndEffectorState:
@@ -188,48 +143,3 @@ class KuavoDexHandTouchState:
         status: int  # 传感器状态
     # 5 fingers
     data: Tuple[KuavoTouchState, KuavoTouchState, KuavoTouchState, KuavoTouchState, KuavoTouchState]
-
-@dataclass
-class AprilTagData:
-    """Represents detected AprilTag information with pose estimation.
-    
-    Attributes:
-        id (list): List of detected AprilTag IDs (integers)
-        size (list): List of tag physical sizes in meters (floats)
-        pose (list): List of PoseQuaternion objects representing tag poses
-    """
-    id: list
-    size: list
-    pose: list
-
-@dataclass
-class HomogeneousMatrix:
-    """4x4 homogeneous transformation matrix for 3D transformations.
-    
-    Represents both rotation and translation in 3D space. Can be used for
-    coordinate frame transformations and pose composition.
-    
-    Attributes:
-        matrix (np.ndarray): 4x4 numpy array of shape (4, 4) containing::
-        
-            [[R, t],
-             [0, 1]] 
-            
-        where R is 3x3 rotation matrix and t is 3x1 translation
-    """
-    matrix: np.ndarray  # Shape (4,4) homogeneous transformation matrix
-
-@dataclass
-class PoseQuaternion:
-    """3D pose representation using position and quaternion orientation.
-    
-    Provides a singularity-free orientation representation. Commonly used
-    in robotics for smooth interpolation between orientations.
-    
-    Attributes:
-        position (Tuple[float, float, float]): XYZ coordinates in meters
-        orientation (Tuple[float, float, float, float]): Unit quaternion in
-            (x, y, z, w) format following ROS convention
-    """
-    position: Tuple[float, float, float]
-    orientation: Tuple[float, float, float, float]

{kuavo_humanoid_sdk-1.1.5 → kuavo_humanoid_sdk-1.1.6}/kuavo_humanoid_sdk/kuavo/__init__.py

@@ -1,10 +1,7 @@
 from .robot import KuavoSDK, KuavoRobot  
 from .robot_info import KuavoRobotInfo
 from .robot_state import KuavoRobotState
-from .robot_vision import KuavoRobotVision
-from .robot_tool import KuavoRobotTools
 from .robot_arm import KuavoRobotArm
 from .robot_head import KuavoRobotHead
-from .robot_audio import KuavoRobotAudio
 from .dexterous_hand import DexterousHand, TouchDexterousHand
 from .leju_claw import LejuClaw

{kuavo_humanoid_sdk-1.1.5 → kuavo_humanoid_sdk-1.1.6}/kuavo_humanoid_sdk/kuavo/core/core.py

@@ -19,37 +19,32 @@ Each state has an entry callback that handles initialization when entering that
 
 import time
 import math
+import rospy
 import threading
 import numpy as np
 from typing import Tuple
 from transitions import Machine, State
 
-from kuavo_humanoid_sdk.interfaces.data_types import KuavoArmCtrlMode, KuavoIKParams, KuavoPose, KuavoManipulationMpcFrame, KuavoManipulationMpcCtrlMode, KuavoManipulationMpcControlFlow
-from kuavo_humanoid_sdk.kuavo.core.ros.control import KuavoRobotControl, KuavoRobotControlWebsocket
-from kuavo_humanoid_sdk.kuavo.core.ros.vision import KuavoRobotVisionCore
-from kuavo_humanoid_sdk.kuavo.core.ros.tools import KuavoRobotToolsCore
-from kuavo_humanoid_sdk.kuavo.core.ros.state import KuavoRobotStateCore, KuavoRobotStateCoreWebsocket
+from kuavo_humanoid_sdk.interfaces.data_types import KuavoArmCtrlMode, KuavoIKParams, KuavoPose
+from kuavo_humanoid_sdk.kuavo.core.ros.control import KuavoRobotControl
+from kuavo_humanoid_sdk.kuavo.core.ros.state import KuavoRobotStateCore
 from kuavo_humanoid_sdk.kuavo.core.ros.param import make_robot_param
 from kuavo_humanoid_sdk.common.logger import SDKLogger
-from kuavo_humanoid_sdk.common.global_config import GlobalConfig
+
 # Define robot states
 ROBOT_STATES = [
     State(name='stance', on_enter=['_on_enter_stance']),
     State(name='walk', on_enter=['_on_enter_walk']), 
     State(name='trot', on_enter=['_on_enter_trot']),
-    State(name='custom_gait', on_enter=['_on_enter_custom_gait']),
-    State(name='command_pose_world', on_enter=['_on_enter_command_pose_world']),
-    State(name='command_pose', on_enter=['_on_enter_command_pose']),
+    State(name='custom_gait', on_enter=['_on_enter_custom_gait'])
 ]
 
 # Define state transitions
 ROBOT_TRANSITIONS = [
-    {'trigger': 'to_stance', 'source': ['walk', 'trot', 'custom_gait', 'command_pose_world', 'command_pose'], 'dest': 'stance'},
+    {'trigger': 'to_stance', 'source': ['walk', 'trot', 'custom_gait'], 'dest': 'stance'},
     {'trigger': 'to_walk', 'source': ['stance', 'trot', 'custom_gait'], 'dest': 'walk'},
     {'trigger': 'to_trot', 'source': ['stance', 'walk', 'custom_gait'], 'dest': 'trot'},
     {'trigger': 'to_custom_gait', 'source': ['stance', 'custom_gait'], 'dest': 'custom_gait'},
-    {'trigger': 'to_command_pose_world', 'source': ['stance', 'command_pose_world'], 'dest': 'command_pose_world'},
-    {'trigger': 'to_command_pose', 'source': ['stance', 'command_pose'], 'dest': 'command_pose'},
 ]
 
 class KuavoRobotCore:
@@ -70,25 +65,9 @@ class KuavoRobotCore:
                 send_event=True
             )
             # robot control
-            if GlobalConfig.use_websocket:
-                self._control = KuavoRobotControlWebsocket()
-                self._rb_state = KuavoRobotStateCoreWebsocket()
-            else:
-                self._control = KuavoRobotControl()
-                self._rb_state = KuavoRobotStateCore()
-    
-            # manipulation mpc
-            self._manipulation_mpc_frame = KuavoManipulationMpcFrame.KeepCurrentFrame
-            self._manipulation_mpc_ctrl_mode = KuavoManipulationMpcCtrlMode.NoControl
-            self._manipulation_mpc_control_flow = KuavoManipulationMpcControlFlow.ThroughFullBodyMpc
-
-            # robot vision
-            self._robot_vision = KuavoRobotVisionCore()
-            # robot ros tf
-            self._robot_tf_tool = KuavoRobotToolsCore()
-                
+            self._control = KuavoRobotControl()
+            self._rb_state = KuavoRobotStateCore()
             self._arm_ctrl_mode = KuavoArmCtrlMode.AutoSwing
-            
             # register gait changed callback
             self._rb_state.register_gait_changed_callback(self._humanoid_gait_changed)
             # initialized
@@ -114,21 +93,6 @@ class KuavoRobotCore:
             if arm_ctrl_mode is not None:
                 self._arm_ctrl_mode = arm_ctrl_mode
                 SDKLogger.debug(f"[Core] initialize arm control mode: {arm_ctrl_mode}")
-            
-            # init manipulation mpc
-            manipulation_mpc_frame = self._rb_state.manipulation_mpc_frame
-            if manipulation_mpc_frame is not None:
-                self._manipulation_mpc_frame = manipulation_mpc_frame
-                SDKLogger.debug(f"[Core] initialize manipulation mpc frame: {manipulation_mpc_frame}")
-            manipulation_mpc_ctrl_mode = self._rb_state.manipulation_mpc_ctrl_mode
-            if manipulation_mpc_ctrl_mode is not None:
-                self._manipulation_mpc_ctrl_mode = manipulation_mpc_ctrl_mode
-                SDKLogger.debug(f"[Core] initialize manipulation mpc ctrl mode: {manipulation_mpc_ctrl_mode}")
-            manipulation_mpc_control_flow = self._rb_state.manipulation_mpc_control_flow
-            if manipulation_mpc_control_flow is not None:
-                self._manipulation_mpc_control_flow = manipulation_mpc_control_flow
-                SDKLogger.debug(f"[Core] initialize manipulation mpc control flow: {manipulation_mpc_control_flow}")
-                
         except Exception as e:
             raise RuntimeError(f"[Core] initialize failed: \n"
                              f"{e}, please check the robot is launched, "
@@ -190,21 +154,6 @@ class KuavoRobotCore:
             SDKLogger.debug(f"[Core] [StateMachine] State unchanged: already in custom_gait state")
             return
         SDKLogger.debug(f"[Core] [StateMachine] Entering custom_gait state, from {previous_state}")
-    
-    def _on_enter_command_pose_world(self, event):
-        previous_state = event.transition.source
-        if self.state  == previous_state:
-            SDKLogger.debug(f"[Core] [StateMachine] State unchanged: already in command_pose_world state")
-            return
-        SDKLogger.debug(f"[Core] [StateMachine] Entering command_pose_world state, from {previous_state}")
-
-    def _on_enter_command_pose(self, event):
-        previous_state = event.transition.source
-        if self.state  == previous_state:
-            SDKLogger.debug(f"[Core] [StateMachine] State unchanged: already in command_pose state")
-            return
-        SDKLogger.debug(f"[Core] [StateMachine] Entering command_pose state, from {previous_state}")
-        
     """ -------------------------------------------------------------"""
 
     """ --------------------------- Control -------------------------"""
@@ -340,54 +289,6 @@ class KuavoRobotCore:
 
         return True
 
-    def control_command_pose(self, target_pose_x:float, target_pose_y:float, target_pose_z:float, target_pose_yaw:float)->bool:
-        """
-        Control robot pose in base_link frame
-        
-        Arguments:
-            - target_pose_x: x position (meters)
-            - target_pose_y: y position (meters)
-            - target_pose_z: z position (meters)
-            - target_pose_yaw: yaw angle (radians)
-        
-        Returns:
-            bool: True if command was sent successfully, False otherwise
-            
-        Raises:
-            RuntimeError: If robot is not in stance state
-        """
-        if self.state != 'stance':
-            raise RuntimeError(f"[Core] control_command_pose failed: robot must be in stance state, current state: {self.state}")
-        
-        # Add any parameter validation if needed
-        # e.g., limit ranges for safety
-        self.to_command_pose()
-        return self._control.control_command_pose(target_pose_x, target_pose_y, target_pose_z, target_pose_yaw)
-
-    def control_command_pose_world(self, target_pose_x:float, target_pose_y:float, target_pose_z:float, target_pose_yaw:float)->bool:
-        """
-        Control robot pose in odom (world) frame
-        
-        Arguments:
-            - target_pose_x: x position (meters)
-            - target_pose_y: y position (meters)
-            - target_pose_z: z position (meters)
-            - target_pose_yaw: yaw angle (radians)
-        
-        Returns:
-            bool: True if command was sent successfully, False otherwise
-            
-        Raises:
-            RuntimeError: If robot is not in stance state
-        """
-        # if self.state != 'stance':
-        #     raise RuntimeError(f"[Core] control_command_pose_world failed: robot must be in stance state, current state: {self.state}")
-        
-        # Add any parameter validation if needed
-        # e.g., limit ranges for safety
-        self.to_command_pose_world()
-        return self._control.control_command_pose_world(target_pose_x, target_pose_y, target_pose_z, target_pose_yaw)
-    
     def execute_gesture(self, gestures:list)->bool:
         return self._control.execute_gesture(gestures)
     
@@ -410,111 +311,30 @@ class KuavoRobotCore:
         pitch_deg = pitch * 180 / math.pi
         return self._control.control_robot_head(yaw_deg, pitch_deg)
     
-    def enable_head_tracking(self, target_id: int)->bool:
-        return self._control.enable_head_tracking(target_id)
-    
-    def disable_head_tracking(self)->bool:
-        return self._control.disable_head_tracking()
-    
-    def control_robot_arm_joint_positions(self, joint_data:list)->bool:
+    def control_robot_arm_traj(self, joint_data:list)->bool:
         if self.state != 'stance':
-            raise RuntimeError(f"[Core] control_robot_arm_joint_positions failed: robot must be in stance state, current state: {self.state}")
+            raise RuntimeError(f"[Core] control_robot_arm_traj failed: robot must be in stance state, current state: {self.state}")
         
         # 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.")
+            SDKLogger.debug("[Core] control_robot_arm_traj, current arm mode != ExternalControl, change it.")
             if not self.change_robot_arm_ctrl_mode(KuavoArmCtrlMode.ExternalControl):
-                SDKLogger.warn("[Core] control_robot_arm_joint_positions failed, change robot arm ctrl mode failed!")
+                SDKLogger.warn("[Core] control_robot_arm_traj failed, change robot arm ctrl mode failed!")
                 return False
-        return self._control.control_robot_arm_joint_positions(joint_data)
+        return self._control.control_robot_arm_traj(joint_data)
     
-    def control_robot_arm_joint_trajectory(self, times:list, joint_q:list)->bool:
+    def control_robot_arm_target_poses(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")
+            raise RuntimeError("[Core] control_robot_arm_target_poses failed: robot must be in stance state")
         
         if self._arm_ctrl_mode != KuavoArmCtrlMode.ExternalControl:
-            SDKLogger.debug("[Core] control_robot_arm_joint_trajectory, current arm mode != ExternalControl, change it.")
+            SDKLogger.debug("[Core] control_robot_arm_target_poses, current arm mode != ExternalControl, change it.")
             if not self.change_robot_arm_ctrl_mode(KuavoArmCtrlMode.ExternalControl):
-                SDKLogger.warn("[Core] control_robot_arm_joint_trajectory failed, change robot arm ctrl mode failed!")
+                SDKLogger.warn("[Core] control_robot_arm_target_poses failed, change robot arm ctrl mode failed!")
                 return False
             
-        return self._control.control_robot_arm_joint_trajectory(times, joint_q)
-    
-    def control_robot_end_effector_pose(self, left_pose: KuavoPose, right_pose: KuavoPose, frame: KuavoManipulationMpcFrame)->bool:        
-        if self._arm_ctrl_mode != KuavoArmCtrlMode.ExternalControl:
-            SDKLogger.debug("[Core] control_robot_end_effector_pose, current arm mode != ExternalControl, change it.")
-            if not self.change_robot_arm_ctrl_mode(KuavoArmCtrlMode.ExternalControl):
-                SDKLogger.warn("[Core] control_robot_end_effector_pose failed, change robot arm ctrl mode failed!")
-                return False
-
-        if self._manipulation_mpc_ctrl_mode == KuavoManipulationMpcCtrlMode.NoControl:
-            SDKLogger.debug("[Core] control_robot_end_effector_pose, manipulation mpc ctrl mode is NoControl, change it.")
-            if not self.change_manipulation_mpc_ctrl_mode(KuavoManipulationMpcCtrlMode.ArmOnly):
-                SDKLogger.warn("[Core] control_robot_end_effector_pose failed, change manipulation mpc ctrl mode failed!")
-                return False
-        
-        return self._control.control_robot_end_effector_pose(left_pose, right_pose, frame)
+        return self._control.control_robot_arm_target_poses(times, joint_q)
 
-    def change_manipulation_mpc_frame(self, frame: KuavoManipulationMpcFrame)->bool:
-        timeout = 1.0
-        count = 0
-        while self._rb_state.manipulation_mpc_frame != frame:
-            SDKLogger.debug(f"[Core] Change manipulation mpc frame from {self._rb_state.manipulation_mpc_frame} to {frame}, retry: {count}")
-            self._control.change_manipulation_mpc_frame(frame)
-            if self._rb_state.manipulation_mpc_frame == frame:
-                break
-            if timeout <= 0:
-                SDKLogger.warn("[Core] Change manipulation mpc frame timeout!")
-                return False
-            timeout -= 0.1
-            time.sleep(0.1)
-            count += 1
-        if not hasattr(self, '_manipulation_mpc_frame_lock'):
-            self._manipulation_mpc_frame_lock = threading.Lock()
-        with self._manipulation_mpc_frame_lock:
-            self._manipulation_mpc_frame = frame
-        return True
-    
-    def change_manipulation_mpc_ctrl_mode(self, control_mode: KuavoManipulationMpcCtrlMode)->bool:
-        timeout = 1.0
-        count = 0
-        while self._rb_state.manipulation_mpc_ctrl_mode != control_mode:
-            SDKLogger.debug(f"[Core] Change manipulation mpc ctrl mode from {self._rb_state.manipulation_mpc_ctrl_mode} to {control_mode}, retry: {count}")
-            self._control.change_manipulation_mpc_ctrl_mode(control_mode)
-            if self._rb_state.manipulation_mpc_ctrl_mode == control_mode:
-                break
-            if timeout <= 0:
-                SDKLogger.warn("[Core] Change manipulation mpc ctrl mode timeout!")
-                return False
-            timeout -= 0.1
-            time.sleep(0.1)
-            count += 1
-        if not hasattr(self, '_manipulation_mpc_ctrl_mode_lock'):
-            self._manipulation_mpc_ctrl_mode_lock = threading.Lock()
-        with self._manipulation_mpc_ctrl_mode_lock:
-            self._manipulation_mpc_ctrl_mode = control_mode
-        return True
-    
-    def change_manipulation_mpc_control_flow(self, control_flow: KuavoManipulationMpcControlFlow)->bool:
-        timeout = 1.0
-        count = 0
-        while self._rb_state.manipulation_mpc_control_flow != control_flow:
-            SDKLogger.debug(f"[Core] Change manipulation mpc control flow from {self._rb_state.manipulation_mpc_control_flow} to {control_flow}, retry: {count}")
-            self._control.change_manipulation_mpc_control_flow(control_flow)
-            if self._rb_state.manipulation_mpc_control_flow == control_flow:
-                break
-            if timeout <= 0:
-                SDKLogger.warn("[Core] Change manipulation mpc control flow timeout!")
-                return False
-            timeout -= 0.1
-            time.sleep(0.1)
-            count += 1
-        if not hasattr(self, '_manipulation_mpc_control_flow_lock'):
-            self._manipulation_mpc_control_flow_lock = threading.Lock()
-        with self._manipulation_mpc_control_flow_lock:
-            self._manipulation_mpc_control_flow = control_flow
-        return True
-    
     def change_robot_arm_ctrl_mode(self, mode:KuavoArmCtrlMode)->bool:
         timeout = 1.0
         count = 0
@@ -544,25 +364,12 @@ class KuavoRobotCore:
             return
         
         # init_pos = [0.0]*14
-        # if not self.control_robot_arm_joint_trajectory([1.5], [init_pos]):
+        # if not self.control_robot_arm_target_poses([1.5], [init_pos]):
         #     SDKLogger.warn("[Core] robot arm reset failed, control robot arm traj failed!")
         #     return False
         
         return self.change_robot_arm_ctrl_mode(KuavoArmCtrlMode.AutoSwing)
         
-    def robot_manipulation_mpc_reset(self)->bool:
-        if self._manipulation_mpc_ctrl_mode != KuavoManipulationMpcCtrlMode.NoControl:
-            SDKLogger.debug("[Core] robot manipulation mpc reset, current manipulation mpc ctrl mode != NoControl, change it.")
-            if not self.change_manipulation_mpc_ctrl_mode(KuavoManipulationMpcCtrlMode.NoControl):
-                SDKLogger.warn("[Core] robot manipulation mpc reset failed, change manipulation mpc ctrl mode failed!")
-                return False
-        if self._manipulation_mpc_control_flow != KuavoManipulationMpcControlFlow.ThroughFullBodyMpc:
-            SDKLogger.debug("[Core] robot manipulation mpc reset, current manipulation mpc control flow != ThroughFullBodyMpc, change it.")
-            if not self.change_manipulation_mpc_control_flow(KuavoManipulationMpcControlFlow.ThroughFullBodyMpc):
-                SDKLogger.warn("[Core] robot manipulation mpc reset failed, change manipulation mpc control flow failed!")
-                return False
-        return True
-        
     """ ------------------------------------------------------------------------"""
     """ Arm Forward kinematics && Arm Inverse kinematics """
     def arm_ik(self, 
@@ -587,28 +394,3 @@ class KuavoRobotCore:
                 SDKLogger.debug(f"[Core] Received gait change notification: {gait_name} at time {current_time}")
                 # Call the transition method if it exists
                 getattr(self, to_method)()
-
-
-if __name__ == "__main__":
-    DEBUG_MODE = 0
-    core = KuavoRobotCore()
-    core.initialize()
-
-    if DEBUG_MODE == 0:
-        time.sleep(1.0)
-        core.change_robot_arm_ctrl_mode(KuavoArmCtrlMode.ExternalControl)
-        core.change_manipulation_mpc_frame(KuavoManipulationMpcFrame.VRFrame)
-        core.change_manipulation_mpc_ctrl_mode(KuavoManipulationMpcCtrlMode.ArmOnly)
-        core.change_manipulation_mpc_control_flow(KuavoManipulationMpcControlFlow.DirectToWbc)
-        core.robot_manipulation_mpc_reset()
-    elif DEBUG_MODE == 1:
-        core.to_stance()
-        print("state now is to_stance:", core.state)
-        core.control_command_pose_world(0.0, 1.0, 0.0, 1.57)
-        print("state now is control_command_pose_world:", core.state)
-    elif DEBUG_MODE == 2:
-        core.to_trot()
-        print("state now is to_trot:", core.state)
-        time.sleep(3.0)
-        core.to_stance()
-        print("state now is to_stance:", core.state)