dfki-ric / pytransform3d Goto Github PK

Homogeneous matrices are used to represent transformations, but this is documented nowhere?

Tutorial

Not sure if it's already implemented in some not obvious way, but... I would like to see the library enhanced in a way that plotting the frames/links can be more easily animated. Currently, if I'm not mistaken, it's just plotting, is that right?
In an animation setup like for example FuncAnimation you'd use set_data to refresh the plot without having to plot everything again (which is very time consuming). Or does pytransform already offer that too? Might be that I've missed something...

...seems to be possible: https://w.wol.ph/2015/07/10/rendering-stl-files-matplotlib-numpy-stl/

Ran into a weird issue of dot product of checked/normalized quaternions becoming >1. Didn't have time to look deeper into it, maybe after the weekend.

import pytransform3d.rotations as pr

a = ((0.9689124217106448 , 0.24740395925452294 , 0.0 , 0.0))
b = ((0.9689124217106448 , 0.247403959254523 , 0.0 , 0.0))
a = pr.check_quaternion(a)
b = pr.check_quaternion(b)

print("dot:" , np.dot(a,b))
print("divisor:", np.linalg.norm(a)*np.linalg.norm(a))
print("angle between quaternions:", pr.angle_between_vectors(a,b))

dot: 1.0000000000000002
divisor: 1.0
angle between quaternions: nan

Thank you for the great library!

Is it possible to animate the URDF?
If yes, could you provide an example? Much appreciated!

There are two interpretations of source and target frame of a transformation:

• Data transformation (also: source to target transformation): Data is located in the source frame and will be transformed to a target frame. Data is not moved but it is interpreted in the target frame instead of the source frame after applying the transformation.
• Frame transformation (also: source in target transformation): The source frame will be transformed to the target frame. This can also be interpreted as the transformation of the target frame in the source frame.

Example from release 0.12:

box = o3d.geometry.TriangleMesh.create_box(2, 2, 1)
render = rendering.OffscreenRenderer(640, 480)
render.scene.add_geometry("box", box, grey)
render.scene.camera.look_at([0, 0, 0], [0, 10, 0], [0, 0, 1])
img = render.render_to_image()

• documentation: http://www.open3d.org/docs/release/python_api/open3d.visualization.rendering.OffscreenRenderer.html#open3d.visualization.rendering.OffscreenRenderer
• example: https://github.com/intel-isl/Open3D/blob/1e211b029c1a7dc1e43f00f1e6dc0b49f2c2c2ba/examples/python/gui/render-to-image.py

... as an alternative representation for transformations.

More information: https://vision.in.tum.de/_media/spezial/bib/rodola-cvpr11-2.pdf

...

see https://github.com/rock-learning/pytransform3d/blob/master/pytransform3d/trajectories.py#L70

See discussion in #40

Terminology:

• rotation about z: yaw, heading, pan, azimuth
• rotation about y: pitch, elevation, tilt
• rotation about x: roll, bank

Ship translations: surge (x), sway (y), heave (z)

Options:

• VTK - not suitable for live visualizations
• Open3D - does it support drawing lines etc.?
• pyrender - doesn't seem to be actively maintained
• pyglet - ?

Eigen offers this function.

If a collision object and a visual have the same name in a URDF this is currently mixed in the UrdfTransformManager.

The documentation says that pytransform3d uses the source-in-target convention for transformations:

Source in target: The first option would be to assume that the source is the base frame in which we represent the target frame and the transformation gives us the translation and rotation to get the location of the target in the source. In the illustration below that would mean that the object (target) is defined in the camera frame (source) and the camera (target) is defined in the body frame (source).

However, the docstring does not refer to this.

 |  get_transform(self, from_frame, to_frame)
 |      Request a transform.
 |      
 |      Parameters
 |      ----------
 |      from_frame : string
 |          Name of the frame for which the transform is requested in the
 |          to_frame coordinate system
 |      
 |      to_frame : string
 |          Name of the frame in which the transform is defined
 |      
 |      Returns
 |      -------
 |      A2B : array-like, shape (4, 4)
 |          Homogeneous matrix that represents the transform from 'from_frame'
 |          to 'to_frame'

Suggested change:

• Rename parameters
  • from_frame should be called target_frame
  • to_frame should be called source_frame
• Reference to convention

The following features could be added:

• check for existing node: has_frame(name)
• check for unconnected graphs: check_connected()

Suggested improvements:

• people tracking, sensor fusion
• behavior learning for robotic manipulation

When using the plot_mesh function there is a parameter called "convex_hull" that can be set to True/False (which appears to be False by default):
ax = plot_mesh(filename=os.path.join(data_dir, "my_mesh.stl"), convex_hull=False)

When using the transformation manager to load an urdf I get the convex hull by default:
with open(BASE_DIR + "urdf/my_urdf.urdf", "r") as f: tm.load_urdf(f.read(), mesh_path=BASE_DIR+"meshes/")
Is there a way to set the convex_hull parameter when loading the mesh through a urdf-file?

Currently we have to transform rotation matrices to quaternions or axis-angle first.

Also SLERP for poses would be convenient.

Alternativ: convert sequence of rotation matrices to quaternions / axis-angle

https://joss.theoj.org/

New publication: https://joss.theoj.org/papers/new

There is already a package called pytransform: https://pypi.org/project/pytransform/, so we should rename this package. Possible names:

• pytransform3d
• transform3d

(https://packaging.python.org/tutorials/packaging-projects/)

The y=arccos(x) function results in numerical imprecision when y should be close to 0 or pi, i.e. when x is close to 1 or x is close to -1.

To circumvent this you could for example write the following:

# determine angle
angle = np.arccos((np.trace(R) - 1.0) / 2.0)
    
if angle < 0.001: # get numerical precision at low angular values
   angle = np.arcsin(np.linalg.norm(R-np.eye(3))/np.sqrt(2))
   if angle == 0.0:
        return np.array([1.0, 0.0, 0.0, 0.0])

a = np.empty(4)
D = R-R.transpose()

if angle > np.pi-0.001:# get numerical precision at high angular values
    angle = np.pi - np.arcsin(np.linalg.norm(D)/np.sqrt(8))

# determin rotation axis
r = np.array([D[2, 1], D[0, 2], D[1, 0]])
# The norm of r is 2.0 * np.sin(angle)
a[:3] = r / (2.0 * np.sin(angle))
a[3] = angle
return a

I am a bit confused about the output of the transform_from function. Consider the following snippet

import numpy as np
import matplotlib.pyplot as plt
import pytransform3d.transformations as pt
from pytransform3d.transform_manager import TransformManager

A = np.array([0.0, 0.0, 0.0, 1.0])
A2B = pt.transform_from(np.identity(3), np.array([0.0, 0.0, 1.0]))


tm = TransformManager()
tm.add_transform("A", "B", A2B)

ax = tm.plot_frames_in("A", s=0.1)
ax.set_xlim((-1, 1))
ax.set_ylim((-1, 1))
ax.set_zlim((0.0, 2.0))
plt.show()

Since I specify that frame B has translation (x,y,z) = (0,0,1) compared to frame A, I expect frame B to be above frame A. But when I plot the transformation using the transformation manager, I see that B is in fact below A. Is there something I am not understanding?

Hi, I might be mistaken here, but it seems to me that the active vs passive matrices are flipped in the code and ambiguity documentation. Looking at the references here and here both suggest that a passive rotation matrix around the z axis should have a -sin() on the top right.

Am I mistaken?

• Euler angles
  • extrinsic / intrinsic zxz
  • extrinsic / intrinsic zyz
  • extrinsic / intrinsic yzy
  • extrinsic / intrinsic yxy
  • extrinsic / intrinsic xyx
  • extrinsic / intrinsic xzx
• Tait-Bryan angles
  • intrinsic zyx / extrinsic xyz
  • intrinsic zyx / extrinsic xyz
  • intrinsic yzx / extrinsic xzy
  • intrinsic yxz / extrinsic zxy
  • intrinsic xyz / extrinsic zyx
  • intrinsic xzy / extrinsic yzx
• Roll-pitch-yaw
  • implement convention

The example:
https://rock-learning.github.io/bolero/auto_examples/representation/plot_cartesian_dmp.html#sphx-glr-auto-examples-representation-plot-cartesian-dmp-py

In function:
ax = plt.subplot(221, projection="3d", aspect="equal")
NotImplementedError: It is not currently possible to manually set the aspect on 3D axes

• http://marctenbosch.com/quaternions/
• https://marctenbosch.com/ndphysics/NDrigidbody.pdf

... to fix this: https://pypi.org/project/pytransform3d/1.3/

Hi. I am trying to use pytransform3d.
Do you think it might be helpful to have an option to draw the coordinates of the origin on the 3d grid?
Something like this.

In most other 3D pipelines, coordinate transforms are represented as arrows instead of lines. It would be nice to have this here too, because it shows much more clearly what is going on.

It would also be great to have a functionality to return a color legend for the axis plots so that one does not have to guess which axis is which.

I could quickly prototype this, if desired.

Hi,

in my opinion the documentation of transform_from_pq is a bit misleading. From the paramater list (x, y, z, w, vx, vy, vz) it's not particularly clear which x, y, z belongs to the quaternion and which is the cartesian point. Since w doesn't have a prefix it might very well belong to the first three parameters and the second set vx, vy, vz might equally well be understood as vector x, vector y, vector z.

This should be pointed out clearer in the documentation in my opinion. I haven't looked yet if this also affects other functions.

Wikipedia

Should rotations or transformations transform points to new locations in the coordinate frame or should they transform coordinate systems?

@HWiese1980 maybe you have an idea.

https://github.com/rock-learning/pytransform3d/blob/153149531b6d7eec92fe688f3d9f9d955a20713a/pytransform3d/transform_manager.py#L69

Is there a reason for using a list and checking existence of a frame before appending, instead of using a set here that inherently only allows for unique items? Question is what's more efficient.

Hi @AlexanderFabisch, do you think it's possible to make pytransform3d compatible with pytorch? That is, make it aware of gradients and such?

I would like to use it in a nn.Module class, but it seems like it's losing the gradients on the way. Python complains about that numpy() cannot be called on tensors that have requires_grad=True. It's required to detach them before. That seems to happen somewhere in the guts of pytransform3d. But as far as I've understood, the tensor must not be detached because it'd get useless for the backward pass.

In my particular case it's a call to transform_from_pq, more precisely, np.asarray in check_pq that results in tensor.numpy() being called.

Only PyQt 4 is supported at the moment

When running the example included in the README, the following error occurs. This was tested in a new virtualenv with the following versions of dependencies:

beautifulsoup4==4.9.1
cycler==0.10.0
kiwisolver==1.2.0
lxml==4.5.2
matplotlib==3.3.0
numpy==1.19.0
pillow==7.2.0
pydot==1.4.1
pyparsing==2.4.7
python-dateutil==2.8.1
pytransform3d==1.2
scipy==1.5.1
six==1.15.0
soupsieve==1.9.6
trimesh==3.7.10

The traceback is:

Traceback (most recent call last):
  File "transformations-test.py", line 24, in <module>
    ax = tm.plot_frames_in("robot", s=0.1)
  File "/home/tch/.cache/pypoetry/virtualenvs/pytransform-test-epSAifj1-py3.6/lib/python3.6/site-packages/pytransform3d/transform_manager.py", line 210, in plot_frames_in
    strict_check=self.strict_check, **kwargs)
  File "/home/tch/.cache/pypoetry/virtualenvs/pytransform-test-epSAifj1-py3.6/lib/python3.6/site-packages/pytransform3d/transformations.py", line 413, in plot_transform
    frame = Frame(A2B, name, s, **kwargs)
  File "/home/tch/.cache/pypoetry/virtualenvs/pytransform-test-epSAifj1-py3.6/lib/python3.6/site-packages/pytransform3d/plot_utils.py", line 48, in __init__
    self.set_data(A2B, label)
  File "/home/tch/.cache/pypoetry/virtualenvs/pytransform-test-epSAifj1-py3.6/lib/python3.6/site-packages/pytransform3d/plot_utils.py", line 67, in set_data
    b.set_3d_properties([p[2], p[2] + self.s * R[2, d]])
  File "/home/tch/.cache/pypoetry/virtualenvs/pytransform-test-epSAifj1-py3.6/lib/python3.6/site-packages/mpl_toolkits/mplot3d/art3d.py", line 143, in set_3d_properties
    zs = np.broadcast_to(zs, xs.shape)
AttributeError: 'list' object has no attribute 'shape'

TODO

• smoke test and check if file has been produced

def _slerp_weights(angle, t):
return (np.sin((1.0 - t) * angle) / np.sin(angle),
np.sin(t * angle) / np.sin(angle))

_slerp_weights divides by 0 when the angle between the rotations to interpolate between is zero, this leads to a nan Quaternion in SLERP. It would be preferable if a SLERP between two identical rotations would always return the same rotation instead of an invalid one.

Hello!

Thank you for this great library!

I wonder if there is some simple way to remove nodes from Transformation Manager. I have a fairly complicated transformation chain. I keep adding new nodes on the fly and the graph gets ridiculously slow. I think that it would help me if I could remove the old nodes. Is it currently possible?

Cheers!

It is only useful if you use it via UrdfTransformManager:

plot_mesh("kuka_lbr/meshes/hand.stl")
/home/afabisch/Projekte/pytransform3d/pytransform3d/plot_utils.py:700: UserWarning: Mesh will be ignored. You have to set a mesh path to plot meshes.
  "Mesh will be ignored. You have to set a mesh path to "

Hi, I would like to use your library for my project but I get a value error in the line below.

I am pretty sure that my matrix is correct as it represents the extrinsics of a camera.
When I increase the a-tol value np.allclose, it works just fine

So my suggestion would be to:

1. change the raise to a warning so that one can keep working without hacking around in the library
2. consider pulling up an a-tol flag to every call of the method. However, this would be a pretty large rafactoring.
3. Alternatively, one could try to find a context manager that switches the precision of np.allclose, however, I was not able to find this.

https://github.com/rock-learning/pytransform3d/blob/f4c5c01132df31c0c81962ee3857e670ff026699/pytransform3d/rotations.py#L251

I am happy to hear back on suggestions. Apart from that, I think the library is pretty nice to work with and goes well with http://matthew-brett.github.io/transforms3d/

enhancement:
A more general form for the camera projection.
implementing sensor2image for sonar sensor (single- and multibeam) too.