def test_png_export():
"""Test if the graph can be exported to PNG."""
random_state = np.random.RandomState(0)
ee2robot = transform_from_pq(
np.hstack((np.array([0.4, -0.3, 0.5]),
random_quaternion(random_state))))
cam2robot = transform_from_pq(
np.hstack((np.array([0.0, 0.0, 0.8]), q_id)))
object2cam = transform_from(
matrix_from_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)
_, filename = tempfile.mkstemp(".png")
try:
tm.write_png(filename)
assert_true(os.path.exists(filename))
except ImportError:
raise SkipTest("pydot is required for this test")
finally:
if os.path.exists(filename):
try:
os.remove(filename)
except WindowsError:
pass # workaround for permission problem on Windows
def test_world2image():
cam2world = transform_from(matrix_from_euler_xyz([np.pi, 0, 0]),
[0, 0, 1.5])
focal_length = 0.0036
sensor_size = (0.00367, 0.00274)
image_size = (640, 480)
world_grid = make_world_grid()
image_grid = world2image(world_grid, cam2world, sensor_size, image_size,
focal_length)
expected_grid = make_world_grid(xlim=(110.73569482, 529.26430518),
ylim=(450.2189781, 29.7810219))
assert_array_almost_equal(image_grid, expected_grid[:, :2])
automatically.
"""
print(__doc__)
import numpy as np
import matplotlib.pyplot as plt
from pytransform3d.rotations import (random_quaternion, matrix_from_euler_xyz,
q_id)
from pytransform3d.transformations import transform_from_pq, transform_from
from pytransform3d.transform_manager import TransformManager
random_state = np.random.RandomState(0)
ee2robot = transform_from_pq(
np.hstack((np.array([0.4, -0.3, 0.5]), random_quaternion(random_state))))
cam2robot = transform_from_pq(np.hstack((np.array([0.0, 0.0, 0.8]), q_id)))
object2cam = transform_from(matrix_from_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()
We can see the camera frame and the world frame. There is a grid of points from
which we know the world coordinates. If we know the location and orientation of
the camera in the world, we can easily compute the location of the points on
the image.
"""
print(__doc__)
import numpy as np
import matplotlib.pyplot as plt
from pytransform3d.rotations import matrix_from_euler_xyz
from pytransform3d.transformations import transform_from, plot_transform
from pytransform3d.camera import make_world_grid, world2image
cam2world = transform_from(matrix_from_euler_xyz([np.pi - 1, 0.2, 0.2]),
[0.2, -1, 0.5])
focal_length = 0.0036
sensor_size = (0.00367, 0.00274)
image_size = (640, 480)
world_grid = make_world_grid()
image_grid = world2image(world_grid, cam2world, sensor_size, image_size,
focal_length)
plt.figure(figsize=(12, 5))
try:
ax = plt.subplot(121, projection="3d", aspect="equal")
except NotImplementedError:
# HACK: workaround for bug in new matplotlib versions (ca. 3.02):
# "It is not currently possible to manually set the aspect"
def merge_fixed_links(parent):
"""Merge fixed links"""
# if child doesn't have 'joint|collision' attributes, merge it to parent
# * add child pose to grandchild
# * add child position to grandchild
# * add child includes to includes
# * add child mesh to parent
# * add child weight to parent
# * remove child
merged = []
for child in parent["includes"]: # this is actually a recursion
if not "joint" in child and not 'collision' in child:
for grandchild in child["includes"]:
# calculate grandchild_to_parent transform (so we can remove 'child' link )
T = lambda pose: transform_from(
matrix_from_euler_xyz(pose[3:]).T, pose[:3])
child_to_parent = T(np.fromstring(child['pose'], sep=' '))
grandchild_to_child = T(
np.fromstring(grandchild['pose'], sep=' '))
grandchild_to_parent = concat(grandchild_to_child,
child_to_parent)
pose = np.concatenate([
grandchild_to_parent[:3, 3],
euler_xyz_from_matrix(grandchild_to_parent[:3, :3].T)
])
grandchild['pose'] = str(pose)[1:-1]
# save position (left/right) modifier to grandchild
if 'position' in child:
grandchild['position'] = child['position']
# recourse into descendents
parent['includes'].append(grandchild)
merged.append((child['model'], child_to_parent))
if merged:
meshes = [
stl.mesh.Mesh.from_file('meshes/%s.stl' % parent['model']).data
]
for part, child_to_parent in merged:
mesh = stl.mesh.Mesh.from_file('meshes/%s.stl' % part)
mesh.transform(child_to_parent) # ?? inverse?
meshes.append(mesh.data)
combined = stl.mesh.Mesh(np.concatenate(meshes))
name = parent['name'] + '_' + parent.get('position', '')
combined.save('meshes/%s.stl' % name, mode=stl.Mode.BINARY)
parts[name] = {
"mass": "0.008",
"count": 1,
"material": parts[parent['model']]['material']
}
# TODO: fix mass, intertia, etc
#parent['model'] = name
# remove merged children
parent["includes"] = [
child for child in parent["includes"]
if "joint" in child or 'collision' in child
]
# recourse into joints
for child in parent["includes"]:
merge_fixed_links(child)
"""
=====================
Transformation Editor
=====================
The transformation editor can be used to manipulate transformations.
"""
from pytransform3d.transform_manager import TransformManager
from pytransform3d.editor import TransformEditor
from pytransform3d.transformations import transform_from
from pytransform3d.rotations import matrix_from_euler_xyz
tm = TransformManager()
tm.add_transform(
"tree", "world",
transform_from(matrix_from_euler_xyz([0, 0.5, 0]), [0, 0, 0.5]))
tm.add_transform(
"car", "world",
transform_from(matrix_from_euler_xyz([0.5, 0, 0]), [0.5, 0, 0]))
te = TransformEditor(tm, "world", s=0.3)
te.show()
print("tree to world:")
print(te.transform_manager.get_transform("tree", "world"))
