trajectory_planning/visualize.py at master · Alevs2R/trajectory_planning · 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
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
import matplotlib.pyplot as plt
import numpy as np

from kinematics import forw_kin
from robot_configuration import L


def motion_plot(pos, v, acc):
        time = np.arange(0, v.shape[1] * 0.01 - 0.005, 0.01)

        plt.figure(1)
        plt.ylabel('position, rad')
        plt.xlabel('time, s')
        plt.title('joint positions')

        for i in range(0, v.shape[0]):
            plt.plot(time, pos[i, :], 'C'+str(i+1))
        plt.savefig('plots/joint_positions.png')
        plt.show()

        plt.figure(2)
        plt.ylabel('velocity, rad')
        plt.xlabel('time, s')
        plt.title('joint velocities')

        for i in range(0, v.shape[0]):
            plt.plot(time, v[i, :], 'C'+str(i+1))
        plt.savefig('plots/joint velocities.png')
        plt.show()

        plt.figure(3)
        plt.ylabel('rad/s^2')
        plt.xlabel('time, ms')
        plt.title('joint accelerations')
        cartesian_pos = np.zeros(shape=(3, pos.shape[1]))
        for i in range(0, v.shape[0]):
            plt.plot(time, acc[i, :], 'C'+str(i+1))
        plt.savefig('plots/joint_accelerations.png')
        plt.show()

        for i in range(0, pos.shape[1]):
            cartesian_pos[:, i] = forw_kin(pos[:, i].flatten(), L)

        # plt.figure(4)
        # ax = plt.axes(projection='3d')
        # ax.set_xlabel('X axis')
        # ax.set_ylabel('Y axis')
        # ax.set_zlabel('Z axis')
        # ax.plot3D(cartesian_pos[0, :].flatten(), cartesian_pos[1, :].flatten(), cartesian_pos[2:, ].flatten(), 'gray')
        # plt.show()

        plt.figure(4)
        plt.ylabel('position, m')
        plt.xlabel('time, s')
        plt.title('X cartesian axis')
        plt.plot(time, cartesian_pos[0])
        plt.ylim([-3, 10])

        plt.savefig('plots/x_position.png')
        plt.show()


        plt.figure(5)
        plt.ylabel('position, m')
        plt.xlabel('time, s')
        plt.title('Y cartesian axis')
        plt.plot(time, cartesian_pos[1])
        plt.ylim([-3, 7])
        plt.savefig('plots/y_position.png')
        plt.show()


        plt.figure(6)
        plt.ylabel('position, m')
        plt.xlabel('time, s')
        plt.title('Z cartesian axis')
        plt.plot(time, cartesian_pos[2])
        plt.ylim([-7, 2])
        plt.savefig('plots/z_position.png')
        plt.show()