# 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) m_nlp.Obj = Objective(rule=objRule, sense=minimize) m_nlp.rx0 = Constraint(expr=m_nlp.r['x',m_nlp.t[1]] == r0[0]/legLength) #m_nlp.rz0 = Constraint(expr=m_nlp.r['z',m_nlp.t[1]] <= 1) m_nlp.th0 = Constraint(expr=m_nlp.th[m_nlp.t[1]] == 0)
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]) setattr(m, slackName, Var(index, bounds=(0.0, slackMax))) def _lbRule(m, *args): return var[args] <= getattr(m, slackName)[args] setattr(m, lbName, Constraint(index, rule=_lbRule))