(z[p][0] - y[p].dihedral(PHI)) +
                                           FTSM.M(x[p], PHI, PSI) *
                                           (z[p][1] - y[p].dihedral(PSI)))
        z[p][1] -= (kappa / gamma) * dt * (FTSM.M(x[p], PSI, PHI) *
                                           (zp0 - y[p].dihedral(PHI)) +
                                           FTSM.M(x[p], PSI, PSI) *
                                           (z[p][1] - y[p].dihedral(PSI)))

        # UPDATE CARTESIAN
        prop[p][0].propagate(scheme="velocityscale",
                             steps=1,
                             dt=dt,
                             forcefield=ff,
                             params={'T0': 300})

        prop[p][1].propagate(scheme="velocityscale",
                             steps=1,
                             dt=dt,
                             forcefield=ff,
                             params={'T0': 300})

        # My own function which sets phi and psi for individual force objects
        # Saves performance since I only change 'angle', I don't want to define
        # all new force objects by changing params.
        #FTSM.setConstraint(phi=z[(p+1)%numpoints][0], psi=z[(p+1)%numpoints][1], kappa=kappa, forcefield=ff)

    z = FTSM.reparam(z)

    print "\nI" + str(iter + 1) + ": ", z
    #io.plotVector(prop[0][0],stringgraph,z, rangex=[-numpy.pi, numpy.pi], rangey=[-numpy.pi, numpy.pi])
            kappa += (100.-40.)/90000.

        if (p != 0 or iter != 0):
           FTSM.setConstraint(PHI, PSI, phi=z[p][0], psi=z[p][1], kappa=kappa, forcefield=ff)
        
        # UPDATE FREE SPACE
        # USE FIRST SYSTEM TO GET M
        # USE SECOND SYSTEM TO OBTAIN PHI AND PSI DIFFERENCES
        # FROM TARGETS
        zp0 = z[p][0]
        z[p][0] -= (kappa/gamma)*dt*(FTSM.M(x[p], PHI, PHI)*(z[p][0]-y[p].dihedral(PHI)) + FTSM.M(x[p], PHI, PSI)*(z[p][1] - y[p].dihedral(PSI)))
        z[p][1] -= (kappa/gamma)*dt*(FTSM.M(x[p], PSI, PHI)*(zp0-y[p].dihedral(PHI)) + FTSM.M(x[p], PSI, PSI)*(z[p][1] - y[p].dihedral(PSI)))
        
        
        # UPDATE CARTESIAN
        prop[p][0].propagate(scheme="velocityscale", steps=1, dt=dt, forcefield=ff, params={'T0':300})

        prop[p][1].propagate(scheme="velocityscale", steps=1, dt=dt, forcefield=ff, params={'T0':300})


        # My own function which sets phi and psi for individual force objects
        # Saves performance since I only change 'angle', I don't want to define
        # all new force objects by changing params.
        #FTSM.setConstraint(phi=z[(p+1)%numpoints][0], psi=z[(p+1)%numpoints][1], kappa=kappa, forcefield=ff)

    z = FTSM.reparam(z)

    print "\nI"+str(iter+1)+": ",z
    #io.plotVector(prop[0][0],stringgraph,z, rangex=[-numpy.pi, numpy.pi], rangey=[-numpy.pi, numpy.pi])