Degrees Of Freedom

1# A rigid body in 3D has 6 Degrees of Freedom
2# 3 Translation (X, Y, Z) and 3 Rotation (Roll, Pitch, Yaw)
3
4class RigidBody:
5    def __init__(self):
6        self.position = [0.0, 0.0, 0.0]  # X, Y, Z
7        self.rotation = [0.0, 0.0, 0.0]  # Rx, Ry, Rz
8        
9    def transform_matrix(self):
10        # Standard 4x4 homogenous translation matrix using position coordinates
11        tx, ty, tz = self.position
12        return [
13            [1.0, 0.0, 0.0, tx],
14            [0.0, 1.0, 0.0, ty],
15            [0.0, 0.0, 1.0, tz],
16            [0.0, 0.0, 0.0, 1.0]
17        ]

1# Prismatic (Translational) Joint
2# Constrains 5 DOFs, leaving only 1 translational DOF.
3
4class PrismaticJoint:
5    def __init__(self, axis=[1, 0, 0]):
6        self.axis = axis
7        self.translation = 0.0  # The single degree of freedom
8        
9    def get_position(self):
10        return [self.translation * a for a in self.axis]

1# Revolute (Rotational) Joint
2# Constrains 5 DOFs, leaving 1 rotational DOF.
3
4class RevoluteJoint:
5    def __init__(self, axis=[0, 1, 0]):
6        self.axis = axis
7        self.angle = 0.0  # The single rotational degree of freedom
8        
9    def get_rotation_matrix(self):
10        # Returns a rotation matrix around the Y axis
11        # by self.angle radians.
12        import math
13        c = math.cos(self.angle)
14        s = math.sin(self.angle)
15        return [
16            [c, 0, s],
17            [0, 1, 0],
18            [-s, 0, c]
19        ]

1# Spherical Joint (Ball and Socket)
2# Constrains 3 translational DOFs, leaving 3 rotational DOFs.
3
4class SphericalJoint:
5    def __init__(self):
6        self.angles = [0.0, 0.0, 0.0]  # Pitch, Yaw, Roll
7        
8    def get_rotation_matrix(self):
9        # Applies Euler rotation sequence (Z-Y-X) using self.angles
10        import math
11        rx, ry, rz = self.angles
12        cx, sx = math.cos(rx), math.sin(rx)
13        cy, sy = math.cos(ry), math.sin(ry)
14        cz, sz = math.cos(rz), math.sin(rz)
15        return [
16            [cy*cz, -cy*sz, sy],
17            [cx*sz + sx*sy*cz, cx*cz - sx*sy*sz, -sx*cy],
18            [sx*sz - cx*sy*cz, sx*cz + cx*sy*sz, cx*cy]
19        ]

1# Kinematic Chain
2# Connecting multiple DOFs sequentially creates a robot arm.
3
4class KinematicChain:
5    def __init__(self):
6        self.joints = []
7        
8    def add_joint(self, joint):
9        self.joints.append(joint)
10        
11    def total_dof(self):
12        return sum(j.dof for j in self.joints)

1# Transform Accumulation
2# Calculating the end effector pose by multiplying matrices.
3
4def calculate_fk(chain_matrices):
5    T = IdentityMatrix()
6    for matrix in chain_matrices:
7        T = T * matrix  # Matrix multiplication
8    return T
9
10# The position of the final node is non-linear w.r.t the joint angles
11# due to trigonometric functions in the rotation matrices.

1# Clamping Joint Limits
2# Physical joints cannot rotate infinitely due to collisions and wires.
3
4def clamp(val, min_val, max_val):
5    return max(min_val, min(val, max_val))
6
7class LimitedJoint:
8    def __init__(self, limit_min=-90, limit_max=90):
9        self.min = limit_min
10        self.max = limit_max
11        self._angle = 0
12        
13    def set_angle(self, target):
14        # Enforce physical constraints
15        self._angle = clamp(target, self.min, self.max)