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
e13ba644
Commit
e13ba644
authored
Nov 16, 2016
by
Steve Tonneau
Browse files
Merge commit '
3639bd65
' into optim_python_ongoing
parents
a9984e01
3639bd65
Changes
5
Hide whitespace changes
Inline
Side-by-side
script/scenarios/demos/stair_bauzil_hrp2_interp.py
View file @
e13ba644
...
...
@@ -194,7 +194,7 @@ def contactPlan():
r
.
client
.
gui
.
setVisibility
(
"hrp2_14"
,
"ON"
)
tp
.
r
.
client
.
gui
.
setVisibility
(
"toto"
,
"OFF"
)
tp
.
r
.
client
.
gui
.
setVisibility
(
"hrp2_trunk_flexible"
,
"OFF"
)
for
i
in
range
(
1
,
len
(
configs
)):
for
i
in
range
(
0
,
len
(
configs
)
-
1
):
r
(
configs
[
i
]);
time
.
sleep
(
0.5
)
...
...
script/scenarios/demos/stair_bauzil_hrp2_interp2.py
0 → 100644
View file @
e13ba644
from
hpp.corbaserver.rbprm.rbprmbuilder
import
Builder
from
hpp.corbaserver.rbprm.rbprmfullbody
import
FullBody
from
hpp.gepetto
import
Viewer
import
stair_bauzil_hrp2_path2
as
tp
import
time
packageName
=
"hrp2_14_description"
meshPackageName
=
"hrp2_14_description"
rootJointType
=
"freeflyer"
##
# Information to retrieve urdf and srdf files.
urdfName
=
"hrp2_14"
urdfSuffix
=
"_reduced"
srdfSuffix
=
""
fullBody
=
FullBody
()
fullBody
.
loadFullBodyModel
(
urdfName
,
rootJointType
,
meshPackageName
,
packageName
,
urdfSuffix
,
srdfSuffix
)
fullBody
.
setJointBounds
(
"base_joint_xyz"
,
[
-
0.135
,
2
,
-
1
,
1
,
0
,
2.2
])
ps
=
tp
.
ProblemSolver
(
fullBody
)
r
=
tp
.
Viewer
(
ps
)
#~ AFTER loading obstacles
rLegId
=
'0rLeg'
rLeg
=
'RLEG_JOINT0'
rLegOffset
=
[
0
,
-
0.105
,
0
,]
rLegNormal
=
[
0
,
1
,
0
]
rLegx
=
0.09
;
rLegy
=
0.05
fullBody
.
addLimb
(
rLegId
,
rLeg
,
''
,
rLegOffset
,
rLegNormal
,
rLegx
,
rLegy
,
10000
,
"manipulability"
,
0.1
)
lLegId
=
'1lLeg'
lLeg
=
'LLEG_JOINT0'
lLegOffset
=
[
0
,
-
0.105
,
0
]
lLegNormal
=
[
0
,
1
,
0
]
lLegx
=
0.09
;
lLegy
=
0.05
fullBody
.
addLimb
(
lLegId
,
lLeg
,
''
,
lLegOffset
,
rLegNormal
,
lLegx
,
lLegy
,
10000
,
"manipulability"
,
0.1
)
rarmId
=
'3Rarm'
rarm
=
'RARM_JOINT0'
rHand
=
'RARM_JOINT5'
rArmOffset
=
[
0
,
0
,
-
0.1
]
rArmNormal
=
[
0
,
0
,
1
]
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
]
fullBody
.
setCurrentConfig
(
q_init
)
q_init
=
[
0.1
,
-
0.82
,
0.648702
,
1.0
,
0.0
,
0.0
,
0.0
,
# Free flyer 0-6
0.0
,
0.0
,
0.0
,
0.0
,
# CHEST HEAD 7-10
0.261799388
,
0.174532925
,
0.0
,
-
0.523598776
,
0.0
,
0.0
,
0.17
,
# LARM 11-17
0.261799388
,
-
0.174532925
,
0.0
,
-
0.523598776
,
0.0
,
0.0
,
0.17
,
# RARM 18-24
0.0
,
0.0
,
-
0.453785606
,
0.872664626
,
-
0.41887902
,
0.0
,
# LLEG 25-30
0.0
,
0.0
,
-
0.453785606
,
0.872664626
,
-
0.41887902
,
0.0
,
# RLEG 31-36
];
r
(
q_init
)
q_init
[
0
:
7
]
=
tp
.
q_init
[
0
:
7
]
fullBody
.
setCurrentConfig
(
q_goal
)
#~ r(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)
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"
)
tp
.
cl
.
problem
.
selectProblem
(
"default"
)
tp
.
r
.
client
.
gui
.
setVisibility
(
"toto"
,
"OFF"
)
tp
.
r
.
client
.
gui
.
setVisibility
(
"hrp2_trunk_flexible"
,
"OFF"
)
r
(
q_init
)
def
endConfig
():
r
.
client
.
gui
.
setVisibility
(
"hrp2_14"
,
"ON"
)
tp
.
cl
.
problem
.
selectProblem
(
"default"
)
tp
.
r
.
client
.
gui
.
setVisibility
(
"toto"
,
"OFF"
)
tp
.
r
.
client
.
gui
.
setVisibility
(
"hrp2_trunk_flexible"
,
"OFF"
)
r
(
q_goal
)
def
rootPath
():
tp
.
cl
.
problem
.
selectProblem
(
"rbprm_path"
)
r
.
client
.
gui
.
setVisibility
(
"hrp2_14"
,
"OFF"
)
tp
.
r
.
client
.
gui
.
setVisibility
(
"toto"
,
"OFF"
)
r
.
client
.
gui
.
setVisibility
(
"hyq"
,
"OFF"
)
r
.
client
.
gui
.
setVisibility
(
"hrp2_trunk_flexible"
,
"ON"
)
tp
.
pp
(
0
)
r
.
client
.
gui
.
setVisibility
(
"hrp2_trunk_flexible"
,
"OFF"
)
r
.
client
.
gui
.
setVisibility
(
"hyq"
,
"ON"
)
tp
.
cl
.
problem
.
selectProblem
(
"default"
)
def
genPlan
():
r
.
client
.
gui
.
setVisibility
(
"hrp2_14"
,
"ON"
)
tp
.
cl
.
problem
.
selectProblem
(
"default"
)
tp
.
r
.
client
.
gui
.
setVisibility
(
"toto"
,
"OFF"
)
tp
.
r
.
client
.
gui
.
setVisibility
(
"hrp2_trunk_flexible"
,
"OFF"
)
global
configs
start
=
time
.
clock
()
configs
=
configs
=
fullBody
.
interpolate
(
0.1
,
True
)
end
=
time
.
clock
()
print
"Contact plan generated in "
+
str
(
end
-
start
)
+
"seconds"
def
contactPlan
():
tp
.
cl
.
problem
.
selectProblem
(
"default"
)
r
.
client
.
gui
.
setVisibility
(
"hrp2_14"
,
"ON"
)
tp
.
r
.
client
.
gui
.
setVisibility
(
"toto"
,
"OFF"
)
tp
.
r
.
client
.
gui
.
setVisibility
(
"hrp2_trunk_flexible"
,
"OFF"
)
for
i
in
range
(
0
,
len
(
configs
)
-
1
):
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/stair_bauzil_hrp2_interp3.py
0 → 100644
View file @
e13ba644
from
hpp.corbaserver.rbprm.rbprmbuilder
import
Builder
from
hpp.corbaserver.rbprm.rbprmfullbody
import
FullBody
from
hpp.gepetto
import
Viewer
import
stair_bauzil_hrp2_path3
as
tp
import
time
packageName
=
"hrp2_14_description"
meshPackageName
=
"hrp2_14_description"
rootJointType
=
"freeflyer"
##
# Information to retrieve urdf and srdf files.
urdfName
=
"hrp2_14"
urdfSuffix
=
"_reduced"
srdfSuffix
=
""
fullBody
=
FullBody
()
fullBody
.
loadFullBodyModel
(
urdfName
,
rootJointType
,
meshPackageName
,
packageName
,
urdfSuffix
,
srdfSuffix
)
fullBody
.
setJointBounds
(
"base_joint_xyz"
,
[
-
0.135
,
2
,
-
1
,
1
,
0
,
2.2
])
ps
=
tp
.
ProblemSolver
(
fullBody
)
r
=
tp
.
Viewer
(
ps
)
#~ AFTER loading obstacles
rLegId
=
'0rLeg'
rLeg
=
'RLEG_JOINT0'
rLegOffset
=
[
0
,
-
0.105
,
0
,]
rLegNormal
=
[
0
,
1
,
0
]
rLegx
=
0.09
;
rLegy
=
0.05
fullBody
.
addLimb
(
rLegId
,
rLeg
,
''
,
rLegOffset
,
rLegNormal
,
rLegx
,
rLegy
,
10000
,
"manipulability"
,
0.1
)
lLegId
=
'1lLeg'
lLeg
=
'LLEG_JOINT0'
lLegOffset
=
[
0
,
-
0.105
,
0
]
lLegNormal
=
[
0
,
1
,
0
]
lLegx
=
0.09
;
lLegy
=
0.05
fullBody
.
addLimb
(
lLegId
,
lLeg
,
''
,
lLegOffset
,
rLegNormal
,
lLegx
,
lLegy
,
10000
,
"manipulability"
,
0.1
)
rarmId
=
'3Rarm'
rarm
=
'RARM_JOINT0'
rHand
=
'RARM_JOINT5'
rArmOffset
=
[
0
,
0
,
-
0.1
]
rArmNormal
=
[
0
,
0
,
1
]
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, "manipulability", 0.05, "_6_DOF", True)
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
]
fullBody
.
setCurrentConfig
(
q_init
)
q_init
=
[
0.1
,
-
0.82
,
0.648702
,
1.0
,
0.0
,
0.0
,
0.0
,
# Free flyer 0-6
0.0
,
0.0
,
0.0
,
0.0
,
# CHEST HEAD 7-10
0.261799388
,
0.174532925
,
0.0
,
-
0.523598776
,
0.0
,
0.0
,
0.17
,
# LARM 11-17
0.261799388
,
-
0.174532925
,
0.0
,
-
0.523598776
,
0.0
,
0.0
,
0.17
,
# RARM 18-24
0.0
,
0.0
,
-
0.453785606
,
0.872664626
,
-
0.41887902
,
0.0
,
# LLEG 25-30
0.0
,
0.0
,
-
0.453785606
,
0.872664626
,
-
0.41887902
,
0.0
,
# RLEG 31-36
];
r
(
q_init
)
q_init
[
0
:
7
]
=
tp
.
q_init
[
0
:
7
]
fullBody
.
setCurrentConfig
(
q_goal
)
#~ r(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)
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"
)
tp
.
cl
.
problem
.
selectProblem
(
"default"
)
tp
.
r
.
client
.
gui
.
setVisibility
(
"toto"
,
"OFF"
)
tp
.
r
.
client
.
gui
.
setVisibility
(
"hrp2_trunk_flexible"
,
"OFF"
)
r
(
q_init
)
def
endConfig
():
r
.
client
.
gui
.
setVisibility
(
"hrp2_14"
,
"ON"
)
tp
.
cl
.
problem
.
selectProblem
(
"default"
)
tp
.
r
.
client
.
gui
.
setVisibility
(
"toto"
,
"OFF"
)
tp
.
r
.
client
.
gui
.
setVisibility
(
"hrp2_trunk_flexible"
,
"OFF"
)
r
(
q_goal
)
def
rootPath
():
tp
.
cl
.
problem
.
selectProblem
(
"rbprm_path"
)
r
.
client
.
gui
.
setVisibility
(
"hrp2_14"
,
"OFF"
)
tp
.
r
.
client
.
gui
.
setVisibility
(
"toto"
,
"OFF"
)
r
.
client
.
gui
.
setVisibility
(
"hyq"
,
"OFF"
)
r
.
client
.
gui
.
setVisibility
(
"hrp2_trunk_flexible"
,
"ON"
)
tp
.
pp
(
0
)
r
.
client
.
gui
.
setVisibility
(
"hrp2_trunk_flexible"
,
"OFF"
)
r
.
client
.
gui
.
setVisibility
(
"hyq"
,
"ON"
)
tp
.
cl
.
problem
.
selectProblem
(
"default"
)
def
genPlan
():
r
.
client
.
gui
.
setVisibility
(
"hrp2_14"
,
"ON"
)
tp
.
cl
.
problem
.
selectProblem
(
"default"
)
tp
.
r
.
client
.
gui
.
setVisibility
(
"toto"
,
"OFF"
)
tp
.
r
.
client
.
gui
.
setVisibility
(
"hrp2_trunk_flexible"
,
"OFF"
)
global
configs
start
=
time
.
clock
()
configs
=
configs
=
fullBody
.
interpolate
(
0.1
,
True
)
end
=
time
.
clock
()
print
"Contact plan generated in "
+
str
(
end
-
start
)
+
"seconds"
def
contactPlan
():
tp
.
cl
.
problem
.
selectProblem
(
"default"
)
r
.
client
.
gui
.
setVisibility
(
"hrp2_14"
,
"ON"
)
tp
.
r
.
client
.
gui
.
setVisibility
(
"toto"
,
"OFF"
)
tp
.
r
.
client
.
gui
.
setVisibility
(
"hrp2_trunk_flexible"
,
"OFF"
)
for
i
in
range
(
0
,
len
(
configs
)
-
1
):
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/stair_bauzil_hrp2_path2.py
0 → 100644
View file @
e13ba644
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
=
'hrp2_trunk_flexible'
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
=
'hrp2_trunk_flexible'
urdfNameRoms
=
[
'hrp2_larm_rom'
,
'hrp2_rarm_rom'
,
'hrp2_lleg_rom'
,
'hrp2_rleg_rom'
]
urdfSuffix
=
""
srdfSuffix
=
""
rbprmBuilder
=
Builder
()
rbprmBuilder
.
loadModel
(
urdfName
,
urdfNameRoms
,
rootJointType
,
meshPackageName
,
packageName
,
urdfSuffix
,
srdfSuffix
)
rbprmBuilder
.
setJointBounds
(
"base_joint_xyz"
,
[
0
,
2
,
-
1
,
1
,
0
,
2.2
])
#~ rbprmBuilder.setFilter(['hrp2_rarm_rom','hrp2_lleg_rom','hrp2_rleg_rom'])
rbprmBuilder
.
setAffordanceFilter
(
'3Rarm'
,
[
'Support'
])
rbprmBuilder
.
setAffordanceFilter
(
'0rLeg'
,
[
'Support'
,])
rbprmBuilder
.
setAffordanceFilter
(
'1lLeg'
,
[
'Support'
])
#~ rbprmBuilder.setNormalFilter('hrp2_rarm_rom', [0,0,1], 0.5)
#~ rbprmBuilder.setNormalFilter('hrp2_lleg_rom', [0,0,1], 0.9)
#~ rbprmBuilder.setNormalFilter('hrp2_rleg_rom', [0,0,1], 0.9)
#~ rbprmBuilder.setNormalFilter('hyq_rhleg_rom', [0,0,1], 0.9)
rbprmBuilder
.
boundSO3
([
-
0.
,
0
,
-
1
,
1
,
-
1
,
1
])
#~ from hpp.corbaserver.rbprm. import ProblemSolver
from
hpp.corbaserver.rbprm.problem_solver
import
ProblemSolver
ps
=
ProblemSolver
(
rbprmBuilder
)
r
=
Viewer
(
ps
)
q_init
=
rbprmBuilder
.
getCurrentConfig
();
q_init
[
0
:
3
]
=
[
0
,
-
0.82
,
0.648702
];
rbprmBuilder
.
setCurrentConfig
(
q_init
);
r
(
q_init
)
q_init
[
0
:
3
]
=
[
0.1
,
0
,
0.648702
];
rbprmBuilder
.
setCurrentConfig
(
q_init
);
r
(
q_init
)
#~ q_init [0:3] = [0, -0.63, 0.6]; rbprmBuilder.setCurrentConfig (q_init); r (q_init)
#~ q_init [3:7] = [ 0.98877108, 0. , 0.14943813, 0. ]
q_goal
=
q_init
[::]
q_goal
[
3
:
7
]
=
[
0.98877108
,
0.
,
0.14943813
,
0.
]
q_goal
[
0
:
3
]
=
[
1.49
,
-
0.65
,
1.25
];
r
(
q_goal
)
#~ q_goal [0:3] = [1.2, -0.65, 1.1]; r (q_goal)
#~ ps.addPathOptimizer("GradientBased")
ps
.
addPathOptimizer
(
"RandomShortcut"
)
ps
.
setInitialConfig
(
q_init
)
ps
.
addGoalConfig
(
q_goal
)
from
hpp.corbaserver.affordance.affordance
import
AffordanceTool
afftool
=
AffordanceTool
()
afftool
.
setAffordanceConfig
(
'Support'
,
[
0.5
,
0.03
,
0.00005
])
afftool
.
loadObstacleModel
(
packageName
,
"stair_bauzil"
,
"planning"
,
r
)
#~ afftool.analyseAll()
#~ afftool.visualiseAffordances('Support', r, [0.25, 0.5, 0.5])
#~ afftool.visualiseAffordances('Lean', r, [0, 0, 0.9])
ps
.
client
.
problem
.
selectConFigurationShooter
(
"RbprmShooter"
)
ps
.
client
.
problem
.
selectPathValidation
(
"RbprmPathValidation"
,
0.05
)
#~ ps.solve ()
t
=
ps
.
solve
()
print
t
;
if
isinstance
(
t
,
list
):
t
=
t
[
0
]
*
3600000
+
t
[
1
]
*
60000
+
t
[
2
]
*
1000
+
t
[
3
]
f
=
open
(
'log.txt'
,
'a'
)
f
.
write
(
"path computation "
+
str
(
t
)
+
"
\n
"
)
f
.
close
()
from
hpp.gepetto
import
PathPlayer
pp
=
PathPlayer
(
rbprmBuilder
.
client
.
basic
,
r
)
#~ pp.fromFile("/home/stonneau/dev/hpp/src/hpp-rbprm-corba/script/paths/stair.path")
#~
#~ pp (2)
#~ pp (0)
#~ pp (1)
#~ pp.toFile(1, "/home/stonneau/dev/hpp/src/hpp-rbprm-corba/script/paths/stair.path")
#~ rbprmBuilder.exportPath (r, ps.client.problem, 1, 0.01, "stair_bauzil_hrp2_path.txt")
cl
=
Client
()
cl
.
problem
.
selectProblem
(
"rbprm_path"
)
rbprmBuilder2
=
Robot
(
"toto"
)
ps2
=
ProblemSolver
(
rbprmBuilder2
)
cl
.
problem
.
selectProblem
(
"default"
)
cl
.
problem
.
movePathToProblem
(
1
,
"rbprm_path"
,
rbprmBuilder
.
getAllJointNames
())
r2
=
Viewer
(
ps2
)
r
.
client
.
gui
.
setVisibility
(
"toto"
,
"OFF"
)
r
.
client
.
gui
.
setVisibility
(
"hrp2_trunk_flexible"
,
"OFF"
)
#~ r2(q_far)
script/scenarios/demos/stair_bauzil_hrp2_path3.py
0 → 100644
View file @
e13ba644
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
):