README.md

Plane tilt corrector

Given three non-square points ABP within a plane, of which A and B can move vertically while P is fixed, the conversion from unaligned tilting to plane orthogonal rotation to an arbitrary XYZ coordinate frame is not trivial and raises the following challenges:

• Requested a pure rotation around the X or Y axes, yield the actuation needed in terms of A and B vertical position.
• Requested a vertical movement in A or B, adjust its partner height so that the plane rotation is square with a fixed coordinate frame.

This script simulates and solves the two above scenarios.

plane_tilt_corrector_plotter.py

import numpy as np
import matplotlib.pyplot as plt
from matplotlib.widgets import Slider, Button
from matplotlib.patches import Ellipse
from mpl_toolkits.mplot3d import Axes3D, art3d


class PlanePlot:
    
    def __init__(self, a, b, p):
        # Moving points a and b (in Z only), p is fixed
        self.a = a
        self.b = b
        self.p = p

        # XY domain to draw plane
        x = np.linspace(-1,1,10)
        y = np.linspace(-1,1,10)
        self.X, self.Y = np.meshgrid(x,y)

        # Create the figure, spawn widgets and plot
        self.fig, __ = plt.subplots()
        self.create_main_plot()
        self.create_sliders()
        self.create_dir_buttons_hadjust()
        self.create_dir_buttons_rotmat()
        self.create_reset_button()

        self.update_plot()


    def create_main_plot(self):
        self.ax = self.fig.add_subplot(projection='3d')
        # adjust the main plot to make room for the sliders
        self.fig.subplots_adjust(left=0.20, bottom=0.32)


    def create_sliders(self):
        
        def on_slider_move(slider, variable, adjust=True):
            def dummy_callback(__):

                if variable is self.a:
                    step = slider.val - self.a[2]
                    self.a[2] += step 
                    self.b[2] += step * (self.b[0] - self.p[0])/(self.a[0]- self.p[0]) if adjust else 0
                elif variable is self.b:
                    step = slider.val - self.b[2]
                    self.a[2] += step * (self.a[1] - self.p[1])/(self.b[1]- self.p[1]) if adjust else 0
                    self.b[2] += step
                
                if step != 0:
                    self.update_all_sliders()              
                    self.update_plot()

            return dummy_callback

        self.ax_slider_a = self.fig.add_axes([0.25, 0.22, 0.35, 0.03])
        self.slider_a = Slider(ax=self.ax_slider_a, label='Height of A',
            valmin=-5, valmax=5, valinit=self.a[2], valstep=0.1
        )

        self.ax_slider_b = self.fig.add_axes([0.25, 0.17, 0.35, 0.03])
        self.slider_b = Slider(ax=self.ax_slider_b, label="Height of B",
            valmin=-5, valmax=5, valinit=self.b[2], valstep=0.1,
            orientation="horizontal"
        )

        self.ax_slider_a_adj = self.fig.add_axes([0.25, 0.12, 0.35, 0.03])
        self.slider_a_adj = Slider(ax=self.ax_slider_a_adj, label='Height of A adjusted',
            valmin=-5, valmax=5, valinit=self.a[2], valstep=0.1
        )

        self.ax_slider_b_adj = self.fig.add_axes([0.25, 0.07, 0.35, 0.03])
        self.slider_b_adj = Slider(ax=self.ax_slider_b_adj, label="Height of B adjusted",
            valmin=-5, valmax=5, valinit=self.b[2], valstep=0.1,
        )

        self.slider_a.on_changed(on_slider_move(self.slider_a, self.a, adjust=False))
        self.slider_b.on_changed(on_slider_move(self.slider_b, self.b, adjust=False))
        self.slider_a_adj.on_changed(on_slider_move(self.slider_a_adj, self.a, adjust=True))
        self.slider_b_adj.on_changed(on_slider_move(self.slider_b_adj, self.b, adjust=True))



    def create_dir_buttons_rotmat(self):
        
        # The rotation matrix used to convert orthogonal commands to 
        self.rotmat = np.array([
            [self.a[0] - self.p[0], self.a[1] - self.p[1]],
            [self.b[0] - self.p[0], self.b[1] - self.p[1]]
        ])

        def move_angle(x, y):
            def dummy_callback(__):
                self.a[2], self.b[2] = (self.a[2], self.b[2]) + self.rotmat @ np.array([x, y])
                self.update_all_sliders()
                self.update_plot()
                pass
            return dummy_callback

        self.ax__move_x_r = self.fig.add_axes([0.75, 0.185, 0.1, 0.04])
        self.btn_move_x_r = Button(self.ax__move_x_r, 'X+', hovercolor='0.975')
        self.btn_move_x_r.on_clicked(move_angle(+1,0))

        self.ax__move_x_l = self.fig.add_axes([0.75, 0.105, 0.1, 0.04])
        self.btn_move_x_l = Button(self.ax__move_x_l, 'X-', hovercolor='0.975')
        self.btn_move_x_l.on_clicked(move_angle(-1,0))

        self.ax__move_y_r = self.fig.add_axes([0.65, 0.145, 0.1, 0.04])
        self.btn_move_y_r = Button(self.ax__move_y_r, 'Y+', hovercolor='0.975')
        self.btn_move_y_r.on_clicked(move_angle(0,+1))

        self.ax__move_y_l = self.fig.add_axes([0.85, 0.145, 0.1, 0.04])
        self.btn_move_y_l = Button(self.ax__move_y_l, 'Y-', hovercolor='0.975')
        self.btn_move_y_l.on_clicked(move_angle(0,-1))


        self.ax__move_xy_rr = self.fig.add_axes([0.65, 0.185, 0.1, 0.04])
        self.btn_move_xy_rr = Button(self.ax__move_xy_rr, 'X+Y+', hovercolor='0.975')
        self.btn_move_xy_rr.on_clicked(move_angle(+1,+1))

        self.ax__move_xy_rl = self.fig.add_axes([0.85, 0.185, 0.1, 0.04])
        self.btn_move_xy_rl = Button(self.ax__move_xy_rl, 'X+Y-', hovercolor='0.975')
        self.btn_move_xy_rl.on_clicked(move_angle(+1,-1))

        self.ax__move_xy_lr = self.fig.add_axes([0.65, 0.105, 0.1, 0.04])
        self.btn_move_xy_lr = Button(self.ax__move_xy_lr, 'X-Y+', hovercolor='0.975')
        self.btn_move_xy_lr.on_clicked(move_angle(-1,+1))

        self.ax__move_xy_ll = self.fig.add_axes([0.85, 0.105, 0.1, 0.04])
        self.btn_move_xy_ll = Button(self.ax__move_xy_ll, 'X-Y-', hovercolor='0.975')
        self.btn_move_xy_ll.on_clicked(move_angle(-1,-1))


    def create_dir_buttons_hadjust(self):

        def move_height(ha, hb):
            def dummy_callback(__):
                self.a[2] += ha
                self.b[2] += hb
                self.update_all_sliders()
                self.update_plot()
                pass
            return dummy_callback

        self.ax__move_a_up = self.fig.add_axes([0.2, 0.01, 0.1, 0.04])
        self.btn_move_a_up = Button(self.ax__move_a_up, 'A+', hovercolor='0.975')
        self.btn_move_a_up.on_clicked(move_height(1, 1 * (self.b[0] - self.p[0])/(self.a[0]- self.p[0])))

        self.ax__move_a_down = self.fig.add_axes([0.3, 0.01, 0.1, 0.04])
        self.btn_move_a_down = Button(self.ax__move_a_down, 'A-', hovercolor='0.975')
        self.btn_move_a_down.on_clicked(move_height(-1, -1 * (self.b[0] - self.p[0])/(self.a[0]- self.p[0])))

        self.ax__move_b_up = self.fig.add_axes([0.4, 0.01, 0.1, 0.04])
        self.btn_move_b_up = Button(self.ax__move_b_up, 'B+', hovercolor='0.975')
        self.btn_move_b_up.on_clicked(move_height(1 * (self.a[1] - self.p[1])/(self.b[1]- self.p[1]), 1))

        self.ax__move_b_down = self.fig.add_axes([0.5, 0.01, 0.1, 0.04])
        self.btn_move_b_down = Button(self.ax__move_b_down, 'B-', hovercolor='0.975')
        self.btn_move_b_down.on_clicked(move_height(-1 * (self.a[1] - self.p[1])/(self.b[1]- self.p[1]), -1))


    def create_reset_button(self):
        
        def btn_reset(__):
            self.a[2] = 0.0
            self.b[2] = 0.0
            self.update_all_sliders()
            self.update_plot()

        self.ax_btn_reset = self.fig.add_axes([0.8, 0.01, 0.1, 0.04])
        self.button = Button(self.ax_btn_reset, 'Reset', hovercolor='0.975')
        self.button.on_clicked(btn_reset)       

    def update_all_sliders(self):
        self.slider_a.set_val(self.a[2])
        self.slider_b.set_val(self.b[2])
        self.slider_a_adj.set_val(self.a[2])
        self.slider_b_adj.set_val(self.b[2])

    def draw_point(self, x, y, z, name):
        # Ideally we'd use scatter but surface hides the points
        elli = Ellipse((x, y), 0.1, 0.1, color="red")
        self.ax.add_patch(elli)
        art3d.pathpatch_2d_to_3d(elli, z=z, zdir="z")

        # Vertical lines and point names
        self.ax.plot([x,x], [y,y], zs=[0,z], color="red")
        self.ax.text(x, y, z, name, size=10, zorder=1, color='k') 


    def update_plot(self):
        A, B, C, t = self.plane_equation_from_pts(self.a, self.b, self.p)

        self.ax.clear()
        self.ax.set_zlim(-5, 5)
        self.ax.set_xlabel("X")
        self.ax.set_ylabel("Y")
        self.ax.set_zlabel("Z")

        # Main surface
        self.ax.plot_surface(self.X, self.Y, self.plane_equation_plot(A, B, C, t), lw=2)

        # Scatters appear hidden behind the surface so we plot mini surface
        self.draw_point(*self.a, "a")
        self.draw_point(*self.b, "b")
        self.draw_point(*self.p, "p")
        
        # Verify orthogonality through the normal vector in title
        self.ax.set_title(f"Normal vector is <{[round(ii,2) for ii in [A,B,C]]}>")
        
        self.fig.canvas.draw_idle()


    def plane_equation_plot(self, A, B, C, t):
        Z = (t - A * self.X - B * self.Y) / C
        return Z

    @staticmethod
    def plane_equation_from_pts(x, y, z):
        """ Yields plane equation coefficients given three points.
        Plane equation: A*x + B*y + C*z + t = 0. 
        """
        # Cross product to get normal vector to plane
        A, B, C = np.cross(x - z, y - z)

        # get t given any of the points
        t = +1 * np.multiply(z, (A,B,C)).sum() 

        return A, B, C, t


if __name__ == "__main__":
    # Simple case
    a = np.array([  1, 0, 0], dtype='float')
    b = np.array([0.2, 1, 0], dtype='float')
    p = np.array([  0, 0, 0], dtype='float')

    # A more intrincated piece
    # a = np.array([.1, .2, .3], dtype='float')
    # b = np.array([.6, .5, .4], dtype='float')
    # p = np.array([.7, .8, .9], dtype='float')


    pplot = PlanePlot(a, b, p)
    plt.show()