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()