Mathematical Utilities

def getQuaternionFromEuler(eulerAngles):
    """
    Convert Euler angles to quaternion.
    
    Args:
        eulerAngles (list): Euler angles [roll, pitch, yaw] in radians
    
    Returns:
        list: Quaternion [x, y, z, w]
    """

def getEulerFromQuaternion(quaternion):
    """
    Convert quaternion to Euler angles.
    
    Args:
        quaternion (list): Quaternion [x, y, z, w]
    
    Returns:
        list: Euler angles [roll, pitch, yaw] in radians
    """

def getMatrixFromQuaternion(quaternion):
    """
    Get 3x3 rotation matrix from quaternion.
    
    Args:
        quaternion (list): Quaternion [x, y, z, w]
    
    Returns:
        list: 3x3 rotation matrix as list of 9 elements (row-major order)
    """

def getQuaternionSlerp(quaternionStart, quaternionEnd, interpolationFraction):
    """
    Spherical linear interpolation between two quaternions.
    
    Args:
        quaternionStart (list): Starting quaternion [x, y, z, w]
        quaternionEnd (list): Ending quaternion [x, y, z, w]
        interpolationFraction (float): Interpolation factor (0.0 to 1.0)
    
    Returns:
        list: Interpolated quaternion [x, y, z, w]
    """

def getQuaternionFromAxisAngle(axis, angle):
    """
    Get quaternion from axis-angle representation.
    
    Args:
        axis (list): Rotation axis [x, y, z] (normalized)
        angle (float): Rotation angle in radians
    
    Returns:
        list: Quaternion [x, y, z, w]
    """

def getAxisAngleFromQuaternion(quaternion):
    """
    Get axis-angle representation from quaternion.
    
    Args:
        quaternion (list): Quaternion [x, y, z, w]
    
    Returns:
        tuple: (axis, angle) where axis is [x, y, z] and angle is in radians
    """

def getDifferenceQuaternion(quaternionStart, quaternionEnd):
    """
    Compute quaternion difference (rotation from start to end).
    
    Args:
        quaternionStart (list): Starting quaternion [x, y, z, w]
        quaternionEnd (list): Ending quaternion [x, y, z, w]
    
    Returns:
        list: Difference quaternion [x, y, z, w]
    """

def getAxisDifferenceQuaternion(quaternionStart, quaternionEnd):
    """
    Compute axis difference from two quaternions.
    
    Args:
        quaternionStart (list): Starting quaternion [x, y, z, w]
        quaternionEnd (list): Ending quaternion [x, y, z, w]
    
    Returns:
        list: Axis difference [x, y, z]
    """

def calculateVelocityQuaternion(quaternionStart, quaternionEnd, timeStep):
    """
    Compute angular velocity from quaternion change.
    
    Args:
        quaternionStart (list): Starting quaternion [x, y, z, w]
        quaternionEnd (list): Ending quaternion [x, y, z, w]
        timeStep (float): Time step in seconds
    
    Returns:
        list: Angular velocity [wx, wy, wz] in rad/s
    """

def rotateVector(vector, quaternion):
    """
    Rotate vector using quaternion.
    
    Args:
        vector (list): Vector to rotate [x, y, z]
        quaternion (list): Rotation quaternion [x, y, z, w]
    
    Returns:
        list: Rotated vector [x, y, z]
    """

def multiplyTransforms(positionA, orientationA, positionB, orientationB):
    """
    Multiply two transforms (compose transformations).
    
    Args:
        positionA (list): Position of first transform [x, y, z]
        orientationA (list): Orientation of first transform [x, y, z, w]
        positionB (list): Position of second transform [x, y, z]
        orientationB (list): Orientation of second transform [x, y, z, w]
    
    Returns:
        tuple: (resultPosition, resultOrientation) as combined transform
    """

def invertTransform(position, orientation):
    """
    Invert transform (position, orientation).
    
    Args:
        position (list): Transform position [x, y, z]
        orientation (list): Transform orientation [x, y, z, w]
    
    Returns:
        tuple: (invertedPosition, invertedOrientation) as inverted transform
    """

import pybullet as p
import math

# Convert Euler angles to quaternion
roll, pitch, yaw = 0.1, 0.2, 0.3  # radians
quaternion = p.getQuaternionFromEuler([roll, pitch, yaw])
print(f"Quaternion: {quaternion}")

# Convert back to Euler angles
euler_angles = p.getEulerFromQuaternion(quaternion)
print(f"Euler angles: {euler_angles}")

# Quaternion interpolation
quat_start = [0, 0, 0, 1]  # Identity quaternion
quat_end = p.getQuaternionFromEuler([0, 0, math.pi/2])  # 90 degree rotation
quat_mid = p.getQuaternionSlerp(quat_start, quat_end, 0.5)  # Halfway rotation

import pybullet as p

# Define two transforms
pos_A = [1, 0, 0]
orn_A = p.getQuaternionFromEuler([0, 0, math.pi/4])  # 45 degree rotation

pos_B = [0, 1, 0]
orn_B = p.getQuaternionFromEuler([0, 0, math.pi/4])  # Another 45 degree rotation

# Compose transforms (A then B)
pos_result, orn_result = p.multiplyTransforms(pos_A, orn_A, pos_B, orn_B)
print(f"Combined transform: position={pos_result}, orientation={orn_result}")

# Invert transform
pos_inv, orn_inv = p.invertTransform(pos_result, orn_result)
print(f"Inverted transform: position={pos_inv}, orientation={orn_inv}")

import pybullet as p

# Rotate a vector using quaternion
vector = [1, 0, 0]  # Unit vector along X-axis
rotation_quat = p.getQuaternionFromEuler([0, 0, math.pi/2])  # 90 degree Z-rotation

rotated_vector = p.rotateVector(vector, rotation_quat)
print(f"Rotated vector: {rotated_vector}")  # Should be approximately [0, 1, 0]

import pybullet as p
import time

# Calculate angular velocity from quaternion changes
quat_start = [0, 0, 0, 1]
quat_end = p.getQuaternionFromEuler([0, 0, 0.1])  # Small rotation
time_step = 1.0/240.0  # 240 Hz simulation

angular_vel = p.calculateVelocityQuaternion(quat_start, quat_end, time_step)
print(f"Angular velocity: {angular_vel} rad/s")

import pybullet as p

def transform_point_to_frame(point, frame_pos, frame_orn):
    """Transform a point from world frame to local frame."""
    # Get inverse transform of the frame
    inv_pos, inv_orn = p.invertTransform(frame_pos, frame_orn)
    
    # Apply inverse transform to the point
    local_point, _ = p.multiplyTransforms(inv_pos, inv_orn, point, [0, 0, 0, 1])
    return local_point

# Example: Transform world point to robot's local frame
world_point = [2, 3, 1]
robot_pos = [1, 1, 0]
robot_orn = p.getQuaternionFromEuler([0, 0, math.pi/4])

local_point = transform_point_to_frame(world_point, robot_pos, robot_orn)
print(f"Point in robot frame: {local_point}")

def interpolate_pose(start_pos, start_orn, end_pos, end_orn, fraction):
    """Interpolate between two poses smoothly."""
    # Linear interpolation for position
    interp_pos = [
        start_pos[i] + fraction * (end_pos[i] - start_pos[i])
        for i in range(3)
    ]
    
    # Spherical interpolation for orientation
    interp_orn = p.getQuaternionSlerp(start_orn, end_orn, fraction)
    
    return interp_pos, interp_orn

# Usage in control loop
start_pose = ([0, 0, 1], [0, 0, 0, 1])
end_pose = ([1, 1, 1], p.getQuaternionFromEuler([0, 0, math.pi/2]))

for t in range(100):
    fraction = t / 99.0
    pos, orn = interpolate_pose(*start_pose, *end_pose, fraction)
    # Set robot end effector to interpolated pose

def world_to_body_frame(world_pos, world_orn, body_pos, body_orn):
    """Convert world coordinates to body-relative coordinates."""
    body_inv_pos, body_inv_orn = p.invertTransform(body_pos, body_orn)
    relative_pos, relative_orn = p.multiplyTransforms(
        body_inv_pos, body_inv_orn, world_pos, world_orn
    )
    return relative_pos, relative_orn

def body_to_world_frame(relative_pos, relative_orn, body_pos, body_orn):
    """Convert body-relative coordinates to world coordinates."""
    world_pos, world_orn = p.multiplyTransforms(
        body_pos, body_orn, relative_pos, relative_orn
    )
    return world_pos, world_orn

def computeProjectionMatrix(left, right, bottom, top, nearVal, farVal):
    """
    Compute projection matrix from frustum parameters.
    
    Args:
        left (float): Left clipping plane
        right (float): Right clipping plane  
        bottom (float): Bottom clipping plane
        top (float): Top clipping plane
        nearVal (float): Near clipping plane distance
        farVal (float): Far clipping plane distance
    
    Returns:
        list: 4x4 projection matrix as list of 16 elements
    """

def computeProjectionMatrixFOV(fov, aspect, nearVal, farVal):
    """
    Compute projection matrix from field of view parameters.
    
    Args:
        fov (float): Vertical field of view in degrees
        aspect (float): Aspect ratio (width/height)
        nearVal (float): Near clipping plane distance
        farVal (float): Far clipping plane distance
    
    Returns:
        list: 4x4 projection matrix as list of 16 elements
    """

def computeViewMatrix(cameraEyePosition, cameraTargetPosition, cameraUpVector):
    """
    Compute view matrix from camera parameters.
    
    Args:
        cameraEyePosition (list): Camera position [x, y, z]
        cameraTargetPosition (list): Camera target position [x, y, z]
        cameraUpVector (list): Camera up vector [x, y, z]
    
    Returns:
        list: 4x4 view matrix as list of 16 elements
    """

def computeViewMatrixFromYawPitchRoll(cameraTargetPosition, distance, yaw, pitch, roll, upAxisIndex):
    """
    Compute view matrix from yaw, pitch, roll parameters.
    
    Args:
        cameraTargetPosition (list): Camera target position [x, y, z]
        distance (float): Distance from target to camera
        yaw (float): Yaw angle in degrees
        pitch (float): Pitch angle in degrees
        roll (float): Roll angle in degrees
        upAxisIndex (int): Up axis index (2 for Z-up)
    
    Returns:
        list: 4x4 view matrix as list of 16 elements
    """

import pybullet as p

# Perspective projection using FOV
width, height = 640, 480
fov, aspect, near, far = 60, width/height, 0.01, 100
proj_matrix = p.computeProjectionMatrixFOV(fov, aspect, near, far)

# View matrix from camera position
eye_pos = [2, 2, 2]
target_pos = [0, 0, 0]
up_vector = [0, 0, 1]
view_matrix = p.computeViewMatrix(eye_pos, target_pos, up_vector)

# Alternative view matrix using angles
view_matrix2 = p.computeViewMatrixFromYawPitchRoll(
    cameraTargetPosition=[0, 0, 0],
    distance=3.0,
    yaw=45,    # degrees
    pitch=-30, # degrees
    roll=0,    # degrees
    upAxisIndex=2  # Z-up
)