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
a7ccfd4b
Commit
a7ccfd4b
authored
Sep 30, 2019
by
Pierre Fernbach
Browse files
[demo] add demo script memmo/talos_moveRandom_flat
parent
3b0dca31
Changes
3
Hide whitespace changes
Inline
Side-by-side
script/scenarios/memmo/talos_randomMove_flat.py
0 → 100644
View file @
a7ccfd4b
from
hpp.corbaserver.rbprm.talos
import
Robot
from
hpp.gepetto
import
Viewer
from
tools.display_tools
import
*
from
hpp.gepetto
import
ViewerFactory
from
hpp.corbaserver
import
ProblemSolver
import
os
import
random
import
time
print
"Plan guide trajectory ..."
import
talos_randomMove_path
as
tp
print
"Done."
import
time
statusFilename
=
tp
.
statusFilename
pId
=
0
f
=
open
(
statusFilename
,
"a"
)
if
tp
.
ps
.
numberPaths
()
>
0
:
print
"Path planning OK."
f
.
write
(
"Planning_success: True"
+
"
\n
"
)
f
.
close
()
else
:
print
"Error during path planning"
f
.
write
(
"Planning_success: False"
+
"
\n
"
)
f
.
close
()
import
sys
sys
.
exit
(
1
)
fullBody
=
Robot
()
# Set the bounds for the root
fullBody
.
setJointBounds
(
"root_joint"
,
[
-
0.3
,
0.3
,
-
0.3
,
0.3
,
tp
.
ROOT_Z_MIN
,
tp
.
ROOT_Z_MAX
])
## reduce bounds on joints along x, to put conservative condition on the contact generation for sideway steps
joint6L_bounds_prev
=
fullBody
.
getJointBounds
(
'leg_left_6_joint'
)
joint2L_bounds_prev
=
fullBody
.
getJointBounds
(
'leg_left_2_joint'
)
joint6R_bounds_prev
=
fullBody
.
getJointBounds
(
'leg_right_6_joint'
)
joint2R_bounds_prev
=
fullBody
.
getJointBounds
(
'leg_right_2_joint'
)
fullBody
.
setJointBounds
(
'leg_left_6_joint'
,[
-
0.25
,
0.25
])
fullBody
.
setJointBounds
(
'leg_left_2_joint'
,[
-
0.25
,
0.25
])
fullBody
.
setJointBounds
(
'leg_right_6_joint'
,[
-
0.25
,
0.25
])
fullBody
.
setJointBounds
(
'leg_right_2_joint'
,[
-
0.25
,
0.25
])
# constraint z axis and y axis :
joint1L_bounds_prev
=
fullBody
.
getJointBounds
(
'leg_left_1_joint'
)
joint3L_bounds_prev
=
fullBody
.
getJointBounds
(
'leg_left_3_joint'
)
joint1R_bounds_prev
=
fullBody
.
getJointBounds
(
'leg_right_1_joint'
)
joint3R_bounds_prev
=
fullBody
.
getJointBounds
(
'leg_right_3_joint'
)
fullBody
.
setJointBounds
(
'leg_left_1_joint'
,[
-
0.2
,
0.7
])
fullBody
.
setJointBounds
(
'leg_left_3_joint'
,[
-
1.3
,
0.4
])
fullBody
.
setJointBounds
(
'leg_right_1_joint'
,[
-
0.7
,
0.2
])
fullBody
.
setJointBounds
(
'leg_right_3_joint'
,[
-
1.3
,
0.4
])
# add the 6 extraDof for velocity and acceleration (see *_path.py script)
fullBody
.
client
.
robot
.
setDimensionExtraConfigSpace
(
6
)
vMax
=
0.5
# linear velocity bound for the root
aMax
=
0.5
# linear acceleration bound for the root
fullBody
.
client
.
robot
.
setExtraConfigSpaceBounds
([
-
vMax
,
vMax
,
-
vMax
,
vMax
,
0
,
0
,
-
aMax
,
aMax
,
-
aMax
,
aMax
,
0
,
0
])
ps
=
tp
.
ProblemSolver
(
fullBody
)
vf
=
ViewerFactory
(
ps
)
ps
.
setParameter
(
"Kinodynamic/velocityBound"
,
vMax
)
ps
.
setParameter
(
"Kinodynamic/accelerationBound"
,
aMax
)
ps
.
setRandomSeed
(
random
.
SystemRandom
().
randint
(
0
,
999999
))
#load the viewer
try
:
v
=
vf
.
createViewer
(
displayCoM
=
True
)
except
Exception
:
print
"No viewer started !"
class
FakeViewer
():
sceneName
=
""
def
__init__
(
self
):
return
def
__call__
(
self
,
q
):
return
def
addLandmark
(
self
,
a
,
b
):
return
v
=
FakeViewer
()
v
.
addLandmark
(
v
.
sceneName
,
0.5
)
v
.
addLandmark
(
'talos/base_link'
,
0.3
)
# load a reference configuration
q_ref
=
fullBody
.
referenceConfig
[::]
+
[
0
]
*
6
#q_ref = fullBody.referenceConfig_legsApart[::]+[0]*6
fullBody
.
setReferenceConfig
(
q_ref
)
fullBody
.
setPostureWeights
(
fullBody
.
postureWeights
[::]
+
[
0
]
*
6
)
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
,
"fixedStep1"
,
0.01
,
kinematicConstraintsPath
=
fullBody
.
rLegKinematicConstraints
,
kinematicConstraintsMin
=
0.7
)
fullBody
.
runLimbSampleAnalysis
(
fullBody
.
rLegId
,
"ReferenceConfiguration"
,
True
)
fullBody
.
addLimb
(
fullBody
.
lLegId
,
fullBody
.
lleg
,
fullBody
.
lfoot
,
fullBody
.
lLegOffset
,
fullBody
.
rLegNormal
,
fullBody
.
lLegx
,
fullBody
.
lLegy
,
nbSamples
,
"fixedStep1"
,
0.01
,
kinematicConstraintsPath
=
fullBody
.
lLegKinematicConstraints
,
kinematicConstraintsMin
=
0.7
)
fullBody
.
runLimbSampleAnalysis
(
fullBody
.
lLegId
,
"ReferenceConfiguration"
,
True
)
tGenerate
=
time
.
time
()
-
tStart
print
"Done."
print
"Databases generated in : "
+
str
(
tGenerate
)
+
" s"
## generate random initial state : root pose at the origin exepct for z translation and both feet in contact with the floor
from
tools.sample_random_transition
import
sampleRandomStateFlatFloor
limbsInContact
=
[
fullBody
.
rLegId
,
fullBody
.
lLegId
]
random
.
seed
()
floor_Z
=
-
0.00095
floor_Z
=
0.
s0
=
sampleRandomStateFlatFloor
(
fullBody
,
limbsInContact
,
floor_Z
)
q_init
=
s0
.
q
()
q_goal
=
q_ref
[::]
q_goal
[
0
:
7
]
=
tp
.
ps
.
configAtParam
(
pId
,
tp
.
ps
.
pathLength
(
pId
))[
0
:
7
]
q_goal
[
2
]
=
q_ref
[
2
]
robTreshold
=
3
fullBody
.
setStaticStability
(
True
)
v
.
addLandmark
(
'talos/base_link'
,
0.3
)
v
(
q_init
)
# specify the full body configurations as start and goal state of the problem
if
q_goal
[
1
]
<
0
:
# goal on the right side of the circle, start motion with right leg first
fullBody
.
setStartState
(
q_init
,[
fullBody
.
rLegId
,
fullBody
.
lLegId
])
fullBody
.
setEndState
(
q_goal
,[
fullBody
.
rLegId
,
fullBody
.
lLegId
])
print
"Right foot first"
else
:
fullBody
.
setStartState
(
q_init
,[
fullBody
.
lLegId
,
fullBody
.
rLegId
])
fullBody
.
setEndState
(
q_goal
,[
fullBody
.
lLegId
,
fullBody
.
rLegId
])
print
"Left foot first"
print
"Generate contact plan ..."
tStart
=
time
.
time
()
configs
=
fullBody
.
interpolate
(
0.005
,
pathId
=
pId
,
robustnessTreshold
=
robTreshold
,
filterStates
=
True
,
quasiStatic
=
True
)
tInterpolateConfigs
=
time
.
time
()
-
tStart
print
"Done."
print
"Contact plan generated in : "
+
str
(
tInterpolateConfigs
)
+
" s"
print
"number of configs :"
,
len
(
configs
)
#raw_input("Press Enter to display the contact sequence ...")
#displayContactSequence(v,configs)
if
len
(
configs
)
<
2
:
cg_success
=
False
print
"Error during contact generation."
else
:
cg_success
=
True
print
"Contact generation Done."
if
abs
(
configs
[
-
1
][
0
]
-
tp
.
q_goal
[
0
])
<
0.01
and
abs
(
configs
[
-
1
][
1
]
-
tp
.
q_goal
[
1
])
<
0.01
and
(
len
(
fullBody
.
getContactsVariations
(
len
(
configs
)
-
2
,
len
(
configs
)
-
1
))
==
1
):
print
"Contact generation successful."
cg_reach_goal
=
True
else
:
print
"Contact generation failed to reach the goal."
cg_reach_goal
=
False
if
len
(
configs
)
>
10
:
cg_too_many_states
=
True
cg_success
=
False
print
"Discarded contact sequence because it was too long."
else
:
cg_too_many_states
=
False
f
=
open
(
statusFilename
,
"a"
)
f
.
write
(
"cg_success: "
+
str
(
cg_success
)
+
"
\n
"
)
f
.
write
(
"cg_reach_goal: "
+
str
(
cg_reach_goal
)
+
"
\n
"
)
f
.
write
(
"cg_too_many_states: "
+
str
(
cg_too_many_states
)
+
"
\n
"
)
f
.
close
()
if
(
not
cg_success
)
or
cg_too_many_states
:
import
sys
sys
.
exit
(
1
)
# put back original bounds for wholebody methods
fullBody
.
setJointBounds
(
"root_joint"
,
[
-
2
,
2
,
-
2
,
2
,
0.6
,
1.4
])
fullBody
.
setJointBounds
(
'leg_left_6_joint'
,
joint6L_bounds_prev
)
fullBody
.
setJointBounds
(
'leg_left_2_joint'
,
joint2L_bounds_prev
)
fullBody
.
setJointBounds
(
'leg_right_6_joint'
,
joint6R_bounds_prev
)
fullBody
.
setJointBounds
(
'leg_right_2_joint'
,
joint2R_bounds_prev
)
fullBody
.
setJointBounds
(
'leg_left_1_joint'
,
joint1L_bounds_prev
)
fullBody
.
setJointBounds
(
'leg_left_3_joint'
,
joint3L_bounds_prev
)
fullBody
.
setJointBounds
(
'leg_right_1_joint'
,
joint1R_bounds_prev
)
fullBody
.
setJointBounds
(
'leg_right_3_joint'
,
joint3R_bounds_prev
)
script/scenarios/memmo/talos_randomMove_path.py
0 → 100644
View file @
a7ccfd4b
from
hpp.corbaserver.rbprm.talos_abstract
import
Robot
from
hpp.gepetto
import
Viewer
from
hpp.corbaserver
import
ProblemSolver
import
numpy
as
np
from
pinocchio
import
Quaternion
import
time
statusFilename
=
"/res/infos.log"
MIN_ROOT_DIST
=
0.05
MAX_ROOT_DIST
=
0.3
FINAL_ORIENTATION_RANDOM
=
False
# if true : random value of yaw orientation, if false : constrained to be along the direction of root_init->root_goal
ROOT_Z_MIN
=
0.85
ROOT_Z_MAX
=
1.05
vMax
=
0.5
# linear velocity bound for the root
vInit
=
0.01
# initial / final velocity to fix the direction
vGoal
=
0.01
aMax
=
0.05
# 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
rbprmBuilder
.
setJointBounds
(
"root_joint"
,
[
-
2
,
2
,
-
2
,
2
,
1.
,
1.
])
# 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
([
'talos_lleg_rom'
,
'talos_rleg_rom'
])
rbprmBuilder
.
setAffordanceFilter
(
'talos_lleg_rom'
,
[
'Support'
,])
rbprmBuilder
.
setAffordanceFilter
(
'talos_rleg_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
])
# 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
ps
=
ProblemSolver
(
rbprmBuilder
)
# define parameters used by various methods :
ps
.
setParameter
(
"Kinodynamic/velocityBound"
,
vMax
)
ps
.
setParameter
(
"Kinodynamic/accelerationBound"
,
aMax
)
ps
.
setParameter
(
"DynamicPlanner/sizeFootX"
,
0.2
)
ps
.
setParameter
(
"DynamicPlanner/sizeFootY"
,
0.12
)
ps
.
setParameter
(
"DynamicPlanner/friction"
,
0.5
)
ps
.
setParameter
(
"Kinodynamic/forceYawOrientation"
,
True
)
# sample only configuration with null velocity and acceleration :
ps
.
setParameter
(
"ConfigurationShooter/sampleExtraDOF"
,
False
)
# initialize the viewer :
from
hpp.gepetto
import
ViewerFactory
vf
=
ViewerFactory
(
ps
)
# 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"
,
"multicontact/ground"
,
"planning"
,
vf
)
#load the viewer
try
:
v
=
vf
.
createViewer
(
displayArrows
=
True
)
except
Exception
:
print
"No viewer started !"
class
FakeViewer
():
sceneName
=
""
def
__init__
(
self
):
return
def
__call__
(
self
,
q
):
return
def
addLandmark
(
self
,
a
,
b
):
return
v
=
FakeViewer
()
v
.
addLandmark
(
v
.
sceneName
,
0.5
)
#afftool.visualiseAffordances('Support', v, v.color.lightBrown)
q_init
=
rbprmBuilder
.
getCurrentConfig
();
q_init
[
0
:
3
]
=
[
0
,
0
,
1.
]
q_init
[
3
:
7
]
=
[
0
,
0
,
0
,
1
]
q_init
[
-
6
]
=
vInit
# sample random position on a circle of radius 2m
import
random
random
.
seed
()
radius
=
random
.
uniform
(
MIN_ROOT_DIST
,
MAX_ROOT_DIST
)
alpha
=
random
.
uniform
(
0.
,
2.
*
np
.
pi
)
print
"Test on a circle, alpha = "
,
alpha
print
"Radius = "
,
radius
q_goal
=
q_init
[::]
q_goal
[
0
:
3
]
=
[
radius
*
np
.
sin
(
alpha
),
-
radius
*
np
.
cos
(
alpha
),
1.
]
if
FINAL_ORIENTATION_RANDOM
:
alpha
=
random
.
uniform
(
0.
,
2.
*
np
.
pi
)
# sample new random yaw value for the orientation
v_goal
=
np
.
matrix
([
radius
*
np
.
sin
(
alpha
),
-
radius
*
np
.
cos
(
alpha
),
0
]).
T
else
:
v_goal
=
np
.
matrix
([
q_goal
[
0
],
q_goal
[
1
],
0
]).
T
# direction root_init -> root_goal
# set final orientation to be along the circle :
vx
=
np
.
matrix
([
1
,
0
,
0
]).
T
quat
=
Quaternion
.
FromTwoVectors
(
vx
,
v_goal
)
q_goal
[
3
:
7
]
=
quat
.
coeffs
().
T
.
tolist
()[
0
]
# set final velocity to fix the orientation :
q_goal
[
-
6
]
=
vGoal
*
np
.
sin
(
alpha
)
q_goal
[
-
5
]
=
-
vGoal
*
np
.
cos
(
alpha
)
v
(
q_goal
)
print
"initial root position : "
,
q_init
print
"final root position : "
,
q_goal
ps
.
setInitialConfig
(
q_init
)
ps
.
addGoalConfig
(
q_goal
)
# write problem in files :
f
=
open
(
statusFilename
,
"w"
)
f
.
write
(
"q_init= "
+
str
(
q_init
)
+
"
\n
"
)
f
.
write
(
"q_goal= "
+
str
(
q_goal
)
+
"
\n
"
)
f
.
close
()
# Choosing RBPRM shooter and path validation methods.
ps
.
selectConfigurationShooter
(
"RbprmShooter"
)
ps
.
selectPathValidation
(
"RbprmPathValidation"
,
0.05
)
# Choosing kinodynamic methods :
ps
.
selectSteeringMethod
(
"RBPRMKinodynamic"
)
ps
.
selectDistance
(
"Kinodynamic"
)
ps
.
selectPathPlanner
(
"DynamicPlanner"
)
# Solve the planning problem :
success
=
ps
.
client
.
problem
.
prepareSolveStepByStep
()
if
not
success
:
print
"planning failed."
import
sys
sys
.
exit
(
1
)
ps
.
client
.
problem
.
finishSolveStepByStep
()
try
:
# display solution :
from
hpp.gepetto
import
PathPlayer
pp
=
PathPlayer
(
v
)
pp
.
dt
=
0.1
pp
.
displayVelocityPath
(
0
)
#v.client.gui.setVisibility("path_0_root","ALWAYS_ON_TOP")
pp
.
dt
=
0.01
#pp(0)
except
Exception
:
pass
# move the robot out of the view before computing the contacts
q_far
=
q_init
[::]
q_far
[
2
]
=
-
2
v
(
q_far
)
script/tools/sample_random_transition.py
View file @
a7ccfd4b
...
...
@@ -5,10 +5,11 @@ import numpy as np
from
numpy.linalg
import
norm
from
pinocchio
import
SE3
,
se3ToXYZQUATtuple
,
Quaternion
import
sys
from
math
import
isnan
eff_x_range
=
[
-
0.4
,
0.4
]
eff_y_range
=
[
-
0.4
,
0.4
]
kin_distance_max
=
0.8
5
kin_distance_max
=
0.8
4
limb_ids
=
{
'talos_rleg_rom'
:
range
(
13
,
19
),
'talos_lleg_rom'
:
range
(
7
,
13
)}
...
...
@@ -24,12 +25,15 @@ def projectInKinConstraints(fullBody,state):
while
distance
>
kin_distance_max
and
successProj
and
maxIt
>
0
:
com_l
[
2
]
-=
0.001
successProj
=
state
.
projectToCOM
(
com_l
,
0
)
fullBody
.
setCurrentConfig
(
state
.
q
())
pL
=
np
.
matrix
(
fullBody
.
getJointPosition
(
'leg_left_sole_fix_joint'
)[
0
:
3
])
pR
=
np
.
matrix
(
fullBody
.
getJointPosition
(
'leg_right_sole_fix_joint'
)[
0
:
3
])
com_l
=
fullBody
.
getCenterOfMass
()
com
=
np
.
matrix
(
com_l
)
distance
=
max
(
norm
(
pL
-
com
),
norm
(
pR
-
com
))
if
isnan
(
state
.
q
()[
0
]):
successProj
=
False
else
:
fullBody
.
setCurrentConfig
(
state
.
q
())
pL
=
np
.
matrix
(
fullBody
.
getJointPosition
(
'leg_left_sole_fix_joint'
)[
0
:
3
])
pR
=
np
.
matrix
(
fullBody
.
getJointPosition
(
'leg_right_sole_fix_joint'
)[
0
:
3
])
com_l
=
fullBody
.
getCenterOfMass
()
com
=
np
.
matrix
(
com_l
)
distance
=
max
(
norm
(
pL
-
com
),
norm
(
pR
-
com
))
maxIt
-=
1
if
maxIt
==
0
:
successProj
=
False
...
...
@@ -73,17 +77,22 @@ def sampleRotationAlongZ(placement):
return
placement
# create a state with legs config and root orientation along z axis random, the rest is equal to the referenceConfig
def
createRandomState
(
fullBody
,
limbsInContact
):
def
createRandomState
(
fullBody
,
limbsInContact
,
root_at_origin
=
True
):
extraDof
=
int
(
fullBody
.
client
.
robot
.
getDimensionExtraConfigSpace
())
q0
=
fullBody
.
referenceConfig
[::]
if
extraDof
>
0
:
q0
+=
[
0
]
*
extraDof
qr
=
fullBody
.
shootRandomConfig
()
root
=
SE3
.
Identity
()
root
.
translation
=
np
.
matrix
([
0
,
0
,
qr
[
2
]]).
T
# sample random orientation along z :
root
=
sampleRotationAlongZ
(
root
)
q0
[
0
:
7
]
=
se3ToXYZQUATtuple
(
root
)
if
root_at_origin
:
q0
[
0
:
2
]
=
[
0
,
0
]
q0
[
2
]
=
qr
[
2
]
q0
[
3
:
7
]
=
[
0
,
0
,
0
,
1
]
else
:
root
=
SE3
.
Identity
()
root
.
translation
=
np
.
matrix
(
qr
[
0
:
3
]).
T
# sample random orientation along z :
root
=
sampleRotationAlongZ
(
root
)
q0
[
0
:
7
]
=
se3ToXYZQUATtuple
(
root
)
# apply random config to legs (FIXME : ID hardcoded for Talos)
q0
[
7
:
19
]
=
qr
[
7
:
19
]
fullBody
.
setCurrentConfig
(
q0
)
...
...
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