Commit 727ecb5e authored by Steve T's avatar Steve T
Browse files

quadruped planning seems ok, need to rotate constraints by 90 degrees, need to...

quadruped planning seems ok, need to rotate constraints by 90 degrees, need to sort initial com repartition
parent a961277c
......@@ -235,13 +235,22 @@ def FootCOMKinConstraint(pb, phaseDataT, A, b, previousStartCol, startCol, endCo
# TODO REMOVE FOR FIRST PHASE
def RelativeDistanceConstraint(pb, phaseDataT, A, b, startCol, endCol, startRow, phaseId):
idRow = startRow
# ~ from tools.plot_plytopes import plot_polytope_H_rep
limbNames = ['LFleg', 'RFleg', 'LHleg', 'RHleg']
for footIdFrame, constraintsInFootIdFrame in enumerate(phaseDataT["allRelativeK"][0]):
# ~ fig = plt.figure()
# ~ fig.suptitle(limbNames[footIdFrame], fontsize=16)
# ~ ax = fig.add_subplot(111, projection="3d")
for (footId, Kk ) in constraintsInFootIdFrame:
K = Kk[0]; k = Kk[1]
consLen = K.shape[0]
A[idRow:idRow+consLen, startCol:startCol+DEFAULT_NUM_VARS] = K.dot(-footExpressionMatrix[footId] + footExpressionMatrix[footIdFrame])
b[idRow:idRow+consLen] = k
idRow += consLen
# ~ print ("KK", K.shape)
# ~ succPlot = plot_polytope_H_rep(K,k.reshape((-1,1)), ax = ax)
# ~ if not (succPlot):
# ~ print ("indices ", footIdFrame, footId)
A[idRow:idRow+consLen, startCol:startCol+DEFAULT_NUM_VARS] = K.dot(footExpressionMatrix[footId] - footExpressionMatrix[footIdFrame])
b[idRow:idRow+consLen] = k[:]
idRow += consLen
return idRow
def SurfaceConstraint(phaseDataT, A, b, startCol, endCol, startRow):
......@@ -355,12 +364,12 @@ def convertProblemToLp(pb, convertSurfaces = True):
#inequality
endCol = startCol + numVariablesForPhase(phaseDataT)
startRow = FootCOMKinConstraint(pb, phaseDataT, A, b, previousStartCol, startCol, endCol, startRow, phaseId)
# ~ startRow = RelativeDistanceConstraint(pb, phaseDataT, A, b, startCol, endCol, startRow, phaseId)
startRow = RelativeDistanceConstraint(pb, phaseDataT, A, b, startCol, endCol, startRow, phaseId)
startRow = SurfaceConstraint(phaseDataT, A, b, startCol, endCol, startRow)
startRow = SlackPositivityConstraint(phaseDataT, A, b, startCol, endCol, startRow)
#equalities
startRowEq = CoMWeightedEqualityConstraint(phaseDataT, E, e, startCol, endCol, startRowEq)
# ~ startRowEq = CoMWeightedEqualityConstraint(phaseDataT, E, e, startCol, endCol, startRowEq)
startRowEq = FootContinuityEqualityConstraint(pb, phaseDataT, E, e, previousStartCol, startCol, endCol, startRowEq, phaseId)
previousStartCol = startCol
startCol = endCol
......@@ -444,11 +453,25 @@ def retrieve_points_from_res(pb, res):
col = 0
for i, phaseDataT in enumerate(pb["phaseData"]):
coms += [COM1_ExpressionMatrix.dot(res[col:col + COM1_ExpressionMatrix.shape[1]]), COM2_ExpressionMatrix.dot(res[col:col + COM2_ExpressionMatrix.shape[1]])]
allFeetPhase = []
for footId in range(N_EFFECTORS):
pos = footExpressionMatrix[footId].dot(res[col:col + footExpressionMatrix[footId].shape[1]])
allFeetPos += [pos]
footPos[footId] += [pos]
col += numVariablesForPhase(phaseDataT)
posE = array(footExpressionMatrix[footId].dot(res[col:col + footExpressionMatrix[footId].shape[1]]) )
pos = posE * res[col + W_START + footId]
# ~ pos = pos + array([11,11,0.])
# ~ allFeetPos += [allFeetPhase]
# ~ if norm(pos) > 0.01:
# ~ allFeetPhase += [pos]
# ~ footPos[footId] += [pos]
footPos[footId] += [posE]
# ~ allFeetPos += [pos]
print("posE", posE)
allFeetPhase += [posE]
print ("allFeetPhase" , allFeetPhase)
allFeetPos += [allFeetPhase]
print ("allFeetPos" , allFeetPos)
col += numVariablesForPhase(phaseDataT)
return coms, footPos, allFeetPos
......@@ -496,9 +519,11 @@ def plotConstraints(ax, pb, allfeetpos, coms):
except:
print("qhullfailed")
from scipy.spatial import ConvexHull
from tools.plot_plytopes import plot_hull
def plotQPRes(pb, res, linewidth=2, ax = None, plot_constraints = False, show = True):
coms, footpos, allfeetpos = retrieve_points_from_res(pb, res)
if ax is None:
fig = plt.figure()
ax = fig.add_subplot(111, projection="3d")
......@@ -510,20 +535,32 @@ def plotQPRes(pb, res, linewidth=2, ax = None, plot_constraints = False, show =
ax.set_autoscale_on(False)
ax.view_init(elev=8.776933438381377, azim=-99.32358055821186)
plotPoints(ax, coms, color = "b")
# ~ plotPoints(ax, coms[1:], color = "b")
footColors = ['r', 'g', 'y', 'b']
# ~ plotPoints(ax, coms, )
[plotPoints(ax, footpos[idx], color = footColors[idx]) for idx in range(N_EFFECTORS)]
cx = [c[0] for c in coms]
cy = [c[1] for c in coms]
cz = [c[2] for c in coms]
cx = [c[0] for c in coms[1:]]
cy = [c[1] for c in coms[1:]]
cz = [c[2] for c in coms[1:]]
ax.plot(cx, cy, cz)
px = [c[0] for c in allfeetpos]
py = [c[1] for c in allfeetpos]
pz = [c[2] for c in allfeetpos]
idC = 0;
for el in allfeetpos[:]:
# ~ print("points ", [el[:2] for el in allfeetpos])
pts = [e[:2] for e in el]
apts = array(pts)
chul = ConvexHull(array([e[:2] for e in el]))
plot_hull(chul, pts = pts, apts = apts, ax = ax, color = footColors[idC])
idC = (idC + 1) % 4
# ~ px = [c[0] for c in el]
# ~ py = [c[1] for c in el]
# ~ pz = [c[2] for c in el]
# ~ ax.plot(px, py, pz)
# ~ px = [c[0] for c in allfeetpos]
# ~ py = [c[1] for c in allfeetpos]
# ~ pz = [c[2] for c in allfeetpos]
# ~ ax.plot(px, py, pz)
if show:
......@@ -533,25 +570,72 @@ def plotQPRes(pb, res, linewidth=2, ax = None, plot_constraints = False, show =
# ~ def addInitEndConstraint(pb, E, e, posInit= array([0.16, 0.585, 0.0]), posEnd = array([1.2, 0.37, 0.40])):
def addInitEndConstraint(pb, E, e, posInit= array([0.16, 0.585, 0.0]), posEnd = array([1., 0.585, 0.])):
# ~ def addInitEndConstraint(pb, E, e, posInit= array([0.0, 0.0, 0.4]), posEnd = array([-0.0, 0.0, 0.4])):
nE = zeros((E.shape[0] +9, E.shape[1] ))
ne = zeros(E.shape[0] +9)
def addInitEndConstraint(pb, E, e, posInit, posEnd, initCom, endCom):
sizeAdd = 0
if posInit is not None:
sizeAdd += len(posInit) * 3
if posEnd is not None:
sizeAdd += len(posEnd) * 3
if initCom is not None:
sizeAdd += 3
if endCom is not None:
sizeAdd += 3
nE = zeros((E.shape[0] +sizeAdd, E.shape[1] ))
ne = zeros(E.shape[0] +sizeAdd)
idRow = E.shape[0]
nE[:idRow,:E.shape[1]] = E
ne[:idRow] = e[:]
nE[idRow:idRow+3,:DEFAULT_NUM_VARS] = footExpressionMatrix[0][:]
ne[idRow:idRow+3] = posInit
idRow += 3
ne[:idRow] = e[:]
print ("heo", E.shape)
idRow = E.shape[0]
nVarEnd = numVariablesForPhase(pb["phaseData"][-1])
if initCom is not None:
nE[idRow:idRow+3,:DEFAULT_NUM_VARS] = COM2_ExpressionMatrix[:]
ne[idRow:idRow+3] = initCom
idRow+=3
if endCom is not None:
print("nVarEnd", nVarEnd, E.shape[1]-nVarEnd, E.shape[1]-nVarEnd+DEFAULT_NUM_VARS)
nE[idRow:idRow+3:,E.shape[1]-nVarEnd:E.shape[1]-nVarEnd+DEFAULT_NUM_VARS] = COM1_ExpressionMatrix[:]
ne[idRow:idRow+3] = endCom
idRow+=3
if posInit is not None:
print("adding ", idRow)
for idFoot, pos in enumerate(posInit):
print("adding ", idRow )
nE[idRow:idRow+3,:DEFAULT_NUM_VARS] = footExpressionMatrix[idFoot][:]
ne[idRow:idRow+3] = pos
idRow+=3
if posEnd is not None:
for idFoot, pos in enumerate(posEnd):
print("adding end", idRow )
nE[idRow:idRow+3:,E.shape[1]-nVarEnd:E.shape[1]-nVarEnd+DEFAULT_NUM_VARS] = footExpressionMatrix[idFoot][:]
ne[idRow:idRow+3] = pos
idRow+=3
return nE, ne
# ~ def addInitEndConstraint(pb, E, e, posInit= array([0.0, 0.0, 0.4]), posEnd = array([-0.0, 0.0, 0.4])):
# ~ nE = zeros((E.shape[0] +9, E.shape[1] ))
# ~ ne = zeros(E.shape[0] +9)
# ~ idRow = E.shape[0]
# ~ nE[:idRow,:E.shape[1]] = E
# ~ ne[:idRow] = e[:]
# ~ nE[idRow:idRow+3,:DEFAULT_NUM_VARS] = footExpressionMatrix[0][:]
# ~ ne[idRow:idRow+3] = posInit
# ~ idRow += 3
# ~ nE[idRow:idRow+3,:DEFAULT_NUM_VARS] = footExpressionMatrix[1][:]
# ~ nE[idRow:idRow+3,:DEFAULT_NUM_VARS] = footExpressionMatrix[0][:]
# ~ ne[idRow:idRow+3] = posInit
# ~ ne[idRow:idRow+3] = posInit + array([0., -.20, 0.0])
nVarEnd = numVariablesForPhase(pb["phaseData"][-1])
# ~ nVarEnd = numVariablesForPhase(pb["phaseData"][-1])
# ~ print ("nVarEnd", nVarEnd, E.shape[1]-nVarEnd, E.shape[1]-nVarEnd,E.shape[1]-nVarEnd+DEFAULT_NUM_VARS )
nE[-3:,E.shape[1]-nVarEnd:E.shape[1]-nVarEnd+DEFAULT_NUM_VARS] = footExpressionMatrix[0][:]
ne[-3:] = posEnd
return nE, ne
# ~ nE[-3:,E.shape[1]-nVarEnd:E.shape[1]-nVarEnd+DEFAULT_NUM_VARS] = footExpressionMatrix[0][:]
# ~ ne[-3:] = posEnd
# ~ return nE, ne
####################### MAIN ###################"
......@@ -651,7 +735,7 @@ def solveMIP(pb, surfaces, MIP = True, draw_scene = None, plot = True):
return pb, res, timMs(t1,t2)
def solveMIPGurobi(pb, surfaces, MIP = True, draw_scene = None, plot = True, initGuess = None, initGuessMip = None, l1Contact = False):
def solveMIPGurobi(pb, surfaces, MIP = True, draw_scene = None, plot = True, initGuess = None, initGuessMip = None, l1Contact = False, initPos = None, endPos = None, initCom = None, endCom = None):
if not MIP_OK:
print ("Mixed integer formulation requires gurobi packaged in cvxpy")
raise ImportError
......@@ -662,7 +746,8 @@ def solveMIPGurobi(pb, surfaces, MIP = True, draw_scene = None, plot = True, ini
grb.setParam('OutputFlag', 1)
A, b, E, e = convertProblemToLp(pb)
E,e = addInitEndConstraint(pb, E, e)
print ("initpos ", initPos)
E,e = addInitEndConstraint(pb, E, e, initPos, endPos, initCom, endCom)
slackMatrix = wSelectionMatrix(pb)
slackIndices = [i for i,el in enumerate (slackMatrix) if el > 0]
numSlackVariables = len([el for el in slackMatrix if el > 0])
......@@ -682,7 +767,6 @@ def solveMIPGurobi(pb, surfaces, MIP = True, draw_scene = None, plot = True, ini
if MIP:
cVars.append(model.addVar(name="slack%d" % i, obj = 0, vtype=grb.GRB.CONTINUOUS, lb = -grb.GRB.INFINITY, ub = grb.GRB.INFINITY))
else:
print ("w")
cVars.append(model.addVar(name="slack%d" % i, obj = 1, vtype=grb.GRB.CONTINUOUS, lb = -grb.GRB.INFINITY, ub = grb.GRB.INFINITY))
elif surfaceSlackMatrix[i]>0:
if MIP and not l1Contact:
......@@ -752,7 +836,8 @@ def solveMIPGurobi(pb, surfaces, MIP = True, draw_scene = None, plot = True, ini
previousL = 0
for i, el in enumerate(slackIndices):
if i!= 0 and el - previousL > 1.:
model.addConstr(grb.quicksum(currentSum) <= len(currentSum) -1, "card%d" % i)
# ~ model.addConstr(grb.quicksum(currentSum) <= len(currentSum) -1, "card%d" % i)
model.addConstr(grb.quicksum(currentSum) == 1, "card%d" % i)
assert len(currentSum) > 0
currentSum = [boolvars[i]]
currentSum2 = [el]
......@@ -761,8 +846,10 @@ def solveMIPGurobi(pb, surfaces, MIP = True, draw_scene = None, plot = True, ini
currentSum2 = currentSum2 + [el]
previousL = el
if len(currentSum) > 1:
model.addConstr(grb.quicksum(currentSum) <= len(currentSum) -1, "card%d" % i)
model.addConstr(grb.quicksum(currentSum) == 1, "card%d" % i)
# ~ model.addConstr(grb.quicksum(boolvars) == 4, "card%d" % i)
print ("bools ", len(boolvars))
if not l1Contact:
boolvarsSurf = []
......@@ -834,7 +921,9 @@ if __name__ == '__main__':
# ~ E,e = addInitEndConstraint(pb, E, e)
# ~ pb, res, time = solveMIP(pb, surfaces, MIP = True, draw_scene = None, plot = True)
pb, res, time = solveMIPGurobi(pb, surfaces, MIP = True, draw_scene = None, plot = True, l1Contact = False)
pb, res, time = solveMIPGurobi(pb, surfaces, MIP = True, draw_scene = None, plot = True, l1Contact = False, initPos = None)
# ~ pb, res, time = solveMIPGurobi(pb, surfaces, MIP = False, draw_scene = None, plot = True, l1Contact = True)
# ~ pb, res, time = solveMIPGurobi(pb, surfaces, MIP = True, draw_scene = None, plot = True, l1Contact = True)
# ~ pb, res, time = solveMIPGurobi(pb, surfaces, MIP = False, draw_scene = None, plot = True)
......
......@@ -19,6 +19,7 @@ afloor = array(floor).T
all_surfaces = [floor]
surfaces = [[afloor] for _ in range (10)]
# ~ surfaces = [[afloor] for _ in range (1)]
def gen_flat_pb():
kinematicConstraints = genCOMConstraints()
......@@ -63,9 +64,39 @@ if __name__ == '__main__':
initGlobals(nEffectors = 4)
pb = gen_flat_pb()
pb, res, time = solveMIPGurobi(pb, surfaces, MIP = True, draw_scene = None, plot = True, l1Contact = False)
# ~ initPos = [[0.2, 0.6, 0.0],[-0.7, 0.1, 0.0], [-0.3, 1.2, 0.0], [-0.8, 1.0, 0.0] ]
initPos = None
endPos = None
initCom = None
endCom = None
# ~ initCom = [0.2, 0., 0.5]
# ~ endCom = [2, 0.7, 0.5]
endCom = [0.2, 1.2, 0.5]
# ~ initPos = [array([0.5, 0.0, 0.0]), array([-0.1, 0.0, -0.0]), array([0.3, 0.5, 0.0]), array([-0.0, 0.4, 0.0])]
# ~ endPos = [array()]
# ~ initPos = [[0.2, 0.6, 0.0] ]; initPos = [array(el) for el in initPos]
# ~ endPos = [[0.7, 0.6, 0.0] ]; endPos = [array(el) for el in endPos]
# ~ initCom = [0.2, 2., 0.35]
# ~ endCom = [0.2, 2., 0.35]
print ("initPos", initPos)
pb, res, time = solveMIPGurobi(pb, surfaces, MIP = True, draw_scene = None, plot = True, l1Contact = False, initPos = initPos, endPos = endPos, initCom = initCom, endCom= endCom)
ax = draw_scene(None)
plotQPRes(pb, res, ax=ax, plot_constraints = False)
# ~ plotQPRes(pb, res, ax=ax, plot_constraints = False, show = False)
plotQPRes(pb, res, ax=ax, plot_constraints = False, show = True)
# ~ for i in range(5):
# ~ coms, footpos, allfeetpos = retrieve_points_from_res(pb, res)
# ~ pb = gen_flat_pb()
# ~ initPos = None
# ~ endPos = None
# ~ initCom = coms[-1]
# ~ endCom = coms[-1] + array([0., 1.0, 0.0])
# ~ initPos = allfeetpos[-1]
# ~ pb, res, time = solveMIPGurobi(pb, surfaces, MIP = True, draw_scene = None, plot = True, l1Contact = False, initPos = initPos, endPos = endPos, initCom = initCom, endCom= endCom)
# ~ plotQPRes(pb, res, ax=ax, plot_constraints = False, show = False)
# ~ plt.show(block = False)
# ~ pb, res, time = solveMIPGurobi(pb, surfaces, MIP = True, draw_scene = None, plot = True, l1Contact = False, initPos = None)
......
import numpy as np
from numpy import array, asmatrix, matrix, zeros, ones
from numpy import array, dot, stack, vstack, hstack, asmatrix, identity, cross, concatenate
from numpy.linalg import norm
from sl1m.constants_and_tools import *
from sl1m.planner_l1_generic_equalities_as_ineq import *
from sl1m.stand_alone_scenarios.constraints_anymal import *
floor = [ [0.16, 1., 0.], [-1.8, 1., 0.], [-1.8, -1., 0.], [0.16, -1., 0.] ]
step1 = [[0.3, 0.6, 0.15],[0.3, -0.16, 0.15],[0.6, -0.16, 0.15],[0.6, 0.6, 0.15]]
step2 = [[0.6, 0.6, 0.3 ],[0.6, -0.16, 0.3 ],[0.9, -0.16, 0.3 ],[0.9, 0.6, 0.3 ]]
step3 = [[0.9, 0.6, 0.45],[0.9, -0.16, 0.45],[1.2, -0.16, 0.45],[1.2, 0.6, 0.45]]
step4 = [[1.2, 0.6, 0.6 ],[1.2, -0.16, 0.6 ],[1.5, -0.16, 0.6 ],[1.5, 0.6, 0.6 ]]
floor = [ [0.16, 1., 0.], [-1.8, 1., 0.], [-1.8, -1., 0.], [0.16, -1., 0.] ]
step1 = [[0.3, 0.6, 0.1],[0.3, -0.16, 0.1],[0.6, -0.16, 0.1],[0.6, 0.6, 0.1]]
step2 = [[0.6, 0.6, 0.2 ],[0.6, -0.16, 0.2],[0.9, -0.16, 0.2 ],[0.9, 0.6, 0.2 ]]
step3 = [[0.9, 0.6, 0.3],[0.9, -0.16, 0.3],[1.2, -0.16, 0.3],[1.2, 0.6, 0.3]]
step4 = [[1.2, 0.6, 0.4 ],[1.2, -0.16, 0.4 ],[1.5, -0.16, 0.4 ],[1.5, 0.6, 0.4 ]]
all_surfaces = [floor, step1, step2, step3, step4]
afloor = array(floor).T
astep1 = array(step1).T
astep2 = array(step2).T
astep3 = array(step3).T
astep4 = array(step4).T
a_all_surfaces = [array(el).T for el in all_surfaces]
# ~ surfaces = [[afloor], [afloor], [astep1,astep2,astep3],[astep2,astep3,astep1], [astep3,astep2,astep1,astep4], [astep3,astep4], [astep4],[astep4]]
# ~ surfaces = [[afloor], [afloor], [astep1],[astep2], [astep3, astep1], [astep3], [astep4],[astep4]]
# ~ surfaces = [[afloor], [afloor], [astep1],[astep2], [astep3], [astep3], [astep4],[astep4]]
# ~ surfaces = [a_all_surfaces, a_all_surfaces, a_all_surfaces,a_all_surfaces, a_all_surfaces, a_all_surfaces, a_all_surfaces,[astep4]]
surfaces = [[afloor],[afloor],[afloor],[afloor],[afloor],[afloor],[afloor],[afloor],[afloor],[afloor],a_all_surfaces,a_all_surfaces,a_all_surfaces,a_all_surfaces,a_all_surfaces,[astep4]]
# ~ surfaces = [a_all_surfaces, a_all_surfaces, a_all_surfaces,a_all_surfaces, a_all_surfaces, a_all_surfaces, a_all_surfaces]
# ~ surfaces = [[afloor], [afloor], a_all_surfaces, a_all_surfaces, a_all_surfaces,a_all_surfaces, a_all_surfaces, a_all_surfaces, a_all_surfaces,[astep4]]
# ~ surfaces = [[astep4]]
def gen_flat_pb():
kinematicConstraints = genCOMConstraints()
relativeConstraints = genRelativeConstraints()
nphases = len(surfaces)
p0 = None
res = { "p0" : p0, "c0" : None, "nphases": nphases}
phaseData = [ {"K" : [copyKin(kinematicConstraints) for _ in range(len(surfaces[i]))], "S" : surfaces[i], "allRelativeK" : [relativeConstraints] } for i in range(nphases)]
res ["phaseData"] = phaseData
return res
import mpl_toolkits.mplot3d as a3
import matplotlib.colors as colors
import scipy as sp
def draw_rectangle(l, ax):
#~ plotPoints(ax,l)
lr = l + [l[0]]
cx = [c[0] for c in lr]
cy = [c[1] for c in lr]
cz = [c[2] for c in lr]
ax.plot(cx, cy, cz)
def draw_scene(surfaces, ax = None, color = "p"):
if ax is None:
fig = plt.figure()
ax = fig.add_subplot(111, projection="3d")
[draw_rectangle(l,ax) for l in all_surfaces]
return ax
############# main ###################
if __name__ == '__main__':
from sl1m.planner_l1_generic_equalities_as_ineq import solveMIPGurobi, plotQPRes, initGlobals
# ~ from sl1m.fix_sparsity import solveL1, solveMIP
initGlobals(nEffectors = 4)
pb = gen_flat_pb()
# ~ initPos = [[0.2, 0.6, 0.0],[-0.7, 0.1, 0.0], [-0.3, 1.2, 0.0], [-0.8, 1.0, 0.0] ]
initPos = None
endPos = None
initCom = None
endCom = None
# ~ initCom = [0.2, 0., 0.5]
# ~ endCom = [2, 0.7, 0.5]
# ~ endCom = [0.2, 1.2, 0.5]
# ~ initPos = [array([0.5, 0.0, 0.0]), array([-0.1, 0.0, -0.0]), array([0.3, 0.5, 0.0]), array([-0.0, 0.4, 0.0])]
# ~ endPos = [array()]
# ~ initPos = [[0.2, 0.6, 0.0] ]; initPos = [array(el) for el in initPos]
# ~ endPos = [[0.7, 0.6, 0.0] ]; endPos = [array(el) for el in endPos]
# ~ initCom = [0.2, 2., 0.35]
# ~ endCom = [0.2, 2., 0.35]
print ("initPos", initPos)
pb, res, time = solveMIPGurobi(pb, surfaces, MIP = True, draw_scene = None, plot = True, l1Contact = False, initPos = initPos, endPos = endPos, initCom = initCom, endCom= endCom)
ax = draw_scene(None)
plotQPRes(pb, res, ax=ax, plot_constraints = False, show = True)
......@@ -14,6 +14,8 @@ __ineq_com = [None for _ in range(len(limbNames))]
__ineq_relative = [ [None for _ in range(len(limbNames)-1)] for __ in range(len(limbNames))]
allFiles = []
# add foot offset
def com_in_limb_effector_frame_constraint(transform, limbId):
global __ineq_com
......@@ -40,6 +42,9 @@ def foot_in_limb_effector_frame_constraint(transform, limbId, footId):
assert (limbId != footId)
if __ineq_relative[limbId][realIdxFootId(limbId,footId)] is None:
# ~ filekin = insdir +"RF_constraints_in_LF.obj"
# ~ print("limbId", limbId)
# ~ print("footId", footId)
# ~ print("name ", "anymal_"+effectorNames[footId]+"_constraints_in_"+limbNames[limbId]+"_reduced.obj")
filekin = insdir +"anymal_"+effectorNames[footId]+"_constraints_in_"+limbNames[limbId]+"_reduced.obj"
obj = load_obj(filekin)
__ineq_relative[limbId][realIdxFootId(limbId,footId)] = as_inequalities(obj)
......
# Blender v2.82 (sub 7) OBJ File: ''
# www.blender.org
o anymal_LF_ADAPTER_TO_FOOT_constraints_in_RHleg.001
v -0.040536 0.005377 0.057735
v 0.690742 -0.009066 0.186888
v -0.049678 -1.036045 0.064317
v 0.404065 -0.850901 0.312882
v -0.099451 -0.711705 -0.115090
v 0.544582 0.078171 -0.248031
v 0.911384 -0.012295 -0.003741
v 1.056899 -0.286493 -0.012084
v 0.988656 -0.870833 -0.078626
v 0.655926 -1.091899 -0.120489
v 0.607129 -1.126902 0.176437
v 0.743033 -0.734358 -0.293145
v 0.182346 -0.960348 -0.208582
v 0.826666 -0.880464 0.261497
vn -0.0163 0.9793 0.2019
vn -0.4835 0.0098 0.8753
vn 0.9586 -0.0102 -0.2844
vn 0.5540 -0.8325 -0.0071
vn 0.4728 -0.2430 0.8470
vn -0.1557 0.0168 -0.9877
vn -0.2686 0.0572 -0.9616
vn 0.6137 0.4522 0.6472
vn -0.1665 0.2012 0.9653
vn 0.5608 0.1571 0.8129
vn -0.5102 -0.1714 0.8428
vn 0.6604 0.0084 -0.7509
vn 0.4137 -0.4757 -0.7763
vn -0.0435 0.4430 0.8955
vn -0.1272 -0.1043 -0.9864
vn 0.6243 0.3095 0.7173
vn 0.0841 -0.3912 0.9165
vn 0.2140 0.5549 -0.8039
vn -0.1425 -0.9786 -0.1484
vn 0.1132 0.9844 0.1346
vn -0.5783 -0.3296 0.7463
vn -0.6606 0.6903 -0.2951
vn -0.8940 0.1524 -0.4214
vn -0.1758 -0.8999 -0.3991
o anymal_LF_ADAPTER_TO_FOOT_constraints_in_RHleg.001_anymal_LF_ADAPTER_TO_FOOT_constraints_in_RHleg.009
v 0.633673 -1.075029 0.306300
v 0.573401 0.011249 0.240147
v -0.153913 -0.617436 -0.004976
v 0.098938 -0.907769 -0.228752
v 0.769842 0.045426 -0.189629
v 0.420095 0.083494 -0.159968
v 0.628751 -1.106725 -0.158208
v 0.870653 -0.966348 -0.240114
v 0.031622 0.000423 0.003187
v 0.076092 -1.064545 0.110170
v 1.032210 -0.536230 0.040106
v 0.949506 -0.879857 0.070838
v 1.004817 -0.113069 -0.002173
v 0.603048 -0.799873 0.361237
vn 0.6286 -0.0844 0.7732
vn -0.0799 0.0417 -0.9959
vn 0.3956 0.8834 0.2511
vn 0.6290 -0.0822 0.7731
vn -0.4337 0.0089 0.9010
vn -0.8207 -0.4955 -0.2845
vn 0.5120 0.1182 0.8508
vn 0.8628 0.0053 -0.5056
vn -0.3274 -0.2199 0.9189
vn -0.1152 0.9689 0.2191
vn 0.5522 -0.8322 0.0509
vn -0.2697 -0.8669 -0.4192
vn 0.9538 -0.2451 -0.1737
vn 0.6618 0.1030 -0.7425
vn 0.1183 0.9841 0.1323
vn -0.0467 -0.4421 -0.8958
vn -0.4940 0.2184 -0.8416
vn -0.4087 0.1201 0.9047
vn -0.0092 0.0722 -0.9973
vn 0.5347 0.1439 0.8327
vn 0.5249 -0.8450 0.1019
vn -0.0428 -0.9967 0.0685
vn -0.4080 0.1345 -0.9030
vn -0.4002 0.1081 0.9100
s off
f 1//1 2//1 6//1
f 3//2 4//2 1//2
f 5//3 1//3 3//3
f 9//4 11//4 10//4
f 5//5 6//5 1//5
f 13//6 6//6 12//6
f 13//7 5//7 6//7
f 3//8 13//8 5//8
f 4//9 2//9 1//9
f 2//10 14//10 8//10
f 8//11 6//11 12//11
f 9//12 12//12 8//12
f 9//13 10//13 12//13
f 13//14 10//14 12//14
f 14//15 4//15 2//15
f 7//16 2//16 8//16
f 11//17 14//17 4//17
f 11//18 3//18 4//18
f 7//19 2//19 6//19
f 10//20 3//20 11//20
f 6//21 8//21 7//21
f 14//22 9//22 11//22
f 14//23 8//23 9//23
f 13//24 10//24 3//24
f 1//1 12//1 14//1
f 8//2 6//2 5//2
f 5//3 2//3 13//3
f 14//4 12//4 11//4
f 3//5 10//5 14//5
f 3//6 4//6 10//6
f 2//7 11//7 13//7
f 8//8 13//8 11//8
f 10//9 1//9 14//9
f 6//10 9//10 2//10
f 7//11 12//11 1//11
f 7//12 10//12 4//12
f 12//13 8//13 11//13
f 8//14 5//14 13//14
f 6//15 2//15 5//15
f 7//16 4//16 8//16
f 3//17 6//17 4//17
f 2//18 3//18 14//18
f 8//19 4//19 6//19
f 2//20 14//20 11//20