|
15 | 15 |
|
16 | 16 | import sys |
17 | 17 | import math |
| 18 | +from math import sin, cos |
18 | 19 | import numpy as np |
19 | 20 | from spatialmath import base |
20 | 21 |
|
@@ -379,29 +380,29 @@ def rpy2r(roll, pitch=None, yaw=None, *, unit='rad', order='zyx'): |
379 | 380 | :type yaw: float |
380 | 381 | :param unit: angular units: 'rad' [default], or 'deg' |
381 | 382 | :type unit: str |
382 | | - :param unit: rotation order: 'zyx' [default], 'xyz', or 'yxz' |
383 | | - :type unit: str |
| 383 | + :param order: rotation order: 'zyx' [default], 'xyz', or 'yxz' |
| 384 | + :type order: str |
384 | 385 | :return: SO(3) rotation matrix |
385 | 386 | :rtype: ndarray(3,3) |
386 | 387 | :raises ValueError: bad argument |
387 | 388 |
|
388 | | - - ``rpy2r(ROLL, PITCH, YAW)`` is an SO(3) orthonormal rotation matrix |
389 | | - (3x3) equivalent to the specified roll, pitch, yaw angles angles. |
390 | | - These correspond to successive rotations about the axes specified by ``order``: |
| 389 | + - ``rpy2r(⍺, β, γ)`` is an SO(3) orthonormal rotation matrix (3x3) |
| 390 | + equivalent to the specified roll (⍺), pitch (β), yaw (γ) angles angles. |
| 391 | + These correspond to successive rotations about the axes specified by |
| 392 | + ``order``: |
391 | 393 |
|
392 | | - - 'zyx' [default], rotate by yaw about the z-axis, then by pitch about the new y-axis, |
393 | | - then by roll about the new x-axis. Convention for a mobile robot with x-axis forward |
394 | | - and y-axis sideways. |
395 | | - - 'xyz', rotate by yaw about the x-axis, then by pitch about the new y-axis, |
396 | | - then by roll about the new z-axis. Covention for a robot gripper with z-axis forward |
397 | | - and y-axis between the gripper fingers. |
398 | | - - 'yxz', rotate by yaw about the y-axis, then by pitch about the new x-axis, |
399 | | - then by roll about the new z-axis. Convention for a camera with z-axis parallel |
400 | | - to the optic axis and x-axis parallel to the pixel rows. |
| 394 | + - 'zyx' [default], rotate by γ about the z-axis, then by β about the new |
| 395 | + y-axis, then by ⍺ about the new x-axis. Convention for a mobile robot |
| 396 | + with x-axis forward and y-axis sideways. |
| 397 | + - 'xyz', rotate by γ about the x-axis, then by β about the new y-axis, |
| 398 | + then by ⍺ about the new z-axis. Convention for a robot gripper with |
| 399 | + z-axis forward and y-axis between the gripper fingers. |
| 400 | + - 'yxz', rotate by γ about the y-axis, then by β about the new x-axis, |
| 401 | + then by ⍺ about the new z-axis. Convention for a camera with z-axis |
| 402 | + parallel to the optic axis and x-axis parallel to the pixel rows. |
401 | 403 |
|
402 | 404 | - ``rpy2r(RPY)`` as above but the roll, pitch, yaw angles are taken |
403 | | - from ``RPY`` which is a 3-vector with values |
404 | | - (ROLL, PITCH, YAW). |
| 405 | + from ``RPY`` which is a 3-vector with values (⍺, β, γ). |
405 | 406 |
|
406 | 407 | .. runblock:: pycon |
407 | 408 |
|
@@ -445,28 +446,27 @@ def rpy2tr(roll, pitch=None, yaw=None, unit='rad', order='zyx'): |
445 | 446 | :type yaw: float |
446 | 447 | :param unit: angular units: 'rad' [default], or 'deg' |
447 | 448 | :type unit: str |
448 | | - :param unit: rotation order: 'zyx' [default], 'xyz', or 'yxz' |
449 | | - :type unit: str |
| 449 | + :param order: rotation order: 'zyx' [default], 'xyz', or 'yxz' |
| 450 | + :type order: str |
450 | 451 | :return: SE(3) transformation matrix |
451 | 452 | :rtype: ndarray(4,4) |
452 | 453 |
|
453 | | - - ``rpy2tr(ROLL, PITCH, YAW)`` is an SO(3) orthonormal rotation matrix |
454 | | - (3x3) equivalent to the specified roll, pitch, yaw angles angles. |
455 | | - These correspond to successive rotations about the axes specified by ``order``: |
| 454 | + - ``rpy2tr(⍺, β, γ)`` is an SE(3) matrix (4x4) equivalent to the specified |
| 455 | + roll (⍺), pitch (β), yaw (γ) angles angles. These correspond to successive |
| 456 | + rotations about the axes specified by ``order``: |
456 | 457 |
|
457 | | - - 'zyx' [default], rotate by yaw about the z-axis, then by pitch about the new y-axis, |
458 | | - then by roll about the new x-axis. Convention for a mobile robot with x-axis forward |
459 | | - and y-axis sideways. |
460 | | - - 'xyz', rotate by yaw about the x-axis, then by pitch about the new y-axis, |
461 | | - then by roll about the new z-axis. Convention for a robot gripper with z-axis forward |
462 | | - and y-axis between the gripper fingers. |
463 | | - - 'yxz', rotate by yaw about the y-axis, then by pitch about the new x-axis, |
464 | | - then by roll about the new z-axis. Convention for a camera with z-axis parallel |
465 | | - to the optic axis and x-axis parallel to the pixel rows. |
| 458 | + - 'zyx' [default], rotate by γ about the z-axis, then by β about the new |
| 459 | + y-axis, then by ⍺ about the new x-axis. Convention for a mobile robot |
| 460 | + with x-axis forward and y-axis sideways. |
| 461 | + - 'xyz', rotate by γ about the x-axis, then by β about the new y-axis, |
| 462 | + then by ⍺ about the new z-axis. Convention for a robot gripper with |
| 463 | + z-axis forward and y-axis between the gripper fingers. |
| 464 | + - 'yxz', rotate by γ about the y-axis, then by β about the new x-axis, |
| 465 | + then by ⍺ about the new z-axis. Convention for a camera with z-axis |
| 466 | + parallel to the optic axis and x-axis parallel to the pixel rows. |
466 | 467 |
|
467 | 468 | - ``rpy2tr(RPY)`` as above but the roll, pitch, yaw angles are taken |
468 | | - from ``RPY`` which is a 3-vector with values |
469 | | - (ROLL, PITCH, YAW). |
| 469 | + from ``RPY`` which is a 3-vector with values (⍺, β, γ). |
470 | 470 |
|
471 | 471 | .. runblock:: pycon |
472 | 472 |
|
@@ -502,12 +502,11 @@ def eul2r(phi, theta=None, psi=None, unit='rad'): |
502 | 502 | :return: SO(3) rotation matrix |
503 | 503 | :rtype: ndarray(3,3) |
504 | 504 |
|
505 | | - - ``R = eul2r(PHI, θ, PSI)`` is an SO(3) orthonornal rotation |
| 505 | + - ``R = eul2r(φ, θ, ψ)`` is an SO(3) orthonornal rotation |
506 | 506 | matrix equivalent to the specified Euler angles. These correspond |
507 | 507 | to rotations about the Z, Y, Z axes respectively. |
508 | 508 | - ``R = eul2r(EUL)`` as above but the Euler angles are taken from |
509 | | - ``EUL`` which is a 3-vector with values |
510 | | - (PHI θ PSI). |
| 509 | + ``EUL`` which is a 3-vector with values (φ θ ψ). |
511 | 510 |
|
512 | 511 | .. runblock:: pycon |
513 | 512 |
|
@@ -892,7 +891,7 @@ def tr2rpy(T, unit='rad', order='zyx', check=False): |
892 | 891 | :param unit: 'rad' or 'deg' |
893 | 892 | :type unit: str |
894 | 893 | :param order: 'xyz', 'zyx' or 'yxz' [default 'zyx'] |
895 | | - :type unit: str |
| 894 | + :type order: str |
896 | 895 | :param check: check that rotation matrix is valid |
897 | 896 | :type check: bool |
898 | 897 | :return: Roll-pitch-yaw angles |
|
0 commit comments