spatialmath-python/docs/intro.html at master · Python-Repository-Hub/spatialmath-python

History

1041 lines (991 loc) · 58.3 KB

Raw

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

<!DOCTYPE html>

<head>

<title>Introduction — Spatial Maths package 0.7.0

documentation</title>

</head><body>

<h1>Introduction<a class="headerlink" href="#introduction" title="Permalink to this headline">¶</a></h1>

Spatial maths capability underpins all of robotics and robotic vision by describing the relative position and orientation of objects in 2D or 3D space. This package:

<li>provides Python classes and functions to manipulate matrices that represent relevant mathematical objects such as rotation matrices \(R \in SO(2), SO(3)\), homogeneous transformation matrices \(T \in SE(2), SE(3)\) and quaternions \(q \in \mathbb{H}\).</li>

<li>replicates, as much as possible, the functionality of the <a class="reference external" href="https://github.com/petercorke/spatial-math">Spatial Math Toolbox</a> for MATLAB ® which underpins the <a class="reference external" href="https://github.com/petercorke/robotics-toolbox-matlab">Robotics Toolbox</a> for MATLAB. Important considerations included:

<ul>

<li>being as similar as possible to the MATLAB Toolbox function names and semantics</li>

<li>but balancing the tension of being as Pythonic as possible</li>

<li>use Python keyword arguments to replace the MATLAB Toolbox string options supported using <cite>tb_optparse`</cite></li>

<li>use <code class="docutils literal notranslate">numpy</code> arrays for rotation and homogeneous transformation matrices, quaternions and vectors</li>

<li>all functions that accept a vector can accept a list, tuple, or <cite>np.ndarray</cite></li>

<li>The classes can hold a sequence of elements, they are polymorphic with lists, which can be used to represent trajectories or time sequences.</li>

</ul>

</li>

</ul>

Quick example:

<div class="highlight-python notranslate"><div class="highlight"><pre>>>> import spatialmath as sm

>>> R = sm.SO3.Rx(30, 'deg')

>>> R

1 0 0

0 0.866025 -0.5

</pre></div>

</div>

which constructs a rotation about the x-axis by 30 degrees.

<h2>High-level classes<a class="headerlink" href="#high-level-classes" title="Permalink to this headline">¶</a></h2>

These classes abstract the low-level numpy arrays into objects of class <cite>SO2</cite>, <cite>SE2</cite>, <cite>SO3</cite>, <cite>SE3</cite>, <cite>UnitQuaternion</cite> that obey the rules associated with the mathematical groups SO(2), SE(2), SO(3), SE(3) and

Using classes has several merits:

<li>ensures type safety, for example it stops us mixing a 2D homogeneous transformation with a 3D rotation matrix – both of which are 3x3 matrices.</li>

<li>ensure that an SO(2), SO(3) or unit-quaternion rotation is always valid because the constraints (eg. orthogonality, unit norm) are enforced when the object is constructed.</li>

</ul>

<div class="highlight-python notranslate"><div class="highlight"><pre>>>> from spatialmath import *

>>> SO2(.1)

[[ 0.99500417 -0.09983342]

[ 0.09983342 0.99500417]]

</pre></div>

</div>

Type safety and type validity are particularly important when we deal with a sequence of such objects. In robotics we frequently deal with trajectories of poses or rotation to describe objects moving in the

world.

However a list of these items has the type <cite>list</cite> and the elements are not enforced to be homogeneous, ie. a list could contain a mixture of classes.

Another option would be to create a <cite>numpy</cite> array of these objects, the upside being it could be a multi-dimensional array. The downside is that again the array is not guaranteed to be homogeneous.

The approach adopted here is to give these classes list super powers so that a single <cite>SE3</cite> object can contain a list of SE(3) poses. The pose objects are a list subclass so we can index it or slice it as we

would a list, but the result each time belongs to the class it was sliced from. Here’s a simple example of SE(3) but applicable to all the classes

<div class="highlight-python notranslate"><div class="highlight"><pre>T = transl(1,2,3) # create a 4x4 np.array

a = SE3(T)

len(a)

type(a)

a.append(a) # append a copy

type(a)

len(a)

a[1] # extract one element of the list

for x in a:

# do a thing

</pre></div>

</div>

These classes are all derived from two parent classes:

<li><cite>RTBPose</cite> which provides common functionality for all</li>

<li><cite>UserList</cite> which provdides the ability to act like a list</li>

</ul>

<h3>Operators for pose objects<a class="headerlink" href="#operators-for-pose-objects" title="Permalink to this headline">¶</a></h3>

Standard arithmetic operators can be applied to all these objects.

</colgroup>

<thead>

<tr class="row-odd"><th class="head">Operator</th>

<th class="head">dunder method</th>

</tr>

</thead>

<tbody>

</tr>

</tr>

<td>__truediv__</td>

</tr>

<td>__itruediv__</td>

</tr>

</tr>

</tr>

</tr>

</tr>

</tr>

</tr>

</tbody>

</table>

This online documentation includes just the method shown in bold.

The other related methods all invoke that method.

The classes represent mathematical groups, and the rules of group are enforced.

If this is a group operation, ie. the operands are of the same type and the operator

is the group operator, the result will be of the input type, otherwise the result

will be a matrix.

For the groups SO(n) and SE(n) the group operator is composition represented

by the multiplication operator. The identity element is a unit matrix.

</colgroup>

<thead>

<tr class="row-odd"><th class="head" colspan="2">Operands</th>

</tr>

<th class="head">right</th>

<th class="head">result</th>

</tr>

</thead>

<tbody>

<td>composition [1]</td>

</tr>

<td>scalar</td>

<td>matrix</td>

<td>elementwise product</td>

</tr>

<tr class="row-odd"><td>scalar</td>

<td>matrix</td>

<td>elementwise product</td>

</tr>

<td>N-vector</td>

<td>vector transform [2]</td>

</tr>

<td>NxM matrix</td>

<td>vector transform [2] [3]</td>

</tr>

</tbody>

</table>

Notes:

<li>Composition is performed by standard matrix multiplication.</li>

<li>Matrix columns are taken as the vectors to transform.</li>

</ol>

</colgroup>

<thead>

<tr class="row-odd"><th class="head" colspan="2">Operands</th>

</tr>

<th class="head">right</th>

<th class="head">result</th>

</tr>

</thead>

<tbody>

<td>matrix * inverse #1</td>

</tr>

<td>scalar</td>

<td>matrix</td>

<td>elementwise product</td>

</tr>

<tr class="row-odd"><td>scalar</td>

<td>matrix</td>

<td>elementwise product</td>

</tr>

</tbody>

</table>

Notes:

<li>The left operand is multiplied by the <code class="docutils literal notranslate">.inv</code> property of the right operand.</li>

</ol>

</colgroup>

<thead>

<tr class="row-odd"><th class="head" colspan="2">Operands</th>

</tr>

<th class="head">right</th>

<th class="head">result</th>

</tr>

</thead>

<tbody>

<td>exponentiation [1]</td>

</tr>

<td>exponentiation [1] then inverse</td>

</tr>

</tbody>

</table>

Notes:

<li>By repeated multiplication.</li>

</ol>

</colgroup>

<thead>

<tr class="row-odd"><th class="head" colspan="2">Operands</th>

</tr>

<th class="head">right</th>

<th class="head">result</th>

</tr>

</thead>

<tbody>

<td>matrix</td>

<td>elementwise sum</td>

</tr>

<td>scalar</td>

<td>matrix</td>

<td>add scalar to all elements</td>

</tr>

<tr class="row-odd"><td>scalar</td>

<td>matrix</td>

<td>add scalarto all elements</td>

</tr>

</tbody>

</table>

</colgroup>

<thead>

<tr class="row-odd"><th class="head" colspan="2">Operands</th>

</tr>

<th class="head">right</th>

<th class="head">result</th>

</tr>

</thead>

<tbody>

<td>matrix</td>

<td>elementwise difference</td>

</tr>

<td>scalar</td>

<td>matrix</td>

<td>subtract scalar from all elements</td>

</tr>

<tr class="row-odd"><td>scalar</td>

<td>matrix</td>

<td>subtract all elements from scalar</td>

</tr>

</tbody>

</table>

</div>

<h4>Unit quaternions and quaternions<a class="headerlink" href="#unit-quaternions-and-quaternions" title="Permalink to this headline">¶</a></h4>

Quaternions form a ring and support the operations of multiplication, addition and

subtraction. Unit quaternions form a group and the group operator is composition represented

by the multiplication operator.

</colgroup>

<thead>

<tr class="row-odd"><th class="head" colspan="2">Operands</th>

</tr>

<th class="head">right</th>

<th class="head">result</th>

</tr>

</thead>

<tbody>

<tr class="row-odd"><td>Quaternion</td>

<td>Quaternion</td>

<td>Hamilton product</td>

</tr>

<tr class="row-even"><td>Quaternion</td>

<td>UnitQuaternion</td>

<td>Quaternion</td>

<td>Hamilton product</td>

</tr>

<tr class="row-odd"><td>Quaternion</td>

<td>scalar</td>

<td>Quaternion</td>

<td>scalar product #2</td>

</tr>

<tr class="row-even"><td>UnitQuaternion</td>

<td>Quaternion</td>

<td>Hamilton product</td>

</tr>

<tr class="row-odd"><td>UnitQuaternion</td>

<td>UnitQuaternion</td>

<td>Hamilton product #1</td>

</tr>

<tr class="row-even"><td>UnitQuaternion</td>

<td>scalar</td>

<td>Quaternion</td>

<td>scalar product #2</td>

</tr>

<tr class="row-odd"><td>UnitQuaternion</td>

<td>3-vector</td>

<td>vector rotation #3</td>

</tr>

<tr class="row-even"><td>UnitQuaternion</td>

<td>3xN matrix</td>

<td>vector transform #2#3</td>

</tr>

</tbody>

</table>

Notes:

<li>Composition.</li>

<li>Matrix columns are taken as the vectors to transform.</li>

</ol>

</colgroup>

<thead>

<tr class="row-odd"><th class="head" colspan="2">Operands</th>

</tr>

<th class="head">right</th>

<th class="head">result</th>

</tr>

</thead>

<tbody>

<tr class="row-odd"><td>UnitQuaternion</td>

<td>UnitQuaternion</td>

<td>Hamilton product with inverse #1</td>

</tr>

</tbody>

</table>

Notes:

<li>The left operand is multiplied by the <code class="docutils literal notranslate">.inv</code> property of the right operand.</li>

</ol>

</colgroup>

<thead>

<tr class="row-odd"><th class="head" colspan="2">Operands</th>

</tr>

<th class="head">right</th>

<th class="head">result</th>

</tr>

</thead>

<tbody>

<tr class="row-odd"><td>Quaternion</td>

<td>Quaternion</td>

<td>exponentiation [1]</td>

</tr>

<tr class="row-even"><td>UnitQuaternion</td>

<td>UnitQuaternion</td>

<td>exponentiation [1]</td>

</tr>

<tr class="row-odd"><td>UnitQuaternion</td>

<td>UnitQuaternion</td>

<td>exponentiation [1] then inverse</td>

</tr>

</tbody>

</table>

Notes:

<li>By repeated multiplication.</li>

</ol>

</colgroup>

<thead>

<tr class="row-odd"><th class="head" colspan="2">Operands</th>

</tr>

<th class="head">right</th>

<th class="head">result</th>

</tr>

</thead>

<tbody>

<tr class="row-odd"><td>Quaternion</td>

<td>Quaternion</td>

<td>elementwise sum</td>

</tr>

<tr class="row-even"><td>Quaternion</td>

<td>UnitQuaternion</td>

<td>Quaternion</td>

<td>elementwise sum</td>

</tr>

<tr class="row-odd"><td>Quaternion</td>

<td>scalar</td>

<td>Quaternion</td>

<td>add to each element</td>

</tr>

<tr class="row-even"><td>UnitQuaternion</td>

<td>Quaternion</td>

<td>elementwise sum</td>

</tr>

<tr class="row-odd"><td>UnitQuaternion</td>

<td>UnitQuaternion</td>

<td>Quaternion</td>

<td>elementwise sum</td>

</tr>

<tr class="row-even"><td>UnitQuaternion</td>

<td>scalar</td>

<td>Quaternion</td>

<td>add to each element</td>

</tr>

</tbody>

</table>

</colgroup>

<thead>

<tr class="row-odd"><th class="head" colspan="2">Operands</th>

</tr>

<th class="head">right</th>

<th class="head">result</th>

</tr>

</thead>

<tbody>

<tr class="row-odd"><td>Quaternion</td>

<td>Quaternion</td>

<td>elementwise difference</td>

</tr>

<tr class="row-even"><td>Quaternion</td>

<td>UnitQuaternion</td>

<td>Quaternion</td>

<td>elementwise difference</td>

</tr>

<tr class="row-odd"><td>Quaternion</td>

<td>scalar</td>

<td>Quaternion</td>

<td>subtract scalar from each element</td>

</tr>

<tr class="row-even"><td>UnitQuaternion</td>

<td>Quaternion</td>

<td>elementwise difference</td>

</tr>

<tr class="row-odd"><td>UnitQuaternion</td>

<td>UnitQuaternion</td>

<td>Quaternion</td>

<td>elementwise difference</td>

</tr>

<tr class="row-even"><td>UnitQuaternion</td>

<td>scalar</td>

<td>Quaternion</td>

<td>subtract scalar from each element</td>

</tr>

</tbody>

</table>

Any other operands will raise a <code class="docutils literal notranslate">ValueError</code> exception.

</div>

<h3>List capability<a class="headerlink" href="#list-capability" title="Permalink to this headline">¶</a></h3>

Each of these object classes has <code class="docutils literal notranslate">UserList</code> as a base class which means it inherits all the functionality of

a Python list

>>> len(R)

1

</pre></div>

</div>

<div class="highlight-python notranslate"><div class="highlight"><pre>>>> R = SO3.Rx(np.arange(0, 2*np.pi, 0.2)))

>>> len(R)

32

>> R[0]

1 0 0

0 1 0

0 0 1

>> R[-1]

1 0 0

0 0.996542 0.0830894

0 -0.0830894 0.996542

</pre></div>

</div>

where each item is an object of the same class as that it was extracted from.

Slice notation is also available, eg. <code class="docutils literal notranslate">R[0:-1:3]</code> is a new SO3 instance containing every third element of <code class="docutils literal notranslate">R</code>.

In particular it includes an iterator allowing comprehensions

<div class="highlight-python notranslate"><div class="highlight"><pre>>>> [x.eul for x in R]

[array([ 90. , 4.76616702, -90. ]),

array([ 90. , 16.22532292, -90. ]),

array([ 90. , 27.68447882, -90. ]),

.

array([-90. , 11.4591559, 90. ]),

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

</pre></div>

</div>

Useful functions that be used on such objects include

</colgroup>

<thead>

<tr class="row-odd"><th class="head">Method</th>

<th class="head">Operation</th>

</tr>

</thead>

<tbody>

<tr class="row-even"><td><code class="docutils literal notranslate">clear</code></td>

<td>Clear all elements, object now has zero length</td>

</tr>

<tr class="row-odd"><td><code class="docutils literal notranslate">append</code></td>

<td>Append a single element</td>

</tr>

</tr>

<tr class="row-odd"><td><code class="docutils literal notranslate">enumerate</code></td>

<td>Iterate over the elments</td>

</tr>

<tr class="row-even"><td><code class="docutils literal notranslate">extend</code></td>

<td>Append a list of same type pose objects</td>

</tr>

<tr class="row-odd"><td><code class="docutils literal notranslate">insert</code></td>

<td>Insert an element</td>

</tr>

<td>Return the number of elements</td>

</tr>

<td>Map a function of each element</td>

</tr>

<td>Remove first element and return it</td>

</tr>

<tr class="row-odd"><td><code class="docutils literal notranslate">slice</code></td>

<td>Index from a slice object</td>

</tr>

<td>Iterate over the elments</td>

</tr>

</tbody>

</table>

</div>

<h3>Vectorization<a class="headerlink" href="#vectorization" title="Permalink to this headline">¶</a></h3>

For most methods, if applied to an object that contains N elements, the result will be the appropriate return object type with N elements.

Most binary operations (<cite>*</cite>, <cite>*=</cite>, <cite>**</cite>, <cite>+</cite>, <cite>+=</cite>, <cite>-</cite>, <cite>-=</cite>, <cite>==</cite>, <cite>!=</cite>) are vectorized. For the case:

<div class="highlight-default notranslate"><div class="highlight"><pre>Z = X op Y

</pre></div>

</div>

the lengths of the operands and the results are given by

</colgroup>

<thead>

<tr class="row-odd"><th class="head" colspan="2">operands</th>

<th class="head" colspan="2">results</th>

</tr>

<th class="head">results</th>

</tr>

</thead>

<tbody>

</tr>

</tr>

</tr>

</tr>

</tbody>

</table>

Any other combination of lengths is not allowed and will raise a <code class="docutils literal notranslate">ValueError</code> exception.

</div>

<h2>Low-level spatial math<a class="headerlink" href="#low-level-spatial-math" title="Permalink to this headline">¶</a></h2>

All the classes just described abstract the <code class="docutils literal notranslate">base</code> package which represent the spatial-math object as a numpy.ndarray.

The inputs to functions in this package are either floats, lists, tuples or numpy.ndarray objects describing vectors or arrays. Functions that require a vector can be passed a list, tuple or numpy.ndarray for a vector – described in the documentation as being of type array_like.

Numpy vectors are somewhat different to MATLAB, and is a gnarly aspect of numpy. Numpy arrays have a shape described by a shape tuple which is a list of the dimensions. Typically all <code class="docutils literal notranslate">np.ndarray</code> vectors have the shape (N,), that is, they have only one dimension. The <code class="docutils literal notranslate">@</code> product of an (M,N) array and a (N,) vector is a (M,) array. A numpy column vector has shape (N,1) and a row vector has shape (1,N) but functions also accept row (1,N) and column (N,1) vectors.

Iterating over a numpy.ndarray is done by row, not columns as in MATLAB. Iterating over a 1D array (N,) returns consecutive elements, iterating a row vector (1,N) returns the entire row, iterating a column vector (N,1) returns consecutive elements (rows).

For example an SE(2) pose is represented by a 3x3 numpy array, an ndarray with shape=(3,3). A unit quaternion is

represented by a 4-element numpy array, an ndarray with shape=(4,).

</colgroup>

<thead>

<tr class="row-odd"><th class="head">Spatial object</th>

<th class="head">equivalent class</th>

<th class="head">numpy.ndarray shape</th>

</tr>

</thead>

<tbody>

<tr class="row-even"><td>2D rotation SO(2)</td>

</tr>

</tr>

<tr class="row-even"><td>3D rotation SO(3)</td>

</tr>

<tr class="row-odd"><td>3D poseSE3 SE(3)</td>

</tr>

<tr class="row-even"><td>3D rotation</td>

<td>UnitQuaternion</td>

</tr>

<td>Quaternion</td>

</tr>

</tbody>

</table>

Tjhe classes <code class="docutils literal notranslate">SO2</code>, <code class="docutils literal notranslate">`SE2</code>, <code class="docutils literal notranslate">`SO3</code>, <code class="docutils literal notranslate">SE3</code>, <code class="docutils literal notranslate">UnitQuaternion</code> can operate conveniently on lists but the <code class="docutils literal notranslate">base</code> functions do not support this.

If you wish to work with these functions and create lists of pose objects you could keep the numpy arrays in high-order numpy arrays (ie. add an extra dimensions),

or keep them in a list, tuple or any other python contai described in the [high-level spatial math section](#high-level-classes).

Let’s show a simple example:

<div class="highlight-python notranslate"><table class="highlighttable"><tr><td class="linenos"><div class="linenodiv"><pre> 1

12</pre></div></td><td class="code"><div class="highlight"><pre> >>> import spatialmath.base.transforms as base

>>> base.rotx(0.3)

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

[ 0. , 0.95533649, -0.29552021],

[ 0. , 0.29552021, 0.95533649]])

>>> base.rotx(30, unit='deg')

[ 0. , 0.8660254, -0.5 ],

[ 0. , 0.5 , 0.8660254]])

>>> R = base.rotx(0.3) @ base.roty(0.2)

</pre></div>

</td></tr></table></div>

At line 1 we import all the base functions into the namespae <code class="docutils literal notranslate">base</code>.

In line 12 when we multiply the matrices we need to use the <cite>@</cite> operator to perform matrix multiplication. The <cite>*</cite> operator performs element-wise multiplication, which is equivalent to the MATLAB <code class="docutils literal notranslate">.*</code> operator.

We also support multiple ways of passing vector information to functions that require it:

<li>as separate positional arguments</li>

</ul>

<div class="highlight-python notranslate"><div class="highlight"><pre>transl2(1, 2)

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

[0., 1., 2.],

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

</pre></div>

</div>

<li>as a list or a tuple</li>

</ul>

<div class="highlight-python notranslate"><div class="highlight"><pre>transl2( [1,2] )

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

[0., 1., 2.],

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

transl2( (1,2) )

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

[0., 1., 2.],

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

</pre></div>

</div>

<li>or as a <cite>numpy</cite> array</li>

</ul>

<div class="highlight-python notranslate"><div class="highlight"><pre>transl2( np.array([1,2]) )

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

[0., 1., 2.],

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

</pre></div>

</div>

There is a single module that deals with quaternions, regular quaternions and unit quaternions, and the representation is a <cite>numpy</cite> array of four elements. As above, functions can accept the <cite>numpy</cite> array, a list, dict or <cite>numpy</cite> row or column vectors.

<div class="highlight-python notranslate"><div class="highlight"><pre>>>> import spatialmath.base.quaternion as quat

>>> q = quat.qqmul([1,2,3,4], [5,6,7,8])

>>> q

array([-60, 12, 30, 24])

>>> quat.qprint(q)

-60.000000 < 12.000000, 30.000000, 24.000000 >

>>> quat.qnorm(q)

72.24956747275377

</pre></div>

</div>

Functions exist to convert to and from SO(3) rotation matrices and a 3-vector representation. The latter is often used for SLAM and bundle adjustment applications, being a minimal representation of orientation.

<h3>Graphics<a class="headerlink" href="#graphics" title="Permalink to this headline">¶</a></h3>

If <code class="docutils literal notranslate">matplotlib</code> is installed then we can add 2D coordinate frames to a figure in a variety of styles:

<div class="highlight-python notranslate"><table class="highlighttable"><tr><td class="linenos"><div class="linenodiv"><pre>1

4</pre></div></td><td class="code"><div class="highlight"><pre> trplot2( transl2(1,2), frame='A', rviz=True, width=1)

trplot2( transl2(3,1), color='red', arrow=True, width=3, frame='B')

trplot2( transl2(4, 3)@trot2(math.pi/3), color='green', frame='c')

plt.grid(True)

</pre></div>

</td></tr></table></div>

Output of <code class="docutils literal notranslate">trplot2</code><a class="headerlink" href="#id1" title="Permalink to this image">¶</a>

</div>

If a figure does not yet exist one is added. If a figure exists but there is no 2D axes then one is added. To add to an existing axes you can pass this in using the <code class="docutils literal notranslate">axes</code> argument. By default the frames are drawn with lines or arrows of unit length. Autoscaling is enabled.

Similarly, we can plot 3D coordinate frames in a variety of styles:

<div class="highlight-python notranslate"><table class="highlighttable"><tr><td class="linenos"><div class="linenodiv"><pre>1

3</pre></div></td><td class="code"><div class="highlight"><pre> trplot( transl(1,2,3), frame='A', rviz=True, width=1, dims=[0, 10, 0, 10, 0, 10])

trplot( transl(3,1, 2), color='red', width=3, frame='B')

trplot( transl(4, 3, 1)@trotx(math.pi/3), color='green', frame='c', dims=[0,4,0,4,0,4])

</pre></div>

</td></tr></table></div>

Output of <code class="docutils literal notranslate">trplot</code><a class="headerlink" href="#id2" title="Permalink to this image">¶</a>

</div>

The <code class="docutils literal notranslate">dims</code> option in lines 1 and 3 sets the workspace dimensions. Note that the last set value is what is displayed.

Depending on the backend you are using you may need to include

<div class="highlight-python notranslate"><div class="highlight"><pre>plt.show()

</pre></div>

</div>

<h3>Symbolic support<a class="headerlink" href="#symbolic-support" title="Permalink to this headline">¶</a></h3>

Some functions have support for symbolic variables, for example

<div class="highlight-python notranslate"><div class="highlight"><pre>import sympy

theta = sym.symbols('theta')

print(rotx(theta))

[[1 0 0]

[0 cos(theta) -sin(theta)]

[0 sin(theta) cos(theta)]]

</pre></div>

</div>

The resulting <cite>numpy</cite> array is an array of symbolic objects not numbers &ndash; the constants are also symbolic objects. You can read the elements of the matrix

<div class="highlight-python notranslate"><div class="highlight"><pre>>>> a = T[0,0]

>>> a

1

>>> type(a)

int

>>> a = T[1,1]

>>> a

cos(theta)

>>> type(a)

cos

</pre></div>

</div>

We see that the symbolic constants are converted back to Python numeric types on read.

Similarly when we assign an element or slice of the symbolic matrix to a numeric value, they are converted to symbolic constants on the way in.

<div class="highlight-python notranslate"><div class="highlight"><pre>>>> T[0,3] = 22

>>> print(T)

[[1 0 0 22]

[0 cos(theta) -sin(theta) 0]

[0 sin(theta) cos(theta) 0]

[0 0 0 1]]

</pre></div>

</div>

but you can’t write a symbolic value into a floating point matrix

<div class="highlight-python notranslate"><div class="highlight"><pre>>>> T = trotx(0.2)

>>> T[0,3]=theta

Traceback (most recent call last):

.

TypeError: can't convert expression to float

</pre></div>

</div>

<h3>MATLAB compatability<a class="headerlink" href="#matlab-compatability" title="Permalink to this headline">¶</a></h3>

We can create a MATLAB like environment by

<div class="highlight-python notranslate"><div class="highlight"><pre>from spatialmath import *

from spatialmath.base import *

</pre></div>

</div>

which has familiar functions like <code class="docutils literal notranslate">rotx</code> and <code class="docutils literal notranslate">rpy2r</code> available, as well as classes like <code class="docutils literal notranslate">SE3</code>

<div class="highlight-python notranslate"><div class="highlight"><pre>R = rotx(0.3)

R2 = rpy2r(0.1, 0.2, 0.3)

T = SE3(1, 2, 3)

</pre></div>

</div>

</a>

Spatial maths and geometry for Python

allowtransparency="true" frameborder="0" scrolling="0" width="200px" height="35px"></iframe>

<h3>Navigation</h3>

<li class="toctree-l1 current"><a class="current reference internal" href="#">Introduction</a><ul>

<li class="toctree-l2"><a class="reference internal" href="#high-level-classes">High-level classes</a></li>

<li class="toctree-l2"><a class="reference internal" href="#low-level-spatial-math">Low-level spatial math</a></li>

</ul>

</li>

<li class="toctree-l1"><a class="reference internal" href="spatialmath.html">Classes and functions</a></li>

<li class="toctree-l1"><a class="reference internal" href="indices.html">Indices</a></li>

</ul>

<h3>Related Topics</h3>

<ul>

<li><a href="index.html">Documentation overview</a><ul>

<li>Previous: <a href="index.html" title="previous chapter">Spatial Maths for Python</a></li>

<li>Next: <a href="spatialmath.html" title="next chapter">Classes and functions</a></li>

</ul></li>

View remainder of file in raw view

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

FilesExpand file tree

intro.html

Latest commit

History

intro.html

File metadata and controls