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
99abb87b
Commit
99abb87b
authored
Feb 02, 2017
by
Steve Tonneau
Browse files
only kept demos for master branch
parent
6a1df7bf
Changes
29
Expand all
Hide whitespace changes
Inline
Side-by-side
script/scenarios/car_hrp2_interp.py
deleted
100755 → 0
View file @
6a1df7bf
from
hpp.corbaserver.rbprm.rbprmbuilder
import
Builder
from
hpp.corbaserver.rbprm.rbprmfullbody
import
FullBody
from
hpp.gepetto
import
Viewer
import
car_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"
,
[
-
1
,
2
,
-
2
,
1
,
0.5
,
2.5
])
ps
=
tp
.
ProblemSolver
(
fullBody
)
r
=
tp
.
Viewer
(
ps
)
#~ AFTER loading obstacles
#~ 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.03
)
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.03
)
rarmId
=
'3Rarm'
rarm
=
'RARM_JOINT0'
rHand
=
'RARM_JOINT5'
rArmOffset
=
[
-
0.05
,
-
0.050
,
-
0.050
]
rArmNormal
=
[
1
,
0
,
0
]
rArmx
=
0.024
;
rArmy
=
0.024
fullBody
.
addLimb
(
rarmId
,
rarm
,
rHand
,
rArmOffset
,
rArmNormal
,
rArmx
,
rArmy
,
20000
,
"EFORT"
,
0.05
)
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
,
20000
,
"EFORT"
,
0.05
)
#~
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
])
fullBody
.
setEndState
(
q_goal
,[
rLegId
,
lLegId
])
#~
#~ r(q_init)
configs
=
fullBody
.
interpolate
(
0.01
)
#~ r.loadObstacleModel ('hpp-rbprm-corba', "car", "contact")
#~ configs = fullBody.interpolate(0.09)
#~ configs = fullBody.interpolate(0.08)
i
=
0
;
#~ r (configs[i]); i=i+1; i-1
#~ q_init = fullBody.generateContacts(q_init, [0,0,-1]); r (q_init)
#~ fullBody.draw(q_0,r)
#~ fullBody.client.rbprm.rbprm.getOctreeTransform(larmId, q_0)
#~ problem = ps.client.problem
#~ length = problem.pathLength (0)
#~ t = 0
#~ i = 0
#~ configs = []
#~ dt = 0.1 / length
#~ while t < length :
#~ q = fullBody.getCurrentConfig()
#~ q[0:confsize] = problem.configAtParam (0, t)[0:confsize]
#~ configs.append(q)
#~ t += dt
#~ i = i+1
#~
#~ i = 0;
#~ fullBody.draw(configs[i],r); i=i+1; i-1
#~
#~ f1 = open("hrp2_standing_29_10_15","w+")
#~ f1.write(str(configs))
#~ f1.close()
#~ import hpp.gepetto.blender.exportmotion as em
#~ fullBody.exportAll(r, configs, 'car_hrp2_robust_2');
script/scenarios/car_hrp2_path.py
deleted
100755 → 0
View file @
6a1df7bf
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"
,
[
-
1
,
2
,
-
1.5
,
1
,
0.5
,
0.9
])
rbprmBuilder
.
setFilter
([
'hrp2_lleg_rom'
,
'hrp2_rleg_rom'
,
'hrp2_larm_rom'
])
#~ rbprmBuilder.setNormalFilter('hrp2_larm_rom', [0,0,1], 0.4)
#~ rbprmBuilder.setNormalFilter('hrp2_rarm_rom', [0,0,1], 0.4)
rbprmBuilder
.
setAffordanceFilter
(
'3Rarm'
,
[
'Support'
,
'Lean'
])
rbprmBuilder
.
setAffordanceFilter
(
'4Larm'
,
[
'Support'
,
'Lean'
])
rbprmBuilder
.
setAffordanceFilter
(
'0rLeg'
,
[
'Support'
])
rbprmBuilder
.
setAffordanceFilter
(
'1lLeg'
,
[
'Support'
])
rbprmBuilder
.
boundSO3
([
-
1.5
,
1.5
,
0
,
3
,
-
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.15
,
-
0.45
,
0.8
];
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.2
,
-
1.1
,
0.58
];
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.5
,
0.03
,
0.00005
])
afftool
.
loadObstacleModel
(
packageName
,
"car"
,
"planning"
,
r
)
#~ afftool.analyseAll()
afftool
.
visualiseAffordances
(
'Support'
,
r
,
[
0.25
,
0.5
,
0.5
])
afftool
.
visualiseAffordances
(
'Lean'
,
r
,
[
0
,
0
,
0.9
])
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)
rob
=
rbprmBuilder
.
client
.
basic
.
robot
r
(
q_init
)
q_loin
=
q_init
[::]
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
script/scenarios/darpa_hrp2_interp.py
deleted
100755 → 0
View file @
6a1df7bf
from
hpp.corbaserver.rbprm.rbprmbuilder
import
Builder
from
hpp.corbaserver.rbprm.rbprmfullbody
import
FullBody
from
hpp.gepetto
import
Viewer
import
darpa_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
,
5
,
-
1
,
1
,
0.3
,
4
])
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
,
"forward"
,
0.03
)
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
,
"forward"
,
0.03
)
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
])
fullBody
.
setEndState
(
q_goal
,[
rLegId
,
lLegId
])
#~
#~ r(q_init)
configs
=
fullBody
.
interpolate
(
0.1
)
r
.
loadObstacleModel
(
'hpp-rbprm-corba'
,
"darpa"
,
"contact"
)
#~ configs = fullBody.interpolate(0.09)
#~ configs = fullBody.interpolate(0.08)
i
=
0
;
#~ r (configs[i]); i=i+1; i-1
#~ q_init = fullBody.generateContacts(q_init, [0,0,-1]); r (q_init)
#~ fullBody.draw(q_0,r)
#~ fullBody.client.rbprm.rbprm.getOctreeTransform(larmId, q_0)
#~ problem = ps.client.problem
#~ length = problem.pathLength (0)
#~ t = 0
#~ i = 0
#~ configs = []
#~ dt = 0.1 / length
#~ while t < length :
#~ q = fullBody.getCurrentConfig()
#~ q[0:confsize] = problem.configAtParam (0, t)[0:confsize]
#~ configs.append(q)
#~ t += dt
#~ i = i+1
#~
i
=
0
;
fullBody
.
draw
(
configs
[
i
],
r
);
i
=
i
+
1
;
i
-
1
#~
#~ f1 = open("hrp2_standing_29_10_15","w+")
#~ f1.write(str(configs))
#~ f1.close()
script/scenarios/darpa_hrp2_path.py
deleted
100755 → 0
View file @
6a1df7bf
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"
,
[
-
2
,
5
,
-
1
,
1
,
0.55
,
4
])
rbprmBuilder
.
setFilter
([
'hrp2_lleg_rom'
,
'hrp2_rleg_rom'
])
rbprmBuilder
.
setNormalFilter
(
'hrp2_larm_rom'
,
[
0
,
0
,
1
],
0.4
)
rbprmBuilder
.
setNormalFilter
(
'hrp2_rarm_rom'
,
[
0
,
0
,
1
],
0.4
)
rbprmBuilder
.
setNormalFilter
(
'hrp2_lleg_rom'
,
[
0
,
0
,
1
],
0.4
)
rbprmBuilder
.
setNormalFilter
(
'hrp2_rleg_rom'
,
[
0
,
0
,
1
],
0.4
)
rbprmBuilder
.
boundSO3
([
-
0.1
,
0.1
,
-
0.1
,
0.1
,
-
1.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
=
rbprmBuilder
.
getCurrentConfig
();
q_init
[
0
:
3
]
=
[
-
2
,
0
,
0.55
];
rbprmBuilder
.
setCurrentConfig
(
q_init
);
r
(
q_init
)
q_goal
=
q_init
[::]
q_goal
[
0
:
3
]
=
[
3
,
0
,
0.55
];
r
(
q_goal
)
ps
.
addPathOptimizer
(
"RandomShortcut"
)
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"
)
t
=
ps
.
solve
()
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")
r
(
q_init
)
script/scenarios/darpa_hyq_interp.py
deleted
100755 → 0
View file @
6a1df7bf
#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
])
# 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.12, 10, 5, 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.04
,
1
,
10
,
True
)
#~ r.loadObstacleModel ('hpp-rbprm-corba', "darpa", "contact")
# calling draw with increasing i will display the sequence
i
=
0
;
fullBody
.
draw
(
configs
[
i
],
r
);
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
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
)
def
saveAll
(
name
):
saveAllData
(
fullBody
,
r
,
name
)
#~ fullBody.exportAll(r, trajec, 'hole_hyq_t_var_04f_andrea');
#~ fullBody.exportAll(r, configs, 'hole_hyq_t_var_04f_andrea_contact_planning');
#~ saveToPinocchio('obstacle_hyq_t_var_04f_andrea')
#~ fullBody.exportAll(r, trajec, 'darpa_hyq_t_var_04f_andrea');
#~ saveToPinocchio('darpa_hyq_t_var_04f_andrea')
gui
=
r
.
client
.
gui
scene
=
"oddct"
r
.
client
.
gui
.
createScene
(
scene
)
resolution
=
0.03
i
=
0
boxname
=
scene
+
"/b"
+
str
(
i
)
gui
.
addBox
(
boxname
,
resolution
,
resolution
,
resolution
,
[
1
,
1
,
1
,
1
])
gui
.
applyConfiguration
(
boxname
,[
0
,
0
,
0
,
1
,
0
,
0
,
0
])
gui
.
addSceneToWindow
(
scene
,
0
)
gui
.
refresh
()
import
hpp.corbaserver.rbprm.tools.cwc_trajectory_helper
as
cwc_trajectory_helper
import
time
def
applycom
():
global
gui
global
com
c
=
fullBody
.
getCenterOfMass
()
gui
.
applyConfiguration
(
boxname
,[
c
[
0
],
c
[
1
],
0
,
1
,
0
,
0
,
0
])
gui
.
refresh
()
def
go
(
dt_framerate
=
1.
/
24.
):
path_player
=
pp
configs
=
cwc_trajectory_helper
.
trajec
for
q
in
configs
:
start
=
time
.
time
()
pp
.
publisher
.
robotConfig
=
q
pp
.
publisher
.
publishRobots
()
elapsed
=
time
.
time
()
-
start
applycom
()
if
elapsed
<
dt_framerate
:
time
.
sleep
(
dt_framerate
-
elapsed
)
#~ for i in range(9,14):
#~ act(i,verbose=True, use_window=0, numOptim=5, optim_effectors=False, draw=False);go()
#~ saveAll("test"+str(i));
#~ for i in range(14,16):
#~ act(i,verbose=True, use_window=1, numOptim=5, optim_effectors=False, draw=False);go()
#~ saveAll("test"+str(i));
#~ for i in range(16,17):
#~ act(i,verbose=True, use_window=2, numOptim=5, optim_effectors=False, draw=False);go()
#~ saveAll("test"+str(i));
#~ for i in range(17,19):
#~ act(i,verbose=True, use_window=1, numOptim=5, optim_effectors=False, draw=False);go()
#~ saveAll("test"+str(i));
#~ for i in range(19,22):
#~ act(i,verbose=True, use_window=1, numOptim=5, optim_effectors=False, draw=False);go()
#~ saveAll("test"+str(i));
#~ for i in range(23,24):
#~ act(i,verbose=True, use_window=2, numOptim=5, optim_effectors=False, draw=False);go()
#~ saveAll("test"+str(i));
#~ for i in range(24,26):
#~ act(i,verbose=True, use_window=1, numOptim=5, optim_effectors=False, draw=False);go()
#~ saveAll("test"+str(i));
for
i
in
range
(
14
,
19
):
act
(
i
,
verbose
=
True
,
use_window
=
0
,
numOptim
=
5
,
optim_effectors
=
True
,
draw
=
False
);
go
()
saveAll
(
"test"
+
str
(
i
));
for
i
in
range
(
19
,
22
):
act
(
i
,
verbose
=
True
,
use_window
=
1
,
numOptim
=
5
,
optim_effectors
=
True
,
draw
=
False
);
go
()
saveAll
(
"test"
+
str
(
i
));
for
i
in
range
(
22
,
28
):
act
(
i
,
verbose
=
True
,
use_window
=
0
,
numOptim
=
5
,
optim_effectors
=
True
,
draw
=
False
);
go
()
saveAll
(
"test"
+
str
(
i
));
for
i
in
range
(
28
,
30
):
act
(
i
,
verbose
=
True
,
use_window
=
0
,
numOptim
=
5
,
optim_effectors
=
True
,
draw
=
False
);
go
()
saveAll
(
"test"
+
str
(
i
));
for
i
in
range
(
30
,
42
):
act
(
i
,
verbose
=
True
,
use_window
=
1
,
numOptim
=
5
,
optim_effectors
=
True
,
draw
=
False
);
go
()
saveAll
(
"test"
+
str
(
i
));
for
i
in
range
(
32
,
83
):
act
(
i
,
verbose
=
True
,
use_window
=
0
,
numOptim
=
5
,
optim_effectors
=
True
,
draw
=
False
);
go
()
saveAll
(
"test"
+
str
(
i
));