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
Jason Chemin
hpp-rbprm-corba
Commits
8bc0f1e4
Commit
8bc0f1e4
authored
Jun 16, 2018
by
Pierre Fernbach
Browse files
[tool] add/update tools script
parent
ed054fb4
Changes
26
Hide whitespace changes
Inline
Side-by-side
CMakeLists.txt
View file @
8bc0f1e4
...
...
@@ -139,6 +139,8 @@ install(FILES
data/urdf/flat_step.urdf
data/urdf/flat_hole.urdf
data/urdf/cross_gap.urdf
data/urdf/floor_bauzil.urdf
data/urdf/floor_bauzil_obstacles.urdf
DESTINATION
${
CATKIN_PACKAGE_SHARE_DESTINATION
}
/urdf
)
install
(
FILES
...
...
@@ -215,6 +217,8 @@ install(FILES
data/srdf/flat_step.srdf
data/srdf/flat_hole.srdf
data/srdf/cross_gap.srdf
data/srdf/floor_bauzil.srdf
data/srdf/floor_bauzil_obstacles.srdf
DESTINATION
${
CATKIN_PACKAGE_SHARE_DESTINATION
}
/srdf
)
install
(
FILES
...
...
@@ -295,6 +299,8 @@ install(FILES
data/meshes/flat_hole.stl
data/meshes/cross_gap.stl
data/meshes/cross_gap2.stl
data/meshes/floor_bauzil.stl
data/meshes/floor_bauzil_obstacles.stl
DESTINATION
${
CATKIN_PACKAGE_SHARE_DESTINATION
}
/meshes
)
...
...
data/meshes/darpa_no_mid.stl
View file @
8bc0f1e4
No preview for this file type
data/meshes/floor_bauzil.stl
View file @
8bc0f1e4
No preview for this file type
idl/hpp/corbaserver/rbprm/rbprmbuilder.idl
View file @
8bc0f1e4
...
...
@@ -771,7 +771,7 @@ module hpp
boolean
areKinematicsConstraintsVerifiedForState
(
in
unsigned
short
stateFrom
,
in
floatSeq
point
)
raises
(
Error
)
;
boolean
isReachableFromState
(
in
unsigned
short
stateFrom
,
in
unsigned
short
stateTo
)
raises
(
Error
)
;
floatSeq
isReachableFromState
(
in
unsigned
short
stateFrom
,
in
unsigned
short
stateTo
)
raises
(
Error
)
;
floatSeq
isDynamicallyReachableFromState
(
in
unsigned
short
stateFrom
,
in
unsigned
short
stateTo
,
in
boolean
addPathPerPhase
,
in
floatSeq
timings
,
in
short
numPointsPerPhase
)
raises
(
Error
)
;
...
...
script/dynamic/talos/flatGround_pyrene_interp.py
View file @
8bc0f1e4
...
...
@@ -7,7 +7,7 @@ from hpp.corbaserver.rbprm.rbprmstate import State,StateHelper
#from display_tools import *
import
talos.flatGround_pyrene_pathKino
as
tp
import
time
from
planning.
robot_config.talos
import
*
from
robot_config.talos
import
*
tPlanning
=
tp
.
tPlanning
...
...
@@ -156,7 +156,7 @@ print "number of configs :", len(configsFull)
from
planning.
configs.talos_flatGround
import
*
from
configs.talos_flatGround
import
*
from
generate_contact_sequence
import
*
beginState
=
0
...
...
script/dynamic/talos/flatGround_pyrene_pathKino.py
View file @
8bc0f1e4
...
...
@@ -4,7 +4,7 @@ from hpp.corbaserver import Client
from
hpp.corbaserver.robot
import
Robot
as
Parent
from
hpp.corbaserver.rbprm.problem_solver
import
ProblemSolver
import
omniORB.any
from
planning.configs.straight_walk_config
import
*
from
configs.talos_flatGround
import
*
import
time
class
Robot
(
Parent
):
...
...
script/tools/bench_contactGeneration.py
0 → 100644
View file @
8bc0f1e4
from
stair_bauzil_hrp2_interp
import
*
import
generate_muscod_problem
as
mp
from
pathlib2
import
Path
import
muscodSSH
as
ssh
tryWarmStart
=
True
print
"run bench with feasibility criterion"
com
=
fullBody
.
getCenterOfMass
()
if
com
[
0
]
>
1.25
:
success
=
True
else
:
success
=
False
numConf
=
len
(
configs
)
if
success
:
# muscod without warm start :
if
Path
(
CONTACT_SEQUENCE_WHOLEBODY_FILE
).
is_file
():
os
.
remove
(
CONTACT_SEQUENCE_WHOLEBODY_FILE
)
filename_xml
=
OUTPUT_DIR
+
"/"
+
OUTPUT_SEQUENCE_FILE
mp
.
generate_muscod_problem
(
filename_xml
,
True
)
successMuscod
,
ssh_ok
=
ssh
.
call_muscod
()
time
.
sleep
(
5.
)
# wait for sync of the ~/home (worst case, usually 0.1 is enough ... )
muscodConverged
=
successMuscod
and
Path
(
CONTACT_SEQUENCE_WHOLEBODY_FILE
).
is_file
()
if
tryWarmStart
:
if
not
success
:
# generate warm start from planning :
if
Path
(
CONTACT_SEQUENCE_WHOLEBODY_FILE
).
is_file
():
os
.
remove
(
CONTACT_SEQUENCE_WHOLEBODY_FILE
)
filename_xml
=
OUTPUT_DIR
+
"/"
+
OUTPUT_SEQUENCE_FILE
cs
=
generateContactSequenceWithInitGuess
(
ps
,
fullBody
,
configs
,
beginState
,
endState
)
cs
.
saveAsXML
(
filename
,
"ContactSequence"
)
mp
.
generate_muscod_problem
(
filename_xml
,
True
)
successMuscod
,
ssh_ok
=
ssh
.
call_muscod
()
time
.
sleep
(
5.
)
# wait for sync of the ~/home (worst case, usually 0.1 is enough ... )
muscodWarmStartConverged
=
successMuscod
and
Path
(
CONTACT_SEQUENCE_WHOLEBODY_FILE
).
is_file
()
else
:
muscodWarmStartConverged
=
True
else
:
muscodWarmStartConverged
=
False
else
:
muscodConverged
=
False
muscodWarmStartConverged
=
False
tInterpolateConfigs
=
0.
numConf
=
0.
name
=
"/local/fernbac/bench_iros18/bench_contactGeneration/stairsMC_croc.log"
f
=
open
(
name
,
"a"
)
f
.
write
(
"new
\n
"
)
f
.
write
(
"success "
+
str
(
success
)
+
"
\n
"
)
f
.
write
(
"muscodNoWarmStart "
+
str
(
muscodConverged
)
+
"
\n
"
)
f
.
write
(
"muscodWarmStart "
+
str
(
muscodWarmStartConverged
)
+
"
\n
"
)
f
.
write
(
"time "
+
str
(
tInterpolateConfigs
)
+
"
\n
"
)
f
.
write
(
"configs "
+
str
(
numConf
)
+
"
\n
"
)
f
.
close
()
script/tools/bench_contactGeneration_noCroc.py
0 → 100644
View file @
8bc0f1e4
from
stair_bauzil_hrp2_interp
import
*
import
generate_muscod_problem
as
mp
from
pathlib2
import
Path
import
muscodSSH
as
ssh
tryWarmStart
=
False
print
"run bench without feasibility criterion"
com
=
fullBody
.
getCenterOfMass
()
if
com
[
0
]
>
1.3
:
success
=
True
else
:
success
=
False
#success = False
numConf
=
len
(
configs
)
if
success
:
# muscod without warm start :
if
Path
(
CONTACT_SEQUENCE_WHOLEBODY_FILE
).
is_file
():
os
.
remove
(
CONTACT_SEQUENCE_WHOLEBODY_FILE
)
filename_xml
=
OUTPUT_DIR
+
"/"
+
OUTPUT_SEQUENCE_FILE
mp
.
generate_muscod_problem
(
filename_xml
,
True
)
successMuscod
,
ssh_ok
=
ssh
.
call_muscod
()
time
.
sleep
(
5.
)
# wait for sync of the ~/home (worst case, usually 0.1 is enough ... )
muscodConverged
=
successMuscod
and
Path
(
CONTACT_SEQUENCE_WHOLEBODY_FILE
).
is_file
()
crocConverged
=
True
for
id_state
in
range
(
beginState
,
endState
):
pid
=
fullBody
.
isDynamicallyReachableFromState
(
id_state
,
id_state
+
1
,
numPointsPerPhases
=
0
)
if
len
(
pid
)
==
0
:
print
"Croc did not converge for state "
+
str
(
id_state
)
crocConverged
=
False
else
:
crocConverged
=
False
muscodConverged
=
False
tInterpolateConfigs
=
0.
numConf
=
0.
name
=
"/local/fernbac/bench_iros18/bench_contactGeneration/stairsMC_noCroc.log"
f
=
open
(
name
,
"a"
)
f
.
write
(
"new
\n
"
)
f
.
write
(
"success "
+
str
(
success
)
+
"
\n
"
)
f
.
write
(
"muscodNoWarmStart "
+
str
(
muscodConverged
)
+
"
\n
"
)
f
.
write
(
"crocConverged "
+
str
(
crocConverged
)
+
"
\n
"
)
f
.
write
(
"time "
+
str
(
tInterpolateConfigs
)
+
"
\n
"
)
f
.
write
(
"configs "
+
str
(
numConf
)
+
"
\n
"
)
f
.
close
()
script/tools/check_qp.py
View file @
8bc0f1e4
from
hpp.corbaserver.rbprm.rbprmstate
import
State
,
StateHelper
from
disp_bezier
import
*
max_acc
=
1
pointsPerPhase
=
4
import
numpy
as
np
max_acc
=
5.
pointsPerPhase
=
5
ROUND
=
1000.
global
curves_initGuess
...
...
@@ -10,6 +11,14 @@ global timings_initGuess
curves_initGuess
=
[]
timings_initGuess
=
[]
def
stdVecToMatrix
(
std_vector
):
vec_l
=
[]
for
vec
in
std_vector
:
vec_l
.
append
(
vec
)
res
=
np
.
hstack
(
tuple
(
vec_l
))
return
res
def
compute_time_array
(
t_qp
,
t_c
):
...
...
@@ -45,57 +54,70 @@ def check_acceleration_bounds(ddc_qp,t_discretized):
print
"acceleration between : "
+
str
(
min
)
+
" ; "
+
str
(
max
)
return
True
;
def
check_projection
(
s
,
c
,
dc
,
ddc
,
t
):
def
check_projection
(
s
,
c
,
t
):
return
True
success
=
s
.
projectToCOM
(
c
(
t
).
transpose
().
tolist
()[
0
],
500
)
if
not
success
:
print
"Unable to project to com at time : "
+
str
(
t
)
return
False
# check dynamic stability
else
:
return
True
def
check_stability
(
s
,
c
,
dc
,
ddc
,
t
):
q
=
s
.
q
()
q
[
-
6
:
-
3
]
=
dc
(
t
).
transpose
().
tolist
()[
0
]
q
[
-
3
:]
=
ddc
(
t
).
transpose
().
tolist
()[
0
]
success
=
s
.
fullBody
.
isConfigBalanced
(
q
,
s
.
getLimbsInContact
())
success
=
s
.
fullBody
.
isConfigBalanced
(
q
,
s
.
getLimbsInContact
()
,
CoM
=
c
(
t
).
transpose
().
tolist
()[
0
]
)
if
not
success
:
print
"UNSTABLE at time : "
+
str
(
t
)
return
False
return
True
#print "q = ",q
return
False
else
:
return
True
def
check_projection_path
(
s0
,
s1
,
c
,
dc
,
ddc
,
t_qp
,
t_per_phase
):
def
check_projection_path
(
s0
,
s1
,
c
,
dc
,
ddc
,
t_per_phase
):
kin_valid
=
True
stab_valid
=
True
moving_limb
=
s0
.
contactsVariations
(
s1
)
s0_orig
=
State
(
s0
.
fullBody
,
q
=
s0
.
q
(),
limbsIncontact
=
s0
.
getLimbsInContact
())
s1_orig
=
State
(
s1
.
fullBody
,
q
=
s1
.
q
(),
limbsIncontact
=
s1
.
getLimbsInContact
())
if
len
(
moving_limb
)
>
1
:
print
"Too many contact variation between adjacent states"
return
False
return
False
,
False
if
len
(
moving_limb
)
==
0
:
print
"No contact between adjacent states"
return
True
print
"test for phase 0 :"
return
True
,
True
#
print "test for phase 0 :"
for
t
in
t_per_phase
[
0
]:
if
not
check_projection
(
s0
,
c
,
dc
,
ddc
,
t
):
return
False
if
kin_valid
and
not
check_projection
(
s0
,
c
,
t
):
kin_valid
=
False
if
stab_valid
and
not
check_stability
(
s0_orig
,
c
,
dc
,
ddc
,
t
):
stab_valid
=
False
smid
,
success
=
StateHelper
.
removeContact
(
s0
,
moving_limb
[
0
])
if
not
success
:
smid_orig
,
success_orig
=
StateHelper
.
removeContact
(
s0_orig
,
moving_limb
[
0
])
if
not
(
success
and
success_orig
):
print
"Error in creation of intermediate state"
return
False
print
"test for phase 1 :"
return
False
,
False
#
print "test for phase 1 :"
for
t
in
t_per_phase
[
1
]:
#
if
t == t_per_phase[0][-1]
:
#
t -= EPS
if
not
check_projection
(
smid
,
c
,
dc
,
ddc
,
t
):
return
False
print
"test for phase 2"
if
kin_valid
and
not
check_projection
(
smid
,
c
,
t
)
:
kin_valid
=
False
if
stab_valid
and
not
check_stability
(
smid_orig
,
c
,
dc
,
ddc
,
t
):
stab_valid
=
False
#
print "test for phase 2"
# go in reverse order for p2, it's easier for the projection :
for
i
in
range
(
1
,
len
(
t_per_phase
[
2
])
+
1
):
t
=
t_per_phase
[
1
][
-
i
]
if
not
check_projection
(
s1
,
c
,
dc
,
ddc
,
t
):
return
False
t
=
t_per_phase
[
1
][
-
1
]
if
not
check_projection
(
s1
,
c
,
dc
,
ddc
,
t
):
return
False
return
True
t
=
t_per_phase
[
2
][
-
i
]
if
kin_valid
and
not
check_projection
(
s1
,
c
,
t
):
kin_valid
=
False
if
stab_valid
and
not
check_stability
(
s1_orig
,
c
,
dc
,
ddc
,
t
):
stab_valid
=
False
return
kin_valid
,
stab_valid
def
check_one_transition
(
ps
,
fullBody
,
s0
,
s1
,
r
=
None
,
pp
=
None
):
global
curves_initGuess
...
...
@@ -116,14 +138,96 @@ def check_one_transition(ps,fullBody,s0,s1,r=None,pp=None):
print
"### test acceleration bounds ###"
valid
=
valid
and
check_acceleration_bounds
(
ddc_qp
,
t_discretized
)
print
"### test kinematic projection ###"
valid
=
valid
and
check_projection_path
(
s0
,
s1
,
c_qp
,
dc_qp
,
ddc_qp
,
t_
qp
,
t_
per_phase
)
valid
=
valid
and
check_projection_path
(
s0
,
s1
,
c_qp
,
dc_qp
,
ddc_qp
,
t_per_phase
)
curves_initGuess
.
append
(
c_qp
)
timings_initGuess
.
append
(
t_qp
)
return
valid
def
genTimeArray
(
Ts
,
dt
):
t
=
0.
res
=
[]
current_t
=
0.
for
phase_id
in
range
(
len
(
Ts
)):
t_phase
=
[]
while
t
<=
(
current_t
+
Ts
[
phase_id
]):
t_phase
+=
[
t
]
t
+=
dt
res
+=
[
t_phase
]
current_t
+=
Ts
[
phase_id
]
return
res
def
check_traj_valid
(
ps
,
fullBody
,
s0_
,
s1_
,
pIds
,
dt
=
0.001
):
# create copy of original states :
s0
=
State
(
fullBody
,
q
=
s0_
.
q
(),
limbsIncontact
=
s0_
.
getLimbsInContact
())
s1
=
State
(
fullBody
,
q
=
s1_
.
q
(),
limbsIncontact
=
s1_
.
getLimbsInContact
())
fullBody
.
setStaticStability
(
False
)
Ts
=
[
ps
.
pathLength
(
int
(
pIds
[
1
])),
ps
.
pathLength
(
int
(
pIds
[
2
])),
ps
.
pathLength
(
int
(
pIds
[
3
]))]
t_array
=
genTimeArray
(
Ts
,
dt
)
#print "Time array : ",t_array
c_qp
=
fullBody
.
getPathAsBezier
(
int
(
pIds
[
0
]))
dc_qp
=
c_qp
.
compute_derivate
(
1
)
ddc_qp
=
dc_qp
.
compute_derivate
(
1
)
return
check_projection_path
(
s0
,
s1
,
c_qp
,
dc_qp
,
ddc_qp
,
t_array
)
def
check_muscod_traj
(
fullBody
,
cs
,
s0_
,
s1_
):
if
cs
.
size
()
!=
3
:
print
"Contact sequence must be of size 3 (one step)"
return
False
,
False
kin_valid
=
True
stab_valid
=
True
stab_valid_discretized
=
True
# create copy of original states :
s0
=
State
(
fullBody
,
q
=
s0_
.
q
(),
limbsIncontact
=
s0_
.
getLimbsInContact
())
s1
=
State
(
fullBody
,
q
=
s1_
.
q
(),
limbsIncontact
=
s1_
.
getLimbsInContact
())
moving_limb
=
s0
.
contactsVariations
(
s1
)
smid
,
success
=
StateHelper
.
removeContact
(
s0
,
moving_limb
[
0
])
if
not
success
:
print
"Error in creation of intermediate state"
return
False
,
False
phases
=
[]
c_array
=
[]
dc_array
=
[]
ddc_array
=
[]
t_array
=
[]
states
=
[
s0
,
smid
,
s1
]
states_copy
=
[
State
(
fullBody
,
q
=
s0_
.
q
(),
limbsIncontact
=
s0_
.
getLimbsInContact
()),
State
(
fullBody
,
q
=
smid
.
q
(),
limbsIncontact
=
smid
.
getLimbsInContact
()),
State
(
fullBody
,
q
=
s1_
.
q
(),
limbsIncontact
=
s1_
.
getLimbsInContact
())
]
for
k
in
range
(
3
)
:
phases
+=
[
cs
.
contact_phases
[
k
]]
state_traj
=
stdVecToMatrix
(
phases
[
k
].
state_trajectory
).
transpose
()
control_traj
=
stdVecToMatrix
(
phases
[
k
].
control_trajectory
).
transpose
()
c_array
=
state_traj
[:,:
3
]
dc_array
=
state_traj
[:,
3
:
6
]
ddc_array
=
control_traj
[:,:
3
]
t_array
=
stdVecToMatrix
(
phases
[
k
].
time_trajectory
).
transpose
()
for
i
in
range
(
len
(
c_array
)):
#if kin_valid and not states[k].projectToCOM(c_array[i].tolist()[0],500) :
# kin_valid = False
q
=
states_copy
[
k
].
q
()
q
[
-
6
:
-
3
]
=
dc_array
[
i
].
tolist
()[
0
]
q
[
-
3
:]
=
ddc_array
[
i
].
tolist
()[
0
]
if
stab_valid_discretized
and
not
states_copy
[
k
].
fullBody
.
isConfigBalanced
(
q
,
states_copy
[
k
].
getLimbsInContact
(),
CoM
=
c_array
[
i
].
tolist
()[
0
])
:
stab_valid_discretized
=
False
for
i
in
range
(
int
(
len
(
c_array
)
/
100
)):
q
=
states_copy
[
k
].
q
()
q
[
-
6
:
-
3
]
=
dc_array
[
i
*
100
].
tolist
()[
0
]
q
[
-
3
:]
=
ddc_array
[
i
*
100
].
tolist
()[
0
]
if
stab_valid
and
not
states_copy
[
k
].
fullBody
.
isConfigBalanced
(
q
,
states_copy
[
k
].
getLimbsInContact
(),
CoM
=
c_array
[
i
*
100
].
tolist
()[
0
])
:
stab_valid
=
False
return
kin_valid
,
stab_valid
,
stab_valid_discretized
def
check_contact_plan
(
ps
,
r
,
pp
,
fullBody
,
idBegin
,
idEnd
):
fullBody
.
client
.
basic
.
robot
.
setExtraConfigSpaceBounds
([
-
0
,
0
,
-
0
,
0
,
-
0
,
0
,
0
,
0
,
0
,
0
,
0
,
0
])
...
...
@@ -166,4 +270,12 @@ valid = True
t_qp = [ps.pathLength(int(pid[1])), ps.pathLength(int(pid[2])), ps.pathLength(int(pid[3]))]
t_discretized,t_per_phase = compute_time_array(t_qp)
"""
\ No newline at end of file
"""
"""
cs = ContactSequenceHumanoid(0)
cs.loadFromXML('/home/pfernbac/Documents/muscod/uc-dual/PROBLEM/random/res/contact_sequence_trajectory.xml',CONTACT_SEQUENCE_XML_TAG)
"""
script/tools/cut_contact_sequence.py
View file @
8bc0f1e4
...
...
@@ -5,11 +5,11 @@ from pinocchio.utils import *
import
locomote
from
locomote
import
WrenchCone
,
SOC6
,
ControlType
,
IntegratorType
,
ContactPatch
,
ContactPhaseHumanoid
,
ContactSequenceHumanoid
import
planning.
generate_muscod_problem
as
mp
import
generate_muscod_problem
as
mp
import
muscodSSH
as
ssh
from
planning.
config
import
*
from
config
import
*
statesPerStep
=
4
# number of double support configs from the planning per call to muscod
statesPerStep
=
5
# number of double support configs from the planning per call to muscod
stepSize
=
statesPerStep
*
2
-
1
# contact_sequence contain double support AND simple support states
def
solveMuscodProblem
(
configsFull
,
cs
):
...
...
script/tools/displayContactFeet_justin.py
0 → 100644
View file @
8bc0f1e4
name
=
's2'
gui
.
addSphere
(
name
,
0.01
,[
1
,
0
,
0
,
1
])
gui
.
setVisibility
(
name
,
"ALWAYS_ON_TOP"
)
gui
.
addToGroup
(
name
,
"world"
)
gui
.
applyConfiguration
(
name
,
p
)
gui
.
refresh
()
gui
=
r
.
client
.
gui
name
=
's2'
gui
.
addSphere
(
name
,
0.01
,[
1
,
0
,
0
,
1
])
gui
.
setVisibility
(
name
,
"ALWAYS_ON_TOP"
)
gui
.
addToGroup
(
name
,
r
.
sceneName
)
gui
.
applyConfiguration
(
name
,
p
)
gui
.
refresh
()
script/tools/gen_hrp2_statically_balanced_positions_2d_state.py
View file @
8bc0f1e4
...
...
@@ -4,7 +4,6 @@ from hpp.gepetto import Viewer
from
hpp.corbaserver.rbprm.problem_solver
import
ProblemSolver
import
numpy
as
np
#from hpp.corbaserver.rbprm.tools.cwc_trajectory import *
from
hpp
import
Error
as
hpperr
from
numpy
import
array
,
matrix
from
hpp.corbaserver.rbprm.rbprmstate
import
State
,
StateHelper
...
...
@@ -21,7 +20,7 @@ srdfSuffix = ""
fullBody
=
FullBody
()
fullBody
.
loadFullBodyModel
(
urdfName
,
rootJointType
,
meshPackageName
,
packageName
,
urdfSuffix
,
srdfSuffix
)
fullBody
.
setJointBounds
(
"base_joint_xyz"
,
[
-
0.
3
,
0.
3
,
-
0.
3
,
0.
3
,
0.6
,
0.7
])
fullBody
.
setJointBounds
(
"base_joint_xyz"
,
[
-
0.
7
,
0.
7
,
-
0.
7
,
0.
7
,
0.6
,
0.7
])
fullBody
.
setJointBounds
(
"CHEST_JOINT0"
,
[
0.
,
0.
])
fullBody
.
setJointBounds
(
"CHEST_JOINT1"
,
[
0.
,
0.
])
fullBody
.
setJointBounds
(
"HEAD_JOINT0"
,
[
0.
,
0.
])
...
...
@@ -61,7 +60,8 @@ rLegLimbOffset=[0,0,-0.035]#0.035
rLegNormal
=
[
0
,
0
,
1
]
rLegx
=
0.09
;
rLegy
=
0.05
#fullBody.addLimbDatabase("./db/hrp2_rleg_db.db",rLegId,"forward")
fullBody
.
addLimb
(
rLegId
,
rLeg
,
''
,
rLegOffset
,
rLegNormal
,
rLegx
,
rLegy
,
100000
,
"manipulability"
,
0.01
,
"_6_DOF"
,
limbOffset
=
rLegLimbOffset
)
fullBody
.
addLimb
(
rLegId
,
rLeg
,
''
,
rLegOffset
,
rLegNormal
,
rLegx
,
rLegy
,
100000
,
"manipulability"
,
0.01
,
"_6_DOF"
,
kinematicConstraintsPath
=
"package://hpp-rbprm-corba/com_inequalities/fullSize/RLEG_JOINT0_com_constraints.obj"
,
limbOffset
=
rLegLimbOffset
,
kinematicConstraintsMin
=
0.2
)
#fullBody.addLimb(rLegId,rLeg,'',rLegOffset,rLegNormal, rLegx, rLegy, 100000, "manipulability", 0.01,"_6_DOF",kinematicConstraintsPath = "package://hpp-rbprm-corba/com_inequalities/empty_com_constraints.obj",limbOffset=rLegLimbOffset,kinematicConstraintsMin=0.2)
fullBody
.
runLimbSampleAnalysis
(
rLegId
,
"ReferenceConfiguration"
,
True
)
#fullBody.saveLimbDatabase(rLegId, "./db/hrp2_rleg_db.db")
...
...
@@ -71,11 +71,14 @@ lLegLimbOffset=[0,0,0.035]
lLegNormal
=
[
0
,
0
,
1
]
lLegx
=
0.09
;
lLegy
=
0.05
#fullBody.addLimbDatabase("./db/hrp2_lleg_db.db",lLegId,"forward")
fullBody
.
addLimb
(
lLegId
,
lLeg
,
''
,
lLegOffset
,
rLegNormal
,
lLegx
,
lLegy
,
100000
,
"manipulability"
,
0.01
,
"_6_DOF"
,
limbOffset
=
lLegLimbOffset
)
fullBody
.
addLimb
(
lLegId
,
lLeg
,
''
,
lLegOffset
,
rLegNormal
,
lLegx
,
lLegy
,
100000
,
"manipulability"
,
0.01
,
"_6_DOF"
,
kinematicConstraintsPath
=
"package://hpp-rbprm-corba/com_inequalities/fullSize/LLEG_JOINT0_com_constraints.obj"
,
limbOffset
=
lLegLimbOffset
,
kinematicConstraintsMin
=
0.2
)
fullBody
.
runLimbSampleAnalysis
(
lLegId
,
"ReferenceConfiguration"
,
True
)
#fullBody.saveLimbDatabase(lLegId, "./db/hrp2_lleg_db.db")
rleg
=
'RLEG_JOINT0'
rfoot
=
"RLEG_JOINT5"
lleg
=
'LLEG_JOINT0'
lfoot
=
"LLEG_JOINT5"
limbsCOMConstraints
=
{
rLegId
:
{
'file'
:
"hrp2/RL_com.ineq"
,
'effector'
:
'RLEG_JOINT5'
},
lLegId
:
{
'file'
:
"hrp2/LL_com.ineq"
,
'effector'
:
'LLEG_JOINT5'
},
...
...
@@ -223,21 +226,35 @@ def _feet_far_enough(fullBody,q):
for
i
in
range
(
3
):
rd
+=
(
c
[
i
]
-
rf
[
i
])
*
(
c
[
i
]
-
rf
[
i
])
ld
+=
(
c
[
i
]
-
lf
[
i
])
*
(
c
[
i
]
-
lf
[
i
])
if
rd
<
0.
2
or
ld
<
0.
2
:
if
rd
<
0.
35
or
ld
<
0.
35
:
return
False
else
:
return
True
def
projectMidFeet
(
fullBody
,
q
,
limbs
):
fullBody
.
setCurrentConfig
(
q
)
s
=
State
(
fullBody
,
q
=
q
,
limbsIncontact
=
limbs
)
com
=
np
.
array
(
fullBody
.
getJointPosition
(
rfoot
)[
0
:
3
])
com
+=
np
.
array
(
fullBody
.
getJointPosition
(
lfoot
)[
0
:
3
])
com
/=
2.
com
[
2
]
=
fullBody
.
getCenterOfMass
()[
2
]
successProj
=
s
.
projectToCOM
(
com
.
tolist
())
if
successProj
and
fullBody
.
isConfigValid
(
s
.
q
())[
0
]
and
fullBody
.
isConfigBalanced
(
s
.
q
(),
limbs
,
5
)
:
return
s
else
:
return
None
def
_genbalance
(
limbs
):
def
_genbalance
(
fullBody
,
limbs
):
for
i
in
range
(
10000
):
q
=
fullBody
.
client
.
basic
.
robot
.
shootRandomConfig
()
q
[
3
:
7
]
=
[
1
,
0
,
0
,
0
]
#q = _boundSO3(q, len(limbs))
if
fullBody
.
isConfigValid
(
q
)[
0
]
and
fullBody
.
isConfigBalanced
(
q
,
limbs
,
5
)
and
__loosely_z_aligned
(
limbs
[
0
],
q
)
and
__loosely_z_aligned
(
limbs
[
1
],
q
)
and
_feet_far_enough
(
fullBody
,
q
):
#~ if fullBody.isConfigValid(q)[0] and __loosely_z_aligned(limbs[0], q) and __loosely_z_aligned(limbs[1], q):
return
q
fullBody
.
setCurrentConfig
(
q
)
#r(q)
if
__loosely_z_aligned
(
limbs
[
0
],
q
)
and
__loosely_z_aligned
(
limbs
[
1
],
q
)
and
_feet_far_enough
(
fullBody
,
q
)
and
fullBody
.
isConfigValid
(
q
)[
0
]
and
fullBody
.
isConfigBalanced
(
q
,
limbs
,
5
):
return
q
print
"can't generate equilibrium config"
return
[]
all_qs
=
[]
def
genStates
(
fullbody
,
limbs
,
num_samples
=
1000
,
coplanar
=
True
):
...
...
@@ -249,10 +266,11 @@ def genStates(fullbody,limbs, num_samples = 1000, coplanar = True):
if
(
coplanar
):
q
=
fullBody
.
generateGroundContact
(
limbs
)
else
:
q
=
_genbalance
(
limbs
)
qs
.
append
(
q
)
s
=
State
(
fullbody
,
q
=
q
,
limbsIncontact
=
limbs
)
states
.
append
(
s
)
q
=
_genbalance
(
fullBody
,
limbs
)
if
len
(
q
)
>
1
:
qs
.
append
(
q
)
s
=
State
(
fullbody
,
q
=
q
,
limbsIncontact
=
limbs
)
states
.
append
(
s
)
return
states
def
genStateWithOneStep
(
fullbody
,
limbs
,
num_samples
=
100
,
coplanar
=
True
):
...
...
@@ -262,35 +280,50 @@ def genStateWithOneStep(fullbody,limbs, num_samples = 100, coplanar = True):
states
=
[]
assert
(
len
(
limbs
)
==
2
and
"only implemented for 2 limbs in contact for now"
)
it
=
0
while
len
(
states
)
<
num_samples
and
it
<
5
000
:
while
len
(
states
)
<
num_samples
and
it
<
10
000
:
i
=
len
(
states
)
if
(
coplanar
):
q0
=
fullBody
.
generateGroundContact
(
limbs
)
else
:
q0
=
_genbalance
(
limbs
)
s0
=
State
(
fullbody
,
q
=
q0
,
limbsIncontact
=
limbs
)
success
=
False
iter
=
0
# try to make a step
while
not
success
and
iter
<
100
:
if
(
coplanar
):
q2
=
fullBody
.
generateGroundContact
(
limbs
)
else
:
q2
=
_genbalance
(
limbs
)
s2
=
State
(
fullbody
,
q
=
q2
,
limbsIncontact
=
limbs
)
moving_limb
=
limbs
[
i
%
2
]
[
p
,
n
]
=
s2
.
getCenterOfContactForLimb
(
moving_limb
)
# get position of the moving limb in the next state
s1
,
success
=
StateHelper
.
addNewContact
(
s0
,
moving_limb
,
p
,
n
)
# try to move the limb for s0 to it's position in s2
if
success
:
success
=
fullBody
.
isConfigValid
(
s1
.
q
())[
0
]
and
fullBody
.
isConfigBalanced
(
s1
.
q
(),
limbs
,
5
)
and
__loosely_z_aligned
(
limbs
[
0
],
s1
.
q
())
and
__loosely_z_aligned
(
limbs
[
1
],
s1
.
q
())
iter
+=
1
if
success
:
# recreate the states to assure the continuity of the index in fullBody :
state0
=
State
(
fullBody
,
q
=
s0
.
q
(),
limbsIncontact
=
limbs
)
states
+=
[[
s1
,
state0
]]
print
"Step "
+
str
(
i
)
+
" done."
else
:
print
"cannot make the step."
q0
=
_genbalance
(
fullBody
,
limbs
)
if
len
(
q0
)
>