Skip to content
GitLab
Projects
Groups
Snippets
/
Help
Help
Support
Community forum
Keyboard shortcuts
?
Submit feedback
Contribute to GitLab
Sign in / Register
Toggle navigation
Menu
Open sidebar
loco-3d
sl1m
Commits
80af5a85
Commit
80af5a85
authored
Feb 27, 2020
by
Steve T
Browse files
all constraints with inequality form written
parent
ffb1ea4b
Changes
3
Expand all
Hide whitespace changes
Inline
Side-by-side
constants_and_tools.py
View file @
80af5a85
...
...
@@ -30,7 +30,7 @@ zero3 = zeros(3)
eps
=
0.000001
#### surface to inequalities ####
def
convert_surface_to_inequality
(
s
):
def
convert_surface_to_inequality
(
s
,
eqAsIneq
):
#TODO does normal orientation matter ?
#it will for collisions
n
=
cross
(
s
[:,
1
]
-
s
[:,
0
],
s
[:,
2
]
-
s
[:,
0
])
...
...
@@ -38,13 +38,13 @@ def convert_surface_to_inequality(s):
for
i
in
range
(
3
):
n
[
i
]
=
-
n
[
i
]
n
/=
norm
(
n
)
return
surfacePointsToIneq
(
s
,
n
)
return
surfacePointsToIneq
(
s
,
n
,
eqAsIneq
)
def
replace_surfaces_with_ineq_in_phaseData
(
phase
):
phase
[
"S"
]
=
[
convert_surface_to_inequality
(
S
)
for
S
in
phase
[
"S"
]]
def
replace_surfaces_with_ineq_in_phaseData
(
phase
,
eqAsIneq
):
phase
[
"S"
]
=
[
convert_surface_to_inequality
(
S
,
eqAsIneq
)
for
S
in
phase
[
"S"
]]
def
replace_surfaces_with_ineq_in_problem
(
pb
):
[
replace_surfaces_with_ineq_in_phaseData
(
phase
)
for
phase
in
pb
[
"phaseData"
]]
def
replace_surfaces_with_ineq_in_problem
(
pb
,
eqAsIneq
=
False
):
[
replace_surfaces_with_ineq_in_phaseData
(
phase
,
eqAsIneq
)
for
phase
in
pb
[
"phaseData"
]]
def
ineqQHull
(
hull
):
A
=
hull
.
equations
[:,:
-
1
]
...
...
@@ -125,8 +125,9 @@ def default_transform_from_pos_normal(pos, normal):
return
vstack
(
[
hstack
([
rot
,
pos
.
reshape
((
-
1
,
1
))]),
[
0.
,
0.
,
0.
,
1.
]
]
)
EPSILON_EQ
=
0.005
#last is equality
def
surfacePointsToIneq
(
S
,
normal
):
def
surfacePointsToIneq
(
S
,
normal
,
eqAsIneq
):
n
=
array
(
normal
)
tr
=
default_transform_from_pos_normal
(
array
([
0.
,
0.
,
0.
]),
n
)
trinv
=
tr
.
copy
();
trinv
[:
3
,:
3
]
=
tr
[:
3
,:
3
].
T
;
...
...
@@ -140,10 +141,13 @@ def surfacePointsToIneq(S, normal):
#adding plane constraint
#plane equation given by first point and normal for instance
d
=
array
([
n
.
dot
(
S
[:,
0
])])
A
=
vstack
([
ine
.
A
,
n
,
-
n
])
b
=
concatenate
([
ine
.
b
,
d
,
-
d
]).
reshape
((
-
1
,))
A
=
vstack
([
ine
.
A
,
n
])
b
=
concatenate
([
ine
.
b
,
d
]).
reshape
((
-
1
,))
if
eqAsIneq
:
A
=
vstack
([
ine
.
A
,
n
,
-
n
])
b
=
concatenate
([
ine
.
b
,
d
+
EPSILON_EQ
,
-
d
+
EPSILON_EQ
]).
reshape
((
-
1
,))
else
:
A
=
vstack
([
ine
.
A
,
n
])
b
=
concatenate
([
ine
.
b
,
d
]).
reshape
((
-
1
,))
return
A
,
b
############ BENCHMARKING ###############
...
...
planner_l1_generic.py
View file @
80af5a85
...
...
@@ -37,7 +37,6 @@ GAMMA_START = None
ALPHA_START
=
None
#TODO : one surface: all inequalities apply, but without alpha
# breakdown of variables for each phase
...
...
@@ -87,8 +86,8 @@ def initGlobals(nEffectors):
global
DEFAULT_NUM_EQUALITY_CONSTRAINTS
N_EFFECTORS
=
nEffectors
# for one phase, for all i:
# [ c_x't, c_y't, c_z_1't, c_z_2't, p_i't, b_i't, g_i't, a_i_l't , d_i_l't ]
# [ 1 , 1 , 1 , 1 ,
3
*n_e, 2*n_e, 3*n_e, [0, n_s], [
0,
n_s]]
# [ c_x't, c_y't, c_z_1't, c_z_2't, p_i't, b_i't, g_i't, a_i_l't , d_i_l't ]
# [ 1 , 1 , 1 , 1 ,
2
*n_e, 2*n_e, 3*n_e, [0, n_s], [n_s
* n_e
]]
DEFAULT_NUM_VARS
=
N_EFFECTORS
*
8
+
4
BETA_START
=
4
+
3
*
N_EFFECTORS
GAMMA_START
=
BETA_START
+
2
*
N_EFFECTORS
...
...
@@ -135,10 +134,11 @@ def numIneqForPhase(phase, phaseId):
# all inequalities relative to each contact surface
ret
+=
sum
([
S
[
1
].
shape
[
0
]
-
1
for
S
in
phase
[
"S"
]])
numSurfaces
=
len
(
phase
[
"S"
])
if
numSurfaces
>
1
:
# ~
if numSurfaces > 1:
#adding all slack inequality comparison variables
# TODO: replace by two inequalities ???
ret
+=
numSurfaces
*
N_EFFECTORS
*
NUM_INEQ_SLACK_PER_SURFACE
# TODO: replace by two inequalities ???
#TODO : one surface: all inequalities apply, but without alpha
ret
+=
numSurfaces
*
N_EFFECTORS
*
NUM_INEQ_SLACK_PER_SURFACE
return
ret
def
getTotalNumVariablesAndIneqConstraints
(
pb
):
...
...
@@ -222,17 +222,28 @@ def SurfaceConstraint(phaseDataT, A, b, startCol, endCol, startRow):
def
SlackPositivityConstraint
(
phaseDataT
,
A
,
b
,
startCol
,
endCol
,
startRow
):
numSurfaces
=
len
(
phaseDataT
[
"S"
])
idRow
=
startRow
if
numSurfaces
>
1
:
colOmegaOffset
=
startCol
+
DEFAULT_NUM_VARS
colDeltaOffset
=
colOmegaOffset
+
numSurfaces
# ~ if numSurfaces > 1:
#TODO : one surface: all inequalities apply, but without alpha
colOmegaOffset
=
startCol
+
DEFAULT_NUM_VARS
if
numSurfaces
>
1
:
colDeltaOffset
=
colOmegaOffset
+
numSurfaces
for
i
in
range
(
numSurfaces
):
# - a0 <= a1_i <= a0
# TODO: beta entre 0 et 1
# M2 (-beta_x't) + M2 (-beta_y't) - M a0 <= a1_i <= M a0 + M2 (1 - beta_x't) + M2 (beta_y't)
for
footId
in
range
(
N_EFFECTORS
):
A
[
idRow
,
colOmegaOffset
+
i
]
=
[
-
1
];
# - a0 + a1_i <= 0
A
[
idRow
,
colDeltaOffset
+
i
*
footId
+
footId
]
=
[
1
];
# - a0 + a1_i <= 0
A
[
idRow
+
1
,
colOmegaOffset
+
i
]
=
[
-
1
];
# - a0 - a1_i <= 0
A
[
idRow
+
1
,
colDeltaOffset
+
i
*
footId
+
footId
]
=
[
-
1
];
# - a0 - a1_i <= 0
idRow
+=
NUM_INEQ_SLACK_PER_SURFACE
else
:
colDeltaOffset
=
colOmegaOffset
for
footId
in
range
(
N_EFFECTORS
):
A
[
idRow
,
colOmegaOffset
+
i
]
=
[
-
1
];
# - a0 + a1_i <= 0
A
[
idRow
,
colDeltaOffset
+
i
*
footId
+
footId
]
=
[
1
];
# - a0 + a1_i <= 0
A
[
idRow
+
1
,
colOmegaOffset
+
i
]
=
[
-
1
];
# - a0 - a1_i <= 0
A
[
idRow
+
1
,
colDeltaOffset
+
i
*
footId
+
footId
]
=
[
-
1
];
# - a0 - a1_i <= 0
idRow
+=
NUM_INEQ_SLACK_PER_SURFACE
return
idRow
...
...
planner_l1_generic_equalities_as_ineq.py
0 → 100644
View file @
80af5a85
This diff is collapsed.
Click to expand it.
Write
Preview
Supports
Markdown
0%
Try again
or
attach a new file
.
Cancel
You are about to add
0
people
to the discussion. Proceed with caution.
Finish editing this message first!
Cancel
Please
register
or
sign in
to comment