import matplotlib.pyplot as plt
from scipy.optimize import minimize
def __init__(self, L1, L2, L3):
self.L = [L1, L2, L3] # Link lengths
def forward_kinematics(self, theta):
"""Calculate end-effector position"""
theta1, theta2, theta3 = theta
x = (self.L[0] * np.cos(theta1) +
self.L[1] * np.cos(theta1 + theta2) +
self.L[2] * np.cos(theta1 + theta2 + theta3))
y = (self.L[0] * np.sin(theta1) +
self.L[1] * np.sin(theta1 + theta2) +
self.L[2] * np.sin(theta1 + theta2 + theta3))
def jacobian(self, theta):
"""Calculate velocity Jacobian matrix"""
theta1, theta2, theta3 = theta
J11 = -(self.L[0] * np.sin(theta1) +
self.L[1] * np.sin(theta1 + theta2) +
self.L[2] * np.sin(theta1 + theta2 + theta3))
J12 = -(self.L[1] * np.sin(theta1 + theta2) +
self.L[2] * np.sin(theta1 + theta2 + theta3))
J13 = -self.L[2] * np.sin(theta1 + theta2 + theta3)
J21 = (self.L[0] * np.cos(theta1) +
self.L[1] * np.cos(theta1 + theta2) +
self.L[2] * np.cos(theta1 + theta2 + theta3))
J22 = (self.L[1] * np.cos(theta1 + theta2) +
self.L[2] * np.cos(theta1 + theta2 + theta3))
J23 = self.L[2] * np.cos(theta1 + theta2 + theta3)
return np.array([[J11, J12, J13],
def static_torques(self, theta, force):
"""Calculate required joint torques for given end-effector force"""
def manipulability(self, theta):
"""Calculate manipulability measure"""
return np.sqrt(np.linalg.det(J @ J.T))
robot = PlanarRobot3DOF(L1=0.4, L2=0.6, L3=0.2)
force_gravity = np.array([0, -payload_mass * g]) # Downward force
# Analyze workspace and torque requirements
theta_range = np.linspace(-np.pi/2, np.pi/2, 50)
manipulability_values = []
for theta1 in theta_range:
for theta2 in theta_range:
for theta3 in theta_range:
theta = [theta1, theta2, theta3]
# Check if configuration is reachable
torques = robot.static_torques(theta, force_gravity)
manip = robot.manipulability(theta)
max_torques.append(np.max(np.abs(torques)))
manipulability_values.append(manip)
except np.linalg.LinAlgError:
# Skip singular configurations
print(f"Maximum required torque: {np.max(max_torques):.1f} N⋅m")
print(f"Average torque requirement: {np.mean(max_torques):.1f} N⋅m")
print(f"Best manipulability: {np.max(manipulability_values):.3f}")
print(f"Worst manipulability: {np.min(manipulability_values):.3f}")
plt.figure(figsize=(12, 4))
plt.hist(max_torques, bins=30)
plt.xlabel('Maximum Joint Torque (N⋅m)')
plt.title('Torque Distribution')
plt.hist(manipulability_values, bins=30)
plt.xlabel('Manipulability')
plt.title('Manipulability Distribution')
plt.scatter(max_torques, manipulability_values, alpha=0.6)
plt.xlabel('Maximum Joint Torque (N⋅m)')
plt.ylabel('Manipulability')
plt.title('Torque vs. Manipulability')
Comments