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)
Exemple #2
0
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 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
Exemple #4
0
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
Exemple #6
0
 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
Exemple #7
0
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
Exemple #9
0
    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)
Exemple #10
0
 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
Exemple #11
0
    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))
Exemple #12
0
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
Exemple #14
0
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)
Exemple #15
0
    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
Exemple #16
0
 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)
Exemple #17
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
Exemple #18
0
def test_tan():
    fun = lambda x: 3.0 * np.tan(x)
    check_grads(fun)(npr.randn())
Exemple #19
0
 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)
Exemple #21
0
def tan(a: Numeric):
    return anp.tan(a)
Exemple #22
0
 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
Exemple #24
0
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())
Exemple #27
0
def test_tan():
    fun = lambda x : 3.0 * np.tan(x)
    check_grads(fun)(npr.randn())
Exemple #28
0
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())
Exemple #29
0
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