Hide keyboard shortcuts

Hot-keys on this page

r m x p   toggle line displays

j k   next/prev highlighted chunk

0   (zero) top of page

1   (one) first highlighted chunk

1

2

3

4

5

6

7

8

9

10

11

12

13

14

15

16

17

18

19

20

21

22

23

24

25

26

27

28

29

30

31

32

33

34

35

36

37

38

39

40

41

42

43

44

45

46

47

48

49

50

51

52

53

54

55

56

57

58

59

60

61

62

63

64

65

66

67

68

69

70

71

72

73

74

75

76

77

78

79

80

81

82

83

84

85

86

87

88

89

90

91

92

93

94

95

96

97

98

99

100

101

102

103

104

105

106

107

108

109

110

111

112

113

114

115

116

117

118

119

120

121

122

123

124

125

126

127

128

129

130

131

132

133

134

135

136

137

138

139

140

141

142

143

144

145

146

147

148

149

150

151

152

153

154

155

156

157

158

159

160

161

162

163

164

165

166

167

168

169

170

171

172

173

174

175

176

177

178

179

180

181

182

183

184

185

186

187

188

189

190

191

192

193

194

195

196

197

198

199

200

201

202

203

204

205

206

207

208

209

210

211

212

213

214

215

216

217

218

219

220

221

222

223

224

225

226

227

228

229

230

231

232

233

234

235

236

237

238

239

240

241

242

243

244

245

246

247

248

249

250

251

252

253

254

255

256

257

258

259

260

261

262

263

264

265

266

267

268

269

270

271

272

273

274

275

276

277

278

279

280

281

282

283

284

285

286

287

288

289

290

291

292

293

294

295

296

297

298

299

300

301

302

303

304

305

306

307

308

309

310

311

312

313

314

315

316

317

318

319

320

321

322

323

324

325

326

327

328

329

330

331

332

333

334

335

336

337

338

339

340

341

342

343

344

345

346

347

348

349

350

351

352

353

354

355

356

357

358

359

360

361

362

363

364

365

366

367

368

369

370

371

372

373

374

375

376

377

378

379

380

381

382

383

384

385

386

387

388

389

390

391

392

393

394

395

396

397

398

399

400

401

402

403

404

405

406

407

408

409

410

411

412

413

414

415

416

417

418

419

420

421

422

423

424

425

426

427

428

429

430

431

432

433

434

435

436

437

438

439

440

441

442

443

444

445

446

447

448

449

450

451

452

453

454

455

456

457

458

459

460

461

462

463

464

465

466

467

468

469

470

471

472

473

474

475

476

477

478

479

480

481

482

483

484

485

486

487

488

489

490

491

492

493

494

495

496

497

498

499

500

501

502

503

504

505

506

507

508

509

510

511

512

513

514

515

516

517

518

519

520

521

522

523

524

525

526

527

528

529

530

531

532

533

534

535

536

537

538

539

540

541

542

543

544

545

546

547

548

549

550

551

552

553

554

555

556

557

558

559

560

561

562

563

564

565

566

567

568

569

570

571

572

573

574

575

576

577

578

579

580

581

582

583

584

585

586

587

588

589

590

591

592

593

594

595

596

597

598

599

600

601

602

603

604

605

606

607

608

609

610

611

612

613

614

615

616

617

618

619

620

621

622

623

624

625

626

627

628

629

630

631

632

633

634

635

636

637

638

639

640

641

642

643

644

645

646

647

648

649

650

651

652

653

654

655

656

657

658

659

660

661

662

663

664

665

666

667

668

669

670

671

672

673

674

675

676

677

678

679

680

681

682

683

684

685

686

687

688

689

690

691

692

693

694

695

696

697

698

699

700

701

702

703

704

705

706

707

708

709

710

711

712

713

714

715

716

717

718

719

720

721

722

723

724

725

726

727

728

729

730

731

732

733

734

735

736

737

738

739

740

741

742

743

744

745

746

747

748

749

750

751

752

753

754

755

756

757

758

759

760

761

762

763

764

765

766

767

768

769

770

771

772

773

774

775

776

777

778

779

780

781

782

783

784

785

786

787

788

789

790

791

792

793

794

795

796

797

798

799

800

801

802

803

804

805

806

807

808

809

810

811

812

813

814

815

816

817

818

819

820

821

822

823

824

825

826

827

828

829

830

831

832

833

834

835

836

837

838

839

840

841

842

843

844

845

846

847

848

849

850

851

852

853

854

855

856

857

858

859

860

861

862

863

864

865

866

867

868

869

870

871

872

873

874

875

876

877

878

879

880

881

882

883

884

885

886

887

888

889

890

891

892

893

894

895

896

897

898

899

900

901

902

903

904

905

906

907

908

909

910

911

912

913

914

915

916

917

918

919

920

921

922

923

924

925

926

927

928

929

930

931

932

933

934

935

936

937

938

939

940

941

942

943

944

945

946

947

948

949

950

951

952

953

954

955

956

957

958

959

960

961

962

963

964

965

966

967

968

969

970

971

972

973

974

975

976

977

978

979

980

981

982

983

984

985

986

987

988

989

990

991

992

993

994

995

996

997

998

999

1000

1001

1002

1003

1004

1005

1006

1007

1008

1009

1010

1011

1012

1013

1014

1015

1016

1017

1018

1019

1020

1021

1022

1023

1024

1025

1026

1027

1028

1029

1030

1031

1032

1033

1034

1035

1036

1037

1038

1039

1040

1041

1042

1043

1044

1045

1046

1047

1048

1049

1050

1051

1052

1053

1054

1055

1056

1057

1058

1059

1060

1061

1062

1063

1064

1065

1066

1067

1068

1069

1070

1071

1072

1073

1074

1075

1076

1077

1078

1079

1080

1081

1082

1083

1084

1085

1086

1087

1088

1089

1090

1091

1092

1093

1094

1095

1096

1097

1098

1099

1100

1101

1102

1103

1104

1105

1106

1107

1108

1109

1110

1111

1112

1113

1114

1115

1116

1117

1118

1119

1120

1121

1122

1123

1124

1125

1126

1127

1128

1129

1130

1131

1132

1133

1134

1135

1136

1137

1138

1139

1140

1141

1142

1143

1144

1145

1146

1147

1148

1149

1150

1151

1152

1153

1154

1155

1156

1157

1158

1159

1160

1161

1162

1163

1164

1165

1166

1167

1168

1169

1170

1171

1172

1173

1174

1175

1176

1177

1178

1179

1180

1181

1182

1183

1184

1185

1186

1187

1188

1189

1190

1191

1192

1193

1194

1195

1196

1197

1198

1199

1200

1201

1202

1203

1204

1205

1206

1207

1208

1209

1210

1211

1212

1213

1214

1215

1216

1217

1218

1219

1220

1221

1222

1223

1224

1225

1226

1227

1228

1229

1230

1231

1232

1233

1234

1235

1236

1237

1238

1239

1240

1241

1242

1243

1244

1245

1246

1247

1248

1249

1250

1251

1252

1253

1254

1255

1256

1257

1258

1259

1260

1261

1262

1263

1264

1265

1266

1267

1268

1269

1270

1271

1272

1273

1274

1275

1276

1277

1278

1279

1280

1281

1282

1283

1284

1285

1286

1287

1288

1289

1290

1291

1292

1293

1294

1295

1296

1297

1298

1299

1300

1301

1302

1303

1304

1305

1306

1307

1308

1309

1310

1311

1312

1313

1314

1315

1316

1317

1318

1319

1320

1321

1322

1323

1324

1325

1326

1327

1328

1329

1330

1331

1332

1333

1334

1335

1336

1337

1338

1339

1340

1341

1342

1343

1344

1345

1346

1347

1348

1349

1350

1351

1352

1353

1354

1355

1356

1357

1358

1359

1360

1361

1362

1363

1364

1365

1366

1367

1368

1369

1370

1371

1372

1373

1374

1375

1376

1377

1378

1379

1380

1381

1382

1383

1384

1385

1386

1387

1388

1389

1390

1391

1392

1393

1394

1395

1396

1397

1398

1399

1400

1401

1402

1403

1404

1405

1406

1407

1408

1409

1410

1411

1412

1413

1414

1415

1416

1417

1418

1419

1420

1421

1422

1423

1424

1425

1426

1427

1428

1429

1430

1431

1432

1433

1434

1435

1436

1437

1438

1439

1440

1441

1442

1443

1444

1445

1446

1447

1448

1449

1450

1451

1452

1453

1454

1455

1456

1457

1458

1459

1460

1461

1462

1463

1464

1465

1466

1467

1468

1469

1470

1471

1472

1473

1474

1475

1476

1477

1478

1479

1480

1481

1482

1483

1484

1485

1486

1487

1488

1489

1490

1491

1492

1493

1494

1495

1496

1497

1498

1499

1500

1501

1502

1503

1504

1505

1506

1507

1508

1509

1510

1511

1512

1513

1514

1515

1516

1517

1518

1519

1520

1521

1522

1523

1524

1525

1526

1527

1528

1529

1530

1531

1532

1533

1534

1535

1536

1537

1538

1539

1540

1541

1542

1543

1544

1545

1546

1547

1548

1549

1550

1551

1552

1553

1554

1555

1556

1557

1558

1559

1560

1561

1562

1563

1564

1565

1566

1567

1568

1569

1570

1571

1572

1573

1574

1575

1576

1577

1578

1579

1580

1581

1582

1583

1584

1585

1586

1587

1588

1589

1590

1591

1592

1593

1594

1595

1596

1597

1598

1599

1600

1601

1602

1603

1604

1605

1606

1607

1608

1609

1610

1611

1612

1613

1614

1615

1616

1617

1618

1619

1620

1621

1622

1623

1624

1625

1626

1627

1628

1629

1630

1631

1632

1633

1634

1635

1636

1637

1638

1639

1640

1641

1642

1643

1644

1645

1646

1647

1648

1649

1650

1651

1652

1653

1654

1655

1656

1657

1658

1659

1660

1661

1662

1663

1664

1665

1666

1667

1668

1669

1670

1671

1672

1673

1674

1675

1676

1677

1678

1679

1680

1681

1682

1683

1684

1685

1686

1687

1688

1689

1690

1691

1692

1693

1694

1695

1696

1697

1698

1699

1700

1701

1702

1703

1704

1705

1706

1707

1708

1709

1710

1711

1712

1713

1714

1715

1716

1717

1718

1719

1720

1721

1722

1723

1724

1725

1726

1727

1728

1729

1730

1731

1732

1733

1734

1735

1736

1737

1738

1739

1740

1741

1742

1743

1744

1745

1746

1747

1748

1749

1750

1751

1752

1753

1754

1755

1756

1757

1758

1759

1760

1761

1762

1763

1764

1765

1766

1767

1768

1769

1770

1771

1772

1773

from __future__ import division, print_function, absolute_import 

 

import re 

import warnings 

import numpy as np 

import scipy.linalg 

from scipy._lib._util import check_random_state 

 

 

_AXIS_TO_IND = {'x': 0, 'y': 1, 'z': 2} 

 

 

def _elementary_basis_vector(axis): 

b = np.zeros(3) 

b[_AXIS_TO_IND[axis]] = 1 

return b 

 

 

def _compute_euler_from_dcm(dcm, seq, extrinsic=False): 

# The algorithm assumes intrinsic frame transformations. For representation 

# the paper uses transformation matrices, which are transpose of the 

# direction cosine matrices used by our Rotation class. 

# Adapt the algorithm for our case by 

# 1. Instead of transposing our representation, use the transpose of the 

# O matrix as defined in the paper, and be careful to swap indices 

# 2. Reversing both axis sequence and angles for extrinsic rotations 

 

if extrinsic: 

seq = seq[::-1] 

 

if dcm.ndim == 2: 

dcm = dcm[None, :, :] 

num_rotations = dcm.shape[0] 

 

# Step 0 

# Algorithm assumes axes as column vectors, here we use 1D vectors 

n1 = _elementary_basis_vector(seq[0]) 

n2 = _elementary_basis_vector(seq[1]) 

n3 = _elementary_basis_vector(seq[2]) 

 

# Step 2 

sl = np.dot(np.cross(n1, n2), n3) 

cl = np.dot(n1, n3) 

 

# angle offset is lambda from the paper referenced in [2] from docstring of 

# `as_euler` function 

offset = np.arctan2(sl, cl) 

c = np.vstack((n2, np.cross(n1, n2), n1)) 

 

# Step 3 

rot = np.array([ 

[1, 0, 0], 

[0, cl, sl], 

[0, -sl, cl], 

]) 

res = np.einsum('...ij,...jk->...ik', c, dcm) 

dcm_transformed = np.einsum('...ij,...jk->...ik', res, c.T.dot(rot)) 

 

# Step 4 

angles = np.empty((num_rotations, 3)) 

# Ensure less than unit norm 

positive_unity = dcm_transformed[:, 2, 2] > 1 

negative_unity = dcm_transformed[:, 2, 2] < -1 

dcm_transformed[positive_unity, 2, 2] = 1 

dcm_transformed[negative_unity, 2, 2] = -1 

angles[:, 1] = np.arccos(dcm_transformed[:, 2, 2]) 

 

# Steps 5, 6 

eps = 1e-7 

safe1 = (np.abs(angles[:, 1]) >= eps) 

safe2 = (np.abs(angles[:, 1] - np.pi) >= eps) 

 

# Step 4 (Completion) 

angles[:, 1] += offset 

 

# 5b 

safe_mask = np.logical_and(safe1, safe2) 

angles[safe_mask, 0] = np.arctan2(dcm_transformed[safe_mask, 0, 2], 

-dcm_transformed[safe_mask, 1, 2]) 

angles[safe_mask, 2] = np.arctan2(dcm_transformed[safe_mask, 2, 0], 

dcm_transformed[safe_mask, 2, 1]) 

 

if extrinsic: 

# For extrinsic, set first angle to zero so that after reversal we 

# ensure that third angle is zero 

# 6a 

angles[~safe_mask, 0] = 0 

# 6b 

angles[~safe1, 2] = np.arctan2( 

dcm_transformed[~safe1, 1, 0] - dcm_transformed[~safe1, 0, 1], 

dcm_transformed[~safe1, 0, 0] + dcm_transformed[~safe1, 1, 1] 

) 

# 6c 

angles[~safe2, 2] = -np.arctan2( 

dcm_transformed[~safe2, 1, 0] + dcm_transformed[~safe2, 0, 1], 

dcm_transformed[~safe2, 0, 0] - dcm_transformed[~safe2, 1, 1] 

) 

else: 

# For instrinsic, set third angle to zero 

# 6a 

angles[~safe_mask, 2] = 0 

# 6b 

angles[~safe1, 0] = np.arctan2( 

dcm_transformed[~safe1, 1, 0] - dcm_transformed[~safe1, 0, 1], 

dcm_transformed[~safe1, 0, 0] + dcm_transformed[~safe1, 1, 1] 

) 

# 6c 

angles[~safe2, 0] = np.arctan2( 

dcm_transformed[~safe2, 1, 0] + dcm_transformed[~safe2, 0, 1], 

dcm_transformed[~safe2, 0, 0] - dcm_transformed[~safe2, 1, 1] 

) 

 

# Step 7 

if seq[0] == seq[2]: 

# lambda = 0, so we can only ensure angle2 -> [0, pi] 

adjust_mask = np.logical_or(angles[:, 1] < 0, angles[:, 1] > np.pi) 

else: 

# lambda = + or - pi/2, so we can ensure angle2 -> [-pi/2, pi/2] 

adjust_mask = np.logical_or(angles[:, 1] < -np.pi / 2, 

angles[:, 1] > np.pi / 2) 

 

# Dont adjust gimbal locked angle sequences 

adjust_mask = np.logical_and(adjust_mask, safe_mask) 

 

angles[adjust_mask, 0] += np.pi 

angles[adjust_mask, 1] = 2 * offset - angles[adjust_mask, 1] 

angles[adjust_mask, 2] -= np.pi 

 

angles[angles < -np.pi] += 2 * np.pi 

angles[angles > np.pi] -= 2 * np.pi 

 

# Step 8 

if not np.all(safe_mask): 

warnings.warn("Gimbal lock detected. Setting third angle to zero since" 

" it is not possible to uniquely determine all angles.") 

 

# Reverse role of extrinsic and intrinsic rotations, but let third angle be 

# zero for gimbal locked cases 

if extrinsic: 

angles = angles[:, ::-1] 

return angles 

 

 

def _make_elementary_quat(axis, angles): 

quat = np.zeros((angles.shape[0], 4)) 

 

quat[:, 3] = np.cos(angles / 2) 

quat[:, _AXIS_TO_IND[axis]] = np.sin(angles / 2) 

return quat 

 

 

def _compose_quat(p, q): 

product = np.empty((max(p.shape[0], q.shape[0]), 4)) 

product[:, 3] = p[:, 3] * q[:, 3] - np.sum(p[:, :3] * q[:, :3], axis=1) 

product[:, :3] = (p[:, None, 3] * q[:, :3] + q[:, None, 3] * p[:, :3] + 

np.cross(p[:, :3], q[:, :3])) 

return product 

 

 

def _elementary_quat_compose(seq, angles, intrinsic=False): 

result = _make_elementary_quat(seq[0], angles[:, 0]) 

 

for idx, axis in enumerate(seq[1:], start=1): 

if intrinsic: 

result = _compose_quat( 

result, 

_make_elementary_quat(axis, angles[:, idx])) 

else: 

result = _compose_quat( 

_make_elementary_quat(axis, angles[:, idx]), 

result) 

return result 

 

 

class Rotation(object): 

"""Rotation in 3 dimensions. 

 

This class provides an interface to initialize from and represent rotations 

with: 

 

- Quaternions 

- Direction Cosine Matrices 

- Rotation Vectors 

- Euler angles 

 

The following operations on rotations are supported: 

 

- Application on vectors 

- Rotation Composition 

- Rotation Inversion 

- Rotation Indexing 

 

Indexing within a rotation is supported since multiple rotation transforms 

can be stored within a single `Rotation` instance. 

 

To create `Rotation` objects use ``from_...`` methods (see examples below). 

``Rotation(...)`` is not supposed to be instantiated directly. 

 

Methods 

------- 

__len__ 

from_quat 

from_dcm 

from_rotvec 

from_euler 

as_quat 

as_dcm 

as_rotvec 

as_euler 

apply 

__mul__ 

inv 

__getitem__ 

random 

match_vectors 

 

See Also 

-------- 

Slerp 

 

Notes 

----- 

.. versionadded: 1.2.0 

 

Examples 

-------- 

>>> from scipy.spatial.transform import Rotation as R 

 

A `Rotation` instance can be initialized in any of the above formats and 

converted to any of the others. The underlying object is independent of the 

representation used for initialization. 

 

Consider a counter-clockwise rotation of 90 degrees about the z-axis. This 

corresponds to the following quaternion (in scalar-last format): 

 

>>> r = R.from_quat([0, 0, np.sin(np.pi/4), np.cos(np.pi/4)]) 

 

The rotation can be expressed in any of the other formats: 

 

>>> r.as_dcm() 

array([[ 2.22044605e-16, -1.00000000e+00, 0.00000000e+00], 

[ 1.00000000e+00, 2.22044605e-16, 0.00000000e+00], 

[ 0.00000000e+00, 0.00000000e+00, 1.00000000e+00]]) 

>>> r.as_rotvec() 

array([0. , 0. , 1.57079633]) 

>>> r.as_euler('zyx', degrees=True) 

array([90., 0., 0.]) 

 

The same rotation can be initialized using a direction cosine matrix: 

 

>>> r = R.from_dcm(np.array([ 

... [0, -1, 0], 

... [1, 0, 0], 

... [0, 0, 1]])) 

 

Representation in other formats: 

 

>>> r.as_quat() 

array([0. , 0. , 0.70710678, 0.70710678]) 

>>> r.as_rotvec() 

array([0. , 0. , 1.57079633]) 

>>> r.as_euler('zyx', degrees=True) 

array([90., 0., 0.]) 

 

The rotation vector corresponding to this rotation is given by: 

 

>>> r = R.from_rotvec(np.pi/2 * np.array([0, 0, 1])) 

 

Representation in other formats: 

 

>>> r.as_quat() 

array([0. , 0. , 0.70710678, 0.70710678]) 

>>> r.as_dcm() 

array([[ 2.22044605e-16, -1.00000000e+00, 0.00000000e+00], 

[ 1.00000000e+00, 2.22044605e-16, 0.00000000e+00], 

[ 0.00000000e+00, 0.00000000e+00, 1.00000000e+00]]) 

>>> r.as_euler('zyx', degrees=True) 

array([90., 0., 0.]) 

 

The ``from_euler`` method is quite flexible in the range of input formats 

it supports. Here we initialize a single rotation about a single axis: 

 

>>> r = R.from_euler('z', 90, degrees=True) 

 

Again, the object is representation independent and can be converted to any 

other format: 

 

>>> r.as_quat() 

array([0. , 0. , 0.70710678, 0.70710678]) 

>>> r.as_dcm() 

array([[ 2.22044605e-16, -1.00000000e+00, 0.00000000e+00], 

[ 1.00000000e+00, 2.22044605e-16, 0.00000000e+00], 

[ 0.00000000e+00, 0.00000000e+00, 1.00000000e+00]]) 

>>> r.as_rotvec() 

array([0. , 0. , 1.57079633]) 

 

It is also possible to initialize multiple rotations in a single instance 

using any of the `from_...` functions. Here we initialize a stack of 3 

rotations using the ``from_euler`` method: 

 

>>> r = R.from_euler('zyx', [ 

... [90, 0, 0], 

... [0, 45, 0], 

... [45, 60, 30]], degrees=True) 

 

The other representations also now return a stack of 3 rotations. For 

example: 

 

>>> r.as_quat() 

array([[0. , 0. , 0.70710678, 0.70710678], 

[0. , 0.38268343, 0. , 0.92387953], 

[0.39190384, 0.36042341, 0.43967974, 0.72331741]]) 

 

Applying the above rotations onto a vector: 

 

>>> v = [1, 2, 3] 

>>> r.apply(v) 

array([[-2. , 1. , 3. ], 

[ 2.82842712, 2. , 1.41421356], 

[ 2.24452282, 0.78093109, 2.89002836]]) 

 

A `Rotation` instance can be indexed and sliced as if it were a single 

1D array or list: 

 

>>> r.as_quat() 

array([[0. , 0. , 0.70710678, 0.70710678], 

[0. , 0.38268343, 0. , 0.92387953], 

[0.39190384, 0.36042341, 0.43967974, 0.72331741]]) 

>>> p = r[0] 

>>> p.as_dcm() 

array([[ 2.22044605e-16, -1.00000000e+00, 0.00000000e+00], 

[ 1.00000000e+00, 2.22044605e-16, 0.00000000e+00], 

[ 0.00000000e+00, 0.00000000e+00, 1.00000000e+00]]) 

>>> q = r[1:3] 

>>> q.as_quat() 

array([[0. , 0.38268343, 0. , 0.92387953], 

[0.39190384, 0.36042341, 0.43967974, 0.72331741]]) 

 

Multiple rotations can be composed using the ``*`` operator: 

 

>>> r1 = R.from_euler('z', 90, degrees=True) 

>>> r2 = R.from_rotvec([np.pi/4, 0, 0]) 

>>> v = [1, 2, 3] 

>>> r2.apply(r1.apply(v)) 

array([-2. , -1.41421356, 2.82842712]) 

>>> r3 = r2 * r1 # Note the order 

>>> r3.apply(v) 

array([-2. , -1.41421356, 2.82842712]) 

 

Finally, it is also possible to invert rotations: 

 

>>> r1 = R.from_euler('z', [90, 45], degrees=True) 

>>> r2 = r1.inv() 

>>> r2.as_euler('zyx', degrees=True) 

array([[-90., 0., 0.], 

[-45., 0., 0.]]) 

 

These examples serve as an overview into the `Rotation` class and highlight 

major functionalities. For more thorough examples of the range of input and 

output formats supported, consult the individual method's examples. 

 

""" 

def __init__(self, quat, normalized=False, copy=True): 

self._single = False 

quat = np.asarray(quat, dtype=float) 

 

if quat.ndim not in [1, 2] or quat.shape[-1] != 4: 

raise ValueError("Expected `quat` to have shape (4,) or (N x 4), " 

"got {}.".format(quat.shape)) 

 

# If a single quaternion is given, convert it to a 2D 1 x 4 matrix but 

# set self._single to True so that we can return appropriate objects 

# in the `to_...` methods 

if quat.shape == (4,): 

quat = quat[None, :] 

self._single = True 

 

if normalized: 

self._quat = quat.copy() if copy else quat 

else: 

self._quat = quat.copy() 

norms = scipy.linalg.norm(quat, axis=1) 

 

zero_norms = norms == 0 

if zero_norms.any(): 

raise ValueError("Found zero norm quaternions in `quat`.") 

 

# Ensure norm is broadcasted along each column. 

self._quat[~zero_norms] /= norms[~zero_norms][:, None] 

 

def __len__(self): 

"""Number of rotations contained in this object. 

 

Multiple rotations can be stored in a single instance. 

 

Returns 

------- 

length : int 

Number of rotations stored in object. 

 

""" 

return self._quat.shape[0] 

 

@classmethod 

def from_quat(cls, quat, normalized=False): 

"""Initialize from quaternions. 

 

3D rotations can be represented using unit-norm quaternions [1]_. 

 

Parameters 

---------- 

quat : array_like, shape (N, 4) or (4,) 

Each row is a (possibly non-unit norm) quaternion in scalar-last 

(x, y, z, w) format. 

normalized : bool, optional 

If False, input quaternions are normalized to unit norm before 

being stored. If True, quaternions are assumed to already have 

unit norm and are stored as given. Default is False. 

 

Returns 

------- 

rotation : `Rotation` instance 

Object containing the rotations represented by input quaternions. 

 

References 

---------- 

.. [1] https://en.wikipedia.org/wiki/Quaternions_and_spatial_rotation 

 

Examples 

-------- 

>>> from scipy.spatial.transform import Rotation as R 

 

Initialize a single rotation: 

 

>>> r = R.from_quat([1, 0, 0, 0]) 

>>> r.as_quat() 

array([1., 0., 0., 0.]) 

>>> r.as_quat().shape 

(4,) 

 

Initialize multiple rotations in a single object: 

 

>>> r = R.from_quat([ 

... [1, 0, 0, 0], 

... [0, 0, 0, 1] 

... ]) 

>>> r.as_quat() 

array([[1., 0., 0., 0.], 

[0., 0., 0., 1.]]) 

>>> r.as_quat().shape 

(2, 4) 

 

It is also possible to have a stack of a single rotation: 

 

>>> r = R.from_quat([[0, 0, 0, 1]]) 

>>> r.as_quat() 

array([[0., 0., 0., 1.]]) 

>>> r.as_quat().shape 

(1, 4) 

 

By default, quaternions are normalized before initialization. 

 

>>> r = R.from_quat([0, 0, 1, 1]) 

>>> r.as_quat() 

array([0. , 0. , 0.70710678, 0.70710678]) 

 

If unit norms are ensured, skip the normalization step. 

 

>>> r = R.from_quat([0, 0, 1, 0], normalized=True) 

>>> r.as_quat() 

array([0., 0., 1., 0.]) 

 

""" 

return cls(quat, normalized) 

 

@classmethod 

def from_dcm(cls, dcm): 

"""Initialize from direction cosine matrices. 

 

Rotations in 3 dimensions can be represented using 3 x 3 proper 

orthogonal matrices [1]_. If the input is not proper orthogonal, 

an approximation is created using the method described in [2]_. 

 

Parameters 

---------- 

dcm : array_like, shape (N, 3, 3) or (3, 3) 

A single matrix or a stack of matrices, where ``dcm[i]`` is the i-th 

matrix. 

 

Returns 

------- 

rotation : `Rotation` instance 

Object containing the rotations represented by the input direction 

cosine matrices. 

 

References 

---------- 

.. [1] https://en.wikipedia.org/wiki/Rotation_matrix#In_three_dimensions 

.. [2] F. Landis Markley, "Unit Quaternion from Rotation Matrix", 

Journal of guidance, control, and dynamics vol. 31.2, pp. 

440-442, 2008. 

 

Examples 

-------- 

>>> from scipy.spatial.transform import Rotation as R 

 

Initialize a single rotation: 

 

>>> r = R.from_dcm([ 

... [0, -1, 0], 

... [1, 0, 0], 

... [0, 0, 1]]) 

>>> r.as_dcm().shape 

(3, 3) 

 

Initialize multiple rotations in a single object: 

 

>>> r = R.from_dcm([ 

... [ 

... [0, -1, 0], 

... [1, 0, 0], 

... [0, 0, 1], 

... ], 

... [ 

... [1, 0, 0], 

... [0, 0, -1], 

... [0, 1, 0], 

... ]]) 

>>> r.as_dcm().shape 

(2, 3, 3) 

 

If input matrices are not special orthogonal (orthogonal with 

determinant equal to +1), then a special orthogonal estimate is stored: 

 

>>> a = np.array([ 

... [0, -0.5, 0], 

... [0.5, 0, 0], 

... [0, 0, 0.5]]) 

>>> np.linalg.det(a) 

0.12500000000000003 

>>> r = R.from_dcm(a) 

>>> dcm = r.as_dcm() 

>>> dcm 

array([[-0.38461538, -0.92307692, 0. ], 

[ 0.92307692, -0.38461538, 0. ], 

[ 0. , 0. , 1. ]]) 

>>> np.linalg.det(dcm) 

1.0000000000000002 

 

It is also possible to have a stack containing a single rotation: 

 

>>> r = R.from_dcm([[ 

... [0, -1, 0], 

... [1, 0, 0], 

... [0, 0, 1]]]) 

>>> r.as_dcm() 

array([[[ 0., -1., 0.], 

[ 1., 0., 0.], 

[ 0., 0., 1.]]]) 

>>> r.as_dcm().shape 

(1, 3, 3) 

 

""" 

is_single = False 

dcm = np.asarray(dcm, dtype=float) 

 

if dcm.ndim not in [2, 3] or dcm.shape[-2:] != (3, 3): 

raise ValueError("Expected `dcm` to have shape (3, 3) or " 

"(N, 3, 3), got {}".format(dcm.shape)) 

 

# If a single dcm is given, convert it to 3D 1 x 3 x 3 matrix but set 

# self._single to True so that we can return appropriate objects in 

# the `to_...` methods 

if dcm.shape == (3, 3): 

dcm = dcm.reshape((1, 3, 3)) 

is_single = True 

 

num_rotations = dcm.shape[0] 

 

decision_matrix = np.empty((num_rotations, 4)) 

decision_matrix[:, :3] = dcm.diagonal(axis1=1, axis2=2) 

decision_matrix[:, -1] = decision_matrix[:, :3].sum(axis=1) 

choices = decision_matrix.argmax(axis=1) 

 

quat = np.empty((num_rotations, 4)) 

 

ind = np.nonzero(choices != 3)[0] 

i = choices[ind] 

j = (i + 1) % 3 

k = (j + 1) % 3 

 

quat[ind, i] = 1 - decision_matrix[ind, -1] + 2 * dcm[ind, i, i] 

quat[ind, j] = dcm[ind, j, i] + dcm[ind, i, j] 

quat[ind, k] = dcm[ind, k, i] + dcm[ind, i, k] 

quat[ind, 3] = dcm[ind, k, j] - dcm[ind, j, k] 

 

ind = np.nonzero(choices == 3)[0] 

quat[ind, 0] = dcm[ind, 2, 1] - dcm[ind, 1, 2] 

quat[ind, 1] = dcm[ind, 0, 2] - dcm[ind, 2, 0] 

quat[ind, 2] = dcm[ind, 1, 0] - dcm[ind, 0, 1] 

quat[ind, 3] = 1 + decision_matrix[ind, -1] 

 

quat /= np.linalg.norm(quat, axis=1)[:, None] 

 

if is_single: 

return cls(quat[0], normalized=True, copy=False) 

else: 

return cls(quat, normalized=True, copy=False) 

 

@classmethod 

def from_rotvec(cls, rotvec): 

"""Initialize from rotation vectors. 

 

A rotation vector is a 3 dimensional vector which is co-directional to 

the axis of rotation and whose norm gives the angle of rotation (in 

radians) [1]_. 

 

Parameters 

---------- 

rotvec : array_like, shape (N, 3) or (3,) 

A single vector or a stack of vectors, where `rot_vec[i]` gives 

the ith rotation vector. 

 

Returns 

------- 

rotation : `Rotation` instance 

Object containing the rotations represented by input rotation 

vectors. 

 

References 

---------- 

.. [1] https://en.wikipedia.org/wiki/Axis%E2%80%93angle_representation#Rotation_vector 

 

Examples 

-------- 

>>> from scipy.spatial.transform import Rotation as R 

 

Initialize a single rotation: 

 

>>> r = R.from_rotvec(np.pi/2 * np.array([0, 0, 1])) 

>>> r.as_rotvec() 

array([0. , 0. , 1.57079633]) 

>>> r.as_rotvec().shape 

(3,) 

 

Initialize multiple rotations in one object: 

 

>>> r = R.from_rotvec([ 

... [0, 0, np.pi/2], 

... [np.pi/2, 0, 0]]) 

>>> r.as_rotvec() 

array([[0. , 0. , 1.57079633], 

[1.57079633, 0. , 0. ]]) 

>>> r.as_rotvec().shape 

(2, 3) 

 

It is also possible to have a stack of a single rotaton: 

 

>>> r = R.from_rotvec([[0, 0, np.pi/2]]) 

>>> r.as_rotvec().shape 

(1, 3) 

 

""" 

is_single = False 

rotvec = np.asarray(rotvec, dtype=float) 

 

if rotvec.ndim not in [1, 2] or rotvec.shape[-1] != 3: 

raise ValueError("Expected `rot_vec` to have shape (3,) " 

"or (N, 3), got {}".format(rotvec.shape)) 

 

# If a single vector is given, convert it to a 2D 1 x 3 matrix but 

# set self._single to True so that we can return appropriate objects 

# in the `as_...` methods 

if rotvec.shape == (3,): 

rotvec = rotvec[None, :] 

is_single = True 

 

num_rotations = rotvec.shape[0] 

 

norms = np.linalg.norm(rotvec, axis=1) 

small_angle = (norms <= 1e-3) 

large_angle = ~small_angle 

 

scale = np.empty(num_rotations) 

scale[small_angle] = (0.5 - norms[small_angle] ** 2 / 48 + 

norms[small_angle] ** 4 / 3840) 

scale[large_angle] = (np.sin(norms[large_angle] / 2) / 

norms[large_angle]) 

 

quat = np.empty((num_rotations, 4)) 

quat[:, :3] = scale[:, None] * rotvec 

quat[:, 3] = np.cos(norms / 2) 

 

if is_single: 

return cls(quat[0], normalized=True, copy=False) 

else: 

return cls(quat, normalized=True, copy=False) 

 

@classmethod 

def from_euler(cls, seq, angles, degrees=False): 

"""Initialize from Euler angles. 

 

Rotations in 3 dimensions can be represented by a sequece of 3 

rotations around a sequence of axes. In theory, any three axes spanning 

the 3D Euclidean space are enough. In practice the axes of rotation are 

chosen to be the basis vectors. 

 

The three rotations can either be in a global frame of reference 

(extrinsic) or in a body centred frame of refernce (intrinsic), which 

is attached to, and moves with, the object under rotation [1]_. 

 

Parameters 

---------- 

seq : string 

Specifies sequence of axes for rotations. Up to 3 characters 

belonging to the set {'X', 'Y', 'Z'} for intrinsic rotations, or 

{'x', 'y', 'z'} for extrinsic rotations. Extrinsic and intrinsic 

rotations cannot be mixed in one function call. 

angles : float or array_like, shape (N,) or (N, [1 or 2 or 3]) 

Euler angles specified in radians (`degrees` is False) or degrees 

(`degrees` is True). 

For a single character `seq`, `angles` can be: 

 

- a single value 

- array_like with shape (N,), where each `angle[i]` 

corresponds to a single rotation 

- array_like with shape (N, 1), where each `angle[i, 0]` 

corresponds to a single rotation 

 

For 2- and 3-character wide `seq`, `angles` can be: 

 

- array_like with shape (W,) where `W` is the width of 

`seq`, which corresponds to a single rotation with `W` axes 

- array_like with shape (N, W) where each `angle[i]` 

corresponds to a sequence of Euler angles describing a single 

rotation 

 

degrees : bool, optional 

If True, then the given angles are assumed to be in degrees. 

Default is False. 

 

Returns 

------- 

rotation : `Rotation` instance 

Object containing the rotation represented by the sequence of 

rotations around given axes with given angles. 

 

References 

---------- 

.. [1] https://en.wikipedia.org/wiki/Euler_angles#Definition_by_intrinsic_rotations 

 

Examples 

-------- 

>>> from scipy.spatial.transform import Rotation as R 

 

Initialize a single rotation along a single axis: 

 

>>> r = R.from_euler('x', 90, degrees=True) 

>>> r.as_quat().shape 

(4,) 

 

Initialize a single rotation with a given axis sequence: 

 

>>> r = R.from_euler('zyx', [90, 45, 30], degrees=True) 

>>> r.as_quat().shape 

(4,) 

 

Initialize a stack with a single rotation around a single axis: 

 

>>> r = R.from_euler('x', [90], degrees=True) 

>>> r.as_quat().shape 

(1, 4) 

 

Initialize a stack with a single rotation with an axis sequence: 

 

>>> r = R.from_euler('zyx', [[90, 45, 30]], degrees=True) 

>>> r.as_quat().shape 

(1, 4) 

 

Initialize multiple elementary rotations in one object: 

 

>>> r = R.from_euler('x', [90, 45, 30], degrees=True) 

>>> r.as_quat().shape 

(3, 4) 

 

Initialize multiple rotations in one object: 

 

>>> r = R.from_euler('zyx', [[90, 45, 30], [35, 45, 90]], degrees=True) 

>>> r.as_quat().shape 

(2, 4) 

 

""" 

num_axes = len(seq) 

if num_axes < 1 or num_axes > 3: 

raise ValueError("Expected axis specification to be a non-empty " 

"string of upto 3 characters, got {}".format(seq)) 

 

intrinsic = (re.match(r'^[XYZ]{1,3}$', seq) is not None) 

extrinsic = (re.match(r'^[xyz]{1,3}$', seq) is not None) 

if not (intrinsic or extrinsic): 

raise ValueError("Expected axes from `seq` to be from ['x', 'y', " 

"'z'] or ['X', 'Y', 'Z'], got {}".format(seq)) 

 

if any(seq[i] == seq[i+1] for i in range(num_axes - 1)): 

raise ValueError("Expected consecutive axes to be different, " 

"got {}".format(seq)) 

 

seq = seq.lower() 

 

angles = np.asarray(angles, dtype=float) 

if degrees: 

angles = np.deg2rad(angles) 

 

is_single = False 

# Prepare angles to have shape (num_rot, num_axes) 

if num_axes == 1: 

if angles.ndim == 0: 

# (1, 1) 

angles = angles.reshape((1, 1)) 

is_single = True 

elif angles.ndim == 1: 

# (N, 1) 

angles = angles[:, None] 

elif angles.ndim == 2 and angles.shape[-1] != 1: 

raise ValueError("Expected `angles` parameter to have shape " 

"(N, 1), got {}.".format(angles.shape)) 

elif angles.ndim > 2: 

raise ValueError("Expected float, 1D array, or 2D array for " 

"parameter `angles` corresponding to `seq`, " 

"got shape {}.".format(angles.shape)) 

else: # 2 or 3 axes 

if angles.ndim not in [1, 2] or angles.shape[-1] != num_axes: 

raise ValueError("Expected `angles` to be at most " 

"2-dimensional with width equal to number " 

"of axes specified, got {} for shape".format( 

angles.shape)) 

 

if angles.ndim == 1: 

# (1, num_axes) 

angles = angles[None, :] 

is_single = True 

 

# By now angles should have shape (num_rot, num_axes) 

# sanity check 

if angles.ndim != 2 or angles.shape[-1] != num_axes: 

raise ValueError("Expected angles to have shape (num_rotations, " 

"num_axes), got {}.".format(angles.shape)) 

 

quat = _elementary_quat_compose(seq, angles, intrinsic) 

return cls(quat[0] if is_single else quat, normalized=True, copy=False) 

 

def as_quat(self): 

"""Represent as quaternions. 

 

Rotations in 3 dimensions can be represented using unit norm 

quaternions [1]_. The mapping from quaternions to rotations is 

two-to-one, i.e. quaternions ``q`` and ``-q``, where ``-q`` simply 

reverses the sign of each component, represent the same spatial 

rotation. 

 

Returns 

------- 

quat : `numpy.ndarray`, shape (4,) or (N, 4) 

Shape depends on shape of inputs used for initialization. 

 

References 

---------- 

.. [1] https://en.wikipedia.org/wiki/Quaternions_and_spatial_rotation 

 

Examples 

-------- 

>>> from scipy.spatial.transform import Rotation as R 

 

Represent a single rotation: 

 

>>> r = R.from_dcm([ 

... [0, -1, 0], 

... [1, 0, 0], 

... [0, 0, 1]]) 

>>> r.as_quat() 

array([0. , 0. , 0.70710678, 0.70710678]) 

>>> r.as_quat().shape 

(4,) 

 

Represent a stack with a single rotation: 

 

>>> r = R.from_quat([[0, 0, 0, 1]]) 

>>> r.as_quat().shape 

(1, 4) 

 

Represent multiple rotaions in a single object: 

 

>>> r = R.from_rotvec([[np.pi, 0, 0], [0, 0, np.pi/2]]) 

>>> r.as_quat().shape 

(2, 4) 

 

""" 

if self._single: 

return self._quat[0].copy() 

else: 

return self._quat.copy() 

 

def as_dcm(self): 

"""Represent as direction cosine matrices. 

 

3D rotations can be represented using direction cosine matrices, which 

are 3 x 3 real orthogonal matrices with determinant equal to +1 [1]_. 

 

Returns 

------- 

dcm : ndarray, shape (3, 3) or (N, 3, 3) 

Shape depends on shape of inputs used for initialization. 

 

References 

---------- 

.. [1] https://en.wikipedia.org/wiki/Rotation_matrix#In_three_dimensions 

 

Examples 

-------- 

>>> from scipy.spatial.transform import Rotation as R 

 

Represent a single rotation: 

 

>>> r = R.from_rotvec([0, 0, np.pi/2]) 

>>> r.as_dcm() 

array([[ 2.22044605e-16, -1.00000000e+00, 0.00000000e+00], 

[ 1.00000000e+00, 2.22044605e-16, 0.00000000e+00], 

[ 0.00000000e+00, 0.00000000e+00, 1.00000000e+00]]) 

>>> r.as_dcm().shape 

(3, 3) 

 

Represent a stack with a single rotation: 

 

>>> r = R.from_quat([[1, 1, 0, 0]]) 

>>> r.as_dcm() 

array([[[ 0., 1., 0.], 

[ 1., 0., 0.], 

[ 0., 0., -1.]]]) 

>>> r.as_dcm().shape 

(1, 3, 3) 

 

Represent multiple rotations: 

 

>>> r = R.from_rotvec([[np.pi/2, 0, 0], [0, 0, np.pi/2]]) 

>>> r.as_dcm() 

array([[[ 1.00000000e+00, 0.00000000e+00, 0.00000000e+00], 

[ 0.00000000e+00, 2.22044605e-16, -1.00000000e+00], 

[ 0.00000000e+00, 1.00000000e+00, 2.22044605e-16]], 

[[ 2.22044605e-16, -1.00000000e+00, 0.00000000e+00], 

[ 1.00000000e+00, 2.22044605e-16, 0.00000000e+00], 

[ 0.00000000e+00, 0.00000000e+00, 1.00000000e+00]]]) 

>>> r.as_dcm().shape 

(2, 3, 3) 

 

""" 

x = self._quat[:, 0] 

y = self._quat[:, 1] 

z = self._quat[:, 2] 

w = self._quat[:, 3] 

 

x2 = x * x 

y2 = y * y 

z2 = z * z 

w2 = w * w 

 

xy = x * y 

zw = z * w 

xz = x * z 

yw = y * w 

yz = y * z 

xw = x * w 

 

num_rotations = len(self) 

dcm = np.empty((num_rotations, 3, 3)) 

 

dcm[:, 0, 0] = x2 - y2 - z2 + w2 

dcm[:, 1, 0] = 2 * (xy + zw) 

dcm[:, 2, 0] = 2 * (xz - yw) 

 

dcm[:, 0, 1] = 2 * (xy - zw) 

dcm[:, 1, 1] = - x2 + y2 - z2 + w2 

dcm[:, 2, 1] = 2 * (yz + xw) 

 

dcm[:, 0, 2] = 2 * (xz + yw) 

dcm[:, 1, 2] = 2 * (yz - xw) 

dcm[:, 2, 2] = - x2 - y2 + z2 + w2 

 

if self._single: 

return dcm[0] 

else: 

return dcm 

 

def as_rotvec(self): 

"""Represent as rotation vectors. 

 

A rotation vector is a 3 dimensional vector which is co-directional to 

the axis of rotation and whose norm gives the angle of rotation (in 

radians) [1]_. 

 

Returns 

------- 

rotvec : ndarray, shape (3,) or (N, 3) 

Shape depends on shape of inputs used for initialization. 

 

References 

---------- 

.. [1] https://en.wikipedia.org/wiki/Axis%E2%80%93angle_representation#Rotation_vector 

 

Examples 

-------- 

>>> from scipy.spatial.transform import Rotation as R 

 

Represent a single rotation: 

 

>>> r = R.from_euler('z', 90, degrees=True) 

>>> r.as_rotvec() 

array([0. , 0. , 1.57079633]) 

>>> r.as_rotvec().shape 

(3,) 

 

Represent a stack with a single rotation: 

 

>>> r = R.from_quat([[0, 0, 1, 1]]) 

>>> r.as_rotvec() 

array([[0. , 0. , 1.57079633]]) 

>>> r.as_rotvec().shape 

(1, 3) 

 

Represent multiple rotations in a single object: 

 

>>> r = R.from_quat([[0, 0, 1, 1], [1, 1, 0, 1]]) 

>>> r.as_rotvec() 

array([[0. , 0. , 1.57079633], 

[1.35102172, 1.35102172, 0. ]]) 

>>> r.as_rotvec().shape 

(2, 3) 

 

""" 

quat = self._quat.copy() 

# w > 0 to ensure 0 <= angle <= pi 

quat[quat[:, 3] < 0] *= -1 

 

angle = 2 * np.arctan2(np.linalg.norm(quat[:, :3], axis=1), quat[:, 3]) 

 

small_angle = (angle <= 1e-3) 

large_angle = ~small_angle 

 

num_rotations = len(self) 

scale = np.empty(num_rotations) 

scale[small_angle] = (2 + angle[small_angle] ** 2 / 12 + 

7 * angle[small_angle] ** 4 / 2880) 

scale[large_angle] = (angle[large_angle] / 

np.sin(angle[large_angle] / 2)) 

 

rotvec = scale[:, None] * quat[:, :3] 

 

if self._single: 

return rotvec[0] 

else: 

return rotvec 

 

def as_euler(self, seq, degrees=False): 

"""Represent as Euler angles. 

 

Any orientation can be expressed as a composition of 3 elementary 

rotations. Once the axis sequence has been chosen, Euler angles define 

the angle of rotation around each respective axis [1]_. 

 

The algorithm from [2]_ has been used to calculate Euler angles for the 

rotation about a given sequence of axes. 

 

Euler angles suffer from the problem of gimbal lock [3]_, where the 

representation loses a degree of freedom and it is not possible to 

determine the first and third angles uniquely. In this case, 

a warning is raised, and the third angle is set to zero. Note however 

that the returned angles still represent the correct rotation. 

 

Parameters 

---------- 

seq : string, length 3 

3 characters belonging to the set {'X', 'Y', 'Z'} for intrinsic 

rotations, or {'x', 'y', 'z'} for extrinsic rotations [1]_. 

Adjacent axes cannot be the same. 

Extrinsic and intrinsic rotations cannot be mixed in one function 

call. 

degrees : boolean, optional 

Returned angles are in degrees if this flag is True, else they are 

in radians. Default is False. 

 

Returns 

------- 

angles : ndarray, shape (3,) or (N, 3) 

Shape depends on shape of inputs used to initialize object. 

The returned angles are in the range: 

 

- First angle belongs to [-180, 180] degrees (both inclusive) 

- Third angle belongs to [-180, 180] degrees (both inclusive) 

- Second angle belongs to: 

 

- [-90, 90] degrees if all axes are different (like xyz) 

- [0, 180] degrees if first and third axes are the same 

(like zxz) 

 

References 

---------- 

.. [1] https://en.wikipedia.org/wiki/Euler_angles#Definition_by_intrinsic_rotations 

.. [2] Malcolm D. Shuster, F. Landis Markley, "General formula for 

extraction the Euler angles", Journal of guidance, control, and 

dynamics, vol. 29.1, pp. 215-221. 2006 

.. [3] https://en.wikipedia.org/wiki/Gimbal_lock#In_applied_mathematics 

 

Examples 

-------- 

>>> from scipy.spatial.transform import Rotation as R 

 

Represent a single rotation: 

 

>>> r = R.from_rotvec([0, 0, np.pi/2]) 

>>> r.as_euler('zxy', degrees=True) 

array([90., 0., 0.]) 

>>> r.as_euler('zxy', degrees=True).shape 

(3,) 

 

Represent a stack of single rotation: 

 

>>> r = R.from_rotvec([[0, 0, np.pi/2]]) 

>>> r.as_euler('zxy', degrees=True) 

array([[90., 0., 0.]]) 

>>> r.as_euler('zxy', degrees=True).shape 

(1, 3) 

 

Represent multiple rotations in a single object: 

 

>>> r = R.from_rotvec([ 

... [0, 0, np.pi/2], 

... [0, -np.pi/3, 0], 

... [np.pi/4, 0, 0]]) 

>>> r.as_euler('zxy', degrees=True) 

array([[ 90., 0., 0.], 

[ 0., 0., -60.], 

[ 0., 45., 0.]]) 

>>> r.as_euler('zxy', degrees=True).shape 

(3, 3) 

 

""" 

if len(seq) != 3: 

raise ValueError("Expected 3 axes, got {}.".format(seq)) 

 

intrinsic = (re.match(r'^[XYZ]{1,3}$', seq) is not None) 

extrinsic = (re.match(r'^[xyz]{1,3}$', seq) is not None) 

if not (intrinsic or extrinsic): 

raise ValueError("Expected axes from `seq` to be from " 

"['x', 'y', 'z'] or ['X', 'Y', 'Z'], " 

"got {}".format(seq)) 

 

if any(seq[i] == seq[i+1] for i in range(2)): 

raise ValueError("Expected consecutive axes to be different, " 

"got {}".format(seq)) 

 

seq = seq.lower() 

 

angles = _compute_euler_from_dcm(self.as_dcm(), seq, extrinsic) 

if degrees: 

angles = np.rad2deg(angles) 

 

return angles[0] if self._single else angles 

 

def apply(self, vectors, inverse=False): 

"""Apply this rotation to a set of vectors. 

 

If the original frame rotates to the final frame by this rotation, then 

its application to a vector can be seen in two ways: 

 

- As a projection of vector components expressed in the final frame 

to the original frame. 

- As the physical rotation of a vector being glued to the original 

frame as it rotates. In this case the vector components are 

expressed in the original frame before and after the rotation. 

 

In terms of DCMs, this application is the same as 

``self.as_dcm().dot(vectors)``. 

 

Parameters 

---------- 

vectors : array_like, shape (3,) or (N, 3) 

Each `vectors[i]` represents a vector in 3D space. A single vector 

can either be specified with shape `(3, )` or `(1, 3)`. The number 

of rotations and number of vectors given must follow standard numpy 

broadcasting rules: either one of them equals unity or they both 

equal each other. 

inverse : boolean, optional 

If True then the inverse of the rotation(s) is applied to the input 

vectors. Default is False. 

 

Returns 

------- 

rotated_vectors : ndarray, shape (3,) or (N, 3) 

Result of applying rotation on input vectors. 

Shape depends on the following cases: 

 

- If object contains a single rotation (as opposed to a stack 

with a single rotation) and a single vector is specified with 

shape ``(3,)``, then `rotated_vectors` has shape ``(3,)``. 

- In all other cases, `rotated_vectors` has shape ``(N, 3)``, 

where ``N`` is either the number of rotations or vectors. 

 

Examples 

-------- 

>>> from scipy.spatial.transform import Rotation as R 

 

Single rotation applied on a single vector: 

 

>>> vector = np.array([1, 0, 0]) 

>>> r = R.from_rotvec([0, 0, np.pi/2]) 

>>> r.as_dcm() 

array([[ 2.22044605e-16, -1.00000000e+00, 0.00000000e+00], 

[ 1.00000000e+00, 2.22044605e-16, 0.00000000e+00], 

[ 0.00000000e+00, 0.00000000e+00, 1.00000000e+00]]) 

>>> r.apply(vector) 

array([2.22044605e-16, 1.00000000e+00, 0.00000000e+00]) 

>>> r.apply(vector).shape 

(3,) 

 

Single rotation applied on multiple vectors: 

 

>>> vectors = np.array([ 

... [1, 0, 0], 

... [1, 2, 3]]) 

>>> r = R.from_rotvec([0, 0, np.pi/4]) 

>>> r.as_dcm() 

array([[ 0.70710678, -0.70710678, 0. ], 

[ 0.70710678, 0.70710678, 0. ], 

[ 0. , 0. , 1. ]]) 

>>> r.apply(vectors) 

array([[ 0.70710678, 0.70710678, 0. ], 

[-0.70710678, 2.12132034, 3. ]]) 

>>> r.apply(vectors).shape 

(2, 3) 

 

Multiple rotations on a single vector: 

 

>>> r = R.from_rotvec([[0, 0, np.pi/4], [np.pi/2, 0, 0]]) 

>>> vector = np.array([1,2,3]) 

>>> r.as_dcm() 

array([[[ 7.07106781e-01, -7.07106781e-01, 0.00000000e+00], 

[ 7.07106781e-01, 7.07106781e-01, 0.00000000e+00], 

[ 0.00000000e+00, 0.00000000e+00, 1.00000000e+00]], 

[[ 1.00000000e+00, 0.00000000e+00, 0.00000000e+00], 

[ 0.00000000e+00, 2.22044605e-16, -1.00000000e+00], 

[ 0.00000000e+00, 1.00000000e+00, 2.22044605e-16]]]) 

>>> r.apply(vector) 

array([[-0.70710678, 2.12132034, 3. ], 

[ 1. , -3. , 2. ]]) 

>>> r.apply(vector).shape 

(2, 3) 

 

Multiple rotations on multiple vectors. Each rotation is applied on the 

corresponding vector: 

 

>>> r = R.from_euler('zxy', [ 

... [0, 0, 90], 

... [45, 30, 60]], degrees=True) 

>>> vectors = [ 

... [1, 2, 3], 

... [1, 0, -1]] 

>>> r.apply(vectors) 

array([[ 3. , 2. , -1. ], 

[-0.09026039, 1.11237244, -0.86860844]]) 

>>> r.apply(vectors).shape 

(2, 3) 

 

It is also possible to apply the inverse rotation: 

 

>>> r = R.from_euler('zxy', [ 

... [0, 0, 90], 

... [45, 30, 60]], degrees=True) 

>>> vectors = [ 

... [1, 2, 3], 

... [1, 0, -1]] 

>>> r.apply(vectors, inverse=True) 

array([[-3. , 2. , 1. ], 

[ 1.09533535, -0.8365163 , 0.3169873 ]]) 

 

""" 

vectors = np.asarray(vectors) 

if vectors.ndim > 2 or vectors.shape[-1] != 3: 

raise ValueError("Expected input of shape (3,) or (P, 3), " 

"got {}.".format(vectors.shape)) 

 

single_vector = False 

if vectors.shape == (3,): 

single_vector = True 

vectors = vectors[None, :] 

 

dcm = self.as_dcm() 

if self._single: 

dcm = dcm[None, :, :] 

 

n_vectors = vectors.shape[0] 

n_rotations = len(self) 

 

if n_vectors != 1 and n_rotations != 1 and n_vectors != n_rotations: 

raise ValueError("Expected equal numbers of rotations and vectors " 

", or a single rotation, or a single vector, got " 

"{} rotations and {} vectors.".format( 

n_rotations, n_vectors)) 

 

if inverse: 

result = np.einsum('ikj,ik->ij', dcm, vectors) 

else: 

result = np.einsum('ijk,ik->ij', dcm, vectors) 

 

if self._single and single_vector: 

return result[0] 

else: 

return result 

 

def __mul__(self, other): 

"""Compose this rotation with the other. 

 

If `p` and `q` are two rotations, then the composition of 'q followed 

by p' is equivalent to `p * q`. In terms of DCMs, the composition can 

be expressed as `p.as_dcm().dot(q.as_dcm())`. 

 

Parameters 

---------- 

other : `Rotation` instance 

Object containing the rotaions to be composed with this one. Note 

that rotation compositions are not commutative, so ``p * q`` is 

different from ``q * p``. 

 

Returns 

------- 

composition : `Rotation` instance 

This function supports composition of multiple rotations at a time. 

The following cases are possible: 

 

- Either ``p`` or ``q`` contains a single rotation. In this case 

`composition` contains the result of composing each rotation in 

the other object with the single rotation. 

- Both ``p`` and ``q`` contain ``N`` rotations. In this case each 

rotation ``p[i]`` is composed with the corresponding rotation 

``q[i]`` and `output` contains ``N`` rotations. 

 

Examples 

-------- 

>>> from scipy.spatial.transform import Rotation as R 

 

Composition of two single rotations: 

 

>>> p = R.from_quat([0, 0, 1, 1]) 

>>> q = R.from_quat([1, 0, 0, 1]) 

>>> p.as_dcm() 

array([[ 0., -1., 0.], 

[ 1., 0., 0.], 

[ 0., 0., 1.]]) 

>>> q.as_dcm() 

array([[ 1., 0., 0.], 

[ 0., 0., -1.], 

[ 0., 1., 0.]]) 

>>> r = p * q 

>>> r.as_dcm() 

array([[0., 0., 1.], 

[1., 0., 0.], 

[0., 1., 0.]]) 

 

Composition of two objects containing equal number of rotations: 

 

>>> p = R.from_quat([[0, 0, 1, 1], [1, 0, 0, 1]]) 

>>> q = R.from_rotvec([[np.pi/4, 0, 0], [-np.pi/4, 0, np.pi/4]]) 

>>> p.as_quat() 

array([[0. , 0. , 0.70710678, 0.70710678], 

[0.70710678, 0. , 0. , 0.70710678]]) 

>>> q.as_quat() 

array([[ 0.38268343, 0. , 0. , 0.92387953], 

[-0.37282173, 0. , 0.37282173, 0.84971049]]) 

>>> r = p * q 

>>> r.as_quat() 

array([[ 0.27059805, 0.27059805, 0.65328148, 0.65328148], 

[ 0.33721128, -0.26362477, 0.26362477, 0.86446082]]) 

 

""" 

if not(len(self) == 1 or len(other) == 1 or len(self) == len(other)): 

raise ValueError("Expected equal number of rotations in both " 

"or a single rotation in either object, " 

"got {} rotations in first and {} rotations in " 

"second object.".format( 

len(self), len(other))) 

result = _compose_quat(self._quat, other._quat) 

if self._single and other._single: 

result = result[0] 

return self.__class__(result, normalized=True, copy=False) 

 

def inv(self): 

"""Invert this rotation. 

 

Composition of a rotation with its inverse results in an identity 

transformation. 

 

Returns 

------- 

inverse : `Rotation` instance 

Object containing inverse of the rotations in the current instance. 

 

Examples 

-------- 

>>> from scipy.spatial.transform import Rotation as R 

 

Inverting a single rotation: 

 

>>> p = R.from_euler('z', 45, degrees=True) 

>>> q = p.inv() 

>>> q.as_euler('zyx', degrees=True) 

array([-45., 0., 0.]) 

 

Inverting multiple rotations: 

 

>>> p = R.from_rotvec([[0, 0, np.pi/3], [-np.pi/4, 0, 0]]) 

>>> q = p.inv() 

>>> q.as_rotvec() 

array([[-0. , -0. , -1.04719755], 

[ 0.78539816, -0. , -0. ]]) 

 

""" 

quat = self._quat.copy() 

quat[:, -1] *= -1 

if self._single: 

quat = quat[0] 

return self.__class__(quat, normalized=True, copy=False) 

 

def __getitem__(self, indexer): 

"""Extract rotation(s) at given index(es) from object. 

 

Create a new `Rotation` instance containing a subset of rotations 

stored in this object. 

 

Parameters 

---------- 

indexer : index, slice, or index array 

Specifies which rotation(s) to extract. A single indexer must be 

specified, i.e. as if indexing a 1 dimensional array or list. 

 

Returns 

------- 

rotation : `Rotation` instance 

Contains 

- a single rotation, if `indexer` is a single index 

- a stack of rotation(s), if `indexer` is a slice, or and index 

array. 

 

Examples 

-------- 

>>> from scipy.spatial.transform import Rotation as R 

>>> r = R.from_quat([ 

... [1, 1, 0, 0], 

... [0, 1, 0, 1], 

... [1, 1, -1, 0]]) 

>>> r.as_quat() 

array([[ 0.70710678, 0.70710678, 0. , 0. ], 

[ 0. , 0.70710678, 0. , 0.70710678], 

[ 0.57735027, 0.57735027, -0.57735027, 0. ]]) 

 

Indexing using a single index: 

 

>>> p = r[0] 

>>> p.as_quat() 

array([0.70710678, 0.70710678, 0. , 0. ]) 

 

Array slicing: 

 

>>> q = r[1:3] 

>>> q.as_quat() 

array([[ 0. , 0.70710678, 0. , 0.70710678], 

[ 0.57735027, 0.57735027, -0.57735027, 0. ]]) 

 

""" 

return self.__class__(self._quat[indexer], normalized=True) 

 

@classmethod 

def random(cls, num=None, random_state=None): 

"""Generate uniformly distributed rotations. 

 

Parameters 

---------- 

num : int or None, optional 

Number of random rotations to generate. If None (default), then a 

single rotation is generated. 

random_state : int, RandomState instance or None, optional 

Accepts an integer as a seed for the random generator or a 

RandomState object. If None (default), uses global `numpy.random` 

random state. 

 

Returns 

------- 

random_rotation : `Rotation` instance 

Contains a single rotation if `num` is None. Otherwise contains a 

stack of `num` rotations. 

 

Examples 

-------- 

>>> from scipy.spatial.transform import Rotation as R 

 

Sample a single rotation: 

 

>>> R.random(random_state=1234).as_euler('zxy', degrees=True) 

array([-110.5976185 , 55.32758512, 76.3289269 ]) 

 

Sample a stack of rotations: 

 

>>> R.random(5, random_state=1234).as_euler('zxy', degrees=True) 

array([[-110.5976185 , 55.32758512, 76.3289269 ], 

[ -91.59132005, -14.3629884 , -93.91933182], 

[ 25.23835501, 45.02035145, -121.67867086], 

[ -51.51414184, -15.29022692, -172.46870023], 

[ -81.63376847, -27.39521579, 2.60408416]]) 

 

""" 

random_state = check_random_state(random_state) 

 

if num is None: 

sample = random_state.normal(size=4) 

else: 

sample = random_state.normal(size=(num, 4)) 

 

return Rotation.from_quat(sample) 

 

@classmethod 

def match_vectors(cls, a, b, weights=None, normalized=False): 

"""Estimate a rotation to match two sets of vectors. 

 

Find a rotation between frames A and B which best matches a set of unit 

vectors `a` and `b` observed in these frames. The following loss 

function is minimized to solve for the direction cosine matrix 

:math:`C`: 

 

.. math:: 

 

L(C) = \\frac{1}{2} \\sum_{i = 1}^{n} w_i \\lVert \\mathbf{a}_i - 

C \\mathbf{b}_i \\rVert^2 , 

 

where :math:`w_i`'s are the `weights` corresponding to each vector. 

 

The rotation is estimated using Markley's SVD method [1]_. 

 

Parameters 

---------- 

a : array_like, shape (N, 3) 

Vector components observed in initial frame A. Each row of `a` 

denotes a vector. 

b : array_like, shape (N, 3) 

Vector components observed in another frame B. Each row of `b` 

denotes a vector. 

weights : array_like shape (N,), optional 

Weights describing the relative importance of the vectors in 

`a`. If None (default), then all values in `weights` are assumed to 

be equal. 

normalized : bool, optional 

If True, assume input vectors `a` and `b` to have unit norm. If 

False, normalize `a` and `b` before estimating rotation. Default 

is False. 

 

Returns 

------- 

estimated_rotation : `Rotation` instance 

Best estimate of the rotation that transforms `b` to `a`. 

sensitivity_matrix : ndarray, shape (3, 3) 

Scaled covariance of the attitude errors expressed as the small 

rotation vector of frame A. Multiply with harmonic mean [3]_ of 

variance in each observation to get true covariance matrix. The 

error model is detailed in [2]_. 

 

References 

---------- 

.. [1] F. Landis Markley, 

"Attitude determination using vector observations: a fast 

optimal matrix algorithm", Journal of Astronautical Sciences, 

Vol. 41, No.2, 1993, pp. 261-280. 

.. [2] F. Landis Markley, 

"Attitude determination using vector observations and the 

Singular Value Decomposition", Journal of Astronautical 

Sciences, Vol. 38, No.3, 1988, pp. 245-258. 

.. [3] https://en.wikipedia.org/wiki/Harmonic_mean 

 

""" 

a = np.asarray(a) 

if a.ndim != 2 or a.shape[-1] != 3: 

raise ValueError("Expected input `a` to have shape (N, 3), " 

"got {}".format(a.shape)) 

b = np.asarray(b) 

if b.ndim != 2 or b.shape[-1] != 3: 

raise ValueError("Expected input `b` to have shape (N, 3), " 

"got {}.".format(b.shape)) 

 

if a.shape != b.shape: 

raise ValueError("Expected inputs `a` and `b` to have same shapes" 

", got {} and {} respectively.".format( 

a.shape, b.shape)) 

 

if b.shape[0] == 1: 

raise ValueError("Rotation cannot be estimated using a single " 

"vector.") 

 

if weights is None: 

weights = np.ones(b.shape[0]) 

else: 

weights = np.asarray(weights) 

if weights.ndim != 1: 

raise ValueError("Expected `weights` to be 1 dimensional, got " 

"shape {}.".format(weights.shape)) 

if weights.shape[0] != b.shape[0]: 

raise ValueError("Expected `weights` to have number of values " 

"equal to number of input vectors, got " 

"{} values and {} vectors.".format( 

weights.shape[0], b.shape[0])) 

weights = weights / np.sum(weights) 

 

if not normalized: 

a = a / scipy.linalg.norm(a, axis=1)[:, None] 

b = b / scipy.linalg.norm(b, axis=1)[:, None] 

 

B = np.einsum('ji,jk->ik', weights[:, None] * a, b) 

u, s, vh = np.linalg.svd(B) 

C = np.dot(u, vh) 

 

zeta = (s[0]+s[1]) * (s[1]+s[2]) * (s[2]+s[0]) 

if np.abs(zeta) <= 1e-16: 

raise ValueError("Three component error vector has infinite " 

"covariance. It is impossible to determine the " 

"rotation uniquely.") 

 

kappa = s[0]*s[1] + s[1]*s[2] + s[2]*s[0] 

sensitivity = ((kappa * np.eye(3) + np.dot(B, B.T)) / 

(zeta * a.shape[0])) 

return cls.from_dcm(C), sensitivity 

 

 

class Slerp(object): 

"""Spherical Linear Interpolation of Rotations. 

 

The interpolation between consecutive rotations is performed as a rotation 

around a fixed axis with a constant angular velocity [1]_. This ensures 

that the interpolated rotations follow the shortest path between initial 

and final orientations. 

 

Parameters 

---------- 

times : array_like, shape (N,) 

Times of the known rotations. At least 2 times must be specified. 

rotations : `Rotation` instance 

Rotations to perform the interpolation between. Must contain N 

rotations. 

 

Methods 

------- 

__call__ 

 

See Also 

-------- 

Rotation 

 

Notes 

----- 

.. versionadded:: 1.2.0 

 

References 

---------- 

.. [1] https://en.wikipedia.org/wiki/Slerp#Quaternion_Slerp 

 

Examples 

-------- 

>>> from scipy.spatial.transform import Rotation as R 

>>> from scipy.spatial.transform import Slerp 

 

Setup the fixed keyframe rotations and times: 

 

>>> key_rots = R.random(5, random_state=2342345) 

>>> key_times = [0, 1, 2, 3, 4] 

 

Create the interpolator object: 

 

>>> slerp = Slerp(key_times, key_rots) 

 

Interpolate the rotations at the given times: 

 

>>> times = [0, 0.5, 0.25, 1, 1.5, 2, 2.75, 3, 3.25, 3.60, 4] 

>>> interp_rots = slerp(times) 

 

The keyframe rotations expressed as Euler angles: 

 

>>> key_rots.as_euler('xyz', degrees=True) 

array([[ 14.31443779, -27.50095894, -3.7275787 ], 

[ -1.79924227, -24.69421529, 164.57701743], 

[146.15020772, 43.22849451, -31.34891088], 

[ 46.39959442, 11.62126073, -45.99719267], 

[-88.94647804, -49.64400082, -65.80546984]]) 

 

The interpolated rotations expressed as Euler angles. These agree with the 

keyframe rotations at both endpoints of the range of keyframe times. 

 

>>> interp_rots.as_euler('xyz', degrees=True) 

array([[ 14.31443779, -27.50095894, -3.7275787 ], 

[ 4.74588574, -32.44683966, 81.25139984], 

[ 10.71094749, -31.56690154, 38.06896408], 

[ -1.79924227, -24.69421529, 164.57701743], 

[ 11.72796022, 51.64207311, -171.7374683 ], 

[ 146.15020772, 43.22849451, -31.34891088], 

[ 68.10921869, 20.67625074, -48.74886034], 

[ 46.39959442, 11.62126073, -45.99719267], 

[ 12.35552615, 4.21525086, -64.89288124], 

[ -30.08117143, -19.90769513, -78.98121326], 

[ -88.94647804, -49.64400082, -65.80546984]]) 

 

""" 

def __init__(self, times, rotations): 

if len(rotations) == 1: 

raise ValueError("`rotations` must contain at least 2 rotations.") 

 

times = np.asarray(times) 

if times.ndim != 1: 

raise ValueError("Expected times to be specified in a 1 " 

"dimensional array, got {} " 

"dimensions.".format(times.ndim)) 

 

if times.shape[0] != len(rotations): 

raise ValueError("Expected number of rotations to be equal to " 

"number of timestamps given, got {} rotations " 

"and {} timestamps.".format( 

len(rotations), times.shape[0])) 

self.times = times 

self.timedelta = np.diff(times) 

 

if np.any(self.timedelta <= 0): 

raise ValueError("Times must be in strictly increasing order.") 

 

self.rotations = rotations[:-1] 

self.rotvecs = (self.rotations.inv() * rotations[1:]).as_rotvec() 

 

def __call__(self, times): 

"""Interpolate rotations. 

 

Compute the interpolated rotations at the given `times`. 

 

Parameters 

---------- 

times : array_like, 1D 

Times to compute the interpolations at. 

 

Returns 

------- 

interpolated_rotation : `Rotation` instance 

Object containing the rotations computed at given `times`. 

 

""" 

# Clearly differentiate from self.times property 

compute_times = np.asarray(times) 

if compute_times.ndim != 1: 

raise ValueError("Expected times to be specified in a 1 " 

"dimensional array, got {} " 

"dimensions.".format(compute_times.ndim)) 

 

# side = 'left' (default) excludes t_min. 

ind = np.searchsorted(self.times, compute_times) - 1 

# Include t_min. Without this step, index for t_min equals -1 

ind[compute_times == self.times[0]] = 0 

if np.any(np.logical_or(ind < 0, ind > len(self.rotations) - 1)): 

raise ValueError("Interpolation times must be within the range " 

"[{}, {}], both inclusive.".format( 

self.times[0], self.times[-1])) 

 

alpha = (compute_times - self.times[ind]) / self.timedelta[ind] 

 

return (self.rotations[ind] * 

Rotation.from_rotvec(self.rotvecs[ind] * alpha[:, None]))