0
# Rotations
1

2
Complete system for representing and converting between rotation formats in three dimensions (SO(3)). Supports matrices, quaternions, axis-angles, Euler angles, Modified Rodrigues Parameters, and geometric algebra rotors with comprehensive conversion functions and mathematical operations.
3

4
## Capabilities
5

6
### Rotation Matrix Operations
7

8
Operations for 3x3 rotation matrices including validation, normalization, and conversions.
9

10
```python { .api }
11
def check_matrix(R, tolerance=1e-6, strict_check=True):
12
    """Validate rotation matrix properties."""
13

14
def norm_matrix(R):
15
    """Normalize rotation matrix using SVD."""
16

17
def matrix_from_euler(e, i, j, k, extrinsic):
18
    """
19
    Convert Euler angles to rotation matrix.
20
    
21
    Parameters:
22
    - e: array-like, shape (3,) - Euler angles in radians
23
    - i: int from [0, 1, 2] - First rotation axis (0: x, 1: y, 2: z)
24
    - j: int from [0, 1, 2] - Second rotation axis (0: x, 1: y, 2: z)
25
    - k: int from [0, 1, 2] - Third rotation axis (0: x, 1: y, 2: z)
26
    - extrinsic: bool - Extrinsic (True) or intrinsic (False) rotations
27
    
28
    Returns:
29
    - R: array, shape (3, 3) - Rotation matrix
30
    """
31

32
def matrix_from_quaternion(q):
33
    """Convert quaternion to rotation matrix."""
34

35
def matrix_from_axis_angle(a):
36
    """Convert axis-angle representation to rotation matrix."""
37

38
def matrix_from_compact_axis_angle(a):
39
    """Convert compact axis-angle to rotation matrix."""
40

41
def matrix_from_two_vectors(a, b):
42
    """Compute rotation matrix aligning vector a to vector b."""
43

44
def matrix_from_rotor(rotor):
45
    """Convert geometric algebra rotor to rotation matrix."""
46
```
47

48
### Quaternion Operations
49

50
Operations for unit quaternions including validation, composition, and interpolation.
51

52
```python { .api }
53
def check_quaternion(q, unit=True, tolerance=1e-6, strict_check=True):
54
    """Validate quaternion properties."""
55

56
def quaternion_from_matrix(R):
57
    """Convert rotation matrix to quaternion."""
58

59
def quaternion_from_euler(e, i, j, k, extrinsic):
60
    """Convert Euler angles to quaternion."""
61

62
def quaternion_from_axis_angle(a):
63
    """Convert axis-angle representation to quaternion."""
64

65
def quaternion_from_compact_axis_angle(a):
66
    """Convert compact axis-angle to quaternion."""
67

68
def concatenate_quaternions(q1, q2):
69
    """Compose two quaternions (q2 * q1)."""
70

71
def q_conj(q):
72
    """Quaternion conjugate."""
73

74
def q_prod_vector(q, v):
75
    """Rotate vector using quaternion."""
76

77
def quaternion_slerp(start, end, t):
78
    """Spherical linear interpolation between quaternions."""
79

80
def quaternion_integrate(q, omega, dt):
81
    """Integrate angular velocity to update quaternion."""
82

83
def quaternion_diff(q1, q2):
84
    """Difference between quaternions."""
85

86
def quaternion_dist(q1, q2):
87
    """Distance between quaternions."""
88

89
def quaternion_wxyz_from_xyzw(q):
90
    """Convert quaternion from [x,y,z,w] to [w,x,y,z] format."""
91

92
def quaternion_xyzw_from_wxyz(q):
93
    """Convert quaternion from [w,x,y,z] to [x,y,z,w] format."""
94

95
def quaternion_double(q):
96
    """
97
    Double quaternion angle (q^2 operation).
98
    
99
    Parameters:
100
    - q: array, shape (4,) - Input quaternion
101
    
102
    Returns:
103
    - q2: array, shape (4,) - Doubled quaternion
104
    """
105

106
def quaternion_gradient(q, omega):
107
    """
108
    Compute quaternion time derivative from angular velocity.
109
    
110
    Parameters:
111
    - q: array, shape (4,) - Current quaternion
112
    - omega: array, shape (3,) - Angular velocity
113
    
114
    Returns:
115
    - dq_dt: array, shape (4,) - Quaternion time derivative
116
    """
117

118
def pick_closest_quaternion(q, quaternions):
119
    """
120
    Pick closest quaternion from list (handles q/-q ambiguity).
121
    
122
    Parameters:
123
    - q: array, shape (4,) - Reference quaternion
124
    - quaternions: array, shape (n, 4) - Candidate quaternions
125
    
126
    Returns:
127
    - closest_q: array, shape (4,) - Closest quaternion
128
    """
129
```
130

131
### Axis-Angle Operations
132

133
Operations for axis-angle representations including compact and full forms.
134

135
```python { .api }
136
def check_axis_angle(a, tolerance=1e-6, strict_check=True):
137
    """Validate axis-angle representation."""
138

139
def check_compact_axis_angle(a, tolerance=1e-6, strict_check=True):
140
    """Validate compact axis-angle representation."""
141

142
def norm_axis_angle(a):
143
    """Normalize axis-angle representation."""
144

145
def axis_angle_from_matrix(R):
146
    """Convert rotation matrix to axis-angle."""
147

148
def axis_angle_from_quaternion(q):
149
    """Convert quaternion to axis-angle."""
150

151
def axis_angle_from_two_directions(a, b):
152
    """Compute axis-angle rotation from direction a to b."""
153

154
def compact_axis_angle(a):
155
    """Extract compact axis-angle from full representation."""
156

157
def compact_axis_angle_from_matrix(R):
158
    """Convert rotation matrix to compact axis-angle."""
159

160
def compact_axis_angle_from_quaternion(q):
161
    """Convert quaternion to compact axis-angle."""
162

163
def axis_angle_slerp(start, end, t):
164
    """Spherical linear interpolation between axis-angles."""
165

166
def axis_angle_from_compact_axis_angle(a):
167
    """
168
    Compute axis-angle from compact axis-angle representation.
169
    
170
    Parameters:
171
    - a: array, shape (3,) - Compact axis-angle: angle * (x, y, z)
172
    
173
    Returns:
174
    - a: array, shape (4,) - Axis-angle: (x, y, z, angle)
175
    """
176

177
def norm_compact_axis_angle(a):
178
    """
179
    Normalize compact axis-angle representation.
180
    
181
    Parameters:
182
    - a: array, shape (3,) - Compact axis-angle
183
    
184
    Returns:
185
    - a_norm: array, shape (3,) - Normalized compact axis-angle
186
    """
187

188
def compact_axis_angle_near_pi(a, threshold=1e-4):
189
    """
190
    Check if compact axis-angle is near pi (singularity).
191
    
192
    Parameters:
193
    - a: array, shape (3,) - Compact axis-angle
194
    - threshold: float - Distance threshold for singularity detection
195
    
196
    Returns:
197
    - near_pi: bool - True if near singularity
198
    """
199
```
200

201
### Euler Angle Operations
202

203
Operations for Euler angle representations with support for all 24 conventions.
204

205
```python { .api }
206
# Note: No general check_euler function - validation is done per convention
207

208
def norm_euler(e, i, j, k):
209
    """Normalize Euler angles to valid range for given axis sequence."""
210

211
def euler_from_matrix(R, i, j, k, extrinsic, strict_check=True):
212
    """
213
    Convert rotation matrix to Euler angles.
214
    
215
    Parameters:
216
    - R: array, shape (3, 3) - Rotation matrix
217
    - i: int from [0, 1, 2] - First rotation axis (0: x, 1: y, 2: z)
218
    - j: int from [0, 1, 2] - Second rotation axis (0: x, 1, y, 2: z)
219
    - k: int from [0, 1, 2] - Third rotation axis (0: x, 1: y, 2: z)
220
    - extrinsic: bool - Extrinsic (True) or intrinsic (False) convention
221
    - strict_check: bool - Enable strict validation
222
    
223
    Returns:
224
    - e: array, shape (3,) - Euler angles
225
    """
226

227
def euler_from_quaternion(q, i, j, k, extrinsic):
228
    """Convert quaternion to Euler angles."""
229

230
def euler_near_gimbal_lock(e, i, j, k, threshold=1e-6):
231
    """Check if Euler angles are near gimbal lock."""
232
```
233

234
### Euler Angle Convention Functions (Deprecated)
235

236
Convenience functions for specific Euler angle conventions. **Note: These are deprecated in favor of the general `matrix_from_euler()` and `euler_from_matrix()` functions.**
237

238
```python { .api }
239
# Active matrices from Euler angles - Extrinsic rotations
240
def active_matrix_from_extrinsic_euler_xyz(e):
241
    """Convert extrinsic XYZ Euler angles to rotation matrix (deprecated)."""
242

243
def active_matrix_from_extrinsic_euler_zyx(e):
244
    """Convert extrinsic ZYX Euler angles to rotation matrix (deprecated)."""
245

246
def active_matrix_from_extrinsic_roll_pitch_yaw(rpy):
247
    """Convert roll-pitch-yaw to rotation matrix (deprecated)."""
248

249
# Active matrices from Euler angles - Intrinsic rotations  
250
def active_matrix_from_intrinsic_euler_xyz(e):
251
    """Convert intrinsic XYZ Euler angles to rotation matrix (deprecated)."""
252

253
def active_matrix_from_intrinsic_euler_zyx(e):
254
    """Convert intrinsic ZYX Euler angles to rotation matrix (deprecated)."""
255

256
# Euler angles from matrices - Intrinsic conventions
257
def intrinsic_euler_xyz_from_active_matrix(R, strict_check=True):
258
    """Extract intrinsic XYZ Euler angles from rotation matrix (deprecated)."""
259

260
def intrinsic_euler_zyx_from_active_matrix(R, strict_check=True):
261
    """Extract intrinsic ZYX Euler angles from rotation matrix (deprecated)."""
262

263
# Euler angles from matrices - Extrinsic conventions  
264
def extrinsic_euler_xyz_from_active_matrix(R, strict_check=True):
265
    """Extract extrinsic XYZ Euler angles from rotation matrix (deprecated)."""
266

267
def extrinsic_euler_zyx_from_active_matrix(R, strict_check=True):
268
    """Extract extrinsic ZYX Euler angles from rotation matrix (deprecated)."""
269

270
# All 24 Euler angle conventions are available as individual functions:
271
# Extrinsic: active_matrix_from_extrinsic_euler_{xyz,xyx,xzx,xzy,yxy,yxz,yzx,yzy,zxy,zxz,zyx,zyz}
272
# Intrinsic: active_matrix_from_intrinsic_euler_{xyz,xyx,xzx,xzy,yxy,yxz,yzx,yzy,zxy,zxz,zyx,zyz}
273
# Matrix to Euler: {intrinsic,extrinsic}_euler_{convention}_from_active_matrix
274
# Recommended: Use matrix_from_euler(e, i, j, k, extrinsic) instead
275

276
def quaternion_from_extrinsic_euler_xyz(e):
277
    """Convert extrinsic XYZ Euler angles to quaternion (deprecated)."""
278
```
279

280
### Vector Utilities
281

282
Utility functions for 3D vector operations.
283

284
```python { .api }
285
def norm_vector(v):
286
    """Normalize vector to unit length."""
287

288
def angle_between_vectors(a, b):
289
    """Compute angle between two vectors."""
290

291
def perpendicular_to_vector(a):
292
    """Find a vector perpendicular to input vector."""
293

294
def vector_projection(a, b):
295
    """Project vector a onto vector b."""
296

297
def perpendicular_to_vectors(a, b):
298
    """Find vector perpendicular to two input vectors."""
299

300
def plane_basis_from_normal(n):
301
    """Generate orthonormal basis vectors for plane from normal."""
302
```
303

304
### Modified Rodrigues Parameters
305

306
Operations for Modified Rodrigues Parameters (MRP) representation.
307

308
```python { .api }
309
def check_mrp(mrp, tolerance=1e-6, strict_check=True):
310
    """Validate Modified Rodrigues Parameters."""
311

312
def norm_mrp(mrp):
313
    """Normalize MRP to avoid singularity."""
314

315
def mrp_from_quaternion(q):
316
    """Convert quaternion to MRP."""
317

318
def mrp_from_axis_angle(a):
319
    """Convert axis-angle to MRP."""
320

321
def quaternion_from_mrp(mrp):
322
    """Convert MRP to quaternion."""
323

324
def axis_angle_from_mrp(mrp):
325
    """Convert MRP to axis-angle."""
326

327
def concatenate_mrp(mrp1, mrp2):
328
    """Compose two MRP representations."""
329

330
def mrp_prod_vector(mrp, v):
331
    """Rotate vector using MRP."""
332

333
def mrp_double(mrp):
334
    """
335
    Double MRP rotation angle.
336
    
337
    Parameters:
338
    - mrp: array, shape (3,) - Modified Rodrigues Parameters
339
    
340
    Returns:
341
    - mrp2: array, shape (3,) - Doubled MRP
342
    """
343

344
def mrp_near_singularity(mrp, threshold=1e-6):
345
    """
346
    Check if MRP is near singularity (norm close to 1).
347
    
348
    Parameters:
349
    - mrp: array, shape (3,) - Modified Rodrigues Parameters
350
    - threshold: float - Distance threshold for singularity
351
    
352
    Returns:
353
    - near_singularity: bool - True if near singularity
354
    """
355
```
356

357
### Geometric Algebra Rotors
358

359
Operations for geometric algebra rotor representation.
360

361
```python { .api }
362
def check_rotor(rotor, tolerance=1e-6, strict_check=True):
363
    """Validate rotor representation."""
364

365
def wedge(a, b):
366
    """Wedge (exterior) product of vectors."""
367

368
def geometric_product(a, b):
369
    """Geometric product of multivectors."""
370

371
def rotor_apply(rotor, v):
372
    """Apply rotor rotation to vector."""
373

374
def rotor_reverse(rotor):
375
    """Reverse (conjugate) of rotor."""
376

377
def concatenate_rotors(r1, r2):
378
    """Compose two rotors."""
379

380
def rotor_from_plane_angle(bivector, angle):
381
    """Create rotor from plane and angle."""
382

383
def rotor_from_two_directions(a, b):
384
    """Create rotor aligning direction a to b."""
385

386
def rotor_slerp(start, end, t):
387
    """Spherical linear interpolation between rotors."""
388

389
def plane_normal_from_bivector(bivector):
390
    """Extract plane normal from bivector."""
391
```
392

393
### Visualization and Plotting
394

395
Functions for visualizing rotation representations and coordinate frames.
396

397
```python { .api }
398
def plot_basis(ax, R=None, p=None, s=1.0, strict_check=True, **kwargs):
399
    """
400
    Plot coordinate frame basis vectors.
401
    
402
    Parameters:
403
    - ax: matplotlib axis - 3D plot axis
404
    - R: array, shape (3, 3) - Rotation matrix (default: identity)
405
    - p: array, shape (3,) - Origin position (default: origin)
406
    - s: float - Scale factor for basis vectors
407
    - strict_check: bool - Enable strict validation
408
    - kwargs: Additional plotting arguments
409
    """
410

411
def plot_axis_angle(ax, a, p=None, s=1.0, **kwargs):
412
    """
413
    Plot axis-angle representation as rotation axis.
414
    
415
    Parameters:
416
    - ax: matplotlib axis - 3D plot axis
417
    - a: array, shape (4,) - Axis-angle representation
418
    - p: array, shape (3,) - Origin position (default: origin)
419
    - s: float - Scale factor for axis vector
420
    - kwargs: Additional plotting arguments
421
    """
422

423
def plot_bivector(ax, bivector, p=None, s=1.0, **kwargs):
424
    """
425
    Plot bivector (oriented plane) in 3D space.
426
    
427
    Parameters:
428
    - ax: matplotlib axis - 3D plot axis  
429
    - bivector: array, shape (3,) - Bivector representation
430
    - p: array, shape (3,) - Origin position (default: origin)
431
    - s: float - Scale factor for bivector
432
    - kwargs: Additional plotting arguments
433
    """
434
```
435

436
### Interpolation
437

438
Spherical linear interpolation (SLERP) operations for smooth rotation interpolation.
439

440
```python { .api }
441
def matrix_slerp(start, end, t):
442
    """Spherical linear interpolation between rotation matrices."""
443

444
def matrix_power(R, t):
445
    """Matrix power operation for rotations."""
446

447
def slerp_weights(t):
448
    """Compute SLERP interpolation weights."""
449

450
def quaternion_slerp(start, end, t):
451
    """Spherical linear interpolation between quaternions."""
452

453
def axis_angle_slerp(start, end, t):
454
    """Spherical linear interpolation between axis-angles."""
455

456
def rotor_slerp(start, end, t):
457
    """Spherical linear interpolation between rotors."""
458
```
459

460
### Random Generation
461

462
Functions for generating random rotations with uniform distribution.
463

464
```python { .api }
465
def random_matrix(random_state=None):
466
    """Generate random rotation matrix."""
467

468
def random_quaternion(random_state=None):
469
    """Generate random unit quaternion."""
470

471
def random_axis_angle(random_state=None):
472
    """Generate random axis-angle representation."""
473

474
def random_compact_axis_angle(random_state=None):
475
    """Generate random compact axis-angle."""
476

477
def random_vector(random_state=None):
478
    """Generate random unit vector."""
479
```
480

481
### Validation and Assertions
482

483
Validation and assertion functions for verifying rotation representations.
484

485
```python { .api }
486
def assert_rotation_matrix(R, *args, **kwargs):
487
    """
488
    Raise an assertion if a matrix is not a rotation matrix.
489
    
490
    Checks that R @ R.T = I and det(R) = 1.
491
    
492
    Parameters:
493
    - R: array, shape (3, 3) - Matrix to validate
494
    - args: Positional arguments passed to assert_array_almost_equal  
495
    - kwargs: Keyword arguments passed to assert_array_almost_equal
496
    """
497

498
def assert_quaternion_equal(q1, q2, *args, **kwargs):
499
    """Assert that two quaternions are equal (considering q and -q equivalence)."""
500

501
def assert_axis_angle_equal(a1, a2, *args, **kwargs):
502
    """Assert that two axis-angle representations are equal."""
503

504
def assert_compact_axis_angle_equal(a1, a2, *args, **kwargs):
505
    """Assert that two compact axis-angle representations are equal."""
506

507
def assert_euler_equal(e1, e2, i, j, k, *args, **kwargs):
508
    """Assert that two Euler angle representations are equal."""
509

510
def assert_mrp_equal(mrp1, mrp2, *args, **kwargs):
511
    """Assert that two MRP representations are equal."""
512

513
def check_quaternions(Q, unit=True):
514
    """
515
    Input validation for multiple quaternions.
516
    
517
    Parameters:
518
    - Q: array, shape (n_steps, 4) - Multiple quaternions
519
    - unit: bool - Normalize quaternions to unit length
520
    
521
    Returns:
522
    - Q: array, shape (n_steps, 4) - Validated quaternions
523
    """
524

525
def check_skew_symmetric_matrix(S):
526
    """Validate that matrix is skew-symmetric (S = -S.T)."""
527

528
def check_rot_log(S):
529
    """Validate rotation logarithm representation."""
530

531
def quaternion_requires_renormalization(q, tolerance=1e-3):
532
    """Check if quaternion needs renormalization."""
533

534
def matrix_requires_renormalization(R, tolerance=1e-3):
535
    """Check if rotation matrix needs renormalization."""
536
```
537

538
### Angle-Based Rotations
539

540
Functions for creating rotations from simple angle rotations around coordinate axes.
541

542
```python { .api }
543
def norm_angle(a):
544
    """
545
    Normalize angle to (-pi, pi] range.
546
    
547
    Parameters:
548
    - a: float or array - Angle(s) in radians
549
    
550
    Returns:
551
    - a_norm: float or array - Normalized angle(s)
552
    """
553

554
def active_matrix_from_angle(basis, angle):
555
    """
556
    Compute active rotation matrix from rotation about basis vector.
557
    
558
    Parameters:
559
    - basis: int from [0, 1, 2] - Rotation axis (0: x, 1: y, 2: z)
560
    - angle: float - Rotation angle in radians
561
    
562
    Returns:
563
    - R: array, shape (3, 3) - Active rotation matrix
564
    """
565

566
def passive_matrix_from_angle(basis, angle):
567
    """
568
    Compute passive rotation matrix from rotation about basis vector.
569
    
570
    Parameters:
571
    - basis: int from [0, 1, 2] - Rotation axis (0: x, 1: y, 2: z) 
572
    - angle: float - Rotation angle in radians
573
    
574
    Returns:
575
    - R: array, shape (3, 3) - Passive rotation matrix
576
    """
577

578
def quaternion_from_angle(basis, angle):
579
    """
580
    Compute quaternion from rotation about basis vector.
581
    
582
    Parameters:
583
    - basis: int from [0, 1, 2] - Rotation axis (0: x, 1: y, 2: z)
584
    - angle: float - Rotation angle in radians
585
    
586
    Returns:
587
    - q: array, shape (4,) - Unit quaternion (w, x, y, z)
588
    """
589
```
590

591
### Mathematical Operations
592

593
Advanced mathematical operations for rotation analysis.
594

595
```python { .api }
596
def cross_product_matrix(v):
597
    """Convert vector to skew-symmetric matrix."""
598

599
def rot_log_from_compact_axis_angle(a):
600
    """Convert compact axis-angle to rotation logarithm."""
601

602
def left_jacobian_SO3(omega):
603
    """Left Jacobian for SO(3) group."""
604

605
def left_jacobian_SO3_inv(omega):
606
    """Inverse left Jacobian for SO(3)."""
607

608
def left_jacobian_SO3_series(omega):
609
    """Left Jacobian for SO(3) using series expansion."""
610

611
def left_jacobian_SO3_inv_series(omega):
612
    """Inverse left Jacobian for SO(3) using series expansion."""
613

614
def robust_polar_decomposition(A):
615
    """Robust polar decomposition of matrix."""
616
```
617

618
### Constants
619

620
Mathematical constants for rotation operations.
621

622
```python { .api }
623
eps: float  # Small epsilon value for numerical computations
624
unitx: np.ndarray  # Unit vector [1, 0, 0]
625
unity: np.ndarray  # Unit vector [0, 1, 0] 
626
unitz: np.ndarray  # Unit vector [0, 0, 1]
627
q_i: np.ndarray    # Quaternion i basis element
628
q_j: np.ndarray    # Quaternion j basis element  
629
q_k: np.ndarray    # Quaternion k basis element
630
q_id: np.ndarray   # Identity quaternion [1, 0, 0, 0]
631
a_id: np.ndarray   # Zero axis-angle representation
632
R_id: np.ndarray   # 3x3 identity rotation matrix
633
p0: np.ndarray     # Origin point [0, 0, 0]
634
```
635

636
## Usage Examples
637

638
### Basic Rotation Conversions
639

640
```python
641
import numpy as np
642
import pytransform3d.rotations as pr
643

644
# Convert Euler angles to rotation matrix (XYZ convention)
645
euler = [0.1, 0.2, 0.3]  # roll, pitch, yaw
646
R = pr.matrix_from_euler(euler, 0, 1, 2, True)  # XYZ extrinsic
647

648
# Convert to quaternion
649
q = pr.quaternion_from_matrix(R)
650

651
# Convert to axis-angle
652
a = pr.axis_angle_from_quaternion(q)
653

654
# Verify round-trip conversion
655
R_reconstructed = pr.matrix_from_axis_angle(a)
656
assert np.allclose(R, R_reconstructed)
657
```
658

659
### Quaternion Composition
660

661
```python
662
import pytransform3d.rotations as pr
663

664
# Create two rotations
665
q1 = pr.quaternion_from_euler([0.1, 0, 0], 0, 1, 2, True)  # X rotation
666
q2 = pr.quaternion_from_euler([0, 0.2, 0], 0, 1, 2, True)  # Y rotation
667

668
# Compose rotations (q2 applied after q1)
669
q_composed = pr.concatenate_quaternions(q1, q2)
670

671
# Interpolate between rotations
672
t = 0.5  # halfway
673
q_interp = pr.quaternion_slerp(q1, q2, t)
674
```
675

676
### Vector Rotation
677

678
```python
679
import numpy as np
680
import pytransform3d.rotations as pr
681

682
# Define rotation and vector
683
q = pr.quaternion_from_euler([0, 0, np.pi/4], 0, 1, 2, True)  # Z rotation
684
v = np.array([1, 0, 0])
685

686
# Rotate vector
687
v_rotated = pr.q_prod_vector(q, v)
688
print(f"Original: {v}")
689
print(f"Rotated: {v_rotated}")
690
```