Medical Robotics

Surgical Robot Control

Forward Kinematics

import numpy as np
from scipy.optimize import minimize

class SurgicalRobotArm:
    def __init__(self, link_lengths, joint_limits):
        self.L = link_lengths
        self.limits = joint_limits

    def forward_kinematics(self, theta):
        x, y = 0, 0
        angle = 0
        for i in range(len(self.L)):
            angle += theta[i]
            x += self.L[i] * np.cos(angle)
            y += self.L[i] * np.sin(angle)
        return np.array([x, y])

    def inverse_kinematics(self, target):
        def cost(theta):
            return np.sum((self.forward_kinematics(theta) - target) ** 2)
        result = minimize(cost, np.zeros(len(self.L)), method='L-BFGS-B',
                          bounds=self.limits)
        return result.x

Path Planning

RRT (Rapidly-exploring Random Trees)

class RRTPlanner:
    def __init__(self, start, goal, max_iter=10000):
        self.tree = [start]
        self.goal = goal

    def plan(self):
        for _ in range(self.max_iter):
            q_rand = np.random.uniform(-10, 10, size=2)
            q_nearest = min(self.tree, key=lambda q: np.linalg.norm(q - q_rand))
            q_new = q_nearest + 0.5 * (q_rand - q_nearest) / np.linalg.norm(q_rand - q_nearest)
            self.tree.append(q_new)
            if np.linalg.norm(q_new - self.goal) < 0.1:
                return True
        return False

Haptic Feedback

Impedance Control

class HapticController:
    def __init__(self, K=500, B=50, M=1):
        self.K, self.B, self.M = K, B, M

    def compute_force(self, position, velocity, ext_force=np.zeros(3)):
        return -self.K * position - self.B * velocity + self.M * ext_force

Safety Considerations