コード例 #1
0
def iv_P_norm_expm(P_sqrt_T, M1, A, M2, tau):
    """
    Bound on P-ellipsoid norm of (M1 (expm(A*t) - I) M2)  for |t| < tau

    using the theorem in arXiv:1911.02537, section "Norm bounding of summands"

    @param P_sqrt_T: see iv_P_norm()
    """
    P_sqrt_T = iv.matrix(P_sqrt_T)
    M1 = iv.matrix(M1)
    A = iv.matrix(A)
    M2 = iv.matrix(M2)
    # coerce tau to maximum
    tau = abs(iv.mpf(tau)).b
    # P-ellipsoid norms
    M1_p = iv_P_norm(M=M1, P_sqrt_T=P_sqrt_T)
    M2_p = iv_P_norm(M=M2, P_sqrt_T=P_sqrt_T)
    A_p = iv_P_norm(M=A, P_sqrt_T=P_sqrt_T)
    # A_pow[i] = A ** i
    A_pow = _iv_matrix_powers(A)
    # Work around bug in mpmath, see comment in iv_P_norm()
    zero = iv.matrix(mp.zeros(len(A)))
    M1 = zero + M1
    # terms from [arXiv:1911.02537]
    M1_Ai_M2_p = lambda i: iv_P_norm(M=M1 @ A_pow[i] @ M2, P_sqrt_T=P_sqrt_T)
    gamma = lambda i: 1 / math.factorial(i) * (M1_Ai_M2_p(i) - M1_p * A_p ** i * M2_p)
    max_norm = sum([gamma(i) * (tau ** i) for i in range(1, IV_NORM_EVAL_ORDER + 1)]) + M1_p * M2_p * (iv.exp(A_p * tau) - 1)
    # the lower bound is always 0 (for t=0)
    return mp.mpi([0, max_norm.b])
コード例 #2
0
 def test_terminal(self):
     interval = iv.mpf([-0.005, 0.005])
     print(interval < 0)
     print(interval < 1)
     print(interval > -1)
     print(True or None)
     print(False or None)
コード例 #3
0
 def reset(self):
     self.state = tuple([iv.mpf([-0.005, 0.005]) for x in range(4)])
     self.steps_beyond_done = None
     return HyperRectangle.from_numpy(
         np.array(
             tuple([(float(x.a), float(x.b))
                    for i, x in enumerate(np.array(self.state))
                    ])).transpose())
コード例 #4
0
def iv_spectral_norm_rough(M):
    """
    Fast but rough interval bound of spectral norm.

    0 <= spectral_norm(M) <= sqrt(sum of all m[i,k]^2)
    """
    norm = iv.norm(M, 2)
    return iv.mpf([0, norm.b])
コード例 #5
0
 def assertInInterval(self, value, interval, relativeTolerance=0):
     """
     assert that value is in the given interval
     
     @param relativeTolerance: widen the interval by this factor
     """
     self.assertIn(
         value,
         interval * (1 + iv.mpf([-1, +1]) * mp.mpf(relativeTolerance)))
コード例 #6
0
    def __init__(self,a,b,dim=False,index=None):
        """Class for estimating the maximum curvature.
        Parameters
        ----------
            a (int) - the starting value of the interval
            b (int) - the ending value of the interval
            dim (bool or int) - False if this is not an interval for a dimension
                                integer indicating the number of dimensions
            index (int) - defines which dimension this interval corresponds to

        """
        self.iv = iv.mpf([a,b])
        self.iv_lambda = iv.mpf([0,0])
        if dim:
            assert isinstance(dim, int)
            assert isinstance(index, int) and 0<=index<dim
            self.iv_prime = np.array([iv.mpf([0,0]) for _ in range(dim)])
            self.iv_prime[index] = iv.mpf([1,1])
        else:
            self.iv_prime = iv.mpf([0,0])
コード例 #7
0
    def from_bounds(cls, lower=None, upper=None, lower_upper=None):
        """
        construct interval matrix from elementwise upper and lower bound

        either given as separate vectors `lower` and `upper`, or as 2-by-n numpy array lower_upper.
        """
        assert ((lower is None and upper is None) != (lower_upper is None))
        if lower_upper is not None:
            assert lower_upper.shape == (len(lower_upper), 2)
            lower = lower_upper[:, 0]
            upper = lower_upper[:, 1]
        return cls.convert(lower) + (cls.convert(upper) -
                                     cls.convert(lower)) * iv.mpf([0, 1])
コード例 #8
0
 def example_matrices(self, include_singular=True, random=100):
     examples = [
         iv.matrix([[1, 2], [4, 5]]) + iv.mpf([-1, +1]) * mp.mpf(1e-10),
         iv.eye(2) * 0.5,
         iv.diag([1, 1e-10]),
         iv.eye(2) * 1e-302
     ]
     for n in [1, 4, 17]:
         for i in range(random // n):
             examples.append(iv.matrix(mp.randmatrix(n)))
     if include_singular:
         examples.append(iv.matrix(mp.zeros(4)))
         examples.append(iv.diag([1, 1e-200]))
     return examples
コード例 #9
0
def iv_spectral_norm(M):
    """
    Good interval bound of spectral norm of a (interval) matrix

    Theorem 3.2 from

    Siegfried M. Rump. “Verified bounds for singular values, in particular for the
    spectral norm of a matrix and its inverse”. In: BIT Numerical Mathematics 51.2
    (Nov. 2010), pp. 367–384. DOI : 10.1007/s10543-010-0294-0.
    """
    M = iv.matrix(M)
    # imprecise SVD of M (no requirement on precision)
    # _, _, V_T = mp.svd(iv_matrix_mid_to_mp(M)) # <-- configurable precision
    _, _, V_T = scipy.linalg.svd(iv_matrix_mid_to_numpy_ndarray(M)) # <-- faster
    # in [Rump 2010], SVD is defined as M = U @ S @ V.T,
    # in mpmath it is M = U @ S @ V (not transposed) for U,S,V=svd(M)
    # in scipy it is effectively the same as in mpmath, M = U @ S @ Vh for U,S,Vh = svd(M)
    V = numpy_ndarray_to_mp_matrix(V_T).T
    # now, everything is named as in [Rump2010], except that here, A is called M.
    # all following computations are interval bounds
    V = iv.matrix(V)
    B = M @ V
    BTB = B.T @ B
    # split BTB such that DE = diagonal D + rest E
    D = BTB @ 0
    E = BTB @ 0
    for i in range(BTB.rows):
        for j in range(BTB.cols):
            if i == j:
                D[i,j] = BTB[i,j]
            else:
                E[i,j] = BTB[i,j]
    # upper bound of spectral norm of I - V.T @ V
    alpha = iv_spectral_norm_rough(iv.eye(len(M)) - V.T @ V)
    # upper bound of spectral norm of E
    epsilon = iv_spectral_norm_rough(E)
    # maximum of D[i,i]  (which are always >= 0)
    d_max = iv.norm(D, mp.inf)
    if alpha.b >= 1:
        # this shouldn't happen - even an imprecise SVD will roughly have V.T @ V = I.
        raise scipy.linalg.LinAlgError("Something's numerically wrong - the singular vectors are far from orthonormal")
        # should this ever happen in reality, a valid return value would be:
        # return iv.mpf([0, mp.inf])
    try:
        lower_bound = iv.sqrt((d_max - epsilon) / (1 + alpha)).a
    except mp.libmp.libmpf.ComplexResult:
        lower_bound=0;
    # note that d_max, epsilon,alpha are intervals, so everything in the following computation is interval arithmetic
    return iv.mpf([lower_bound, iv.sqrt((d_max + epsilon) / (1 - alpha)).b])
コード例 #10
0
ファイル: integer.py プロジェクト: morganastra/Mathics
 def apply(self, n, b, evaluation):
     'IntegerLength[n_, b_]'
     
     # Use interval arithmetic to account for "right" rounding
     
     n, b = n.get_int_value(), b.get_int_value()
     if n is None or b is None:
         evaluation.message('IntegerLength', 'int')
         return
     if b <= 1:
         evaluation.message('IntegerLength', 'base', b)
         return
      
     result = mp.mpf(iv.log(iv.mpf(mpz2mpmath(abs(n))), mpz2mpmath(b)).b)
     result = mpz(mpmath2gmpy(result)) + 1
     return Integer(result)
コード例 #11
0
def approx_P_norm_expm(P_sqrt_T, M1, A, M2, tau):
    """
    approximate max ( P_norm( M1 (expm(A*t) - I) M2 )  for |t| < tau )

    @param P_sqrt_T: see iv_P_norm()
    """
    P_sqrt_T = iv_matrix_mid_to_numpy_ndarray(P_sqrt_T)
    M1 = iv_matrix_mid_to_numpy_ndarray(M1)
    A = iv_matrix_mid_to_numpy_ndarray(A)
    M2 = iv_matrix_mid_to_numpy_ndarray(M2)
    # coerce tau to maximum
    tau = float(mp.mpf(abs(iv.mpf(tau)).b))
    max_norm = 0
    for t in numpy.linspace(-tau, tau, 100):
        matrix = numpy.matmul(M1, numpy.matmul(scipy.linalg.expm(A*t) - numpy.eye(len(A)), M2))
        max_norm = max(max_norm, approx_P_norm(M=matrix, P_sqrt_T=P_sqrt_T))
    return max_norm
コード例 #12
0
ファイル: interpolation.py プロジェクト: labimage/openrave
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
コード例 #13
0
ファイル: interpolation.py プロジェクト: EdsterG/openrave
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
コード例 #14
0
ファイル: interpolation.py プロジェクト: AbuShaqra/openrave
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
コード例 #15
0
 def test_matrix_abs_max(self):
     A = iv.matrix([[1, 2], [4, 5]]) + iv.mpf([-1, +1]) * mp.mpf(1e-10)
     assert mp.matrix([[1, 2], [4, 5]]) + mp.mpf(1e-10) == matrix_abs_max(A)
     assert mp.matrix([[1, 2], [4, 5]
                       ]) + mp.mpf(1e-10) == matrix_abs_max(-A)
コード例 #16
0
 def set_state(self, state: Tuple[Tuple]):
     self.state = tuple([iv.mpf([x[0], x[1]]) for x in state])
コード例 #17
0
 def test_membership(self):
     assert iv.mpf([0.5, 0.5]) in iv.mpf([0.5, 0.5])
コード例 #18
0
def beta(a,b):
    return iv.mpf([-1,1])*iv.sqrt(lambda_s(a)*lambda_s(b))
コード例 #19
0
def lambda_s(a):
    return sum(iv.mpf([0,1])*max(ai.a**2,ai.b**2) for ai in a)
コード例 #20
0
    def run_analysis_continuized(self, name, outdir):
        """
        Run Analysis with Continuization
        see run_analysis()

        WARNING: this will modify parameters in self.s (TODO fix that)
        """
        print("")
        print("====================")
        print("Analysing System with Continuization: " + name)
        assert self.s.immediate_ctrl
        # FIXME: this will modify self.s (which is okay for how this function currently used, but may be annoying in the future)
        self.s.continuize = True

        # compute continuized dynamics
        self.s.A_continuized_p_p = self.s.A_p + self.s.B_p @ self.s.B_d
        self.s.A_continuized_p_c_tilde = self.s.B_p @ self.s.A_d
        self.s.A_continuized_p_delta_p = self.s.B_p @ self.s.B_d
        self.s.A_continuized_p_delta_c = self.s.A_continuized_p_c_tilde
        self.s.A_continuized_c_tilde_p = 1/self.s.T * self.s.B_d
        self.s.A_continuized_c_tilde_c_tilde = 1/self.s.T * (self.s.A_d - np.eye(self.s.n_d))
        self.s.A_continuized_c_tilde_delta_p = self.s.A_continuized_c_tilde_p
        self.s.A_continuized_c_tilde_delta_c = self.s.A_continuized_c_tilde_c_tilde

        # build block matrices
        # d/dt ([x_p; x_c_tilde]) = A_continuized_total @ [x_p; x_c_tilde] + B_continuized_total @ [delta_p; delta_c]
        self.s.A_continuized_total = \
            np.block([[self.s.A_continuized_p_p, self.s.A_continuized_p_c_tilde],
                      [self.s.A_continuized_c_tilde_p, self.s.A_continuized_c_tilde_c_tilde]])
        self.s.B_continuized_total = \
            np.block([[self.s.A_continuized_p_delta_p, self.s.A_continuized_p_delta_c],
                      [self.s.A_continuized_c_tilde_delta_p, self.s.A_continuized_c_tilde_delta_c]])
        print("Eigenvalues of continuized system without delta:")
        print(scipy.linalg.eig(self.s.A_continuized_total)[0])


        self.s.spaceex_iterations = 1

        # Note: SpaceEx uses unsound numerics (at least in the default settings).
        # Epsilon here does not strictly guarantee soundness from a mathematical point of view (although it makes engineers sleep well).
        eps = iv.mpf([-1e-10, 1e-10])
        # \underline{\Delta}
        delta_pc_assumed_without_eps = IntervalMatrix.zeros(self.s.n_p + self.s.n_d, 1)
        i = 0
        final_run = False
        while True:
            i += 1
            print(f"\n\n\n# Iteration {i}")
            if final_run:
                print("(Final run - bound was already proven, now we generate all the plots and results)")
            # run analysis
            # NOTE: self.results now refers to continuized results
            # \Delta = \underline{\Delta} + B_\epsilon   (overapproximated as interval)
            delta_pc_assumed = delta_pc_assumed_without_eps + eps
            print("assumed bound for delta_p, delta_c:")
            print(delta_pc_assumed)
            self.s.delta_p_assumed = delta_pc_assumed[0:self.s.n_p]
            self.s.delta_c_assumed = delta_pc_assumed[self.s.n_p:(self.s.n_p + self.s.n_d)]
            self.run_analysis(name, outdir, extra_plots=final_run, print_header=False)
            if self.results['spaceex_hypy']['code'] != hypy.Engine.SUCCESS:
                print("SpaceEx failed. Giving up.")
                self.results['continuization'] = 'fail (SpaceEx errored)'
                return

            # compute delta-bounds
            x_pc_intv = IntervalMatrix.from_bounds(lower_upper=spaceex_bounds_to_array(self.results['spaceex_hypy']['output']['variables'])[0:self.s.n_p + self.s.n_d])
            delta_pc_actual = iv.mpf([0, 1]) * self.s.T * (-1) * (IntervalMatrix.convert(self.s.A_continuized_total) @ (x_pc_intv + eps) + IntervalMatrix.convert(self.s.B_continuized_total) @ delta_pc_assumed)
            print("resulting actual bound for delta_p, delta_c:")
            print(delta_pc_actual)
            # check delta-bounds

            if final_run:
                # this was the last run - see below for conditions
                assert all([(delta_pc_actual[i]) in delta_pc_assumed_without_eps[i] for i in range(len(delta_pc_assumed))])
                print("successfully analyzed")
                self.results['continuization'] = 'success'
                return
            elif all([(delta_pc_actual[i]) in delta_pc_assumed_without_eps[i] for i in range(len(delta_pc_assumed))]):
                print("assumed interval is correct. Done. Re-running analysis with final bounds.")
                delta_pc_assumed_without_eps = delta_pc_actual
                final_run = True
            elif i < 5:
                print("assumed interval for delta_p and/or delta_c is to small. Enlarging.")
                delta_pc_assumed_without_eps = delta_pc_actual * iv.mpf([0, 2])
            else:
                print("Iteration did not converge. System is possibly unstable or otherwise unsuitable for Continuization.")
                self.results['continuization'] = 'fail (no convergence)'
                return