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):
# return sum(m.beta[foot, bv, ti]**2 for foot in m.feet for bv in m.BV_INDEX for ti in m.t)
# + sum(m.pdd[foot, i, j]**2 for foot in m.feet for i in m.R2_INDEX for j in m.t)
#return sum(m.f[foot, i, j]**2 + m.pd[foot, i, j]**2 + m.pdd[foot, i, j]**2 for foot in m.feet for i in m.R2_INDEX for j in m.t) + sum(m.T[ti]**2 for ti in m.t)
return sum(m.f[foot, i, j]**2 + m.pd[foot, i, j]**2 + m.pdd[foot, i, j]**2 for foot in m.feet for i in m.R2_INDEX for j in m.t) + sum(m.hipTorque[foot, ti]**2 for foot in m.feet for ti in m.t) + summation(m.dt)
#return sum(m.f[foot, i, j]**2 for foot in m.feet for i in m.R2_INDEX for j in m.t) + sum(m.hipTorque[foot, ti]**2 for foot in m.feet for ti in m.t)
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):
index = var.index_set()
slackName = '%sSlacks' % var.cname()
lbName = '%sLB' % slackName
ubName = '%sUB' % slackName
slackMax = max([max(np.abs(var[i].bounds)) for i in index])