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
Humanoid Path Planner
talos-rbprm
Commits
9d1ff951
Commit
9d1ff951
authored
Sep 07, 2019
by
Pierre Fernbach
Browse files
[script] add python scripts to generate constraints for the LP formulation
parent
5273be39
Changes
2
Hide whitespace changes
Inline
Side-by-side
script/constants_and_tools.py
0 → 100644
View file @
9d1ff951
import
numpy
as
np
#~ from hpp.corbaserver.rbprm.hrp2 import Robot as rob
#~ from hpp.corbaserver.rbprm.tools.obj_to_constraints import load_obj, as_inequalities, rotate_inequalities
#~ from hpp_centroidal_dynamics import *
#~ from hpp_spline import *e
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
import
numpy
as
np
from
scipy.spatial
import
ConvexHull
#~ from hpp_bezier_com_traj import *
#~ from qp import solve_lp
#~ import eigenpy
#~ import cdd
#~ from curves import bezier3
from
random
import
random
as
rd
from
random
import
randint
as
rdi
from
numpy
import
squeeze
,
asarray
Id
=
matrix
([[
1.
,
0.
,
0.
],
[
0.
,
1.
,
0.
],
[
0.
,
0.
,
1.
]])
z
=
array
([
0.
,
0.
,
1.
])
zero3
=
zeros
(
3
)
def
generators
(
A
,
b
,
Aeq
=
None
,
beq
=
None
):
m
=
np
.
hstack
([
b
,
-
A
])
matcdd
=
cdd
.
Matrix
(
m
);
matcdd
.
rep_type
=
cdd
.
RepType
.
INEQUALITY
if
Aeq
is
not
None
:
meq
=
np
.
hstack
([
beq
,
-
Aeq
])
matcdd
.
extend
(
meq
.
tolist
(),
True
)
H
=
cdd
.
Polyhedron
(
matcdd
)
g
=
H
.
get_generators
()
return
[
array
(
g
[
el
][
1
:])
for
el
in
range
(
g
.
row_size
)],
H
def
filter
(
pts
):
hull
=
ConvexHull
(
pts
,
qhull_options
=
'Q12'
)
return
[
pts
[
i
]
for
i
in
hull
.
vertices
.
tolist
()]
def
ineq
(
pts
,
canonicalize
=
False
):
apts
=
array
(
pts
)
m
=
np
.
hstack
([
ones
((
apts
.
shape
[
0
],
1
)),
apts
])
matcdd
=
cdd
.
Matrix
(
m
);
matcdd
.
rep_type
=
cdd
.
RepType
.
GENERATOR
H
=
cdd
.
Polyhedron
(
matcdd
)
bmA
=
H
.
get_inequalities
()
if
canonicalize
:
bmA
.
canonicalize
()
Ares
=
zeros
((
bmA
.
row_size
,
bmA
.
col_size
-
1
))
bres
=
zeros
(
bmA
.
row_size
)
for
i
in
range
(
bmA
.
row_size
):
l
=
array
(
bmA
[
i
])
Ares
[
i
,:]
=
-
l
[
1
:]
bres
[
i
]
=
l
[
0
]
return
Ares
,
bres
def
ineqQHull
(
hull
):
A
=
hull
.
equations
[:,:
-
1
]
b
=
-
hull
.
equations
[:,
-
1
]
return
A
,
b
def
canon
(
A
,
b
):
m
=
np
.
hstack
([
b
,
-
A
])
matcdd
=
cdd
.
Matrix
(
m
);
matcdd
.
rep_type
=
1
H
=
cdd
.
Polyhedron
(
matcdd
)
bmA
=
H
.
get_inequalities
()
#~ bmA.canonicalize()
Ares
=
zeros
((
bmA
.
row_size
,
bmA
.
col_size
-
1
))
bres
=
zeros
((
bmA
.
row_size
,
1
))
for
i
in
range
(
bmA
.
row_size
):
#~ print "line ", array(bmA[i])
#~ print "A ", A[i][:]
#~ print "b ", b[i]
l
=
array
(
bmA
[
i
])
Ares
[
i
,:]
=
-
l
[
1
:]
bres
[
i
]
=
l
[
0
]
#~ print "Ares ",Ares[i,:]
#~ print "bres ",bres[i]
return
Ares
,
bres
def
genPolytope
(
A
,
b
):
pts
,
H
=
generators
(
A
,
b
)
apts
=
array
(
pts
)
if
len
(
apts
)
>
0
:
hull
=
ConvexHull
(
apts
)
return
hull
,
pts
,
apts
,
H
return
None
,
None
,
None
,
None
def
convex_hull_ineq
(
pts
):
return
None
m
=
cData
.
contactPhase_
.
getMass
()
#~ #get 6D polytope
(
H
,
h
)
=
ineqFromCdata
(
cData
)
#project to the space where aceleration is 0
D
=
zeros
((
6
,
3
))
D
[
3
:,:]
=
m
*
gX
d
=
zeros
((
6
,))
d
[:
3
]
=
-
m
*
g
A
=
H
.
dot
(
D
)
b
=
h
.
reshape
((
-
1
,))
-
H
.
dot
(
d
)
#add kinematic polytope
(
K
,
k
)
=
(
cData
.
Kin_
[
0
],
cData
.
Kin_
[
1
].
reshape
(
-
1
,))
resA
=
vstack
([
A
,
K
])
resb
=
concatenate
([
b
,
k
]).
reshape
((
-
1
,
1
))
#DEBUG
allpts
=
generators
(
resA
,
resb
)[
0
]
error
=
False
for
pt
in
allpts
:
print
"pt "
,
pt
assert
(
resA
.
dot
(
pt
.
reshape
((
-
1
,
1
)))
-
resb
).
max
()
<
0.001
,
"antecedent point not in End polytope"
+
str
((
resA
.
dot
(
pt
.
reshape
((
-
1
,
1
)))
-
resb
).
max
())
if
(
H
.
dot
(
w
(
m
,
pt
).
reshape
((
-
1
,
1
)))
-
h
).
max
()
>
0.001
:
error
=
True
print
"antecedent point not in End polytope"
+
str
((
H
.
dot
(
w
(
m
,
pt
).
reshape
((
-
1
,
1
)))
-
h
).
max
())
assert
not
error
,
str
(
len
(
allpts
))
return
(
resA
,
resb
)
#~ return (A, b)
#~ return (vstack([A, K]), None)
def
default_transform_from_pos_normal
(
pos
,
normal
):
#~ print "pos ", pos
#~ print "normal ", normal
f
=
array
([
0.
,
0.
,
1.
])
t
=
array
(
normal
)
v
=
np
.
cross
(
f
,
t
)
c
=
np
.
dot
(
f
,
t
)
if
c
>
0.99
:
rot
=
identity
(
3
)
else
:
u
=
v
/
norm
(
v
)
h
=
(
1.
-
c
)
/
(
1.
-
c
**
2
)
vx
,
vy
,
vz
=
v
rot
=
array
([[
c
+
h
*
vx
**
2
,
h
*
vx
*
vy
-
vz
,
h
*
vx
*
vz
+
vy
],
[
h
*
vx
*
vy
+
vz
,
c
+
h
*
vy
**
2
,
h
*
vy
*
vz
-
vx
],
[
h
*
vx
*
vz
-
vy
,
h
*
vy
*
vz
+
vx
,
c
+
h
*
vz
**
2
]])
return
vstack
(
[
hstack
([
rot
,
pos
.
reshape
((
-
1
,
1
))]),
[
0.
,
0.
,
0.
,
1.
]
]
)
import
os
def
continuous
(
h
,
initpts
):
dic
=
{}
pts
=
[]
for
i
,
pt
in
enumerate
(
h
.
vertices
.
tolist
()):
pts
+=
[
initpts
[
pt
]]
dic
[
pt
]
=
i
faces
=
[]
for
f
in
h
.
simplices
:
faces
+=
[[
dic
[
idx
]
+
1
for
idx
in
f
]]
return
pts
,
faces
def
hull_to_obj
(
h
,
pts
,
name
):
pts
,
faces
=
continuous
(
h
,
pts
)
f
=
open
(
name
,
"w"
)
#first write points
for
pt
in
pts
:
#~ print "??"
f
.
write
(
'v '
+
str
(
pt
[
0
])
+
' '
+
str
(
pt
[
1
])
+
' '
+
str
(
pt
[
2
])
+
'
\n
'
);
f
.
write
(
'g foo
\n
'
)
for
pt
in
faces
:
#~ print "???"
f
.
write
(
'f '
+
str
(
pt
[
0
])
+
' '
+
str
(
pt
[
1
])
+
' '
+
str
(
pt
[
2
])
+
'
\n
'
);
f
.
write
(
'g
\n
'
)
f
.
close
()
#~ function vertface2obj(v,f,name)
#~ % VERTFACE2OBJ Save a set of vertice coordinates and faces as a Wavefront/Alias Obj file
#~ % VERTFACE2OBJ(v,f,fname)
#~ % v is a Nx3 matrix of vertex coordinates.
#~ % f is a Mx3 matrix of vertex indices.
#~ % fname is the filename to save the obj file.
#~ fid = fopen(name,'w');
#~ for i=1:size(v,1)
#~ fprintf(fid,'v %f %f %f\n',v(i,1),v(i,2),v(i,3));
#~ end
#~ fprintf(fid,'g foo\n');
#~ for i=1:size(f,1);
#~ fprintf(fid,'f %d %d %d\n',f(i,1),f(i,2),f(i,3));
#~ end
#~ fprintf(fid,'g\n');
#~ fclose(fid);
script/relativeFootPositionQuasiFlat.py
0 → 100644
View file @
9d1ff951
from
hpp.corbaserver.rbprm.rbprmbuilder
import
Builder
from
hpp.corbaserver.rbprm.rbprmfullbody
import
FullBody
from
hpp.gepetto
import
Viewer
from
hpp.gepetto
import
ViewerFactory
from
numpy
import
array
from
numpy.linalg
import
norm
from
hpp.corbaserver.rbprm.talos
import
Robot
#from plot_plytopes import *
from
constants_and_tools
import
*
from
pinocchio
import
Quaternion
NUM_SAMPLES
=
18000
IT_DISPLAY_PROGRESS
=
NUM_SAMPLES
/
10
MIN_DIST_BETWEEN_FEET_Y
=
0.18
MAX_DIST_BETWEEN_FEET_X
=
0.35
MIN_HEIGHT_COM
=
0.3
MARGIN_FEET_SIDE
=
0.05
# margin used to constrain the com y position : if it's on the left of the left foot or on the right of the right foot for more than this margin, we reject this sample
fullBody
=
Robot
()
#~ fullBody.setJointBounds ("base_joint_xyz", [-2,2, -2, 2, -2, 2])
fullBody
.
setJointBounds
(
"root_joint"
,
[
-
20
,
20
,
-
20
,
20
,
-
20
,
20
])
fullBody
.
setConstrainedJointsBounds
()
#~ from hpp.corbaserver.rbprm.problem_solver import ProblemSolver
from
hpp.corbaserver
import
ProblemSolver
nbSamples
=
1
ps
=
ProblemSolver
(
fullBody
)
vf
=
ViewerFactory
(
ps
)
v
=
vf
.
createViewer
()
rootName
=
'root_joint'
nbSamples
=
10000
heuristic
=
"static"
print
"gen limb db"
fullBody
.
addLimb
(
fullBody
.
rLegId
,
fullBody
.
rleg
,
fullBody
.
rfoot
,
fullBody
.
rLegOffset
,
fullBody
.
rLegNormal
,
fullBody
.
rLegx
,
fullBody
.
rLegy
,
nbSamples
,
heuristic
,
0.01
,
kinematicConstraintsPath
=
fullBody
.
rLegKinematicConstraints
,
kinematicConstraintsMin
=
0.75
)
fullBody
.
runLimbSampleAnalysis
(
fullBody
.
rLegId
,
"ReferenceConfiguration"
,
True
)
fullBody
.
addLimb
(
fullBody
.
lLegId
,
fullBody
.
lleg
,
fullBody
.
lfoot
,
fullBody
.
lLegOffset
,
fullBody
.
rLegNormal
,
fullBody
.
lLegx
,
fullBody
.
lLegy
,
nbSamples
,
heuristic
,
0.01
,
kinematicConstraintsPath
=
fullBody
.
lLegKinematicConstraints
,
kinematicConstraintsMin
=
0.75
)
fullBody
.
runLimbSampleAnalysis
(
fullBody
.
lLegId
,
"ReferenceConfiguration"
,
True
)
print
"db generated."
rLegId
=
fullBody
.
rLegId
lLegId
=
fullBody
.
lLegId
rfoot
=
fullBody
.
rfoot
lfoot
=
fullBody
.
lfoot
rLegOffset
=
fullBody
.
rLegOffset
lLegOffset
=
fullBody
.
lLegOffset
#make sure this is 0
q_0
=
fullBody
.
getCurrentConfig
()
zeroConf
=
[
0
,
0
,
0
,
0
,
0
,
0
,
1.
]
q_0
[
0
:
7
]
=
zeroConf
fullBody
.
setCurrentConfig
(
q_0
)
effectors
=
[
rfoot
,
lfoot
]
limbIds
=
[
rLegId
,
lLegId
]
offsets
=
[
array
(
rLegOffset
),
array
(
lLegOffset
)]
import
numpy
as
np
points
=
[[],[]]
#~ compoints = [[[0.012471792486262121, 0.0015769611415203033, 0.8127583093263778]],[[0.012471792486262121, 0.0015769611415203033, 0.8127583093263778]]]
compoints
=
[[],[]]
success
=
0
fails
=
0
from
hpp.corbaserver.rbprm
import
rbprmstate
,
state_alg
from
scipy.spatial
import
ConvexHull
def
genFlat
():
q
=
fullBody
.
shootRandomConfig
()
q
[
0
:
7
]
=
zeroConf
fullBody
.
setCurrentConfig
(
q
)
#~ v(q)
posrf
=
fullBody
.
getJointPosition
(
rfoot
)[:
3
]
poslf
=
fullBody
.
getJointPosition
(
lfoot
)[:
3
]
s
=
rbprmstate
.
State
(
fullBody
,
q
=
q
,
limbsIncontact
=
limbIds
)
s
,
succ
=
state_alg
.
addNewContact
(
s
,
rLegId
,
posrf
,
[
0.
,
0.
,
1.
],
num_max_sample
=
0
)
if
succ
:
s
,
succ
=
state_alg
.
addNewContact
(
s
,
lLegId
,
poslf
,
[
0.
,
0.
,
1.
],
num_max_sample
=
0
)
if
succ
:
succ
=
fullBody
.
isConfigValid
(
q
)[
0
]
and
norm
(
array
(
posrf
[:
2
])
-
array
(
poslf
[:
2
])
)
>=
0.3
#print "sid = "+str(s.sId)
#~ if succ and norm (array(posrf[:2]) - array(poslf[:2]) ) <= 0.1:
if
succ
and
norm
(
array
(
posrf
)
-
array
(
poslf
)
)
<=
0.1
:
v
(
s
.
q
())
return
s
.
q
(),
succ
,
s
,
[
posrf
,
poslf
]
def
printFootPositionRelativeToOther
(
nbConfigs
):
for
i
in
range
(
0
,
nbConfigs
):
if
i
>
0
and
not
i
%
IT_DISPLAY_PROGRESS
:
print
str
(
int
((
i
*
100
)
/
nbConfigs
))
+
" % done"
q
,
succ
,
s
,
pos
=
genFlat
()
if
succ
:
global
success
success
+=
1
addCom
=
True
for
j
in
range
(
0
,
len
(
effectors
)):
#~ for j in range(1):
fullBody
.
setCurrentConfig
(
q
)
otheridx
=
(
j
+
1
)
%
2
#~ print "otheridx", otheridx
#~ print "q ", q[:3]
oeffectorName
=
effectors
[
otheridx
]
#~ oqEffector = fullBody.getJointPosition(oeffectorName)
pos_other
=
fullBody
.
getJointPosition
(
oeffectorName
)
#~ print "other pos 1", pos_other[:3]
effectorName
=
effectors
[
j
]
limbId
=
limbIds
[
j
]
qEffector
=
fullBody
.
getJointPosition
(
effectorName
)
#~ print "pos 1", qEffector[:3]
qtr
=
q
[:]
qtr
[:
3
]
=
[
qtr
[
0
]
-
pos_other
[
0
],
qtr
[
1
]
-
pos_other
[
1
],
qtr
[
2
]
-
pos_other
[
2
]
]
#~ for l in range(3):
#~ qtr[l] -= pos_other[l]
#~ qtr[i] -= qEffector[i]
fullBody
.
setCurrentConfig
(
qtr
)
effectorName
=
effectors
[
j
]
limbId
=
limbIds
[
j
]
qEffector
=
fullBody
.
getJointPosition
(
effectorName
)
#~ print "pos 2", qEffector[3:]
#~ print "pos 2", qEffector[:3]
#~ print "other effectorName 1", oeffectorName
#~ print "effectorName 1", effectorName
pos_other
=
fullBody
.
getJointPosition
(
oeffectorName
)
# check current joint pos is now zero
#~ print "other pos 2", pos_other[:3]
#~ v(qtr)
q0
=
Quaternion
(
qEffector
[
6
],
qEffector
[
3
],
qEffector
[
4
],
qEffector
[
5
])
rot
=
q0
.
matrix
()
#compute rotation matrix world -> local
p
=
qEffector
[
0
:
3
]
#(0,0,0) coordinate expressed in effector fram
rm
=
np
.
zeros
((
4
,
4
))
for
k
in
range
(
0
,
3
):
for
l
in
range
(
0
,
3
):
rm
[
k
,
l
]
=
rot
[
k
,
l
]
for
m
in
range
(
0
,
3
):
rm
[
m
,
3
]
=
qEffector
[
m
]
rm
[
3
,
3
]
=
1
invrm
=
np
.
linalg
.
inv
(
rm
)
#~ print invrm
#~ p = invrm.dot([0,0,0,1])
p
=
invrm
.
dot
([
0
,
0
,
0.
,
1
])
#~ print "p ", p
#~ print norm (array(posrf) - array(poslf) )
if
(
j
==
0
and
p
[
1
]
>
MIN_DIST_BETWEEN_FEET_Y
and
abs
(
p
[
0
])
<
MAX_DIST_BETWEEN_FEET_X
):
points
[
j
].
append
(
p
[:
3
])
elif
(
j
==
1
and
p
[
1
]
<
-
MIN_DIST_BETWEEN_FEET_Y
and
abs
(
p
[
0
])
<
MAX_DIST_BETWEEN_FEET_X
):
points
[
j
].
append
(
p
[:
3
])
else
:
addCom
=
False
#~ print (points[j])
#now compute coms
fullBody
.
setCurrentConfig
(
q
)
com
=
fullBody
.
getCenterOfMass
()
for
x
in
range
(
0
,
3
):
q
[
x
]
=
-
com
[
x
]
for
j
in
range
(
0
,
len
(
effectors
)):
effectorName
=
effectors
[
j
]
limbId
=
limbIds
[
j
]
qEffector
=
fullBody
.
getJointPosition
(
effectorName
)
q0
=
Quaternion
(
qEffector
[
6
],
qEffector
[
3
],
qEffector
[
4
],
qEffector
[
5
])
rot
=
q0
.
matrix
()
#compute rotation matrix world -> local
p
=
qEffector
[
0
:
3
]
#(0,0,0) coordinate expressed in effector fram
rm
=
np
.
zeros
((
4
,
4
))
for
k
in
range
(
0
,
3
):
for
l
in
range
(
0
,
3
):
rm
[
k
,
l
]
=
rot
[
k
,
l
]
for
m
in
range
(
0
,
3
):
rm
[
m
,
3
]
=
qEffector
[
m
]
rm
[
3
,
3
]
=
1
invrm
=
np
.
linalg
.
inv
(
rm
)
p
=
invrm
.
dot
([
0
,
0
,
0
,
1
])
# add offset
rp
=
array
(
p
[:
3
]
-
offsets
[
j
]).
tolist
()
#~ print "p ", p
#~ print "rp "
if
(
rp
[
2
]
<
MIN_HEIGHT_COM
):
addCom
=
False
if
addCom
:
if
j
==
1
:
if
rp
[
1
]
<
MARGIN_FEET_SIDE
:
compoints
[
j
].
append
(
rp
)
else
:
if
rp
[
1
]
>
-
MARGIN_FEET_SIDE
:
compoints
[
j
].
append
(
rp
)
#~ elif(p[1] < -0.05):
#~ points[j].append(p[:3])
#~ compoints[j].append(p[:3])
else
:
global
fails
fails
+=
1
#~ print fullBody.isConfigValid(q)[1]
#~ for j in range(0,len(limbIds)):
#~ f1=open('./'+str(limbIds[j])+'_com.erom', 'w+')
#~ for p in points[j]:
#~ f1.write(str(p[0]) + "," + str(p[1]) + "," + str(p[2]) + "\n")
#~ f1.close()
#~ printRootPosition(rLegId, rfoot, nbSamples)
#~ printRootPosition(lLegId, lfoot, nbSamples)
#~ printRootPosition(rarmId, rHand, nbSamples)
#~ printRootPosition(larmId, lHand, nbSamples)
printFootPositionRelativeToOther
(
NUM_SAMPLES
)
print
"successes "
,
success
print
"fails "
,
fails
hcomRF
=
ConvexHull
(
compoints
[
0
])
hcomLF
=
ConvexHull
(
compoints
[
1
])
hull_to_obj
(
hcomRF
,
compoints
[
0
],
"talos_COM_constraints_in_RF_effector_frame.obj"
)
hull_to_obj
(
hcomLF
,
compoints
[
1
],
"talos_COM_constraints_in_LF_effector_frame.obj"
)
hptsRF
=
ConvexHull
(
points
[
0
])
hptsLF
=
ConvexHull
(
points
[
1
])
hull_to_obj
(
hptsRF
,
points
[
0
],
"talos_LF_constraints_in_RF.obj"
)
hull_to_obj
(
hptsLF
,
points
[
1
],
"talos_RF_constraints_in_LF.obj"
)
#~ for k in range(2):
#~ hcom = ConvexHull(compoints[k])
#~ plot_hull(hcom, compoints[k], array(compoints[k]))
#~ hpts = ConvexHull(points[k])
#~ plot_hull(hpts, points[k], array(points[k]), color = "b", plot = k == 1 and True)
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