0
# Mathematical Utilities
1

2
Quaternion operations, coordinate transformations, and mathematical utilities for robotics and 3D graphics applications. Essential for pose representation, coordinate frame transformations, and spatial computations.
3

4
## Capabilities
5

6
### Quaternion Operations
7

8
```python { .api }
9
def getQuaternionFromEuler(eulerAngles):
10
    """
11
    Convert Euler angles to quaternion.
12
    
13
    Args:
14
        eulerAngles (list): Euler angles [roll, pitch, yaw] in radians
15
    
16
    Returns:
17
        list: Quaternion [x, y, z, w]
18
    """
19

20
def getEulerFromQuaternion(quaternion):
21
    """
22
    Convert quaternion to Euler angles.
23
    
24
    Args:
25
        quaternion (list): Quaternion [x, y, z, w]
26
    
27
    Returns:
28
        list: Euler angles [roll, pitch, yaw] in radians
29
    """
30

31
def getMatrixFromQuaternion(quaternion):
32
    """
33
    Get 3x3 rotation matrix from quaternion.
34
    
35
    Args:
36
        quaternion (list): Quaternion [x, y, z, w]
37
    
38
    Returns:
39
        list: 3x3 rotation matrix as list of 9 elements (row-major order)
40
    """
41

42
def getQuaternionSlerp(quaternionStart, quaternionEnd, interpolationFraction):
43
    """
44
    Spherical linear interpolation between two quaternions.
45
    
46
    Args:
47
        quaternionStart (list): Starting quaternion [x, y, z, w]
48
        quaternionEnd (list): Ending quaternion [x, y, z, w]
49
        interpolationFraction (float): Interpolation factor (0.0 to 1.0)
50
    
51
    Returns:
52
        list: Interpolated quaternion [x, y, z, w]
53
    """
54

55
def getQuaternionFromAxisAngle(axis, angle):
56
    """
57
    Get quaternion from axis-angle representation.
58
    
59
    Args:
60
        axis (list): Rotation axis [x, y, z] (normalized)
61
        angle (float): Rotation angle in radians
62
    
63
    Returns:
64
        list: Quaternion [x, y, z, w]
65
    """
66

67
def getAxisAngleFromQuaternion(quaternion):
68
    """
69
    Get axis-angle representation from quaternion.
70
    
71
    Args:
72
        quaternion (list): Quaternion [x, y, z, w]
73
    
74
    Returns:
75
        tuple: (axis, angle) where axis is [x, y, z] and angle is in radians
76
    """
77

78
def getDifferenceQuaternion(quaternionStart, quaternionEnd):
79
    """
80
    Compute quaternion difference (rotation from start to end).
81
    
82
    Args:
83
        quaternionStart (list): Starting quaternion [x, y, z, w]
84
        quaternionEnd (list): Ending quaternion [x, y, z, w]
85
    
86
    Returns:
87
        list: Difference quaternion [x, y, z, w]
88
    """
89

90
def getAxisDifferenceQuaternion(quaternionStart, quaternionEnd):
91
    """
92
    Compute axis difference from two quaternions.
93
    
94
    Args:
95
        quaternionStart (list): Starting quaternion [x, y, z, w]
96
        quaternionEnd (list): Ending quaternion [x, y, z, w]
97
    
98
    Returns:
99
        list: Axis difference [x, y, z]
100
    """
101

102
def calculateVelocityQuaternion(quaternionStart, quaternionEnd, timeStep):
103
    """
104
    Compute angular velocity from quaternion change.
105
    
106
    Args:
107
        quaternionStart (list): Starting quaternion [x, y, z, w]
108
        quaternionEnd (list): Ending quaternion [x, y, z, w]
109
        timeStep (float): Time step in seconds
110
    
111
    Returns:
112
        list: Angular velocity [wx, wy, wz] in rad/s
113
    """
114

115
def rotateVector(vector, quaternion):
116
    """
117
    Rotate vector using quaternion.
118
    
119
    Args:
120
        vector (list): Vector to rotate [x, y, z]
121
        quaternion (list): Rotation quaternion [x, y, z, w]
122
    
123
    Returns:
124
        list: Rotated vector [x, y, z]
125
    """
126
```
127

128
### Transform Operations
129

130
```python { .api }
131
def multiplyTransforms(positionA, orientationA, positionB, orientationB):
132
    """
133
    Multiply two transforms (compose transformations).
134
    
135
    Args:
136
        positionA (list): Position of first transform [x, y, z]
137
        orientationA (list): Orientation of first transform [x, y, z, w]
138
        positionB (list): Position of second transform [x, y, z]
139
        orientationB (list): Orientation of second transform [x, y, z, w]
140
    
141
    Returns:
142
        tuple: (resultPosition, resultOrientation) as combined transform
143
    """
144

145
def invertTransform(position, orientation):
146
    """
147
    Invert transform (position, orientation).
148
    
149
    Args:
150
        position (list): Transform position [x, y, z]
151
        orientation (list): Transform orientation [x, y, z, w]
152
    
153
    Returns:
154
        tuple: (invertedPosition, invertedOrientation) as inverted transform
155
    """
156
```
157

158
## Usage Examples
159

160
### Basic Quaternion Operations
161

162
```python
163
import pybullet as p
164
import math
165

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

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

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

181
### Transform Composition
182

183
```python
184
import pybullet as p
185

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

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

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

197
# Invert transform
198
pos_inv, orn_inv = p.invertTransform(pos_result, orn_result)
199
print(f"Inverted transform: position={pos_inv}, orientation={orn_inv}")
200
```
201

202
### Vector Rotation
203

204
```python
205
import pybullet as p
206

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

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

215
### Angular Velocity Calculation
216

217
```python
218
import pybullet as p
219
import time
220

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

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

230
### Coordinate Frame Transformations
231

232
```python
233
import pybullet as p
234

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

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

249
local_point = transform_point_to_frame(world_point, robot_pos, robot_orn)
250
print(f"Point in robot frame: {local_point}")
251
```
252

253
## Common Patterns
254

255
### Pose Interpolation for Smooth Motion
256

257
```python
258
def interpolate_pose(start_pos, start_orn, end_pos, end_orn, fraction):
259
    """Interpolate between two poses smoothly."""
260
    # Linear interpolation for position
261
    interp_pos = [
262
        start_pos[i] + fraction * (end_pos[i] - start_pos[i])
263
        for i in range(3)
264
    ]
265
    
266
    # Spherical interpolation for orientation
267
    interp_orn = p.getQuaternionSlerp(start_orn, end_orn, fraction)
268
    
269
    return interp_pos, interp_orn
270

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

275
for t in range(100):
276
    fraction = t / 99.0
277
    pos, orn = interpolate_pose(*start_pose, *end_pose, fraction)
278
    # Set robot end effector to interpolated pose
279
```
280

281
### Reference Frame Conversions
282

283
```python
284
def world_to_body_frame(world_pos, world_orn, body_pos, body_orn):
285
    """Convert world coordinates to body-relative coordinates."""
286
    body_inv_pos, body_inv_orn = p.invertTransform(body_pos, body_orn)
287
    relative_pos, relative_orn = p.multiplyTransforms(
288
        body_inv_pos, body_inv_orn, world_pos, world_orn
289
    )
290
    return relative_pos, relative_orn
291

292
def body_to_world_frame(relative_pos, relative_orn, body_pos, body_orn):
293
    """Convert body-relative coordinates to world coordinates."""
294
    world_pos, world_orn = p.multiplyTransforms(
295
        body_pos, body_orn, relative_pos, relative_orn
296
    )
297
    return world_pos, world_orn
298
```
299

300
### Camera Matrix Computation
301

302
```python { .api }
303
def computeProjectionMatrix(left, right, bottom, top, nearVal, farVal):
304
    """
305
    Compute projection matrix from frustum parameters.
306
    
307
    Args:
308
        left (float): Left clipping plane
309
        right (float): Right clipping plane  
310
        bottom (float): Bottom clipping plane
311
        top (float): Top clipping plane
312
        nearVal (float): Near clipping plane distance
313
        farVal (float): Far clipping plane distance
314
    
315
    Returns:
316
        list: 4x4 projection matrix as list of 16 elements
317
    """
318

319
def computeProjectionMatrixFOV(fov, aspect, nearVal, farVal):
320
    """
321
    Compute projection matrix from field of view parameters.
322
    
323
    Args:
324
        fov (float): Vertical field of view in degrees
325
        aspect (float): Aspect ratio (width/height)
326
        nearVal (float): Near clipping plane distance
327
        farVal (float): Far clipping plane distance
328
    
329
    Returns:
330
        list: 4x4 projection matrix as list of 16 elements
331
    """
332

333
def computeViewMatrix(cameraEyePosition, cameraTargetPosition, cameraUpVector):
334
    """
335
    Compute view matrix from camera parameters.
336
    
337
    Args:
338
        cameraEyePosition (list): Camera position [x, y, z]
339
        cameraTargetPosition (list): Camera target position [x, y, z]
340
        cameraUpVector (list): Camera up vector [x, y, z]
341
    
342
    Returns:
343
        list: 4x4 view matrix as list of 16 elements
344
    """
345

346
def computeViewMatrixFromYawPitchRoll(cameraTargetPosition, distance, yaw, pitch, roll, upAxisIndex):
347
    """
348
    Compute view matrix from yaw, pitch, roll parameters.
349
    
350
    Args:
351
        cameraTargetPosition (list): Camera target position [x, y, z]
352
        distance (float): Distance from target to camera
353
        yaw (float): Yaw angle in degrees
354
        pitch (float): Pitch angle in degrees
355
        roll (float): Roll angle in degrees
356
        upAxisIndex (int): Up axis index (2 for Z-up)
357
    
358
    Returns:
359
        list: 4x4 view matrix as list of 16 elements
360
    """
361
```
362

363
## Extended Usage Examples
364

365
### Camera Matrix Setup
366

367
```python
368
import pybullet as p
369

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

375
# View matrix from camera position
376
eye_pos = [2, 2, 2]
377
target_pos = [0, 0, 0]
378
up_vector = [0, 0, 1]
379
view_matrix = p.computeViewMatrix(eye_pos, target_pos, up_vector)
380

381
# Alternative view matrix using angles
382
view_matrix2 = p.computeViewMatrixFromYawPitchRoll(
383
    cameraTargetPosition=[0, 0, 0],
384
    distance=3.0,
385
    yaw=45,    # degrees
386
    pitch=-30, # degrees
387
    roll=0,    # degrees
388
    upAxisIndex=2  # Z-up
389
)
390
```