def max_acc_int(Xi, tau):
tau = interval([-tau, tau])
q = interval(Xi[0:2])
a_min = (tau[0][0] - m * l * g * imath.sin(q)[0][0]) / (m * l**2)
a_max = (tau[0][1] - m * l * g * imath.sin(q)[0][1]) / (m * l**2)
print(q, tau, a_min, a_max)
return (a_min, a_max)
def max_acc_int(Xi, tau):
tau = interval(
[-tau, tau]
) # Pay attention : do not touch for this case but it is general sligtly incorrect
q = interval(Xi[0:2])
a_min = (tau[0][0] - m * l * g * imath.sin(q)[0][0]) / (m * l**2)
a_max = (tau[0][1] - m * l * g * imath.sin(q)[0][1]) / (m * l**2)
print(q, tau, a_min, a_max)
return (a_min, a_max)
def step(self, u):
th, thdot = self.state # th := theta
g = self.g
m = self.m
l = self.l
dt = self.dt
# u = min(max(u, -self.max_torque), self.max_torque)
u = self.max_torque * 1 if u == 1 else -self.max_torque * 1
self.last_u = u # for rendering
costs = angle_normalize_tuple(th)**2 + .1 * thdot**2 + .001 * (u**2)
newthdot = thdot + (-3 * g / (2 * l) * imath.sin(th + np.pi) + 3. /
(m * l**2) * u) * dt
newth = th + newthdot * dt
newthdot = clip_interval(newthdot, -self.max_speed, self.max_speed) # pylint: disable=E1111
self.state = (newth, newthdot)
done = newth[0][1] < -self.max_angle or newth[0][0] > self.max_angle
half_done = done or newth[0][0] < -self.max_angle or newth[0][
1] > self.max_angle
return HyperRectangle.from_numpy(
np.array(tuple([make_tuple(x) for x in self.state
])).transpose()), -costs, done, half_done
def test_trig(self):
assert imath.sin(imath.pi / 2) == interval[helpers.nudge(1, -1), 1]
assert imath.cos(imath.pi) == interval[-1, helpers.nudge(-1, +1)]
assert imath.cos(imath.pi /
interval[3]) == interval[helpers.nudge(0.5, -6),
helpers.nudge(0.5, 1)]
assert imath.tan(imath.pi / 4) == interval[helpers.nudge(1, -1),
helpers.nudge(1, +1)]
def max_acc_int(Xi1,Xi2,torque):
tau1=torque[0]
tau2=torque[1]
q1=interval(Xi1[0:2])
q2=interval(Xi1[2:4])
dq1=interval(Xi2[0:2])
dq2=interval(Xi2[2:4])
a_min_1= -(-tau1+ m2*g*imath.sin(q2)*imath.cos(q1+q2) -m2*imath.sin(q1-q2)*(l1*dq1*imath.cos(q1-q2)+l2*dq2**2) - (m1+m2)*g*imath.sin(q1) ) / (l1*(m1+m2*(imath.sin(q1-q2)*imath.sin(q1-q2)) ))
a_max_1= -(-tau2+ m2*g*imath.sin(q2)*imath.cos(q1+q2) -m2*imath.sin(q1-q2)*(l1*dq1*imath.cos(q1-q2)+l2*dq2**2) - (m1+m2)*g*imath.sin(q1) ) / (l1*(m1+m2*(imath.sin(q1-q2)*imath.sin(q1-q2)) ))
a_min_2= (-tau1 +(m1+m2)*(l1*dq1**2*imath.sin(q1-q2) -g*imath.sin(q2)+g*imath.sin(q1)*imath.cos(q1-q2)+m2*l2*dq2**2*imath.sin(q1-q2)*imath.cos(q1-q2) ) ) / (l1*(m1+m2*(imath.sin(q1-q2)*imath.sin(q1-q2))))
a_max_2= (-tau2 +(m1+m2)*(l1*dq1**2*imath.sin(q1-q2) -g*imath.sin(q2)+g*imath.sin(q1)*imath.cos(q1-q2)+m2*l2*dq2**2*imath.sin(q1-q2)*imath.cos(q1-q2) ) ) / (l1*(m1+m2*(imath.sin(q1-q2)*imath.sin(q1-q2))))
print(q1,q2,tau1,tau2,a_min_1,a_max_1,a_min_2,a_max_2)
return (a_min_1,a_max_1,a_min_2,a_max_2)
def __fwd_eval(self, n_id, box):
n = self.dag[n_id]
fwd = self.__fwd
rec = self.__fwd_eval
if n[0] == '+':
rec(n[1], box)
rec(n[2], box)
fwd[n_id] = fwd[n[1]] + fwd[n[2]]
elif n[0] == '-':
rec(n[1], box)
rec(n[2], box)
fwd[n_id] = fwd[n[1]] - fwd[n[2]]
elif n[0] == '*':
rec(n[1], box)
rec(n[2], box)
fwd[n_id] = fwd[n[1]] * fwd[n[2]]
elif n[0] == '/':
rec(n[1], box)
rec(n[2], box)
fwd[n_id] = fwd[n[1]] / fwd[n[2]]
elif n[0] == '^':
rec(n[1], box)
i = self.dag[n[2]][1]
fwd[n_id] = fwd[n[1]]**i
elif n[0] == 'exp':
rec(n[1], box)
fwd[n_id] = imath.exp(fwd[n[1]])
elif n[0] == 'sqrt':
rec(n[1], box)
fwd[n_id] = imath.sqrt(fwd[n[1]])
elif n[0] == 'sin':
rec(n[1], box)
fwd[n_id] = imath.sin(fwd[n[1]])
elif n[0] == 'cos':
rec(n[1], box)
fwd[n_id] = imath.cos(fwd[n[1]])
elif n[0] == 'C':
fwd[n_id] = interval[n[1]]
elif n[0] == 'V':
fwd[n_id] = box[n[1]]
else:
print('unsupported node: ' + n)
assert (False)
def get_sin_cos_table(max_theta, min_theta, max_theta_dot, min_theta_dot, action):
assert min_theta <= max_theta, f"min_theta = {min_theta},max_theta={max_theta}"
assert min_theta_dot <= max_theta_dot, f"min_theta_dot = {min_theta_dot},max_theta_dot={max_theta_dot}"
step_theta = 0.1
step_theta_dot = 0.1
step_thetaacc = 0.3
min_theta = max(min_theta, -math.pi / 2)
max_theta = min(max_theta, math.pi / 2)
split_theta1 = np.arange(min(min_theta, 0), min(max_theta, 0), step_theta)
split_theta2 = np.arange(max(min_theta, 0), max(max_theta, 0), step_theta)
split_theta = np.concatenate([split_theta1, split_theta2])
split_theta_dot1 = np.arange(min(min_theta_dot, 0), min(max_theta_dot, 0), step_theta)
split_theta_dot2 = np.arange(max(min_theta_dot, 0), max(max_theta_dot, 0), step_theta)
split_theta_dot = np.concatenate([split_theta_dot1, split_theta_dot2])
env = CartPoleEnv(None)
force = env.force_mag if action == 1 else -env.force_mag
split = []
for t_dot in split_theta_dot:
for theta in split_theta:
lb_theta_dot = t_dot
ub_theta_dot = min(t_dot + step_theta_dot, max_theta_dot)
lb = theta
ub = min(theta + step_theta, max_theta)
split.append((interval([lb_theta_dot, ub_theta_dot]), interval([lb, ub])))
sin_cos_table = []
while (len(split)):
theta_dot, theta = split.pop()
sintheta = imath.sin(theta)
costheta = imath.cos(theta)
temp = (force + env.polemass_length * theta_dot ** 2 * sintheta) / env.total_mass
thetaacc: interval = (env.gravity * sintheta - costheta * temp) / (env.length * (4.0 / 3.0 - env.masspole * costheta ** 2 / env.total_mass))
xacc = temp - env.polemass_length * thetaacc * costheta / env.total_mass
if thetaacc[0].sup - thetaacc[0].inf > step_thetaacc:
# split theta theta_dot
mid_theta = (theta[0].sup + theta[0].inf) / 2
mid_theta_dot = (theta_dot[0].sup + theta_dot[0].inf) / 2
theta_1 = interval([theta[0].inf, mid_theta])
theta_2 = interval([mid_theta, theta[0].sup])
theta_dot_1 = interval([theta_dot[0].inf, mid_theta_dot])
theta_dot_2 = interval([mid_theta_dot, theta_dot[0].sup])
split.append((theta_1, theta_dot_1))
split.append((theta_1, theta_dot_2))
split.append((theta_2, theta_dot_1))
split.append((theta_2, theta_dot_2))
else:
sin_cos_table.append((theta, theta_dot, thetaacc, xacc))
return sin_cos_table
def test_trig(self):
assert imath.sin(imath.pi/2) == interval[helpers.nudge(1, -1), 1]
assert imath.cos(imath.pi) == interval[-1, helpers.nudge(-1, +1)]
assert imath.cos(imath.pi/interval[3]) == interval[helpers.nudge(0.5,-6), helpers.nudge(0.5, 1)]
assert imath.tan(imath.pi/4) == interval[helpers.nudge(1,-1), helpers.nudge(1, +1)]
def get_sin_cos_table(max_theta, min_theta, max_theta_dot, min_theta_dot,
action):
assert min_theta <= max_theta, f"min_theta = {min_theta},max_theta={max_theta}"
assert min_theta_dot <= max_theta_dot, f"min_theta_dot = {min_theta_dot},max_theta_dot={max_theta_dot}"
step_theta = 0.3
step_theta_dot = 0.1
# min_theta = max(min_theta, -math.pi / 2)
# max_theta = min(max_theta, math.pi / 2)
split_theta1 = np.arange(min(min_theta, 0), min(max_theta, 0),
step_theta)
split_theta2 = np.arange(max(min_theta, 0), max(max_theta, 0),
step_theta)
split_theta = np.concatenate([split_theta1, split_theta2])
split_theta_dot1 = np.arange(min(min_theta_dot, 0),
min(max_theta_dot, 0), step_theta)
split_theta_dot2 = np.arange(max(min_theta_dot, 0),
max(max_theta_dot, 0), step_theta)
split_theta_dot = np.concatenate([split_theta_dot1, split_theta_dot2])
if len(split_theta_dot) == 0:
split_theta_dot = np.array([max_theta_dot])
if len(split_theta) == 0:
split_theta = np.array([max_theta])
env = MonitoredPendulum(None)
force = 0
if action == 1:
force = -env.max_torque
elif action == 2:
force = env.max_torque
else:
force = 0
split = []
for t_dot in split_theta_dot:
for theta in split_theta:
lb_theta_dot = t_dot
ub_theta_dot = min(t_dot + step_theta_dot, max_theta_dot)
lb = theta
ub = min(theta + step_theta, max_theta)
split.append((interval([lb_theta_dot,
ub_theta_dot]), interval([lb, ub])))
sin_cos_table = []
while (len(split)):
theta_dot, theta = split.pop()
# dynamics
newthdot = theta_dot + (-3 * env.g /
(2 * env.l) * imath.sin(theta + np.pi) +
3. / (env.m * env.l**2) * force) * env.dt
newth = theta + newthdot * env.dt
newthdot = interval([
np.clip(newthdot[0].inf, -env.max_speed, env.max_speed),
np.clip(newthdot[0].sup, -env.max_speed, env.max_speed)
])
if newth[0].sup > np.pi:
if newth[0].inf >= np.pi: # both over the limit, just make a single interval
sin_cos_table.append(
(theta, theta_dot,
PendulumExperiment.interval_normalise_theta_interval(
newth), interval(newthdot)))
else: # only the upper bound is over the limit, make 2 intervals
newth2 = interval([newth[0].inf, np.pi])
sin_cos_table.append(
(theta, theta_dot, newth2, interval(newthdot)))
normalised2 = PendulumExperiment.normalise_theta_value(
newth[0].sup)
newth3 = interval([-np.pi, normalised2])
sin_cos_table.append(
(theta, theta_dot, newth3, interval(newthdot)))
continue
elif newth[0].inf < -np.pi:
if newth[0].sup <= -np.pi: # both over the limit, just make a single interval
sin_cos_table.append(
(theta, theta_dot,
PendulumExperiment.interval_normalise_theta_interval(
newth), interval(newthdot)))
else: # only the lower bound is over the limit, make 2 intervals
normalise_theta2 = PendulumExperiment.normalise_theta_value(
newth[0].inf)
newth2 = interval([np.pi, normalise_theta2])
sin_cos_table.append(
(theta, theta_dot, newth2, interval(newthdot)))
newth3 = interval([-np.pi, newth[0].sup])
sin_cos_table.append(
(theta, theta_dot, newth3, interval(newthdot)))
continue
else:
sin_cos_table.append(
(theta, theta_dot, newth, interval(newthdot)))
return sin_cos_table
# coding: utf-8
# In[25]:
from interval import interval, inf, imath
a = interval[5.0, 5.1]
b = interval[1.0, 2.0]
c = interval[0.36, 0.899]
d = interval[-1.0, 1.0]
print("a = ", a)
print("b = ", b)
print("c = ", c)
print("d = ", d)
print("a+b = ", a + b)
print("c inter d = ", c & d)
print("a*c = ", a * c)
print("sin(a) = ", imath.sin(a))
print("cos(b) = ", imath.cos(b))
print("midpoint(d) =", d.midpoint)
def __eval(self, n_id, box):
n = self.__dag[n_id]
rec = self.__eval
if n[0] == '+':
v1 = rec(n[1], box)
v2 = rec(n[2], box)
return v1 + v2
elif n[0] == '-':
v1 = rec(n[1], box)
v2 = rec(n[2], box)
return v1 - v2
elif n[0] == '*':
v1 = rec(n[1], box)
v2 = rec(n[2], box)
return v1 * v2
elif n[0] == '/':
v1 = rec(n[1], box)
v2 = rec(n[2], box)
return v1 / v2
elif n[0] == '^':
v = rec(n[1], box)
i = self.__dag[n[2]][1]
return v**i
elif n[0] == 'exp':
v = rec(n[1], box)
return imath.exp(v)
elif n[0] == 'log':
v = rec(n[1], box)
return imath.log(v)
elif n[0] == 'sqrt':
v = rec(n[1], box)
return imath.sqrt(v)
elif n[0] == 'sin':
v = rec(n[1], box)
return imath.sin(v)
elif n[0] == 'cos':
v = rec(n[1], box)
return imath.cos(v)
elif n[0] == 'tan':
v = rec(n[1], box)
return imath.tan(v)
elif n[0] == 'asin':
v = rec(n[1], box)
return imath.asin(v)
elif n[0] == 'acos':
v = rec(n[1], box)
return imath.acos(v)
elif n[0] == 'atan':
v = rec(n[1], box)
return imath.atan(v)
elif n[0] == 'sinh':
v = rec(n[1], box)
return imath.sinh(v)
elif n[0] == 'cosh':
v = rec(n[1], box)
return imath.cosh(v)
elif n[0] == 'tanh':
v = rec(n[1], box)
return imath.tanh(v)
elif n[0] == 'C':
return interval[n[1]]
elif n[0] == 'V':
return box[n[1]]
else:
print('unsupported node: ' + str(n))
assert (False)
def g1(x):
return x[0] * imath.sin(x[0]) + 0.1 * (x[0]**2) + 1.0
