0
# Trajectories
1

2
Trajectories in three dimensions (SE(3)) providing batch operations on poses or transformations that are 400 to 1000 times faster than individual conversions. This module handles sequences of transformations using various representations including dual quaternions, position-quaternions, and exponential coordinates.
3

4
## Capabilities
5

6
### Transform Operations
7

8
Core batch operations for transformation matrices including inversion and concatenation.
9

10
```python { .api }
11
def invert_transforms(A2B):
12
    """
13
    Invert transformation matrices.
14
    
15
    Parameters:
16
    - A2B: array-like, shape (..., 4, 4) - transformation matrices
17
    
18
    Returns:
19
    - B2A: array, shape (..., 4, 4) - inverted transformations
20
    """
21

22
def concat_one_to_many(A2B, B2C):
23
    """
24
    Concatenate one transformation with many transformations.
25
    
26
    Parameters:
27
    - A2B: array-like, shape (4, 4) - single transformation
28
    - B2C: array-like, shape (..., 4, 4) - multiple transformations
29
    
30
    Returns:
31
    - A2C: array, shape (..., 4, 4) - concatenated transformations
32
    """
33

34
def concat_many_to_one(A2B, B2C):
35
    """
36
    Concatenate many transformations with one transformation.
37
    
38
    Parameters:
39
    - A2B: array-like, shape (..., 4, 4) - multiple transformations
40
    - B2C: array-like, shape (4, 4) - single transformation
41
    
42
    Returns:
43
    - A2C: array, shape (..., 4, 4) - concatenated transformations
44
    """
45

46
def concat_many_to_many(A2B, B2C):
47
    """
48
    Concatenate corresponding pairs of transformations.
49
    
50
    Parameters:
51
    - A2B: array-like, shape (..., 4, 4) - first transformations
52
    - B2C: array-like, shape (..., 4, 4) - second transformations
53
    
54
    Returns:
55
    - A2C: array, shape (..., 4, 4) - concatenated transformations
56
    """
57

58
def concat_dynamic(A2B, B2C):
59
    """
60
    Dynamically concatenate transformations with broadcasting.
61
    
62
    Parameters:
63
    - A2B: array-like, shape (..., 4, 4) - first transformations
64
    - B2C: array-like, shape (..., 4, 4) - second transformations
65
    
66
    Returns:
67
    - A2C: array, shape (..., 4, 4) - concatenated transformations
68
    """
69
```
70

71
### Position-Quaternion Operations
72

73
Operations for position-quaternion representation combining translation and rotation.
74

75
```python { .api }
76
def transforms_from_pqs(pqs):
77
    """
78
    Convert position-quaternions to transformation matrices.
79
    
80
    Parameters:
81
    - pqs: array-like, shape (..., 7) - position-quaternions [x, y, z, qw, qx, qy, qz]
82
    
83
    Returns:
84
    - transforms: array, shape (..., 4, 4) - transformation matrices
85
    """
86

87
def pqs_from_transforms(transforms):
88
    """
89
    Convert transformation matrices to position-quaternions.
90
    
91
    Parameters:
92
    - transforms: array-like, shape (..., 4, 4) - transformation matrices
93
    
94
    Returns:
95
    - pqs: array, shape (..., 7) - position-quaternions [x, y, z, qw, qx, qy, qz]
96
    """
97
```
98

99
### Dual Quaternion Operations
100

101
Dual quaternion representation for efficient transformation operations and interpolation.
102

103
```python { .api }
104
def dual_quaternions_from_pqs(pqs):
105
    """
106
    Convert position-quaternions to dual quaternions.
107
    
108
    Parameters:
109
    - pqs: array-like, shape (..., 7) - position-quaternions
110
    
111
    Returns:
112
    - dqs: array, shape (..., 8) - dual quaternions
113
    """
114

115
def dual_quaternions_from_transforms(transforms):
116
    """
117
    Convert transformation matrices to dual quaternions.
118
    
119
    Parameters:
120
    - transforms: array-like, shape (..., 4, 4) - transformation matrices
121
    
122
    Returns:
123
    - dqs: array, shape (..., 8) - dual quaternions
124
    """
125

126
def pqs_from_dual_quaternions(dqs):
127
    """
128
    Convert dual quaternions to position-quaternions.
129
    
130
    Parameters:
131
    - dqs: array-like, shape (..., 8) - dual quaternions
132
    
133
    Returns:
134
    - pqs: array, shape (..., 7) - position-quaternions
135
    """
136

137
def transforms_from_dual_quaternions(dqs):
138
    """
139
    Convert dual quaternions to transformation matrices.
140
    
141
    Parameters:
142
    - dqs: array-like, shape (..., 8) - dual quaternions
143
    
144
    Returns:
145
    - transforms: array, shape (..., 4, 4) - transformation matrices
146
    """
147

148
def batch_concatenate_dual_quaternions(dq1, dq2):
149
    """
150
    Concatenate dual quaternions.
151
    
152
    Parameters:
153
    - dq1: array-like, shape (..., 8) - first dual quaternions
154
    - dq2: array-like, shape (..., 8) - second dual quaternions
155
    
156
    Returns:
157
    - dq_result: array, shape (..., 8) - concatenated dual quaternions
158
    """
159

160
def batch_dq_conj(dqs):
161
    """
162
    Compute dual quaternion conjugates.
163
    
164
    Parameters:
165
    - dqs: array-like, shape (..., 8) - dual quaternions
166
    
167
    Returns:
168
    - dq_conj: array, shape (..., 8) - conjugated dual quaternions
169
    """
170

171
def batch_dq_q_conj(dqs):
172
    """
173
    Compute quaternion part conjugates of dual quaternions.
174
    
175
    Parameters:
176
    - dqs: array-like, shape (..., 8) - dual quaternions
177
    
178
    Returns:
179
    - dq_q_conj: array, shape (..., 8) - quaternion-conjugated dual quaternions
180
    """
181

182
def batch_dq_prod_vector(dqs, vectors):
183
    """
184
    Apply dual quaternions to transform vectors.
185
    
186
    Parameters:
187
    - dqs: array-like, shape (..., 8) - dual quaternions
188
    - vectors: array-like, shape (..., 3) - vectors to transform
189
    
190
    Returns:
191
    - transformed: array, shape (..., 3) - transformed vectors
192
    """
193

194
def dual_quaternions_power(dqs, t):
195
    """
196
    Raise dual quaternions to a power.
197
    
198
    Parameters:
199
    - dqs: array-like, shape (..., 8) - dual quaternions
200
    - t: array-like, shape (...,) - power values
201
    
202
    Returns:
203
    - dq_power: array, shape (..., 8) - powered dual quaternions
204
    """
205

206
def dual_quaternions_sclerp(start, end, t):
207
    """
208
    Screw linear interpolation between dual quaternions.
209
    
210
    Parameters:
211
    - start: array-like, shape (..., 8) - start dual quaternions
212
    - end: array-like, shape (..., 8) - end dual quaternions
213
    - t: array-like, shape (...,) - interpolation parameters [0, 1]
214
    
215
    Returns:
216
    - interpolated: array, shape (..., 8) - interpolated dual quaternions
217
    """
218
```
219

220
### Screw Theory Operations
221

222
Operations using screw theory for representing transformations and motions.
223

224
```python { .api }
225
def dual_quaternions_from_screw_parameters(S, theta):
226
    """
227
    Convert screw parameters to dual quaternions.
228
    
229
    Parameters:
230
    - S: array-like, shape (..., 6) - screw axes [omega, v]
231
    - theta: array-like, shape (...,) - screw angles
232
    
233
    Returns:
234
    - dqs: array, shape (..., 8) - dual quaternions
235
    """
236

237
def screw_parameters_from_dual_quaternions(dqs):
238
    """
239
    Convert dual quaternions to screw parameters.
240
    
241
    Parameters:
242
    - dqs: array-like, shape (..., 8) - dual quaternions
243
    
244
    Returns:
245
    - S: array, shape (..., 6) - screw axes
246
    - theta: array, shape (...,) - screw angles
247
    """
248

249
def transforms_from_exponential_coordinates(Stheta):
250
    """
251
    Convert exponential coordinates to transformation matrices.
252
    
253
    Parameters:
254
    - Stheta: array-like, shape (..., 6) - exponential coordinates [omega*theta, v*theta]
255
    
256
    Returns:
257
    - transforms: array, shape (..., 4, 4) - transformation matrices
258
    """
259

260
def exponential_coordinates_from_transforms(transforms):
261
    """
262
    Convert transformation matrices to exponential coordinates.
263
    
264
    Parameters:
265
    - transforms: array-like, shape (..., 4, 4) - transformation matrices
266
    
267
    Returns:
268
    - Stheta: array, shape (..., 6) - exponential coordinates
269
    """
270

271
def mirror_screw_axis_direction(S):
272
    """
273
    Mirror screw axis direction.
274
    
275
    Parameters:
276
    - S: array-like, shape (..., 6) - screw axes
277
    
278
    Returns:
279
    - S_mirrored: array, shape (..., 6) - mirrored screw axes
280
    """
281
```
282

283
### Trajectory Generation and Visualization
284

285
Tools for generating random trajectories and plotting trajectory paths.
286

287
```python { .api }
288
def random_trajectories(n_steps, n_trajectories=1, **kwargs):
289
    """
290
    Generate random trajectories.
291
    
292
    Parameters:
293
    - n_steps: int - number of steps in each trajectory
294
    - n_trajectories: int - number of trajectories to generate
295
    - **kwargs: additional parameters for trajectory generation
296
    
297
    Returns:
298
    - trajectories: array, shape (n_trajectories, n_steps, 4, 4) - random trajectories
299
    """
300

301
def plot_trajectory(P=None, show_direction=True, **kwargs):
302
    """
303
    Plot 3D trajectory.
304
    
305
    Parameters:  
306
    - P: array-like, shape (n_steps, 3) - trajectory positions
307
    - show_direction: bool - whether to show trajectory direction
308
    - **kwargs: additional plotting parameters
309
    
310
    Returns:
311
    - artists: list - matplotlib artists for the trajectory
312
    """
313
```
314

315
## Usage Examples
316

317
### Batch Transformation Processing
318

319
```python
320
import numpy as np
321
from pytransform3d.trajectories import (
322
    transforms_from_pqs,
323
    invert_transforms,
324
    concat_many_to_many
325
)
326

327
# Create trajectory as position-quaternions
328
n_steps = 100
329
pqs = np.zeros((n_steps, 7))
330
pqs[:, :3] = np.random.rand(n_steps, 3)  # positions
331
pqs[:, 3] = 1.0  # quaternion w component
332

333
# Convert to transformation matrices
334
transforms = transforms_from_pqs(pqs)
335
print(transforms.shape)  # (100, 4, 4)
336

337
# Compute inverse transformations
338
inv_transforms = invert_transforms(transforms)
339

340
# Chain transformations
341
base_transforms = np.eye(4).reshape(1, 4, 4).repeat(n_steps, axis=0)
342
final_transforms = concat_many_to_many(base_transforms, transforms)
343
```
344

345
### Dual Quaternion Interpolation
346

347
```python
348
import numpy as np
349
from pytransform3d.trajectories import (
350
    dual_quaternions_from_pqs,
351
    dual_quaternions_sclerp,
352
    pqs_from_dual_quaternions
353
)
354

355
# Start and end poses
356
start_pq = np.array([0, 0, 0, 1, 0, 0, 0])  # origin
357
end_pq = np.array([1, 1, 1, 0.707, 0, 0, 0.707])  # translated and rotated
358

359
# Convert to dual quaternions
360
start_dq = dual_quaternions_from_pqs(start_pq)
361
end_dq = dual_quaternions_from_pqs(end_pq)
362

363
# Interpolate using screw linear interpolation
364
t_values = np.linspace(0, 1, 50)
365
interpolated_dqs = dual_quaternions_sclerp(
366
    np.broadcast_to(start_dq, (50, 8)),
367
    np.broadcast_to(end_dq, (50, 8)),
368
    t_values
369
)
370

371
# Convert back to position-quaternions
372
interpolated_pqs = pqs_from_dual_quaternions(interpolated_dqs)
373
```
374

375
### Trajectory Visualization
376

377
```python
378
import numpy as np
379
import matplotlib.pyplot as plt
380
from pytransform3d.trajectories import plot_trajectory
381

382
# Generate spiral trajectory
383
t = np.linspace(0, 4*np.pi, 100)
384
positions = np.column_stack([
385
    np.cos(t),
386
    np.sin(t), 
387
    t / (4*np.pi)
388
])
389

390
# Plot trajectory
391
fig = plt.figure()
392
ax = fig.add_subplot(111, projection='3d')
393
plot_trajectory(positions, show_direction=True, ax=ax)
394
plt.show()
395
```