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
723c10c2
Commit
723c10c2
authored
Aug 19, 2019
by
stevet
Browse files
wrap initialization for anymal
parent
07d1f3c6
Changes
6
Hide whitespace changes
Inline
Side-by-side
script/scenarios/sandbox/anymal_one_step/anymal_circle_one_step.py
View file @
723c10c2
...
...
@@ -188,35 +188,51 @@ if (not cg_success) or cg_too_many_states or (not cg_reach_goal):
import
hpp.corbaserver.rbprm.fewstepsplanner
as
sp
import
time
def
dispContactPlan
(
states
,
step
=
0.5
):
for
s
in
states
:
v
(
s
.
q
());
time
.
sleep
(
step
)
states
=
sp
.
interpolateState
(
fullBody
,
0.002
,
pathId
=
pId
,
robustnessTreshold
=
1
,
filterStates
=
True
,
quasiStatic
=
True
)
fsp
=
sp
.
FewStepPlanner
(
tp
.
cl
,
tp
.
ps
,
tp
.
rbprmBuilder
,
fullBody
,
pathPlayer
=
tp
.
pp
)
print
"contact start "
states
,
cfgs
=
fsp
.
interpolateStates
(
0.002
,
pathId
=
pId
,
robustnessTreshold
=
1
,
filterStates
=
True
,
quasiStatic
=
True
)
print
"contact start "
#~ print "names ", fsp.rbprmBuilder.getAllJointNames()
#~ print "names ", tp.rbprmBuilder.getAllJointNames()
n_goal
=
tp
.
q_goal
[:
7
]
n_goal
[
0
]
+=
1
n_goal
=
tp
.
q_goal
[:
7
][:]
n_goal
[
0
]
+=
2
n_goal
[
1
]
+=
1
n_goal
[
3
:
7
]
=
[
0.
,
0.
,
0.7071
,
0.7071
]
goal_state
=
states
[
-
1
].
q
()[:]
goal_state
[:
7
]
=
n_goal
fullBody
.
setStartState
(
states
[
-
1
].
q
(),[
fullBody
.
rArmId
,
fullBody
.
rLegId
,
fullBody
.
lArmId
,
fullBody
.
lLegId
],
normals
)
fullBody
.
setEndState
(
goal_state
,
[
fullBody
.
rArmId
,
fullBody
.
rLegId
,
fullBody
.
lArmId
,
fullBody
.
lLegId
],
normals
)
n_
goal_state
=
states
[
-
1
].
q
()[:]
n_
goal_state
[:
7
]
=
n_goal
[:]
fullBody
.
setStartState
Id
(
states
[
-
1
].
sId
)
fullBody
.
setEndState
(
n_
goal_state
,
[
fullBody
.
rArmId
,
fullBody
.
rLegId
,
fullBody
.
lArmId
,
fullBody
.
lLegId
],
normals
)
pId
=
fsp
.
guidePath
(
tp
.
q_goal
[:
7
],
n_goal
)
states2
=
sp
.
interpolateState
(
fullBody
,
0.002
,
pathId
=
pId
,
robustnessTreshold
=
1
,
filterStates
=
True
,
quasiStatic
=
True
,
erasePreviousStates
=
False
)
states2
,
cfgs2
=
f
sp
.
interpolateState
s
(
0.002
,
pathId
=
pId
,
robustnessTreshold
=
1
,
filterStates
=
True
,
quasiStatic
=
True
,
erasePreviousStates
=
False
)
#~ displayContactSequence(v,cfgs2,0.1)
print
"cplan"
#~ dispContactPlan(states2,0.1)
goal_state
[
6
]
=
-
1
print
"end cplan"
fullBody
.
setStartState
(
states
[
-
1
].
q
(),[
fullBody
.
rArmId
,
fullBody
.
rLegId
,
fullBody
.
lArmId
,
fullBody
.
lLegId
],
normals
)
fullBody
.
setEndState
(
goal_state
,
[
fullBody
.
rArmId
,
fullBody
.
rLegId
,
fullBody
.
lArmId
,
fullBody
.
lLegId
],
normals
)
pId
=
fsp
.
guidePath
(
n_goal
,
tp
.
q_goal
[:
7
])
fullBody
.
setStartState
(
n_goal_state
,[
fullBody
.
rArmId
,
fullBody
.
rLegId
,
fullBody
.
lArmId
,
fullBody
.
lLegId
],
normals
)
fullBody
.
setEndState
(
states
[
-
1
].
q
(),
[
fullBody
.
rArmId
,
fullBody
.
rLegId
,
fullBody
.
lArmId
,
fullBody
.
lLegId
],
normals
)
pId
=
fsp
.
guidePath
(
tp
.
q_goal
[:
7
],
n_goal
)
states3
=
sp
.
interpolateState
(
fullBody
,
0.001
,
pathId
=
pId
,
robustnessTreshold
=
1
,
filterStates
=
True
,
quasiStatic
=
True
,
erasePreviousStates
=
False
)
print
"new state cplan"
#~ dispContactPlan(states2,0.1)
states3
,
cfgs3
=
fsp
.
interpolateStates
(
0.002
,
pathId
=
pId
,
robustnessTreshold
=
1
,
filterStates
=
True
,
quasiStatic
=
True
,
erasePreviousStates
=
False
)
n_goal
[
1
]
-=
3
states4
,
cfgs4
=
fsp
.
goToQuasiStatic
(
states3
[
-
1
],
n_goal
)
script/scenarios/sandbox/anymal_one_step/config.py
0 → 100644
View file @
723c10c2
SCENE
=
"multicontact/ground"
INIT_CONFIG_ROOT
=
[
0
,
0
,
0.465
,
0
,
0
,
0
,
1
]
INIT_CONFIG_WB
=
None
ROOT_BOUNDS
=
[
-
20
,
20
,
-
20
,
20
,
0.4
,
0.5
]
script/scenarios/sandbox/anymal_one_step/demo.py
0 → 100644
View file @
723c10c2
import
setup_one_step
as
sos
fsp
=
sos
.
fewStepPlanner
#planner instance
q_init
=
sos
.
q_init
#initial configuration
initState
=
sos
.
initState
#initial state.
viewer
=
sos
.
v
### Go somewhere
n_goal
=
q_init
[:
7
][:]
n_goal
[
0
]
+=
2
n_goal
[
1
]
+=
1
n_goal
[
3
:
7
]
=
[
0.
,
0.
,
0.7071
,
0.7071
]
states
,
configs
=
fsp
.
goToQuasiStatic
(
initState
,
n_goal
,
displayGuidePath
=
True
)
#equivalent to
# fsp.goToQuasiStatic(self, initState, n_goal, stepsize = 0.002, goalLimbsInContact = None, goalNormals = None, displayGuidePath = False)
#display computed States:
sos
.
dispContactPlan
(
states
,
0.051
)
#2nd argument is frame rateue
s
=
states
[
-
1
]
#last state computed
#display configuration
viewer
(
s
.
q
())
#some helpers:
s
.
q
()
# configuration associated to state
#~ initState. # configuration associated to state
script/scenarios/sandbox/anymal_one_step/setup_one_step.py
0 → 100644
View file @
723c10c2
import
config
as
cfg
#root path configuration
from
hpp.corbaserver.rbprm.anymal_abstract
import
Robot
from
hpp.gepetto
import
Viewer
from
hpp.corbaserver
import
ProblemSolver
import
numpy
as
np
import
time
statusFilename
=
"/tmp/infos.log"
vMax
=
0.3
# linear velocity bound for the root
aMax
=
1.
# linear acceleration bound for the root
extraDof
=
6
mu
=
0.5
# coefficient of friction
# Creating an instance of the helper class, and loading the robot
# Creating an instance of the helper class, and loading the robot
rbprmBuilder
=
Robot
()
# Define bounds for the root : bounding box of the scenario
root_bounds
=
cfg
.
ROOT_BOUNDS
rbprmBuilder
.
setJointBounds
(
"root_joint"
,
root_bounds
)
# The following lines set constraint on the valid configurations:
# a configuration is valid only if all limbs can create a contact with the corresponding afforcances type
rbprmBuilder
.
setFilter
(
rbprmBuilder
.
urdfNameRom
)
for
rom
in
rbprmBuilder
.
urdfNameRom
:
rbprmBuilder
.
setAffordanceFilter
(
rom
,
[
'Support'
])
# We also bound the rotations of the torso. (z, y, x)
#~ rbprmBuilder.boundSO3([-1.7,1.7,-0.1,0.1,-0.1,0.1])
rbprmBuilder
.
boundSO3
([
-
3
,
3
,
-
3.
,
3.
,
-
3.
,
-
3
])
# Add 6 extraDOF to the problem, used to store the linear velocity and acceleration of the root
rbprmBuilder
.
client
.
robot
.
setDimensionExtraConfigSpace
(
extraDof
)
# We set the bounds of this extraDof with velocity and acceleration bounds (expect on z axis)
rbprmBuilder
.
client
.
robot
.
setExtraConfigSpaceBounds
([
-
vMax
,
vMax
,
-
vMax
,
vMax
,
0
,
0
,
-
aMax
,
aMax
,
-
aMax
,
aMax
,
0
,
0
])
indexECS
=
rbprmBuilder
.
getConfigSize
()
-
rbprmBuilder
.
client
.
robot
.
getDimensionExtraConfigSpace
()
# Creating an instance of HPP problem solver
psGuide
=
ProblemSolver
(
rbprmBuilder
)
# define parameters used by various methods :
psGuide
.
setParameter
(
"Kinodynamic/velocityBound"
,
vMax
)
psGuide
.
setParameter
(
"Kinodynamic/accelerationBound"
,
aMax
)
psGuide
.
setParameter
(
"DynamicPlanner/sizeFootX"
,
0.01
)
psGuide
.
setParameter
(
"DynamicPlanner/sizeFootY"
,
0.01
)
psGuide
.
setParameter
(
"DynamicPlanner/friction"
,
mu
)
# sample only configuration with null velocity and acceleration :
psGuide
.
setParameter
(
"ConfigurationShooter/sampleExtraDOF"
,
False
)
# initialize the viewer :
from
hpp.gepetto
import
ViewerFactory
vf
=
ViewerFactory
(
psGuide
)
# load the module to analyse the environnement and compute the possible contact surfaces
from
hpp.corbaserver.affordance.affordance
import
AffordanceTool
afftool
=
AffordanceTool
()
afftool
.
setAffordanceConfig
(
'Support'
,
[
0.5
,
0.03
,
0.00005
])
afftool
.
loadObstacleModel
(
"hpp_environments"
,
cfg
.
SCENE
,
"planning"
,
vf
)
try
:
v
=
vf
.
createViewer
(
displayArrows
=
True
)
except
Exception
:
print
"No viewer started !"
class
FakeViewer
():
def
__init__
(
self
):
return
def
__call__
(
self
,
q
):
return
v
=
FakeViewer
()
#afftool.visualiseAffordances('Support', v, v.color.lightBrown)
q_init
=
rbprmBuilder
.
getCurrentConfig
();
q_init
[:
7
]
=
cfg
.
INIT_CONFIG_ROOT
# Choosing RBPRM shooter and path validation methods.
psGuide
.
selectConfigurationShooter
(
"RbprmShooter"
)
psGuide
.
selectPathValidation
(
"RbprmPathValidation"
,
0.05
)
# Choosing kinodynamic methods :
psGuide
.
selectSteeringMethod
(
"RBPRMKinodynamic"
)
psGuide
.
selectDistance
(
"Kinodynamic"
)
psGuide
.
selectPathPlanner
(
"DynamicPlanner"
)
# Solve the planning problem :
# move the robot out of the view before computing the contacts
from
hpp.corbaserver
import
Client
#~ #DEMO code to play root path and final contact plan
cl
=
Client
()
cl
.
problem
.
selectProblem
(
"rbprm_path"
)
rbprmBuilder2
=
Robot
(
"toto"
)
ps2
=
ProblemSolver
(
rbprmBuilder2
)
cl
.
problem
.
selectProblem
(
"default"
)
#~ cl.problem.movePathToProblem(pId,"rbprm_path",rbprmBuilder.getAllJointNames()[1:])
r2
=
Viewer
(
ps2
,
viewerClient
=
v
.
client
)
q_far
=
q_init
[::]
q_far
[
2
]
=
-
2
r2
(
q_far
)
from
hpp.gepetto
import
PathPlayer
pp
=
PathPlayer
(
v
)
###### WHOLE BODY INIT
from
hpp.corbaserver.rbprm.anymal
import
Robot
#~ from hpp.gepetto import Viewer
from
tools.display_tools
import
*
import
time
pId
=
0
fullBody
=
Robot
()
# Set the bounds for the root
fullBody
.
setJointBounds
(
"root_joint"
,
root_bounds
)
## reduce bounds on joints along x, to put conservative condition on the contact generation for sideway steps
fullBody
.
setVeryConstrainedJointsBounds
()
# add the 6 extraDof for velocity and acceleration (see *_path.py script)
fullBody
.
client
.
robot
.
setDimensionExtraConfigSpace
(
extraDof
)
fullBody
.
client
.
robot
.
setExtraConfigSpaceBounds
([
-
vMax
,
vMax
,
-
vMax
,
vMax
,
0
,
0
,
-
aMax
,
aMax
,
-
aMax
,
aMax
,
0
,
0
])
ps
=
ProblemSolver
(
fullBody
)
ps
.
setParameter
(
"Kinodynamic/velocityBound"
,
vMax
)
ps
.
setParameter
(
"Kinodynamic/accelerationBound"
,
aMax
)
#load the viewer
try
:
v
=
Viewer
(
ps
,
viewerClient
=
v
.
client
,
displayCoM
=
True
)
except
Exception
:
print
"No viewer started !"
class
FakeViewer
():
def
__init__
(
self
):
return
def
__call__
(
self
,
q
):
return
def
addLandmark
(
self
,
a
,
b
):
return
v
=
FakeViewer
()
if
cfg
.
INIT_CONFIG_WB
is
None
:
# load a reference configuration
q_ref
=
fullBody
.
referenceConfig
[::]
+
[
0
]
*
6
#q_ref = fullBody.referenceConfig_legsApart[::]+[0]*6
else
:
q_ref
=
cfg
.
INIT_CONFIG_WB
+
[
0
]
*
6
q_init
=
q_ref
[::]
fullBody
.
setReferenceConfig
(
q_ref
)
fullBody
.
setPostureWeights
(
fullBody
.
postureWeights
[::]
+
[
0
]
*
6
)
fullBody
.
usePosturalTaskContactCreation
(
True
)
fullBody
.
setCurrentConfig
(
q_init
)
print
"Generate limb DB ..."
tStart
=
time
.
time
()
# generate databases :
"""
nbSamples = 100000
fullBody.addLimb(fullBody.rLegId,fullBody.rleg,fullBody.rfoot,fullBody.rLegOffset,fullBody.rLegNormal, fullBody.rLegx, fullBody.rLegy, nbSamples, heuristicR, 0.01,kinematicConstraintsPath=fullBody.rLegKinematicConstraints,kinematicConstraintsMin = 0.85)
fullBody.runLimbSampleAnalysis(fullBody.rLegId, "ReferenceConfiguration", True)
fullBody.addLimb(fullBody.lLegId,fullBody.lleg,fullBody.lfoot,fullBody.lLegOffset,fullBody.rLegNormal, fullBody.lLegx, fullBody.lLegy, nbSamples, heuristicL, 0.01,kinematicConstraintsPath=fullBody.lLegKinematicConstraints,kinematicConstraintsMin = 0.85)
fullBody.runLimbSampleAnalysis(fullBody.lLegId, "ReferenceConfiguration", True)
"""
#~ fullBody.loadAllLimbs("fixedStep04","ReferenceConfiguration")
fullBody
.
loadAllLimbs
(
"static"
,
"ReferenceConfiguration"
)
tGenerate
=
time
.
time
()
-
tStart
print
"Done."
print
"Databases generated in : "
+
str
(
tGenerate
)
+
" s"
#define initial and final configurations :
configSize
=
fullBody
.
getConfigSize
()
-
fullBody
.
client
.
robot
.
getDimensionExtraConfigSpace
()
q_init
[
0
:
7
]
=
cfg
.
INIT_CONFIG_ROOT
[
0
:
7
]
# use this to get the correct orientation
vel_init
=
[
0
,
0
,
0
]
acc_init
=
[
0
,
0
,
0
]
vel_goal
=
[
0
,
0
,
0
]
acc_goal
=
[
0
,
0
,
0
]
robTreshold
=
3
# copy extraconfig for start and init configurations
q_init
[
configSize
:
configSize
+
3
]
=
vel_init
[::]
q_init
[
configSize
+
3
:
configSize
+
6
]
=
acc_init
[::]
#~ q_goal[configSize:configSize+3] = vel_goal[::]
#~ q_goal[configSize+3:configSize+6] = [0,0,0]
q_init
[
2
]
=
q_ref
[
2
]
#~ q_goal[2] = q_ref[2]
fullBody
.
setStaticStability
(
True
)
fullBody
.
setCurrentConfig
(
q_init
)
v
(
q_init
)
v
.
addLandmark
(
'anymal/base_0'
,
0.3
)
v
(
q_init
)
#fullBody.setReferenceConfig(fullBody.referenceConfig_legsApart[::]+[0]*6)
fullBody
.
setStartState
(
q_init
,[
fullBody
.
rArmId
,
fullBody
.
rLegId
,
fullBody
.
lArmId
,
fullBody
.
lLegId
],[[
0.
,
0.
,
1.
]
for
_
in
range
(
4
)])
from
hpp.corbaserver.rbprm
import
rbprmstate
from
hpp.corbaserver.rbprm.rbprmstate
import
State
initState
=
State
(
fullBody
,
fullBody
.
getNumStates
()
-
1
)
import
hpp.corbaserver.rbprm.fewstepsplanner
as
sp
def
dispContactPlan
(
states
,
step
=
0.5
):
for
s
in
states
:
v
(
s
.
q
());
time
.
sleep
(
step
)
fewStepPlanner
=
sp
.
FewStepPlanner
(
cl
,
psGuide
,
rbprmBuilder
,
fullBody
,
pathPlayer
=
pp
)
src/hpp/corbaserver/rbprm/fewstepsplanner.py
View file @
723c10c2
...
...
@@ -24,26 +24,38 @@ from numpy import array
from
hpp.corbaserver.rbprm
import
rbprmstate
from
hpp.corbaserver.rbprm.rbprmstate
import
State
def
interpolateState
(
fullBody
,
stepsize
,
pathId
=
1
,
robustnessTreshold
=
0
,
filterStates
=
False
,
testReachability
=
True
,
quasiStatic
=
False
,
erasePreviousStates
=
Fals
e
):
def
_
interpolateState
(
ps
,
fullBody
,
stepsize
,
pathId
,
robustnessTreshold
=
0
,
filterStates
=
False
,
testReachability
=
True
,
quasiStatic
=
False
,
erasePreviousStates
=
Tru
e
):
if
(
filterStates
):
filt
=
1
else
:
filt
=
0
configs
=
fullBody
.
clientRbprm
.
rbprm
.
interpolate
(
stepsize
,
pathId
,
robustnessTreshold
,
filt
,
testReachability
,
quasiStatic
,
erasePreviousStates
)
#discretize path
totalLength
=
ps
.
pathLength
(
pathId
)
configsPlan
=
[];
step
=
0.
configSize
=
fullBody
.
getConfigSize
()
-
len
(
ps
.
configAtParam
(
pathId
,
0.
))
z
=
[
0.
for
_
in
range
(
configSize
)
]
while
step
<
totalLength
:
configsPlan
+=
[
ps
.
configAtParam
(
pathId
,
step
)
+
z
[:]]
step
+=
stepsize
configsPlan
+=
[
ps
.
configAtParam
(
pathId
,
totalLength
)
+
z
[:]]
configs
=
fullBody
.
clientRbprm
.
rbprm
.
interpolateConfigs
(
configsPlan
,
robustnessTreshold
,
filt
,
testReachability
,
quasiStatic
,
erasePreviousStates
)
firstStateId
=
fullBody
.
clientRbprm
.
rbprm
.
getNumStates
()
-
len
(
configs
)
return
[
State
(
fullBody
,
i
)
for
i
in
range
(
firstStateId
,
firstStateId
+
len
(
configs
))
]
return
[
State
(
fullBody
,
i
)
for
i
in
range
(
firstStateId
,
firstStateId
+
len
(
configs
))
]
,
configs
def
guidePath
(
problemSolver
,
fromPos
,
toPos
):
def
_
guidePath
(
problemSolver
,
fromPos
,
toPos
):
ps
=
problemSolver
ps
.
setInitialConfig
(
fromPos
)
ps
.
resetGoalConfigs
()
ps
.
addGoalConfig
(
toPos
)
ps
.
solve
()
return
ps
.
numberPaths
()
-
1
class
FewStepPlanner
(
object
):
def
__init__
(
self
,
client
,
problemSolver
,
rbprmBuilder
,
fullBody
,
planContext
=
"rbprm_path"
,
fullBodyContext
=
"default"
,
pathPlayer
=
None
):
def
__init__
(
self
,
client
,
problemSolver
,
rbprmBuilder
,
fullBody
,
planContext
=
"rbprm_path"
,
fullBodyContext
=
"default"
,
pathPlayer
=
None
,
viewer
=
None
):
self
.
fullBody
=
fullBody
self
.
rbprmBuilder
=
rbprmBuilder
self
.
client
=
client
...
...
@@ -51,6 +63,7 @@ class FewStepPlanner(object):
self
.
fullBodyContext
=
fullBodyContext
self
.
problemSolver
=
problemSolver
self
.
pathPlayer
=
pathPlayer
self
.
viewer
=
viewer
def
setPlanningContext
(
self
):
self
.
client
.
problem
.
selectProblem
(
self
.
planContext
)
...
...
@@ -71,13 +84,32 @@ class FewStepPlanner(object):
self
.
setCurrentContext
(
oldContext
)
return
res
def
guidePath
(
self
,
fromPos
,
toPos
):
pId
=
self
.
_actInContext
(
self
.
planContext
,
guidePath
,
self
.
problemSolver
,
fromPos
,
toPos
)
def
guidePath
(
self
,
fromPos
,
toPos
,
displayPath
=
False
):
pId
=
self
.
_actInContext
(
self
.
planContext
,
_
guidePath
,
self
.
problemSolver
,
fromPos
,
toPos
)
self
.
setPlanningContext
()
names
=
self
.
rbprmBuilder
.
getAllJointNames
()[
1
:]
self
.
pathPlayer
(
pId
)
if
displayPath
:
if
self
.
pathPlayer
is
None
:
print
"can't display path, no path player given"
else
:
self
.
pathPlayer
(
pId
)
self
.
client
.
problem
.
movePathToProblem
(
pId
,
self
.
fullBodyContext
,
names
)
self
.
setFullBodyContext
()
return
pId
def
interpolateStates
(
self
,
stepsize
,
pathId
=
1
,
robustnessTreshold
=
0
,
filterStates
=
False
,
testReachability
=
True
,
quasiStatic
=
False
,
erasePreviousStates
=
True
):
return
_interpolateState
(
self
.
problemSolver
,
self
.
fullBody
,
stepsize
,
pathId
,
robustnessTreshold
,
filterStates
,
testReachability
,
quasiStatic
,
erasePreviousStates
)
def
goToQuasiStatic
(
self
,
currentState
,
toPos
,
stepsize
=
0.002
,
goalLimbsInContact
=
None
,
goalNormals
=
None
,
displayGuidePath
=
False
):
pId
=
self
.
guidePath
(
currentState
.
q
()[:
7
],
toPos
,
displayPath
=
displayGuidePath
)
self
.
fullBody
.
setStartStateId
(
currentState
.
sId
)
targetState
=
currentState
.
q
()[:];
targetState
[:
7
]
=
toPos
if
goalLimbsInContact
is
None
:
goalLimbsInContact
=
currentState
.
getLimbsInContact
()
if
goalNormals
is
None
:
goalNormals
=
[[
0.
,
0.
,
1.
]
for
_
in
range
(
len
(
goalLimbsInContact
))]
self
.
fullBody
.
setEndState
(
targetState
,
goalLimbsInContact
,
goalNormals
)
return
self
.
interpolateStates
(
stepsize
,
pathId
=
pId
,
robustnessTreshold
=
1
,
filterStates
=
True
,
quasiStatic
=
True
,
erasePreviousStates
=
False
)
src/rbprmbuilder.impl.cc
View file @
723c10c2
...
...
@@ -1574,8 +1574,8 @@ namespace hpp {
std
::
size_t
id
=
0
;
for
(
std
::
vector
<
State
>::
const_iterator
cit
=
newStates
.
begin
();
cit
!=
newStates
.
end
();
++
cit
,
++
id
)
{
std
::
cout
<<
"ID "
<<
id
;
cit
->
print
();
/*
std::cout << "ID " << id;
cit->print();
*/
const
core
::
Configuration_t
config
=
cit
->
configuration_
;
_CORBA_ULong
size
=
(
_CORBA_ULong
)
config
.
size
();
double
*
dofArray
=
hpp
::
floatSeq
::
allocbuf
(
size
);
...
...
Write
Preview
Supports
Markdown
0%
Try again
or
attach a new file
.
Cancel
You are about to add
0
people
to the discussion. Proceed with caution.
Finish editing this message first!
Cancel
Please
register
or
sign in
to comment