Add CODEOWNERS file

[traversaro]

At least for me this is useful to be automatically added as a reviewer to new PRs, and avoid to miss them. I took inspiration from other gz-* repos, @scpeters I listed you as a mantainer but I do not know if we should add someone else, thanks!

[scpeters]

thanks!

[DIWAKAR-M]

import numpy as np

Forward Kinematics

def forward_kinematics(joint_angles):

DH parameters

d1 = 0.330
a2 = 0.035
a3 = 0.330
d4 = 0.035
d5 = 0.280
d6 = 0.080

# Joint angles
theta1, theta2, theta3, theta4, theta5, theta6 = joint_angles

# Homogeneous transformation matrices
T01 = np.array([[np.cos(theta1), -np.sin(theta1), 0, 0],
                [np.sin(theta1), np.cos(theta1), 0, 0],
                [0, 0, 1, d1],
                [0, 0, 0, 1]])

T12 = np.array([[np.cos(theta2), -np.sin(theta2), 0, a2*np.cos(theta2)],
                [np.sin(theta2), np.cos(theta2), 0, a2*np.sin(theta2)],
                [0, 0, 1, 0],
                [0, 0, 0, 1]])

T23 = np.array([[np.cos(theta3), -np.sin(theta3), 0, a3*np.cos(theta3)],
                [np.sin(theta3), np.cos(theta3), 0, a3*np.sin(theta3)],
                [0, 0, 1, 0],
                [0, 0, 0, 1]])

T34 = np.array([[np.cos(theta4), -np.sin(theta4), 0, 0],
                [0, 0, -1, -d4],
                [np.sin(theta4), np.cos(theta4), 0, 0],
                [0, 0, 0, 1]])

T45 = np.array([[np.cos(theta5), -np.sin(theta5), 0, 0],
                [0, 0, 1, -d5],
                [-np.sin(theta5), -np.cos(theta5), 0, 0],
                [0, 0, 0, 1]])

T56 = np.array([[np.cos(theta6), -np.sin(theta6), 0, 0],
                [0, 0, -1, -d6],
                [np.sin(theta6), np.cos(theta6), 0, 0],
                [0, 0, 0, 1]])

# Forward Kinematics transformation
T06 = np.dot(np.dot(np.dot(np.dot(np.dot(T01, T12), T23), T34), T45), T56)

# Extract position and orientation
position = T06[:3, 3]
orientation = T06[:3, :3]

return position, orientation

Inverse Kinematics

def inverse_kinematics(position, orientation):

DH parameters

d1 = 0.330
a2 = 0.035
a3 = 0.330
d4 = 0.035
d5 = 0.280
d6 = 0.080

# Inverse Kinematics calculations
theta1 = np.arctan2(position[1], position[0])
r = np.sqrt(position[0]**2 + position[1]**2) - a2
s = position[2] - d1
D = (r**2 + s**2 - a3**2 - d4**2 - d5**2 - d6**2) / (2*a3)
theta3 = np.arctan2(-np.sqrt(1 - D**2), D)
theta2 = np.arctan2(s, r) - np.arctan2(a3*np.sin(theta3), a3*np.cos(theta3))
theta4 = np.arctan2(orientation[2, 0], -orientation[2, 1])
theta5 = np.arctan2(np.sqrt(orientation[0, 2]**2 + orientation[1, 2]**2), orientation[2, 2])
theta6 = np.arctan2(-orientation[0, 2], orientation[1, 2])

# Convert angles to degrees
joint_angles = np.degrees([theta1, theta2, theta3, theta4, theta5, theta6])

return joint_angles

Example usage

joint_angles = [0, 0, 0, 0, 0, 0] # Initial joint angles position, orientation = forward_kinematics(joint_angles) print("Forward Kinematics:") print("Position:", position) print("Orientation:", orientation)

new_position = [0.5, 0.2, 0.4] # Desired position new_orientation = np.eye(3) # Desired orientation as a 3x3 identity matrix new_joint_angles = inverse_kinematics(new_position, new_orientation) print("\nInverse Kinematics:") print("Joint Angles (degrees):", new_joint_angles)