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
5b2f85b5
Commit
5b2f85b5
authored
Jan 18, 2019
by
Pierre Fernbach
Browse files
[script] delete old scripts
parent
8c6eb02e
Changes
9
Hide whitespace changes
Inline
Side-by-side
script/tests/darpa_hyq_interp.py
deleted
100755 → 0
View file @
8c6eb02e
#Importing helper class for RBPRM
from
hpp.corbaserver.rbprm.rbprmbuilder
import
Builder
from
hpp.corbaserver.rbprm.rbprmfullbody
import
FullBody
from
hpp.corbaserver.rbprm.problem_solver
import
ProblemSolver
from
hpp.gepetto
import
Viewer
#calling script darpa_hyq_path to compute root path
import
darpa_hyq_path
as
tp
packageName
=
"hyq_description"
meshPackageName
=
"hyq_description"
rootJointType
=
"freeflyer"
# Information to retrieve urdf and srdf files.
urdfName
=
"hyq"
urdfSuffix
=
""
srdfSuffix
=
""
# This time we load the full body model of HyQ
fullBody
=
FullBody
()
fullBody
.
loadFullBodyModel
(
urdfName
,
rootJointType
,
meshPackageName
,
packageName
,
urdfSuffix
,
srdfSuffix
)
# Setting a number of sample configurations used
nbSamples
=
20000
ps
=
tp
.
ProblemSolver
(
fullBody
)
r
=
tp
.
Viewer
(
ps
)
rootName
=
'base_joint_xyz'
# Creating limbs
# cType is "_3_DOF": positional constraint, but no rotation (contacts are punctual)
cType
=
"_3_DOF"
# string identifying the limb
rLegId
=
'rfleg'
# First joint of the limb, as in urdf file
rLeg
=
'rf_haa_joint'
# Last joint of the limb, as in urdf file
rfoot
=
'rf_foot_joint'
# Specifying the distance between last joint and contact surface
offset
=
[
0.
,
-
0.021
,
0.
]
# Specifying the contact surface direction when the limb is in rest pose
normal
=
[
0
,
1
,
0
]
# Specifying the rectangular contact surface length
legx
=
0.02
;
legy
=
0.02
# remaining parameters are the chosen heuristic (here, manipulability), and the resolution of the octree (here, 10 cm).
fullBody
.
addLimb
(
rLegId
,
rLeg
,
rfoot
,
offset
,
normal
,
legx
,
legy
,
nbSamples
,
"manipulability"
,
0.1
,
cType
)
lLegId
=
'lhleg'
lLeg
=
'lh_haa_joint'
lfoot
=
'lh_foot_joint'
fullBody
.
addLimb
(
lLegId
,
lLeg
,
lfoot
,
offset
,
normal
,
legx
,
legy
,
nbSamples
,
"manipulability"
,
0.05
,
cType
)
rarmId
=
'rhleg'
rarm
=
'rh_haa_joint'
rHand
=
'rh_foot_joint'
fullBody
.
addLimb
(
rarmId
,
rarm
,
rHand
,
offset
,
normal
,
legx
,
legy
,
nbSamples
,
"manipulability"
,
0.05
,
cType
)
larmId
=
'lfleg'
larm
=
'lf_haa_joint'
lHand
=
'lf_foot_joint'
fullBody
.
addLimb
(
larmId
,
larm
,
lHand
,
offset
,
normal
,
legx
,
legy
,
nbSamples
,
"forward"
,
0.05
,
cType
)
q_0
=
fullBody
.
getCurrentConfig
();
q_init
=
fullBody
.
getCurrentConfig
();
q_init
[
0
:
7
]
=
tp
.
q_init
[
0
:
7
]
q_goal
=
fullBody
.
getCurrentConfig
();
q_goal
[
0
:
7
]
=
tp
.
q_goal
[
0
:
7
]
# Randomly generating a contact configuration at q_init
fullBody
.
setCurrentConfig
(
q_init
)
q_init
=
fullBody
.
generateContacts
(
q_init
,
[
0
,
0
,
1
])
# Randomly generating a contact configuration at q_end
fullBody
.
setCurrentConfig
(
q_goal
)
q_goal
=
fullBody
.
generateContacts
(
q_goal
,
[
0
,
0
,
1
])
# specifying the full body configurations as start and goal state of the problem
fullBody
.
setStartState
(
q_init
,[])
fullBody
.
setEndState
(
q_goal
,[
rLegId
,
lLegId
,
rarmId
,
larmId
])
r
(
q_init
)
# computing the contact sequence
configs
=
fullBody
.
interpolate
(
0.1
,
0
,
0
)
r
.
client
.
gui
.
removeFromGroup
(
"planning"
,
r
.
sceneName
)
r
.
client
.
gui
.
removeFromGroup
(
"hyq_trunk_large"
,
r
.
sceneName
)
r
.
loadObstacleModel
(
'hpp-rbprm-corba'
,
"darpa"
,
"contact"
)
import
time
i
=
0
;
while
(
i
<
(
len
(
configs
)
-
1
)):
fullBody
.
draw
(
configs
[
i
],
r
);
i
=
i
+
1
;
i
-
1
time
.
sleep
(
0.3
)
#~ fullBody.exportAll(r, configs, 'darpa_hyq_robust_1');
script/tests/darpa_hyq_path.py
deleted
100644 → 0
View file @
8c6eb02e
from
hpp.corbaserver.rbprm.rbprmbuilder
import
Builder
from
hpp.gepetto
import
Viewer
white
=
[
1.0
,
1.0
,
1.0
,
1.0
]
green
=
[
0.23
,
0.75
,
0.2
,
0.5
]
yellow
=
[
0.85
,
0.75
,
0.15
,
1
]
pink
=
[
1
,
0.6
,
1
,
1
]
orange
=
[
1
,
0.42
,
0
,
1
]
brown
=
[
0.85
,
0.75
,
0.15
,
0.5
]
blue
=
[
0.0
,
0.0
,
0.8
,
1.0
]
grey
=
[
0.7
,
0.7
,
0.7
,
1.0
]
red
=
[
0.8
,
0.0
,
0.0
,
1.0
]
rootJointType
=
'freeflyer'
packageName
=
'hpp-rbprm-corba'
meshPackageName
=
'hpp-rbprm-corba'
urdfName
=
'hyq_trunk_large_realist'
urdfNameRom
=
[
'hyq_lhleg_rom'
,
'hyq_lfleg_rom'
,
'hyq_rfleg_rom'
,
'hyq_rhleg_rom'
]
urdfSuffix
=
""
srdfSuffix
=
""
rbprmBuilder
=
Builder
()
rbprmBuilder
.
loadModel
(
urdfName
,
urdfNameRom
,
rootJointType
,
meshPackageName
,
packageName
,
urdfSuffix
,
srdfSuffix
)
rbprmBuilder
.
setJointBounds
(
"base_joint_xyz"
,
[
-
2
,
5
,
-
1
,
1
,
0.3
,
4
])
rbprmBuilder
.
setFilter
([
'hyq_rhleg_rom'
,
'hyq_lfleg_rom'
,
'hyq_rfleg_rom'
,
'hyq_lhleg_rom'
])
rbprmBuilder
.
setNormalFilter
(
'hyq_lhleg_rom'
,
[
0
,
0
,
1
],
0.5
)
rbprmBuilder
.
setNormalFilter
(
'hyq_rfleg_rom'
,
[
0
,
0
,
1
],
0.5
)
rbprmBuilder
.
setNormalFilter
(
'hyq_lfleg_rom'
,
[
0
,
0
,
1
],
0.5
)
rbprmBuilder
.
setNormalFilter
(
'hyq_rhleg_rom'
,
[
0
,
0
,
1
],
0.5
)
rbprmBuilder
.
boundSO3
([
-
0.4
,
0.4
,
-
0.5
,
0.5
,
-
0.5
,
0.5
])
rbprmBuilder
.
client
.
basic
.
robot
.
setDimensionExtraConfigSpace
(
3
)
rbprmBuilder
.
client
.
basic
.
robot
.
setExtraConfigSpaceBounds
([
0
,
0
,
0
,
0
,
0
,
0
])
#~ from hpp.corbaserver.rbprm. import ProblemSolver
from
hpp.corbaserver.rbprm.problem_solver
import
ProblemSolver
ps
=
ProblemSolver
(
rbprmBuilder
)
r
=
Viewer
(
ps
)
q_init
=
[
-
2
,
0
,
0.63
,
1
,
0
,
0
,
0
,
0
,
0
,
1
];
rbprmBuilder
.
setCurrentConfig
(
q_init
);
r
(
q_init
)
#~ q_init [0:3] = [2, 0, 0.63]; rbprmBuilder.setCurrentConfig (q_init); r (q_init)
q_goal
=
q_init
[::]
q_goal
[
0
:
3
]
=
[
3
,
0
,
0.63
];
r
(
q_goal
)
#~ ps.addPathOptimizer("GradientBased")
# ps.addPathOptimizer("RandomShortcut")
ps
.
selectPathPlanner
(
"RRTdynamic"
)
ps
.
setInitialConfig
(
q_init
)
ps
.
addGoalConfig
(
q_goal
)
ps
.
client
.
problem
.
selectConFigurationShooter
(
"RbprmShooter"
)
ps
.
client
.
problem
.
selectPathValidation
(
"RbprmPathValidation"
,
0.05
)
r
.
loadObstacleModel
(
packageName
,
"darpa"
,
"planning"
)
r
(
q_init
)
ps
.
solve
()
#r.solveAndDisplay("rm",1,0.02)
#r.displayRoadmap("rm",0.005)
r
.
displayPathMap
(
"rmPath"
,
0
,
0.02
)
from
hpp.gepetto
import
PathPlayer
pp
=
PathPlayer
(
rbprmBuilder
.
client
.
basic
,
r
)
#~ rbprmBuilder.exportPath (r, ps.client.problem, 1, 0.1, "darpa_hyq_robust_20_path.txt")
#~ pp.fromFile("/home/stonneau/dev/hpp/src/hpp-rbprm-corba/script/paths/stair.path")
#~
#~ pp (2)
#~ pp (0)
pp
.
displayPath
(
0
)
r
.
client
.
gui
.
setVisibility
(
"path_0_root"
,
"ALWAYS_ON_TOP"
)
pp
(
0
)
#r.client.gui.removeFromGroup("rm",r.sceneName)
r
.
client
.
gui
.
removeFromGroup
(
"rmPath"
,
r
.
sceneName
)
#r.client.gui.removeFromGroup("path_1_root",r.sceneName)
#~ pp.toFile(1, "/home/stonneau/dev/hpp/src/hpp-rbprm-corba/script/paths/stair.path")
script/tests/robot_bigStep_STEVE_path.py
deleted
100644 → 0
View file @
8c6eb02e
from
hpp.corbaserver.rbprm.rbprmbuilder
import
Builder
from
hpp.gepetto
import
Viewer
white
=
[
1.0
,
1.0
,
1.0
,
1.0
]
green
=
[
0.23
,
0.75
,
0.2
,
0.5
]
yellow
=
[
0.85
,
0.75
,
0.15
,
1
]
pink
=
[
1
,
0.6
,
1
,
1
]
orange
=
[
1
,
0.42
,
0
,
1
]
brown
=
[
0.85
,
0.75
,
0.15
,
0.5
]
blue
=
[
0.0
,
0.0
,
0.8
,
1.0
]
grey
=
[
0.7
,
0.7
,
0.7
,
1.0
]
red
=
[
0.8
,
0.0
,
0.0
,
1.0
]
rootJointType
=
'freeflyer'
packageName
=
'hpp-rbprm-corba'
meshPackageName
=
'hpp-rbprm-corba'
urdfName
=
'robot_test_trunk'
urdfNameRom
=
[
'robot_test_lleg_rom'
,
'robot_test_rleg_rom'
]
urdfSuffix
=
""
srdfSuffix
=
""
rbprmBuilder
=
Builder
()
rbprmBuilder
.
loadModel
(
urdfName
,
urdfNameRom
,
rootJointType
,
meshPackageName
,
packageName
,
urdfSuffix
,
srdfSuffix
)
rbprmBuilder
.
setJointBounds
(
"base_joint_xyz"
,
[
-
6
,
6
,
-
3
,
3
,
0
,
1.5
])
rbprmBuilder
.
boundSO3
([
-
0.1
,
0.1
,
-
3
,
3
,
-
1.0
,
1.0
])
rbprmBuilder
.
setFilter
([
'robot_test_lleg_rom'
,
'robot_test_rleg_rom'
])
rbprmBuilder
.
setNormalFilter
(
'robot_test_lleg_rom'
,
[
0
,
0
,
1
],
0.5
)
rbprmBuilder
.
setNormalFilter
(
'robot_test_rleg_rom'
,
[
0
,
0
,
1
],
0.5
)
#~ from hpp.corbaserver.rbprm. import ProblemSolver
from
hpp.corbaserver.rbprm.problem_solver
import
ProblemSolver
ps
=
ProblemSolver
(
rbprmBuilder
)
r
=
Viewer
(
ps
)
r
.
loadObstacleModel
(
packageName
,
"ground_jump_easy"
,
"planning"
)
q_init
=
rbprmBuilder
.
getCurrentConfig
();
q_init
[
0
:
3
]
=
[
-
4
,
1
,
0.9
];
rbprmBuilder
.
setCurrentConfig
(
q_init
);
r
(
q_init
)
q_goal
=
q_init
[::]
#q_goal [0:3] = [-2, 0, 0.9]; r (q_goal) # premiere passerelle
q_goal
[
0
:
3
]
=
[
4
,
-
1
,
0.9
];
r
(
q_goal
)
# pont
#~ ps.addPathOptimizer("GradientBased")
ps
.
addPathOptimizer
(
"RandomShortcut"
)
#ps.client.problem.selectSteeringMethod("SteeringParabola")
#ps.selectPathPlanner("RRTdynamic")
ps
.
setInitialConfig
(
q_init
)
ps
.
addGoalConfig
(
q_goal
)
ps
.
client
.
problem
.
selectConFigurationShooter
(
"RbprmShooter"
)
ps
.
client
.
problem
.
selectPathValidation
(
"RbprmPathValidation"
,
0.05
)
r
(
q_init
)
# (nameRoadmap,numberIt,colorNode,radiusSphere,sizeAxis,colorEdge
r
.
solveAndDisplay
(
"rm"
,
10
,
white
,
0.02
,
1
,
brown
)
#t = ps.solve ()
#r.displayRoadmap("rm",0.005)
#r.displayPathMap("rmPath",0,0.02)
from
hpp.gepetto
import
PathPlayer
pp
=
PathPlayer
(
rbprmBuilder
.
client
.
basic
,
r
)
pp
(
0
)
#pp.displayPath(1,blue)
#r.client.gui.setVisibility("path_0_root","ALWAYS_ON_TOP")
pp
(
1
)
#r.client.gui.removeFromGroup("rm",r.sceneName)
#r.client.gui.removeFromGroup("rmPath",r.sceneName)
#r.client.gui.removeFromGroup("path_1_root",r.sceneName)
#~ pp.toFile(1, "/home/stonneau/dev/hpp/src/hpp-rbprm-corba/script/paths/stair.path")
script/tests/robot_bigStep_path.py
deleted
100644 → 0
View file @
8c6eb02e
from
hpp.corbaserver.rbprm.rbprmbuilder
import
Builder
from
hpp.gepetto
import
Viewer
white
=
[
1.0
,
1.0
,
1.0
,
1.0
]
green
=
[
0.23
,
0.75
,
0.2
,
0.5
]
yellow
=
[
0.85
,
0.75
,
0.15
,
1
]
pink
=
[
1
,
0.6
,
1
,
1
]
orange
=
[
1
,
0.42
,
0
,
1
]
brown
=
[
0.85
,
0.75
,
0.15
,
0.5
]
blue
=
[
0.0
,
0.0
,
0.8
,
1.0
]
grey
=
[
0.7
,
0.7
,
0.7
,
1.0
]
red
=
[
0.8
,
0.0
,
0.0
,
1.0
]
black
=
[
0
,
0
,
0
,
1
]
rootJointType
=
'freeflyer'
packageName
=
'hpp-rbprm-corba'
meshPackageName
=
'hpp-rbprm-corba'
urdfName
=
'robot_test_trunk'
urdfNameRom
=
[
'robot_test_lleg_rom'
,
'robot_test_rleg_rom'
]
urdfSuffix
=
""
srdfSuffix
=
""
rbprmBuilder
=
Builder
()
rbprmBuilder
.
loadModel
(
urdfName
,
urdfNameRom
,
rootJointType
,
meshPackageName
,
packageName
,
urdfSuffix
,
srdfSuffix
)
rbprmBuilder
.
setJointBounds
(
"base_joint_xyz"
,
[
-
6
,
6
,
-
3
,
3
,
0
,
1.5
])
rbprmBuilder
.
boundSO3
([
-
0.1
,
0.1
,
-
3
,
3
,
-
1.0
,
1.0
])
rbprmBuilder
.
setFilter
([
'robot_test_lleg_rom'
,
'robot_test_rleg_rom'
])
rbprmBuilder
.
setNormalFilter
(
'robot_test_lleg_rom'
,
[
0
,
0
,
1
],
0.5
)
rbprmBuilder
.
setNormalFilter
(
'robot_test_rleg_rom'
,
[
0
,
0
,
1
],
0.5
)
rbprmBuilder
.
client
.
basic
.
robot
.
setDimensionExtraConfigSpace
(
3
)
rbprmBuilder
.
client
.
basic
.
robot
.
setExtraConfigSpaceBounds
([
0
,
0
,
0
,
0
,
0
,
0
])
#~ from hpp.corbaserver.rbprm. import ProblemSolver
from
hpp.corbaserver.rbprm.problem_solver
import
ProblemSolver
ps
=
ProblemSolver
(
rbprmBuilder
)
r
=
Viewer
(
ps
)
r
.
loadObstacleModel
(
packageName
,
"ground_bigStep"
,
"planning"
)
q_init
=
rbprmBuilder
.
getCurrentConfig
();
#q_init[(len(q_init)-3):]=[0,0,1] # set normal for init / goal config
q_init
[
0
:
3
]
=
[
-
4
,
1
,
0.9
];
rbprmBuilder
.
setCurrentConfig
(
q_init
);
r
(
q_init
)
q_goal
=
q_init
[::]
#q_goal [0:3] = [-2, 1, 0.6]; r (q_goal) # tryDirectPath
q_goal
[
0
:
3
]
=
[
4
,
-
1
,
0.9
];
r
(
q_goal
)
#~ ps.addPathOptimizer("GradientBased")
#ps.addPathOptimizer("RandomShortcut")
ps
.
selectPathPlanner
(
"RRTdynamic"
)
ps
.
setInitialConfig
(
q_init
)
ps
.
addGoalConfig
(
q_goal
)
ps
.
client
.
problem
.
selectConFigurationShooter
(
"RbprmShooter"
)
ps
.
client
.
problem
.
selectPathValidation
(
"RbprmPathValidation"
,
0.05
)
r
(
q_init
)
#r.solveAndDisplay("rm",1,0.02)
t
=
ps
.
solve
()
r
.
displayRoadmap
(
"rm"
,
0.005
)
#r.displayPathMap("rmPath",0,0.02,colorEdge=blue)
from
hpp.gepetto
import
PathPlayer
pp
=
PathPlayer
(
rbprmBuilder
.
client
.
basic
,
r
)
pp
.
displayPath
(
0
,
red
)
pp
(
0
)
r
.
client
.
gui
.
setVisibility
(
"path_0_root"
,
"ALWAYS_ON_TOP"
)
pp
.
displayPath
(
1
,
black
)
pp
(
1
)
#r.client.gui.removeFromGroup("rm",r.sceneName)
r
.
client
.
gui
.
removeFromGroup
(
"rmPath"
,
r
.
sceneName
)
r
.
client
.
gui
.
removeFromGroup
(
"path_1_root"
,
r
.
sceneName
)
#~ pp.toFile(1, "/home/stonneau/dev/hpp/src/hpp-rbprm-corba/script/paths/stair.path")
script/tests/robot_jumpEasy_path.py
deleted
100644 → 0
View file @
8c6eb02e
from
hpp.corbaserver.rbprm.rbprmbuilder
import
Builder
from
hpp.gepetto
import
Viewer
white
=
[
1.0
,
1.0
,
1.0
,
1.0
]
green
=
[
0.23
,
0.75
,
0.2
,
0.5
]
yellow
=
[
0.85
,
0.75
,
0.15
,
1
]
pink
=
[
1
,
0.6
,
1
,
1
]
orange
=
[
1
,
0.42
,
0
,
1
]
brown
=
[
0.85
,
0.75
,
0.15
,
0.5
]
blue
=
[
0.0
,
0.0
,
0.8
,
1.0
]
grey
=
[
0.7
,
0.7
,
0.7
,
1.0
]
red
=
[
0.8
,
0.0
,
0.0
,
1.0
]
rootJointType
=
'freeflyer'
packageName
=
'hpp-rbprm-corba'
meshPackageName
=
'hpp-rbprm-corba'
urdfName
=
'robot_test_trunk'
urdfNameRom
=
[
'robot_test_lleg_rom'
,
'robot_test_rleg_rom'
]
urdfSuffix
=
""
srdfSuffix
=
""
rbprmBuilder
=
Builder
()
rbprmBuilder
.
loadModel
(
urdfName
,
urdfNameRom
,
rootJointType
,
meshPackageName
,
packageName
,
urdfSuffix
,
srdfSuffix
)
rbprmBuilder
.
setJointBounds
(
"base_joint_xyz"
,
[
-
6
,
6
,
-
3
,
3
,
0
,
2.5
])
rbprmBuilder
.
boundSO3
([
-
0.1
,
0.1
,
-
1
,
1
,
-
1
,
1
])
rbprmBuilder
.
setFilter
([
'robot_test_lleg_rom'
,
'robot_test_rleg_rom'
])
rbprmBuilder
.
setNormalFilter
(
'robot_test_lleg_rom'
,
[
0
,
0
,
1
],
0.5
)
rbprmBuilder
.
setNormalFilter
(
'robot_test_rleg_rom'
,
[
0
,
0
,
1
],
0.5
)
rbprmBuilder
.
client
.
basic
.
robot
.
setDimensionExtraConfigSpace
(
3
)
rbprmBuilder
.
client
.
basic
.
robot
.
setExtraConfigSpaceBounds
([
0
,
0
,
0
,
0
,
0
,
0
])
#~ from hpp.corbaserver.rbprm. import ProblemSolver
from
hpp.corbaserver.rbprm.problem_solver
import
ProblemSolver
ps
=
ProblemSolver
(
rbprmBuilder
)
r
=
Viewer
(
ps
)
r
.
loadObstacleModel
(
packageName
,
"ground_jump_easy"
,
"planning"
)
q_init
=
rbprmBuilder
.
getCurrentConfig
();
#q_init[(len(q_init)-3):]=[0,0,1] # set normal for init / goal config
q_init
[
0
:
3
]
=
[
-
4
,
1
,
0.9
];
rbprmBuilder
.
setCurrentConfig
(
q_init
);
r
(
q_init
)
q_goal
=
q_init
[::]
#q_goal [0:3] = [-2, 0, 0.9]; r (q_goal) # premiere passerelle
q_goal
[
0
:
3
]
=
[
3
,
1
,
0.9
];
r
(
q_goal
)
# pont
#~ ps.addPathOptimizer("GradientBased")
ps
.
addPathOptimizer
(
"RandomShortcut"
)
#ps.client.problem.selectSteeringMethod("SteeringDynamic")
ps
.
selectPathPlanner
(
"RRTdynamic"
)
ps
.
setInitialConfig
(
q_init
)
ps
.
addGoalConfig
(
q_goal
)
ps
.
client
.
problem
.
selectConFigurationShooter
(
"RbprmShooter"
)
ps
.
client
.
problem
.
selectPathValidation
(
"RbprmPathValidation"
,
0.05
)
r
(
q_init
)
#ps.client.problem.prepareSolveStepByStep()
#i = 0
#r.displayRoadmap("rm"+str(i),0.02)
#ps.client.problem.executeOneStep() ;i = i+1; r.displayRoadmap("rm"+str(i),0.02) ; r.client.gui.removeFromGroup("rm"+str(i-1),r.sceneName) ;
#t = ps.solve ()
r
.
solveAndDisplay
(
"rm"
,
1
,
0.02
)
#t = ps.solve ()
#r.displayRoadmap("rm",0.02)
r
.
displayPathMap
(
"rmPath"
,
0
,
0.02
)
from
hpp.gepetto
import
PathPlayer
pp
=
PathPlayer
(
rbprmBuilder
.
client
.
basic
,
r
)
pp
.
displayPath
(
0
,
r
.
color
.
lightGreen
)
pp
(
0
)
pp
.
displayPath
(
1
,
blue
)
r
.
client
.
gui
.
setVisibility
(
"path_0_root"
,
"ALWAYS_ON_TOP"
)
pp
.
displayPath
(
1
,
black
)
pp
(
1
)
#r.client.gui.removeFromGroup("rm",r.sceneName)
r
.
client
.
gui
.
removeFromGroup
(
"rmPath"
,
r
.
sceneName
)
r
.
client
.
gui
.
removeFromGroup
(
"path_1_root"
,
r
.
sceneName
)
#~ pp.toFile(1, "/home/stonneau/dev/hpp/src/hpp-rbprm-corba/script/paths/stair.path")
script/tests/robot_test_path.py
deleted
100644 → 0
View file @
8c6eb02e
from
hpp.corbaserver.rbprm.rbprmbuilder
import
Builder
from
hpp.gepetto
import
Viewer
white
=
[
1.0
,
1.0
,
1.0
,
1.0
]
green
=
[
0.23
,
0.75
,
0.2
,
0.5
]
yellow
=
[
0.85
,
0.75
,
0.15
,
1
]
pink
=
[
1
,
0.6
,
1
,
1
]
orange
=
[
1
,
0.42
,
0
,
1
]
brown
=
[
0.85
,
0.75
,
0.15
,
0.5
]
blue
=
[
0.0
,
0.0
,
0.8
,
1.0
]
grey
=
[
0.7
,
0.7
,
0.7
,
1.0
]
red
=
[
0.8
,
0.0
,
0.0
,
1.0
]
rootJointType
=
'freeflyer'
packageName
=
'hpp-rbprm-corba'
meshPackageName
=
'hpp-rbprm-corba'
urdfName
=
'robot_test_trunk'
urdfNameRom
=
[
'robot_test_lleg_rom'
,
'robot_test_rleg_rom'
]
urdfSuffix
=
""
srdfSuffix
=
""
rbprmBuilder
=
Builder
()
rbprmBuilder
.
loadModel
(
urdfName
,
urdfNameRom
,
rootJointType
,
meshPackageName
,
packageName
,
urdfSuffix
,
srdfSuffix
)
rbprmBuilder
.
setJointBounds
(
"base_joint_xyz"
,
[
-
8
,
6
,
-
2
,
2
,
0.4
,
1.3
])
rbprmBuilder
.
boundSO3
([
-
0.1
,
0.1
,
-
3
,
3
,
-
1.0
,
1.0
])
rbprmBuilder
.
setFilter
([
'robot_test_lleg_rom'
,
'robot_test_rleg_rom'
])
rbprmBuilder
.
setNormalFilter
(
'robot_test_lleg_rom'
,
[
0
,
0
,
1
],
0.5
)
rbprmBuilder
.
setNormalFilter
(
'robot_test_rleg_rom'
,
[
0
,
0
,
1
],
0.5
)
rbprmBuilder
.
client
.
basic
.
robot
.
setDimensionExtraConfigSpace
(
3
)
rbprmBuilder
.
client
.
basic
.
robot
.
setExtraConfigSpaceBounds
([
0
,
0
,
0
,
0
,
0
,
0
])
#~ from hpp.corbaserver.rbprm. import ProblemSolver
from
hpp.corbaserver.rbprm.problem_solver
import
ProblemSolver
ps
=
ProblemSolver
(
rbprmBuilder
)
r
=
Viewer
(
ps
)
r
.
loadObstacleModel
(
packageName
,
"groundcrouch"
,
"planning"
)
q_init
=
rbprmBuilder
.
getCurrentConfig
();
q_init
[(
len
(
q_init
)
-
3
):]
=
[
0
,
0
,
1
]
# set normal for init / goal config
q_init
[
0
:
3
]
=
[
-
6
,
0
,
0.9
];
rbprmBuilder
.
setCurrentConfig
(
q_init
);
r
(
q_init
)
q_goal
=
q_init
[::]
#q_goal [0:3] = [-2, 0, 0.9]; r (q_goal) # premiere passerelle
q_goal
[
0
:
3
]
=
[
4
,
0
,
0.9
];
r
(
q_goal
)
# pont
#~ ps.addPathOptimizer("GradientBased")
ps
.
addPathOptimizer
(
"RandomShortcut"
)
ps
.
selectPathPlanner
(
"RRTdynamic"
)
ps
.
setInitialConfig
(
q_init
)
ps
.
addGoalConfig
(
q_goal
)
ps
.
client
.
problem
.
selectConFigurationShooter
(
"RbprmShooter"
)
ps
.
client
.
problem
.
selectPathValidation
(
"RbprmPathValidation"
,
0.05
)
r
(
q_init
)
r
.
solveAndDisplay
(
"rm"
,
1
,
0.02
)
#t = ps.solve ()
#r.displayRoadmap("rm",0.005)
r
.
displayPathMap
(
"rmPath"
,
0
,
0.02
)
from
hpp.gepetto
import
PathPlayer
pp
=
PathPlayer
(
rbprmBuilder
.
client
.
basic
,
r
)
pp
(
0
)
pp
.
displayPath
(
1
,
blue
)
r
.
client
.
gui
.
setVisibility
(
"path_0_root"
,
"ALWAYS_ON_TOP"
)
pp
(
1
)
#r.client.gui.removeFromGroup("rm",r.sceneName)
r
.
client
.
gui
.
removeFromGroup
(
"rmPath"
,
r
.
sceneName
)
r
.
client
.
gui
.
removeFromGroup
(
"path_1_root"
,
r
.
sceneName
)
#~ pp.toFile(1, "/home/stonneau/dev/hpp/src/hpp-rbprm-corba/script/paths/stair.path")
script/tests/robot_withBridge_path.py
deleted
100644 → 0
View file @
8c6eb02e
from
hpp.corbaserver.rbprm.rbprmbuilder
import
Builder
from
hpp.gepetto
import
Viewer
white
=
[
1.0
,
1.0
,
1.0
,
1.0
]
green
=
[
0.23
,
0.75
,
0.2
,
0.5
]
yellow
=
[
0.85
,
0.75
,
0.15
,
1
]
pink
=
[
1
,
0.6
,
1
,
1
]
orange
=
[
1
,
0.42
,
0
,
1
]
brown
=
[
0.85
,
0.75
,
0.15
,
0.5
]
blue
=
[
0.0
,
0.0
,
0.8
,
1.0
]
grey
=
[
0.7
,
0.7
,
0.7
,
1.0
]
red
=
[
0.8
,
0.0
,
0.0
,
1.0
]
black
=
[
0
,
0
,
0
,
1
]
rootJointType
=
'freeflyer'
packageName
=
'hpp-rbprm-corba'
meshPackageName
=
'hpp-rbprm-corba'
urdfName
=
'robot_test_trunk'
urdfNameRom
=
[
'robot_test_lleg_rom'
,
'robot_test_rleg_rom'
]
urdfSuffix
=
""
srdfSuffix
=
""
rbprmBuilder
=
Builder
()
rbprmBuilder
.
loadModel
(
urdfName
,
urdfNameRom
,
rootJointType
,
meshPackageName
,
packageName
,
urdfSuffix
,
srdfSuffix
)
rbprmBuilder
.
setJointBounds
(
"base_joint_xyz"
,
[
-
6
,
6
,
-
3
,
3
,
0
,
2.5
])
# limits zyx
rbprmBuilder
.
boundSO3
([
-
1
,
1
,
-
0.2
,
0.2
,
-
0.2
,
0.2
])
rbprmBuilder
.
setFilter
([
'robot_test_lleg_rom'
,
'robot_test_rleg_rom'
])
rbprmBuilder
.
setNormalFilter
(
'robot_test_lleg_rom'
,
[
0
,
0
,
1
],
0.5
)
rbprmBuilder
.
setNormalFilter
(
'robot_test_rleg_rom'
,
[
0
,
0
,
1
],
0.5
)
rbprmBuilder
.
client
.
basic
.
robot
.
setDimensionExtraConfigSpace
(
3
)
rbprmBuilder
.
client
.
basic
.
robot
.
setExtraConfigSpaceBounds
([
0
,
0
,
0
,
0
,
0
,
0
])
#~ from hpp.corbaserver.rbprm. import ProblemSolver
from