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