TASK 8.txt

import numpy as np

class RoboticArmFABRIK:
    def __init__(self, link_lengths, joint_limits):
        self.link_lengths = link_lengths
        self.joint_limits = joint_limits
    
    def check_reachability(self, initial_angles, target_position):
        joint_positions = self.calculate_joint_positions(np.radians(initial_angles))
        end_effector_position = joint_positions[-1]
        
        # Calculate distance between current end effector position and target position
        distance = np.linalg.norm(end_effector_position - np.array(target_position))
        
        # Compare distance with maximum reach (sum of link lengths)
        max_reach = np.sum(self.link_lengths)
        
        return distance <= max_reach
    
    def fabrik_algorithm(self, initial_angles, target_position, tolerance=1e-5, max_iterations=100):
        current_angles = np.radians(initial_angles)
        target_position = np.array(target_position)
        num_joints = len(initial_angles)
        
        # Check initial reachability
        if not self.check_reachability(initial_angles, target_position):
            print("Target position is not reachable.")
            return None
        
        # Initialize joint positions using forward kinematics
        joint_positions = self.calculate_joint_positions(current_angles)
        
        iteration = 0
        while iteration < max_iterations:
            # Backward reaching phase
            joint_positions[-1] = target_position
            for i in range(num_joints - 2, -1, -1):
                joint_positions[i] = self.backward_reach(joint_positions[i+1], joint_positions[i], self.link_lengths[i])
            
            # Forward reaching phase
            joint_positions[0] = np.array([0, 0, 0])  # Base joint position
            for i in range(num_joints - 1):
                joint_positions[i+1] = self.forward_reach(joint_positions[i], joint_positions[i+1], self.link_lengths[i])
            
            # Calculate current end effector position
            current_end_effector = joint_positions[-1]
            
            # Check convergence
            if np.linalg.norm(current_end_effector - target_position) < tolerance:
                break
            
            # Update joint angles
            current_angles = self.calculate_joint_angles(joint_positions)
            print(f"Iteration {iteration+1}: {np.degrees(current_angles)}")
            
            iteration += 1
        
        return np.degrees(current_angles)
    
    def forward_reach(self, start_point, end_point, link_length):
        direction = end_point - start_point
        current_distance = np.linalg.norm(direction)
        unit_direction = direction / current_distance
        return start_point + unit_direction * link_length
    
    def backward_reach(self, start_point, end_point, link_length):
        direction = start_point - end_point
        current_distance = np.linalg.norm(direction)
        unit_direction = direction / current_distance
        return start_point - unit_direction * link_length  # Corrected typo: should be `-` instead of `+`
    
    def calculate_joint_positions(self, joint_angles):
        joint_positions = [np.array([0, 0, 0])]  # Base joint position
        current_position = np.array([0, 0, 0])
        
        for i in range(len(joint_angles)):
            link_length = self.link_lengths[i]
            current_position += np.array([
                link_length * np.cos(np.sum(joint_angles[:i+1])),
                link_length * np.sin(np.sum(joint_angles[:i+1])),
                0
            ])
            joint_positions.append(current_position)
        
        return np.array(joint_positions)
    
    def calculate_joint_angles(self, joint_positions):
        joint_angles = []
        for i in range(1, len(joint_positions)):
            vec1 = joint_positions[i] - joint_positions[i-1]
            vec2 = np.array([1, 0, 0])  # X-axis
            angle = np.arccos(np.dot(vec1, vec2) / (np.linalg.norm(vec1) * np.linalg.norm(vec2)))
            joint_angles.append(angle)
        return np.array(joint_angles)
    
if __name__ == "__main__":
    # Define link lengths and joint constraints
    link_lengths = [23, 15, 1]  # Update with your specific roll number's last digit
    joint_limits = [(0, 180)] * 4  # Each joint has 180-degree freedom
    
    # Create robotic arm instance
    robotic_arm = RoboticArmFABRIK(link_lengths, joint_limits)
    
    # Example usage:
    initial_angles = [90, 90, 90, 90]  # Example initial joint angles (degrees)
    target_position = [30, 40, 0]      # Example target position (x, y, z)
    
    # Check reachability and compute optimal joint angles
    optimal_angles = robotic_arm.fabrik_algorithm(initial_angles, target_position)
    if optimal_angles:
        print(f"Optimal Joint Angles: {optimal_angles}")