def check_trans_matrix_from_file(file_path):
import matplotlib.pyplot as plt
import pytransform3d.transformations as ptrans
from pytransform3d.transform_manager import TransformManager
from mpl_toolkits.mplot3d import Axes3D # noqa: F401 unused import
data = np.load(file_path)
cam_T_marker_mats_R = data['arr_0']
cam_T_marker_mats_t = data['arr_1']
EE_T_base_mats_R = data['arr_2']
EE_T_base_mats_t = data['arr_3']
cam_T_base_R, cam_T_base_t = cv2.calibrateHandEye(EE_T_base_mats_R,
EE_T_base_mats_t,
cam_T_marker_mats_R,
cam_T_marker_mats_t)
# chane (3,1) shape to (3, )
cam_T_base_t = cam_T_base_t.squeeze(1)
print("cam_T_base_R:\n", cam_T_base_R.shape, "\n", cam_T_base_R)
print("cam_T_base_t:\n", cam_T_base_t.shape, "\n", cam_T_base_t)
base_T_cam_R = cam_T_base_R.transpose()
base_T_cam_t = -cam_T_base_R.transpose().dot(cam_T_base_t)
x_errs = []
y_errs = []
z_errs = []
abs_errs = []
trans_pts = []
base_T_EE_mats_t = []
for i, EE_T_base_t in enumerate(EE_T_base_mats_t):
base_T_EE_t = -EE_T_base_mats_R[i].transpose().dot(EE_T_base_t)
base_T_EE_mats_t.append(base_T_EE_t) # For visualization
print("Point in base: ", i, " ",
base_T_EE_t.transpose().shape, " ", base_T_EE_t)
print("Point in camera: ", i, " ", cam_T_marker_mats_t[i].shape, " ",
cam_T_marker_mats_t[i])
trans_pt = np.add(cam_T_base_R.dot(cam_T_marker_mats_t[i]),
cam_T_base_t)
trans_pts.append(trans_pt)
print("Transed point: ", trans_pt.shape, trans_pt)
x_errs.append(abs(trans_pt[0] - base_T_EE_t[0]))
y_errs.append(abs(trans_pt[1] - base_T_EE_t[1]))
z_errs.append(abs(trans_pt[2] - base_T_EE_t[2]))
abs_errs.append(
np.sqrt(
np.square(trans_pt[0] - base_T_EE_t[0]) +
np.square(trans_pt[1] - base_T_EE_t[1]) +
np.square(trans_pt[2] - base_T_EE_t[2])))
print("X-axis error: ", "max: ", max(x_errs), "min: ", min(x_errs),
"mean: ",
sum(x_errs) / len(x_errs))
print("Y-axis error: ", "max: ", max(y_errs), "min: ", min(y_errs),
"mean: ",
sum(y_errs) / len(y_errs))
print("Z-axis error: ", "max: ", max(z_errs), "min: ", min(z_errs),
"mean: ",
sum(z_errs) / len(z_errs))
print("Abs error: ", "max: ", max(abs_errs), "min: ", min(abs_errs),
"mean: ",
sum(abs_errs) / len(abs_errs))
abs_errs = np.sort(abs_errs)
plt.figure(1)
plt.plot(range(len(abs_errs)), abs_errs)
fig_3d = plt.figure(2)
ax = fig_3d.add_subplot(111, projection='3d')
for pt in cam_T_marker_mats_t:
ax.scatter(pt[0], pt[1], pt[2], marker='^')
for pt in trans_pts:
ax.scatter(pt[0], pt[1], pt[2], marker='.')
plt.figure(3)
base_T_cam = ptrans.transform_from(base_T_cam_R, base_T_cam_t, True)
tm = TransformManager()
tm.add_transform("base", "cam", base_T_cam)
axis = tm.plot_frames_in("base", s=0.3)
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))
ax = plt.subplot(121, projection="3d", aspect="equal")
ax.view_init(elev=30, azim=-70)
ax.set_xlim((-1, 1))
ax.set_ylim((-1, 1))
ax.set_zlim((-1, 1))
ax.set_xlabel("X")
===============================
Visualize Transformed Cylinders
===============================
Plots transformed cylinders.
"""
print(__doc__)
import numpy as np
from pytransform3d.transformations import transform_from
from pytransform3d.rotations import random_axis_angle, matrix_from_axis_angle
import pytransform3d.visualizer as pv
fig = pv.figure()
random_state = np.random.RandomState(42)
A2B = transform_from(
R=matrix_from_axis_angle(random_axis_angle(random_state)),
p=random_state.randn(3))
fig.plot_cylinder(length=1.0, radius=0.3)
fig.plot_transform(A2B=np.eye(4))
fig.plot_cylinder(length=1.0, radius=0.3, A2B=A2B)
fig.plot_transform(A2B=A2B)
fig.view_init()
fig.set_zoom(2)
if "__file__" in globals():
fig.show()
else:
fig.save_image("__open3d_rendered_image.jpg")
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.plot_utils import make_3d_axis
from pytransform3d.rotations import active_matrix_from_intrinsic_euler_xyz
from pytransform3d.transformations import transform_from, plot_transform
from pytransform3d.camera import make_world_grid, world2image, plot_camera
cam2world = transform_from(
active_matrix_from_intrinsic_euler_xyz([-np.pi + 1, -0.1, 0.3]),
[0.2, -1, 0.5])
focal_length = 0.0036
sensor_size = (0.00367, 0.00274)
image_size = (640, 480)
intrinsic_camera_matrix = np.array([[focal_length, 0, sensor_size[0] / 2],
[0, focal_length, sensor_size[1] / 2],
[0, 0, 1]])
world_grid = make_world_grid(n_points_per_line=101)
image_grid = world2image(world_grid,
cam2world,
sensor_size,
image_size,
focal_length,
kappa=0.4)
#!/usr/bin/python3
import numpy as np
from pytransform3d.transform_manager import TransformManager
import pytransform3d.transformations as pytr
import pytransform3d.rotations as pyrot
tm = TransformManager()
# place the world with rotation and translationa as seen from origin
tm.add_transform("origin", "world", pytr.transform_from(pyrot.active_matrix_from_extrinsic_euler_xyz([0, 0, 0]), [0.0, 0.0, 0]))
# place the robot with rotation and translation as seen from world
tm.add_transform("world", "robot", pytr.transform_from(pyrot.active_matrix_from_intrinsic_euler_xyz([0, 0, np.pi/2.0]), [-10.0, 0, 0]))
t = tm.get_transform("world", "robot")
print(t)
# transform the nose of the robot to world CS
p = pytr.transform(t, np.array([0, 10, 0, 1.0]))
print(p)
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)
def time_scaling(t, t_max):
"""Linear time scaling."""
return np.asarray(t) / t_max
def straight_line_path(start, goal, s):
"""Compute straight line path in exponential coordinates."""
start = np.asarray(start)
goal = np.asarray(goal)
return (start[np.newaxis] * (1.0 - s)[:, np.newaxis] +
goal[np.newaxis] * s[:, np.newaxis])
s = time_scaling(np.linspace(0.0, 5.0, 50001), 5.0)
start = transform_from(R=active_matrix_from_angle(1, -0.4 * np.pi),
p=np.array([-1, -2.5, 0]))
goal1 = transform_from(R=active_matrix_from_angle(1, -0.1 * np.pi),
p=np.array([-1, 1, 0]))
goal2 = transform_from(R=active_matrix_from_angle(2, -np.pi),
p=np.array([-0.65, -0.75, 0]))
path1 = straight_line_path(exponential_coordinates_from_transform(start),
exponential_coordinates_from_transform(goal1), s)
path2 = straight_line_path(exponential_coordinates_from_transform(goal1),
exponential_coordinates_from_transform(goal2), s)
H = transforms_from_exponential_coordinates(np.vstack((path1, path2)))
ax = make_3d_axis(1.0)
trajectory = Trajectory(H, n_frames=1000, show_direction=False, s=0.3)
trajectory.add_trajectory(ax)
ax.view_init(azim=-95, elev=70)
ax.set_xlim((-2.2, 1.3))
ax.set_ylim((-2.5, 1))
config = rs.config()
config.enable_device("925122110807")
config.enable_stream(rs.stream.pose)
pipeline = rs.pipeline()
pipeline.start(config)
except Exception as e:
print("Error initializing camera")
print(e)
sys.exit(-1)
tm = TransformManager()
tm.add_transform(
"robot", "origin",
pytr.transform_from(
pyrot.active_matrix_from_intrinsic_euler_xyz([0.0, 0.0, 0.0]),
[2000, -30000, 0.0]))
tm.add_transform(
"slam_sensor", "robot",
pytr.transform_from(
pyrot.active_matrix_from_intrinsic_euler_xyz([0.0, 0.0, 0]),
[0.0, 0.0, 0.0]))
while True:
frames = pipeline.wait_for_frames()
f = frames.first_or_default(rs.stream.pose)
data = f.as_pose_frame().get_pose_data()
#angles = quaternion_to_euler_angle([data.rotation.w, data.rotation.x, data.rotation.y, data.rotation.z])
tm.add_transform(
"""
=====================
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"))
random_state = np.random.RandomState(42)
A2B = np.eye(4)
length = 1.0
radius = 0.1
mass = 1.0
dt = 0.2
I = inertia_of_cylinder(mass, length, radius)
tau = np.array([0.05, 0.05, 0.0])
angular_velocity = np.zeros(3)
orientation = np.zeros(3)
ax = None
for p_xy in np.linspace(-2, 2, 21):
A2B = transform_from(R=matrix_from_compact_axis_angle(orientation),
p=np.array([p_xy, p_xy, 0.0]))
ax = plot_cylinder(length=length,
radius=radius,
A2B=A2B,
wireframe=False,
alpha=0.2,
ax_s=2.0,
ax=ax)
plot_transform(ax=ax, A2B=A2B, s=radius, lw=3)
angular_acceleration = np.linalg.inv(I).dot(tau)
angular_velocity += dt * angular_acceleration
orientation += dt * angular_velocity
ax.view_init(elev=30, azim=70)
plt.show()
