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
hpp-rbprm-corba
Commits
ac5acb3d
Unverified
Commit
ac5acb3d
authored
Sep 29, 2021
by
Guilhem Saurel
Committed by
GitHub
Sep 29, 2021
Browse files
Merge pull request #79 from pFernbach/solo
Solo
parents
fda5fce8
156d8225
Pipeline
#16203
canceled with stage
in 0 seconds
Changes
12
Pipelines
1
Hide whitespace changes
Inline
Side-by-side
src/CMakeLists.txt
View file @
ac5acb3d
...
...
@@ -119,6 +119,8 @@ SET(${PROJECT_NAME}_PYTHON_SCENARIOS
__init__.py
talos_contact_generator.py
talos_path_planner.py
solo_contact_generator.py
solo_path_planner.py
)
SET
(
${
PROJECT_NAME
}
_PYTHON_SCENARIOS_DEMOS
...
...
@@ -143,6 +145,12 @@ SET(${PROJECT_NAME}_PYTHON_SCENARIOS_DEMOS
talos_plateformes.py
talos_stairs10_path.py
talos_stairs10.py
talos_stairs15_path.py
talos_stairs15.py
solo_flatGround_path.py
solo_flatGround.py
solo_slalom_path.py
solo_pallet_path.py
)
SET
(
${
PROJECT_NAME
}
_PYTHON_SCENARIOS_MEMMO
...
...
src/hpp/corbaserver/rbprm/scenarios/abstract_contact_generator.py
View file @
ac5acb3d
...
...
@@ -83,7 +83,16 @@ class AbstractContactGenerator:
self
.
fullbody
.
setPostureWeights
(
self
.
weight_postural
)
self
.
fullbody
.
usePosturalTaskContactCreation
(
use_postural_task
)
def
load_limbs
(
self
,
heuristic
=
"fixedStep06"
,
analysis
=
"ReferenceConfiguration"
,
nb_samples
=
None
,
octree_size
=
None
):
def
set_rom_filters
(
self
):
"""
Define which type of affordance should be considered to create contact with each limb
By default, use "Support" type for all limbs
"""
for
limb
in
self
.
used_limbs
:
self
.
path_planner
.
rbprmBuilder
.
setAffordanceFilter
(
limb
,
[
'Support'
])
def
load_limbs
(
self
,
heuristic
=
"fixedStep06"
,
analysis
=
"ReferenceConfiguration"
,
nb_samples
=
None
,
octree_size
=
None
,
disableEffectorCollision
=
False
):
"""
Generate the samples used for each limbs in 'used_limbs'
:param heuristic: the name of the heuristic used,
...
...
@@ -93,6 +102,7 @@ class AbstractContactGenerator:
:param nb_samples: The number of samples for each limb database. Default is set in the Robot python class
:param octree_size: The size of each cell of the octree. Default is set in the Robot python class
"""
self
.
set_rom_filters
()
self
.
fullbody
.
limbs_names
=
self
.
used_limbs
if
nb_samples
is
None
:
nb_samples
=
self
.
fullbody
.
nbSamples
...
...
@@ -100,7 +110,8 @@ class AbstractContactGenerator:
octree_size
=
self
.
fullbody
.
octreeSize
print
(
"Generate limb DB ..."
)
t_start
=
time
.
time
()
self
.
fullbody
.
loadAllLimbs
(
heuristic
,
analysis
,
nb_samples
,
octree_size
)
self
.
fullbody
.
loadAllLimbs
(
heuristic
,
analysis
,
nb_samples
,
octree_size
,
disableEffectorCollision
=
disableEffectorCollision
)
t_generate
=
time
.
time
()
-
t_start
print
(
"Databases generated in : "
+
str
(
t_generate
)
+
" s"
)
...
...
src/hpp/corbaserver/rbprm/scenarios/abstract_path_planner.py
View file @
ac5acb3d
...
...
@@ -113,7 +113,8 @@ class AbstractPathPlanner:
# sample only configuration with null velocity and acceleration :
self
.
ps
.
setParameter
(
"ConfigurationShooter/sampleExtraDOF"
,
False
)
def
init_viewer
(
self
,
env_name
,
env_package
=
"hpp_environments"
,
reduce_sizes
=
[
0
,
0
,
0
],
visualize_affordances
=
[]):
def
init_viewer
(
self
,
env_name
,
env_package
=
"hpp_environments"
,
reduce_sizes
=
[
0
,
0
,
0
],
visualize_affordances
=
[],
min_area
=
None
):
"""
Build an instance of hpp-gepetto-viewer from the current problemSolver
:param env_name: name of the urdf describing the environment
...
...
@@ -121,6 +122,7 @@ class AbstractPathPlanner:
:param reduce_sizes: Distance used to reduce the affordances plan toward the center of the plane
(in order to avoid putting contacts closes to the edges of the surface)
:param visualize_affordances: list of affordances type to visualize, default to none
:param min_area: list of couple [affordanceType, size]. If provided set the minimal area for each affordance
"""
vf
=
ViewerFactory
(
self
.
ps
)
if
self
.
context
:
...
...
@@ -130,11 +132,13 @@ class AbstractPathPlanner:
else
:
self
.
afftool
=
AffordanceTool
()
self
.
afftool
.
setAffordanceConfig
(
'Support'
,
[
0.5
,
0.03
,
0.00005
])
if
min_area
is
not
None
:
for
(
aff_type
,
min_size
)
in
min_area
:
self
.
afftool
.
setMinimumArea
(
aff_type
,
min_size
)
self
.
afftool
.
loadObstacleModel
(
"package://"
+
env_package
+
"/urdf/"
+
env_name
+
".urdf"
,
"planning"
,
vf
,
reduceSizes
=
reduce_sizes
)
self
.
v
=
vf
.
createViewer
(
ghost
=
True
,
displayArrows
=
True
)
self
.
pp
=
PathPlayer
(
self
.
v
)
for
aff_type
in
visualize_affordances
:
...
...
src/hpp/corbaserver/rbprm/scenarios/demos/solo_flatGround.py
0 → 100644
View file @
ac5acb3d
from
hpp.corbaserver.rbprm.scenarios.demos.solo_flatGround_path
import
PathPlanner
from
hpp.corbaserver.rbprm.scenarios.solo_contact_generator
import
SoloContactGenerator
class
ContactGenerator
(
SoloContactGenerator
):
def
__init__
(
self
):
super
().
__init__
(
PathPlanner
())
self
.
testReachability
=
False
if
__name__
==
"__main__"
:
cg
=
ContactGenerator
()
cg
.
run
()
src/hpp/corbaserver/rbprm/scenarios/demos/solo_flatGround_path.py
0 → 100644
View file @
ac5acb3d
from
hpp.corbaserver.rbprm.scenarios.solo_path_planner
import
SoloPathPlanner
class
PathPlanner
(
SoloPathPlanner
):
def
init_problem
(
self
):
self
.
a_max
=
0.1
super
().
init_problem
()
# greatly increase the number of loops of the random shortcut
self
.
ps
.
setParameter
(
"PathOptimization/RandomShortcut/NumberOfLoops"
,
50
)
# force the base orientation to follow the direction of motion along the Z axis
self
.
ps
.
setParameter
(
"Kinodynamic/forceYawOrientation"
,
True
)
def
run
(
self
):
self
.
init_problem
()
self
.
q_init
[:
2
]
=
[
0
,
0
]
self
.
q_goal
[:
2
]
=
[
0.5
,
0
]
# Constraint the initial orientation when forceYawOrientation = True, expressed as a 3D vector (x,y,z)
#self.q_init[-6:-3] = [0.1, 0, 0]
#self.q_goal[-6:-3] = [0, -0.1, 0]
self
.
init_viewer
(
"multicontact/ground"
,
visualize_affordances
=
[
"Support"
])
self
.
init_planner
(
True
,
False
)
self
.
solve
()
self
.
display_path
()
# self.play_path()
self
.
hide_rom
()
if
__name__
==
"__main__"
:
planner
=
PathPlanner
()
planner
.
run
()
src/hpp/corbaserver/rbprm/scenarios/demos/solo_pallet_path.py
0 → 100644
View file @
ac5acb3d
from
hpp.corbaserver.rbprm.scenarios.solo_path_planner
import
SoloPathPlanner
class
PathPlanner
(
SoloPathPlanner
):
def
init_problem
(
self
):
self
.
a_max
=
0.1
super
().
init_problem
()
# greatly increase the number of loops of the random shortcut
self
.
ps
.
setParameter
(
"PathOptimization/RandomShortcut/NumberOfLoops"
,
100
)
# force the base orientation to follow the direction of motion along the Z axis
self
.
ps
.
setParameter
(
"Kinodynamic/forceYawOrientation"
,
True
)
def
run
(
self
):
self
.
init_problem
()
self
.
q_init
[:
2
]
=
[
-
0.85
,
0.
]
# self.q_goal[:2] = [1, 0.]
self
.
q_goal
[:
2
]
=
[
0.
,
0.
]
self
.
init_viewer
(
"ori/modular_palet_low"
,
visualize_affordances
=
[
"Support"
],
reduce_sizes
=
[
0.06
,
0
,
0
],
min_area
=
[[
'Support'
,
0.05
]])
self
.
init_planner
(
True
,
False
)
self
.
ps
.
directPath
(
self
.
q_init
,
self
.
q_goal
,
False
)
self
.
display_path
()
# self.play_path()
self
.
hide_rom
()
if
__name__
==
"__main__"
:
planner
=
PathPlanner
()
planner
.
run
()
src/hpp/corbaserver/rbprm/scenarios/demos/solo_slalom_path.py
0 → 100644
View file @
ac5acb3d
from
hpp.corbaserver.rbprm.scenarios.solo_path_planner
import
SoloPathPlanner
class
PathPlanner
(
SoloPathPlanner
):
def
init_problem
(
self
):
self
.
a_max
=
0.1
super
().
init_problem
()
# greatly increase the number of loops of the random shortcut
self
.
ps
.
setParameter
(
"PathOptimization/RandomShortcut/NumberOfLoops"
,
100
)
# force the base orientation to follow the direction of motion along the Z axis
self
.
ps
.
setParameter
(
"Kinodynamic/forceYawOrientation"
,
True
)
def
run
(
self
):
self
.
init_problem
()
self
.
q_init
[:
2
]
=
[
-
0.7
,
0.95
]
#self.q_goal[:2] = [1.05, 0.95]
self
.
q_goal
[:
2
]
=
[
-
0.2
,
0.95
]
# Constraint the initial orientation when forceYawOrientation = True, expressed as a 3D vector (x,y,z)
#self.q_init[-6:-3] = [0.1, 0, 0]
#self.q_goal[-6:-3] = [0, -0.1, 0]
self
.
init_viewer
(
"multicontact/slalom_debris"
,
visualize_affordances
=
[
"Support"
],
reduce_sizes
=
[
0.05
,
0
,
0
])
self
.
init_planner
(
True
,
False
)
self
.
solve
()
"""
ps = self.ps
q_init = self.rbprmBuilder.getCurrentConfig();
q_init[0:3] = [-1.8, 0., self.rbprmBuilder.ref_height];
self.v(q_init)
q_init[-6:-3] = [0.1, 0, 0]
q_goal = q_init[::]
q_goal[0:3] = [-0.9, 0.95, self.rbprmBuilder.ref_height];
self.v(q_goal)
q_goal[-6:-3] = [0.1, 0, 0]
ps.setInitialConfig(q_init)
ps.addGoalConfig(q_goal)
self.v(q_goal)
q_init_0 = q_init[::]
t = ps.solve()
print("done planning, optimize path ...")
# v.solveAndDisplay('rm',2,0.005)
# for i in range(5):
# ps.optimizePath(ps.numberPaths() -1)
pId_begin = ps.numberPaths() - 1
### END BEGIN up to the rubbles #####
ps.resetGoalConfigs()
### BEGIN rubbles #####
ps.setParameter("Kinodynamic/velocityBound", 0.4)
ps.setParameter("Kinodynamic/accelerationBound", 0.1)
q_init = self.rbprmBuilder.getCurrentConfig();
q_init = q_goal[::];
self.v(q_init)
# q_init[-6:-3] = [0.,0,0]
q_goal[0:3] = [1.05, 0.95, self.rbprmBuilder.ref_height];
self.v(q_goal)
q_goal[-6:-3] = [0.1, 0, 0]
ps.setInitialConfig(q_init)
ps.addGoalConfig(q_goal)
self.v(q_goal)
t = ps.solve()
print("done planning, optimize path ...")
# v.solveAndDisplay('rm',2,0.005)
for i in range(10):
ps.optimizePath(ps.numberPaths() -1)
pId_rubbles = ps.numberPaths() - 1
### END rubbles #####
ps.resetGoalConfigs()
### BEGIN after rubbles #####
ps.setParameter("Kinodynamic/velocityBound", 0.15)
ps.setParameter("Kinodynamic/accelerationBound", 0.1)
q_init = self.rbprmBuilder.getCurrentConfig();
q_init = q_goal[::];
self.v(q_init)
q_goal[0:3] = [2.2, 0, self.rbprmBuilder.ref_height];
self.v(q_goal)
q_goal[-6:-3] = [0.05, 0, 0]
ps.setInitialConfig(q_init)
ps.addGoalConfig(q_goal)
self.v(q_goal)
t = ps.solve()
print("done planning, optimize path ...")
# v.solveAndDisplay('rm',2,0.005)
# for i in range(5):
# ps.optimizePath(ps.numberPaths() -1)
pId_end = ps.numberPaths() - 1
### END after rubbles #####
self.pathId = pId_begin
ps.concatenatePath(self.pathId, pId_rubbles)
ps.concatenatePath(self.pathId, pId_end)
"""
self
.
display_path
()
# self.play_path()
self
.
hide_rom
()
if
__name__
==
"__main__"
:
planner
=
PathPlanner
()
planner
.
run
()
src/hpp/corbaserver/rbprm/scenarios/demos/talos_stairs15.py
0 → 100644
View file @
ac5acb3d
from
hpp.corbaserver.rbprm.scenarios.demos.talos_stairs15_path
import
PathPlanner
from
hpp.corbaserver.rbprm.scenarios.talos_contact_generator
import
TalosContactGenerator
class
ContactGenerator
(
TalosContactGenerator
):
def
__init__
(
self
):
super
().
__init__
(
PathPlanner
())
self
.
test_reachability
=
False
def
load_fullbody
(
self
):
super
().
load_fullbody
()
self
.
q_ref
=
self
.
fullbody
.
referenceConfig_elbowsUp
[::]
+
[
0
]
*
self
.
path_planner
.
extra_dof
# path planning use different limbs, reset it to right/left feet
self
.
used_limbs
=
[
self
.
fullbody
.
lLegId
,
self
.
fullbody
.
rLegId
,
self
.
fullbody
.
rArmId
]
self
.
init_contacts
=
[
self
.
fullbody
.
lLegId
,
self
.
fullbody
.
rLegId
]
self
.
end_contacts
=
self
.
used_limbs
def
load_limbs
(
self
):
super
().
load_limbs
(
"static"
,
nb_samples
=
100000
)
def
compute_configs_from_guide
(
self
):
super
().
compute_configs_from_guide
()
self
.
q_goal
[
2
]
=
self
.
q_ref
[
2
]
+
0.75
# set height to the top of the stairs
if
__name__
==
"__main__"
:
cg
=
ContactGenerator
()
cg
.
run
()
src/hpp/corbaserver/rbprm/scenarios/demos/talos_stairs15_path.py
0 → 100644
View file @
ac5acb3d
from
hpp.corbaserver.rbprm.scenarios.talos_path_planner
import
TalosPathPlanner
class
PathPlanner
(
TalosPathPlanner
):
def
load_rbprm
(
self
):
from
talos_rbprm.talos_abstract
import
Robot
Robot
.
urdfNameRom
=
[
Robot
.
lLegId
,
Robot
.
rLegId
,
Robot
.
rArmId
]
self
.
used_limbs
=
Robot
.
urdfNameRom
self
.
rbprmBuilder
=
Robot
()
def
init_problem
(
self
):
super
().
init_problem
()
# greatly increase the number of loops of the random shortcut
self
.
ps
.
setParameter
(
"PathOptimization/RandomShortcut/NumberOfLoops"
,
100
)
self
.
ps
.
setParameter
(
"Kinodynamic/synchronizeVerticalAxis"
,
True
)
self
.
ps
.
setParameter
(
"Kinodynamic/verticalAccelerationBound"
,
3.
)
def
compute_extra_config_bounds
(
self
):
# bounds for the extradof : by default use v_max/a_max on x and y axis and a large value on z axis
self
.
extra_dof_bounds
=
[
-
self
.
v_max
,
self
.
v_max
,
-
self
.
v_max
,
self
.
v_max
,
-
10
,
10
,
-
self
.
a_max
,
self
.
a_max
,
-
self
.
a_max
,
self
.
a_max
,
-
10
,
10
]
def
run
(
self
):
self
.
root_translation_bounds
=
[
-
2.
,
-
1.7
,
0
,
2.
,
0.95
,
2.
]
self
.
init_problem
()
self
.
q_init
[:
2
]
=
[
-
1.9
,
1.9
]
self
.
q_init
[
3
:
7
]
=
[
0
,
0
,
-
0.7071
,
0.7071
]
self
.
q_init
[
2
]
=
0.98
self
.
q_goal
=
self
.
q_init
[::]
self
.
q_goal
[
1
]
=
0.9
self
.
q_goal
[
2
]
+=
3
*
0.15
# 4 steps of 15cm each
self
.
init_viewer
(
"multicontact/bauzil_stairs"
,
reduce_sizes
=
[
0.08
,
0.
,
0.
],
visualize_affordances
=
[
"Support"
])
self
.
init_planner
()
self
.
solve
()
self
.
display_path
()
# self.play_path()
self
.
hide_rom
()
if
__name__
==
"__main__"
:
planner
=
PathPlanner
()
planner
.
run
()
src/hpp/corbaserver/rbprm/scenarios/solo_contact_generator.py
0 → 100644
View file @
ac5acb3d
from
.abstract_contact_generator
import
AbstractContactGenerator
class
SoloContactGenerator
(
AbstractContactGenerator
):
def
__init__
(
self
,
path_planner
):
super
().
__init__
(
path_planner
)
self
.
robustness
=
5
self
.
used_limbs
=
[
'FRleg'
,
'HLleg'
,
'FLleg'
,
'HRleg'
]
self
.
init_contacts
=
self
.
used_limbs
[::]
self
.
end_contacts
=
self
.
used_limbs
[::]
self
.
robot_node_name
=
"solo"
def
set_start_end_states
(
self
):
# for 3D contacts, we must specify the normals manually
normals_init
=
[]
normals_end
=
[]
for
limb_id
in
self
.
init_contacts
:
normals_init
+=
[
self
.
fullbody
.
dict_normal
[
self
.
fullbody
.
dict_limb_joint
[
limb_id
]]]
for
limb_id
in
self
.
init_contacts
:
normals_end
+=
[
self
.
fullbody
.
dict_normal
[
self
.
fullbody
.
dict_limb_joint
[
limb_id
]]]
self
.
fullbody
.
setStartState
(
self
.
q_init
,
self
.
init_contacts
,
normals_init
)
self
.
fullbody
.
setEndState
(
self
.
q_goal
,
self
.
end_contacts
,
normals_end
)
def
load_fullbody
(
self
):
from
solo_rbprm.solo
import
Robot
self
.
fullbody
=
Robot
()
self
.
q_ref
=
self
.
fullbody
.
referenceConfig
[::]
+
[
0
]
*
self
.
path_planner
.
extra_dof
self
.
weight_postural
=
self
.
fullbody
.
postureWeights
[::]
+
[
0
]
*
self
.
path_planner
.
extra_dof
self
.
fullbody
.
setConstrainedJointsBounds
()
def
init_viewer
(
self
):
super
().
init_viewer
()
self
.
v
.
addLandmark
(
'solo/base_link_0'
,
0.3
)
def
load_limbs
(
self
,
heuristic
=
"fixedStep04"
,
analysis
=
None
,
nb_samples
=
None
,
octree_size
=
None
):
super
().
load_limbs
(
heuristic
,
analysis
,
nb_samples
,
octree_size
,
disableEffectorCollision
=
True
)
src/hpp/corbaserver/rbprm/scenarios/solo_path_planner.py
0 → 100644
View file @
ac5acb3d
from
.abstract_path_planner
import
AbstractPathPlanner
class
SoloPathPlanner
(
AbstractPathPlanner
):
def
__init__
(
self
,
context
=
None
):
super
().
__init__
(
context
)
# set default bounds to a large workspace on x,y with small interval around reference z value
self
.
root_translation_bounds
=
[
-
5.
,
5.
,
-
5.
,
5.
,
0.241
,
0.241
]
# set default used limbs to be both feet
self
.
used_limbs
=
[
'solo_RFleg_rom'
,
'solo_LHleg_rom'
,
'solo_LFleg_rom'
,
'solo_RHleg_rom'
]
self
.
size_foot_x
=
0.01
# size of the feet along the x axis
self
.
size_foot_y
=
0.01
# size of the feet along the y axis
self
.
v_max
=
0.3
self
.
a_max
=
1.
self
.
extra_dof_bounds
=
[
-
self
.
v_max
,
self
.
v_max
,
-
self
.
v_max
,
self
.
v_max
,
0
,
0
,
-
self
.
a_max
,
self
.
a_max
,
-
self
.
a_max
,
self
.
a_max
,
0
,
0
]
self
.
robot_node_name
=
"solo_trunk"
def
load_rbprm
(
self
):
from
solo_rbprm.solo_abstract
import
Robot
self
.
rbprmBuilder
=
Robot
(
client
=
self
.
hpp_client
,
clientRbprm
=
self
.
rbprm_client
)
def
set_joints_bounds
(
self
):
super
().
set_joints_bounds
()
def
set_configurations
(
self
):
super
().
set_configurations
()
src/hpp/corbaserver/rbprm/tools/surfaces_from_path.py
View file @
ac5acb3d
...
...
@@ -62,18 +62,33 @@ def getRotationMatrixFromConfigs(configs):
#print "R in getRotationMatrixFromConfigs : ",R
return
R
def
getALlContactsNames
(
rbprmBuilder
,
q
):
step_contacts
=
[]
for
limb
in
rbprmBuilder
.
urdfNameRom
:
step_contacts
+=
rbprmBuilder
.
clientRbprm
.
rbprm
.
getCollidingObstacleAtConfig
(
q
,
limb
)
return
step_contacts
# get contacted surface names at configuration
def
getContactsNames
(
rbprmBuilder
,
i
,
q
):
def
getContactsNames
(
rbprmBuilder
,
i
,
q
,
use_all_limbs
=
False
):
if
use_all_limbs
:
return
getALlContactsNames
(
rbprmBuilder
,
q
)
if
i
%
2
==
LF
:
# left leg
step_contacts
=
rbprmBuilder
.
clientRbprm
.
rbprm
.
getCollidingObstacleAtConfig
(
q
,
rbprmBuilder
.
lLegId
)
elif
i
%
2
==
RF
:
# right leg
step_contacts
=
rbprmBuilder
.
clientRbprm
.
rbprm
.
getCollidingObstacleAtConfig
(
q
,
rbprmBuilder
.
rLegId
)
return
step_contacts
def
getALlContactsIntersections
(
rbprmBuilder
,
q
):
intersections
=
[]
for
limb
in
rbprmBuilder
.
urdfNameRom
:
intersections
+=
rbprmBuilder
.
getContactSurfacesAtConfig
(
q
,
limb
)
return
intersections
# get intersections with the rom and surface at configuration
def
getContactsIntersections
(
rbprmBuilder
,
i
,
q
):
def
getContactsIntersections
(
rbprmBuilder
,
i
,
q
,
use_all_limbs
=
False
):
if
use_all_limbs
:
return
getALlContactsIntersections
(
rbprmBuilder
,
q
)
if
i
%
2
==
LF
:
# left leg
intersections
=
rbprmBuilder
.
getContactSurfacesAtConfig
(
q
,
rbprmBuilder
.
lLegId
)
elif
i
%
2
==
RF
:
# right leg
...
...
@@ -118,7 +133,8 @@ def getSurfacesFromGuideContinuous(rbprmBuilder,
useIntersection
=
False
,
mergeCandidates
=
False
,
max_yaw
=
0.5
,
max_surface_area
=
MAX_SURFACE
):
max_surface_area
=
MAX_SURFACE
,
use_all_limbs
=
False
):
pathLength
=
ps
.
pathLength
(
pId
)
#length of the path
discretizationStep
=
0.01
# step at which we check the colliding surfaces
#print "path length = ",pathLength
...
...
@@ -152,24 +168,42 @@ def getSurfacesFromGuideContinuous(rbprmBuilder,
t
+=
0.0001
q
=
ps
.
configAtParam
(
pId
,
t
)
#print " t in getSurfacesFromGuideContinuous : ",t
step_contacts
=
getContactsNames
(
rbprmBuilder
,
i
,
q
)
step_contacts
=
getContactsNames
(
rbprmBuilder
,
i
,
q
,
use_all_limbs
)
for
contact_name
in
step_contacts
:
if
not
contact_name
in
phase_contacts_names
:
phase_contacts_names
.
append
(
contact_name
)
# end current phase
# get all the surfaces from the names and add it to seqs:
if
useIntersection
:
intersections
=
getContactsIntersections
(
rbprmBuilder
,
i
,
q
)
phase_surfaces
=
[]
if
useIntersection
:
intersections
=
getContactsIntersections
(
rbprmBuilder
,
i
,
q
,
use_all_limbs
)
# First add intersection for the surfaces in contact at the last discretization step
for
k
,
name
in
enumerate
(
step_contacts
):
surface
=
surfaces_dict
[
name
][
0
]
# [0] because the last vector contain the normal of the surface
if
area
(
surface
)
>
max_surface_area
:
intersection
=
intersections
[
k
]
if
len
(
intersection
)
>
3
:
phase_surfaces
.
append
(
intersection
)
else
:
phase_surfaces
.
append
(
surfaces_dict
[
name
][
0
])
# then add possible small surfaces encountered during the discretization of the path
for
name
in
phase_contacts_names
:
if
name
not
in
step_contacts
:
phase_surfaces
.
append
(
surfaces_dict
[
name
][
0
])
else
:
# add all the surfaces encountered during the path
for
name
in
phase_contacts_names
:
phase_surfaces
.
append
(
surfaces_dict
[
name
][
0
])
for
name
in
phase_contacts_names
:
surface
=
surfaces_dict
[
name
][
0
]
surface
=
surfaces_dict
[
name
][
0
]
# [0] because the last vector contain the normal of the surface
if
useIntersection
and
area
(
surface
)
>
max_surface_area
:
if
name
in
step_contacts
:
intersection
=
intersections
[
step_contacts
.
index
(
name
)]
if
len
(
intersection
)
>
3
:
phase_surfaces
.
append
(
intersection
)
else
:
phase_surfaces
.
append
(
surface
)
# [0] because the last vector contain the normal of the surface
phase_surfaces
.
append
(
surface
)
#print "There was "+str(len(phase_contacts_names))+" surfaces in contact during this phase."
phase_surfaces
=
sorted
(
phase_surfaces
)
# why is this step required ? without out the lp can fail
phase_surfaces_array
=
[]
# convert from list to array, we cannot do this before because sorted() require list
...
...
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