# 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])

Glossary Site map Index Support Contact Legal Notices