step_height = 0.1 platform1_start = -0.5 platform1_end = 0.3 platform1_height = 0*step_height platform2_start = 0.7 platform2_end = 1.3 platform2_height = step_height platform3_start = 1.7 platform3_end = 9.5 platform3_height = 2*step_height matlab_hopper = eng.Hopper(legLength, hipOffset) hop = Hopper(N, eng, matlab_hopper) # hop.mdt_precision = int(ceil(-np.log2(desiredPrecision))) hop.dtBounds = tuple(tf/N/sqrt(legLength/9.81)*np.array([0.1, 1.9])) hop.rotationMax = np.pi/8 hop.nOrientationSectors = 1 #int(floor(np.pi/8/desiredPrecision)) print 'hop.nOrientationSectors = %d' % hop.nOrientationSectors hop.velocityMax = 3 hop.positionMax = 7 hop.forceMax = 2 hop.addPlatform(platform1_start/legLength, platform1_end/legLength, platform1_height/legLength, 1) hop.addPlatform(platform2_start/legLength, platform2_end/legLength, platform2_height/legLength, 1) hop.addPlatform(platform3_start/legLength, platform3_end/legLength, platform3_height/legLength, 1) hop.addFreeBlock(bottom=platform1_height/legLength, right=platform2_start/legLength) hop.addFreeBlock(bottom=platform2_height/legLength, right=platform3_start/legLength) hop.addFreeBlock(bottom=platform3_height/legLength) hop.constructVisualizer() m_nlp = hop.constructPyomoModel() def objRule(m):
desiredPrecision = 2
N = 25
tf = 2*1.6
legLength = 0.16
r0 = [0, legLength/2]
rf = [1.0, legLength]
v0 = [0, 0]
w0 = 0
hipOffset = {'front': {'x': 0.5, 'z': -0.25}, 'hind': {'x': -0.5, 'z': -0.25}}
matlab_hopper = eng.Hopper(legLength, hipOffset)
hop = Hopper(N, eng, matlab_hopper)
# hop.mdt_precision = int(ceil(-np.log2(desiredPrecision)))
hop.dtBounds = tuple((1/sqrt(legLength/9.81))*np.array([0.05, 0.2]))
hop.dtNom = 0.04*(1/sqrt(legLength/9.81))
hop.rotationMax = np.pi/8
hop.nOrientationSectors = 1 #int(floor(np.pi/8/desiredPrecision))
print 'hop.nOrientationSectors = %d' % hop.nOrientationSectors
hop.velocityMax = 3.
hop.positionMax = 1.5*rf[0]/legLength
hop.forceMax = 3.
hop.angularVelocityMax = 5.
addThreePlatfomWorld(hop, legLength, 0.3*legLength)
#addFlatWorld(hop, legLength)
hop.constructVisualizer()
m_nlp = hop.constructPyomoModel()
def normL2(m, var):
index = var.index_set()
return sum(var[i]**2 for i in index)
def normL1(m, var):
