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

mapping/kinematic_mapper.py

@@ -0,0 +1,405 @@
+#!/usr/bin/env python3
+"""
+Kinematic mapper for converting hand landmarks to robot joint angles.
+
+This module implements the core mapping algorithms that translate human hand
+poses into robot joint configurations.
+"""
+
+import numpy as np
+import yaml
+from typing import Dict, List, Optional, Tuple
+from dataclasses import dataclass
+import math
+
+# Import hand landmarks from hand tracking module
+import sys
+from pathlib import Path
+sys.path.append(str(Path(__file__).parent.parent))
+from hand_tracking.hand_detector import HandLandmarks
+
+
+@dataclass
+class RobotJointState:
+    """Container for robot joint state."""
+    joint_names: List[str]
+    joint_positions: np.ndarray  # Shape: (27,) for 
+    timestamp: float
+
+
+class KinematicMapper:
+    """Maps human hand poses to robot joint configurations."""
+    
+    def __init__(self, config_path: Optional[str] = None, control_mode: str = "full_body"):
+        """
+        Initialize kinematic mapper with configuration.
+        
+        Args:
+            config_path: Path to configuration file
+            control_mode: Control mode - "full_body" or "fingers_only"
+        """
+        self.config = self._load_config(config_path)
+        self.joint_names = self._get_joint_names()
+        self.control_mode = control_mode
+        
+        # Previous joint states for smoothing
+        self.previous_joints = np.zeros(27)
+        
+    def _load_config(self, config_path: Optional[str]) -> Dict:
+        """Load configuration from YAML file."""
+        if config_path is None:
+            # Use default config path
+            config_path = Path(__file__).parent.parent.parent / "config" / "robot_mapping_config.yaml"
+        
+        if not Path(config_path).exists():
+            # Return default configuration
+            return self._get_default_config()
+            
+        with open(config_path, 'r') as f:
+            return yaml.safe_load(f)
+    
+    def _get_default_config(self) -> Dict:
+        """Get default configuration if file doesn't exist."""
+        return {
+            'robot_mapping': {
+                'mapping': {
+                    'workspace': {
+                        'x_range': [-0.8, 0.8],
+                        'y_range': [-0.6, 0.6],
+                        'z_range': [0.5, 1.5]
+                    },
+                    'smoothing': {
+                        'joint_alpha': 0.9
+                    }
+                },
+                'joint_limits': {
+                    'shoulder_pan': [-3.14, 3.14],
+                    'shoulder_lift': [-1.57, 1.57],
+                    'elbow_flex': [-2.61, 0.0],
+                    'finger_joint': [0.0, 1.57]
+                }
+            }
+        }
+    
+    def _get_joint_names(self) -> List[str]:
+        """Get ordered list of joint names."""
+        return [
+            # Left arm (7 DOF)
+            "left_shoulder_x", "left_shoulder_z", "left_shoulder_y",
+            "left_elbow_y", "left_wrist_z", "left_wrist_x", "left_wrist_y",
+            # Right arm (7 DOF)
+            "right_shoulder_x", "right_shoulder_z", "right_shoulder_y", 
+            "right_elbow_y", "right_wrist_z", "right_wrist_x", "right_wrist_y",
+            # Torso (1 DOF)
+            "Torso_lin_z",
+            # Left hand (6 DOF)
+            "left_thumb_finger_proximal_pitch", "left_thumb_finger_proximal_yaw",
+            "left_index_finger_proximal", "left_middle_finger_proximal", 
+            "left_ring_finger_proximal", "left_pinky_finger_proximal",
+            # Right hand (6 DOF)
+            "right_thumb_finger_proximal_pitch", "right_thumb_finger_proximal_yaw",
+            "right_index_finger_proximal", "right_middle_finger_proximal",
+            "right_ring_finger_proximal", "right_pinky_finger_proximal"
+        ]
+    
+    def set_control_mode(self, mode: str):
+        """
+        Set the control mode.
+        
+        Args:
+            mode: Control mode - "full_body" or "fingers_only"
+        """
+        valid_modes = ["full_body", "fingers_only"]
+        if mode not in valid_modes:
+            raise ValueError(f"Invalid control mode: {mode}. Valid modes: {valid_modes}")
+        self.control_mode = mode
+        print(f"Control mode set to: {mode}")
+
+    def map_hands_to_joints(self, hands: List[HandLandmarks]) -> RobotJointState:
+        """
+        Main mapping function: convert hand landmarks to robot joint angles.
+        
+        Args:
+            hands: List of detected hand landmarks
+            
+        Returns:
+            RobotJointState with joint positions for all 27 DOF
+        """
+        # Initialize joint positions
+        joint_positions = np.zeros(27)
+        
+        # Separate left and right hands
+        left_hand = None
+        right_hand = None
+        
+        for hand in hands:
+            if hand.handedness == "Left":
+                left_hand = hand
+            elif hand.handedness == "Right":
+                right_hand = hand
+        
+        # Apply control mode logic
+        if self.control_mode == "full_body":
+            # Map both arms and hands when detected
+            if left_hand is not None:
+                left_arm_joints = self._map_hand_to_arm(left_hand, "left")
+                left_finger_joints = self._map_hand_to_fingers(left_hand, "left")
+                joint_positions[0:7] = left_arm_joints  # Left arm
+                joint_positions[15:21] = left_finger_joints  # Left fingers
+            
+            if right_hand is not None:
+                right_arm_joints = self._map_hand_to_arm(right_hand, "right")
+                right_finger_joints = self._map_hand_to_fingers(right_hand, "right")
+                joint_positions[7:14] = right_arm_joints  # Right arm
+                joint_positions[21:27] = right_finger_joints  # Right fingers
+                
+        elif self.control_mode == "fingers_only":
+            # Only map fingers, keep arms at current position
+            if left_hand is not None:
+                left_finger_joints = self._map_hand_to_fingers(left_hand, "left")
+                joint_positions[15:21] = left_finger_joints  # Left fingers
+            
+            if right_hand is not None:
+                right_finger_joints = self._map_hand_to_fingers(right_hand, "right")
+                joint_positions[21:27] = right_finger_joints  # Right fingers
+        
+        # Set torso joint (index 14) - keep neutral for now
+        joint_positions[14] = 0.0
+        
+        # Apply smoothing
+        joint_positions = self._apply_smoothing(joint_positions)
+        
+        # Apply joint limits
+        joint_positions = self._apply_joint_limits(joint_positions)
+        
+        return RobotJointState(
+            joint_names=self.joint_names,
+            joint_positions=joint_positions,
+            timestamp=0.0  # Will be set by caller
+        )
+    
+    def _map_hand_to_arm(self, hand: HandLandmarks, side: str) -> np.ndarray:
+        """Map hand position and orientation to arm joint angles."""
+        landmarks = hand.landmarks
+        
+        # Get key points
+        wrist = landmarks[0]  # Wrist
+        
+        # Calculate hand position relative to camera
+        hand_pos = self._landmarks_to_world_position(wrist)
+        hand_orientation = self._calculate_hand_orientation(landmarks)
+        
+        # Simple mapping to arm joints
+        arm_joints = np.zeros(7)
+        
+        # Shoulder pan: based on hand x position
+        arm_joints[0] = np.clip(hand_pos[0] * 2.0, -1.5, 1.5)  # shoulder_pan
+        
+        # Shoulder lift: based on hand y position  
+        arm_joints[1] = np.clip(-hand_pos[1] * 1.5, -1.0, 1.0)  # shoulder_lift
+        
+        # Upper arm roll: based on hand orientation
+        arm_joints[2] = hand_orientation[2] * 0.5  # upper_arm_roll
+        
+        # Elbow flex: based on hand distance (closer = more bent)
+        hand_distance = np.linalg.norm(hand_pos)
+        elbow_flex = np.clip((1.0 - hand_distance) * 2.0, -2.0, 0.0)
+        arm_joints[3] = elbow_flex  # elbow_flex
+        
+        # Wrist joints: based on hand orientation
+        arm_joints[4] = hand_orientation[0] * 0.8  # forearm_roll
+        arm_joints[5] = hand_orientation[1] * 0.6  # wrist_flex  
+        arm_joints[6] = hand_orientation[2] * 0.4  # wrist_roll
+        
+        return arm_joints
+    
+    def _map_hand_to_fingers(self, hand: HandLandmarks, side: str) -> np.ndarray:
+        """Map hand landmarks to finger joint angles."""
+        landmarks = hand.landmarks
+        finger_joints = np.zeros(6)
+        
+        # Simple finger mapping based on finger curl
+        wrist = landmarks[0]  # Wrist
+        
+        # Thumb (2 DOF) - pitch and yaw
+        thumb_tip = landmarks[4]   # Thumb tip
+        thumb_mcp = landmarks[2]   # Thumb MCP
+        thumb_curl = self._calculate_finger_curl(wrist, thumb_mcp, thumb_tip)
+        finger_joints[0] = np.clip(thumb_curl * 3.0, 0.0, 1.5)  # thumb_proximal_pitch (increased scale)
+        finger_joints[1] = np.clip(thumb_curl * 2.0, 0.0, 1.5)  # thumb_proximal_yaw (increased scale)
+        
+        # Index finger (1 DOF)
+        index_tip = landmarks[8]   # Index tip
+        index_mcp = landmarks[5]   # Index MCP
+        index_curl = self._calculate_finger_curl(wrist, index_mcp, index_tip)
+        finger_joints[2] = np.clip(index_curl * 2.5, 0.0, 1.5)  # index_proximal (increased scale)
+        
+        # Middle finger (1 DOF)
+        middle_tip = landmarks[12]  # Middle tip
+        middle_mcp = landmarks[9]   # Middle MCP
+        middle_curl = self._calculate_finger_curl(wrist, middle_mcp, middle_tip)
+        finger_joints[3] = np.clip(middle_curl * 2.5, 0.0, 1.5)  # middle_proximal (increased scale)
+        
+        # Ring finger (1 DOF)
+        ring_tip = landmarks[16]   # Ring tip
+        ring_mcp = landmarks[13]   # Ring MCP
+        ring_curl = self._calculate_finger_curl(wrist, ring_mcp, ring_tip)
+        finger_joints[4] = np.clip(ring_curl * 2.5, 0.0, 1.5)  # ring_proximal (increased scale)
+        
+        # Pinky finger (1 DOF)
+        pinky_tip = landmarks[20]  # Pinky tip
+        pinky_mcp = landmarks[17]  # Pinky MCP
+        pinky_curl = self._calculate_finger_curl(wrist, pinky_mcp, pinky_tip)
+        finger_joints[5] = np.clip(pinky_curl * 2.5, 0.0, 1.5)  # pinky_proximal (increased scale)
+        
+        return finger_joints
+    
+    def _landmarks_to_world_position(self, wrist_landmark: np.ndarray) -> np.ndarray:
+        """Convert wrist landmark to world position."""
+        workspace = self.config['robot_mapping']['mapping']['workspace']
+        
+        # Map from image coordinates to world coordinates
+        x = (wrist_landmark[0] / 640.0 - 0.5) * 2.0  # Normalize to [-1, 1]
+        y = (wrist_landmark[1] / 480.0 - 0.5) * 2.0  # Normalize to [-1, 1]
+        z = wrist_landmark[2]  # Use MediaPipe's depth estimate
+        
+        # Scale to workspace
+        world_pos = np.array([
+            x * (workspace['x_range'][1] - workspace['x_range'][0]) / 2.0,
+            y * (workspace['y_range'][1] - workspace['y_range'][0]) / 2.0,
+            z * (workspace['z_range'][1] - workspace['z_range'][0]) / 2.0 + workspace['z_range'][0]
+        ])
+        
+        return world_pos
+    
+    def _calculate_hand_orientation(self, landmarks: np.ndarray) -> np.ndarray:
+        """Calculate hand orientation from landmarks."""
+        # Use wrist, middle MCP, and index MCP to define hand orientation
+        wrist = landmarks[0]
+        middle_mcp = landmarks[9]
+        index_mcp = landmarks[5]
+        
+        # Calculate vectors
+        palm_vector = middle_mcp - wrist
+        finger_vector = index_mcp - wrist
+        
+        # Calculate orientation angles (simplified)
+        roll = np.arctan2(palm_vector[1], palm_vector[0])
+        pitch = np.arctan2(palm_vector[2], np.linalg.norm(palm_vector[:2]))
+        yaw = np.arctan2(finger_vector[1], finger_vector[0])
+        
+        return np.array([roll, pitch, yaw])
+    
+    def _calculate_finger_curl(self, wrist, mcp, tip):
+        """Calculate finger curl based on distance ratios."""
+        # Calculate distances (landmarks are numpy arrays with [x, y, z])
+        wrist_to_mcp = np.sqrt((mcp[0] - wrist[0])**2 + (mcp[1] - wrist[1])**2)
+        mcp_to_tip = np.sqrt((tip[0] - mcp[0])**2 + (tip[1] - mcp[1])**2)
+        wrist_to_tip = np.sqrt((tip[0] - wrist[0])**2 + (tip[1] - wrist[1])**2)
+        
+        # Calculate expected distance if finger was straight
+        expected_distance = wrist_to_mcp + mcp_to_tip
+        
+        # Curl ratio: 0 = straight, 1 = fully curled
+        if expected_distance > 0:
+            curl_ratio = 1.0 - (wrist_to_tip / expected_distance)
+            # Apply exponential scaling to make small curls more noticeable
+            curl_ratio = curl_ratio ** 0.7  # Makes small values larger
+            return max(0.0, min(1.0, curl_ratio))
+        return 0.0
+    
+    def _calculate_finger_angles(self, landmarks: np.ndarray, finger_indices: List[int]) -> np.ndarray:
+        """Calculate finger joint angles from landmarks."""
+        if len(finger_indices) < 3:
+            return np.array([0.0])
+        
+        angles = []
+        
+        # Calculate angles between consecutive finger segments
+        for i in range(len(finger_indices) - 2):
+            p1 = landmarks[finger_indices[i]]
+            p2 = landmarks[finger_indices[i + 1]]
+            p3 = landmarks[finger_indices[i + 2]]
+            
+            # Calculate vectors
+            v1 = p1 - p2
+            v2 = p3 - p2
+            
+            # Calculate angle between vectors
+            cos_angle = np.dot(v1, v2) / (np.linalg.norm(v1) * np.linalg.norm(v2) + 1e-6)
+            angle = np.arccos(np.clip(cos_angle, -1.0, 1.0))
+            
+            # Convert to joint angle (0 = straight, positive = bent)
+            joint_angle = np.pi - angle
+            angles.append(joint_angle)
+        
+        return np.array(angles)
+    
+    def _apply_smoothing(self, joint_positions: np.ndarray) -> np.ndarray:
+        """Apply temporal smoothing to joint positions."""
+        alpha = self.config['robot_mapping']['mapping']['smoothing']['joint_alpha']
+        
+        # Exponential moving average
+        smoothed = alpha * self.previous_joints + (1 - alpha) * joint_positions
+        self.previous_joints = smoothed.copy()
+        
+        return smoothed
+    
+    def _apply_joint_limits(self, joint_positions: np.ndarray) -> np.ndarray:
+        """Apply joint limits to prevent invalid configurations."""
+        limits = self.config['robot_mapping']['joint_limits']
+        
+        # Apply limits to different joint types
+        for i, joint_name in enumerate(self.joint_names):
+            if 'shoulder_pan' in joint_name:
+                joint_positions[i] = np.clip(joint_positions[i], 
+                                           limits['shoulder_pan'][0], limits['shoulder_pan'][1])
+            elif 'shoulder_lift' in joint_name:
+                joint_positions[i] = np.clip(joint_positions[i], 
+                                           limits['shoulder_lift'][0], limits['shoulder_lift'][1])
+            elif 'elbow_flex' in joint_name:
+                joint_positions[i] = np.clip(joint_positions[i], 
+                                           limits['elbow_flex'][0], limits['elbow_flex'][1])
+            elif any(finger in joint_name for finger in ['thumb', 'index', 'middle', 'ring', 'pinky']):
+                joint_positions[i] = np.clip(joint_positions[i], 
+                                           limits['finger_joint'][0], limits['finger_joint'][1])
+        
+        return joint_positions
+    
+    def get_joint_names(self) -> List[str]:
+        """Get the ordered list of joint names."""
+        return self.joint_names.copy()
+    
+    def get_available_modes(self) -> List[str]:
+        """Get list of available control modes."""
+        return ["full_body", "fingers_only"]
+    
+    def get_current_mode(self) -> str:
+        """Get current control mode."""
+        return self.control_mode
+    
+    def is_ik_compatible(self) -> bool:
+        """Check if current mode is compatible with IK control."""
+        return self.control_mode == "full_body"
+    
+    def requires_both_hands(self) -> bool:
+        """Check if current mode requires both hands for IK."""
+        return False  # Individual planning groups work with any number of hands
+    
+    def print_joint_state(self, joint_state: RobotJointState):
+        """Print joint state in a readable format."""
+        print(f"Robot Joint State - Mode: {self.control_mode}")
+        print("-" * 50)
+        
+        for i, (name, pos) in enumerate(zip(joint_state.joint_names, joint_state.joint_positions)):
+            if abs(pos) > 0.001:  # Only print non-zero joints
+                print(f"{i:2d}. {name:<25} : {pos:6.3f} rad ({np.degrees(pos):6.1f}°)")
+
+
+if __name__ == "__main__":
+    # Test the kinematic mapper
+    mapper = KinematicMapper()
+    print("Kinematic mapper initialized successfully!")
+    print(f"Joint names: {len(mapper.get_joint_names())}")
+    for i, name in enumerate(mapper.get_joint_names()):
+        print(f"{i:2d}. {name}")