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