def sigmoid(self): rb = -(self.src.shape[1] * np.tan(np.deg2rad(100 / 2))) / ( np.tan(np.deg2rad(self.fov / 2)) * np.log(99e-8)) ra = rb * np.log(99) return np.repeat( np.power( 1 + np.exp(-(np.linalg.norm(self.uniform_grid, axis=2) - ra) / rb), -1), 2).reshape(self.uniform_grid.shape)
def score_tan(G): """ Takes advantage of tangent function. """ sum_score = 0 for n in G.nodes(): sum_score += np.tan(n) for nbr in G.neighbors(n): sum_score += np.tan(nbr) return sum_score
def build_roatation_matrix_3D(pitch, roll, yaw): base = np.sqrt(np.tan(-pitch) ** 2 + np.tan(-roll) ** 2 + 1) rotation_matrix_p_r = np.array([[1 / base, np.tan(-roll) / base, np.tan(-roll) * np.tan(-pitch) * base], [-np.tan(-roll) / base, 1 / base, np.tan(-pitch) / base], [np.tan(-roll) * np.tan(-pitch) / base, -np.tan(-pitch) / base, 1 / base]]) rotation_matrix_y = np.array([[np.cos(yaw), 0, -np.sin(yaw)], [0, 1, 0], [np.sin(yaw), 0, np.cos(yaw)]]) return rotation_matrix_p_r, rotation_matrix_y
def Hapke_vect(SZA, VZA, DPHI, W, R, BB, CC, HH, B0, variant="2002"): # En radian theta0r = SZA * np.pi / 180 thetar = VZA * np.pi / 180 phir = DPHI * np.pi / 180 R = R * np.pi / 180 CTHETA = np.cos(theta0r) * np.cos(thetar) + np.sin(thetar) * np.sin( theta0r) * np.cos(phir) THETA = np.arccos(CTHETA) MUP, MU, S = roughness(theta0r, thetar, phir, R) P = (1 - CC) * (1 - BB**2) / ((1 + 2 * BB * CTHETA + BB**2)**(3 / 2)) P = P + CC * (1 - BB**2) / ((1 - 2 * BB * CTHETA + BB**2)**(3 / 2)) B = B0 * HH / (HH + np.tan(THETA / 2)) gamma = np.sqrt(1 - W) # H0 = (1 + 2 * MUP) / (1 + 2 * MUP * gamma) # H = (1 + 2 * MU) / (1 + 2 * MU * gamma) if variant == "1993": H0 = H_1993(MUP, gamma) H = H_1993(MU, gamma) else: H0 = H_2002(MUP, gamma) H = H_2002(MU, gamma) REFF = W / 4 / (MU + MUP) * ((1 + B) * P + H0 * H - 1) REFF = S * REFF * MUP / np.cos(theta0r) return REFF
def dynamics(self, x, u): # x, y, th, v xn = x + self._dt * np.array([ x[3] * np.cos(x[2]), x[3] * np.sin(x[2]), x[3] * np.tan(u[1]) / self._l, u[0] ]) return xn
def test_function_overloading(): a = pe.pseudo_Obs(17, 2.9, 'e1') b = pe.pseudo_Obs(4, 0.8, 'e1') fs = [ lambda x: x[0] + x[1], lambda x: x[1] + x[0], lambda x: x[0] - x[1], lambda x: x[1] - x[0], lambda x: x[0] * x[1], lambda x: x[1] * x[0], lambda x: x[0] / x[1], lambda x: x[1] / x[0], lambda x: np.exp(x[0]), lambda x: np.sin(x[0]), lambda x: np.cos(x[0]), lambda x: np.tan(x[0]), lambda x: np.log(x[0]), lambda x: np.sqrt(np.abs(x[0])), lambda x: np.sinh(x[0]), lambda x: np.cosh(x[0]), lambda x: np.tanh(x[0]) ] for i, f in enumerate(fs): t1 = f([a, b]) t2 = pe.derived_observable(f, [a, b]) c = t2 - t1 assert c.is_zero() assert np.log(np.exp(b)) == b assert np.exp(np.log(b)) == b assert np.sqrt(b**2) == b assert np.sqrt(b)**2 == b np.arcsin(1 / b) np.arccos(1 / b) np.arctan(1 / b) np.arctanh(1 / b) np.sinc(1 / b)
def sample(self, params, n_samples): m, s = self.unpack_params(params) unif_samples = agnp.random.uniform(0, 1, size=(n_samples, self.n_params)) samples = m + agnp.exp(s) * agnp.tan(agnp.pi * (unif_samples - 0.5)) return samples
def vjp(g): a = m * 1/np.tan(th)*ans if abs(m+1) <= n: b = np.exp(-1j*ph) * np.sqrt((n+1)*n - (m+1)*m) * sph_harm(m+1, n, ph, th) else: b = 0 return g * (a + b)
def dynamics(self, x, u): _u = np.clip(u, -self.ulim, self.ulim) # x, y, th, v xn = x + self._dt * np.array([x[3] * np.cos(x[2]), x[3] * np.sin(x[2]), x[3] * np.tan(_u[1]) / self._l, _u[0]]) xn = np.clip(xn, -self._xmax, self._xmax) return xn
def visit_Function(self, node): f = node.value if f == EXP: return np.exp(self.visit(node.expr)) if (f == LOG) or (f == LN): return np.log(self.visit(node.expr)) if f == LOG10: return np.log10(self.visit(node.expr)) if f == SQRT: return np.sqrt(self.visit(node.expr)) if f == ABS: return np.abs(self.visit(node.expr)) if f == SIGN: return np.sign(self.visit(node.expr)) if f == SIN: return np.sin(self.visit(node.expr)) if f == COS: return np.cos(self.visit(node.expr)) if f == TAN: return np.tan(self.visit(node.expr)) if f == ASIN: return np.arcsin(self.visit(node.expr)) if f == ACOS: return np.arccos(self.visit(node.expr)) if f == ATAN: return np.arctan(self.visit(node.expr)) if f == MAX: raise NotImplementedError(MAX) if f == MIN: raise NotImplementedError(MIN) if f == NORMCDF: raise NotImplementedError(NORMCDF) if f == NORMPDF: raise NotImplementedError(NORMPDF) if f == ERF: return erf(self.visit(node.expr))
def roughness(theta0, theta, phi, R): xidz = 1 / np.sqrt(1 + np.pi * (np.tan(R)**2)) cose = np.cos(theta) sine = np.sin(theta) cosi = np.cos(theta0) sini = np.sin(theta0) mu_b = xidz * (cose + sine * np.tan(R) * e2(R, theta) / (2 - e1(R, theta))) mu0_b = xidz * (cosi + sini * np.tan(R) * e2(R, theta0) / (2 - e1(R, theta0))) f = np.exp(-2 * np.tan(phi / 2)) mask1 = theta0 <= theta mask2 = ~mask1 mu0_e, mu_e, S = np.empty(theta.shape), np.empty(theta.shape), np.empty( theta.shape) # Case 1 mu0_e1 = xidz * (cosi + sini * np.tan(R) * (np.cos(phi) * e2(R, theta) + (np.sin(phi / 2)**2) * e2(R, theta0)) / (2 - e1(R, theta) - (phi / np.pi) * e1(R, theta0))) mu_e1 = xidz * (cose + sine * np.tan(R) * (e2(R, theta) - (np.sin(phi / 2)**2) * e2(R, theta0)) / (2 - e1(R, theta) - (phi / np.pi) * e1(R, theta0))) S1 = mu_e1 * cosi * xidz / mu_b / mu0_b / (1 - f + f * xidz * cosi / mu0_b) # Case 2 mu0_e2 = xidz * (cosi + sini * np.tan(R) * (e2(R, theta0) - np.sin(phi / 2)**2 * e2(R, theta)) / (2 - e1(R, theta0) - (phi / np.pi) * e1(R, theta))) mu_e2 = xidz * ( cose + sine * np.tan(R) * (np.cos(phi) * e2(R, theta0) + np.sin(phi / 2)**2 * e2(R, theta)) / (2 - e1(R, theta0) - (phi / np.pi) * e1(R, theta))) S2 = mu_e2 * cosi * xidz / mu_b / mu0_b / (1 - f + f * xidz * cose / mu_b) # mu0_e[mask1] = mu0_e1[mask1] # mu0_e[mask2] = mu0_e2[mask2] # mu_e[mask1] = mu_e1[mask1] # mu_e[mask2] = mu_e2[mask2] # S[mask1] = S1[mask1] # S[mask2] = S2[mask2] mu0_e = mask1 * mu0_e1 + mask2 * mu0_e2 mu_e = mask1 * mu_e1 + mask2 * mu_e2 S = mask1 * S1 + mask2 * S2 return mu0_e, mu_e, S
def reparameterize(x, prior=None): if prior == None: return x elif prior == 'horseshoe': eta = np.tan(.5 * np.pi * norm.cdf(x)) return eta**2 * x elif prior == 'exp': return -np.log(x) elif prior == 'laplace': return -np.sign(x) * np.log(1 - 2 * x) elif prior == 'lognorm': return np.exp(x) elif prior == 'IG': return 1 / x elif prior == 'dropout': return npr.binomial(1, .8, size=x.shape) * x
def test_man_grad(): a = pe.pseudo_Obs(17, 2.9, 'e1') b = pe.pseudo_Obs(4, 0.8, 'e1') fs = [ lambda x: x[0] + x[1], lambda x: x[1] + x[0], lambda x: x[0] - x[1], lambda x: x[1] - x[0], lambda x: x[0] * x[1], lambda x: x[1] * x[0], lambda x: x[0] / x[1], lambda x: x[1] / x[0], lambda x: np.exp(x[0]), lambda x: np.sin(x[0]), lambda x: np.cos(x[0]), lambda x: np.tan(x[0]), lambda x: np.log(x[0]), lambda x: np.sqrt(x[0]), lambda x: np.sinh(x[0]), lambda x: np.cosh(x[0]), lambda x: np.tanh(x[0]) ] for i, f in enumerate(fs): t1 = f([a, b]) t2 = pe.derived_observable(f, [a, b]) c = t2 - t1 assert c.value == 0.0, str(i) assert np.all(np.abs(c.deltas['e1']) < 1e-14), str(i)
def value(self, x, u): # Controls w = u[0] # Front wheel angle a = u[1] # Front wheel acceleration o = x[2] # Car angle v = x[3] # Velocity # Change in car angle d_angle = v / CAR_AXLES_DISTANCE * np.tan(w) xdot = np.array([v * np.cos(o), v * np.sin(o), d_angle, a], dtype=np.float32) if self.Q is not None: # Add system noise xdot += np.random.multivariate_normal(np.zeros_like(x), self.Q) return xdot
def g4(self,angles4): # output = np.ones((4,3)) # determine distances between points # angles411 = np.arccos(angles4[1, 1]) # angles410 = np.arccos(angles4[1, 0]) # angles421 = np.arccos(angles4[2, 1]) # angles431 = np.arccos(angles4[3, 1]) # angles420 = np.arccos(angles4[2, 0]) # angles430 = np.arccos(angles4[3, 0]) # angles400 = np.arccos(angles4[0, 0]) # angles401 = np.arccos(angles4[0, 1]) angles411 = (angles4[1, 1]) angles410 = (angles4[1, 0]) angles421 = (angles4[2, 1]) angles431 = (angles4[3, 1]) angles420 = (angles4[2, 0]) angles430 = (angles4[3, 0]) angles400 = (angles4[0, 0]) #angles401 = (angles4[0, 1]) c2d2 = 1 / np.sin(angles411) c1d2 = 1 / np.sin(angles410) d2_d1c2 = c2d2 * np.sin(angles421) d2_c1d1 = np.sin(angles431) * c1d2 d1c2 = (d2_d1c2 / np.tan(angles420)) + (d2_d1c2 / np.tan(angles421)) d1c1 = (d2_c1d1 / np.tan(angles430)) + (d2_c1d1 / np.tan(angles431)) d1d2 = d2_d1c2 / np.sin(angles420) #d1_c1c2 = np.sin(angles401) * d1c1 # law of cosines #c1c2 = (d1c1 ** 2 + d1c2 ** 2 - 2 * d1c2 * d1c1 * np.cos(angles400)) ** 0.5 result = [] result.append(0.0) result.append(0.0) result.append(0.0) result.append(d1c1) result.append(0.0) result.append(0.0) result.append(d1c2 * np.cos(angles400)) result.append(d1c2 * np.sin(angles400)) result.append(0.0) s = c1d2 * np.cos(angles410) / (c2d2 * np.cos(angles411) + c1d2 * np.cos(angles410)) asdf = [(1 - s) * d1c1 + s * d1c2 * np.cos(angles400), s * d1c2 * np.sin(angles400), 0.] slope = (result[3] - result[6]) / (result[7]) x = d1d2 * np.cos(angles430) result.append(x) # rise = slope * (x - d1c1) rise = slope * (x - asdf[0]) y = asdf[1] + rise result.append(y) z = (d1d2 ** 2 - (x ** 2 + y ** 2)) ** 0.5 result.append(z) # return(asdf[2]) # print(result) result = np.asarray(result) # return(result[11]) d1 = result[0:3] # return(d1[]) c1 = result[3:6] c2 = result[6:9] d2 = result[9:12] cc = (c2 - c1) ip = np.inner((d1 - c1), (c2 - c1)) / (np.sum((c2 - c1) ** 2)) # return(output) tilded1 = [d1[0] - ip * cc[0], d1[1] - ip * cc[1], d1[2] - ip * cc[2]] iq = np.inner((d2 - c2), (c1 - c2)) / (np.sum((c1 - c2) ** 2)) cc2 = (c1 - c2) tilded2 = [d2[0] - iq * cc2[0], d2[1] - iq * cc2[1], d2[2] - iq * cc2[2]] tilded2star = [tilded2[0] + cc2[0], tilded2[1] + cc2[1], tilded2[2] + cc2[2]] ab = np.sqrt((tilded2star[0] - c1[0]) ** 2 + (tilded2star[1] - c1[1]) ** 2 + (tilded2star[2] - c1[2]) ** 2) bc = np.sqrt((tilded2star[0] - tilded1[0]) ** 2 + (tilded2star[1] - tilded1[1]) ** 2 + (tilded2star[2] - tilded1[2]) ** 2) ca = np.sqrt((tilded1[0] - c1[0]) ** 2 + (tilded1[1] - c1[1]) ** 2 + (tilded1[2] - c1[2]) ** 2) output = (ab ** 2 - bc ** 2 + ca ** 2) / (2 * ab * ca) return (output)
def complicated_fun(a,b,c,d,e,f=1.1, g=9.0): return a + np.sin(b) + np.cosh(c) + np.cos(d) + np.tan(e) + f + g
def test_tan(): fun = lambda x: 3.0 * np.tan(x) check_grads(fun)(npr.randn())
def f(x, u): # x, y, th, v x, y, th, v = x a, phi = u return np.array( [v * np.cos(th), v * np.sin(th), v * np.tan(phi) / self.l, a])
# x_slice is progressed for one step every iterations x_slice = matrix[:, i:i + 1] w_batch_hist = gradient_descent(least_square, w[i:i + 1], alpha, max_its, beta=0) w[i:i + 1] = w_batch_hist[-1] b = model(matrix[:, :i + 1], w[:i + 1]) w_hist.append(w_batch_hist[-1]) return w sin = lambda w: np.sin(w) cos = lambda w: np.cos(w) tan = lambda w: np.tan(w) def function_vector(a, b, c): result = np.array((sin(a), cos(b), tan(c))) return result.reshape(-1, 1) def matrix_function(w, function_list): for i in range(w.shape[1]): w[:, i] = np.array(list(map(eval(function_list[i]), w[:, i]))) return w if __name__ == "__main__": matrix = np.random.randn(50, 5)
def tan(a: Numeric): return anp.tan(a)
def f(x, u): return np.hstack([ x[3] * np.cos(x[2]), x[3] * np.sin(x[2]), x[3] * np.tan(u[1]) / self.l, u[0] ])
def complicated_fun(a, b, c, d, e, f=1.1, g=9.0): return a + np.sin(b) + np.cosh(c) + np.cos(d) + np.tan(e) + f + g
def e2(R, x): arg = (np.tan(R)**2 * np.tan(x)**2 * np.pi) mask = np.logical_not(np.isclose(arg, 0)) y = np.where(mask, np.exp(-1 / arg[mask]), 0) return y
def __init__(self): #=============# # ROS Objects #=============# # ROS Parameters rospy.init_node("maneuver_path") self.approach_pose_offset = rospy.get_param( "~approach_pose_offset", 8.0 ) # distance in meters used to offset the maneuver starting pose from the roll, this is to give a good initial value for the optimization self.roll_radius = rospy.get_param("/roll/radius", 0.20) self.resolution = rospy.get_param( "~maneuver_resolution", 0.10 ) # resolution for determining number of waypoints in the maneuver paths self.rescale_factor = rospy.get_param( "~rescale_factor", 0.5 ) # Decreases the resolution of the image along each axis by this fraction self.max_angle = rospy.get_param("/forklift/steering/max_angle", 65 * (np.pi / 180.)) # deg # Optimization Parameters print("Namespace: %s" % rospy.get_name()) self.optimization_method = rospy.get_param( "~optimization_method", 0) # 0 = use hardcoded, 1 = use scipy, 2 = use pyipopt self.start_x_s = rospy.get_param("~start_x_s", 11.5) self.start_y_s = rospy.get_param("~start_y_s", -6.8) self.start_theta_s = rospy.get_param("~start_theta_s", 0.2) self.start_r_1 = rospy.get_param("~start_r_1", -0.4) self.start_alpha_1 = rospy.get_param("~start_alpha_1", -1.6) self.start_r_2 = rospy.get_param("~start_r_2", 1.0) self.start_alpha_2 = rospy.get_param("~start_alpha_2", 1.4) self.target_x = None self.target_y = None self.target_approach_angle = None self.obstacles = None self.maneuver_path = PathWithGear() self.maneuver_path.path.header.frame_id = "/odom" self.optimization_success = False self.update_obstacle_end_pose = False self.current_pose = None self.rate = rospy.Rate(30) # Forklift dimensions self.base_to_clamp = rospy.get_param("/forklift/body/base_to_clamp", 1.4658) # {m} self.base_to_back = rospy.get_param("/forklift/body/base_to_back", 1.9472) # {m} self.width = rospy.get_param("/forklift/body/width", 1.3599) # {m} self.total_length = rospy.get_param("/forklift/body/total", 3.5659) # {m} self.buffer = 0.5 # {m} gap between edge of forklift and the bounding box ''' Forklift Bounding Box L_3 |--------------| ___ ___ | buffer | _|_ | / | \ / | \ / | L_1 ----------------- | |__ ^ __| | | | | | | | |__| <--o |__|buffer--- | |-----| | | | | | | | | | | L_2 ----------------- | | buffer | _|_ _|_ ''' self.L_1 = self.base_to_clamp + self.buffer # length from base to back end of box self.L_2 = self.base_to_back + self.buffer # length from base to front end of box self.L_3 = (self.width / 2.) + self.buffer # length from base to side # Max turning radius self.axle_distance = 1.7249 self.min_radius = 1.5 * self.axle_distance / np.tan( self.max_angle ) # add a scaling factor to give more space to make the turn # ROS Publishers and Subscribers self.path1_pub = rospy.Publisher("~path1", Path, queue_size=3, latch=True) self.path2_pub = rospy.Publisher("~path2", Path, queue_size=3, latch=True) self.path1_gear_pub = rospy.Publisher("~path1_gear", PathWithGear, queue_size=3, latch=True) self.path2_gear_pub = rospy.Publisher("~path2_gear", PathWithGear, queue_size=3, latch=True) self.approach_pose_pub = rospy.Publisher("/forklift/approach_pose", PoseStamped, queue_size=3) self.occupancy_grid_sub = rospy.Subscriber("/map", OccupancyGrid, self.occupancyGridCallback, queue_size=1) self.roll_pose_sub = rospy.Subscriber("/roll/pose", PoseStamped, self.rollCallback, queue_size=3) self.odom_sub = rospy.Subscriber("/odom", Odometry, self.odomCallback, queue_size=1) self.obstacle_path_sub = rospy.Subscriber("/obstacle_avoidance_path", Path, self.obstaclePathCallback, queue_size=1) # indicates whether the optimzation completed successfully or not, to know whether the path is usable self.optimize_maneuver_srv = rospy.Service("~optimize_maneuver", OptimizeManeuver, self.maneuverService)
def test_tan(): fun = lambda x : 3.0 * np.tan(x) d_fun = grad(fun) check_grads(fun, npr.randn()) check_grads(d_fun, npr.randn())
def test_tan(): fun = lambda x : 3.0 * np.tan(x) check_grads(fun)(npr.randn())
def phi_z_0_0(Cphi, Dphi, nc, nd, psi_dot, psi): term1 = (Cphi / psi_dot * (np.sqrt(nc) / (nc - 1)) * np.arctan( (1 - np.sqrt(nc)) * np.tan(psi) / (1 + np.sqrt(nc) * np.tan(psi)**2))) term2 = (Dphi / psi_dot * (np.sqrt(nd) / (nd - 1)) * np.arctan( (1 - np.sqrt(nd)) * np.tan(psi) / (1 + np.sqrt(nd) * np.tan(psi)**2))) return term1 + term2