-
Notifications
You must be signed in to change notification settings - Fork 1
/
slope_function.py
153 lines (125 loc) · 7.05 KB
/
slope_function.py
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
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
import ast
import math
import marble_path
import marble_util
def get_drop(arclengths, min_angle, max_angle, start_time_step, end_time_step):
"""
Given a set of arclengths, the start & end times, and the angles
to gradually transition between, return the total drop for that span.
TODO: refactor with the code that assigns the actual angles in update_slopes_weighted?
"""
total_drop = 0.0
delta_time_step = end_time_step - start_time_step
for time_step in range(delta_time_step):
if time_step < delta_time_step * 0.2:
slope = min_angle + (max_angle - min_angle) * time_step / (delta_time_step * 0.2)
elif time_step > delta_time_step * 0.8:
slope = min_angle + (max_angle - min_angle) * (delta_time_step - time_step) / (delta_time_step * 0.2)
else:
slope = max_angle
total_drop = total_drop + (arclengths[time_step + start_time_step + 1] - arclengths[time_step + start_time_step]) * math.tan(slope * math.pi / 180)
#print("Total drop for angle", max_angle, "is", total_drop)
return total_drop
def get_drop_angle(arclengths, slope_angle, start_time_step, end_time_step, needed_dz):
"""
Get the drop angle needed to gradually achieve the desired dz in the given time span
Works by using binary search between the base slope_angle and 45 degrees down
"""
total_drop = get_drop(arclengths, slope_angle, slope_angle, start_time_step, end_time_step)
if total_drop > needed_dz:
return
total_drop = get_drop(arclengths, slope_angle, 45, start_time_step, end_time_step)
if total_drop < needed_dz:
raise ValueError("Even an angle of 45 is not sufficient to achieve this drop")
min_angle = slope_angle
max_angle = 45
while max_angle - min_angle > 0.01:
test_angle = (max_angle + min_angle) / 2
total_drop = get_drop(arclengths, slope_angle, test_angle, start_time_step, end_time_step)
if total_drop < needed_dz:
min_angle = test_angle
else:
max_angle = test_angle
return (max_angle + min_angle) / 2.0
def update_slopes_weighted(slopes, start_time_step, end_time_step, slope_angle, final_angle, sharpness, use_max):
delta_time_step = end_time_step - start_time_step
for time_step in range(delta_time_step+1):
if time_step < delta_time_step * sharpness:
new_slope = slope_angle + (final_angle - slope_angle) * time_step / (delta_time_step * sharpness)
elif time_step > delta_time_step * (1.0 - sharpness):
new_slope = slope_angle + (final_angle - slope_angle) * (delta_time_step - time_step) / (delta_time_step * sharpness)
else:
new_slope = final_angle
if use_max:
slopes[time_step+start_time_step] = max(new_slope, slopes[time_step+start_time_step])
else:
slopes[time_step+start_time_step] = min(new_slope, slopes[time_step+start_time_step])
def update_slopes_overlap(slopes, arclengths, times, slope_angle, start_t, end_t, needed_dz):
"""
Update a list of slopes, changing the slopes in a way such that
between start_t and end_t, the path goes down by needed_dz
"""
start_time_step = marble_util.get_time_step(times, start_t)
end_time_step = marble_util.get_time_step(times, end_t)
if end_time_step < start_time_step:
end_time_step, start_time_step = start_time_step, end_time_step
best_angle = get_drop_angle(arclengths, slope_angle, start_time_step, end_time_step, needed_dz)
if best_angle is None:
print("Nothing to do for the overlap at interval %.4f, %.4f" % (start_t, end_t))
return
update_slopes_weighted(slopes, start_time_step, end_time_step, slope_angle, best_angle, 0.2, True)
def update_slopes_kink(slopes, times, slope_angle, kink_args, t):
start_t = t - kink_args.kink_width
start_time_step = marble_util.get_time_step(times, start_t)
end_t = t + kink_args.kink_width
end_time_step = marble_util.get_time_step(times, end_t)
if end_time_step < start_time_step:
end_time_step, start_time_step = start_time_step, end_time_step
print("Adding kink from ", start_time_step, " to ", end_time_step)
update_slopes_weighted(slopes, start_time_step, end_time_step, slope_angle, kink_args.kink_slope, kink_args.kink_sharpness, False)
def slope_function(x_t, y_t, time_t, slope_angle, num_time_steps, overlap_args, kink_args):
arclengths = marble_path.calculate_arclengths(x_t, y_t, num_time_steps)
times = [time_t(t) for t in range(num_time_steps+1)]
slopes = [slope_angle for t in range(num_time_steps+1)]
if kink_args and getattr(kink_args, 'kinks', None):
for t in kink_args.kinks:
update_slopes_kink(slopes, times, slope_angle, kink_args, t)
if overlap_args and getattr(overlap_args, 'overlaps', None):
for start_t, end_t in overlap_args.overlaps:
# for the basic 2 loop cycloid, want +/- .16675, 1.40405
update_slopes_overlap(slopes, arclengths, times, slope_angle,
start_t, end_t, overlap_args.overlap_separation)
#for i, s in enumerate(slopes):
# print("%4d %.4f" % (i, s))
slope_angle_t = lambda time_step_t: slopes[time_step_t]
return slope_angle_t
def parse_kinks(kink_str):
kink_tuple = ast.literal_eval(kink_str)
return kink_tuple
def parse_kink_sharpness(sharp_str):
"""Kink sharpness will determine how quickly to go from the base slope to the kink slope
If the sharpness is too close to 0, the quickly changing angle
will throw off the tube and make more kinks along the way. If the
sharpness is too close to 0.5, the slope might not change enough
to have much effect on the kink.
"""
sharp = float(sharp_str)
if sharp < 0 or sharp > 0.5:
raise ValueError("kink_sharpness must be between 0 and 0.5")
return sharp
def add_kink_args(parser):
parser.add_argument('--kinks', default=None, type=parse_kinks,
help='Tuple of t to represent where to make the slope closer to 0. Intended to make tight corners less disruptive to the model')
parser.add_argument('--kink_width', default=0.1, type=float,
help='How wide to make the kinks in terms of time')
parser.add_argument('--kink_slope', default=0.5, type=float,
help='Angle to make the kink')
parser.add_argument('--kink_sharpness', default=0.2, type=parse_kink_sharpness,
help='How steep to make the transition from regular slope to kink_slope. 0..0.5')
def parse_overlaps(overlap_str):
return marble_util.parse_tuple_tuple(overlap_str, "--overlaps")
def add_overlap_args(parser):
parser.add_argument('--overlaps', default=None, type=parse_overlaps,
help='Tuple of (start, end) pairs which represents the time periods where overlaps occur. Angle will be changed to enforce a large enough drop there.')
parser.add_argument('--overlap_separation', default=25.0, type=float,
help='Required vertical distance between loops')