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
Guilhem Saurel
hpp-rbprm-corba
Commits
d924e400
Commit
d924e400
authored
Mar 29, 2017
by
Steve Tonneau
Browse files
cleaned scenarios
parent
6ad6ecf5
Changes
23
Hide whitespace changes
Inline
Side-by-side
script/
scenarios/
demos/darpa_hyq.py
→
script/demos/darpa_hyq.py
View file @
d924e400
...
...
@@ -101,24 +101,6 @@ from hpp.gepetto import PathPlayer
pp
=
PathPlayer
(
fullBody
.
client
.
basic
,
r
)
from
hpp.corbaserver.rbprm.tools.cwc_trajectory_helper
import
step
,
clean
,
stats
,
saveAllData
,
play_traj
limbsCOMConstraints
=
{
rLegId
:
{
'file'
:
"hyq/"
+
rLegId
+
"_com.ineq"
,
'effector'
:
rfoot
},
lLegId
:
{
'file'
:
"hyq/"
+
lLegId
+
"_com.ineq"
,
'effector'
:
lfoot
},
rarmId
:
{
'file'
:
"hyq/"
+
rarmId
+
"_com.ineq"
,
'effector'
:
rHand
},
larmId
:
{
'file'
:
"hyq/"
+
larmId
+
"_com.ineq"
,
'effector'
:
lHand
}
}
def
act
(
i
,
numOptim
=
0
,
use_window
=
0
,
friction
=
0.5
,
optim_effectors
=
True
,
verbose
=
False
,
draw
=
False
):
return
step
(
fullBody
,
configs
,
i
,
numOptim
,
pp
,
limbsCOMConstraints
,
0.4
,
optim_effectors
=
optim_effectors
,
time_scale
=
20.
,
useCOMConstraints
=
True
,
use_window
=
use_window
,
verbose
=
verbose
,
draw
=
draw
)
def
play
(
frame_rate
=
1.
/
24.
):
play_traj
(
fullBody
,
pp
,
frame_rate
)
import
time
#DEMO METHODS
...
...
script/
scenarios/
demos/darpa_hyq_path.py
→
script/demos/darpa_hyq_path.py
View file @
d924e400
File moved
script/
scenarios/
demos/run.sh
→
script/demos/run.sh
View file @
d924e400
File moved
script/
scenarios/
demos/stair_bauzil_hrp2_interp.py
→
script/demos/stair_bauzil_hrp2_interp.py
View file @
d924e400
...
...
@@ -49,41 +49,7 @@ rArmx = 0.024; rArmy = 0.024
#disabling collision for hook
fullBody
.
addLimb
(
rarmId
,
rarm
,
rHand
,
rArmOffset
,
rArmNormal
,
rArmx
,
rArmy
,
10000
,
"manipulability"
,
0.05
,
"_6_DOF"
,
True
)
#~ AFTER loading obstacles
larmId
=
'4Larm'
larm
=
'LARM_JOINT0'
lHand
=
'LARM_JOINT5'
lArmOffset
=
[
-
0.05
,
-
0.050
,
-
0.050
]
lArmNormal
=
[
1
,
0
,
0
]
lArmx
=
0.024
;
lArmy
=
0.024
#~ fullBody.addLimb(larmId,larm,lHand,lArmOffset,lArmNormal, lArmx, lArmy, 10000, 0.05)
rKneeId
=
'0RKnee'
rLeg
=
'RLEG_JOINT0'
rKnee
=
'RLEG_JOINT3'
rLegOffset
=
[
0.105
,
0.055
,
0.017
]
rLegNormal
=
[
-
1
,
0
,
0
]
rLegx
=
0.05
;
rLegy
=
0.05
#~ fullBody.addLimb(rKneeId, rLeg,rKnee,rLegOffset,rLegNormal, rLegx, rLegy, 10000, 0.01)
#~
lKneeId
=
'1LKnee'
lLeg
=
'LLEG_JOINT0'
lKnee
=
'LLEG_JOINT3'
lLegOffset
=
[
0.105
,
0.055
,
0.017
]
lLegNormal
=
[
-
1
,
0
,
0
]
lLegx
=
0.05
;
lLegy
=
0.05
#~ fullBody.addLimb(lKneeId,lLeg,lKnee,lLegOffset,lLegNormal, lLegx, lLegy, 10000, 0.01)
#~
#~ fullBody.runLimbSampleAnalysis(rLegId, "jointLimitsDistance", True)
#~ fullBody.runLimbSampleAnalysis(lLegId, "jointLimitsDistance", True)
#~ fullBody.client.basic.robot.setJointConfig('LARM_JOINT0',[1])
#~ fullBody.client.basic.robot.setJointConfig('RARM_JOINT0',[-1])
q_0
=
fullBody
.
getCurrentConfig
();
#~ fullBody.createOctreeBoxes(r.client.gui, 1, rarmId, q_0,)
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
]
...
...
@@ -103,54 +69,14 @@ fullBody.setCurrentConfig (q_goal)
q_goal
=
fullBody
.
generateContacts
(
q_goal
,
[
0
,
0
,
1
])
#~ r(q_goal)
fullBody
.
setStartState
(
q_init
,[
rLegId
,
lLegId
])
#,rarmId,larmId])
fullBody
.
setEndState
(
q_goal
,[
rLegId
,
lLegId
])
#,rarmId,larmId])
#~
#~ configs = fullBody.interpolate(0.1)
fullBody
.
setStartState
(
q_init
,[
rLegId
,
lLegId
])
fullBody
.
setEndState
(
q_goal
,[
rLegId
,
lLegId
])
configs
=
fullBody
.
interpolate
(
0.1
)
#~ configs = fullBody.interpolate(0.15)
i
=
0
;
fullBody
.
draw
(
configs
[
i
],
r
);
i
=
i
+
1
;
i
-
1
r
.
loadObstacleModel
(
'hpp-rbprm-corba'
,
"stair_bauzil"
,
"contact"
)
#~ fullBody.exportAll(r, configs, 'stair_bauzil_hrp2_robust_2');
#~ fullBody.client.basic.robot.setJointConfig('LLEG_JOINT0',[-1])
#~ q_0 = fullBody.getCurrentConfig();
#~ fullBody.draw(q_0,r);
#~ print(fullBody.client.rbprm.rbprm.getOctreeTransform(rarmId, q_0))
#~
#~
#~ fullBody.client.basic.robot.setJointConfig('LLEG_JOINT0',[1])
#~ q_0 = fullBody.getCurrentConfig();
#~ fullBody.draw(q_0,r);
#~ print(fullBody.client.rbprm.rbprm.getOctreeTransform(rarmId, q_0))
#~ q_init = fullBody.generateContacts(q_init, [0,0,-1]); r (q_init)
#~ f1 = open("secondchoice","w+")
#~ f1 = open("hrp2_stair_not_robust_configs","w+")
#~ f1.write(str(configs))
#~ f1.close()
limbsCOMConstraints
=
{
rLegId
:
{
'file'
:
"hrp2/RL_com.ineq"
,
'effector'
:
'RLEG_JOINT5'
},
lLegId
:
{
'file'
:
"hrp2/LL_com.ineq"
,
'effector'
:
'LLEG_JOINT5'
},
rarmId
:
{
'file'
:
"hrp2/RA_com.ineq"
,
'effector'
:
rHand
}
}
#~ larmId : {'file': "hrp2/LA_com.ineq", 'effector' : lHand} }
#~ fullBody.limbRRTFromRootPath(0,len(configs)-1,0,2)
from
hpp.corbaserver.rbprm.tools.cwc_trajectory_helper
import
step
,
clean
,
stats
,
saveAllData
,
play_traj
from
hpp.gepetto
import
PathPlayer
pp
=
PathPlayer
(
fullBody
.
client
.
basic
,
r
)
def
act
(
i
,
numOptim
=
0
,
use_window
=
0
,
friction
=
0.5
,
optim_effectors
=
True
,
verbose
=
False
,
draw
=
False
):
return
step
(
fullBody
,
configs
,
i
,
numOptim
,
pp
,
limbsCOMConstraints
,
0.4
,
optim_effectors
=
optim_effectors
,
time_scale
=
20.
,
useCOMConstraints
=
True
,
use_window
=
use_window
,
verbose
=
verbose
,
draw
=
draw
)
def
play
(
frame_rate
=
1.
/
24.
):
play_traj
(
fullBody
,
pp
,
frame_rate
)
def
saveAll
(
name
):
saveAllData
(
fullBody
,
r
,
name
)
def
initConfig
():
r
.
client
.
gui
.
setVisibility
(
"hrp2_14"
,
"ON"
)
...
...
script/
scenarios/
demos/stair_bauzil_hrp2_path.py
→
script/demos/stair_bauzil_hrp2_path.py
View file @
d924e400
File moved
script/
scenarios/
demos/stair_bauzil_hrp2_path2.py
→
script/demos/stair_bauzil_hrp2_path2.py
View file @
d924e400
File moved
script/
scenarios/
demos/stair_bauzil_hrp2_path3.py
→
script/demos/stair_bauzil_hrp2_path3.py
View file @
d924e400
File moved
script/scenarios/demos/ground_crouch_hyq_interp.py
deleted
100755 → 0
View file @
6ad6ecf5
import
matplotlib
#~ matplotlib.use('Agg')
import
matplotlib.pyplot
as
plt
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
from
os
import
environ
ins_dir
=
environ
[
'DEVEL_DIR'
]
db_dir
=
ins_dir
+
"/install/share/hyq-rbprm/database/hyq_"
import
ground_crouch_hyq_path
as
tp
#~ import ground_crouch_hyq_path_bridge as tp
packageName
=
"hyq_description"
meshPackageName
=
"hyq_description"
rootJointType
=
"freeflyer"
##
# Information to retrieve urdf and srdf files.
urdfName
=
"hyq"
urdfSuffix
=
""
srdfSuffix
=
""
fullBody
=
FullBody
()
fullBody
.
loadFullBodyModel
(
urdfName
,
rootJointType
,
meshPackageName
,
packageName
,
urdfSuffix
,
srdfSuffix
)
fullBody
.
setJointBounds
(
"base_joint_xyz"
,
[
-
6
,
5
,
-
4
,
4
,
0.6
,
2
])
from
hpp.corbaserver.rbprm.problem_solver
import
ProblemSolver
nbSamples
=
20000
ps
=
tp
.
ProblemSolver
(
fullBody
)
r
=
tp
.
Viewer
(
ps
,
viewerClient
=
tp
.
r
.
client
)
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).
def
addLimbDb
(
limbId
,
heuristicName
,
loadValues
=
True
,
disableEffectorCollision
=
False
):
fullBody
.
addLimbDatabase
(
str
(
db_dir
+
limbId
+
'.db'
),
limbId
,
heuristicName
,
loadValues
,
disableEffectorCollision
)
fullBody
.
addLimb
(
rLegId
,
rLeg
,
rfoot
,
offset
,
normal
,
legx
,
legy
,
nbSamples
,
"forward"
,
0.1
,
cType
)
lLegId
=
'lhleg'
lLeg
=
'lh_haa_joint'
lfoot
=
'lh_foot_joint'
fullBody
.
addLimb
(
lLegId
,
lLeg
,
lfoot
,
offset
,
normal
,
legx
,
legy
,
nbSamples
,
"backward"
,
0.05
,
cType
)
rarmId
=
'rhleg'
rarm
=
'rh_haa_joint'
rHand
=
'rh_foot_joint'
fullBody
.
addLimb
(
rarmId
,
rarm
,
rHand
,
offset
,
normal
,
legx
,
legy
,
nbSamples
,
"backward"
,
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
)
fullBody
.
runLimbSampleAnalysis
(
rLegId
,
"jointLimitsDistance"
,
True
)
fullBody
.
runLimbSampleAnalysis
(
rarmId
,
"jointLimitsDistance"
,
True
)
fullBody
.
runLimbSampleAnalysis
(
larmId
,
"jointLimitsDistance"
,
True
)
fullBody
.
runLimbSampleAnalysis
(
lLegId
,
"jointLimitsDistance"
,
True
)
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
]
fullBody
.
setCurrentConfig
(
q_init
)
q_init
=
fullBody
.
generateContacts
(
q_init
,
[
0
,
0
,
1
])
q_0
=
fullBody
.
getCurrentConfig
();
fullBody
.
setCurrentConfig
(
q_goal
)
q_goal
=
fullBody
.
generateContacts
(
q_goal
,
[
0
,
0
,
1
])
fullBody
.
setStartState
(
q_init
,[])
fullBody
.
setEndState
(
q_goal
,[
rLegId
,
lLegId
,
rarmId
,
larmId
])
r
(
q_init
)
configs
=
[]
#~ configs = fullBody.interpolate(0.08,1,5) # bridge
from
hpp.gepetto
import
PathPlayer
pp
=
PathPlayer
(
fullBody
.
client
.
basic
,
r
)
from
hpp.corbaserver.rbprm.tools.cwc_trajectory_helper
import
step
,
clean
,
stats
,
saveAllData
,
play_traj
limbsCOMConstraints
=
{
rLegId
:
{
'file'
:
"hyq/"
+
rLegId
+
"_com.ineq"
,
'effector'
:
rfoot
},
lLegId
:
{
'file'
:
"hyq/"
+
lLegId
+
"_com.ineq"
,
'effector'
:
lfoot
},
rarmId
:
{
'file'
:
"hyq/"
+
rarmId
+
"_com.ineq"
,
'effector'
:
rHand
},
larmId
:
{
'file'
:
"hyq/"
+
larmId
+
"_com.ineq"
,
'effector'
:
lHand
}
}
def
act
(
i
,
numOptim
=
0
,
use_window
=
0
,
friction
=
0.5
,
optim_effectors
=
True
,
time_scale
=
20
,
verbose
=
False
,
draw
=
False
,
trackedEffectors
=
[]):
return
step
(
fullBody
,
configs
,
i
,
numOptim
,
pp
,
limbsCOMConstraints
,
friction
,
optim_effectors
=
optim_effectors
,
time_scale
=
time_scale
,
useCOMConstraints
=
False
,
use_window
=
use_window
,
verbose
=
verbose
,
draw
=
draw
,
trackedEffectors
=
trackedEffectors
)
#~ for i in range(6,25):
#~ act(i, 60, optim_effectors = True)
import
time
#DEMO METHODS
def
initConfig
():
r
.
client
.
gui
.
setVisibility
(
"hyq"
,
"ON"
)
tp
.
cl
.
problem
.
selectProblem
(
"default"
)
tp
.
r
.
client
.
gui
.
setVisibility
(
"toto"
,
"OFF"
)
tp
.
r
.
client
.
gui
.
setVisibility
(
"hyq_trunk"
,
"OFF"
)
r
(
q_init
)
def
endConfig
():
r
.
client
.
gui
.
setVisibility
(
"hyq"
,
"ON"
)
tp
.
cl
.
problem
.
selectProblem
(
"default"
)
tp
.
r
.
client
.
gui
.
setVisibility
(
"toto"
,
"OFF"
)
tp
.
r
.
client
.
gui
.
setVisibility
(
"hyq_trunk"
,
"OFF"
)
r
(
q_goal
)
def
rootPath
():
r
.
client
.
gui
.
setVisibility
(
"hyq"
,
"OFF"
)
tp
.
cl
.
problem
.
selectProblem
(
"rbprm_path"
)
tp
.
r
.
client
.
gui
.
setVisibility
(
"toto"
,
"OFF"
)
r
.
client
.
gui
.
setVisibility
(
"hyq"
,
"OFF"
)
r
.
client
.
gui
.
setVisibility
(
"hyq_trunk"
,
"ON"
)
tp
.
pp
(
0
)
r
.
client
.
gui
.
setVisibility
(
"hyq_trunk"
,
"OFF"
)
r
.
client
.
gui
.
setVisibility
(
"hyq"
,
"ON"
)
tp
.
cl
.
problem
.
selectProblem
(
"default"
)
def
genPlan
():
r
.
client
.
gui
.
setVisibility
(
"hyq"
,
"ON"
)
tp
.
cl
.
problem
.
selectProblem
(
"default"
)
tp
.
r
.
client
.
gui
.
setVisibility
(
"toto"
,
"OFF"
)
tp
.
r
.
client
.
gui
.
setVisibility
(
"hyq_trunk"
,
"OFF"
)
global
configs
start
=
time
.
clock
()
configs
=
fullBody
.
interpolate
(
0.12
,
10
,
10
,
True
)
end
=
time
.
clock
()
print
"Contact plan generated in "
+
str
(
end
-
start
)
+
"seconds"
def
contactPlan
():
tp
.
cl
.
problem
.
selectProblem
(
"default"
)
r
.
client
.
gui
.
setVisibility
(
"hyq"
,
"ON"
)
tp
.
r
.
client
.
gui
.
setVisibility
(
"toto"
,
"OFF"
)
tp
.
r
.
client
.
gui
.
setVisibility
(
"hyq_trunk"
,
"OFF"
)
for
i
in
range
(
1
,
len
(
configs
)):
r
(
configs
[
i
]);
time
.
sleep
(
0.5
)
def
a
():
print
"initial configuration"
initConfig
()
def
b
():
print
"end configuration"
endConfig
()
def
c
():
print
"displaying root path"
rootPath
()
def
d
():
print
"computing contact plan"
genPlan
()
def
e
():
print
"displaying contact plan"
contactPlan
()
print
"Root path generated in "
+
str
(
tp
.
t
)
+
" ms."
script/scenarios/demos/ground_crouch_hyq_interp_bridge.py
deleted
100644 → 0
View file @
6ad6ecf5
import
matplotlib
#~ matplotlib.use('Agg')
import
matplotlib.pyplot
as
plt
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
from
os
import
environ
ins_dir
=
environ
[
'DEVEL_DIR'
]
db_dir
=
ins_dir
+
"/install/share/hyq-rbprm/database/hyq_"
import
ground_crouch_hyq_path_bridge
as
tp
#~ import ground_crouch_hyq_path_bridge as tp
packageName
=
"hyq_description"
meshPackageName
=
"hyq_description"
rootJointType
=
"freeflyer"
##
# Information to retrieve urdf and srdf files.
urdfName
=
"hyq"
urdfSuffix
=
""
srdfSuffix
=
""
fullBody
=
FullBody
()
fullBody
.
loadFullBodyModel
(
urdfName
,
rootJointType
,
meshPackageName
,
packageName
,
urdfSuffix
,
srdfSuffix
)
fullBody
.
setJointBounds
(
"base_joint_xyz"
,
[
-
6
,
5
,
-
4
,
4
,
0.6
,
2
])
from
hpp.corbaserver.rbprm.problem_solver
import
ProblemSolver
nbSamples
=
20000
ps
=
tp
.
ProblemSolver
(
fullBody
)
r
=
tp
.
Viewer
(
ps
,
viewerClient
=
tp
.
r
.
client
)
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).
def
addLimbDb
(
limbId
,
heuristicName
,
loadValues
=
True
,
disableEffectorCollision
=
False
):
fullBody
.
addLimbDatabase
(
str
(
db_dir
+
limbId
+
'.db'
),
limbId
,
heuristicName
,
loadValues
,
disableEffectorCollision
)
fullBody
.
addLimb
(
rLegId
,
rLeg
,
rfoot
,
offset
,
normal
,
legx
,
legy
,
nbSamples
,
"forward"
,
0.1
,
cType
)
lLegId
=
'lhleg'
lLeg
=
'lh_haa_joint'
lfoot
=
'lh_foot_joint'
fullBody
.
addLimb
(
lLegId
,
lLeg
,
lfoot
,
offset
,
normal
,
legx
,
legy
,
nbSamples
,
"backward"
,
0.05
,
cType
)
rarmId
=
'rhleg'
rarm
=
'rh_haa_joint'
rHand
=
'rh_foot_joint'
fullBody
.
addLimb
(
rarmId
,
rarm
,
rHand
,
offset
,
normal
,
legx
,
legy
,
nbSamples
,
"backward"
,
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
)
fullBody
.
runLimbSampleAnalysis
(
rLegId
,
"jointLimitsDistance"
,
True
)
fullBody
.
runLimbSampleAnalysis
(
rarmId
,
"jointLimitsDistance"
,
True
)
fullBody
.
runLimbSampleAnalysis
(
larmId
,
"jointLimitsDistance"
,
True
)
fullBody
.
runLimbSampleAnalysis
(
lLegId
,
"jointLimitsDistance"
,
True
)
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
]
fullBody
.
setCurrentConfig
(
q_init
)
q_init
=
fullBody
.
generateContacts
(
q_init
,
[
0
,
0
,
1
])
q_0
=
fullBody
.
getCurrentConfig
();
fullBody
.
setCurrentConfig
(
q_goal
)
q_goal
=
fullBody
.
generateContacts
(
q_goal
,
[
0
,
0
,
1
])
fullBody
.
setStartState
(
q_init
,[])
fullBody
.
setEndState
(
q_goal
,[
rLegId
,
lLegId
,
rarmId
,
larmId
])
r
(
q_init
)
configs
=
[]
#~ configs = fullBody.interpolate(0.08,1,5) # bridge
from
hpp.gepetto
import
PathPlayer
pp
=
PathPlayer
(
fullBody
.
client
.
basic
,
r
)
from
hpp.corbaserver.rbprm.tools.cwc_trajectory_helper
import
step
,
clean
,
stats
,
saveAllData
,
play_traj
limbsCOMConstraints
=
{
rLegId
:
{
'file'
:
"hyq/"
+
rLegId
+
"_com.ineq"
,
'effector'
:
rfoot
},
lLegId
:
{
'file'
:
"hyq/"
+
lLegId
+
"_com.ineq"
,
'effector'
:
lfoot
},
rarmId
:
{
'file'
:
"hyq/"
+
rarmId
+
"_com.ineq"
,
'effector'
:
rHand
},
larmId
:
{
'file'
:
"hyq/"
+
larmId
+
"_com.ineq"
,
'effector'
:
lHand
}
}
def
act
(
i
,
numOptim
=
0
,
use_window
=
0
,
friction
=
0.5
,
optim_effectors
=
True
,
time_scale
=
20
,
verbose
=
False
,
draw
=
False
,
trackedEffectors
=
[]):
return
step
(
fullBody
,
configs
,
i
,
numOptim
,
pp
,
limbsCOMConstraints
,
friction
,
optim_effectors
=
optim_effectors
,
time_scale
=
time_scale
,
useCOMConstraints
=
False
,
use_window
=
use_window
,
verbose
=
verbose
,
draw
=
draw
,
trackedEffectors
=
trackedEffectors
)
#~ for i in range(6,25):
#~ act(i, 60, optim_effectors = True)
import
time
#DEMO METHODS
def
initConfig
():
r
.
client
.
gui
.
setVisibility
(
"hyq"
,
"ON"
)
tp
.
cl
.
problem
.
selectProblem
(
"default"
)
tp
.
r
.
client
.
gui
.
setVisibility
(
"toto"
,
"OFF"
)
tp
.
r
.
client
.
gui
.
setVisibility
(
"hyq_trunk"
,
"OFF"
)
r
(
q_init
)
def
endConfig
():
r
.
client
.
gui
.
setVisibility
(
"hyq"
,
"ON"
)
tp
.
cl
.
problem
.
selectProblem
(
"default"
)
tp
.
r
.
client
.
gui
.
setVisibility
(
"toto"
,
"OFF"
)
tp
.
r
.
client
.
gui
.
setVisibility
(
"hyq_trunk"
,
"OFF"
)
r
(
q_goal
)
def
rootPath
():
r
.
client
.
gui
.
setVisibility
(
"hyq"
,
"OFF"
)
tp
.
cl
.
problem
.
selectProblem
(
"rbprm_path"
)
tp
.
r
.
client
.
gui
.
setVisibility
(
"toto"
,
"OFF"
)
r
.
client
.
gui
.
setVisibility
(
"hyq"
,
"OFF"
)
r
.
client
.
gui
.
setVisibility
(
"hyq_trunk"
,
"ON"
)
tp
.
pp
(
0
)
r
.
client
.
gui
.
setVisibility
(
"hyq_trunk"
,
"OFF"
)
r
.
client
.
gui
.
setVisibility
(
"hyq"
,
"ON"
)
tp
.
cl
.
problem
.
selectProblem
(
"default"
)
def
genPlan
():
r
.
client
.
gui
.
setVisibility
(
"hyq"
,
"ON"
)
tp
.
cl
.
problem
.
selectProblem
(
"default"
)
tp
.
r
.
client
.
gui
.
setVisibility
(
"toto"
,
"OFF"
)
tp
.
r
.
client
.
gui
.
setVisibility
(
"hyq_trunk"
,
"OFF"
)
global
configs
start
=
time
.
clock
()
configs
=
fullBody
.
interpolate
(
0.12
,
10
,
10
,
True
)
end
=
time
.
clock
()
print
"Contact plan generated in "
+
str
(
end
-
start
)
+
"seconds"
def
contactPlan
():
tp
.
cl
.
problem
.
selectProblem
(
"default"
)
r
.
client
.
gui
.
setVisibility
(
"hyq"
,
"ON"
)
tp
.
r
.
client
.
gui
.
setVisibility
(
"toto"
,
"OFF"
)
tp
.
r
.
client
.
gui
.
setVisibility
(
"hyq_trunk"
,
"OFF"
)
for
i
in
range
(
1
,
len
(
configs
)):
r
(
configs
[
i
]);
time
.
sleep
(
0.5
)
def
interpolate
():
tp
.
cl
.
problem
.
selectProblem
(
"default"
)
r
.
client
.
gui
.
setVisibility
(
"hyq"
,
"ON"
)
tp
.
r
.
client
.
gui
.
setVisibility
(
"toto"
,
"OFF"
)
tp
.
r
.
client
.
gui
.
setVisibility
(
"hyq_trunk"
,
"OFF"
)
for
i
in
range
(
7
,
20
):
act
(
i
,
1
,
optim_effectors
=
True
)
def
play
(
frame_rate
=
1.
/
24.
):
play_traj
(
fullBody
,
pp
,
frame_rate
)
def
a
():
print
"initial configuration"
initConfig
()
def
b
():
print
"end configuration"
endConfig
()
def
c
():
print
"displaying root path"
rootPath
()
def
d
():
print
"computing contact plan"
genPlan
()
def
e
():
print
"displaying contact plan"
contactPlan
()
def
f
():
print
"computing feasible com trajectory"
interpolate
()
def
g
():
print
"playing feasible trajectory"
play
()
print
"Root path generated in "
+
str
(
tp
.
t
)
+
" ms."
script/scenarios/demos/ground_crouch_hyq_path.py
deleted
100755 → 0
View file @
6ad6ecf5
from
hpp.corbaserver.rbprm.rbprmbuilder
import
Builder
from
hpp.gepetto
import
Viewer
from
hpp.corbaserver
import
Client
from
hpp.corbaserver.robot
import
Robot
as
Parent
class
Robot
(
Parent
):
rootJointType
=
'freeflyer'
packageName
=
'hpp-rbprm-corba'
meshPackageName
=
'hpp-rbprm-corba'
# URDF file describing the trunk of the robot HyQ
urdfName
=
'hyq_trunk_large'
urdfSuffix
=
""
srdfSuffix
=
""
def
__init__
(
self
,
robotName
,
load
=
True
):
Parent
.
__init__
(
self
,
robotName
,
self
.
rootJointType
,
load
)
self
.
tf_root
=
"base_footprint"
self
.
client
.
basic
=
Client
()
self
.
load
=
load
rootJointType
=
'freeflyer'
packageName
=
'hpp-rbprm-corba'
meshPackageName
=
'hpp-rbprm-corba'
urdfName
=
'hyq_trunk'
urdfNameRom
=
[
'hyq_lhleg_rom'
,
'hyq_lfleg_rom'
,
'hyq_rfleg_rom'
,
'hyq_rhleg_rom'
]