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
538ca47d
Commit
538ca47d
authored
Jun 21, 2018
by
Pierre Fernbach
Browse files
[script] update slalom script
parent
86155073
Changes
3
Hide whitespace changes
Inline
Side-by-side
script/dynamic/slalom_bauzil_hrp2_interStatic.py
View file @
538ca47d
...
...
@@ -7,7 +7,7 @@ import time
import
omniORB.any
from
constraint_to_dae
import
*
from
display_tools
import
*
from
configs.s
tairs10_bauzil_stairs
import
*
from
configs.s
lalom_bauzil
import
*
from
disp_bezier
import
*
from
hpp.corbaserver.rbprm.rbprmstate
import
State
,
StateHelper
...
...
@@ -24,7 +24,7 @@ fullBody = FullBody ()
tPlanning
=
0.
fullBody
.
loadFullBodyModel
(
urdfName
,
rootJointType
,
meshPackageName
,
packageName
,
urdfSuffix
,
srdfSuffix
)
fullBody
.
setJointBounds
(
"base_joint_xyz"
,
[
-
1
,
2.5
,
0.5
,
3
,
0.
5
,
0.
7
])
fullBody
.
setJointBounds
(
"base_joint_xyz"
,
[
-
1
,
2.5
,
0.5
,
3
,
0.
4
,
0.
8
])
fullBody
.
setJointBounds
(
"LLEG_JOINT3"
,
[
0.35
,
2.61799
])
fullBody
.
setJointBounds
(
"RLEG_JOINT3"
,
[
0.35
,
2.61799
])
...
...
@@ -55,7 +55,7 @@ rLegLimbOffset=[0,0,-0.035]#0.035
rLegNormal
=
[
0
,
0
,
1
]
rLegx
=
0.09
;
rLegy
=
0.05
#fullBody.addLimbDatabase("./db/hrp2_rleg_db.db",rLegId,"forward")
fullBody
.
addLimb
(
rLegId
,
rleg
,
''
,
rLegOffset
,
rLegNormal
,
rLegx
,
rLegy
,
100000
,
"fixedStep06"
,
0.01
,
"_6_DOF"
,
kinematicConstraintsPath
=
"package://hpp-rbprm-corba/com_inequalities/full
s
ize/RLEG_JOINT0_com_constraints.obj"
,
limbOffset
=
rLegLimbOffset
,
kinematicConstraintsMin
=
0.
6
)
fullBody
.
addLimb
(
rLegId
,
rleg
,
''
,
rLegOffset
,
rLegNormal
,
rLegx
,
rLegy
,
100000
,
"fixedStep06"
,
0.01
,
"_6_DOF"
,
kinematicConstraintsPath
=
"package://hpp-rbprm-corba/com_inequalities/full
S
ize/RLEG_JOINT0_com_constraints.obj"
,
limbOffset
=
rLegLimbOffset
,
kinematicConstraintsMin
=
0.
4
)
fullBody
.
runLimbSampleAnalysis
(
rLegId
,
"ReferenceConfiguration"
,
True
)
#fullBody.saveLimbDatabase(rLegId, "./db/hrp2_rleg_db.db")
...
...
@@ -65,7 +65,7 @@ lLegLimbOffset=[0,0,0.035]
lLegNormal
=
[
0
,
0
,
1
]
lLegx
=
0.09
;
lLegy
=
0.05
#fullBody.addLimbDatabase("./db/hrp2_lleg_db.db",lLegId,"forward")
fullBody
.
addLimb
(
lLegId
,
lleg
,
''
,
lLegOffset
,
rLegNormal
,
lLegx
,
lLegy
,
100000
,
"fixedStep06"
,
0.01
,
"_6_DOF"
,
kinematicConstraintsPath
=
"package://hpp-rbprm-corba/com_inequalities/full
s
ize/LLEG_JOINT0_com_constraints.obj"
,
limbOffset
=
lLegLimbOffset
,
kinematicConstraintsMin
=
0.
6
)
fullBody
.
addLimb
(
lLegId
,
lleg
,
''
,
lLegOffset
,
rLegNormal
,
lLegx
,
lLegy
,
100000
,
"fixedStep06"
,
0.01
,
"_6_DOF"
,
kinematicConstraintsPath
=
"package://hpp-rbprm-corba/com_inequalities/full
S
ize/LLEG_JOINT0_com_constraints.obj"
,
limbOffset
=
lLegLimbOffset
,
kinematicConstraintsMin
=
0.
4
)
fullBody
.
runLimbSampleAnalysis
(
lLegId
,
"ReferenceConfiguration"
,
True
)
#fullBody.saveLimbDatabase(lLegId, "./db/hrp2_lleg_db.db")
...
...
@@ -99,7 +99,7 @@ acc_init = [0,0,0]
dir_goal
=
tp
.
ps
.
configAtParam
(
pId
,
eps
)[
tp
.
indexECS
:
tp
.
indexECS
+
3
]
acc_goal
=
[
0
,
0
,
0
]
robTreshold
=
3
robTreshold
=
1
# copy extraconfig for start and init configurations
q_init
[
configSize
:
configSize
+
3
]
=
dir_init
[::]
q_init
[
configSize
+
3
:
configSize
+
6
]
=
acc_init
[::]
...
...
@@ -169,7 +169,7 @@ import check_qp
#cs = generateContactSequence(fullBody,configs,beginState, endState,curves_initGuess =curves_initGuess , timings_initGuess =timings_initGuess)
#
displayContactSequence(r,configsFull)
displayContactSequence
(
r
,
configsFull
)
filename
=
OUTPUT_DIR
+
"/"
+
OUTPUT_SEQUENCE_FILE
...
...
script/dynamic/slalom_bauzil_hrp2_pathKino.py
View file @
538ca47d
...
...
@@ -51,6 +51,7 @@ vMax = 0.2;
aMax
=
0.1
;
extraDof
=
6
#rbprmBuilder.setJointBounds ("base_joint_xyz", [-1,2.5,0.5 ,3, 0.6, 0.6])
rbprmBuilder
.
setJointBounds
(
"base_joint_xyz"
,
[
-
1
,
2.5
,
0.5
,
3
,
0.6
,
0.6
])
rbprmBuilder
.
setJointBounds
(
'CHEST_JOINT0'
,[
-
0.05
,
0.05
])
rbprmBuilder
.
setJointBounds
(
'CHEST_JOINT1'
,[
-
0.05
,
0.05
])
...
...
@@ -86,10 +87,10 @@ r.addLandmark(r.sceneName,1)
# Setting initial and goal configurations
q_init
=
rbprmBuilder
.
getCurrentConfig
();
q_init
[
3
:
7
]
=
[
1
,
0
,
0
,
0
]
#
q_init [0:3] = [-0.9, 1, 0.6]; r (q_init)
q_init
[
0
:
3
]
=
[
0.5
,
2.5
,
0.6
];
r
(
q_init
)
q_init
[
0
:
3
]
=
[
-
0.9
,
1
,
0.6
];
r
(
q_init
)
#
q_init [0:3] = [0.5, 2.5, 0.6]; r (q_init)
q_init
[
-
6
:
-
3
]
=
[
0.05
,
0
,
0
]
#q_init [0:3] = [0.05, 0.86, 0.59]; r (q_init)
rbprmBuilder
.
setCurrentConfig
(
q_init
)
q_goal
=
q_init
[::]
...
...
script/dynamic/slalom_hrp2_pathKino.py
View file @
538ca47d
...
...
@@ -6,7 +6,7 @@ from hpp.gepetto import Viewer
import
time
import
math
import
omniORB.any
from
planning.config
import
*
from
configs.slalom_bauzil
import
*
from
hpp.corbaserver
import
Client
from
hpp.corbaserver.robot
import
Robot
as
Parent
...
...
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