Commit 24cae4e5 authored by stevet's avatar stevet Committed by Steve T
Browse files

testing scaling of variables for better fixing sparsity

parent 83ac5b92
......@@ -8,6 +8,7 @@ from sl1m.problem_definition import *
#### constraint specification ####
DEFAULT_NUM_VARS = 4
SLACK_SCALE = 10.
#extra variables when multiple surface: a0, variable for inequality, a1 slack for equality constraint, then a2 = |a1|
#so vars are x, y, z, zcom, a0, a1
......@@ -196,7 +197,7 @@ def SurfaceConstraint(phaseDataT, A, b, startCol, endCol, startRow):
A[sRow:idRow, startCol:startCol+footExpressionMatrix.shape[1]] = S[:-1,:].dot(footExpressionMatrix)
b[sRow:idRow ] = s[:-1]
if nSurfaces >1:
A[sRow:idRow, startCol+idS] = -ones(idRow-sRow)
A[sRow:idRow, startCol+idS] = -ones(idRow-sRow) * SLACK_SCALE
sRow = idRow
idS += NUM_SLACK_PER_SURFACE
return idRow
......@@ -226,7 +227,7 @@ def EqualityConstraint(phaseDataT, E, e, startCol, endCol, startRowEq):
for (S,s) in phaseDataT["S"]:
E[sRow, startCol:startCol+footExpressionMatrix.shape[1]] = S[-1,:].dot(footExpressionMatrix)
#~ E[sRow, startCol+idS+1] = -1 # E x + a1 = e
E[sRow, startCol+idS+1] = -1 # E x + a1 = e
E[sRow, startCol+idS+1] = -1 * SLACK_SCALE # E x + a1 = e
e[sRow] = s[-1]
idS += NUM_SLACK_PER_SURFACE
sRow += 1
......
......@@ -70,9 +70,9 @@ end = [astep6, astep7, abridge, aplatfo ]
surfaces = [[arub2],[arub3,arub2],[arub4,arub5],[arub5],[arub6,arub7],[arub6,arub7],[arub75,arub9],[arub9],[arub9,afloor],[afloor,astep1],[astep1,astep2],[astep2,astep3],[astep3,astep4],[astep4,astep5],[astep5,astep6],[astep6,astep7],[astep6,astep7],[astep7,abridge],[astep7,abridge],[abridge],[abridge],[abridge],[abridge],[abridge],[abridge],[abridge],[abridge],[abridge],[abridge],[abridge,aplatfo],[abridge,aplatfo], [aplatfo] ]
#~ surfaces = [[arub2],[arub3,arub2],[arub4,arub5],[arub5],[arub6,arub7],[arub6,arub7],[arub75,arub9],[arub9],[arub9,afloor],[afloor,astep1],[astep1,astep2],[astep2,astep3],[astep3,astep4],[astep4,astep5],[astep5,astep6],[astep6,astep7],[astep6,astep7],[astep7,abridge],[astep7,abridge],[abridge],[abridge],[abridge],[abridge],[abridge],[abridge],[abridge],[abridge],[abridge],[abridge],[abridge,aplatfo],[abridge,aplatfo], [aplatfo] ]
#~ surfaces = [[arub2],[arub3,arub2],[arub4],[arub5],[arub6],[arub7],[arub75],allrub,[afloor],[afloor, astep1],[afloor, astep1],[astep1,astep2,astep3],[astep4,astep2,astep3],[astep4,astep2,astep3],[astep4,astep2,astep5],[astep6,astep2,astep5],[astep6],[astep7],end,end,[abridge],[abridge],[abridge],[abridge],[abridge],[abridge],[abridge],[abridge],[abridge],[abridge],[aplatfo] ]
surfaces = [[arub2],[arub3,arub2],[arub4],[arub5],[arub6],[arub7],[arub75],allrub,[afloor],[afloor, astep1],[afloor, astep1],[astep1,astep2,astep3],[astep4,astep2,astep3],[astep4,astep2,astep3],[astep4,astep2,astep5],[astep6,astep2,astep5],[astep6],[astep7],end,end,[abridge],[abridge],[abridge],[abridge],[abridge],[abridge],[abridge],[abridge],[abridge],[abridge],[aplatfo] ]
def gen_stair_pb():
kinematicConstraints = genKinematicConstraints(left_foot_constraints, right_foot_constraints)
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment