hand-tracking-teleop 0.1.0__py3-none-any.whl

robot_interface/dual_interface.py

@@ -0,0 +1,780 @@
+"""
+Robot Interface for Hand Tracking Data Collection
+
+This module provides a simplified interface to control the dual-arm robot
+using hand tracking data. The robot model now handles MoveIt IK internally.
+"""
+
+import time
+import numpy as np
+from typing import Dict, List, Optional, Tuple, Any
+from dataclasses import dataclass
+
+# Import the existing robot model
+import sys
+import os
+sys.path.append(os.path.join(os.path.dirname(__file__), '../../../../src'))
+from lerobot.robots.dual_arm.robot_dual_arm import DualArmRobot
+from lerobot.robots.dual_arm.configuration_dual_arm import DualArmConfig
+
+# Import the mapping module
+sys.path.append(os.path.join(os.path.dirname(__file__), '..'))
+from mapping.kinematic_mapper import RobotJointState
+
+
+@dataclass
+class RobotState:
+    """Current state of the robot."""
+    joint_positions: np.ndarray
+    joint_velocities: np.ndarray
+    joint_names: List[str]
+    timestamp: float
+    is_moving: bool = False
+
+
+@dataclass
+class HandPose:
+    """Hand pose for IK solving."""
+    position: np.ndarray  # [x, y, z]
+    orientation: np.ndarray  # [x, y, z, w] quaternion
+    hand_side: str  # "left" or "right"
+
+
+class RobotInterface:
+    """Simplified interface to control robot using the robot model's built-in MoveIt IK."""
+    
+    def __init__(self, use_action_client: bool = False):
+        """
+        Initialize the robot interface.
+        
+        Args:
+            use_action_client: Whether to use action client (True) or direct control (False)
+        """
+        self.is_connected = False
+        self.current_state = None
+        
+        # Cache for current hand poses to reduce FK service calls
+        self._cached_hand_poses = None
+        self._last_pose_update = 0
+        self._pose_cache_duration = 2.0  # Cache poses for 2 seconds (was 500ms)
+        self._disable_fk_calls = False  # Option to completely disable FK calls
+        
+        # Create robot configuration
+        self.config = DualArmConfig(mock=False)
+        
+        # Initialize the robot with use_action_client parameter
+        self.robot = DualArmRobot(self.config, use_action_client=use_action_client)
+        
+        # Connect to robot
+        self.robot.connect()
+        self.is_connected = True
+        
+        print(f"✅ Connected to robot with built-in MoveIt IK (action_client: {use_action_client})")
+    
+    def send_joint_command(self, joint_state: RobotJointState, execution_time: float = 1.0, 
+                          fingers_only: bool = True):
+        """
+        Send joint command to robot using direct control for fingers.
+        
+        Args:
+            joint_state: Target joint positions
+            execution_time: Time to execute the movement
+            fingers_only: If True, only send finger commands (safer for testing)
+        """
+        if not self.is_connected:
+            print("❌ Robot not connected")
+            return False
+        
+        try:
+            # Always handle fingers with direct control
+            success = self._send_finger_commands(joint_state)
+            
+            # Handle arms with direct control if requested (IK is separate method)
+            if not fingers_only:
+                success &= self._send_arm_commands_direct(joint_state)
+            
+            return success
+            
+        except Exception as e:
+            print(f"❌ Failed to send joint commands to robot: {e}")
+            return False
+    
+    def _send_finger_commands(self, joint_state: RobotJointState) -> bool:
+        """Send finger commands using direct control."""
+        try:
+            left_finger_commands = {}
+            right_finger_commands = {}
+            
+            # Group finger joint commands
+            for i, joint_name in enumerate(joint_state.joint_names):
+                if i < len(joint_state.joint_positions):
+                    position = float(joint_state.joint_positions[i])
+                    
+                    if joint_name in self.config.left_finger_joints:
+                        left_finger_commands[joint_name] = position
+                    elif joint_name in self.config.right_finger_joints:
+                        right_finger_commands[joint_name] = position
+            
+            # Send finger commands
+            if left_finger_commands:
+                print(f"🤏 Sending {len(left_finger_commands)} left finger commands")
+                self.robot._send_direct_joint_commands(
+                    self.config.left_hand_controller,
+                    left_finger_commands
+                )
+            
+            if right_finger_commands:
+                print(f"🤏 Sending {len(right_finger_commands)} right finger commands")
+                self.robot._send_direct_joint_commands(
+                    self.config.right_hand_controller,
+                    right_finger_commands
+                )
+            
+            return True
+            
+        except Exception as e:
+            print(f"❌ Failed to send finger commands: {e}")
+            return False
+    
+    def _send_arm_commands_direct(self, joint_state: RobotJointState) -> bool:
+        """Send arm commands using direct control (fallback)."""
+        try:
+            arm_commands = {}
+            
+            # Group arm/torso joint commands
+            for i, joint_name in enumerate(joint_state.joint_names):
+                if i < len(joint_state.joint_positions):
+                    position = float(joint_state.joint_positions[i])
+                    
+                    if joint_name in (self.config.left_arm_joints + 
+                                    self.config.right_arm_joints + 
+                                    self.config.torso_joints):
+                        arm_commands[joint_name] = position
+            
+            if arm_commands:
+                print(f"🦾 Sending {len(arm_commands)} direct arm commands")
+                self.robot._send_direct_joint_commands(
+                    self.config.torso_controller,
+                    arm_commands
+                )
+                return True
+            
+            return False
+            
+        except Exception as e:
+            print(f"❌ Failed to send direct arm commands: {e}")
+            return False
+    
+    def _get_cached_hand_poses(self) -> Tuple[Optional[Dict], Optional[Dict]]:
+        """Get current hand poses with caching to reduce FK service calls."""
+        # If FK calls are disabled, always return fallback poses
+        if self._disable_fk_calls:
+            fallback_left = {
+                'position': [0.3, 0.3, 0.6],
+                'orientation': [0.0, 0.0, 0.0, 1.0]
+            }
+            fallback_right = {
+                'position': [0.3, -0.3, 0.6],
+                'orientation': [0.0, 0.0, 0.0, 1.0]
+            }
+            return fallback_left, fallback_right
+        
+        current_time = time.time()
+        
+        # Check if cache is still valid
+        if (self._cached_hand_poses is not None and 
+            current_time - self._last_pose_update < self._pose_cache_duration):
+            return self._cached_hand_poses
+        
+        # Cache expired or doesn't exist, get new poses
+        try:
+            current_left, current_right = self.robot.get_current_hand_poses()
+            self._cached_hand_poses = (current_left, current_right)
+            self._last_pose_update = current_time
+            return self._cached_hand_poses
+        except Exception as e:
+            # If FK service fails, use fallback poses or cached poses
+            print(f"⚠️ FK service call failed, using fallback: {e}")
+            # Disable FK calls after repeated failures
+            self._disable_fk_calls = True
+            print("🚫 FK calls disabled due to repeated failures")
+            
+            if self._cached_hand_poses is not None:
+                return self._cached_hand_poses
+            else:
+                # Fallback to reasonable default poses
+                fallback_left = {
+                    'position': [0.3, 0.3, 0.6],
+                    'orientation': [0.0, 0.0, 0.0, 1.0]
+                }
+                fallback_right = {
+                    'position': [0.3, -0.3, 0.6],
+                    'orientation': [0.0, 0.0, 0.0, 1.0]
+                }
+                return fallback_left, fallback_right
+    
+    def update_hand_pose_cache(self):
+        """Force update of hand pose cache."""
+        self._cached_hand_poses = None
+        self._last_pose_update = 0
+        # Next call to _get_cached_hand_poses will fetch fresh data
+    
+    def disable_fk_calls(self):
+        """Disable FK service calls completely to eliminate timeouts."""
+        self._disable_fk_calls = True
+        print("🚫 FK service calls disabled - using fallback poses")
+    
+    def enable_fk_calls(self):
+        """Re-enable FK service calls."""
+        self._disable_fk_calls = False
+        self._cached_hand_poses = None  # Clear cache to force fresh data
+        print("✅ FK service calls enabled")
+    
+    def convert_hand_landmarks_to_poses(self, hands_data, control_mode: str = "full_body") -> Tuple[Optional[Dict], Optional[Dict]]:
+        """
+        Convert hand tracking landmarks to robot hand poses.
+        
+        Args:
+            hands_data: List of HandLandmarks from hand tracking
+            control_mode: "full_body" or "fingers_only" - if fingers_only, skip pose conversion
+            
+        Returns:
+            Tuple of (left_pose_dict, right_pose_dict) for robot IK
+        """
+        # Skip pose conversion entirely in fingers_only mode to avoid FK calls
+        if control_mode == "fingers_only":
+            return None, None
+        
+        left_pose = None
+        right_pose = None
+        
+        # Get current hand poses as reference (with caching to reduce FK calls)
+        current_left, current_right = self._get_cached_hand_poses()
+        
+        try:
+            for hand in hands_data:
+                # Get wrist position (landmark 0)
+                wrist = hand.landmarks[0]
+                
+                # Debug: Print raw hand tracking data
+                # print(f"Hand {hand.handedness}: wrist at ({wrist[0]:.3f}, {wrist[1]:.3f}, {wrist[2]:.3f})")
+                
+                # Convert to relative movement from current position
+                # Map camera movement to robot movement (TOP-VIEW CAMERA)
+                # Camera X (left-right) → Robot Y (left-right, but inverted: right in image = -Y in robot)
+                # Camera Y (top-bottom) → Robot X (forward-back: down in image = -X in robot) 
+                # Camera Z (depth) → Robot -Z (up-down, inverted)
+                dy = -(wrist[0] / 640.0 - 0.5) * 0.8  # ±40cm movement in Y (left/right, inverted)
+                dx = -(wrist[1] / 480.0 - 0.5) * 0.8   # ±40cm movement in X (forward/back)
+                dz = -wrist[2] * 0.8  # 0-20cm movement in Z (up/down)
+                
+                # Apply movement relative to current hand position
+                if hand.handedness == "Left" and current_left:
+                    x = current_left['position'][0] + dx
+                    y = current_left['position'][1] + dy
+                    z = current_left['position'][2] + dz
+                    base_orientation = current_left['orientation']
+                elif hand.handedness == "Right" and current_right:
+                    x = current_right['position'][0] + dx
+                    y = current_right['position'][1] + dy
+                    z = current_right['position'][2] + dz
+                    base_orientation = current_right['orientation']
+                else:
+                    continue
+                
+                # print(f"Movement: dx={dx:.3f}, dy={dy:.3f}, dz={dz:.3f}")
+                print(f"Target position {hand.handedness}: ({x:.3f}, {y:.3f}, {z:.3f})")
+                
+                # Calculate hand orientation from landmarks for realistic training data
+                # Use wrist, middle finger MCP, and index finger MCP for orientation
+                middle_mcp = hand.landmarks[9]  # Middle finger MCP
+                index_mcp = hand.landmarks[5]   # Index finger MCP
+                
+                # Calculate hand vectors
+                palm_vec = middle_mcp - wrist  # Palm direction
+                finger_vec = index_mcp - wrist # Finger direction
+                
+                # Calculate hand normal (cross product)
+                palm_normal = np.cross(palm_vec, finger_vec)
+                palm_normal = palm_normal / (np.linalg.norm(palm_normal) + 1e-6)
+                
+                # Simple orientation mapping - blend with current orientation for stability
+                blend_factor = 0.7  # 70% current, 30% tracked
+                
+                # Convert to simple rotation around Z-axis (yaw)
+                yaw = np.arctan2(palm_vec[1], palm_vec[0]) * 0.3  # Scale down rotation
+                
+                # Create quaternion from yaw rotation
+                qw_new = np.cos(yaw/2)
+                qx_new = 0.0
+                qy_new = 0.0  
+                qz_new = np.sin(yaw/2)
+                
+                # Blend with current orientation for stability
+                qx = blend_factor * base_orientation[0] + (1-blend_factor) * qx_new
+                qy = blend_factor * base_orientation[1] + (1-blend_factor) * qy_new
+                qz = blend_factor * base_orientation[2] + (1-blend_factor) * qz_new
+                qw = blend_factor * base_orientation[3] + (1-blend_factor) * qw_new
+                
+                # Normalize quaternion
+                quat_norm = np.sqrt(qx*qx + qy*qy + qz*qz + qw*qw)
+                qx, qy, qz, qw = qx/quat_norm, qy/quat_norm, qz/quat_norm, qw/quat_norm
+                
+                # Create pose dictionary for robot
+                pose_dict = {
+                    'position': [x, y, z],
+                    'orientation': [qx, qy, qz, qw]
+                }
+                
+                if hand.handedness == "Left":
+                    left_pose = pose_dict
+                elif hand.handedness == "Right":
+                    right_pose = pose_dict
+                    
+        except Exception as e:
+            print(f"Warning: Failed to convert hand landmarks to poses: {e}")
+        
+        return left_pose, right_pose
+
+    def convert_hand_landmarks_to_poses_fast(self, hands_data, control_mode: str = "full_body") -> Tuple[Optional[Dict], Optional[Dict]]:
+        """
+        Convert hand tracking landmarks to robot hand poses (fast version without FK calls).
+        
+        This version uses fixed reference poses to avoid FK service calls entirely,
+        making it suitable for continuous real-time control.
+        
+        Args:
+            hands_data: List of HandLandmarks from hand tracking
+            control_mode: "full_body" or "fingers_only" - if fingers_only, skip pose conversion
+            
+        Returns:
+            Tuple of (left_pose_dict, right_pose_dict) for robot IK
+        """
+        # Skip pose conversion entirely in fingers_only mode
+        if control_mode == "fingers_only":
+            return None, None
+        
+        left_pose = None
+        right_pose = None
+        
+        # Get current hand poses as reference (with caching to reduce FK calls)
+        current_left, current_right = self._get_cached_hand_poses()
+        
+        
+        
+        try:
+            for hand in hands_data:
+                # Get wrist position (landmark 0)
+                wrist = hand.landmarks[0]
+                
+                # Use much smaller, more conservative movement ranges
+                # Map camera movement to robot movement (TOP-VIEW CAMERA)
+                dy = -(wrist[0] / 640.0 - 0.5) * 0.15  # ±7.5cm movement in Y (reduced from 15cm)
+                dx = -(wrist[1] / 480.0 - 0.5) * 0.10  # ±5cm movement in X (reduced from 10cm)
+                dz = -wrist[2] * 0.10  # 0-10cm movement in Z (reduced from 20cm)
+                
+                # Apply movement relative to reference position
+                if hand.handedness == "Left":
+                    x = current_left['position'][0] + dx
+                    y = current_left['position'][1] + dy
+                    z = current_left['position'][2] + dz
+                    base_orientation = current_left['orientation']
+                elif hand.handedness == "Right":
+                    x = current_right['position'][0] + dx
+                    y = current_right['position'][1] + dy
+                    z = current_right['position'][2] + dz
+                    base_orientation = current_right['orientation']
+                else:
+                    continue
+                
+                # # Clamp to safe workspace bounds to avoid IK failures
+                # x = max(0.2, min(0.6, x))  # 20-60cm forward from base
+                # y = max(-0.4, min(0.4, y))  # ±40cm left/right from base
+                # z = max(0.5, min(1.0, z))   # 50-100cm height from base
+                
+                print(f"Target position {hand.handedness}: ({x:.3f}, {y:.3f}, {z:.3f}) [clamped to workspace]")
+                
+                # Use fixed, simple orientation to avoid orientation IK failures
+                qx, qy, qz, qw = base_orientation
+                
+                # Create pose dictionary for robot
+                pose_dict = {
+                    'position': [x, y, z],
+                    'orientation': [qx, qy, qz, qw]
+                }
+                
+                if hand.handedness == "Left":
+                    left_pose = pose_dict
+                elif hand.handedness == "Right":
+                    right_pose = pose_dict
+                    
+        except Exception as e:
+            print(f"Warning: Failed to convert hand landmarks to poses: {e}")
+        
+        return left_pose, right_pose
+    
+    def send_hand_poses(self, left_pose: Optional[Dict], right_pose: Optional[Dict], 
+                       execution_time: float = 1.0, use_motion_planning: bool = True,
+                       use_individual_groups: bool = False) -> bool:
+        """
+        Send target poses for hands using MoveIt planning groups.
+        
+        Args:
+            left_pose: Target pose for left hand (None to ignore)
+            right_pose: Target pose for right hand (None to ignore)
+            execution_time: Time to execute the movement
+            use_motion_planning: If True, use full MoveIt pipeline; if False, use direct IK
+            use_individual_groups: If True, use left_arm+right_arm groups; if False, use dual_arm group
+            
+        Returns:
+            True if successful (dual_arm) or at least one arm succeeded (individual)
+        """
+        if not self.is_connected:
+            print("❌ Robot not connected")
+            return False
+        
+        # Handle async call
+        import asyncio
+        try:
+            loop = asyncio.get_event_loop()
+        except RuntimeError:
+            loop = asyncio.new_event_loop()
+            asyncio.set_event_loop(loop)
+        
+        if use_individual_groups:
+            # Use individual planning groups (left_arm + right_arm)
+            if not (left_pose or right_pose):
+                print("⚠️ Need at least one hand pose for individual groups")
+                return False
+            
+            print("🎯 Using individual planning groups (left_arm + right_arm)...")
+            success_count = 0
+            total_attempts = 0
+            
+            # Send left arm command if left pose available
+            if left_pose:
+                print("  🎯 Sending left_arm command...")
+                total_attempts += 1
+                try:
+                    left_success = loop.run_until_complete(
+                        self.send_single_arm_pose("left_arm", "left_tcp", left_pose, execution_time, use_motion_planning)
+                    )
+                    if left_success:
+                        success_count += 1
+                        print("  ✅ Left arm command succeeded")
+                    else:
+                        print("  ❌ Left arm command failed")
+                except Exception as e:
+                    print(f"  ❌ Left arm command error: {e}")
+            
+            # Send right arm command if right pose available
+            if right_pose:
+                print("  🎯 Sending right_arm command...")
+                total_attempts += 1
+                try:
+                    right_success = loop.run_until_complete(
+                        self.send_single_arm_pose("right_arm", "right_tcp", right_pose, execution_time, use_motion_planning)
+                    )
+                    if right_success:
+                        success_count += 1
+                        print("  ✅ Right arm command succeeded")
+                    else:
+                        print("  ❌ Right arm command failed")
+                except Exception as e:
+                    print(f"  ❌ Right arm command error: {e}")
+            
+            # Return success if at least one arm succeeded
+            overall_success = success_count > 0
+            print(f"📊 Individual groups result: {success_count}/{total_attempts} succeeded")
+            return overall_success
+            
+        else:
+            # Use original dual_arm planning group
+            if not (left_pose and right_pose):
+                print("⚠️ Dual arm planning group requires both hand poses")
+                return False
+            
+            print("🎯 Using dual_arm planning group...")
+            try:
+                return loop.run_until_complete(
+                    self.robot.send_dual_arm_pose(left_pose, right_pose, execution_time, use_motion_planning)
+                )
+            except Exception as e:
+                print(f"❌ Dual arm planning failed: {e}")
+                return False
+    
+    async def send_single_arm_pose(self, group_name: str, link_name: str, pose: Dict, 
+                                  execution_time: float = 1.0, use_motion_planning: bool = True) -> bool:
+        """
+        Send target pose for a single arm using MoveIt.
+        
+        Args:
+            group_name: MoveIt planning group name ("left_arm" or "right_arm")
+            link_name: End effector link name ("left_tcp" or "right_tcp")
+            pose: Target pose dictionary with 'position' and 'orientation'
+            execution_time: Time to execute the movement
+            use_motion_planning: If True, use full MoveIt pipeline; if False, use direct IK
+            
+        Returns:
+            True if successful
+        """
+        try:
+            if use_motion_planning:
+                # Skip IK solving and send pose directly to MoveIt for non-blocking execution
+                success = await self.robot._execute_single_arm_moveit_planning(group_name, link_name, pose, execution_time)
+                return success
+            else:
+                # For direct execution, we still need IK but make it faster
+                current_state = await self.robot.get_current_robot_state()
+                ik_solution = await self.robot.solve_single_arm_ik(group_name, link_name, pose, current_state)
+                
+                if ik_solution is None:
+                    print(f"❌ No IK solution found for {group_name}")
+                    return False
+                
+                # Extract joint commands from IK solution for direct execution
+                arm_commands = {}
+                for i, joint_name in enumerate(ik_solution.joint_state.name):
+                    # Filter for arm and torso joints only
+                    if joint_name in (self.robot.config.left_arm_joints + 
+                                    self.robot.config.right_arm_joints + 
+                                    self.robot.config.torso_joints):
+                        arm_commands[joint_name] = ik_solution.joint_state.position[i]
+                
+                # Send joint commands directly
+                if arm_commands:
+                    print(f"🔧 Sending {len(arm_commands)} joint commands for {group_name}")
+                    self.robot._send_direct_joint_commands(self.robot.config.torso_controller, arm_commands)
+                    return True
+                else:
+                    print(f"❌ No valid joint commands extracted for {group_name}")
+                    return False
+            
+        except Exception as e:
+            print(f"❌ Single arm pose failed for {group_name}: {e}")
+            return False
+
+    def get_current_state(self) -> Optional[RobotState]:
+        """Get current robot state."""
+        try:
+            # Get all joint names
+            all_joints = (
+                self.config.left_arm_joints +
+                self.config.right_arm_joints +
+                self.config.torso_joints +
+                self.config.left_finger_joints +
+                self.config.right_finger_joints
+            )
+            
+            # Get current joint positions from robot's internal state
+            current_positions = []
+            for joint in all_joints:
+                # Access the robot's internal joint position tracking
+                pos = self.robot._joint_positions.get(joint, 0.0)
+                current_positions.append(pos)
+            
+            return RobotState(
+                joint_positions=np.array(current_positions),
+                joint_velocities=np.zeros(len(all_joints)),  # Could get from _joint_velocities
+                joint_names=all_joints,
+                timestamp=time.time(),
+                is_moving=False  # Could check if any velocities > threshold
+            )
+        except Exception as e:
+            print(f"Warning: Could not get robot state: {e}")
+            return None
+    
+    def get_joint_names(self) -> List[str]:
+        """Get ordered list of joint names."""
+        return (
+            self.config.left_arm_joints +
+            self.config.right_arm_joints +
+            self.config.torso_joints +
+            self.config.left_finger_joints +
+            self.config.right_finger_joints
+        )
+    
+    def shutdown(self):
+        """Shutdown the robot interface."""
+        try:
+            # Disconnect robot (robot handles its own ROS cleanup)
+            self.robot.disconnect()
+        except Exception as e:
+            print(f"Warning during robot disconnect: {e}")
+        
+        print("🔌 Robot interface shutdown")
+
+    def send_combined_command(self, left_pose: Optional[Dict], right_pose: Optional[Dict], 
+                             finger_commands: Optional[Any], execution_time: float = 1.0,
+                             use_motion_planning: bool = False, use_individual_groups: bool = False) -> bool:
+        """
+        Send both arm motion and finger commands together.
+        
+        Args:
+            left_pose: Target pose for left hand (None to ignore)
+            right_pose: Target pose for right hand (None to ignore)
+            finger_commands: RobotJointState with finger positions
+            execution_time: Time to execute the movement
+            use_motion_planning: If True, use full MoveIt pipeline; if False, use direct IK
+            use_individual_groups: If True, use left_arm+right_arm; if False, use dual_arm
+            
+        Returns:
+            True if at least one command succeeded
+        """
+        if not self.is_connected:
+            print("❌ Robot not connected")
+            return False
+        
+        group_type = "individual groups" if use_individual_groups else "dual_arm group"
+        print(f"🚀 Sending combined command ({group_type} + fingers)...")
+        success_count = 0
+        total_attempts = 0
+        
+        # Send arm motion using selected planning approach
+        if left_pose or right_pose:
+            if use_motion_planning:
+                print(f"  🎯 Sending arm motion with MoveIt planning ({group_type})...")
+            else:
+                print(f"  🔧 Sending arm motion with direct IK ({group_type})...")
+            total_attempts += 1
+            if self.send_hand_poses(left_pose, right_pose, execution_time, use_motion_planning, use_individual_groups):
+                success_count += 1
+                print("  ✅ Arm motion command sent")
+            else:
+                print("  ❌ Arm motion command failed")
+        else:
+            print("  ⚠️ Skipping arm motion (no hand poses provided)")
+        
+        # Send finger commands if available
+        if finger_commands:
+            print("  🤏 Sending finger commands...")
+            total_attempts += 1
+            if self.send_joint_command(finger_commands, execution_time, fingers_only=True):
+                success_count += 1
+                print("  ✅ Finger commands sent")
+            else:
+                print("  ❌ Finger commands failed")
+        else:
+            print("  ⚠️ No finger commands to send")
+        
+        result = success_count > 0
+        print(f"📊 Combined command result: {success_count}/{total_attempts} succeeded")
+        return result
+
+    def send_combined_command_async(self, left_pose: Optional[Dict], right_pose: Optional[Dict], 
+                                   finger_commands: Optional[Any], execution_time: float = 1.0,
+                                   use_motion_planning: bool = True, use_individual_groups: bool = False) -> bool:
+        """
+        Send both arm motion and finger commands together (non-blocking version for real-time control).
+        
+        This version is optimized for continuous data collection and won't block the main thread.
+        
+        Args:
+            left_pose: Target pose for left hand (None to ignore)
+            right_pose: Target pose for right hand (None to ignore)
+            finger_commands: RobotJointState with finger positions
+            execution_time: Time to execute the movement
+            use_motion_planning: If True, use full MoveIt pipeline; if False, use direct IK
+            use_individual_groups: If True, use left_arm+right_arm; if False, use dual_arm
+            
+        Returns:
+            True if commands were queued successfully (not necessarily executed)
+        """
+        if not self.is_connected:
+            return False
+        
+        success_count = 0
+        total_attempts = 0
+        
+        # Send arm motion using direct action client calls (non-blocking)
+        if left_pose or right_pose and use_motion_planning:
+            total_attempts += 1
+            try:
+                if use_individual_groups:
+                    # Send individual arm commands directly to action clients
+                    if left_pose:
+                        success = self.robot._send_single_arm_goal_async("left_arm", "left_tcp", left_pose, execution_time)
+                        if success:
+                            print("🚀 Left arm goal queued")
+                    
+                    if right_pose:
+                        success = self.robot._send_single_arm_goal_async("right_arm", "right_tcp", right_pose, execution_time)
+                        if success:
+                            print("🚀 Right arm goal queued")
+                    
+                    success_count += 1
+                else:
+                    # For dual arm, just send one pose for now (simplified)
+                    if left_pose:
+                        success = self.robot._send_single_arm_goal_async("left_arm", "left_tcp", left_pose, execution_time)
+                        if success:
+                            success_count += 1
+                
+            except Exception as e:
+                print(f"⚠️ Arm command queuing failed: {e}")
+        
+        # Send finger commands (these are fast, so keep synchronous)
+        if finger_commands:
+            total_attempts += 1
+            if self.send_joint_command(finger_commands, execution_time, fingers_only=True):
+                success_count += 1
+        
+        return success_count > 0
+    
+
+if __name__ == "__main__":
+    # Test the robot interface with MoveIt
+    print("Testing Robot Interface with built-in MoveIt dual_arm IK...")
+    
+    try:
+        robot = RobotInterface(use_action_client=False)
+        
+        print("✅ Robot interface initialized successfully")
+        
+        # Get joint names
+        joint_names = robot.get_joint_names()
+        print(f"Robot has {len(joint_names)} joints")
+        
+        # Test finger-only control
+        test_state = RobotJointState(
+            joint_names=joint_names,
+            joint_positions=np.random.rand(len(joint_names)) * 0.3,  # Random small angles
+            timestamp=time.time()
+        )
+        
+        # Send finger-only command
+        print("\n🧪 Testing finger-only control...")
+        robot.send_joint_command(test_state, execution_time=2.0, fingers_only=True)
+        
+        # Test dual arm IK with example poses
+        print("\n🧪 Testing dual arm IK with example poses...")
+        left_pose = {
+            'position': [0.3, 0.3, 0.60],
+            'orientation': [0.0, 0.0, 0.0, 1.0]
+        }
+        right_pose = {
+            'position': [-0.3, 0.3, 0.60], 
+            'orientation': [0.0, 0.0, 0.0, 1.0]
+        }
+        
+        ik_success = robot.send_hand_poses(left_pose, right_pose, execution_time=3.0)
+        if ik_success:
+            print("✅ Dual arm IK test successful!")
+        else:
+            print("⚠️ Dual arm IK test failed (expected if MoveIt not fully configured)")
+        
+        # Get current state
+        current = robot.get_current_state()
+        if current:
+            print(f"\nCurrent robot state:")
+            print(f"  Joints: {len(current.joint_names)}")
+            print(f"  Timestamp: {current.timestamp}")
+        
+        time.sleep(2)
+        robot.shutdown()
+        
+    except Exception as e:
+        print(f"❌ Failed to initialize robot interface: {e}")
+        print("Make sure Gazebo and MoveIt are running with the robot!")
+        import traceback
+        traceback.print_exc()