Managing Transformations

It is sometimes very difficult to have an overview of all the transformations that are required to calculate another transformation. Suppose you have a robot with a camera that can observe the robot’s end-effector and an object that we want to manipulate. We would like to know the position of the end-effector in the object’s frame so that we can control it. The TransformManager can handle this for you.

"""
======================
Transformation Manager
======================

In this example, we will use the TransformManager to infer a transformation
automatically.
"""
print(__doc__)


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


random_state = np.random.RandomState(0)

ee2robot = pt.transform_from_pq(
    np.hstack((np.array([0.4, -0.3, 0.5]),
               pr.random_quaternion(random_state))))
cam2robot = pt.transform_from_pq(
    np.hstack((np.array([0.0, 0.0, 0.8]), pr.q_id)))
object2cam = pt.transform_from(
    pr.active_matrix_from_intrinsic_euler_xyz(np.array([0.0, 0.0, -0.5])),
    np.array([0.5, 0.1, 0.1]))

tm = TransformManager()
tm.add_transform("end-effector", "robot", ee2robot)
tm.add_transform("camera", "robot", cam2robot)
tm.add_transform("object", "camera", object2cam)

ee2object = tm.get_transform("end-effector", "object")

ax = tm.plot_frames_in("robot", s=0.1)
ax.set_xlim((-0.25, 0.75))
ax.set_ylim((-0.5, 0.5))
ax.set_zlim((0.0, 1.0))
plt.show()

(Source code, png, hires.png, pdf)

_images/plot_transform_manager.png

We can also export the underlying graph structure as a PNG with

tm.write_png(filename)
_images/graph.png

A subclass of TransformManager is UrdfTransformManager which can load robot definitions from URDF files. The same class can be used to display collision objects or visuals from URDF files. The library trimesh will be used to load meshes. Here is a simple example with one visual that is used for two links:

"""
================
URDF with Meshes
================

This example shows how to load a URDF with STL meshes. This example must be
run from within the examples folder or the main folder because it uses a
hard-coded path to the URDF file and the meshes.
"""
import os
import matplotlib.pyplot as plt
from pytransform3d.urdf import UrdfTransformManager


BASE_DIR = "test/test_data/"
data_dir = BASE_DIR
search_path = "."
while (not os.path.exists(data_dir) and
       os.path.dirname(search_path) != "pytransform3d"):
    search_path = os.path.join(search_path, "..")
    data_dir = os.path.join(search_path, BASE_DIR)

tm = UrdfTransformManager()
with open(os.path.join(data_dir, "simple_mechanism.urdf"), "r") as f:
    tm.load_urdf(f.read(), mesh_path=data_dir)
tm.set_joint("joint", -1.1)
ax = tm.plot_frames_in(
    "lower_cone", s=0.1, whitelist=["upper_cone", "lower_cone"],
    show_name=True)
ax = tm.plot_connections_in("lower_cone", ax=ax)
tm.plot_visuals("lower_cone", ax=ax)
ax.set_xlim((-0.1, 0.15))
ax.set_ylim((-0.1, 0.15))
ax.set_zlim((0.0, 0.25))
plt.show()

(Source code, png, hires.png, pdf)

_images/plot_urdf_with_meshes.png