bdaiinstitute / spatialmath-python Goto Github PK

Installed everything with
pip install rvc3python
When I run ~/.local/bin/rvctool I get the following:

phil@Office:~$ ~/.local/bin/rvctool 
Traceback (most recent call last):
  File "/home/phil/.local/bin/rvctool", line 5, in <module>
    from RVC3.bin.rvctool import main
  File "/home/phil/.local/lib/python3.10/site-packages/RVC3/bin/rvctool.py", line 42, in <module>
    from spatialmath import *  # lgtm [py/polluting-import]
  File "/home/phil/.local/lib/python3.10/site-packages/spatialmath/__init__.py", line 3, in <module>
    from spatialmath.pose2d import SO2, SE2
  File "/home/phil/.local/lib/python3.10/site-packages/spatialmath/pose2d.py", line 26, in <module>
    import spatialmath.base as smb
  File "/home/phil/.local/lib/python3.10/site-packages/spatialmath/base/__init__.py", line 5, in <module>
    from spatialmath.base.argcheck import *  # lgtm [py/polluting-import]
  File "/home/phil/.local/lib/python3.10/site-packages/spatialmath/base/argcheck.py", line 19, in <module>
    from spatialmath.base.symbolic import issymbol, symtype
  File "/home/phil/.local/lib/python3.10/site-packages/spatialmath/base/symbolic.py", line 15, in <module>
    from spatialmath.base.types import *
  File "/home/phil/.local/lib/python3.10/site-packages/spatialmath/base/types.py", line 9, in <module>
    from spatialmath.base._types_39 import *
  File "/home/phil/.local/lib/python3.10/site-packages/spatialmath/base/_types_39.py", line 37, in <module>
    ArrayLike = Union[float, List[float], Tuple[float, ...], ndarray[Any, dtype[floating]]]
TypeError: 'numpy._DTypeMeta' object is not subscriptable

Which, looking at the other issues, looks like it may be more typing challenges.
Python is v. 3.10.12, OS is Ubuntu 22.04.3 LTS

Hi,

I need to only get the position coordinate a from spatialmath.pose3d.SE3 object, so I tried to run T[-1] as it says in the robotics toolbox documentation. However that gives me the entire T matrix, not just the last column (with or without 1 at the end). I have also tried getting T[3,0], T[-1,0] and so on, but then I get warnings.

Example:

T = robot.fkine(q)   # for a joint configuration q and robot = rtb.models.Panda()
print(type(T[-1]), np.shape(T[-1]))
>>> <class 'spatialmath.pose3d.SE3'> (4, 4)

I am running spatialmath version 1.1.8 (and robotics toolbox 1.1.0).

Hi, i have tried simple plotting for SE3

but it dosent show anything.

from spatialmath import * import matplotlib.pyplot as plt X = SE3(1,2,3)*SE3.Rx(30, 'deg') print(X) X.plot()

this code print out the X but not the figure. and it doesn't show any error.
is there anything i missed?

my python version is 3.6.8

The docstring says this:

• angdiff(a, b) is the difference a - b wrapped to the range
  :math:[-\pi, \pi). This is the operator :math:a \circleddash b used
  in the RVC book
  - If a and b are both scalars, the result is scalar

The output of the function is this:
In [9]: angdiff(10,0) Out[9]: array([-2.56637061])

I have some confusion with converting unit quaternions to rotations matrices and back.

>>> import numpy as np
>>> from spatialmath import UnitQuaternion as UQ
>>> from spatialmath import SO3
>>> vb = np.array([-0.424545,0.329415,-0.125123,0.834021])
>>> qb = UQ(vb)
>>> print(qb)
 0.4245 << -0.3294,  0.1251, -0.8340 >>
>>> # Scalar part is always positive, see qunit
>>> Cb = qb.SO3()
>>> qbp = UQ(Cb)
>>> print(qbp)
-0.4245 <<  0.3294, -0.3294,  0.8340 >>
>>> # Two things to note. Firstly, scalar part is not positive. Secondly, one of the values does not match the one in the initial vector.
>>> (qbp.v[0] + qbp.v[1]) == 0
True
>>> from scipy.spatial.transform import Rotation
>>> qbs = Rotation.from_matrix(Cb).as_quat()
>>> print(qbs)
[ 0.32941508 -0.12512303  0.83402121 -0.42454511]
>>> # With scipy, the conversation seems to be correct

I can observe three things

• Conversion from rotation matrix to unit quaternion with spatialmath does not result in unit quaternion with positive scalar part
• Conversion seems to be erroneous because v[0] equal to v[1]
• Conversion with scipy seems to be correct

I had a look at the source code quaternions.py and the reference https://doi.org/10.1115/1.4041889
I haven't had yet the time to study the details. Just by analogy, there seems to be an error at line 632:

    if i == 0:
        e[1] = math.copysign(e[1], R[2, 1] - R[1, 2])
        e[2] = math.copysign(e[2], R[0, 2] - R[2, 0])
        e[3] = math.copysign(e[3], R[1, 0] - R[0, 1])
    elif i == 1:
        e[0] = math.copysign(e[0], R[2, 1] - R[1, 2])
        e[2] = math.copysign(e[2], R[1, 0] + R[0, 1])
        e[3] = math.copysign(e[3], R[0, 2] + R[2, 0])
    elif i == 2:
        e[0] = math.copysign(e[0], R[0, 2] - R[2, 0])
        e[1] = math.copysign(e[1], R[1, 0] + R[0, 1])
        e[3] = math.copysign(e[3], R[2, 1] + R[1, 2])
    else:
        e[0] = math.copysign(e[0], R[1, 0] - R[0, 1])
        e[1] = math.copysign(e[1], R[0, 2] + R[2, 0])
        e[2] = math.copysign(e[1], R[2, 1] + R[1, 2])

that is, at e[2] = math.copysign(e[1], R[2, 1] + R[1, 2]). Might it be e[2] = math.copysign(e[2], R[2, 1] + R[1, 2])? However, this might not yet explain the error above.

I initially tried multiplying an SE3 by an SO3 when trying to apply an arbitrary rotation to an SE3 pose. This currently returns an identity SE3:

In [1]: from spatialmath import *

In [2]: a = SE3(1, 2, 3)

In [3]: a
Out[3]: 
   1         0         0         1         
   0         1         0         2         
   0         0         1         3         
   0         0         0         1         


In [4]: b = SO3.Rz(45, unit='deg')

In [5]: b
Out[5]: 
   0.7071   -0.7071    0         
   0.7071    0.7071    0         
   0         0         1         


In [6]: a * b
Out[6]: 
   1         0         0         0         
   0         1         0         0         
   0         0         1         0         
   0         0         0         1 

The behaviour seems incorrect, but I'm not sure what the correct resolution should be:

• Throw an error if SE3 * SO3 is a pattern that shouldn't be used, or
• Return the same result as the equivalent SE3 * SE3 operation

Same behaviour is also currently present with SE2s and SO2s.

When I try to initialize a SE3 instance with a 4X4 numpy array, it doesn't seem to work，（maybe I don't have enough skills ？）

instead I tried to create an instance of SO3 using a 3X3 rotation matrix, and I was pretty sure the numpy array A I gave was a rotation matrix (normalized). and
A @ A.T = array([[ 1.00000000e+00, -2.77555756e-17, 0.00000000e+00], [-2.77555756e-17, 1.00000000e+00, 0.00000000e+00], [ 0.00000000e+00, 0.00000000e+00, 1.00000005e+00]])
but the code always report error to me that
`ValueError Traceback (most recent call last)
Cell In[119], line 7
4 # # u,_,v = np.linalg.svd(a)
5 # a1 = np.dot(u,v)
6 from spatialmath import SO3
----> 7 SO3(a)
8 # # a.dtype
9 # # a1.T@a1
10 # a.T@a

File c:\pythonproject2\venv1\lib\site-packages\spatialmath\pose3d.py:104, in SO3.init(self, arg, check)
101 self.data = [smb.t2r(x) for x in arg.data]
103 elif not super().arghandler(arg, check=check):
--> 104 raise ValueError("bad argument to constructor")
when I change the value of A to A1 = np.asarray([[ 0.00000000000001, -1., 0.],
[ 1., 0., 0.],
[ 0., 0., 1.]])`
the SO3 instance is created successfully.
hence, I suspect the root of the problem may be a matter of precision , But there is no proper way to solve it

First of all, let me express my deep appreciation for your work and thank you for sharing your experience and knowledge with the world in this way.
I hope I'am not wrong in assuming that a mistake has crept in the transforms3d.py in lines 1772-1775.

I think that for the trplot function to work properly the following changes need to be made:

o = T @ np.array([0, 0, 0, 1])
x = T @ np.array([length, 0, 0, 1])
y = T @ np.array([0, length, 0, 1])
z = T @ np.array([0, 0, length, 1])

Best regards,
Christian

Commit 4251e79 seems to have added the strline() function for getting string representations as a replacement for printline(file=None).

Documentation for SE3 still suggests printline() will output a string when supplied with file=None:

Documentation for the new strline() function also doesn't seem to have been built.

I attempted to interpolate between two spatialmath.pose3d.SE3 objects and the returned result was also a spatialmath.pose3d.SE3 object but, the A matrix was of type None rather than a ndarray(4,4) as expected. I am trying to interpolate from transform 1 to transform 2 a certain amount, so s in the function is set in the interval 0 to 1. Both transform 1 and transform 2 are valid, yet the interpolated transform matrix (i.e. the NumPy array) returned from line 1504 in spatialmath.base.transforms3d.py (trinterp function), is not valid. I am curious as to how interpolating between two valid poses could result in an invalid pose. Any thoughts?

I might be using the isvalid function incorrectly.
Side note: why have you made the isvalid function static rather than a class member function? Seems odd to have to do se3_object.isvalid(se3_object.A) rather than se3_object.isvalid().

Code and a pickle data to recreate is provided via Google Drive.

The SE3.Eul method returns the same matrix for a 180 degree rotation about the X and Z axes.

Running

import spatialmath as sm
print(sm.SE3.Eul([0, 0, 180], unit='deg')) # this creates same rotation matrix as line below
print(sm.SE3.Eul([180, 0, 0], unit='deg'))

Haven't had a chance to look further as to why. Will hopefully find the root of the problem and do a PR.

Hi,
I'm trying to convert an array of x,y,theta values to a SE2 Objekt. I used this function to do so.

def ar2se2(array):
    """Create se2-object from numpy array"""
    poselist = list(array)
    T = SE2().Empty()
    for p in poselist:
        T.append(SE2(p, unit='deg'))
    return T

Is there a more efficient way?

Thank you,

Lukas

I am having a hard time showcasing the animation of tranimate in python-notebooks. What is the recipe to get them to visualize here?

i got this error when run on jetson nano python 3.6 :

Traceback (most recent call last):
File "", line 1, in
File "/home/sadid/pyenv/sadidrobot/lib/python3.6/site-packages/spatialmath/init.py", line 1, in
from spatialmath.pose2d import SO2, SE2
File "/home/sadid/pyenv/sadidrobot/lib/python3.6/site-packages/spatialmath/pose2d.py", line 26, in
from spatialmath.base import argcheck
File "/home/sadid/pyenv/sadidrobot/lib/python3.6/site-packages/spatialmath/base/init.py", line 7, in
from spatialmath.base.transforms2d import * # lgtm [py/polluting-import]
File "/home/sadid/pyenv/sadidrobot/lib/python3.6/site-packages/spatialmath/base/transforms2d.py", line 20, in
import spatialmath.base as smb
AttributeError: module 'spatialmath' has no attribute 'base'

from spatialmath import base as base
Traceback (most recent call last):
File "", line 1, in
File "/home/sadid/pyenv/sadidrobot/lib/python3.6/site-packages/spatialmath/init.py", line 1, in
from spatialmath.pose2d import SO2, SE2
File "/home/sadid/pyenv/sadidrobot/lib/python3.6/site-packages/spatialmath/pose2d.py", line 26, in
from spatialmath.base import argcheck
File "/home/sadid/pyenv/sadidrobot/lib/python3.6/site-packages/spatialmath/base/init.py", line 7, in
from spatialmath.base.transforms2d import * # lgtm [py/polluting-import]
File "/home/sadid/pyenv/sadidrobot/lib/python3.6/site-packages/spatialmath/base/transforms2d.py", line 20, in
import spatialmath.base as smb
AttributeError: module 'spatialmath' has no attribute 'base'

Error is throwing when using interp1():

In [12]: a = SE3(1, 2, 0) * SE3.Rz(45, unit='deg')

In [13]: b = a.interp1(0.5)
---------------------------------------------------------------------------
UnboundLocalError                         Traceback (most recent call last)
<ipython-input-13-efa36fbc2384> in <module>
----> 1 b = a.interp1(0.5)

~/.python-venv/lib/python3.8/site-packages/spatialmath/baseposematrix.py in interp1(self, s)
    477         s = np.clip(s, 0, 1)
    478 
--> 479         if start is not None:
    480             assert len(start) == 1, 'len(start) must == 1'
    481             start = start.A

UnboundLocalError: local variable 'start' referenced before assignment

Error occurs for both SE2 and SE3 classes.

A workaround is to call interp() explicity with the identity pose, e.g.:

SE3().interp(a, 0.5)

When calling angvec() on the SE3 object with the below values:

dTx:

1 3.881e-14 0 -1.985e-13
-3.881e-14 1 1.438e-11 1.192e-13
0 -1.438e-11 1 0
0 0 0 1

I am getting a returned value of

ang:nan

from spatialmath import *
Traceback (most recent call last):
File "", line 1, in
File "/datasets/magnetic_tool/spatialmath-python/spatialmath/init.py", line 1, in
from spatialmath.pose2d import SO2, SE2
File "/datasets/magnetic_tool/spatialmath-python/spatialmath/pose2d.py", line 26, in
from spatialmath.base import argcheck
File "/datasets/magnetic_tool/spatialmath-python/spatialmath/base/init.py", line 8, in
from spatialmath.base.transforms3d import * # lgtm [py/polluting-import]
File "/datasets/magnetic_tool/spatialmath-python/spatialmath/base/transforms3d.py", line 24, in
import spatialmath.base.symbolic as sym
AttributeError: module 'spatialmath' has no attribute 'base'

I think, in the example the first variable should be T instead of T^{-1}:
https://github.com/petercorke/spatialmath-python/blob/140d499e733ed9775762df90d36e4b2c4c2fc6eb/spatialmath/pose2d.py#L515

Fall through error in the binary operator for BasePoseMatrix:

https://github.com/bdaiinstitute/spatialmath-python/blob/1b89c49395a21b5241e2f0a233e69394f3bc27b1/spatialmath/baseposematrix.py#L1632C7-L1632C7

If you want to compare the rotation of an SE3 to an SO3, it returns none as opposed to throwing an error or returning a valid value. This should probably raise an error as opposed to returning None.

import numpy as np
from spatialmath import SE3, SO3

T0 = SE3()
print(T0.angdist(SO3()))

Could be fixed with closing else in BasePoseMatrix._op2

    else:
        raise ValueError(f'Invalid type ({right.__class__}) for binary operation with {left.__class__}')

I am wondering if there is a better way to derive SO3 from SE3 than the following:

X = SE3.Rand()
R = SO3(X.R)

Any suggestions?

Trying to import the "base" attribute of the "spatialmath" module gives me an error message.
I am trying to import it using the following statement at the beginning of a jupyter notebook:
import spatialmath.base as tr

The error message is:
`---------------------------------------------------------------------------
AttributeError Traceback (most recent call last)
in
----> 1 import spatialmath.base

~/test_prep_sem_3/viewpoints_generation/automated-part-inspection/.venv/lib/python3.6/site-packages/spatialmath/init.py in
----> 1 from spatialmath.pose2d import SO2, SE2
2 from spatialmath.pose3d import SO3, SE3
3 from spatialmath.baseposematrix import BasePoseMatrix
4 from spatialmath.geom2d import Line2, Polygon2
5 from spatialmath.geom3d import Line3, Plane3

~/test_prep_sem_3/viewpoints_generation/automated-part-inspection/.venv/lib/python3.6/site-packages/spatialmath/pose2d.py in
24 import numpy as np
25
---> 26 from spatialmath.base import argcheck
27 from spatialmath import base as base
28 from spatialmath.baseposematrix import BasePoseMatrix

~/test_prep_sem_3/viewpoints_generation/automated-part-inspection/.venv/lib/python3.6/site-packages/spatialmath/base/init.py in
6 from spatialmath.base.quaternions import * # lgtm [py/polluting-import]
7 from spatialmath.base.transforms2d import * # lgtm [py/polluting-import]
----> 8 from spatialmath.base.transforms3d import * # lgtm [py/polluting-import]
9 from spatialmath.base.transformsNd import * # lgtm [py/polluting-import]
10 from spatialmath.base.vectors import * # lgtm [py/polluting-import]

~/test_prep_sem_3/viewpoints_generation/automated-part-inspection/.venv/lib/python3.6/site-packages/spatialmath/base/transforms3d.py in
22
23 from spatialmath import base as smb
---> 24 import spatialmath.base.symbolic as sym
25
26 _eps = np.finfo(np.float64).eps

AttributeError: module 'spatialmath' has no attribute 'base'`

What would be the thoughts around adding an ordering flag/argument to the function spatialmath.base.q2r? The spatialmath.base.r2q already as this flag/argument. While you have defined a quaternion to have the order (w,x,y,z) (documentation link). There other packages, like ROS, that work in an x,y,z,w order.

Even without that feature, I think the q2r documentation should be explicit about the quaternion order, as it is something that is easy to forget (that different packages use different quaternion orders).

Hi @petercorke,
We are trying to get the RTB to work with emscripten-forge (which compiles into WebAssembly). @wolfv noticed that the geom2d.py file on pypi contains a weird import:

from ssl import ALERT_DESCRIPTION_HANDSHAKE_FAILURE

Which causes trouble on emscripten-forge. I then noticed that the GitHub repo is out-of-sync (still on version 1.0.0, whereas pypi is at 1.0.1). Is there any chance to remove this odd import, publish a new version 1.0.2, and update the repo?

Many thanks,
Tobi
/cc @jhavl @wolfv

Line 1206 in pose3d.py:

Z = np.random.uniform(low=yrange[0], high=zrange[1], size=N) # random values in the range

"low" should be assigned with zrange[0] and not yrange[0].

When adding two spatial inertia using the + override I get a AttributeError: 'SpatialInertia' object has no attribute 'I'.

I think I'm noticing 3 potential mistakes in the source code at line 637

return SpatialInertia(left.I + left.I)

1. It seems SpatialInertia has no member I but rather a data[0]
2. We're adding left twice and not right.
3. I am not sure the syntax matches the definition in the constructor...

To make minimum changes, I would suggest rewritting this line as such:

total_inertia=SpatialInertia()
total_inertia.data[0] = left.data[0] + right.data[0]
return total_inertia

But I'm not familiar with the whole code corpus, and I just infer that from quickly reading the class. Does that seem correct? If so could we fix it? If it helps I am happy making a PR and expending tests so this doesn't happen anymore.

Otherwise, thanks for a cool library!

All axes are x in coordinate axis ,how do I fix it?

Pycharm2021 community
`
import spatialmath as sp
import numpy as np
import matplotlib.pyplot as plt
R1 = sp.SO3.Rx(0.3)
R2 = sp.SO3.Rz(30, 'deg')

R2.plot()
plt.show()
`

the issue comes from this line

import spatialmath.base as smb

in the file :

spatialmath-python/spatialmath/base/transforms2d.py", line 20 .

this line gives the following error :

import spatialmath.base as smb
AttributeError: module 'spatialmath' has no attribute 'base'

the solution is to change import model from above to below :

👍 from spatialmath import base as smb

I installed spatialmath by using the command "pip install spatialmath-python",

Then tried importing it in ipython:

"from spatialmath.base import *"

I got the error(s) as shown in the attached text file.

spatialmath_error.txt

ImportError: cannot import name 'plotvol3' from 'spatialmath.base.graphics' (/home/op/.local/lib/python3.10/site-packages/spatialmath/base/graphics.py)

I can see plotvol3 is there in the graphics.py, but keep getting this error

I got stuck with the following. I read the documentation and study the source code. The later, up to a certain degree.
I have a covariance matrix A that I would like to rotate by C. The covariance and rotation matrices are given by 2x2 numpy arrays:

>>> import numpy as np
>>> A = np.array([[10,0],[0,1]])
>>> alpha=0.5
>>> C = np.array([[np.cos(alpha), -np.sin(alpha)],[np.sin(alpha), np.cos(alpha)]])
>>> C
array([[ 0.87758256, -0.47942554],
       [ 0.47942554,  0.87758256]])
>>> B = C @ A @ np.transpose(C)
>>> B
array([[7.93136038, 3.78661943],
       [3.78661943, 3.06863962]])

If the rotation is given by SO2, then the following happens:

>>> from spatialmath import SO2
>>> Csm = SO2(0.5)
>>> Csm
SO2(array([[ 0.87758256, -0.47942554],
           [ 0.47942554,  0.87758256]]))
>>> Bsm = Csm * A * Csm.inv()
>>> Bsm
array([[1.00000000e+01, 2.58022041e-17],
       [3.59774358e-17, 1.00000000e+00]])

If I am not mistaken, the multiplication of the SO2 by the 2x2 numpy array is "column-wise". The operator * acts per column and not as a matrix multiplication. I can understand this behavior if one wants to transform a sequence of vectors but this can be a source of nasty errors.
I am wondering then how to best represent a "covariance matrix". I could not find a solution in the documentation.
One solution could be

>>> Csm.A @ A @ np.transpose(Csm.A)
array([[7.93136038, 3.78661943],
       [3.78661943, 3.06863962]])

But is there a better way? Is it possible to give the covariance a "matrix property" such that SE2 understands it?

Hi,

I want to create a SE3 object with a 4x4 np.array argument, the document says I can do it, but it seems that the implemented code doesn't accept np.array input when initialization. What's more, I can't find an easy way to work around this, e.g. create the SE3 by a SO3 and a translation.

Please check the code below

https://github.com/petercorke/spatialmath-python/blob/a3116021b8bd95b4f6015b180053941599ebd6cc/spatialmath/pose3d.py#L731-L785

THX

In the code related to applying length to each axis in this snippet: (transforms3D.py)
x = T @ np.array([1, 0, 0, 1]) * length
y = T @ np.array([0, 1, 0, 1]) * length
z = T @ np.array([0, 0, 1, 1]) * length
The length should not be multiplied by the last element
x = T @ np.array([1* length, 0, 0, 1])
y = T @ np.array([0, 1 * length, 0, 1])
z = T @ np.array([0, 0, 1 * length, 1])

I found using the interp function of the spatialmath toolbox that the interpolation results depend on the initial rotation frames. On further investigation I also found difference between the SE3.interp function and an implementation using
scipy.spatial.transform.Slerp.

Steps to Reproduce

• Create two SE3 frames with varying initial rotations.
• Interpolate between these two frames using SE3.interp.
• Compare the results by applying an additional rotation before interpolation and then unapplying it.
• Observe that the outputs vary depending on the initial angles.

Demo code

The code below provides both examples where SE3.interp is not working as expected, and an alternative implementation based using scipy.

from spatialmath import SE3
import numpy as np
from scipy.spatial.transform import Slerp, Rotation

def sp_interp(start_frame: SE3, end_frame: SE3, num_steps: int) -> list[SE3]:
    """
    Interpolate between two SE3 frames using spherical linear interpolation (SLERP) for rotation
    and linear interpolation for translation.

    Args:
    start_frame (SE3): The starting transformation frame.
    end_frame (SE3): The ending transformation frame.
    num_steps (int): Number of interpolation steps including start and end.

    Returns:
    list[SE3]: List of interpolated SE3 frames.
    """
    interpolated_frames = []
    time_vector = np.linspace(0, 1, num_steps)
    rotation_slerp = Slerp([0, 1], Rotation.from_matrix([start_frame.R, end_frame.R]))
    for rot, fraction in zip(rotation_slerp(time_vector), time_vector):
        translation = end_frame.t * fraction + start_frame.t * (1 - fraction)
        interpolated_frames.append(SE3.Rt(rot.as_matrix(), translation))
    return interpolated_frames

# Testing the function with various orientations and rotations
test_angles = (0, 88, 100, -179, -181)
test_cases = [(i, j, k) for i in test_angles for j in test_angles for k in test_angles]

for i, j, k in test_cases:
    initial_frame = SE3(0, 2, 0) * SE3.Rz(j, unit="deg") * SE3.Rx(i, unit="deg")
    final_frame = SE3.Ry(j, unit="deg")
    rotation_frame = SE3.Rz(k, unit="deg")

    # Standard interpolation and rotation transformation
    direct_interp = initial_frame.interp(final_frame, 5)[1]
    rotated_interp = rotation_frame.inv() * (rotation_frame * initial_frame).interp(rotation_frame * final_frame, 5)[1]

    # Using the spherical interpolation function
    slerp_result = sp_interp(initial_frame, final_frame, 5)[1]
    rotated_slerp_result = rotation_frame.inv() * sp_interp(rotation_frame * initial_frame, rotation_frame * final_frame, 5)[1]

    # Check and output any inconsistencies
    if not np.isclose(direct_interp.data, rotated_interp.data, atol=1e-2).all():
        print(f"SE3.interp depends on initial rotation, i={i}, j={j}, k={k}")

    if not np.isclose(slerp_result.data, rotated_slerp_result.data, atol=1e-2).all():
        print(f"sp_slerp depends on initial rotation, i={i}, j={j}, k={k}")

    if not np.isclose(direct_interp.data, slerp_result.data, atol=1e-2).all():
        print(f"SE3.interp and sp_slerp differ, i={i}, j={j}, k={k}")

Output / Reported inconsistencies

SE3.interp depends on initial rotation, i=0, j=100, k=100
SE3.interp depends on initial rotation, i=88, j=100, k=100
SE3.interp depends on initial rotation, i=100, j=100, k=100
SE3.interp and sp_slerp differ, i=-179, j=0, k=0
SE3.interp and sp_slerp differ, i=-179, j=0, k=88
SE3.interp and sp_slerp differ, i=-179, j=0, k=100
SE3.interp and sp_slerp differ, i=-179, j=0, k=-179
SE3.interp and sp_slerp differ, i=-179, j=0, k=-181
SE3.interp depends on initial rotation, i=-179, j=100, k=100
SE3.interp depends on initial rotation, i=-181, j=100, k=100

Feature description

I was wondering if it would make sense to add a look_at() function for this library for initializing an SE3 transformation matrix. I personally use this when positioning a camera etc. where I want the camera at an exact position, look at a specific point, and define which axis is up. This makes defining the pose of the camera very easy. I use an implementation of this function in mitsuba2, where the parameters are

• origin: defines the translation part of the matrix
• target: the target point the z-axis should point towards
• up: the direction which is perpendicular with the x-axis of the transform and the direction of the vector from the origin to the target

Examples

Below are a couple of examples of how the function works in Mitsuba2

Example 1

T = look_at(origin=[1,2,3], target=[0,0,0], up=[0,0,1])
print(T)

[[0.894427, -0.358569, -0.267261, 1],
 [-0.447214, -0.717137, -0.534522, 2],
 [0, 0.597614, -0.801784, 3],
 [0, 0, 0, 1]]

Example 2

T = look_at(origin=[0,0,0], target=[3,0,0], up=[0,1,0])
print(T)

[[0, -0, 1, 0],
 [0, 1, 0, 0],
 [-1, 0, 0, 0],
 [0, 0, 0, 1]]

Mitsuba C++ implementation

Source: https://github.com/mitsuba-renderer/mitsuba2/blob/ab5a49d4199a1b08d4d6557dfe6b0336fff4cfdf/include/mitsuba/core/transform.h

    static Transform look_at(const Point<Float, 3> &origin,
                             const Point<Float, 3> &target,
                             const Vector<Float, 3> &up) {
        using Vector3 = Vector<Float, 3>;

        Vector3 dir = normalize(target - origin);
        dir = normalize(dir);
        Vector3 left = normalize(cross(up, dir));

        Vector3 new_up = cross(dir, left);

        Matrix result = Matrix::from_cols(
            concat(left, Scalar(0)),
            concat(new_up, Scalar(0)),
            concat(dir, Scalar(0)),
            concat(origin, Scalar(1))
        );

        Matrix inverse = Matrix::from_rows(
            concat(left, Scalar(0)),
            concat(new_up, Scalar(0)),
            concat(dir, Scalar(0)),
            Vector<Float, 4>(0.f, 0.f, 0.f, 1.f)
        );

        inverse[3] = inverse * concat(-origin, Scalar(1));

        return Transform(result, transpose(inverse));
    }

Generalization

The Mitsuba implementation only works for pointing the z-axis at a specific direction defined by the direction of the vector between the origin and the target. The up-axis is also predefined to specify a specific axis in the resulting transformation matrix. For generalization of the function, it could make sense to specify which axes should point toward to target, and which axis is up. This will however complicate the function further. One could split the functionality into three functions for example:

SE3.x_look_at(origin, target, up)
SE3.y_look_at(origin, target, up)
SE3.z_look_at(origin, target, up)

Does this makes sense for anyone else, or would it bloat the code and documentation?

I'm using the module on KITTI dataset and I found a problem that your module is using tol=100 which is really tricky to check valid SO3 or SE3 matrix (orthogonal or not). Could you give a function to set up the tol value or increase the tol to 1e9?

add coloured to setup.py
make code robust to lack of colored package

python3

Python 3.8.5 (default, Jan 27 2021, 15:41:15)
[GCC 9.3.0] on linux
Type "help", "copyright", "credits" or "license" for more information.

from spatialmath import
T = SE3(0.5, 0.0, 0.0) * SE3.RPY([0.1, 0.2, 0.3], order='xyz') * SE3.Rx(-90, unit='deg')
T.plot(color='red', label='2')
Traceback (most recent call last):
File "/usr/lib/python3/dist-packages/matplotlib/projections/init.py", line 58, in get_projection_class
return projection_registry.get_projection_class(projection)
File "/usr/lib/python3/dist-packages/matplotlib/projections/init.py", line 25, in get_projection_class
return self._all_projection_types[name]
KeyError: '3d'

During handling of the above exception, another exception occurred:

Traceback (most recent call last):
File "", line 1, in
File "/home/knd/work/Robots/spatialmath-python/spatialmath/baseposematrix.py", line 830, in plot
base.trplot(self.A, *args, **kwargs)
File "/home/knd/work/Robots/spatialmath-python/spatialmath/base/transforms3d.py", line 2032, in trplot
ax = fig.add_subplot(111, projection='3d', proj_type=projection)
File "/usr/lib/python3/dist-packages/matplotlib/figure.py", line 1396, in add_subplot
self._process_projection_requirements(*args, **kwargs)
File "/usr/lib/python3/dist-packages/matplotlib/figure.py", line 1120, in _process_projection_requirements
projection_class = projections.get_projection_class(projection)
File "/usr/lib/python3/dist-packages/matplotlib/projections/init.py", line 60, in get_projection_class
raise ValueError("Unknown projection %r" % projection)
ValueError: Unknown projection '3d'

pip3 list

Package Version Location

alabaster 0.7.8
ansitable 0.9.5
apturl 0.5.2
Babel 2.6.0
backcall 0.1.0
bcrypt 3.1.7
beautifulsoup4 4.8.2
blinker 1.4
breezy 3.0.2
Brlapi 0.7.0
catfish 1.4.13
catkin-pkg 0.4.16
certifi 2019.11.28
chardet 3.0.4
chrome-gnome-shell 0.0.0
Click 7.0
colorama 0.4.3
colored 1.4.2
command-not-found 0.3
configobj 5.0.6
cryptography 2.8
css-parser 1.0.4
cssselect 1.1.0
cssutils 1.0.2
cupshelpers 1.0
cx-Freeze 6.4.2
cycler 0.10.0
dblatex 0.3.11py3
dbus-python 1.2.16
decorator 4.4.2
defer 1.0.6
defusedxml 0.6.0
Deprecated 1.2.7
distro 1.4.0
distro-info 0.23ubuntu1
dnspython 1.16.0
docutils 0.16
dulwich 0.19.15
duplicity 0.8.12.0
dxf2gcode 20191025
empy 3.3.2
entrypoints 0.3
fasteners 0.14.1
fastimport 0.9.8
feedparser 5.2.1
future 0.18.2
galternatives 1.0.6
gencpp 0.6.2
genlisp 0.4.17
genmsg 0.5.14
genpy 0.6.9
gpg 1.13.1-unknown
greenlet 0.4.15
guacamole 0.9.2
Guake 3.6.3
html5lib 1.0.1
httplib2 0.14.0
idna 2.8
imagesize 1.2.0
importlib-metadata 3.3.0
ipython 7.13.0
ipython-genutils 0.2.0
jedi 0.15.2
Jinja2 2.10.1
kazam 1.4.5
keyring 18.0.1
kiwisolver 1.0.1
language-selector 0.1
launchpadlib 1.10.13
lazr.restfulclient 0.14.2
lazr.uri 1.0.3
libvirt-python 6.1.0
lightdm-gtk-greeter-settings 1.2.2
lockfile 0.12.2
louis 3.12.0
lxml 4.5.0
macaroonbakery 1.3.1
Mako 1.1.0
Markdown 3.1.1
MarkupSafe 1.1.0
matplotlib 3.1.2
meld 3.20.2
menulibre 2.2.1
meteo-qt 1.5
mock 3.0.5
monotonic 1.5
msgpack 0.6.2
mugshot 0.4.2
netifaces 0.10.4
nose 1.3.7
numpy 1.17.4
oauthlib 3.1.0
olefile 0.46
onboard 1.4.1
packaging 20.3
padme 1.1.1
paramiko 2.6.0
parso 0.5.2
pbr 5.4.5
pexpect 4.6.0
pickleshare 0.7.5
Pillow 7.0.0
pip 20.0.2
prompt-toolkit 2.0.10
protobuf 3.6.1
psutil 5.5.1
pyasn1 0.4.2
pycairo 1.16.2
pycrypto 2.6.1
pycups 1.9.73
pycurl 7.43.0.2
PyGithub 1.43.7
Pygments 2.3.1
PyGObject 3.36.0
PyJWT 1.7.1
pymacaroons 0.13.0
PyNaCl 1.3.0
pynvim 0.4.1
PyOpenGL 3.1.0
pyparsing 2.4.6
PyQt5 5.14.1
pyRFC3339 1.1
PySide2 5.15.1
python-apt 2.0.0+ubuntu0.20.4.3
python-dateutil 2.7.3
python-debian 0.1.36ubuntu1
python-gitlab 2.0.1
python-gnupg 0.4.5
python-qt-binding 0.3.6
python-xlib 0.23
pytz 2019.3
pyxattr 0.6.1
pyxdg 0.26
PyYAML 5.3.1
QtPy 1.9.0
regex 2019.8.19
reportlab 3.5.34
requests 2.22.0
requests-unixsocket 0.2.0
roman 2.0.0
rosdep 0.18.0
rosdistro 0.8.0
rosinstall 0.7.8
rosinstall-generator 0.1.18
rospkg 1.2.3
scipy 1.6.2
SecretStorage 2.3.1
setuptools 45.2.0
sgt-launcher 0.2.5
shiboken2 5.15.1
simplejson 3.16.0
sip 4.19.21
six 1.14.0
soupsieve 1.9.5
spatialmath-python 0.9.5 /home/knd/work/Robots/spatialmath-python
Sphinx 1.8.5
ssh-import-id 5.10
system-service 0.3
systemd-python 234
traitlets 4.3.3
ubuntu-advantage-tools 20.3
ubuntu-drivers-common 0.0.0
ufw 0.36
unattended-upgrades 0.1
unity-scope-calculator 0.1
unity-scope-devhelp 0.1
unity-scope-manpages 0.1
unity-scope-tomboy 0.1
unity-scope-virtualbox 0.1
unity-scope-yelp 0.1
unity-scope-zotero 0.1
urllib3 1.25.8
urwid 2.0.1
usb-creator 0.3.7
vboxapi 1.0
vcstools 0.1.42
wadllib 1.3.3
wcwidth 0.1.8
webencodings 0.5.1
wheel 0.34.2
wrapt 1.11.2
wstool 0.1.18
wxPython 4.0.7
xcffib 0.8.1
xdiagnose 3.8.9
xkit 0.0.0
XlsxWriter 1.1.2
Yapps2 2.2.1
youtube-dl 2020.3.24
zipp 3.4.0

It seems that SE2.norm tries to call some trnorm2 function which is not defined. Here's how to reproduce, after cloning a fresh copy of this repo and pip install . in a virtualenv:

Python 3.10.2 (main, Jan 15 2022, 19:56:27) [GCC 11.1.0] on linux
Type "help", "copyright", "credits" or "license" for more information.
>>> import spatialmath as sm
>>> sm.SE2().norm()
Traceback (most recent call last):
  File "<stdin>", line 1, in <module>
  File "/home/dtamino/oss/spatialmath-python/spatialmath/baseposematrix.py", line 541, in norm
    return self.__class__([base.trnorm2(x) for x in self.data])
  File "/home/dtamino/oss/spatialmath-python/spatialmath/baseposematrix.py", line 541, in <listcomp>
    return self.__class__([base.trnorm2(x) for x in self.data])
AttributeError: module 'spatialmath.base' has no attribute 'trnorm2'. Did you mean: 'trnorm'?

Hello,
I noticed that matrices with long symbolic equations are printed very shrink.
I found my own priting method and I suggest to align them by the longest term like this:

def print_SE3(SE3_matrix : SE3):
    SE3_numpy = SE3_matrix.A
    max_item_length = SE3_numpy.astype(np.string_).dtype.itemsize
    print(np.array2string(SE3_numpy,max_line_width=np.inf,precision=3,suppress_small=True,formatter = {'all':lambda x: f"{str(x):{max_item_length}}"}))

or:

def print_SE3(SE3_matrix : SE3):
    SE3_numpy = SE3_matrix.A
    max_item_length = SE3_numpy.astype(np.string_).dtype.itemsize
    with np.printoptions(precision=3, suppress=True, linewidth = np.inf, formatter = {'all':lambda x: f"{str(x):{max_item_length}}"}):
        print(SE3_numpy)

Results without print_SE3:

  -1.0*sin(theta2)*cos(theta1) 1.0*sin(theta1) 1.0*cos(theta1)*cos(theta2) (d3 + l2)*cos(theta1)*cos(theta2)  
  -1.0*sin(theta1)*sin(theta2) -1.0*cos(theta1) 1.0*sin(theta1)*cos(theta2) (d3 + l2)*sin(theta1)*cos(theta2)  
  1.0*cos(theta2) 0            1.0*sin(theta2) l1 + (d3 + l2)*sin(theta2)  
  0            0            0            1             

Results with print_SE3:

[[-1.0*sin(theta2)*cos(theta1)      1.0*sin(theta1)                   1.0*cos(theta1)*cos(theta2)       (d3 + l2)*cos(theta1)*cos(theta2)]
 [-1.0*sin(theta1)*sin(theta2)      -1.0*cos(theta1)                  1.0*sin(theta1)*cos(theta2)       (d3 + l2)*sin(theta1)*cos(theta2)]
 [1.0*cos(theta2)                   0                                 1.0*sin(theta2)                   l1 + (d3 + l2)*sin(theta2)       ]
 [0                                 0                                 0                                 1                                ]]

I know that this function could be better so the inner columns should have less separation but I think it's good first step towards the good change.

Hi,
Currently the rmul works like follows

SE3.Rt(R=matrix.R, t=(0,0,0)) * SE3.Trans(matrix.t) * line

However, it should work like below.
SE3.Trans(matrix.t) * SE3.Rt(R=matrix.R, t=(0,0,0)) * line
It should translate the line first and then rotate it.
Or am I missing something?

I used

from spatialmath.base.transforms3d import *
tranimate(transl(4, 3, 4)@trotx(2)@troty(-2), frame='A', arrow=True, dims=[-5, 5], nframes=100)

to show translation and rotation.
But three motions(transl(4, 3, 4),trotx(2),troty(-2)) are performed simultaneously.

Can I show three motions one by one?

Thanks a lot!

I get the following surprising result when using the UnitQuaternion class as the input

R = np.array([ [0, -1, 0], 
               [-1, 0, 0], 
               [0,  0, -1]])

UnitQuaternion(SO3(R)).SO3()

produces a different rotation matrix

SO3(array([[ 0.,  1.,  0.],
           [ 1.,  0.,  0.],
           [ 0.,  0., -1.]]))

and

UnitQuaternion(SO3(R)).vec_xyzs

produces

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

If I try it with scipy, I get:

from scipy.spatial.transform import Rotation
Rotation.from_matrix(R).as_quat()

Output in (xyzs) format:

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

Also, the spatialmath.base.r2q_old method gives the same result as scipy (in sxyz format):

r2q_old(R)
array([ 0. ,  0.70710678, -0.70710678,  0.        ])

which indicates that spatialmath.base.r2q might be erroneous.

Is it possible that spatialmath.base.r2q be the culprit?

Hello, running the below example from the readme:

T = SE3(1, 2, 3) * SE3.Rx(30, 'deg')
T.plot()
plt.show()

gives me the plot with the global axes marked as "XXX" instead of "XYZ".

I work on Windows 11, with Python 3.8 and I was using the latest snapshot installed with:
pip3 install roboticstoolbox-python[vpython] sympy

PS. for tranimate function it works fine

Line3 currently accepts an arbitrary combination of moment and direction vectors, which could directly result in an invalid Plücker coordinates representation. Nevertheless, a valid Plücker coordinate representation is guaranteed in the docs, which is wrong. The following snippet illustrates this neatly:

v = np.array([2, 2, 3])
w = np.array([-3, 2, 1])
tw = Twist3( v, w)
recv_line = tw.line()
assert abs(np.dot(recv_line.v, recv_line.w)) < 1e-4

I believe the root cause of this issue is the mismatch between docs and implementation. The associated docs suggest that the input is a valid Plücker representation, whereas the implementation does not check for the validity of the input and assumes it is valid. Therefore, the test above fails.