SoftBank Robotics documentation What's new in NAOqi 2.8?

Alternatives to ALMath’s python bindings

ALMath provides some types which are useful for kinematics computations.

However, equivalent functionality is now available from the numpy and scipy.spatial python modules, which are relevant alternatives to ALMath’s python bindings.

In particular, they better comply to recent python ecosystem evolutions such as PEP 465.

This page shows how numpy and scipy.spatial modules can be used in place of some of ALMath types.

Conventions

>>> import almath
>>> import math
>>> import numpy
>>> import scipy.spatial.transform

>>> FRAME_WORLD = 0
>>> class FakeALMotionProxy:
...     def getCOM(self, name, space, useSensors):
...         return [2., 0., 6.]
...     def getPosition(self, name):
...         return [1, 2, 3, 4, 5, 6] # x, y, z, wx, wy, wz
...     def getTransform(self, name, frame, useSensors):
...         return [1, 0, 0, 10.,
...                 0, 1, 0, 20.,
...                 0, 0, 1, 30.,
...                 0, 0, 0, 1]
...     def getRobotPosition(self, name):
...         return [1.1, 0.2, math.pi/6] # x, y, wz
...     def getWorldRotation(self):
...         return [1, 2, 3] # wx, wy, wz
>>> almotion = FakeALMotionProxy()

Spatial kinematics

  • almath.Position3D

    three floating-point numbers representing the position of a point in 3 dimensional space.

    Alternative: a (3,)-shaped numpy array.

    Create an instance from individual coordinates:

    >>> almath.Position3D()
Position3D(x=0, y=0, z=0)
>>> v0_al = almath.Position3D(1., 2., 3.)
>>> v0_al
Position3D(x=1, y=2, z=3)
>>> list(v0_al.toVector())
[1.0, 2.0, 3.0]

    >>> numpy.zeros(3)
array([0., 0., 0.])
>>> v0_np = numpy.array([1., 2., 3.])
>>> v0_np
array([1., 2., 3.])
>>> list(v0_np)
[1.0, 2.0, 3.0]

    Create an instance from a vector of coordinates, such as returned by ALMotionProxy::getCOM:

    >>> v1 = almotion.getCOM("Body", 0, False)
>>> v1
[2.0, 0.0, 6.0]

    >>> v1_al = almath.Position3D(v1)
>>> v1_al
Position3D(x=2, y=0, z=6)

    >>> v1_np = numpy.array(v1)
>>> v1_np
array([2., 0., 6.])

    operations

    >>> v0_al - 0.5 * v1_al
Position3D(x=0, y=2, z=0)
>>> almath.distance(v0_al, v1_al/2)
2.0

    >>> v0_np - 0.5 * v1_np
array([0., 2., 0.])
>>> numpy.linalg.norm(v0_np - v1_np/2)
2.0

    >>> almath.dotProduct(v0_al, v1_al)
20.0
>>> almath.crossProduct(v0_al, v1_al)
Position3D(x=12, y=0, z=-4)

    >>> numpy.inner(v0_np, v1_np)
20.0
>>> numpy.cross(v0_np, v1_np)
array([12.,  0., -4.])

  • almath.Velocity3D

    three floating-point numbers representing the velocity of a point in a 3 dimensional space.

    Alternative: a (3,) shaped numpy array. See almath.Position3D above.

    >>> almath.Velocity3D()
Velocity3D(xd=0, yd=0, zd=0)

    >>> numpy.zeros(3)
array([0., 0., 0.])

  • almath.Rotation

    represents a rotation in euclidean space as a 3x3 (orthogonal) matrix.

    Alternatives:

    • a (3, 3)-shaped numpy.array.
    • a scipy.spatial.transform.Rotation

    Create a rotation:

    >>> almath.Rotation()
Rotation([1, 0, 0
          0, 1, 0
          0, 0, 1])
>>> r0_al = almath.rotationFromRotZ(almath.PI/4)
>>> r0_al
Rotation([0.707107, -0.707107, 0
          0.707107, 0.707107, 0
          0, 0, 1])

    >>> numpy.identity(3)
array([[1., 0., 0.],
       [0., 1., 0.],
       [0., 0., 1.]])
>>> def rotationFromRotZ(a):
...     return numpy.array([[ math.cos(a), -math.sin(a), 0.],
...                         [ math.sin(a),  math.cos(a), 0.],
...                         [ 0.         ,  0.         , 1.]])
>>> r0_np = rotationFromRotZ(math.pi/4)
>>> r0_np
array([[ 0.70710678, -0.70710678,  0.        ],
       [ 0.70710678,  0.70710678,  0.        ],
       [ 0.        ,  0.        ,  1.        ]])

    >>> scipy.spatial.transform.Rotation.from_rotvec([0, 0, 0]) 
<scipy.spatial.transform.rotation.Rotation object at ...>
>>> scipy.spatial.transform.Rotation.from_rotvec([0, 0, 0]).as_matrix()
array([[1., 0., 0.],
       [0., 1., 0.],
       [0., 0., 1.]])
>>> r0_sp = scipy.spatial.transform.Rotation.from_rotvec([math.pi/4, 0, 0])
>>> r0_sp.as_matrix()
array([[ 1.        ,  0.        ,  0.        ],
       [ 0.        ,  0.70710678, -0.70710678],
       [ 0.        ,  0.70710678,  0.70710678]])

    Apply a rotation to a point or a vector:

    >>> r0_al * almath.Position3D(10, 20, 30)
Position3D(x=-7.07107, y=21.2132, z=30)

    >>> numpy.matmul(r0_np, numpy.array([10, 20, 30]))
array([-7.07106781, 21.21320344, 30.        ])

    >>> r0_sp.apply(numpy.array([10, 20, 30]))
array([10.        , -7.07106781, 35.35533906])
>>> numpy.matmul(r0_sp.as_matrix(), numpy.array([10, 20, 30]))
array([10.        , -7.07106781, 35.35533906])

  • almath.Transform

    represents a rigid displacement in euclidean space as a 4x4 homogeneous matrix.

    Alternative: a (4, 4)-shaped numpy array.

    Create an homogenous matrix:

    >>> almath.Transform()
Transform([1, 0, 0, 0
           0, 1, 0, 0
           0, 0, 1, 0])

    >>> numpy.identity(4)
array([[1., 0., 0., 0.],
       [0., 1., 0., 0.],
       [0., 0., 1., 0.],
       [0., 0., 0., 1.]])

    >>> tr0_al = almath.transformFromRotZ(almath.PI/4)
>>> tr0_al
Transform([0.707107, -0.707107, 0, 0
           0.707107, 0.707107, 0, 0
           0, 0, 1, 0])

    >>> def transformFromRotation(r):
...     ret = numpy.identity(4)
...     ret[0:3, 0:3] = r
...     return ret
>>> tr0_np = transformFromRotation(rotationFromRotZ(math.pi/4))
>>> tr0_np
array([[ 0.70710678, -0.70710678,  0.        ,  0.        ],
       [ 0.70710678,  0.70710678,  0.        ,  0.        ],
       [ 0.        ,  0.        ,  1.        ,  0.        ],
       [ 0.        ,  0.        ,  0.        ,  1.        ]])

    >>> xy = math.sqrt(2)
>>> tr1_al = almath.transformFromPosition(xy, xy, 3.)
>>> tr1_al
Transform([1, 0, 0, 1.41421
           0, 1, 0, 1.41421
           0, 0, 1, 3])

    >>> def transformFromPosition(x, y, z):
...     return numpy.array([[ 1., 0., 0., x  ],
...                         [ 0., 1., 0., y  ],
...                         [ 0., 0., 1., z  ],
...                         [ 0., 0., 0., 1. ]])
>>> xy = math.sqrt(2)
>>> tr1_np = transformFromPosition(xy, xy, 3.)
>>> tr1_np
array([[1.        , 0.        , 0.        , 1.41421356],
       [0.        , 1.        , 0.        , 1.41421356],
       [0.        , 0.        , 1.        , 3.        ],
       [0.        , 0.        , 0.        , 1.        ]])

    Extract the translation part:

    >>> almath.position3DFromTransform(tr1_al)
Position3D(x=1.41421, y=1.41421, z=3)

    >>> tr1_np[0:3, 3]
array([1.41421356, 1.41421356, 3.        ])

    Extract the rotation part:

    >>> almath.rotationFromTransform(tr1_al)
Rotation([1, 0, 0
          0, 1, 0
          0, 0, 1])

    >>> tr1_np[0:3, 0:3]
array([[1., 0., 0.],
       [0., 1., 0.],
       [0., 0., 1.]])

    Serialize as a list of elements (row major):

    >>> list(tr1_al.toVector())
[1.0, 0.0, 0.0, 1.4142135381698608, 0.0, 1.0, 0.0, 1.4142135381698608, 0.0, 0.0, 1.0, 3.0, 0.0, 0.0, 0.0, 1.0]

    >>> list(tr1_np.reshape(16))
[1.0, 0.0, 0.0, 1.4142135623730951, 0.0, 1.0, 0.0, 1.4142135623730951, 0.0, 0.0, 1.0, 3.0, 0.0, 0.0, 0.0, 1.0]

    Deserialize from a list of elements (row major), as returned by ALMotion’s getTransform:

    >>> almath.Transform(almotion.getTransform("Head", FRAME_WORLD, True))
Transform([1, 0, 0, 10
           0, 1, 0, 20
           0, 0, 1, 30])

    >>> numpy.array(almotion.getTransform("Head", FRAME_WORLD, True)).reshape(4, 4)
array([[ 1.,  0.,  0., 10.],
       [ 0.,  1.,  0., 20.],
       [ 0.,  0.,  1., 30.],
       [ 0.,  0.,  0.,  1.]])

    apply displacement to a point using matrix multiplication:

    >>> tr1_al * almath.Position3D(10, 20, 30)
Position3D(x=11.4142, y=21.4142, z=33)

    >>> numpy.matmul(tr1_np, numpy.array([10, 20, 30, 1]))[0:3]
array([11.41421356, 21.41421356, 33.        ])

    compose displacements with matrix multiplication and matrix inverse:

    >>> tr0_al * tr1_al
Transform([0.707107, -0.707107, 0, 0
           0.707107, 0.707107, 0, 2
           0, 0, 1, 3])


>>> almath.transformInverse(tr0_al)
Transform([0.707107, 0.707107, 0, -0
           -0.707107, 0.707107, 0, -0
           0, 0, 1, -0])

    >>> numpy.matmul(tr0_np, tr1_np) 
array([[ 7.07106781e-01, -7.07106781e-01,  0.00000000e+00, 2.22044605e-16],
       [ 7.07106781e-01,  7.07106781e-01,  0.00000000e+00, 2.00000000e+00],
       [ 0.00000000e+00,  0.00000000e+00,  1.00000000e+00, 3.00000000e+00],
       [ 0.00000000e+00,  0.00000000e+00,  0.00000000e+00, 1.00000000e+00]])

>>> numpy.linalg.inv(tr0_np)
array([[ 0.70710678,  0.70710678,  0.        ,  0.        ],
       [-0.70710678,  0.70710678,  0.        ,  0.        ],
       [ 0.        ,  0.        ,  1.        ,  0.        ],
       [ 0.        ,  0.        ,  0.        ,  1.        ]])

  • almath.Quaternion

    represents a rotation in euclidean space as a unit quaternion.

    Alternative: a scipy.spatial.transform.Rotation

    >>> almath.Quaternion()
Quaternion(w=1, x=0, y=0, z=0)

    >>> r = scipy.spatial.transform.Rotation.from_rotvec([0, 0, 0])
>>> r.as_quat() # quaternion in scalar-last (x, y, z, w) format
array([0., 0., 0., 1.])

  • almath.Rotation3D

    a set of Euler angles, used to represent a rotation in euclidean space.

    Euler angles usage is discouraged.

    Alternative: scipy.spatial.transform.Rotation, which supports Euler angles conversion.

    >>> almath.Rotation3D()
Rotation3D(wx=0, wy=0, wz=0)
>>> almath.quaternionFromRotation3D(almath.Rotation3D(1,2,3))
Quaternion(w=0.435953, x=-0.718287, y=0.310622, z=0.444435)
>>> almath.rotationFromTransform(almath.transformFromRotation3D(almath.Rotation3D(1,2,3)))
Rotation([0.411982, -0.833738, -0.36763
          -0.0587266, -0.426918, 0.902382
          -0.909297, -0.350175, -0.224845])

    >>> r = scipy.spatial.transform.Rotation.from_euler('xyz', [1, 2, 3])
>>> r.as_quat() # quaternion in scalar-last (x, y, z, w) format
array([-0.71828702,  0.31062245,  0.44443511,  0.43595284])
>>> r.as_matrix()
array([[ 0.41198225, -0.83373765, -0.36763046],
       [-0.05872664, -0.42691762,  0.90238159],
       [-0.90929743, -0.35017549, -0.2248451 ]])

  • almath.Position6D

    a set of Euler angles and translations, used to represent a pose in euclidean space.

    Euler angles usage is discouraged. Alternative: none.

    Examples:

    >>> almath.Position6D()
Position6D(x=0, y=0, z=0, wx=0, wy=0, wz=0)

Planar kinematics

  • almath.Position2D

    two floating-point numbers representing the position of a point in 2 dimensional space (the XY plane)

    Alternative: a (2,) shaped numpy array.

    >>> almath.Position2D()
Position2D(x=0, y=0)

>>> almath.position2DFromTransform(tr1_al)
Position2D(x=1.41421, y=1.41421)

    >>> numpy.zeros([2])
array([0., 0.])

>>> tr1_np[0:2, 3]
array([1.41421356, 1.41421356])

  • almath.Pose2D

    three floating-point numbers representing a pose in 2 dimensional space (the XY plane), as a translation in XY and a rotation along Z.

    Usage of planar transforms is discouraged. For most computations, using spatial geometry is easier and less error-prone.

    Whenever a given spatial rotation is along the Z-axis, its angle can be computed

    >>> almath.pose2DFromTransform(tr0_al).theta
0.7853981852531433

    >>> math.atan2(tr0_np[1, 0], tr0_np[0, 0])
0.7853981633974483

    >>> scipy.spatial.transform.Rotation.from_dcm(tr0_np[0:3, 0:3]).as_rotvec()
array([0.        , 0.        , 0.78539816])