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
39e5a2a0
Commit
39e5a2a0
authored
Mar 30, 2018
by
Pierre Fernbach
Browse files
update flatGround pyrene scripts
parent
b916d5da
Changes
2
Hide whitespace changes
Inline
Side-by-side
script/dynamic/talos/flatGround_pyrene_interp.py
View file @
39e5a2a0
...
...
@@ -7,18 +7,13 @@ from hpp.corbaserver.rbprm.rbprmstate import State,StateHelper
#from display_tools import *
import
talos.flatGround_pyrene_pathKino
as
tp
import
time
from
planning.robot_config.talos
import
*
tPlanning
=
tp
.
tPlanning
packageName
=
"talos_description"
meshPackageName
=
"talos_description"
rootJointType
=
"freeflyer"
##
# Information to retrieve urdf and srdf files.
urdfName
=
"talos"
urdfSuffix
=
"_full_v2"
srdfSuffix
=
""
pId
=
tp
.
ps
.
numberPaths
()
-
1
fullBody
=
FullBody
()
...
...
@@ -33,20 +28,18 @@ r = tp.Viewer (ps,viewerClient=tp.r.client, displayCoM = True)
q_ref
=
[
0
,
0
,
1.0192720229567027
,
1
,
0
,
0
,
0
,
# root_joint
0.0
,
0.0
,
-
0.411354
,
0.859395
,
-
0.448041
,
-
0.001708
,
# leg_left
0.0
,
0.0
,
-
0.411354
,
0.859395
,
-
0.448041
,
-
0.001708
,
# leg_right
0
,
0.006761
,
# torso
0.25847
,
0.173046
,
-
0.0002
,
-
0.525366
,
0
,
0
,
0.1
,
# arm_left
0
,
0
,
0
,
0
,
0
,
0
,
0
,
# gripper_left
-
0.25847
,
-
0.173046
,
0.0002
,
-
0.525366
,
0
,
0
,
0.1
,
# arm_right
0
,
0
,
0
,
0
,
0
,
0
,
0
,
# gripper_right
0
,
0
,
# head
0.0
,
0.0
,
1.0232773
,
1
,
0.0
,
0.0
,
0.0
,
#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
];
r
(
q_ref
)
q_init
=
q_ref
[::]
#
fullBody.setReferenceConfig(q_ref)
fullBody
.
setReferenceConfig
(
q_ref
)
"""
#test correspondance with reduced :
q_init[19] = -0.5
...
...
@@ -59,38 +52,33 @@ qfar=q_ref[::]
qfar
[
2
]
=
-
5
tStart
=
time
.
time
()
# generate databases :
nbSamples
=
50000
rLegId
=
'rleg'
rLeg
=
'leg_right_1_joint'
rfoot
=
'leg_right_sole_fix_joint'
rLegOffset
=
[
0
,
0
,
0
]
rLegOffset
=
MRsole_offset
.
translation
.
transpose
().
tolist
()[
0
]
rLegOffset
[
2
]
+=
0.006
rLegNormal
=
[
0
,
0
,
1
]
rLegx
=
0.1
;
rLegy
=
0.06
fullBody
.
addLimb
(
rLegId
,
rLeg
,
rfoot
,
rLegOffset
,
rLegNormal
,
rLegx
,
rLegy
,
nbSamples
,
"forward"
,
0.01
)
#fullBody.runLimbSampleAnalysis(rLegId, "ReferenceConfiguration", True)
fullBody
.
addLimb
(
rLegId
,
rleg
,
rfoot
,
rLegOffset
,
rLegNormal
,
rLegx
,
rLegy
,
nbSamples
,
"fixedStep06"
,
0.01
)
fullBody
.
runLimbSampleAnalysis
(
rLegId
,
"ReferenceConfiguration"
,
True
)
#fullBody.saveLimbDatabase(rLegId, "./db/talos_rLeg_walk.db")
lLegId
=
'lleg'
lLeg
=
'leg_left_1_joint'
lfoot
=
'leg_left_sole_fix_joint'
lLegOffset
=
[
0
,
0
,
0
]
lLegOffset
=
MLsole_offset
.
translation
.
transpose
().
tolist
()[
0
]
lLegOffset
[
2
]
+=
0.006
lLegNormal
=
[
0
,
0
,
1
]
lLegx
=
0.1
;
lLegy
=
0.06
fullBody
.
addLimb
(
lLegId
,
lLeg
,
lfoot
,
lLegOffset
,
rLegNormal
,
lLegx
,
lLegy
,
nbSamples
,
"forward"
,
0.01
)
#fullBody.runLimbSampleAnalysis(lLegId, "ReferenceConfiguration", True)
fullBody
.
addLimb
(
lLegId
,
lleg
,
lfoot
,
lLegOffset
,
rLegNormal
,
lLegx
,
lLegy
,
nbSamples
,
"fixedStep06"
,
0.01
)
fullBody
.
runLimbSampleAnalysis
(
lLegId
,
"ReferenceConfiguration"
,
True
)
#fullBody.saveLimbDatabase(rLegId, "./db/talos_lLeg_walk.db")
"""
rarmId = 'rarm'
rarm = 'arm_right_1_joint'
rHand = 'gripper_right_inner_single_joint'
rArmOffset = [0,0,0.1]
rArmNormal = [0,0,1]
rArmx = 0.02; rArmy = 0.02
fullBody.addLimb(rarmId,rarm,rHand,rArmOffset,rArmNormal, rArmx, rArmy, nbSamples, "EFORT", 0.01)
fullBody.runLimbSampleAnalysis(rarmId, "ReferenceConfiguration", True)
larmId = 'larm'
larm = 'arm_left_1_joint'
lHand = 'gripper_left_inner_single_joint'
lArmOffset = [0,0,-0.1]
lArmNormal = [0,0,1]
lArmx = 0.02; lArmy = 0.02
...
...
@@ -98,6 +86,11 @@ fullBody.addLimb(larmId,larm,lHand,lArmOffset,lArmNormal, lArmx, lArmy, nbSample
fullBody.runLimbSampleAnalysis(larmId, "ReferenceConfiguration", True)
"""
"""
# load databases from files :
fullBody.addLimbDatabase("./db/talos_rLeg_walk.db",rLegId,"fixedStep06")
fullBody.addLimbDatabase("./db/talos_lLeg_walk.db",lLegId,"fixedStep06")
"""
tGenerate
=
time
.
time
()
-
tStart
...
...
@@ -108,8 +101,6 @@ q_0 = fullBody.getCurrentConfig();
#~ fullBody.createOctreeBoxes(r.client.gui, 1, rarmId, q_0,)
configSize
=
fullBody
.
getConfigSize
()
-
fullBody
.
client
.
basic
.
robot
.
getDimensionExtraConfigSpace
()
q_init
[
0
:
7
]
=
tp
.
ps
.
configAtParam
(
pId
,
0.01
)[
0
:
7
]
# use this to get the correct orientation
...
...
@@ -126,8 +117,10 @@ 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
]
+
0.01
q_goal
[
2
]
=
q_ref
[
2
]
+
0.01
q_init
[
2
]
=
q_ref
[
2
]
q_goal
[
2
]
=
q_ref
[
2
]
fullBody
.
setStaticStability
(
True
)
# Randomly generating a contact configuration at q_init
...
...
@@ -153,23 +146,23 @@ fullBody.setEndState(q_goal,[rLegId,lLegId])
from
hpp.gepetto
import
PathPlayer
pp
=
PathPlayer
(
fullBody
.
client
.
basic
,
r
)
import
fullBodyPlayerHrp2
tStart
=
time
.
time
()
configsFull
=
fullBody
.
interpolate
(
0.
0
01
,
pathId
=
pId
,
robustnessTreshold
=
2
,
filterStates
=
False
)
configsFull
=
fullBody
.
interpolate
(
0.01
,
pathId
=
pId
,
robustnessTreshold
=
2
,
filterStates
=
False
)
tInterpolateConfigs
=
time
.
time
()
-
tStart
print
"number of configs :"
,
len
(
configsFull
)
from
planning.config
import
*
from
planning.configs.talos_flatGround
import
*
from
generate_contact_sequence
import
*
beginState
=
0
endState
=
len
(
configsFull
)
-
2
configs
=
configsFull
[
beginState
:
endState
+
1
]
cs
=
generateContactSequence
(
fullBody
,
configs
,
beginState
,
endState
,
r
,
curves_initGuess
=
curves_initGuess
,
timings_initGuess
=
timings_initGuess
)
cs
=
generateContactSequence
(
fullBody
,
configs
,
beginState
,
endState
,
r
)
#player.displayContactPlan()
...
...
script/dynamic/talos/flatGround_pyrene_pathKino.py
View file @
39e5a2a0
...
...
@@ -29,7 +29,7 @@ urdfName = 'talos_trunk'
urdfNameRom
=
[
'talos_larm_rom'
,
'talos_rarm_rom'
,
'talos_lleg_rom'
,
'talos_rleg_rom'
]
urdfSuffix
=
""
srdfSuffix
=
""
vMax
=
omniORB
.
any
.
to_any
(
0.
2
);
vMax
=
omniORB
.
any
.
to_any
(
0.
3
);
aMax
=
omniORB
.
any
.
to_any
(
0.1
);
#aMax = omniORB.any.to_any(0.3);
extraDof
=
6
...
...
@@ -72,7 +72,7 @@ afftool.loadObstacleModel (ENV_PACKAGE_NAME, ENV_NAME, ENV_PREFIX, r)
# Setting initial and goal configurations
q_init
=
rbprmBuilder
.
getCurrentConfig
();
q_init
[
3
:
7
]
=
[
1
,
0
,
0
,
0
]
q_init
[
0
:
3
]
=
[
0
,
0
,
0.98
];
r
(
q_init
)
q_init
[
0
:
3
]
=
[
0
,
0
,
1.
];
r
(
q_init
)
"""
# test correspondance with fullBody :
...
...
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