具身智能感知系统：让机器人看懂世界

class RGBDPerception:
    def __init__(self):
        self.rgb_camera = RGBCamera()
        self.depth_camera = DepthCamera()
        self.calibrated = False

    def calibrate(self):
        """相机标定"""
        # 内参标定
        self.rgb_intrinsics = self.calibrate_intrinsics(self.rgb_camera)
        self.depth_intrinsics = self.calibrate_intrinsics(self.depth_camera)

        # 外参标定（RGB-D 对齐）
        self.extrinsics = self.calibrate_extrinsics(
            self.rgb_camera, self.depth_camera
        )

        self.calibrated = True

    def get_point_cloud(self, rgb_image, depth_image):
        """生成点云"""
        # 深度图转点云
        h, w = depth_image.shape
        points = []

        for v in range(h):
            for u in range(w):
                # 像素坐标转相机坐标
                z = depth_image[v, u] * self.depth_scale
                x = (u - self.cx) * z / self.fx
                y = (v - self.cy) * z / self.fy

                # 获取颜色
                color = rgb_image[v, u]

                points.append({
                    'position': [x, y, z],
                    'color': color
                })

        return points

class ObjectPerception:
    def __init__(self):
        self.detector = YOLOv8()
        self.segmentor = SAM()
        self.recognizer = CLIP()

    def detect_and_recognize(self, image):
        """检测并识别物体"""
        # 目标检测
        detections = self.detector.detect(image)

        results = []
        for det in detections:
            # 实例分割
            mask = self.segmentor.segment(image, det.bbox)

            # 物体识别
            object_class = self.recognizer.classify(image, mask)

            # 6D 位姿估计
            pose = self.estimate_pose(image, mask, det.bbox)

            results.append({
                'bbox': det.bbox,
                'mask': mask,
                'class': object_class,
                'confidence': det.confidence,
                'pose': pose
            })

        return results

    def estimate_pose(self, image, mask, bbox):
        """估计物体 6D 位姿"""
        # 使用 FoundationPose 或类似方法
        pose = self.pose_estimator.estimate(
            image=image,
            mask=mask,
            bbox=bbox
        )

        return pose

class SceneUnderstanding:
    def __init__(self):
        self.layout_estimator = LayoutEstimator()
        self.relationship_detector = RelationshipDetector()
        self.semantic_segmentor = SemanticSegmentor()

    def understand_scene(self, image, point_cloud):
        """理解场景语义"""
        # 语义分割
        semantic_map = self.semantic_segmentor.segment(image)

        # 场景布局
        layout = self.layout_estimator.estimate(point_cloud)

        # 物体关系
        relationships = self.relationship_detector.detect(
            image, semantic_map
        )

        # 场景图构建
        scene_graph = self.build_scene_graph(
            semantic_map, layout, relationships
        )

        return {
            'semantic_map': semantic_map,
            'layout': layout,
            'relationships': relationships,
            'scene_graph': scene_graph
        }

    def build_scene_graph(self, semantic_map, layout, relationships):
        """构建场景图"""
        graph = SceneGraph()

        # 添加节点（物体）
        for obj in semantic_map.objects:
            graph.add_node(obj.id, obj.properties)

        # 添加边（关系）
        for rel in relationships:
            graph.add_edge(rel.subject, rel.object, rel.predicate)

        return graph

class TactileSensor:
    def __init__(self, sensor_type='gelSight'):
        self.sensor_type = sensor_type
        self.resolution = (32, 32)  # 触觉阵列分辨率

    def read(self):
        """读取触觉数据"""
        if self.sensor_type == 'gelSight':
            return self.read_gelSight()
        elif self.sensor_type == 'tactile_array':
            return self.read_tactile_array()

    def read_gelSight(self):
        """读取 GelSight 触觉图像"""
        # GelSight 返回接触表面的变形图像
        tactile_image = self.capture_image()

        # 提取接触信息
        contact_mask = self.extract_contact_mask(tactile_image)
        contact_force = self.estimate_force(tactile_image)
        contact_shape = self.extract_shape(tactile_image)

        return {
            'image': tactile_image,
            'contact_mask': contact_mask,
            'force': contact_force,
            'shape': contact_shape
        }

    def read_tactile_array(self):
        """读取触觉阵列数据"""
        # 触觉阵列返回压力分布
        pressure_map = np.zeros(self.resolution)

        for i in range(self.resolution[0]):
            for j in range(self.resolution[1]):
                pressure_map[i, j] = self.read_taxel(i, j)

        return pressure_map

class TactileFeatureExtractor:
    def __init__(self):
        self.texture_analyzer = TextureAnalyzer()
        self.shape_analyzer = ShapeAnalyzer()
        self.slip_detector = SlipDetector()

    def extract_features(self, tactile_data):
        """提取触觉特征"""
        features = {}

        # 纹理特征
        features['texture'] = self.texture_analyzer.analyze(
            tactile_data['image']
        )

        # 形状特征
        features['shape'] = self.shape_analyzer.analyze(
            tactile_data['contact_mask']
        )

        # 滑动检测
        features['slip'] = self.slip_detector.detect(
            tactile_data['force']
        )

        # 硬度估计
        features['hardness'] = self.estimate_hardness(
            tactile_data['force'],
            tactile_data['deformation']
        )

        return features

    def estimate_hardness(self, force, deformation):
        """估计物体硬度"""
        # 硬度 = 力 / 变形量
        if deformation > 0:
            hardness = force / deformation
        else:
            hardness = float('inf')

        return hardness

class ForceTorqueSensor:
    def __init__(self, sensor_location='wrist'):
        self.location = sensor_location
        self.bias = np.zeros(6)  # [fx, fy, fz, tx, ty, tz]

    def calibrate(self):
        """传感器标定"""
        # 零点标定
        readings = []
        for _ in range(100):
            readings.append(self.read_raw())

        self.bias = np.mean(readings, axis=0)

    def read(self):
        """读取力/力矩数据"""
        raw = self.read_raw()

        # 去除偏置
        corrected = raw - self.bias

        # 坐标变换
        if self.location == 'wrist':
            corrected = self.transform_to_ee_frame(corrected)

        force = corrected[:3]
        torque = corrected[3:]

        return {
            'force': force,
            'torque': torque,
            'magnitude': np.linalg.norm(force)
        }

    def detect_contact(self, threshold=1.0):
        """检测接触"""
        reading = self.read()
        return reading['magnitude'] > threshold

class ForceController:
    def __init__(self, robot, force_sensor):
        self.robot = robot
        self.sensor = force_sensor
        self.Kp = np.diag([100, 100, 100])  # 比例增益
        self.Ki = np.diag([10, 10, 10])    # 积分增益
        self.Kd = np.diag([10, 10, 10])    # 微分增益

    def impedance_control(self, desired_force, desired_position):
        """阻抗控制"""
        # 读取实际力
        actual_force = self.sensor.read()['force']

        # 力误差
        force_error = desired_force - actual_force

        # 位置误差
        actual_position = self.robot.get_end_effector_position()
        position_error = desired_position - actual_position

        # 阻抗控制律
        # F = M*a + D*v + K*x
        M = np.diag([1, 1, 1])  # 惯性
        D = np.diag([10, 10, 10])  # 阻尼
        K = np.diag([100, 100, 100])  # 刚度

        # 计算期望加速度
        acceleration = np.linalg.inv(M) @ (
            force_error - D @ self.robot.get_velocity() - K @ position_error
        )

        # 转换为关节力矩
        jacobian = self.robot.get_jacobian()
        joint_torques = jacobian.T @ (M @ acceleration + force_error)

        return joint_torques

    def hybrid_control(self, task_frame, force_direction, position_direction):
        """混合力/位控制"""
        # 在力控制方向：控制力
        # 在位置控制方向：控制位置

        # 选择矩阵
        Sf = np.diag(force_direction)  # 力控制方向
        Sp = np.diag(position_direction)  # 位置控制方向

        # 读取传感器
        actual_force = self.sensor.read()['force']
        actual_position = self.robot.get_end_effector_position()

        # 力控制部分
        force_error = Sf @ (self.desired_force - actual_force)
        force_control = self.Kp @ force_error

        # 位置控制部分
        position_error = Sp @ (self.desired_position - actual_position)
        position_control = self.Kp @ position_error

        # 合并控制
        total_control = force_control + position_control

        return total_control

class JointStatePerception:
    def __init__(self, robot):
        self.robot = robot
        self.encoders = robot.joint_encoders
        self.motor_drivers = robot.motor_drivers

    def get_joint_state(self):
        """获取关节状态"""
        state = {
            'positions': [],
            'velocities': [],
            'torques': [],
            'temperatures': []
        }

        for i in range(self.robot.num_joints):
            # 关节角度
            pos = self.encoders[i].read_position()

            # 关节速度
            vel = self.encoders[i].read_velocity()

            # 电机力矩
            torque = self.motor_drivers[i].read_torque()

            # 电机温度
            temp = self.motor_drivers[i].read_temperature()

            state['positions'].append(pos)
            state['velocities'].append(vel)
            state['torques'].append(torque)
            state['temperatures'].append(temp)

        return state

    def detect_anomaly(self, state):
        """检测异常状态"""
        anomalies = []

        # 检查温度过高
        for i, temp in enumerate(state['temperatures']):
            if temp > self.robot.max_temperature:
                anomalies.append({
                    'type': 'overheat',
                    'joint': i,
                    'value': temp
                })

        # 检查力矩过大
        for i, torque in enumerate(state['torques']):
            if abs(torque) > self.robot.max_torque:
                anomalies.append({
                    'type': 'over_torque',
                    'joint': i,
                    'value': torque
                })

        return anomalies

class BasePerception:
    def __init__(self, robot):
        self.robot = robot
        self.imu = robot.imu
        self.odometry = robot.odometry

    def get_base_state(self):
        """获取基座状态"""
        # IMU 数据
        imu_data = self.imu.read()

        # 里程计数据
        odom_data = self.odometry.read()

        # 融合状态估计
        state = self.fuse_sensors(imu_data, odom_data)

        return {
            'position': state['position'],
            'orientation': state['orientation'],
            'linear_velocity': state['linear_velocity'],
            'angular_velocity': state['angular_velocity'],
            'acceleration': imu_data['acceleration']
        }

    def fuse_sensors(self, imu_data, odom_data):
        """传感器融合"""
        # 使用扩展卡尔曼滤波（EKF）
        ekf = ExtendedKalmanFilter()

        # 预测步骤
        ekf.predict(imu_data['acceleration'], imu_data['angular_velocity'])

        # 更新步骤
        ekf.update(odom_data['position'], odom_data['orientation'])

        return ekf.get_state()

class SensorFusion:
    def __init__(self):
        self.sensors = {}
        self.time_sync = TimeSynchronizer()
        self.spatial_align = SpatialAligner()

    def register_sensor(self, name, sensor, extrinsics):
        """注册传感器"""
        self.sensors[name] = {
            'sensor': sensor,
            'extrinsics': extrinsics,
            'buffer': deque(maxlen=100)
        }

    def fuse(self, timestamp):
        """融合多传感器数据"""
        # 时间同步
        synced_data = {}
        for name, sensor_info in self.sensors.items():
            # 获取最近的数据
            data = self.get_nearest_data(
                sensor_info['buffer'], timestamp
            )
            synced_data[name] = data

        # 空间对齐
        aligned_data = {}
        for name, data in synced_data.items():
            # 转换到统一坐标系
            aligned = self.spatial_align.transform(
                data, self.sensors[name]['extrinsics']
            )
            aligned_data[name] = aligned

        # 数据融合
        fused = self.fuse_data(aligned_data)

        return fused

class SemanticFusion:
    def __init__(self):
        self.visual_features = None
        self.tactile_features = None
        self.force_features = None

    def fuse(self, visual, tactile, force):
        """融合多模态语义"""
        # 提取各模态特征
        v_feat = self.extract_visual_features(visual)
        t_feat = self.extract_tactile_features(tactile)
        f_feat = self.extract_force_features(force)

        # 特征拼接
        combined = np.concatenate([v_feat, t_feat, f_feat])

        # 跨模态注意力
        attended = self.cross_modal_attention(
            v_feat, t_feat, f_feat
        )

        # 语义理解
        semantic = self.semantic_understanding(attended)

        return {
            'visual': v_feat,
            'tactile': t_feat,
            'force': f_feat,
            'fused': attended,
            'semantic': semantic
        }

    def cross_modal_attention(self, v, t, f):
        """跨模态注意力机制"""
        # 计算注意力权重
        attn_vt = self.attention(v, t)
        attn_vf = self.attention(v, f)
        attn_tf = self.attention(t, f)

        # 加权融合
        fused = (
            attn_vt * (v + t) +
            attn_vf * (v + f) +
            attn_tf * (t + f)
        ) / 3

        return fused

class GraspPerception:
    def __init__(self):
        self.visual = VisualPerception()
        self.tactile = TactilePerception()
        self.grasp_planner = GraspPlanner()

    def perceive_for_grasp(self, target_object):
        """为抓取任务感知"""
        # 视觉感知：物体位姿
        object_pose = self.visual.estimate_pose(target_object)

        # 视觉感知：抓取点候选
        grasp_candidates = self.visual.detect_grasp_points(
            target_object
        )

        # 触觉感知：表面属性
        surface_properties = self.tactile.sense_surface(
            target_object
        )

        # 选择最佳抓取点
        best_grasp = self.grasp_planner.select_grasp(
            grasp_candidates,
            surface_properties,
            object_pose
        )

        return {
            'object_pose': object_pose,
            'grasp_point': best_grasp,
            'surface_properties': surface_properties
        }

class NavigationPerception:
    def __init__(self):
        self.lidar = LidarSensor()
        self.camera = CameraArray()
        self.map_builder = MapBuilder()

    def perceive_for_navigation(self):
        """为导航任务感知"""
        # 障碍物检测
        obstacles = self.detect_obstacles()

        # 可通行区域
        traversable = self.detect_traversable_areas()

        # 定位
        position = self.localize()

        # 地图更新
        self.map_builder.update(obstacles, traversable, position)

        return {
            'obstacles': obstacles,
            'traversable': traversable,
            'position': position,
            'map': self.map_builder.get_map()
        }

    def detect_obstacles(self):
        """检测障碍物"""
        # 激光雷达点云
        point_cloud = self.lidar.scan()

        # 聚类分割
        clusters = self.cluster_points(point_cloud)

        # 障碍物分类
        obstacles = []
        for cluster in clusters:
            obstacle = {
                'position': cluster.centroid,
                'size': cluster.bounding_box,
                'type': self.classify_obstacle(cluster)
            }
            obstacles.append(obstacle)

        return obstacles