Esempio n. 1
0
def _SolveForT0(A, B, t, tInterval):
    """There are four solutions to the equation (including complex ones). Here we use x instead of t0
    for convenience.
    """
    """
    SolveQuartic(2*A, -4*A*T + 2*B, 3*A*T**2 - 3*B*T, -A*T**3 + 3*B*T**2, -B*T**3)
    """
    if (Abs(A) < epsilon):

        def f(x):
            return Mul(number('2'), B) * x * x * x - Prod(
                [number('3'), B, t]) * x * x + Prod(
                    [number('3'), B, t, t]) * x - Mul(B, mp.power(t, 3))

        sols = [mp.findroot(f, x0=0.5 * t)]
    else:
        sols = SolveQuartic(
            Add(A, A), Add(Prod([number('-4'), A, t]), Mul(number('2'), B)),
            Sub(Prod([number('3'), A, t, t]), Prod([number('3'), B, t])),
            Sub(Prod([number('3'), B, t, t]), Mul(A, mp.power(t, 3))),
            Neg(Mul(B, mp.power(t, 3))))

    realSols = [
        sol for sol in sols if type(sol) is mp.mpf and sol in tInterval
    ]
    if len(realSols) > 1:
        # I think this should not happen. We should either have one or no solution.
        raise NotImplementedError
    elif len(realSols) == 0:
        return None
    else:
        return realSols[0]
Esempio n. 2
0
def _SolveForT0(A, B, t, tInterval):
    """There are four solutions to the equation (including complex ones). Here we use x instead of t0
    for convenience.
    """

    """
    SolveQuartic(2*A, -4*A*T + 2*B, 3*A*T**2 - 3*B*T, -A*T**3 + 3*B*T**2, -B*T**3)
    """
    if (Abs(A) < epsilon):
        def f(x):
            return Mul(number('2'), B)*x*x*x - Prod([number('3'), B, t])*x*x + Prod([number('3'), B, t, t])*x - Mul(B, mp.power(t, 3))
        sols = [mp.findroot(f, x0=0.5*t)]
    else:
        sols = SolveQuartic(Add(A, A),
                            Add(Prod([number('-4'), A, t]), Mul(number('2'), B)),
                            Sub(Prod([number('3'), A, t, t]), Prod([number('3'), B, t])),
                            Sub(Prod([number('3'), B, t, t]), Mul(A, mp.power(t, 3))),
                            Neg(Mul(B, mp.power(t, 3))))

    realSols = [sol for sol in sols if type(sol) is mp.mpf and sol in tInterval]
    if len(realSols) > 1:
        # I think this should not happen. We should either have one or no solution.
        raise NotImplementedError
    elif len(realSols) == 0:
        return None
    else:
        return realSols[0]
Esempio n. 3
0
def Interpolate1DFixedDuration(x0, x1, v0, v1, newDuration, vm, am):
    x0 = ConvertFloatToMPF(x0)
    x1 = ConvertFloatToMPF(x1)
    v0 = ConvertFloatToMPF(v0)
    v1 = ConvertFloatToMPF(v1)
    vm = ConvertFloatToMPF(vm)
    am = ConvertFloatToMPF(am)
    newDuration = ConvertFloatToMPF(newDuration)
    log.debug("\nx0 = {0}; x1 = {1}; v0 = {2}; v1 = {3}; vm = {4}; am = {5}; newDuration = {6}".\
              format(mp.nstr(x0, n=_prec), mp.nstr(x1, n=_prec), mp.nstr(v0, n=_prec), mp.nstr(v1, n=_prec),
                     mp.nstr(vm, n=_prec), mp.nstr(am, n=_prec), mp.nstr(newDuration, n=_prec)))

    # Check inputs
    assert (vm > zero)
    assert (am > zero)

    if (newDuration < -epsilon):
        return ParabolicCurve()
    if (newDuration <= epsilon):
        # Check if this is a stationary trajectory
        if (FuzzyEquals(x0, x1, epsilon) and FuzzyEquals(v0, v1, epsilon)):
            ramp0 = Ramp(v0, 0, 0, x0)
            newCurve = ParabolicCurve(ramp0)
            return newCurve
        else:
            # newDuration is too short to any movement to be made
            return ParabolicCurve()

    d = Sub(x1, x0)

    # First assume no velocity bound -> re-interpolated trajectory will have only two ramps.
    # Solve for a0 and a1 (the acceleration of the first and the last ramps).
    #         a0 = A + B/t0
    #         a1 = A + B/(t - t0)
    # where t is the (new) total duration, t0 is the (new) duration of the first ramp, and
    #         A = (v1 - v0)/t
    #         B = (2d/t) - (v0 + v1).
    newDurInverse = mp.fdiv(one, newDuration)
    A = Mul(Sub(v1, v0), newDurInverse)
    B = Sub(Prod([mp.mpf('2'), d, newDurInverse]), Add(v0, v1))

    interval0 = iv.mpf([zero, newDuration])  # initial interval for t0

    # Now consider the interval(s) computed from a0's constraints
    sum1 = Neg(Add(am, A))
    sum2 = Sub(am, A)
    C = mp.fdiv(B, sum1)
    D = mp.fdiv(B, sum2)

    log.debug("\nA = {0}; \nB = {1}; \nC = {2}; \nD = {3}; \nsum1 = {4}; \nsum2 = {5};".\
              format(mp.nstr(A, n=_prec), mp.nstr(B, n=_prec), mp.nstr(C, n=_prec), mp.nstr(D, n=_prec),
                     mp.nstr(sum1, n=_prec), mp.nstr(sum2, n=_prec)))

    if (sum1 > zero):
        # This implied that the duration is too short
        log.debug("the given duration ({0}) is too short.".format(newDuration))
        return ParabolicCurve()
    if (sum2 < zero):
        # This implied that the duration is too short
        log.debug("the given duration ({0}) is too short.".format(newDuration))
        return ParabolicCurve()

    if IsEqual(sum1, zero):
        raise NotImplementedError  # not yet considered
    elif sum1 > epsilon:
        log.debug("sum1 > 0. This implies that newDuration is too short.")
        return ParabolicCurve()
    else:
        interval1 = iv.mpf([C, inf])

    if IsEqual(sum2, zero):
        raise NotImplementedError  # not yet considered
    elif sum2 > epsilon:
        interval2 = iv.mpf([D, inf])
    else:
        log.debug("sum2 < 0. This implies that newDuration is too short.")
        return ParabolicCurve()

    if Sub(interval2.a, interval1.b) > epsilon or Sub(interval1.a,
                                                      interval2.b) > epsilon:
        # interval1 and interval2 do not intersect each other
        return ParabolicCurve()
    # interval3 = interval1 \cap interval2 : valid interval for t0 computed from a0's constraints
    interval3 = iv.mpf(
        [max(interval1.a, interval2.a),
         min(interval1.b, interval2.b)])

    # Now consider the interval(s) computed from a1's constraints
    if IsEqual(sum1, zero):
        raise NotImplementedError  # not yet considered
    elif sum1 > epsilon:
        log.debug("sum1 > 0. This implies that newDuration is too short.")
        return ParabolicCurve()
    else:
        interval4 = iv.mpf([Neg(inf), Add(C, newDuration)])

    if IsEqual(sum2, zero):
        raise NotImplementedError  # not yet considered
    elif sum2 > epsilon:
        interval5 = iv.mpf([Neg(inf), Add(D, newDuration)])
    else:
        log.debug("sum2 < 0. This implies that newDuration is too short.")
        return ParabolicCurve()

    if Sub(interval5.a, interval4.b) > epsilon or Sub(interval4.a,
                                                      interval5.b) > epsilon:
        log.debug("interval4 and interval5 do not intersect each other")
        return ParabolicCurve()
    # interval6 = interval4 \cap interval5 : valid interval for t0 computed from a1's constraints
    interval6 = iv.mpf(
        [max(interval4.a, interval5.a),
         min(interval4.b, interval5.b)])

    # import IPython; IPython.embed()

    if Sub(interval3.a, interval6.b) > epsilon or Sub(interval6.a,
                                                      interval3.b) > epsilon:
        log.debug("interval3 and interval6 do not intersect each other")
        return ParabolicCurve()
    # interval7 = interval3 \cap interval6
    interval7 = iv.mpf(
        [max(interval3.a, interval6.a),
         min(interval3.b, interval6.b)])

    if Sub(interval0.a, interval7.b) > epsilon or Sub(interval7.a,
                                                      interval0.b) > epsilon:
        log.debug("interval0 and interval7 do not intersect each other")
        return ParabolicCurve()
    # interval8 = interval0 \cap interval7 : valid interval of t0 when considering all constraints (from a0 and a1)
    interval8 = iv.mpf(
        [max(interval0.a, interval7.a),
         min(interval0.b, interval7.b)])

    # import IPython; IPython.embed()

    # We choose the value t0 (the duration of the first ramp) by selecting the mid point of the
    # valid interval of t0.

    t0 = _SolveForT0(A, B, newDuration, interval8)
    if t0 is None:
        # The fancy procedure fails. Now consider no optimization whatsoever.
        # TODO: Figure out why solving fails.
        t0 = mp.convert(interval8.mid)  # select the midpoint
        # return ParabolicCurve()
    t1 = Sub(newDuration, t0)

    a0 = Add(A, Mul(mp.fdiv(one, t0), B))
    if (Abs(t1) < epsilon):
        a1 = zero
    else:
        a1 = Add(A, Mul(mp.fdiv(one, Neg(t1)), B))
    assert (Sub(Abs(a0), am) < epsilon
            )  # check if a0 is really below the bound
    assert (Sub(Abs(a1), am) < epsilon
            )  # check if a1 is really below the bound

    # import IPython; IPython.embed()

    # Check if the velocity bound is violated
    vp = Add(v0, Mul(a0, t0))
    if Abs(vp) > vm:
        vmnew = Mul(mp.sign(vp), vm)
        D2 = Prod([
            pointfive,
            Sqr(Sub(vp, vmnew)),
            Sub(mp.fdiv(one, a0), mp.fdiv(one, a1))
        ])
        # print "D2",
        # mp.nprint(D2, n=_prec)
        # print "vmnew",
        # mp.nprint(vmnew, n=_prec)
        A2 = Sqr(Sub(vmnew, v0))
        B2 = Neg(Sqr(Sub(vmnew, v1)))
        t0trimmed = mp.fdiv(Sub(vmnew, v0), a0)
        t1trimmed = mp.fdiv(Sub(v1, vmnew), a1)
        C2 = Sum([
            Mul(t0trimmed, Sub(vmnew, v0)),
            Mul(t1trimmed, Sub(vmnew, v1)),
            Mul(mp.mpf('-2'), D2)
        ])

        log.debug("\nA2 = {0}; \nB2 = {1}; \nC2 = {2}; \nD2 = {3};".format(
            mp.nstr(A2, n=_prec), mp.nstr(B2, n=_prec), mp.nstr(C2, n=_prec),
            mp.nstr(D2, n=_prec)))

        temp = Prod([A2, B2, B2])
        initguess = mp.sign(temp) * (Abs(temp)**(1. / 3.))
        root = mp.findroot(lambda x: Sub(Prod([x, x, x]), temp), x0=initguess)

        # import IPython; IPython.embed()
        log.debug("root = {0}".format(mp.nstr(root, n=_prec)))
        a0new = mp.fdiv(Add(A2, root), C2)
        if (Abs(a0new) > Add(am, epsilon)):
            if FuzzyZero(Sub(Mul(C2, a0new), A2), epsilon):
                # The computed a0new is exceeding the bound and its corresponding a1new is
                # zero. Therefore, there is no other way to fix this. This is probably because the
                # given newDuration is less than the minimum duration (x0, x1, v0, v1, vm, am) can
                # get.
                log.debug(
                    "abs(a0new) > am and a1new = 0; Cannot fix this case. This happens probably because the given newDuration is too short."
                )
                return ParabolicCurve()

            a0new = Mul(mp.sign(a0new), am)

        if (Abs(a0new) < epsilon):
            a1new = mp.fdiv(B2, C2)
            if (Abs(a1new) > Add(am, epsilon)):
                # Similar to the case above
                log.debug(
                    "a0new = 0 and abs(a1new) > am; Cannot fix this case. This happens probably because the given newDuration is too short."
                )
                return ParabolicCurve()

        else:
            if FuzzyZero(Sub(Mul(C2, a0new), A2), epsilon):
                # import IPython; IPython.embed()
                a1new = 0
            else:
                a1new = Mul(mp.fdiv(B2, C2),
                            Add(one, mp.fdiv(A2, Sub(Mul(C2, a0new), A2))))
                if (Abs(a1new) > Add(am, epsilon)):
                    a1new = Mul(mp.sign(a1new), am)
                    a0new = Mul(mp.fdiv(A2, C2),
                                Add(one, mp.fdiv(B2, Sub(Mul(C2, a1new), B2))))

        if (Abs(a0new) > Add(am, epsilon)) or (Abs(a1new) > Add(am, epsilon)):
            log.warn("Cannot fix acceleration bounds violation")
            return ParabolicCurve()

        log.debug(
            "\na0 = {0}; \na0new = {1}; \na1 = {2}; \na1new = {3};".format(
                mp.nstr(a0, n=_prec), mp.nstr(a0new, n=_prec),
                mp.nstr(a1, n=_prec), mp.nstr(a1new, n=_prec)))

        if (Abs(a0new) < epsilon) and (Abs(a1new) < epsilon):
            log.warn("Both accelerations are zero. Should we allow this case?")
            return ParabolicCurve()

        if (Abs(a0new) < epsilon):
            # This is likely because v0 is at the velocity bound
            t1new = mp.fdiv(Sub(v1, vmnew), a1new)
            assert (t1new > 0)
            ramp2 = Ramp(v0, a1new, t1new)

            t0new = Sub(newDuration, t1new)
            assert (t0new > 0)
            ramp1 = Ramp(v0, zero, t0new, x0)
            newCurve = ParabolicCurve([ramp1, ramp2])
            return newCurve

        elif (Abs(a1new) < epsilon):
            t0new = mp.fdiv(Sub(vmnew, v0), a0new)
            assert (t0new > 0)
            ramp1 = Ramp(v0, a0new, t0new, x0)

            t1new = Sub(newDuration, t0new)
            assert (t1new > 0)
            ramp2 = Ramp(ramp1.v1, zero, t1new)
            newCurve = ParabolicCurve([ramp1, ramp2])
            return newCurve

        else:
            # No problem with those new accelerations
            # import IPython; IPython.embed()
            t0new = mp.fdiv(Sub(vmnew, v0), a0new)
            if (t0new < 0):
                log.debug(
                    "t0new < 0. The given newDuration not achievable with the given bounds"
                )
                return ParabolicCurve()

            t1new = mp.fdiv(Sub(v1, vmnew), a1new)
            if (t1new < 0):
                log.debug(
                    "t1new < 0. The given newDuration not achievable with the given bounds"
                )
                return ParabolicCurve()

            if (Add(t0new, t1new) > newDuration):
                # Final fix. Since we give more weight to acceleration bounds, we make the velocity
                # bound saturated. Therefore, we set vp to vmnew.

                # import IPython; IPython.embed()
                if FuzzyZero(A, epsilon):
                    log.warn(
                        "(final fix) A is zero. Don't know how to fix this case"
                    )
                    return ParabolicCurve()

                t0new = mp.fdiv(Sub(Sub(vmnew, v0), B), A)
                if (t0new < 0):
                    log.debug("(final fix) t0new is negative")
                    return ParabolicCurve()

                t1new = Sub(newDuration, t0new)

                a0new = Add(A, Mul(mp.fdiv(one, t0new), B))
                a1new = Add(A, Mul(mp.fdiv(one, Neg(t1new)), B))
                ramp1 = Ramp(v0, a0new, t0new, x0)
                ramp2 = Ramp(ramp1.v1, a1new, t1new)
                newCurve = ParabolicCurve([ramp1, ramp2])

            else:
                ramp1 = Ramp(v0, a0new, t0new, x0)
                ramp3 = Ramp(ramp1.v1, a1new, t1new)
                ramp2 = Ramp(ramp1.v1, zero, Sub(newDuration,
                                                 Add(t0new, t1new)))
                newCurve = ParabolicCurve([ramp1, ramp2, ramp3])

                # import IPython; IPython.embed()

            return newCurve
    else:
        ramp1 = Ramp(v0, a0, t0, x0)
        ramp2 = Ramp(ramp1.v1, a1, t1)
        newCurve = ParabolicCurve([ramp1, ramp2])
        return newCurve
Esempio n. 4
0
def Ht_complex_root_finder(complex_guess, t):
    result = mp.findroot(lambda z: Ht_complex(z, t), mp.mpc(complex_guess))
    return result
Esempio n. 5
0
def Interpolate1DFixedDuration(x0, x1, v0, v1, newDuration, vm, am):
    x0 = ConvertFloatToMPF(x0)
    x1 = ConvertFloatToMPF(x1)
    v0 = ConvertFloatToMPF(v0)
    v1 = ConvertFloatToMPF(v1)
    vm = ConvertFloatToMPF(vm)
    am = ConvertFloatToMPF(am)
    newDuration = ConvertFloatToMPF(newDuration)
    log.debug("\nx0 = {0}; x1 = {1}; v0 = {2}; v1 = {3}; vm = {4}; am = {5}; newDuration = {6}".\
              format(mp.nstr(x0, n=_prec), mp.nstr(x1, n=_prec), mp.nstr(v0, n=_prec), mp.nstr(v1, n=_prec),
                     mp.nstr(vm, n=_prec), mp.nstr(am, n=_prec), mp.nstr(newDuration, n=_prec)))
    
    # Check inputs
    assert(vm > zero)
    assert(am > zero)

    if (newDuration < -epsilon):
        log.info("duration = {0} is negative".format(newDuration))
        return ParabolicCurve()
    if (newDuration <= epsilon):
        # Check if this is a stationary trajectory
        if (FuzzyEquals(x0, x1, epsilon) and FuzzyEquals(v0, v1, epsilon)):
            log.info("stationary trajectory")
            ramp0 = Ramp(v0, 0, 0, x0)
            newCurve = ParabolicCurve(ramp0)
            return newCurve
        else:
            log.info("newDuration is too short for any movement to be made")
            return ParabolicCurve()

    # Correct small discrepancies if any
    if (v0 > vm):
        if FuzzyEquals(v0, vm, epsilon):
            v0 = vm
        else:
            log.info("v0 > vm: {0} > {1}".format(v0, vm))
            return ParabolicCurve()
    elif (v0 < -vm):
        if FuzzyEquals(v0, -vm, epsilon):
            v0 = -vm
        else:
            log.info("v0 < -vm: {0} < {1}".format(v0, -vm))
            return ParabolicCurve()
    if (v1 > vm):
        if FuzzyEquals(v1, vm, epsilon):
            v1 = vm
        else:
            log.info("v1 > vm: {0} > {1}".format(v1, vm))
            return ParabolicCurve()
    elif (v1 < -vm):
        if FuzzyEquals(v1, -vm, epsilon):
            v1 = -vm
        else:
            log.info("v1 < -vm: {0} < {1}".format(v1, -vm))
            return ParabolicCurve()

    d = Sub(x1, x0)

    # First assume no velocity bound -> re-interpolated trajectory will have only two ramps.
    # Solve for a0 and a1 (the acceleration of the first and the last ramps).
    #         a0 = A + B/t0
    #         a1 = A + B/(t - t0)
    # where t is the (new) total duration, t0 is the (new) duration of the first ramp, and
    #         A = (v1 - v0)/t
    #         B = (2d/t) - (v0 + v1).
    newDurInverse = mp.fdiv(one, newDuration)
    A = Mul(Sub(v1, v0), newDurInverse)
    B = Sub(Prod([mp.mpf('2'), d, newDurInverse]), Add(v0, v1))

    interval0 = iv.mpf([zero, newDuration]) # initial interval for t0

    # Now consider the interval(s) computed from a0's constraints
    sum1 = Neg(Add(am, A))
    sum2 = Sub(am, A)
    C = mp.fdiv(B, sum1)
    D = mp.fdiv(B, sum2)

    log.debug("\nA = {0}; \nB = {1}; \nC = {2}; \nD = {3}; \nsum1 = {4}; \nsum2 = {5};".\
              format(mp.nstr(A, n=_prec), mp.nstr(B, n=_prec), mp.nstr(C, n=_prec), mp.nstr(D, n=_prec),
                     mp.nstr(sum1, n=_prec), mp.nstr(sum2, n=_prec)))

    if (sum1 > zero):
        # This implied that the duration is too short
        log.debug("the given duration ({0}) is too short.".format(newDuration))
        return ParabolicCurve()
    if (sum2 < zero):
        # This implied that the duration is too short
        log.debug("the given duration ({0}) is too short.".format(newDuration))
        return ParabolicCurve()
    
    if IsEqual(sum1, zero):
        raise NotImplementedError # not yet considered
    elif sum1 > epsilon:
        log.debug("sum1 > 0. This implies that newDuration is too short.")
        return ParabolicCurve()
    else:
        interval1 = iv.mpf([C, inf])
        
    if IsEqual(sum2, zero):
        raise NotImplementedError # not yet considered
    elif sum2 > epsilon:
        interval2 = iv.mpf([D, inf])
    else:
        log.debug("sum2 < 0. This implies that newDuration is too short.")
        return ParabolicCurve()
        
    if Sub(interval2.a, interval1.b) > epsilon or Sub(interval1.a, interval2.b) > epsilon:
        # interval1 and interval2 do not intersect each other
        return ParabolicCurve()    
    # interval3 = interval1 \cap interval2 : valid interval for t0 computed from a0's constraints
    interval3 = iv.mpf([max(interval1.a, interval2.a), min(interval1.b, interval2.b)])
    
    # Now consider the interval(s) computed from a1's constraints
    if IsEqual(sum1, zero):
        raise NotImplementedError # not yet considered
    elif sum1 > epsilon:
        log.debug("sum1 > 0. This implies that newDuration is too short.")
        return ParabolicCurve()
    else:
        interval4 = iv.mpf([Neg(inf), Add(C, newDuration)])
        
    if IsEqual(sum2, zero):
        raise NotImplementedError # not yet considered
    elif sum2 > epsilon:
        interval5 = iv.mpf([Neg(inf), Add(D, newDuration)])
    else:
        log.debug("sum2 < 0. This implies that newDuration is too short.")
        return ParabolicCurve()

    if Sub(interval5.a, interval4.b) > epsilon or Sub(interval4.a, interval5.b) > epsilon:
        log.debug("interval4 and interval5 do not intersect each other")
        return ParabolicCurve()
    # interval6 = interval4 \cap interval5 : valid interval for t0 computed from a1's constraints
    interval6 = iv.mpf([max(interval4.a, interval5.a), min(interval4.b, interval5.b)])

    # import IPython; IPython.embed()

    if Sub(interval3.a, interval6.b) > epsilon or Sub(interval6.a, interval3.b) > epsilon:
        log.debug("interval3 and interval6 do not intersect each other")
        return ParabolicCurve()
    # interval7 = interval3 \cap interval6
    interval7 = iv.mpf([max(interval3.a, interval6.a), min(interval3.b, interval6.b)])

    if Sub(interval0.a, interval7.b) > epsilon or Sub(interval7.a, interval0.b) > epsilon:
        log.debug("interval0 and interval7 do not intersect each other")
        return ParabolicCurve()
    # interval8 = interval0 \cap interval7 : valid interval of t0 when considering all constraints (from a0 and a1)
    interval8 = iv.mpf([max(interval0.a, interval7.a), min(interval0.b, interval7.b)])

    # import IPython; IPython.embed()
    
    # We choose the value t0 (the duration of the first ramp) by selecting the mid point of the
    # valid interval of t0.
    
    t0 = _SolveForT0(A, B, newDuration, interval8)
    if t0 is None:
        # The fancy procedure fails. Now consider no optimization whatsoever.
        # TODO: Figure out why solving fails.
        t0 = mp.convert(interval8.mid) # select the midpoint
        # return ParabolicCurve()
    t1 = Sub(newDuration, t0)

    a0 = Add(A, Mul(mp.fdiv(one, t0), B))
    if (Abs(t1) < epsilon):
        a1 = zero
    else:
        a1 = Add(A, Mul(mp.fdiv(one, Neg(t1)), B))
    assert(Sub(Abs(a0), am) < epsilon) # check if a0 is really below the bound
    assert(Sub(Abs(a1), am) < epsilon) # check if a1 is really below the bound

    # import IPython; IPython.embed()
    
    # Check if the velocity bound is violated    
    vp = Add(v0, Mul(a0, t0))
    if Abs(vp) > vm:
        vmnew = Mul(mp.sign(vp), vm)
        D2 = Prod([pointfive, Sqr(Sub(vp, vmnew)), Sub(mp.fdiv(one, a0), mp.fdiv(one, a1))])
        # print("D2", end=' ')
        # mp.nprint(D2, n=_prec)
        # print("vmnew", end=' ')
        # mp.nprint(vmnew, n=_prec)
        A2 = Sqr(Sub(vmnew, v0))
        B2 = Neg(Sqr(Sub(vmnew, v1)))
        t0trimmed = mp.fdiv(Sub(vmnew, v0), a0)
        t1trimmed = mp.fdiv(Sub(v1, vmnew), a1)
        C2 = Sum([Mul(t0trimmed, Sub(vmnew, v0)), Mul(t1trimmed, Sub(vmnew, v1)), Mul(mp.mpf('-2'), D2)])

        log.debug("\nA2 = {0}; \nB2 = {1}; \nC2 = {2}; \nD2 = {3};".format(mp.nstr(A2, n=_prec), mp.nstr(B2, n=_prec), mp.nstr(C2, n=_prec), mp.nstr(D2, n=_prec)))
        
        temp = Prod([A2, B2, B2])
        initguess = mp.sign(temp)*(Abs(temp)**(1./3.))
        root = mp.findroot(lambda x: Sub(Prod([x, x, x]), temp), x0=initguess)

        # import IPython; IPython.embed()
        log.debug("root = {0}".format(mp.nstr(root, n=_prec)))
        a0new = mp.fdiv(Add(A2, root), C2)
        if (Abs(a0new) > Add(am, epsilon)):
            if FuzzyZero(Sub(Mul(C2, a0new), A2), epsilon):
                # The computed a0new is exceeding the bound and its corresponding a1new is
                # zero. Therefore, there is no other way to fix this. This is probably because the
                # given newDuration is less than the minimum duration (x0, x1, v0, v1, vm, am) can
                # get.
                log.debug("abs(a0new) > am and a1new = 0; Cannot fix this case. This happens probably because the given newDuration is too short.")
                return ParabolicCurve()
            
            a0new = Mul(mp.sign(a0new), am)

        if (Abs(a0new) < epsilon):
            a1new = mp.fdiv(B2, C2)
            if (Abs(a1new) > Add(am, epsilon)):
                # Similar to the case above
                log.debug("a0new = 0 and abs(a1new) > am; Cannot fix this case. This happens probably because the given newDuration is too short.")
                return ParabolicCurve()

        else:
            if FuzzyZero(Sub(Mul(C2, a0new), A2), epsilon):
                # import IPython; IPython.embed()
                a1new = 0
            else:
                a1new = Mul(mp.fdiv(B2, C2), Add(one, mp.fdiv(A2, Sub(Mul(C2, a0new), A2))))
                if (Abs(a1new) > Add(am, epsilon)):
                    a1new = Mul(mp.sign(a1new), am)
                    a0new = Mul(mp.fdiv(A2, C2), Add(one, mp.fdiv(B2, Sub(Mul(C2, a1new), B2))))

        if (Abs(a0new) > Add(am, epsilon)) or (Abs(a1new) > Add(am, epsilon)):
            log.warn("Cannot fix acceleration bounds violation")
            return ParabolicCurve()        

        log.debug("\na0 = {0}; \na0new = {1}; \na1 = {2}; \na1new = {3};".format(mp.nstr(a0, n=_prec), mp.nstr(a0new, n=_prec), mp.nstr(a1, n=_prec), mp.nstr(a1new, n=_prec)))
        
        if (Abs(a0new) < epsilon) and (Abs(a1new) < epsilon):
            log.warn("Both accelerations are zero. Should we allow this case?")
            return ParabolicCurve()

        if (Abs(a0new) < epsilon):
            # This is likely because v0 is at the velocity bound
            t1new = mp.fdiv(Sub(v1, vmnew), a1new)
            assert(t1new > 0)
            ramp2 = Ramp(v0, a1new, t1new)

            t0new = Sub(newDuration, t1new)
            assert(t0new > 0)
            ramp1 = Ramp(v0, zero, t0new, x0)
            newCurve = ParabolicCurve([ramp1, ramp2])
            return newCurve

        elif (Abs(a1new) < epsilon):
            t0new = mp.fdiv(Sub(vmnew, v0), a0new)
            assert(t0new > 0)
            ramp1 = Ramp(v0, a0new, t0new, x0)
            
            t1new = Sub(newDuration, t0new)
            assert(t1new > 0)
            ramp2 = Ramp(ramp1.v1, zero, t1new)
            newCurve = ParabolicCurve([ramp1, ramp2])
            return newCurve

        else:
            # No problem with those new accelerations
            # import IPython; IPython.embed()
            t0new = mp.fdiv(Sub(vmnew, v0), a0new)
            if (t0new < 0):
                log.debug("t0new < 0. The given newDuration not achievable with the given bounds")
                return ParabolicCurve()
            
            t1new = mp.fdiv(Sub(v1, vmnew), a1new)
            if (t1new < 0):
                log.debug("t1new < 0. The given newDuration not achievable with the given bounds")
                return ParabolicCurve()
            
            if (Add(t0new, t1new) > newDuration):
                # Final fix. Since we give more weight to acceleration bounds, we make the velocity
                # bound saturated. Therefore, we set vp to vmnew.

                # import IPython; IPython.embed()
                if FuzzyZero(A, epsilon):
                    log.warn("(final fix) A is zero. Don't know how to fix this case")
                    return ParabolicCurve()

                t0new = mp.fdiv(Sub(Sub(vmnew, v0), B), A)
                if (t0new < 0):
                    log.debug("(final fix) t0new is negative")
                    return ParabolicCurve()

                t1new = Sub(newDuration, t0new)
                
                a0new = Add(A, Mul(mp.fdiv(one, t0new), B))
                a1new = Add(A, Mul(mp.fdiv(one, Neg(t1new)), B))
                ramp1 = Ramp(v0, a0new, t0new, x0)
                ramp2 = Ramp(ramp1.v1, a1new, t1new)
                newCurve = ParabolicCurve([ramp1, ramp2])

            else:
                ramp1 = Ramp(v0, a0new, t0new, x0)
                ramp3 = Ramp(ramp1.v1, a1new, t1new)
                ramp2 = Ramp(ramp1.v1, zero, Sub(newDuration, Add(t0new , t1new)))
                newCurve = ParabolicCurve([ramp1, ramp2, ramp3])
                
                # import IPython; IPython.embed()
            
            return newCurve
    else:    
        ramp1 = Ramp(v0, a0, t0, x0)
        ramp2 = Ramp(ramp1.v1, a1, t1)
        newCurve = ParabolicCurve([ramp1, ramp2])
        return newCurve
 def _getcusps(self):
     """Get cusps coordinates"""
     # precision parameters
     maxsteps = 300  # max iterations of mp.findroot
     tol = 1e-20  # tolerance in mp.findroot
     # compute (Ox) roots
     witt = [
         1., 2. * self.s, self.s**2 - 1., -2. * self.s / (1. + self.q),
         -(self.s**2 / (1. + self.q))
     ]
     zc = np.roots(witt)
     # topology: close
     if self.topo == 'close':
         self._yl = []
         method = 'illinois'
         dico_interp = self._enveloppes()
         # (Ox)
         arg = np.argsort(np.imag(zc))
         arg = arg[[1, 2]]
         zc = zc[arg]
         arg = np.argsort(np.real(zc))
         sort = zc[arg]
         ## central caustic
         # A, b3, [1, 1+q] -> Witt
         ccA = complex(np.real(sort[1]))
         # C, b4, [1, 1+1/q] -> Witt
         ccC = complex(np.real(sort[0]))
         # B, b3, [0, 1] -> poly
         loglbdmin, loglbdmax = dico_interp[
             'cb3B']  # Use interpolation on the envelopes to find the optimize search interval of lambda
         loglbdmax = np.log10(1.)
         self._br = 2
         loglbd = mp.findroot(
             self._fpoly, (mp.mpf(loglbdmin), mp.mpf(loglbdmax)),
             tol=tol,
             maxsteps=maxsteps,
             solver=method)  # Use mpmath to compute lbd near 1
         lbd = mp.power(10, loglbd)
         bn, sn = self._sortbnccu(lbd)
         ccB = complex(bn[2]) * self.s
         self._yl.append((3, lbd, ccB))
         # D -> conj(B)
         ccD = np.conj(ccB)
         ## secondary caustics
         bn, sn = self._sortbnccu(0.)
         zsadd = complex(bn[0])
         # E, b1, [0, ∞] -> poly
         loglbdmin, loglbdmax = dico_interp[
             'cb1']  # Use interpolation on the envelopes to find the optimize search interval of lambda
         self._br = 0
         loglbd = mp.findroot(self._fpoly,
                              (mp.mpf(loglbdmin), mp.mpf(loglbdmax)),
                              tol=tol,
                              maxsteps=maxsteps,
                              solver=method)
         lbd = mp.power(10, loglbd)
         bn, sn = self._sortbnccu(lbd)
         ccE = complex(bn[0])
         self._yl.append((1, lbd, ccE))
         ccE = ccE * self.s
         # H -> conj(E)
         ccH = np.conj(ccE)
         # F, b3, [1+q, ∞] -> poly
         loglbdmin, loglbdmax = dico_interp[
             'cb3F']  # Use interpolation on the envelopes to find the optimize search interval of lambda
         self._br = 2
         loglbd = mp.findroot(self._fpoly,
                              (mp.mpf(loglbdmin), mp.mpf(loglbdmax)),
                              tol=tol,
                              maxsteps=maxsteps,
                              solver=method)
         lbd = mp.power(10, loglbd)
         bn, sn = self._sortbnccu(lbd)
         ccF = complex(bn[2])
         self._yl.append((3, lbd, ccF))
         ccF = ccF * self.s
         # I -> conj(F)
         ccI = np.conj(ccF)
         # G, b5, [1+1/q, ∞] -> poly
         loglbdmin, loglbdmax = dico_interp[
             'cb5']  # Use interpolation on the envelopes to find the optimize search interval of lambda
         self._br = 4
         loglbd = mp.findroot(self._fpoly,
                              (mp.mpf(loglbdmin), mp.mpf(loglbdmax)),
                              tol=tol,
                              maxsteps=maxsteps,
                              solver=method)
         lbd = mp.power(10, loglbd)
         bn, sn = self._sortbnccu(lbd)
         ccG = complex(bn[4])
         self._yl.append((5, lbd, ccG))
         ccG = ccG * self.s
         # J -> conj(G)
         ccJ = np.conj(ccG)
         ## store cusps
         zcu = np.array([ccA, ccB, ccC, ccD])
         self.cusps['central'] = zcu - 1. / (
             1. + self.q) * (1. / np.conj(zcu) + self.q /
                             (np.conj(zcu) + self.s)) + self._AC2CM
         zcu = np.array([ccE, ccF, ccG])
         self.cusps['secondary_up'] = zcu - 1. / (
             1. + self.q) * (1. / np.conj(zcu) + self.q /
                             (np.conj(zcu) + self.s)) + self._AC2CM
         zcu = np.array([ccH, ccI, ccJ])
         self.cusps['secondary_down'] = zcu - 1. / (
             1. + self.q) * (1. / np.conj(zcu) + self.q /
                             (np.conj(zcu) + self.s)) + self._AC2CM
         # for cusp curves only
         self._zcu = np.array([
             ccA, ccB, ccC, ccD, ccE, ccF, ccG, ccH, ccI, ccJ
         ]) + self._AC2CM
     # topology: intermediate
     if self.topo == 'interm':
         self._yl = []
         method = 'illinois'
         dico_interp = self._enveloppes()
         # (Ox)
         #            zc = copy(self.zc[0] - self._AC2CM)
         arg = np.argsort(np.imag(zc))
         arg = arg[[1, 2]]
         zc = zc[arg]
         arg = np.argsort(np.real(zc))
         sort = zc[arg]
         ## resonant caustic
         # A, b3, [1, 1+q] -> Witt
         ccA = complex(np.real(sort[1]))
         # D, b4, [1, 1+1/q]  -> Witt
         ccD = complex(np.real(sort[0]))
         # B, b3, [1+q, ∞]
         loglbdmin, loglbdmax = dico_interp[
             'cb3']  # Use interpolation on the envelopes to find the optimize search interval of lambda
         loglbdmin = np.log10(1. + self.q)
         self._br = 2
         loglbd = mp.findroot(self._fpoly,
                              (mp.mpf(loglbdmin), mp.mpf(loglbdmax)),
                              tol=tol,
                              maxsteps=maxsteps,
                              solver=method)
         lbd = mp.power(10, loglbd)
         bn, sn = self._sortbnccu(lbd)
         ccB = complex(bn[2]) * self.s
         self._yl.append((3, lbd, ccB))
         # F -> conj(B)
         ccF = np.conj(ccB)
         # C, b5, [1+1/q, ∞]
         loglbdmin, loglbdmax = dico_interp[
             'cb5']  # Use interpolation on the envelopes to find the optimize search interval of lambda
         loglbdmin = np.log10(1. + 1. / self.q)
         self._br = 4
         loglbd = mp.findroot(self._fpoly,
                              (mp.mpf(loglbdmin), mp.mpf(loglbdmax)),
                              tol=tol,
                              maxsteps=maxsteps,
                              solver=method)
         lbd = mp.power(10, loglbd)
         bn, sn = self._sortbnccu(lbd)
         ccC = complex(bn[4])
         self._yl.append((5, lbd, ccC))
         ccC = ccC * self.s
         # E -> conj(C)
         ccE = np.conj(ccC)
         ## store cusps
         zcu = np.array([ccA, ccB, ccC, ccD, ccE, ccF])
         self.cusps['resonant'] = zcu - 1. / (
             1. + self.q) * (1. / np.conj(zcu) + self.q /
                             (np.conj(zcu) + self.s)) + self._AC2CM
         # for cusp curves only
         self._zcu = zcu + self._AC2CM
     # topology: wide
     if self.topo == 'wide':
         self._yl = []
         method = 'illinois'
         dico_interp = self._enveloppes()
         # (Ox)
         #            zc = copy(self.zc[0] - self._AC2CM)
         arg = np.argsort(np.real(zc))
         sort = zc[arg]
         ## central caustic
         # A, b3, [1, 1+q] -> Witt
         ccA = complex(np.real(sort[3]))
         # C, b6, [0, 1+q] -> Witt
         ccC = complex(np.real(sort[2]))
         # B, b3, [1+q, ∞] -> poly
         loglbdmin, loglbdmax = dico_interp[
             'cb3']  # Use interpolation on the envelopes to find the optimize search interval of lambda
         loglbdmin = np.log10(1. + self.q)
         self._br = 2
         loglbd = mp.findroot(self._fpoly,
                              (mp.mpf(loglbdmin), mp.mpf(loglbdmax)),
                              tol=tol,
                              maxsteps=maxsteps,
                              solver=method)
         lbd = mp.power(10, loglbd)
         bn, sn = self._sortbnccu(lbd)
         ccB = complex(bn[2]) * self.s
         self._yl.append((3, lbd, ccB))
         # D -> conj(B)
         ccD = np.conj(ccB)
         ## secondary caustic
         # E, b5,  [0, 1+1/q] -> Witt
         ccE = complex(np.real(sort[1]))
         # G, b4, [1, 1+1/q] -> Witt
         ccG = complex(np.real(sort[0]))
         # F, b5, [1+1/q, ∞] -> poly
         loglbdmin, loglbdmax = dico_interp[
             'cb5']  # Use interpolation on the envelopes to find the optimize search interval of lambda
         loglbdmin = np.log10(1. + 1. / self.q)
         self._br = 4
         loglbd = mp.findroot(self._fpoly,
                              (mp.mpf(loglbdmin), mp.mpf(loglbdmax)),
                              tol=tol,
                              maxsteps=maxsteps,
                              solver=method)
         lbd = mp.power(10, loglbd)
         bn, sn = self._sortbnccu(lbd)
         ccF = complex(bn[4])
         self._yl.append((5, lbd, ccF))
         ccF = ccF * self.s
         # H -> conj(F)
         ccH = np.conj(ccF)
         ## store cusps
         zcu = np.array([ccA, ccB, ccC, ccD])
         self.cusps['central'] = zcu - 1. / (
             1. + self.q) * (1. / np.conj(zcu) + self.q /
                             (np.conj(zcu) + self.s)) + self._AC2CM
         zcu = np.array([ccE, ccF, ccG, ccH])
         self.cusps['secondary'] = zcu - 1. / (
             1. + self.q) * (1. / np.conj(zcu) + self.q /
                             (np.conj(zcu) + self.s)) + self._AC2CM
         # for cusp curves only
         self._zcu = np.array([ccA, ccB, ccC, ccD, ccE, ccF, ccG, ccH
                               ]) + self._AC2CM
Esempio n. 7
0
def _Stretch1D(curve, newDuration, vm, am):

    log.debug("\nx0 = {0}; x1 = {1}; v0 = {2}; v1 = {3}; vm = {4}; am = {5}; prevDuration = {6}; newDuration = {7}".\
              format(mp.nstr(curve.x0, n=_prec), mp.nstr(curve.EvalPos(curve.duration), n=_prec),
                     mp.nstr(curve.v0, n=_prec), mp.nstr(curve.EvalVel(curve.duration), n=_prec),
                     mp.nstr(vm, n=_prec), mp.nstr(am, n=_prec), mp.nstr(curve.duration, n=_prec),
                     mp.nstr(newDuration, n=_prec)))
    
    # Check types
    if type(newDuration) is not mp.mpf:
        newDuration = mp.mpf("{:.15e}".format(newDuration))
    if type(vm) is not mp.mpf:
        vm = mp.mpf("{:.15e}".format(vm))
    if type(am) is not mp.mpf:
        am = mp.mpf("{:.15e}".format(am))

    # Check inputs
    # assert(newDuration > curve.duration)
    assert(vm > zero)
    assert(am > zero)

    if (newDuration < -epsilon):
        return ParabolicCurve()
    if (newDuration <= epsilon):
        # Check if this is a stationary trajectory
        if (FuzzyEquals(curve.x0, curve.EvalPos(x1), epsilon) and FuzzyEquals(curve.v0, curve.v1, epsilon)):
            ramp0 = Ramp(curve.v0, 0, 0, curve.x0)
            newCurve = ParabolicCurve(ramp0)
            return newCurve
        else:
            # newDuration is too short to any movement to be made
            return ParabolicCurve()

    v0 = curve[0].v0
    v1 = curve[-1].v1
    d = curve.d

    # First assume no velocity bound -> re-interpolated trajectory will have only two ramps.
    # Solve for a0 and a1 (the acceleration of the first and the last ramps).
    #         a0 = A + B/t0
    #         a1 = A + B/(t - t0)
    # where t is the (new) total duration, t0 is the (new) duration of the first ramp, and
    #         A = (v1 - v0)/t
    #         B = (2d/t) - (v0 + v1).
    newDurInverse = mp.fdiv(one, newDuration)
    A = Mul(Sub(v1, v0), newDurInverse)
    B = Sub(Prod([mp.mpf('2'), d, newDurInverse]), Add(v0, v1))

    interval0 = iv.mpf([zero, newDuration]) # initial interval for t0

    # Now consider the interval(s) computed from a0's constraints
    sum1 = Neg(Add(am, A))
    sum2 = Sub(am, A)
    C = mp.fdiv(B, sum1)
    D = mp.fdiv(B, sum2)

    log.debug("\nA = {0}; \nB = {1}; \nC = {2}; \nD = {3}; \nsum1 = {4}; \nsum2 = {5};".\
              format(mp.nstr(A, n=_prec), mp.nstr(B, n=_prec), mp.nstr(C, n=_prec), mp.nstr(D, n=_prec),
                     mp.nstr(sum1, n=_prec), mp.nstr(sum2, n=_prec)))

    assert(not (sum1 > zero))
    assert(not (sum2 < zero))
    
    if IsEqual(sum1, zero):
        raise NotImplementedError # not yet considered
    elif sum1 > epsilon:
        log.debug("sum1 > 0. This implies that newDuration is too short.")
        return ParabolicCurve()
    else:
        interval1 = iv.mpf([C, inf])
        
    if IsEqual(sum2, zero):
        raise NotImplementedError # not yet considered
    elif sum2 > epsilon:
        interval2 = iv.mpf([D, inf])
    else:
        log.debug("sum2 < 0. This implies that newDuration is too short.")
        return ParabolicCurve()
        
    if Sub(interval2.a, interval1.b) > epsilon or Sub(interval1.a, interval2.b) > epsilon:
        # interval1 and interval2 do not intersect each other
        return ParabolicCurve()    
    # interval3 = interval1 \cap interval2 : valid interval for t0 computed from a0's constraints
    interval3 = iv.mpf([max(interval1.a, interval2.a), min(interval1.b, interval2.b)])
    
    # Now consider the interval(s) computed from a1's constraints
    if IsEqual(sum1, zero):
        raise NotImplementedError # not yet considered
    elif sum1 > epsilon:
        log.debug("sum1 > 0. This implies that newDuration is too short.")
        return ParabolicCurve()
    else:
        interval4 = iv.mpf([Neg(inf), Add(C, newDuration)])
        
    if IsEqual(sum2, zero):
        raise NotImplementedError # not yet considered
    elif sum2 > epsilon:
        interval5 = iv.mpf([Neg(inf), Add(D, newDuration)])
    else:
        log.debug("sum2 < 0. This implies that newDuration is too short.")
        return ParabolicCurve()

    if Sub(interval5.a, interval4.b) > epsilon or Sub(interval4.a, interval5.b) > epsilon:
        # interval4 and interval5 do not intersect each other
        return ParabolicCurve()
    # interval6 = interval4 \cap interval5 : valid interval for t0 computed from a1's constraints
    interval6 = iv.mpf([max(interval4.a, interval5.a), min(interval4.b, interval5.b)])

    # import IPython; IPython.embed()

    if Sub(interval3.a, interval6.b) > epsilon or Sub(interval6.a, interval3.b) > epsilon:
        # interval3 and interval6 do not intersect each other
        return ParabolicCurve()
    # interval7 = interval3 \cap interval6
    interval7 = iv.mpf([max(interval3.a, interval6.a), min(interval3.b, interval6.b)])

    if Sub(interval0.a, interval7.b) > epsilon or Sub(interval7.a, interval0.b) > epsilon:
        # interval0 and interval7 do not intersect each other
        return ParabolicCurve()
    # interval8 = interval0 \cap interval7 : valid interval of t0 when considering all constraints (from a0 and a1)
    interval8 = iv.mpf([max(interval0.a, interval7.a), min(interval0.b, interval7.b)])

    # import IPython; IPython.embed()
    
    # We choose the value t0 (the duration of the first ramp) by selecting the mid point of the
    # valid interval of t0.
    
    t0 = _SolveForT0(A, B, newDuration, interval8)
    if t0 is None:
        # The fancy procedure fails. Now consider no optimization whatsoever.
        # TODO: Figure out why solving fails.
        t0 = mp.convert(interval8.mid) # select the midpoint
        # return ParabolicCurve()
    t1 = Sub(newDuration, t0)

    a0 = Add(A, Mul(mp.fdiv(one, t0), B))
    if (Abs(t1) < epsilon):
        a1 = zero
    else:
        a1 = Add(A, Mul(mp.fdiv(one, Neg(t1)), B))
    assert(Sub(Abs(a0), am) < epsilon) # check if a0 is really below the bound
    assert(Sub(Abs(a1), am) < epsilon) # check if a1 is really below the bound

    # import IPython; IPython.embed()
    
    # Check if the velocity bound is violated    
    vp = Add(v0, Mul(a0, t0))
    if Abs(vp) > vm:
        vmnew = Mul(mp.sign(vp), vm)
        D2 = Prod([pointfive, Sqr(Sub(vp, vmnew)), Sub(mp.fdiv(one, a0), mp.fdiv(one, a1))])
        # print "D2",
        # mp.nprint(D2, n=_prec)
        # print "vmnew",
        # mp.nprint(vmnew, n=_prec)
        A2 = Sqr(Sub(vmnew, v0))
        B2 = Neg(Sqr(Sub(vmnew, v1)))
        t0trimmed = mp.fdiv(Sub(vmnew, v0), a0)
        t1trimmed = mp.fdiv(Sub(v1, vmnew), a1)
        C2 = Sum([Mul(t0trimmed, Sub(vmnew, v0)), Mul(t1trimmed, Sub(vmnew, v1)), Mul(mp.mpf('-2'), D2)])

        log.debug("\nA2 = {0}; \nB2 = {1}; \nC2 = {2}; \nD2 = {3};".format(mp.nstr(A2, n=_prec), mp.nstr(B2, n=_prec), mp.nstr(C2, n=_prec), mp.nstr(D2, n=_prec)))
        
        temp = Prod([A2, B2, B2])
        initguess = mp.sign(temp)*(Abs(temp)**(1./3.))
        root = mp.findroot(lambda x: Sub(Prod([x, x, x]), temp), x0=initguess)

        # import IPython; IPython.embed()
        log.debug("root = {0}".format(mp.nstr(root, n=_prec)))
        a0new = mp.fdiv(Add(A2, root), C2)
        if (Abs(a0new) > Add(am, epsilon)):
            a0new = Mul(mp.sign(a0new), am)

        if (Abs(a0new) < epsilon):
            a1new = mp.fdiv(B2, C2)
            if (Abs(a1new) > Add(am, epsilon)):
                log.warn("abs(a1new) > am; cannot fix this case")
                # Cannot fix this case
                return ParabolicCurve()

        else:
            if FuzzyZero(Sub(Mul(C2, a0new), A2), epsilon):
                # import IPython; IPython.embed()
                a1new = 0
            else:
                a1new = Mul(mp.fdiv(B2, C2), Add(one, mp.fdiv(A2, Sub(Mul(C2, a0new), A2))))
                if (Abs(a1new) > Add(am, epsilon)):
                    a1new = Mul(mp.sign(a1new), am)
                    a0new = Mul(mp.fdiv(A2, C2), Add(one, mp.fdiv(B2, Sub(Mul(C2, a1new), B2))))

        if (Abs(a0new) > Add(am, epsilon)) or (Abs(a1new) > Add(am, epsilon)):
            log.warn("Cannot fix acceleration bounds violation")
            return ParabolicCurve()        

        log.debug("\na0 = {0}; \na0new = {1}; \na1 = {2}; \na1new = {3};".format(mp.nstr(a0, n=_prec), mp.nstr(a0new, n=_prec), mp.nstr(a1, n=_prec), mp.nstr(a1new, n=_prec)))
        
        if (Abs(a0new) < epsilon) and (Abs(a1new) < epsilon):
            log.warn("Both accelerations are zero. Should we allow this case?")
            return ParabolicCurve()

        if (Abs(a0new) < epsilon):
            # This is likely because v0 is at the velocity bound
            t1new = mp.fdiv(Sub(v1, vmnew), a1new)
            assert(t1new > 0)
            ramp2 = Ramp(v0, a1new, t1new)

            t0new = Sub(newDuration, t1new)
            assert(t0new > 0)
            ramp1 = Ramp(v0, zero, t0new, curve.x0)
            newCurve = ParabolicCurve([ramp1, ramp2])
            return newCurve

        elif (Abs(a1new) < epsilon):
            t0new = mp.fdiv(Sub(vmnew, v0), a0new)
            assert(t0new > 0)
            ramp1 = Ramp(v0, a0new, t0new, curve.x0)
            
            t1new = Sub(newDuration, t0new)
            assert(t1new > 0)
            ramp2 = Ramp(ramp1.v1, zero, t1new)
            newCurve = ParabolicCurve([ramp1, ramp2])
            return newCurve

        else:
            # No problem with those new accelerations
            # import IPython; IPython.embed()
            t0new = mp.fdiv(Sub(vmnew, v0), a0new)
            assert(t0new > 0)
            t1new = mp.fdiv(Sub(v1, vmnew), a1new)
            assert(t1new > 0)
            if (Add(t0new, t1new) > newDuration):
                # Final fix. Since we give more weight to acceleration bounds, we make the velocity
                # bound saturated. Therefore, we set vp to vmnew.

                # import IPython; IPython.embed()
                t0new = mp.fdiv(Sub(Sub(vmnew, v0), B), A)
                t1new = Sub(newDuration, t0new)
                assert(t1new > zero)
                a0new = Add(A, Mul(mp.fdiv(one, t0new), B))
                a1new = Add(A, Mul(mp.fdiv(one, Neg(t1new)), B))
                ramp1 = Ramp(v0, a0new, t0new, curve.x0)
                ramp2 = Ramp(ramp1.v1, a1new, t1new)
                newCurve = ParabolicCurve([ramp1, ramp2])

            else:
                # t0new = mp.fdiv(Sub(vmnew, v0), a0new)
                # assert(t0new > 0)
                ramp1 = Ramp(v0, a0new, t0new, curve.x0)
                
                # t1new = mp.fdiv(Sub(v1, vmnew), a1new)
                # assert(t1new > 0)
                ramp3 = Ramp(ramp1.v1, a1new, t1new)
                # print "a1",
                # mp.nprint(a1, n=_prec)
                # print "a1new",
                # mp.nprint(a1new, n=_prec)
                
                # print "t0trimmed",
                # mp.nprint(t0trimmed, n=_prec)
                # print "t0new",
                # mp.nprint(t0new, n=_prec)
                # print "t1trimmed", 
                # mp.nprint(t1trimmed, n=_prec)
                # print "t1new", 
                # mp.nprint(t1new, n=_prec)
                # print "T", 
                # mp.nprint(newDuration, n=_prec)
                
                ramp2 = Ramp(ramp1.v1, zero, Sub(newDuration, Add(t0new , t1new)))
                newCurve = ParabolicCurve([ramp1, ramp2, ramp3])
                
                # import IPython; IPython.embed()
            
            return newCurve
    else:    
        ramp1 = Ramp(v0, a0, t0, curve.x0)
        ramp2 = Ramp(ramp1.v1, a1, t1)
        newCurve = ParabolicCurve([ramp1, ramp2])
        return newCurve
Esempio n. 8
0
t = 0.0
evalfilename = get_file_path("Ht_eval_vlargez_"+str(t)+".csv")
Htrootfilename = get_file_path("Ht_roots_vlargez_"+str(t)+".csv")
evalrows = []
htroots = []
step_size = 0.01
prev_eval = 0.0
rootcount = 0
z = 25.0

for i in range(1, 5000000001):
    z += step_size
    curr_eval = (Ht_AFE_ABC(z, t).real)*mp.exp(mp.pi()*z/8)
    root_check = sign_change(curr_eval, prev_eval)
    if root_check == 1:
        try: approx_root = mp.findroot(lambda y: Ht_AFE_ABC(y,t).real,[z,z-step_size],solver="bisect")
        except: approx_root = z - step_size/2    
        print(approx_root)
        rootcount += 1
        htroots.append([t, rootcount, approx_root])
    evalrows.append([t, z, curr_eval, root_check])
    prev_eval = curr_eval
    if i % 10000 == 0:
        append_data(evalfilename, evalrows)
        evalrows = []
    if rootcount % 100 == 0:
        append_data(Htrootfilename, htroots)
        htroots = []


append_data(evalfilename, evalrows)
Esempio n. 9
0
t = 0.4
Htrootfilename = "Htroots_" + str(t) + ".csv"
htroots = []
rootcount = 0
known_root = 684130
midpoint_estimate_flag = 0
for i in range(1, 5000001):
    avggap = expected_zero_gap(t, known_root)
    interval_min, interval_max = known_root + avggap / 2, known_root + 3 * avggap / 2
    interval_min_eval = Ht_AFE_ADJ_AB(interval_min, t).real
    interval_max_eval = Ht_AFE_ADJ_AB(interval_max, t).real
    root_check = sign_change(interval_min_eval, interval_max_eval)
    if root_check == 1:
        try:
            approx_root = mp.findroot(lambda y: Ht_AFE_ADJ_AB(y, t).real,
                                      [interval_min, interval_max],
                                      solver="ridder")
            midpoint_estimate_flag = 0
        except:
            approx_root = (interval_min + interval_max) / 2
            midpoint_estimate_flag = 1
        print(approx_root)
        rootcount += 1
        htroots.append([
            t, rootcount, approx_root, interval_min, interval_max,
            midpoint_estimate_flag
        ])
        known_root = approx_root
    else:
        known_root += avggap
    if rootcount % 100 == 0: