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
916029ca
Commit
916029ca
authored
Jan 12, 2017
by
Steve Tonneau
Browse files
updated profile scenarios
parent
7cb806bd
Changes
8
Hide whitespace changes
Inline
Side-by-side
profile/scenarios/darpa_hyq_interp.py
0 → 100644
View file @
916029ca
#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
from
os
import
environ
ins_dir
=
environ
[
'DEVEL_DIR'
]
db_dir
=
ins_dir
+
"/install/share/hyq-rbprm/database/hyq_"
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
)
fullBody
.
setJointBounds
(
"base_joint_xyz"
,
[
-
2
,
5
,
-
1
,
1
,
0.3
,
4
])
# 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).
def
addLimbDb
(
limbId
,
heuristicName
,
loadValues
=
True
,
disableEffectorCollision
=
False
):
fullBody
.
addLimbDatabase
(
str
(
db_dir
+
limbId
+
'.db'
),
limbId
,
heuristicName
,
loadValues
,
disableEffectorCollision
)
lLegId
=
'lhleg'
rarmId
=
'rhleg'
larmId
=
'lfleg'
#~ addLimbDb(rLegId, "static")
#~ addLimbDb(lLegId, "static")
#~ addLimbDb(rarmId, "static")
#~ addLimbDb(larmId, "static")
fullBody
.
addLimb
(
rLegId
,
rLeg
,
rfoot
,
offset
,
normal
,
legx
,
legy
,
nbSamples
,
"jointlimits"
,
0.1
,
cType
)
lLegId
=
'lhleg'
lLeg
=
'lh_haa_joint'
lfoot
=
'lh_foot_joint'
fullBody
.
addLimb
(
lLegId
,
lLeg
,
lfoot
,
offset
,
normal
,
legx
,
legy
,
nbSamples
,
"jointlimits"
,
0.05
,
cType
)
#~
rarmId
=
'rhleg'
rarm
=
'rh_haa_joint'
rHand
=
'rh_foot_joint'
fullBody
.
addLimb
(
rarmId
,
rarm
,
rHand
,
offset
,
normal
,
legx
,
legy
,
nbSamples
,
"jointlimits"
,
0.05
,
cType
)
larmId
=
'lfleg'
larm
=
'lf_haa_joint'
lHand
=
'lf_foot_joint'
fullBody
.
addLimb
(
larmId
,
larm
,
lHand
,
offset
,
normal
,
legx
,
legy
,
nbSamples
,
"jointlimits"
,
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
])
#~ from pickle import load
#~ f = open("config_"+str(tp.config_i), 'r')
#~ q_init = load(f)
#~ f.close()
# 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.setStartState(q_init,[rLegId,lLegId,rarmId,larmId])
fullBody
.
setEndState
(
q_goal
,[
rLegId
,
lLegId
,
rarmId
,
larmId
])
r
(
q_init
)
# computing the contact sequence
#~ configs = fullBody.interpolate(0.05, 10, 1, True)
configs
=
fullBody
.
interpolate
(
0.08
,
10
,
10
,
True
)
#~ configs = fullBody.interpolate(0.11, 7, 10, True)
#~ configs = fullBody.interpolate(0.1, 1, 10, True)
#~ configs = fullBody.interpolate(0.02, 10, 10, True)
import
time
try
:
time
.
sleep
(
2
)
fullBody
.
dumpProfile
()
except
Exception
as
e
:
pass
profile/scenarios/darpa_hyq_path.py
View file @
916029ca
...
...
@@ -35,9 +35,21 @@ r = Viewer (ps)
# Setting initial and goal configurations
q_init
=
rbprmBuilder
.
getCurrentConfig
();
q_init
[
0
:
3
]
=
[
-
2
,
0
,
0.63
];
rbprmBuilder
.
setCurrentConfig
(
q_init
);
r
(
q_init
)
#~ q_init [0:3] = [-2, 0, 0.63]; rbprmBuilder.setCurrentConfig (q_init); r (q_init)
q_init
[
0
:
3
]
=
[
-
1.8
,
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
)
config_i
=
0
#~ from pickle import load
#~ f = open("config_"+str(config_i), 'r')
#~ q_init [0:7] = load(f)[0:7]
#~ f.close()
#~ q_goal [0:3] = [-1.5, 0, 0.63]; r (q_goal)
# Choosing a path optimizer
...
...
@@ -58,14 +70,12 @@ ps.client.problem.selectPathValidation("RbprmPathValidation",0.05)
# Solve the problem
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
()
# Playing the computed path
from
hpp.gepetto
import
PathPlayer
pp
=
PathPlayer
(
rbprmBuilder
.
client
.
basic
,
r
)
...
...
@@ -74,3 +84,6 @@ pp = PathPlayer (rbprmBuilder.client.basic, r)
q_far
=
q_init
[::]
q_far
[
0
:
3
]
=
[
-
2
,
-
3
,
0.63
];
r
(
q_far
)
for
i
in
range
(
1
,
10
):
rbprmBuilder
.
client
.
basic
.
problem
.
optimizePath
(
i
)
profile/scenarios/ground_crouch_hyq_interp.py
View file @
916029ca
...
...
@@ -58,15 +58,46 @@ lLegId = 'lhleg'
rarmId
=
'rhleg'
larmId
=
'lfleg'
addLimbDb
(
rLegId
,
"static"
)
addLimbDb
(
lLegId
,
"static"
)
addLimbDb
(
rarmId
,
"static"
)
addLimbDb
(
larmId
,
"static"
)
#~
addLimbDb(rLegId, "static")
#~
addLimbDb(lLegId, "static")
#~
addLimbDb(rarmId, "static")
#~
addLimbDb(larmId, "static")
lfoot
=
'lh_foot_joint'
rHand
=
'rh_foot_joint'
lHand
=
'lf_foot_joint'
#~ lLegId = 'lhleg'
#~ rarmId = 'rhleg'
#~ larmId = 'lfleg'
#~
#~ addLimbDb(rLegId, "static")
#~ addLimbDb(lLegId, "static")
#~ addLimbDb(rarmId, "static")
#~ addLimbDb(larmId, "static")
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
]
...
...
@@ -84,49 +115,10 @@ fullBody.setEndState(q_goal,[rLegId,lLegId,rarmId,larmId])
r
(
q_init
)
configs
=
fullBody
.
interpolate
(
0.08
,
1
,
3
)
#hole
#~ configs = fullBody.interpolate(0.08,1,5) # bridge
#~ r.loadObstacleModel ('hpp-rbprm-corba', "groundcrouch", "contact")
#~ fullBody.exportAll(r, configs, 'obstacle_hyq_robust_10');
i
=
0
;
r
(
configs
[
i
]);
i
=
i
+
1
;
i
-
1
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
,
profile
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
):
return
step
(
fullBody
,
configs
,
i
,
numOptim
,
pp
,
limbsCOMConstraints
,
0.5
,
optim_effectors
=
False
,
time_scale
=
20.
,
useCOMConstraints
=
False
)
def
play
(
frame_rate
=
1.
/
24.
):
play_traj
(
fullBody
,
pp
,
frame_rate
)
def
saveAll
(
name
):
saveAllData
(
fullBody
,
r
,
name
)
#~ fullBody.exportAll(r, trajec, 'obstacle_hyq_t_var_04f_andrea');
#~ saveToPinocchio('obstacle_hyq_t_var_04f_andrea')
#~ profile(fullBody, configs, 4, 10, limbsCOMConstraints)
profile
(
fullBody
,
configs
,
4
,
len
(
configs
)
-
4
,
limbsCOMConstraints
)
import
time
try
:
time
.
sleep
(
2
)
fullBody
.
dumpProfile
()
except
Exception
as
e
:
pass
profile/scenarios/polaris_hrp2_interp.py
0 → 100644
View file @
916029ca
from
hpp.corbaserver.rbprm.rbprmbuilder
import
Builder
from
hpp.corbaserver.rbprm.rbprmfullbody
import
FullBody
from
hpp.gepetto
import
Viewer
import
polaris_hrp2_path_no_step
as
tp
from
numpy
import
array
,
matrix
import
numpy
as
np
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"
,
[
-
1
,
2
,
-
2
,
1
,
0.5
,
2.5
])
ps
=
tp
.
ProblemSolver
(
fullBody
)
r
=
tp
.
Viewer
(
ps
)
#~ AFTER loading obstacles
#~ AFTER loading obstacles
rLegId
=
'hrp2_rleg_rom'
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.03
,
"_6_DOF"
)
lLegId
=
'hrp2_lleg_rom'
lLeg
=
'LLEG_JOINT0'
lLegOffset
=
[
0
,
-
0.115
,
0
]
lLegNormal
=
[
0
,
1
,
0
]
lLegx
=
0.09
;
lLegy
=
0.05
fullBody
.
addLimb
(
lLegId
,
lLeg
,
''
,
lLegOffset
,
rLegNormal
,
lLegx
,
lLegy
,
10000
,
"manipulability"
,
0.03
,
"_6_DOF"
)
rarmId
=
'hrp2_rarm_rom'
rarm
=
'RARM_JOINT0'
rHand
=
'RARM_JOINT5'
rArmOffset
=
[
-
0.040
,
-
0.01
,
-
0.085
]
rArmNormal
=
[
1
,
0
,
0
]
rArmx
=
0.024
;
rArmy
=
0.024
fullBody
.
addLimb
(
rarmId
,
rarm
,
rHand
,
rArmOffset
,
rArmNormal
,
rArmx
,
rArmy
,
20000
,
"EFORT"
,
0.05
,
"_6_DOF"
,
False
)
larmId
=
'hrp2_larm_rom'
larm
=
'LARM_JOINT0'
lHand
=
'LARM_JOINT5'
lArmOffset
=
[
-
0.040
,
0.01
,
-
0.085
]
lArmNormal
=
[
1
,
0
,
0
]
lArmx
=
0.024
;
lArmy
=
0.024
fullBody
.
addLimb
(
larmId
,
larm
,
lHand
,
lArmOffset
,
lArmNormal
,
lArmx
,
lArmy
,
20000
,
"EFORT"
,
0.05
,
"_6_DOF"
,
False
)
#~
q_0
=
fullBody
.
getCurrentConfig
();
#~ fullBody.createOctreeBoxes(r.client.gui, 1, larmId, q_0,)
#~ fullBody.client.basic.robot.setJointConfig('LARM_JOINT0',[1])
#~ fullBody.client.basic.robot.setJointConfig('RARM_JOINT0',[-1])
confsize
=
len
(
tp
.
q_init
)
q_init
=
[
#~ 0.12, -0.45, 0.95, 1.0, 0.0 , 0.0, 0.0, # Free flyer 0-6
0.36
,
0
,
1.01
,
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
,
-
1
,
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
:
confsize
]
=
tp
.
q_init
[
0
:
confsize
]
q_goal
=
fullBody
.
getCurrentConfig
();
q_goal
[
0
:
confsize
]
=
tp
.
q_goal
[
0
:
confsize
]
fullBody
.
setCurrentConfig
(
q_init
)
#~ q_0 = fullBody.getCurrentConfig();
q_init
=
fullBody
.
generateContacts
(
q_init
,
[
0
,
0
,
-
1
]);
r
(
q_init
)
fullBody
.
setCurrentConfig
(
q_goal
)
#~ r(q_goal)
q_goal
=
fullBody
.
generateContacts
(
q_goal
,
[
0
,
0
,
1
])
#~ r(q_goal)
#~ gui = r.client.gui
#~ fullBody.setStartState(q_init,[rLegId,lLegId,rarmId,larmId])
fullBody
.
setStartState
(
q_init
,[
rLegId
,
lLegId
,
larmId
])
fullBody
.
setEndState
(
q_goal
,[
rLegId
,
lLegId
])
#~
#~ r(q_init)
configs
=
fullBody
.
interpolate
(
0.03
,
10
,
5
,
True
)
import
time
try
:
time
.
sleep
(
2
)
fullBody
.
dumpProfile
()
except
Exception
as
e
:
pass
profile/scenarios/polaris_hrp2_path_no_step.py
0 → 100644
View file @
916029ca
from
hpp.corbaserver.rbprm.rbprmbuilder
import
Builder
from
hpp.gepetto
import
Viewer
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.4
,
1.01
,
0
,
1.01
])
rbprmBuilder
.
setFilter
([
'hrp2_larm_rom'
,
'hrp2_lleg_rom'
,
'hrp2_rleg_rom'
])
#~ rbprmBuilder.setFilter(['hrp2_lleg_rom','hrp2_rleg_rom'])
#~ rbprmBuilder.setAffordanceFilter('hrp2_rarm_rom', ['Support'])
rbprmBuilder
.
setAffordanceFilter
(
'hrp2_rarm_rom'
,
[
'Support'
,
'Lean'
])
rbprmBuilder
.
setAffordanceFilter
(
'hrp2_larm_rom'
,
[
'Support'
,
'Lean'
])
rbprmBuilder
.
setAffordanceFilter
(
'hrp2_rleg_rom'
,
[
'Support'
])
rbprmBuilder
.
setAffordanceFilter
(
'hrp2_lleg_rom'
,
[
'Support'
])
rbprmBuilder
.
boundSO3
([
-
0.7
,
0.7
,
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
)
q0
=
rbprmBuilder
.
getCurrentConfig
();
q_init
=
rbprmBuilder
.
getCurrentConfig
();
r
(
q_init
)
q_goal
=
q_init
[::]
q_init
=
rbprmBuilder
.
getCurrentConfig
();
#~ q_init[0:3] = [0.36, -0.32, 1.01]; r(q_init)
q_init
[
0
:
3
]
=
[
0.36
,
0
,
1.01
];
r
(
q_init
)
rbprmBuilder
.
setCurrentConfig
(
q_init
);
r
(
q_init
)
#~ q_goal[0:3] = [1.2,-1,0.5]; r(q_goal)
#~ q_goal[0:3] = [0.9, -1.1, 0.6]; r(q_goal)
#~ q_goal[0:3] = [1.5, -1.1, 0.5]; r(q_goal)
q_goal
[
0
:
3
]
=
[
0.36
,
-
1
,
0.9
];
r
(
q_goal
)
ps
.
addPathOptimizer
(
"RandomShortcut"
)
ps
.
setInitialConfig
(
q_init
)
ps
.
addGoalConfig
(
q_goal
)
from
hpp.corbaserver.affordance.affordance
import
AffordanceTool
afftool
=
AffordanceTool
()
afftool
.
setAffordanceConfig
(
'Support'
,
[
0.2
,
0.03
,
0.00005
])
afftool
.
setAffordanceConfig
(
'Lean'
,
[
0.5
,
0.05
,
0.00005
])
afftool
.
loadObstacleModel
(
packageName
,
"polaris"
,
"planning"
,
r
)
#~ afftool.analyseAll()
#~ afftool.visualiseAffordances('Support', r, [0.25, 0.5, 0.5])
#~ afftool.visualiseAffordances('Lean', r, [1, 0, 0])
ps
.
client
.
problem
.
selectConFigurationShooter
(
"RbprmShooter"
)
ps
.
client
.
problem
.
selectPathValidation
(
"RbprmPathValidation"
,
0.01
)
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
()
#~ print ("solving time " + str(t));
from
hpp.gepetto
import
PathPlayer
pp
=
PathPlayer
(
rbprmBuilder
.
client
.
basic
,
r
)
#~ pp (0)
#~ pp (1)
for
i
in
range
(
1
,
10
):
rbprmBuilder
.
client
.
basic
.
problem
.
optimizePath
(
i
)
rob
=
rbprmBuilder
.
client
.
basic
.
robot
r
(
q_init
)
q_loin
=
q_init
[::]
r
.
client
.
gui
.
setVisibility
(
"hrp2_trunk_flexible"
,
"OFF"
)
#~ q_loin[0:3] = [100,100,100]
#~ r (q_loin)
#~ configs = []
#~ problem = ps.client.problem
#~ length = problem.pathLength (0)
#~ t = 0
#~ i = 0
#~ configs = []
#~ dt = 0.1 / length
#~ while t < length :
#~ q = rbprmBuilder.getCurrentConfig()
#~ q[0:9]= problem.configAtParam (0, t)[0:9]
#~ configs.append(q)
#~ t += dt
#~ i = i+1
#~
#~ i = 0;
#~ r(configs[i]); i = +1
profile/scenarios/stair_bauzil_hrp2_interp.py
View file @
916029ca
...
...
@@ -131,6 +131,12 @@ def play(frame_rate = 1./24.):
def
saveAll
(
name
):
saveAllData
(
fullBody
,
r
,
name
)
import
time
try
:
time
.
sleep
(
2
)
fullBody
.
dumpProfile
()
except
Exception
as
e
:
pass
#~ for i in range(4,25):
#~ act(i,0)
profile/scenarios/standing_hrp2_interp.py
0 → 100755
View file @
916029ca
from
hpp.corbaserver.rbprm.rbprmbuilder
import
Builder
from
hpp.corbaserver.rbprm.rbprmfullbody
import
FullBody
from
hpp.gepetto
import
Viewer
import
standing_hrp2_path
as
tp
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"
,
[
-
2
,
1
,
-
1
,
1
,
0
,
2.2
])
ps
=
tp
.
ProblemSolver
(
fullBody
)
r
=
tp
.
Viewer
(
ps
)
rLegId
=
'hrp2_rleg_rom'
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.03
,
"_6_DOF"
)
lLegId
=
'hrp2_lleg_rom'
lLeg
=
'LLEG_JOINT0'
lLegOffset
=
[
0
,
-
0.115
,
0
]
lLegNormal
=
[
0
,
1
,
0
]
lLegx
=
0.09
;
lLegy
=
0.05
fullBody
.
addLimb
(
lLegId
,
lLeg
,
''
,
lLegOffset
,
rLegNormal
,
lLegx
,
lLegy
,
10000
,
"manipulability"
,
0.03
,
"_6_DOF"
)
rarmId
=
'hrp2_rarm_rom'
rarm
=
'RARM_JOINT0'
rHand
=
'RARM_JOINT5'
rArmOffset
=
[
-
0.040
,
-
0.01
,
-
0.085
]
rArmNormal
=
[
1
,
0
,
0
]
rArmx
=
0.024
;
rArmy
=
0.024
fullBody
.
addLimb
(
rarmId
,
rarm
,
rHand
,
rArmOffset
,
rArmNormal
,
rArmx
,
rArmy
,
20000
,
"EFORT"
,
0.05
,
"_6_DOF"
,
False
)
larmId
=
'hrp2_larm_rom'
larm
=
'LARM_JOINT0'
lHand
=
'LARM_JOINT5'
lArmOffset
=
[
-
0.040
,
0.01
,
-
0.085
]
lArmNormal
=
[
1
,
0
,
0
]
lArmx
=
0.024
;
lArmy
=
0.024
fullBody
.
addLimb
(
larmId
,
larm
,
lHand
,
lArmOffset
,
lArmNormal
,
lArmx
,
lArmy
,
20000
,
"EFORT"
,
0.05
,
"_6_DOF"
,
False
)
#~
q_0
=
fullBody
.
getCurrentConfig
();
#~ fullBody.createOctreeBoxes(r.client.gui, 1, larmId, q_0,)
fullBody
.
client
.
basic
.
robot
.
setJointConfig
(
'LARM_JOINT0'
,[
1
])
fullBody
.
client
.
basic
.
robot
.
setJointConfig
(
'RARM_JOINT0'
,[
-
1
])
confsize
=
len
(
tp
.
q_init
)
q_init
=
fullBody
.
getCurrentConfig
();
q_init
[
0
:
confsize
]
=
tp
.
q_init
[
0
:
confsize
]
q_goal
=
fullBody
.
getCurrentConfig
();
q_goal
[
0
:
confsize
]
=
tp
.
q_goal
[
0
:
confsize
]
fullBody
.
setCurrentConfig
(
q_init
)
#~ q_0 = fullBody.getCurrentConfig();
q_init
=
fullBody
.
generateContacts
(
q_init
,
[
0
,
0
,
-
1
]);
r
(
q_init
)
fullBody
.
setCurrentConfig
(
q_goal
)
#~ r(q_goal)
q_goal
=
fullBody
.
generateContacts
(
q_goal
,
[
0
,
0
,
1
])
#~ r(q_goal)
#~ gui = r.client.gui
fullBody
.
setStartState
(
q_init
,[
rLegId
,
lLegId
,
rarmId
,
larmId
])
fullBody
.
setEndState
(
q_goal
,[
rLegId
,
lLegId
,
rarmId
,
larmId
])
#~
#~ r(q_init)
configs
=
fullBody
.
interpolate
(
0.1
)
import
time
try
:
time
.
sleep
(
2
)
fullBody
.
dumpProfile
()
except
Exception
as
e
:
pass
profile/scenarios/standing_hrp2_path.py
0 → 100755
View file @
916029ca
from
hpp.corbaserver.rbprm.rbprmbuilder
import
Builder