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)]
# 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 g2(x):
return imath.cos(x[1]) + 0.1 * (x[1]**2)