def TOPPInterface(traj0, W):
    Ndim = 2
    Adim = 2
    discrtimestep = 1e-3
    ndiscrsteps = int((traj0.duration + 1e-10) / discrtimestep) + 1
    q = np.zeros((Ndim, ndiscrsteps))
    qs = np.zeros((Ndim, ndiscrsteps))
    qss = np.zeros((Ndim, ndiscrsteps))
    a = np.zeros((ndiscrsteps, 2 * Adim))
    b = np.zeros((ndiscrsteps, 2 * Adim))
    c = np.zeros((ndiscrsteps, 2 * Adim))
    F = np.zeros((ndiscrsteps, Ndim))

    tvect = arange(0, traj0.duration + discrtimestep, discrtimestep)
    umax = np.array((1, 1))
    umin = np.array((-1, -1))
    Fborder = 0.2
    for i in range(ndiscrsteps):
        t = i * discrtimestep

        q[:, i] = traj0.Eval(t)
        qs[:, i] = traj0.Evald(t)
        qss[:, i] = traj0.Evaldd(t)

        Fi = np.array((0, 0))
        if q[0, i] < Fborder:
            Fi = np.array((1.5, 0))

        I = np.eye(Ndim)
        G = np.vstack((I, -I))
        h = np.hstack((-Fi - umax, Fi + umin))
        a[i, :] = np.dot(G, qs[:, i]).flatten()
        b[i, :] = np.dot(G, qss[:, i]).flatten()
        c[i, :] = h

    vmax = 1e5 * np.ones(Ndim)
    topp_inst = TOPP.QuadraticConstraints(traj0, discrtimestep, vmax, list(a),
                                          list(b), list(c))
    x = topp_inst.solver
    #ret = x.RunComputeProfiles(0,0)
    ret = x.RunVIP(0, 0)
    if ret == 1:
        print "TOPP VIP solution:", x.sdendmin, x.sdendmax
    else:
        print "TOPP VIP failure"
        def getSpeedIntervalAtPoint(self, N):
                if (N < 0) or (N > self.Nwaypoints):
                        print "Critical Point",N,"is out of bounds of trajectory"
                        sys.exit(1)
                if N == 0:
                        return [0,0]

                Wn = self.waypoints[:,0:N]
                trajN = Utilities.InterpolateViapoints(Wn)
                [a,b,c] = self.GetABC(trajN, self.discrtimestep)
                vmax = 1e5*np.ones(self.Ndim)

                topp_inst = TOPP.QuadraticConstraints(trajN, self.discrtimestep, vmax, list(a), list(b), list(c))
                x = topp_inst.solver
                ret = x.RunVIP(0.0, 0.0)

                if ret != 1:
                        semin = 0.0
                        semax = 0.0
                else:
                        semin = x.sdendmin
                        semax = x.sdendmax
                        if semin<=1e-5:
                                ret2 = x.RunComputeProfiles(0.0, semin)
                                if ret2 != 1:
                                        #print N,ret,ret2,semin,semax
                                        #np.savetxt('topp/a2',a)
                                        #np.savetxt('topp/b2',b)
                                        #np.savetxt('topp/c2',c)

                                        #tstr = "trajectorystring=\"\"\" %s \"\"\""%(str(trajN))
                                        #with open("topp/traj2", "w") as fh:
                                        #        fh.write("%s" % str(trajN))
                                        self.SaveToFile('vip')

                                        sys.exit(0)


                return [semin, semax]
        def initializeFromSpecifications(self, W):
                self.filename = 'images/topp_'+str(params.FILENAME)+'.png'

                self.Ndim = W.shape[0]
                self.Nwaypoints = W.shape[1]
                self.traj0 = Utilities.InterpolateViapoints(W)
                self.length = self.traj0.duration

                dendpoint = np.linalg.norm(self.traj0.Eval(self.length)-W[:,-1])
                if dendpoint > self.TRAJECTORY_ACCURACY_REQUIRED:
                        print "###############"
                        print "TOPP INTERFACE ERROR"
                        print "path duration:",self.length
                        print "###############"
                        print "FINAL POINT on piecewise C^2 traj:",self.traj0.Eval(self.length)
                        print "FINAL WAYPOINT                   :",W[:,-1]
                        print "distance between points:",dendpoint
                        print "###############"
                        sys.exit(1)

                [a,b,c] = self.GetABC(self.traj0, self.discrtimestep)
                vmax = 1e5*np.ones(self.Ndim)
                self.topp_inst = TOPP.QuadraticConstraints(self.traj0, self.discrtimestep, vmax, list(a), list(b), list(c))
                self.waypoints = W
def TOPPInterface(Ndim, trajectorystring, discrtimestep, a, b, c):
    vmax = 1e5 * np.ones(Ndim)  ## no velocity constraints
    topp_inst = TOPP.QuadraticConstraints(trajectorystring, discrtimestep,
                                          vmax, list(a), list(b), list(c))
    x = topp_inst.solver

    ##### compute all possible speeds at end of traj
    ret = x.RunVIP(0, 0)
    print "discrtimestep", discrtimestep, "TOPP RunVIP             (0,0) code:", ret,
    if ret == 1:
        print "(Success)",
        semin = x.sdendmin
        semax = x.sdendmax
        print "sd_end: [", semin, ",", semax, "]"

        ##### compute speed profile for some speeds inside sd_end
        for speed in np.linspace(semin, semax, 5):
            ret = x.RunComputeProfiles(0, speed)
            print "discrtimestep", discrtimestep, "TOPP RunComputeProfiles (0,", speed, ") code:", ret,

            if ret == 1:
                print "(Success)"
            else:
                print "(Failure)"
예제 #5
0
def Shortcut(robot,
             taumax,
             vmax,
             lietraj,
             maxiter,
             expectedduration=-1,
             meanduration=0,
             upperlimit=-1,
             inertia=None,
             trackingplot=None):
    if trackingplot == 1:
        plt.axis([0, maxiter, 0, lietraj.duration])
        plt.ion()
        plt.show()
        ylabel('Trajectory duration (s)')
        xlabel('Iteration')

    t_sc_start = time.time()
    originalduration = lietraj.duration
    #return shortcuted traj
    if upperlimit < 0:
        dur = lietraj.duration
        upperlimit = lietraj.duration
    else:
        dur = upperlimit
    attempt = 0

    ## for shortcutting
    integrationtimestep = 1e-2
    reparamtimestep = 1e-2
    passswitchpointnsteps = 5
    discrtimestep = 1e-2

    constraintsstring = str(discrtimestep)
    constraintsstring += "\n" + ' '.join([str(v) for v in taumax])
    if not (inertia is None):
        for v in inertia:
            constraintsstring += "\n" + ' '.join([str(i) for i in v])

    assert (dur > 10.0 * discrtimestep)

    ncollision = 0
    nnotretimable = 0
    nnotshorter = 0

    for it in range(maxiter):
        if trackingplot == 1:
            plt.scatter(it, lietraj.duration)
            plt.draw()
        if (
                expectedduration > 0
        ):  # check, if newlietraj.duration is short enough, stop SHORTCUTING
            if (lietraj.duration < expectedduration):
                print "\033[1;32mTrajectory's duration is already shorter than expected time --> stop shortcuting\033[0m"
                break
        if (dur < discrtimestep):
            print "[Utils::Shortcut] trajectory duration is less than discrtimestep.\n"
            break  ## otherwise, this will cause an error in TOPP

        ## select an interval for shortcutting
        t0 = random.rand() * dur

        if meanduration == 0:
            meanduration = dur - t0

        T = random.rand() * min(meanduration, dur - t0)
        t1 = t0 + T

        while (T < 2.0 * discrtimestep):
            t0 = random.rand() * dur
            if meanduration == 0:
                meanduration = dur - t0

            T = random.rand() * min(meanduration, dur - t0)
            t1 = t0 + T

            if t1 > upperlimit:
                t1 = upperlimit
                if (t1 < t0):
                    temp = t0
                    t0 = t1
                    t1 = temp
                    T = t1 - t0

        # print "\n\nShortcutting iteration", it + 1
        # print t0, t1, t1- t0
        # interpolate from t0 to t1
        R_beg = lietraj.EvalRotation(t0)
        R_end = lietraj.EvalRotation(t1)
        omega0 = lietraj.EvalOmega(t0)
        omega1 = lietraj.EvalOmega(t1)

        shortcuttraj = lie.InterpolateSO3(R_beg, R_end, omega0, omega1, T)
        #check feasibility only for the new portion

        isincollision = CheckCollisionTraj(robot, shortcuttraj, R_beg,
                                           discrtimestep)
        if (not isincollision):
            # a,b,c = lie.ComputeSO3Constraints(shortcuttraj, taumax, discrtimestep)
            abc = TOPPbindings.RunComputeSO3Constraints(
                str(shortcuttraj), constraintsstring)  # discrtimestep)
            a, b, c = lie.Extractabc(abc)

            topp_inst = TOPP.QuadraticConstraints(shortcuttraj, discrtimestep,
                                                  vmax, list(a), list(b),
                                                  list(c))
            x = topp_inst.solver
            ret = x.RunComputeProfiles(1, 1)
            if (ret == 1):
                x.resduration
                ## check whether the new one has shorter duration
                if (x.resduration + 0.01 < T):  #skip if not shorter than 0.3 s

                    x.ReparameterizeTrajectory()
                    x.WriteResultTrajectory()
                    TOPPed_shortcuttraj = Trajectory.PiecewisePolynomialTrajectory.FromString(
                        x.restrajectorystring)

                    newlietraj = ReplaceTrajectorySegment(
                        lietraj, TOPPed_shortcuttraj, t0, t1)
                    lietraj = newlietraj
                    dur = lietraj.duration
                    #print "*******************************************"
                    print "Success at iteration", it + 1, ":", t0, t1, "Deta_t:", t1 - t0 - x.resduration
                    attempt += 1
                    #print "T:", nnotretimable, "; S:", nnotshorter , "; C:", ncollision , "; OK:", attempt
                    #print "*******************************************"
                else:
                    # print "Not shorter"
                    nnotshorter += 1
            else:
                # print "Not retimable"
                nnotretimable += 1
        else:
            # print "Collision"
            ncollision += 1

            # print "T:", nnotretimable, "; S:", nnotshorter , "; C:", ncollision , "; OK:", attempt

    print "\033[1;32mT:", nnotretimable, "; S:", nnotshorter, "; C:", ncollision, "; OK:", attempt, "\033[0m"
    print "\033[1;32m", originalduration - lietraj.duration, "sec. shorter\033[0m"
    t_sc_end = time.time()
    print "\033[1;32mRunning time:", t_sc_end - t_sc_start, "sec.\033[0m"

    return lietraj
예제 #6
0
def SE3Shortcut(robot,
                taumax,
                fmax,
                vmax,
                se3traj,
                Rlist,
                maxiter,
                expectedduration=-1,
                meanduration=0,
                upperlimit=-1,
                plotdura=None):
    if plotdura == 1:
        plt.axis([0, maxiter, 0, se3traj.duration])
        plt.ion()
        plt.show()
        ylabel('Trajectory duration (s)')
        xlabel('Iteration')

    t_sc_start = time.time()
    originalduration = se3traj.duration
    #return shortcuted traj
    if upperlimit < 0:
        dur = se3traj.duration
        upperlimit = se3traj.duration
    else:
        dur = upperlimit
    attempt = 0

    ## for shortcutting
    integrationtimestep = 1e-2
    reparamtimestep = 1e-2
    passswitchpointnsteps = 5
    discrtimestep = 1e-2
    assert (dur > 10.0 * discrtimestep)

    ncollision = 0
    nnotretimable = 0
    nnotshorter = 0

    transtraj, rtraj = TransRotTrajFromSE3Traj(se3traj)
    lietraj = lie.SplitTraj(Rlist, rtraj)

    for it in range(maxiter):
        if plotdura == 1:
            plt.scatter(it, se3traj.duration)
            plt.draw()
        #transtraj, rtraj = TransRotTrajFromSE3Traj(se3traj)
        #lietraj = lie.SplitTraj2(Rlist, rtraj)
        if (
                expectedduration > 0
        ):  # check, if newlietraj.duration is short enough, stop SHORTCUTING
            if (se3traj.duration < expectedduration):
                print "\033[1;32mTrajectory's duration is already shorter than expected time --> stop shortcuting\033[0m"
                break
        if (dur < discrtimestep):
            print "[Utils::Shortcut] trajectory duration is less than discrtimestep.\n"
            break  ## otherwise, this will cause an error in TOPP

        ## select an interval for shortcutting
        t0 = random.rand() * dur

        if meanduration == 0:
            meanduration = dur - t0

        T = random.rand() * min(meanduration, dur - t0)
        t1 = t0 + T

        while (T < 2.0 * discrtimestep):
            t0 = random.rand() * dur
            if meanduration == 0:
                meanduration = dur - t0

            T = random.rand() * min(meanduration, dur - t0)
            t1 = t0 + T

            if t1 > upperlimit:
                t1 = upperlimit
                if (t1 < t0):
                    temp = t0
                    t0 = t1
                    t1 = temp
                    T = t1 - t0

        # print "\n\nShortcutting iteration", it + 1
        # print t0, t1, t1- t0
        # interpolate from t0 to t1
        R_beg = lietraj.EvalRotation(t0)
        R_end = lietraj.EvalRotation(t1)
        omega0 = lietraj.EvalOmega(t0)
        omega1 = lietraj.EvalOmega(t1)
        shortcutrtraj = lie.InterpolateSO3(R_beg, R_end, omega0, omega1, T)

        t_beg = transtraj.Eval(t0)
        t_end = transtraj.Eval(t1)
        v_beg = transtraj.Evald(t0)
        v_end = transtraj.Evald(t1)

        shortcuttranstraj = Trajectory.PiecewisePolynomialTrajectory.FromString(
            TrajString3rdDegree(t_beg, t_end, v_beg, v_end, T))

        shortcutse3traj = SE3TrajFromTransandSO3(shortcuttranstraj,
                                                 shortcutrtraj)
        #check feasibility only for the new portion

        isincollision = CheckCollisionSE3Traj(robot, shortcuttranstraj,
                                              shortcutrtraj, R_beg,
                                              discrtimestep)
        if (not isincollision):
            a, b, c = ComputeSE3Constraints(shortcutse3traj, taumax, fmax,
                                            discrtimestep)
            topp_inst = TOPP.QuadraticConstraints(shortcutse3traj,
                                                  discrtimestep, vmax, list(a),
                                                  list(b), list(c))
            x = topp_inst.solver
            ret = x.RunComputeProfiles(1, 1)
            if (ret == 1):
                x.resduration
                ## check whether the new one has shorter duration
                if (x.resduration + 0.1 < T):  #skip if not shorter than 0.1 s

                    x.ReparameterizeTrajectory()
                    x.WriteResultTrajectory()
                    TOPPed_shortcutse3traj = Trajectory.PiecewisePolynomialTrajectory.FromString(
                        x.restrajectorystring)
                    TOPPed_shortcuttranstraj, TOPPed_shortcutrtraj = TransRotTrajFromSE3Traj(
                        TOPPed_shortcutse3traj)
                    newlietraj = ReplaceTrajectorySegment(
                        lietraj, TOPPed_shortcutrtraj, t0, t1)

                    newtranstraj = ReplaceTransTrajectorySegment(
                        transtraj, TOPPed_shortcuttranstraj, t0, t1)

                    #####################################################
                    newrtraj = Trajectory.PiecewisePolynomialTrajectory.FromString(
                        TrajStringFromTrajList(newlietraj.trajlist))
                    newse3traj = SE3TrajFromTransandSO3(newtranstraj, newrtraj)

                    Rlist = newlietraj.Rlist
                    rtraj = newrtraj
                    transtraj = newtranstraj
                    lietraj = newlietraj
                    se3traj = newse3traj

                    dur = se3traj.duration

                    #print "*******************************************"
                    print "Success at iteration", it + 1, ":", t0, t1, "Deta_t:", t1 - t0 - x.resduration
                    attempt += 1
                    #print "T:", nnotretimable, "; S:", nnotshorter , "; C:", ncollision , "; OK:", attempt
                    #print "*******************************************"
                else:
                    # print "Not shorter"
                    nnotshorter += 1
            else:
                # print "Not retimable"
                nnotretimable += 1
        else:
            # print "Collision"
            ncollision += 1

            # print "T:", nnotretimable, "; S:", nnotshorter , "; C:", ncollision , "; OK:", attempt
    print "\033[1;32mT:", nnotretimable, "; S:", nnotshorter, "; C:", ncollision, "; OK:", attempt, "\033[0m"
    print "\033[1;32m", originalduration - se3traj.duration, "sec. shorter\033[0m"
    t_sc_end = time.time()
    print "\033[1;32mRunning time:", t_sc_end - t_sc_start, "sec.\033[0m"

    return se3traj, Rlist
예제 #7
0
파일: SO3.py 프로젝트: indivisibleatom/TOPP
inertia = eye(3)
vmax = ones(3)
accelmax = ones(3)
discrtimestep = 1e-3
constraintsstr = str(discrtimestep)
constraintsstr += "\n" + ' '.join([str(a) for a in accelmax])
for v in inertia:
    constraintsstr += "\n" + ' '.join([str(i) for i in v])
#When Inertia is an Identity matrix, angular accelerations are the same as torques
t0 = time.time()
abc = TOPPbindings.RunComputeSO3Constraints(trajstr, constraintsstr)
a, b, c = Extractabc(abc)

t1 = time.time()

topp_inst = TOPP.QuadraticConstraints(traj, discrtimestep, vmax, list(a),
                                      list(b), list(c))

x = topp_inst.solver
ret = x.RunComputeProfiles(0, 0)

x.ReparameterizeTrajectory()
t2 = time.time()
print("Compute a,b,c:", t1 - t0)
print("Run TOPP:", t2 - t1)
print("Total:", t2 - t0)

# Display results
ion()
x.WriteProfilesList()
x.WriteSwitchPointsList()
profileslist = TOPPpy.ProfilesFromString(x.resprofilesliststring)
예제 #8
0
def computeReparametrizationTrajectory(F, R, amin, amax, W, dW, ddW):
        Ndim = W.shape[0]
        Nwaypoints = W.shape[1]

        [trajstr,durationVector] = GetTrajectoryString(W, dW, ddW)

        L = np.sum(durationVector)

        traj0 = Trajectory.PiecewisePolynomialTrajectory.FromString(trajstr)

        dendpoint = np.linalg.norm(traj0.Eval(L)-W[:,-1])

        if dendpoint > 0.01:
                print L
                print "###############"
                print "FINAL POINT on piecewise C^2 traj:",traj0.Eval(L)
                print "FINAL WAYPOINT                   :",W[:,-1]
                print "###############"
                sys.exit(1)

        ### compute a,b,c
        q = np.zeros((Ndim,Nwaypoints))
        qs = np.zeros((Ndim,Nwaypoints))
        qss = np.zeros((Ndim,Nwaypoints))
        for i in range(0,Nwaypoints):
                duration = np.sum(durationVector[0:i])
                q[:,i] = traj0.Eval(duration)
                qs[:,i] = traj0.Evald(duration)
                qss[:,i] = traj0.Evaldd(duration)

        I = np.identity(Ndim)
        G = np.vstack((I,-I))

        a = np.zeros((Nwaypoints, 2*Ndim))
        b = np.zeros((Nwaypoints, 2*Ndim))
        c = np.zeros((Nwaypoints, 2*Ndim))

        for i in range(0,Nwaypoints):
                Rmax = np.maximum(np.dot(R[:,:,i],amax),np.dot(R[:,:,i],amin))
                Rmin = np.minimum(np.dot(R[:,:,i],amax),np.dot(R[:,:,i],amin))
                H1 = F[:,i] - Rmax
                H2 = -F[:,i] + Rmin
                for j in range(Ndim):
                        #print "qvol[",j,"]=",np.linalg.norm(-H1[j]-H2[j])
                        if H2[j] > -H1[j]:
                                print H2[j],"<= q[",j,"]<=",-H1[j]
                                sys.exit(1)

                c[i,:] = np.hstack((H1,H2)).flatten()

        for i in range(0,Nwaypoints):
                a[i,:] = np.dot(G,qs[:,i]).flatten()
                b[i,:] = np.dot(G,qss[:,i]).flatten()

        vmax = 1000*np.ones(Ndim)
        durationQ = traj0.duration/(Nwaypoints-1)
        #durationQ = 1e-1
        #topp_inst = TOPP.QuadraticConstraints(traj0, durationVector[0], vmax, list(a), list(b), list(c))
        topp_inst = TOPP.QuadraticConstraints(traj0, durationQ, vmax, list(a), list(b), list(c))
        x = topp_inst.solver

        #customPlotTrajectory(traj0)
        #x.integrationtimestep = 0.001
        #x.reparamtimestep = 0.001
        #x.extrareps = 10

        try:
                #print "W=",repr(W[0:2,:])
                #print "q=",repr(q[0:2,:])
                #print "dW=",repr(dW[0:2,:])
                #print "qs=",repr(qs[0:2,:])
                #print "ddW=",repr(ddW[0:2,:])
                #print "qss=",repr(qss[0:2,:])
                #print "a=",repr(np.around(a,decimals=2))
                #print "b=",repr(np.around(b,decimals=2))
                #print "c=",repr(np.around(c,decimals=2))
                #print "d=",repr(durationVector)
                #print durationQ*Nwaypoints,"(",durationQ,") VS. ",traj0.duration
                #print trajstr
                ret = topp_inst.solver.RunComputeProfiles(0,0)
                if ret == 4:
                        #[semin,semax] = topp_inst.AVP(0, 0)
                        #print "TOPP critical pt:",semin,semax
                        Nc = topp_inst.solver.GetMVCCriticalPoint()
                        return Nc
                if ret == 1:
                        print "TOPP: success"
                        return Nwaypoints
                if ret== 0:
                        print "TOPP: unspecified error"
                        sys.exit(0)
                else:
                        print "TOPP: ",ret
                        sys.exit(0)
                return None

        except Exception as e:
                print "TOPP EXCEPTION: ",e
                print W
                print duration
                print durationVector
                print traj0.duration
                sys.exit(0)
예제 #9
0
def VisualizeTrajectory(traj0, W, Fx, Fy, offset, PostMakeup=True):

    Ndim = W.shape[0]
    #Ndim = 2
    Adim = 3
    #######################################################################
    #######################################################################
    #discrtimestep = 1.0/(W.shape[1]-1)#0.05
    discrtimestep = 0.5 * 1e-3
    #discrtimestep = 1e-2
    ndiscrsteps = int((traj0.duration + 1e-10) / discrtimestep) + 1
    q = np.zeros((Ndim, ndiscrsteps))
    qs = np.zeros((Ndim, ndiscrsteps))
    qss = np.zeros((Ndim, ndiscrsteps))
    a = np.zeros((ndiscrsteps, 2 * Adim))
    b = np.zeros((ndiscrsteps, 2 * Adim))
    c = np.zeros((ndiscrsteps, 2 * Adim))
    F = np.zeros((ndiscrsteps, Ndim))

    tvect = arange(0, traj0.duration + discrtimestep, discrtimestep)
    for i in range(ndiscrsteps):
        t = i * discrtimestep

        q[:, i] = traj0.Eval(t)
        qs[:, i] = traj0.Evald(t)
        qss[:, i] = traj0.Evaldd(t)

        if q[0, i] >= 2:
            F[i, :] = np.array((Fx, Fy, 0, 0))

        Ri = params.GetControlMatrixAtWaypoint(q[:, i])

        [G, h] = params.GetControlConstraintMatricesFromControl(Ri, F[i, :])
        a[i, :] = np.dot(G, qs[:, i]).flatten()
        b[i, :] = np.dot(G, qss[:, i]).flatten()
        c[i, :] = h

    #print "discrtimestep=",discrtimestep
    #PrintNumpy("a",a[:,[0,1,3,4]])
    #PrintNumpy("b",b[:,[0,1,3,4]])
    #PrintNumpy("c",c[:,[0,1,3,4]])
    #print a.shape
    #print b.shape
    #print c.shape
    ######################################################
    t0 = time.time()
    trajectorystring = str(traj0)
    vmax = 1e5 * np.ones(Ndim)
    topp_inst = TOPP.QuadraticConstraints(traj0, discrtimestep, vmax, list(a),
                                          list(b), list(c))

    x = topp_inst.solver
    t1 = time.time()
    #ret = x.RunComputeProfiles(0,0)
    ret = x.RunVIP(0, 0)
    print "TOPP:", ret
    #print t1-t0
    if ret == 1:
        x.ReparameterizeTrajectory()
        t2 = time.time()
        x.WriteResultTrajectory()
        msstyle = 'g'
        try:
            #print x.restrajectorystring
            #traj1 = TOPPTrajectory.PiecewisePolynomialTrajectory.FromString(x.restrajectorystring)
            semin = x.sdendmin
            semax = x.sdendmax
            print "speed profile [", semin, ",", semax, "]"
            #plot(qddvect[:,0], qddvect[:,1]-offset, 'r')
            #if PostMakeup:
            #tvect = arange(0, traj1.duration + discrtimestep, discrtimestep)
            #qddvect = np.array([traj1.Evaldd(t) for t in tvect])
            #for i in range(ndiscrsteps):
            #plot([q[0,i],q[0,i]+qddvect[i,0]],[q[1,i]-offset,q[1,i]+qddvect[i,1]-offset], '-om', linewidth=2)

            if semin > 0:
                msstyle = 'm'

        except Exception as e:
            print "Exception:", e
            msstyle = 'k'
            sys.exit(0)
            pass
    else:
        msstyle = 'r'

    if PostMakeup:
        for i in range(ndiscrsteps):
            plot([q[0, i], q[0, i] + F[i, 0]],
                 [q[1, i] - offset, q[1, i] + F[i, 1] - offset],
                 '-ob',
                 linewidth=2)

        plot(q[0, :],
             q[1, :] - offset,
             '|' + msstyle,
             markersize=40,
             markeredgewidth=3)
    Npts = 1000
    tvect = np.linspace(0, traj0.duration, Npts)
    qvect = np.array([traj0.Eval(t) for t in tvect])

    plot(qvect[:, 0], qvect[:, 1] - offset, '-' + msstyle)
    plot(W[0, :], W[1, :] - offset, 'o' + msstyle, markersize=15)
예제 #10
0
def getSpeedProfile(RdimIdx, SO3dimIdx, F, R, amin, amax, W, dW, ddW):

    Ndim = W.shape[0]
    Nwaypoints = W.shape[1]

    ### ManifoldIDX: 0 -> R-subspace. 1-> SO(2) subspace. 2-> SO(3) subspace
    ### (ordered three consecutive numbers as yaw/pitch/roll

    Rdim = RdimIdx.shape[0]

    [Rtrajstr, durationVector] = GetTrajectoryStringR(RdimIdx, W, dW, ddW)
    L = np.sum(durationVector)

    Rtraj = Trajectory.PiecewisePolynomialTrajectory.FromString(Rtrajstr)

    print "FINAL POINT on piecewise C^2 traj:", Rtraj.Eval(L)
    print "###############"

    ### compute a,b,c
    qs = np.zeros((Rdim, Nwaypoints))
    qss = np.zeros((Rdim, Nwaypoints))
    for i in range(0, Nwaypoints):
        duration = np.sum(durationVector[0:i])
        qs[:, i] = Rtraj.Evald(duration)
        qss[:, i] = Rtraj.Evaldd(duration)
        print duration, Rtraj.Eval(duration), qs[:, i], qss[:, i]

    print "###############"
    subplot(3, 1, 1)
    Rtraj.Plot(0.001)
    subplot(3, 1, 2)
    Rtraj.Plotd(0.001)
    subplot(3, 1, 3)
    Rtraj.Plotdd(0.001)
    plt.show()
    #sys.exit(0)

    I = np.identity(Rdim)
    G = np.vstack((I, -I))

    ar = np.zeros((Nwaypoints, 2 * Rdim))
    br = np.zeros((Nwaypoints, 2 * Rdim))
    cr = np.zeros((Nwaypoints, 2 * Rdim))

    for i in range(0, Nwaypoints):
        Rmax = np.maximum(np.dot(R[RdimIdx, :, i], amax),
                          np.dot(R[RdimIdx, :, i], amin))
        Rmin = np.minimum(np.dot(R[RdimIdx, :, i], amax),
                          np.dot(R[RdimIdx, :, i], amin))
        H1 = F[RdimIdx, i] - Rmax
        H2 = -F[RdimIdx, i] + Rmin
        #for j in RdimIdx:
        #        print "qvol[",j,"]=",np.linalg.norm(-H1[j]-H2[j])
        #        if H2[j] > -H1[j]:
        #                print H2[j],"<= q[",j,"]<=",-H1[j]
        #                sys.exit(1)

        cr[i, :] = np.hstack((H1, H2)).flatten()

    for i in range(0, Nwaypoints):
        ar[i, :] = np.dot(G, qs[:, i]).flatten()
        br[i, :] = np.dot(G, qss[:, i]).flatten()

    # Constraints
    t0 = time.time()

    [aso3, bso3, cso3] = GetABCFromSO3Manifold(SO3dimIdx, amin, amax, W, dW,
                                               ddW)

    a = np.hstack((ar, aso3))
    b = np.hstack((br, bso3))
    c = np.hstack((cr, cso3))
    traj0 = GetTrajectoryStringComplete(W, dW, ddW)
    vmax = 1000 * np.ones(Ndim)

    ############################################################################
    ### DEBUG: only retime on R manifold subspace
    ############################################################################
    #a = list(ar)
    #b = list(br)
    #c = list(cr)
    #traj0 = Rtraj
    #vmax = 1000*np.ones(Rdim)
    ############################################################################

    #topp_inst = TOPP.QuadraticConstraints(traj0, duration, vmax, list(a), list(b), list(c))
    print "###########################"

    topp_inst = TOPP.QuadraticConstraints(traj0, durationVector[0], vmax,
                                          list(a), list(b), list(c))
    x = topp_inst.solver

    #x.integrationtimestep = 0.001
    #x.reparamtimestep = 0.001
    #x.extrareps = 10

    ret = x.RunComputeProfiles(0, 0)
    x.ReparameterizeTrajectory()

    print "TOPP Output:", ret
    if (ret == 4):
        print ret, " [ERROR TOPP: MVC hit zero]"

    if (ret == 1):
        x.ReparameterizeTrajectory()
        # Display results
        ion()
        #x.WriteProfilesList()
        #x.WriteSwitchPointsList()
        #profileslist = list(TOPPpy.ProfilesFromString(x.resprofilesliststring))
        #switchpointslist = TOPPpy.SwitchPointsFromString(x.switchpointsliststring)
        #TOPPpy.PlotProfiles(profileslist,switchpointslist,4)
        x.WriteResultTrajectory()

        print x.restrajectorystring
        traj1 = Trajectory.PiecewisePolynomialTrajectory.FromString(
            x.restrajectorystring)
        print "Trajectory duration before TOPP: ", traj0.duration
        print "Trajectory duration after TOPP: ", traj1.duration
        P = []
        tstep = traj1.duration / Nwaypoints
        subplot(3, 1, 1)
        traj1.Plot(0.001)
        subplot(3, 1, 2)
        traj1.Plotd(0.001)
        subplot(3, 1, 3)
        traj1.Plotdd(0.001)
        plt.show()

        t = 0
        while t < traj1.duration:
            P.append(np.linalg.norm(traj1.Evald(t)))
            t = t + tstep
        print "profile computed"
        #mvcbobrow = profileslist.pop(0)
        #mvc = np.array(mvcbobrow[3])
        #print mvc
        return np.array(P)
    return None
예제 #11
0
파일: RL.py 프로젝트: sigfos/IT3105_Mod3
                            verbose=settings.verbose,
                            loss=settings.loss,
                            optimizer=settings.optimizer,
                            epsilon=settings.epsilon,
                            lrate=settings.lrate)
                anet.create_anet()
                hex_nn = HexNN(anet,
                               hex_state,
                               save_int=settings.save_interval,
                               file_add=settings.file_add,
                               sim_increment=settings.sim_increment,
                               verbose=settings.verbose)
                hex_nn.run(settings.simulations, settings.training_games)
            elif choice == '2':
                players = list()
                for anet in settings.anet_files:
                    if anet.strip().lower() == "none":
                        players.append(None)
                    elif anet.split('_')[0] == "Demo":
                        players.append(ANET.load_model("./Demo_files/" + anet))
                    else:
                        players.append(ANET.load_model(anet))
                topp = TOPP(players,
                            g=settings.games,
                            board_dim=settings.root_board_dim,
                            epsilon=settings.epsilon)
                topp.play_tournament()
            elif choice == '4':
                script = "../fireworks/./main"
                os.system("bash -c '%s'" % script)