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
7947aa71
Unverified
Commit
7947aa71
authored
Jan 14, 2019
by
stonneau
Committed by
GitHub
Jan 14, 2019
Browse files
Merge pull request #14 from pFernbach/topic/kino
Topic/kino
parents
338503cc
9b69166e
Changes
15
Hide whitespace changes
Inline
Side-by-side
CMakeLists.txt
View file @
7947aa71
...
...
@@ -316,6 +316,7 @@ install(FILES
data/meshes/cross_gap2.stl
data/meshes/floor_bauzil.stl
data/meshes/floor_bauzil_obstacles.stl
data/meshes/bauzil_stairs_noRamp_simplified.stl
DESTINATION
${
CATKIN_PACKAGE_SHARE_DESTINATION
}
/meshes
)
...
...
data/meshes/bauzil_stairs.stl
View file @
7947aa71
No preview for this file type
data/meshes/bauzil_stairs_noRamp_simplified.stl
0 → 100644
View file @
7947aa71
File added
data/urdf/bauzil_stairs.urdf
View file @
7947aa71
...
...
@@ -12,7 +12,7 @@
<collision>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<mesh filename="package://hpp-rbprm-corba/meshes/bauzil_
walk_
stairs.stl"/>
<mesh filename="package://hpp-rbprm-corba/meshes/bauzil_stairs
_noRamp_simplified
.stl"/>
</geometry>
</collision>
</link>
...
...
data/urdf/hyq/hyq_trunk_large.urdf
View file @
7947aa71
...
...
@@ -5,7 +5,7 @@
<inertia ixx="1." ixy="0.0" ixz="0.0" iyy="1." iyz="0.0" izz="1."/>
</inertial>
<visual>
<origin xyz="0 0 0" rpy="0 0 0"
scale="2 2 2"
/>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<mesh filename="package://hpp-rbprm-corba/meshes/hyq/hyq_all.stl"/>
</geometry>
...
...
@@ -14,9 +14,9 @@
</material>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0"
scale="5 5 5"
/>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<mesh filename="package://hpp-rbprm-corba/meshes/hyq/hyq_trunk_large
2
.stl"
scale="1 1 1"
/>
<mesh filename="package://hpp-rbprm-corba/meshes/hyq/hyq_trunk_large.stl" />
</geometry>
</collision>
</link>
...
...
idl/hpp/corbaserver/rbprm/rbprmbuilder.idl
View file @
7947aa71
...
...
@@ -106,6 +106,10 @@ module hpp
void setReferenceConfig(in floatSeq referenceConfig)
raises (Error);
/// set a reference position of the end effector for the given ROM
void setReferenceEndEffector(in string romName, in floatSeq ref)
raises (Error);
/// Set Rom constraints for the configuration shooter
/// a configuration will only be valid if all roms indicated
/// are colliding with the environment.
...
...
script/dynamic/talos_table.py
0 → 100644
View file @
7947aa71
from
hpp.corbaserver.rbprm.rbprmbuilder
import
Builder
from
hpp.corbaserver.rbprm.rbprmfullbody
import
FullBody
from
hpp.gepetto
import
Viewer
import
time
from
hpp.corbaserver
import
ProblemSolver
from
hpp.corbaserver.rbprm.rbprmstate
import
State
,
StateHelper
import
time
rLegId
=
'talos_rleg_rom'
rLeg
=
'leg_right_1_joint'
rFoot
=
'leg_right_6_joint'
lLegId
=
'talos_lleg_rom'
lLeg
=
'leg_left_1_joint'
lFoot
=
'leg_left_6_joint'
rArmId
=
'talos_rarm_rom'
rArm
=
'arm_right_1_joint'
rHand
=
'arm_right_7_joint'
lArmId
=
'talos_larm_rom'
lArm
=
'arm_left_1_joint'
lHand
=
'arm_left_7_joint'
packageName
=
"talos_data"
meshPackageName
=
"talos_data"
rootJointType
=
"freeflyer"
urdfName
=
"talos"
urdfSuffix
=
"_reduced"
srdfSuffix
=
""
fullBody
=
FullBody
()
fullBody
.
loadFullBodyModel
(
urdfName
,
rootJointType
,
meshPackageName
,
packageName
,
urdfSuffix
,
srdfSuffix
)
fullBody
.
setJointBounds
(
"root_joint"
,
[
-
5
,
5
,
-
1.5
,
1.5
,
0.95
,
1.05
])
ps
=
ProblemSolver
(
fullBody
)
from
hpp.gepetto
import
ViewerFactory
vf
=
ViewerFactory
(
ps
)
vf
.
loadObstacleModel
(
"hpp-rbprm-corba"
,
"table_140_70_73"
,
"planning"
)
q_ref
=
[
0.0
,
0.0
,
1.0232773
,
0.0
,
0.0
,
0.0
,
1
,
#Free flyer
0.0
,
0.0
,
-
0.411354
,
0.859395
,
-
0.448041
,
-
0.001708
,
#Left Leg
0.0
,
0.0
,
-
0.411354
,
0.859395
,
-
0.448041
,
-
0.001708
,
#Right Leg
0.0
,
0.006761
,
#Chest
0.25847
,
0.173046
,
-
0.0002
,
-
0.525366
,
0.0
,
-
0.0
,
0.1
,
-
0.005
,
#Left Arm
-
0.25847
,
-
0.173046
,
0.0002
,
-
0.525366
,
0.0
,
0.0
,
0.1
,
-
0.005
,
#Right Arm
0.
,
0.
];
# head
q_init
=
q_ref
[::]
fullBody
.
setReferenceConfig
(
q_ref
)
tStart
=
time
.
time
()
# generate databases :
nbSamples
=
1000
rLegOffset
=
[
0
,
-
0.00018
,
-
0.107
]
rLegOffset
[
2
]
+=
0.006
rLegNormal
=
[
0
,
0
,
1
]
rLegx
=
0.1
;
rLegy
=
0.06
fullBody
.
addLimb
(
rLegId
,
rLeg
,
rFoot
,
rLegOffset
,
rLegNormal
,
rLegx
,
rLegy
,
nbSamples
,
"static"
,
0.01
)
fullBody
.
runLimbSampleAnalysis
(
rLegId
,
"ReferenceConfiguration"
,
True
)
#fullBody.saveLimbDatabase(rLegId, "./db/talos_rLeg_walk.db")
lLegOffset
=
[
0
,
-
0.00018
,
-
0.107
]
lLegOffset
[
2
]
+=
0.006
lLegNormal
=
[
0
,
0
,
1
]
lLegx
=
0.1
;
lLegy
=
0.06
fullBody
.
addLimb
(
lLegId
,
lLeg
,
lFoot
,
lLegOffset
,
rLegNormal
,
lLegx
,
lLegy
,
nbSamples
,
"static"
,
0.01
)
fullBody
.
runLimbSampleAnalysis
(
lLegId
,
"ReferenceConfiguration"
,
True
)
#fullBody.saveLimbDatabase(rLegId, "./db/talos_lLeg_walk.db")
#rArmOffset = [0.055,-0.04,-0.13]
rArmOffset
=
[
-
0.01
,
0.
,
-
0.154
]
rArmNormal
=
[
0
,
0
,
1
]
rArmx
=
0.005
;
rArmy
=
0.005
fullBody
.
addLimb
(
rArmId
,
rArm
,
rHand
,
rArmOffset
,
rArmNormal
,
rArmx
,
rArmy
,
nbSamples
,
"EFORT"
,
0.01
)
fullBody
.
runLimbSampleAnalysis
(
rArmId
,
"ReferenceConfiguration"
,
True
)
"""
lArmOffset = [0.055,0.04,-0.13]
lArmNormal = [0,0,1]
lArmx = 0.02; lArmy = 0.02
fullBody.addLimb(larmId,larm,lHand,lArmOffset,lArmNormal, lArmx, lArmy, nbSamples, "EFORT", 0.01)
fullBody.runLimbSampleAnalysis(larmId, "ReferenceConfiguration", True)
"""
tGenerate
=
time
.
time
()
-
tStart
print
"generate databases in : "
+
str
(
tGenerate
)
+
" s"
v
=
vf
.
createViewer
(
displayCoM
=
True
)
v
(
q_init
)
v
.
addLandmark
(
v
.
sceneName
,
0.5
)
v
.
addLandmark
(
'talos/arm_right_7_link'
,
0.1
)
q_init
[
0
:
2
]
=
[
-
0.5
,
0.8
]
q_init
[
32
]
-=
1.5
# right elbow
v
(
q_init
)
# sphere = target
from
display_tools
import
*
createSphere
(
'target'
,
v
,
size
=
0.05
,
color
=
v
.
color
.
red
)
v
.
client
.
gui
.
setVisibility
(
'target'
,
'ON'
)
moveSphere
(
'target'
,
v
,[
0
,
0
,
0.5
])
# create contact :
fullBody
.
setStartState
(
q_init
,[
lLegId
,
rLegId
])
q_ref
[
0
:
3
]
=
q_init
[
0
:
3
]
sref
=
State
(
fullBody
,
q
=
q_ref
,
limbsIncontact
=
[
lLegId
,
rLegId
])
s0
=
State
(
fullBody
,
q
=
q_init
,
limbsIncontact
=
[
lLegId
,
rLegId
])
createSphere
(
's'
,
v
)
p0
=
[
-
0.25
,
0.5
,
0.75
]
#p1 = [-0,0.45,0.8]
p
=
[
0.1
,
0.5
,
0.75
]
moveSphere
(
's'
,
v
,
p
)
s0_bis
,
success
=
StateHelper
.
addNewContact
(
sref
,
rArmId
,
p0
,[
0
,
0
,
1
])
#s0_bis2,success = StateHelper.addNewContact(s0_bis,rArmId,p1,[0,0,1])
s1
,
success
=
StateHelper
.
addNewContact
(
s0_bis
,
rArmId
,
p
,[
0
,
0
,
1
])
assert
(
success
)
v
(
s1
.
q
())
#project com
v
(
q_init
)
com_i
=
fullBody
.
getCenterOfMass
()
com_i
[
2
]
-=
0.03
com_i
[
0
]
+=
0.06
createSphere
(
"com"
,
v
)
moveSphere
(
"com"
,
v
,
com_i
)
s1
.
projectToCOM
(
com_i
)
v
(
s1
.
q
())
s1_feet
=
State
(
fullBody
,
q
=
s1
.
q
(),
limbsIncontact
=
[
lLegId
,
rLegId
])
s2
,
success
=
StateHelper
.
addNewContact
(
s0_bis
,
rArmId
,
p
,[
0
,
0
,
1
])
com
=
s2
.
getCenterOfMass
()
#com[0] += 0.03
com
[
1
]
-=
0.06
com
[
2
]
-=
0.02
moveSphere
(
"com"
,
v
,
com
)
s2
.
projectToCOM
(
com
)
v
(
s2
.
q
())
q3
=
q_init
[::]
q3
[
20
]
=
1.2
s3_0
=
State
(
fullBody
,
q
=
q3
,
limbsIncontact
=
[
lLegId
,
rLegId
])
s3
,
success
=
StateHelper
.
addNewContact
(
s3_0
,
rArmId
,
p
,[
0
,
0
,
1
])
assert
(
success
)
com
=
s3
.
getCenterOfMass
()
#com[0] += 0.03
com
[
1
]
-=
0.1
com
[
2
]
-=
0.02
moveSphere
(
"com"
,
v
,
com
)
s3
.
projectToCOM
(
com
)
v
(
s3
.
q
())
"""
jointsName = [rFoot,lFoot,rHand]
contactPos = []
contactNormal = []
pn = s1.getCenterOfContactForLimb(rLegId)
contactPos += [pn[0]]
contactNormal += [pn[1]]
pn = s1.getCenterOfContactForLimb(lLegId)
contactPos += [pn[0]]
contactNormal += [pn[1]]
pn = s1.getCenterOfContactForLimb(rArmId)
contactPos += [pn[0]]
contactNormal += [pn[1]]
ps.client.problem.createStaticStabilityConstraint ('staticStability',
jointsName, contactPos, contactNormal,'root_joint')
"""
q2
=
q_ref
[::]
script/scenarios/demos/talos_flatGround.py
0 → 100644
View file @
7947aa71
from
hpp.corbaserver.rbprm.rbprmfullbody
import
FullBody
from
hpp.gepetto
import
Viewer
from
display_tools
import
*
import
time
from
hpp.corbaserver.rbprm.rbprmstate
import
State
,
StateHelper
print
"Plan guide trajectory ..."
import
talos_flatGround_path
as
tp
print
"Done."
import
time
##
# Information to retrieve urdf and srdf files.
packageName
=
"talos_data"
meshPackageName
=
"talos_data"
rootJointType
=
"freeflyer"
urdfName
=
"talos"
urdfSuffix
=
"_reduced"
srdfSuffix
=
""
rLegId
=
'talos_rleg_rom'
rleg
=
'leg_right_1_joint'
rfoot
=
'leg_right_6_joint'
lLegId
=
'talos_lleg_rom'
lleg
=
'leg_left_1_joint'
lfoot
=
'leg_left_6_joint'
rArmId
=
'talos_rarm_rom'
rarm
=
'arm_right_1_joint'
rhand
=
'arm_right_7_joint'
lArmId
=
'talos_larm_rom'
larm
=
'arm_left_1_joint'
lhand
=
'arm_left_7_joint'
pId
=
tp
.
ps
.
numberPaths
()
-
1
fullBody
=
FullBody
()
fullBody
.
loadFullBodyModel
(
urdfName
,
rootJointType
,
meshPackageName
,
packageName
,
urdfSuffix
,
srdfSuffix
)
fullBody
.
setJointBounds
(
"root_joint"
,
[
-
5
,
5
,
-
1.5
,
1.5
,
0.95
,
1.05
])
fullBody
.
client
.
robot
.
setDimensionExtraConfigSpace
(
tp
.
extraDof
)
fullBody
.
client
.
robot
.
setExtraConfigSpaceBounds
([
-
tp
.
vMax
,
tp
.
vMax
,
-
tp
.
vMax
,
tp
.
vMax
,
0
,
0
,
-
tp
.
aMax
,
tp
.
aMax
,
-
tp
.
aMax
,
tp
.
aMax
,
0
,
0
])
ps
=
tp
.
ProblemSolver
(
fullBody
)
ps
.
setParameter
(
"Kinodynamic/velocityBound"
,
tp
.
vMax
)
ps
.
setParameter
(
"Kinodynamic/accelerationBound"
,
tp
.
aMax
)
v
=
tp
.
Viewer
(
ps
,
viewerClient
=
tp
.
v
.
client
,
displayCoM
=
True
)
q_ref
=
[
0.0
,
0.0
,
1.0232773
,
0.0
,
0.0
,
0.0
,
1.
,
#Free flyer
0.0
,
0.0
,
-
0.411354
,
0.859395
,
-
0.448041
,
-
0.001708
,
#Left Leg
0.0
,
0.0
,
-
0.411354
,
0.859395
,
-
0.448041
,
-
0.001708
,
#Right Leg
0.0
,
0.006761
,
#Chest
0.25847
,
0.173046
,
-
0.0002
,
-
0.525366
,
0.0
,
-
0.0
,
0.1
,
-
0.005
,
#Left Arm
-
0.25847
,
-
0.173046
,
0.0002
,
-
0.525366
,
0.0
,
0.0
,
0.1
,
-
0.005
,
#Right Arm
0.
,
0.
,
#Head
0
,
0
,
0
,
0
,
0
,
0
];
v
(
q_ref
)
q_init
=
q_ref
[::]
fullBody
.
setReferenceConfig
(
q_ref
)
fullBody
.
setCurrentConfig
(
q_init
)
print
"Generate limb DB ..."
tStart
=
time
.
time
()
# generate databases :
nbSamples
=
10000
rLegOffset
=
[
0.
,
-
0.00018
,
-
0.107
]
rLegOffset
[
2
]
+=
0.006
rLegNormal
=
[
0
,
0
,
1
]
rLegx
=
0.1
;
rLegy
=
0.06
fullBody
.
addLimb
(
rLegId
,
rleg
,
rfoot
,
rLegOffset
,
rLegNormal
,
rLegx
,
rLegy
,
nbSamples
,
"fixedStep08"
,
0.01
)
fullBody
.
runLimbSampleAnalysis
(
rLegId
,
"ReferenceConfiguration"
,
True
)
#fullBody.saveLimbDatabase(rLegId, "./db/talos_rLeg_walk.db")
lLegOffset
=
[
0.
,
-
0.00018
,
-
0.107
]
lLegOffset
[
2
]
+=
0.006
lLegNormal
=
[
0
,
0
,
1
]
lLegx
=
0.1
;
lLegy
=
0.06
fullBody
.
addLimb
(
lLegId
,
lleg
,
lfoot
,
lLegOffset
,
rLegNormal
,
lLegx
,
lLegy
,
nbSamples
,
"fixedStep08"
,
0.01
)
fullBody
.
runLimbSampleAnalysis
(
lLegId
,
"ReferenceConfiguration"
,
True
)
#fullBody.saveLimbDatabase(rLegId, "./db/talos_lLeg_walk.db")
tGenerate
=
time
.
time
()
-
tStart
print
"Done."
print
"Databases generated in : "
+
str
(
tGenerate
)
+
" s"
q_0
=
fullBody
.
getCurrentConfig
();
configSize
=
fullBody
.
getConfigSize
()
-
fullBody
.
client
.
robot
.
getDimensionExtraConfigSpace
()
q_init
[
0
:
7
]
=
tp
.
ps
.
configAtParam
(
pId
,
0.01
)[
0
:
7
]
# use this to get the correct orientation
q_goal
=
q_init
[::];
q_goal
[
0
:
7
]
=
tp
.
ps
.
configAtParam
(
pId
,
tp
.
ps
.
pathLength
(
pId
))[
0
:
7
]
dir_init
=
tp
.
ps
.
configAtParam
(
pId
,
0.01
)[
tp
.
indexECS
:
tp
.
indexECS
+
3
]
acc_init
=
tp
.
ps
.
configAtParam
(
pId
,
0.01
)[
tp
.
indexECS
+
3
:
tp
.
indexECS
+
6
]
dir_goal
=
tp
.
ps
.
configAtParam
(
pId
,
tp
.
ps
.
pathLength
(
pId
)
-
0.01
)[
tp
.
indexECS
:
tp
.
indexECS
+
3
]
acc_goal
=
[
0
,
0
,
0
]
robTreshold
=
3
# copy extraconfig for start and init configurations
q_init
[
configSize
:
configSize
+
3
]
=
dir_init
[::]
q_init
[
configSize
+
3
:
configSize
+
6
]
=
acc_init
[::]
q_goal
[
configSize
:
configSize
+
3
]
=
dir_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
)
fullBody
.
setCurrentConfig
(
q_goal
)
v
(
q_goal
)
# specifying the full body configurations as start and goal state of the problem
v
.
addLandmark
(
'talos/base_link'
,
0.3
)
v
(
q_init
)
fullBody
.
setStartState
(
q_init
,[
rLegId
,
lLegId
])
fullBody
.
setEndState
(
q_goal
,[
rLegId
,
lLegId
])
from
hpp.gepetto
import
PathPlayer
pp
=
PathPlayer
(
v
)
print
"Generate contact plan ..."
tStart
=
time
.
time
()
configs
=
fullBody
.
interpolate
(
0.01
,
pathId
=
pId
,
robustnessTreshold
=
2
,
filterStates
=
False
)
tInterpolateConfigs
=
time
.
time
()
-
tStart
print
"Done."
print
"number of configs :"
,
len
(
configs
)
raw_input
(
"Press Enter to display the contact sequence ..."
)
displayContactSequence
(
v
,
configs
)
script/scenarios/demos/talos_flatGround_path.py
0 → 100644
View file @
7947aa71
from
hpp.corbaserver.rbprm.rbprmbuilder
import
Builder
from
hpp.gepetto
import
Viewer
from
hpp.corbaserver
import
Client
from
hpp.corbaserver
import
ProblemSolver
import
time
rootJointType
=
'freeflyer'
packageName
=
'talos-rbprm'
meshPackageName
=
'talos-rbprm'
urdfName
=
'talos_trunk'
urdfNameRom
=
[
'talos_larm_rom'
,
'talos_rarm_rom'
,
'talos_lleg_rom'
,
'talos_rleg_rom'
]
urdfSuffix
=
""
srdfSuffix
=
""
vMax
=
0.3
aMax
=
0.1
extraDof
=
6
mu
=
0.5
# Creating an instance of the helper class, and loading the robot
rbprmBuilder
=
Builder
()
rbprmBuilder
.
loadModel
(
urdfName
,
urdfNameRom
,
rootJointType
,
meshPackageName
,
packageName
,
urdfSuffix
,
srdfSuffix
)
rbprmBuilder
.
setJointBounds
(
"root_joint"
,
[
-
5
,
5
,
-
1.5
,
1.5
,
0.95
,
1.05
])
# The following lines set constraint on the valid configurations:
# a configuration is valid only if all limbs can create a contact ...
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
])
rbprmBuilder
.
client
.
robot
.
setDimensionExtraConfigSpace
(
extraDof
)
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 and the viewer
ps
=
ProblemSolver
(
rbprmBuilder
)
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
(
"ConfigurationShooter/sampleExtraDOF"
,
False
)
p_lLeg
=
[
-
0.008846952891378526
,
0.0848172440888579
,
-
1.019272022956703
]
p_lLeg
[
0
]
=
0.
# assure symetry of dynamic constraints on flat ground
p_rLeg
=
[
-
0.008846952891378526
,
-
0.0848172440888579
,
-
1.019272022956703
]
p_rLeg
[
0
]
=
0.
p_lArm
=
[
0.13028765672452458
,
0.44360498616312666
,
-
0.2881211563246389
]
p_rArm
=
[
0.13028765672452458
,
-
0.44360498616312666
,
-
0.2881211563246389
]
rbprmBuilder
.
setReferenceEndEffector
(
'talos_lleg_rom'
,
p_lLeg
)
rbprmBuilder
.
setReferenceEndEffector
(
'talos_rleg_rom'
,
p_rLeg
)
from
hpp.gepetto
import
ViewerFactory
vf
=
ViewerFactory
(
ps
)
from
hpp.corbaserver.affordance.affordance
import
AffordanceTool
afftool
=
AffordanceTool
()
afftool
.
setAffordanceConfig
(
'Support'
,
[
0.5
,
0.03
,
0.00005
])
afftool
.
loadObstacleModel
(
"hpp-rbprm-corba"
,
"ground"
,
"planning"
,
vf
)
v
=
vf
.
createViewer
(
displayArrows
=
True
)
afftool
.
visualiseAffordances
(
'Support'
,
v
,
v
.
color
.
lightBrown
)
# Setting initial configuration
q_init
=
rbprmBuilder
.
getCurrentConfig
();
q_init
[
3
:
7
]
=
[
0
,
0
,
0
,
1
]
q_init
[
0
:
3
]
=
[
0
,
0
,
1.
]
v
(
q_init
)
# set goal config
rbprmBuilder
.
setCurrentConfig
(
q_init
)
q_goal
=
q_init
[::]
q_goal
[
0
]
=
1.5
v
(
q_goal
)
ps
.
setInitialConfig
(
q_init
)
ps
.
addGoalConfig
(
q_goal
)
# Choosing RBPRM shooter and path validation methods.
ps
.
selectConfigurationShooter
(
"RbprmShooter"
)
ps
.
addPathOptimizer
(
"RandomShortcutDynamic"
)
ps
.
selectPathValidation
(
"RbprmPathValidation"
,
0.05
)
# Choosing kinodynamic methods :
ps
.
selectSteeringMethod
(
"RBPRMKinodynamic"
)
ps
.
selectDistance
(
"Kinodynamic"
)
ps
.
selectPathPlanner
(
"DynamicPlanner"
)
t
=
ps
.
solve
()
print
"Guide planning time : "
,
t
# display solution :
from
hpp.gepetto
import
PathPlayer
pp
=
PathPlayer
(
v
)
pp
.
dt
=
0.03
pp
.
displayVelocityPath
(
0
)
v
.
client
.
gui
.
setVisibility
(
"path_0_root"
,
"ALWAYS_ON_TOP"
)
pp
(
0
)
q_far
=
q_init
[::]
q_far
[
2
]
=
-
2
v
(
q_far
)
script/scenarios/demos/talos_navBauzil.py
0 → 100644
View file @
7947aa71
from
hpp.corbaserver.rbprm.rbprmfullbody
import
FullBody
from
hpp.gepetto
import
Viewer
from
display_tools
import
*
import
time
from
hpp.corbaserver.rbprm.rbprmstate
import
State
,
StateHelper
print
"Plan guide trajectory ..."
import
talos_navBauzil_path
as
tp
print
"Done."
import
time
##
# Information to retrieve urdf and srdf files.
packageName
=
"talos_data"
meshPackageName
=
"talos_data"
rootJointType
=
"freeflyer"
urdfName
=
"talos"
urdfSuffix
=
"_reduced"
srdfSuffix
=
""
rLegId
=
'talos_rleg_rom'
rleg
=
'leg_right_1_joint'
rfoot
=
'leg_right_6_joint'
lLegId
=
'talos_lleg_rom'
lleg
=
'leg_left_1_joint'
lfoot
=
'leg_left_6_joint'
rArmId
=
'talos_rarm_rom'
rarm
=
'arm_right_1_joint'
rhand
=
'arm_right_7_joint'
lArmId
=
'talos_larm_rom'
larm
=
'arm_left_1_joint'
lhand
=
'arm_left_7_joint'
pId
=
tp
.
ps
.
numberPaths
()
-
1
fullBody
=
FullBody
()
fullBody
.
loadFullBodyModel
(
urdfName
,
rootJointType
,
meshPackageName
,
packageName
,
urdfSuffix
,
srdfSuffix
)
root_bounds
=
tp
.
root_bounds
[::]
root_bounds
[
0
]
-=
0.2
root_bounds
[
1
]
+=
0.2
root_bounds
[
2
]
-=
0.2
root_bounds
[
3
]
+=
0.2
root_bounds
[
-
1
]
=
1.2
root_bounds
[
-
2
]
=
0.8
fullBody
.
setJointBounds
(
"root_joint"
,
root_bounds
)
fullBody
.
client
.
robot
.
setDimensionExtraConfigSpace
(
tp
.
extraDof
)
fullBody
.
client
.
robot
.
setExtraConfigSpaceBounds
([
-
tp
.
vMax
,
tp
.
vMax
,
-
tp
.
vMax
,
tp
.
vMax
,
0
,
0
,
-
tp
.
aMax
,
tp
.
aMax
,
-
tp
.
aMax
,
tp
.
aMax
,
0
,
0
])
ps
=
tp
.
ProblemSolver
(
fullBody
)
ps
.
setParameter
(
"Kinodynamic/velocityBound"
,
tp
.
vMax
)
ps
.
setParameter
(
"Kinodynamic/accelerationBound"
,
tp
.
aMax
)
v
=
tp
.
Viewer
(
ps
,
viewerClient
=
tp
.
v
.
client
,
displayCoM
=
True
)
q_ref
=
[
0.0
,
0.0
,
1.0232773
,
0.0
,
0.0
,
0.0
,
1.
,
#Free flyer
0.0
,
0.0
,
-
0.411354
,
0.859395
,
-
0.448041
,
-
0.001708
,
#Left Leg
0.0
,
0.0
,
-
0.411354
,
0.859395
,
-
0.448041
,
-
0.001708
,
#Right Leg
0.0
,
0.006761
,
#Chest
0.25847
,
0.173046
,
-
0.0002
,
-
0.525366
,
0.0
,
-
0.0
,
0.1
,
-
0.005
,
#Left Arm
-
0.25847
,
-
0.173046
,
0.0002
,
-
0.525366
,
0.0
,
0.0
,
0.1
,
-
0.005
,
#Right Arm
0.
,
0.
,
#Head
0
,
0
,
0
,
0
,
0
,
0
];
v
(
q_ref
)
q_init
=
q_ref
[::]
fullBody
.
setReferenceConfig
(
q_ref
)
fullBody
.
setCurrentConfig
(
q_init
)
print
"Generate limb DB ..."
tStart
=
time
.
time
()
# generate databases :
nbSamples
=
10000
rLegOffset
=
[
0.
,
-
0.00018
,
-
0.107
]
rLegOffset
[
2
]
+=
0.006
rLegNormal
=
[
0
,
0
,
1
]
rLegx
=
0.1
;
rLegy
=
0.06
fullBody
.
addLimb
(
rLegId
,
rleg
,
rfoot
,
rLegOffset
,
rLegNormal
,
rLegx
,
rLegy
,
nbSamples
,
"fixedStep08"
,
0.01
)
fullBody
.
runLimbSampleAnalysis
(
rLegId
,
"ReferenceConfiguration"
,
True
)
#fullBody.saveLimbDatabase(rLegId, "./db/talos_rLeg_walk.db")
lLegOffset
=
[
0.
,
-
0.00018
,
-
0.107
]
lLegOffset
[
2
]
+=
0.006
lLegNormal
=
[
0
,
0
,
1
]
lLegx
=
0.1
;
lLegy
=
0.06
fullBody
.
addLimb
(
lLegId
,
lleg
,
lfoot
,
lLegOffset
,
rLegNormal
,
lLegx
,
lLegy
,
nbSamples
,
"fixedStep08"
,
0.01
)
fullBody
.
runLimbSampleAnalysis
(
lLegId
,
"ReferenceConfiguration"
,
True
)
#fullBody.saveLimbDatabase(rLegId, "./db/talos_lLeg_walk.db")