dynamic-motion/debug_plot.py at main · OskarLinde/dynamic-motion · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50

import matplotlib.pyplot as plt
import matplotlib as mpl
import numpy as np
#mpl.use('tkagg')


def plot_spline(spline):
	left = spline.knots[0]
	right = spline.knots[-1]
	o = 0# 0.02 * (right-left)
	xx = np.linspace(left-o, right+o, 1000)
	yy = [spline[x] for x in xx]

	plt.plot(xx,yy)
	#plt.plot([left, right],[spline[left],spline[right]],'o')


def plot_spline_accel(spline):
    ax1 = plt.subplot(3,1,3)
    plot_spline(spline.integrate().integrate())
    plt.title('Position')
    plt.xlabel('time (s)')
    plt.ylabel('mm')

    ax2 = plt.subplot(3,1,2, sharex = ax1)
    plot_spline(spline.integrate())
    plt.setp(ax2.get_xticklabels(), visible=False)
    plt.title('Velocity')
    plt.ylabel('mm/s')

    ax3 = plt.subplot(3,1,1, sharex = ax1)
    plot_spline(spline)
    plt.setp(ax3.get_xticklabels(), visible=False)
    plt.title('Acceleration')
    plt.ylabel('mm/s$^2$')
    plt.show()

def plot_discretized(curves):
    ax1 = plt.subplot(len(curves)-1,1,len(curves)-1)
    for i in range(len(curves)-1):
        if i > 0:
            ax = plt.subplot(len(curves)-1,1,len(curves)-i-1, sharex = ax1)
            plt.setp(ax.get_xticklabels(), visible=False)
        else:
            ax = ax1
        plt.plot(curves[-1],curves[i])
        plt.plot(curves[-1],curves[i],'o')
    plt.show()