velocity_point_generation.py 962 Bytes
Newer Older
Marlene Böhmer's avatar
Marlene Böhmer committed
1 2
import numpy as np

3 4 5 6
n_points = 2
radius = 0.25
circle_time = 2
time_for_segment = circle_time/n_points
Marlene Böhmer's avatar
Marlene Böhmer committed
7 8 9 10 11 12 13 14 15 16

y_points = np.arange(n_points+1, dtype=np.float64)
z_points = np.arange(n_points+1, dtype=np.float64)

y_points = [x * 2*np.pi/n_points for x in y_points]
z_points = [x * 2*np.pi/n_points for x in z_points]

y_points = [np.cos(x) for x in y_points]
z_points = [np.sin(x) for x in z_points]

17 18
y_points = [x * radius for x in y_points]
z_points = [x * radius for x in z_points]
Marlene Böhmer's avatar
Marlene Böhmer committed
19 20 21 22 23 24 25

y_diff = [y_points[i+1]-y_points[i] for i in range(n_points)]
z_diff = [z_points[i+1]-z_points[i] for i in range(n_points)]

y_velocity = [y_diff[i]/time_for_segment for i in range(n_points)]
z_velocity = [z_diff[i]/time_for_segment for i in range(n_points)]

26 27 28
y_velocity = np.around(y_velocity, 10)
z_velocity = np.around(z_velocity, 10)

Marlene Böhmer's avatar
Marlene Böhmer committed
29
for i in range(n_points):
30
    print("mc.start_linear_motion(0.0,", y_velocity[i], ",", z_velocity[i], ")\ntime.sleep(", time_for_segment, ")")