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
1500977e
Commit
1500977e
authored
Jul 17, 2019
by
stevet
Browse files
fixed anymal slalom scripts + start state in rbprmFullBody
parent
85d46de9
Changes
5
Hide whitespace changes
Inline
Side-by-side
script/scenarios/sandbox/ANYmal/anymal_plinth_path.py
View file @
1500977e
...
...
@@ -28,7 +28,7 @@ for rom in rbprmBuilder.urdfNameRom :
rbprmBuilder
.
boundSO3
([
-
0.1
,
0.1
,
-
0.5
,
0.5
,
-
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
)
c
# We set the bounds of this extraDof with velocity and acceleration bounds (expect on z axis)
# We set the bounds of this extraDof with velocity and acceleration bounds (expect on z axis)
rbprmBuilder
.
client
.
robot
.
setExtraConfigSpaceBounds
([
-
vMax
,
vMax
,
-
vMax
,
vMax
,
-
vMax
,
vMax
,
-
aMax
,
aMax
,
-
aMax
,
aMax
,
-
aMaxZ
,
aMaxZ
])
indexECS
=
rbprmBuilder
.
getConfigSize
()
-
rbprmBuilder
.
client
.
robot
.
getDimensionExtraConfigSpace
()
...
...
@@ -39,7 +39,7 @@ ps.setParameter("Kinodynamic/velocityBound",vMax)
ps
.
setParameter
(
"Kinodynamic/accelerationBound"
,
aMax
)
ps
.
setParameter
(
"Kinodynamic/verticalAccelerationBound"
,
aMaxZ
)
# force the orientation of the trunk to match the direction of the motion
ps
.
setParameter
(
"Kinodynamic/forceOrientation"
,
True
)
ps
.
setParameter
(
"Kinodynamic/force
All
Orientation"
,
True
)
ps
.
setParameter
(
"DynamicPlanner/sizeFootX"
,
0.1
)
ps
.
setParameter
(
"DynamicPlanner/sizeFootY"
,
0.1
)
ps
.
setParameter
(
"DynamicPlanner/friction"
,
mu
)
...
...
script/scenarios/sandbox/ANYmal/anymal_slalom_debris.py
View file @
1500977e
...
...
@@ -77,8 +77,8 @@ v.addLandmark('anymal/base',0.3)
v
(
q_init
)
# specify the full body configurations as start and goal state of the problem
fullBody
.
setStartState
(
q_init
,
fullBody
.
limbs_names
)
fullBody
.
setEndState
(
q_goal
,
fullBody
.
limbs_names
)
fullBody
.
setStartState
(
q_init
,
fullBody
.
limbs_names
,
[[
0.
,
0.
,
1.
]
for
_
in
range
(
4
)]
)
fullBody
.
setEndState
(
q_goal
,
fullBody
.
limbs_names
,
[[
0.
,
0.
,
1.
]
for
_
in
range
(
4
)]
)
print
"Generate contact plan ..."
...
...
script/scenarios/sandbox/ANYmal/anymal_slalom_debris_path.py
View file @
1500977e
...
...
@@ -37,7 +37,7 @@ ps = ProblemSolver( rbprmBuilder )
ps
.
setParameter
(
"Kinodynamic/velocityBound"
,
vMax
)
ps
.
setParameter
(
"Kinodynamic/accelerationBound"
,
aMax
)
# force the orientation of the trunk to match the direction of the motion
ps
.
setParameter
(
"Kinodynamic/forceOrientation"
,
True
)
ps
.
setParameter
(
"Kinodynamic/force
All
Orientation"
,
True
)
ps
.
setParameter
(
"DynamicPlanner/sizeFootX"
,
0.01
)
ps
.
setParameter
(
"DynamicPlanner/sizeFootY"
,
0.01
)
ps
.
setParameter
(
"DynamicPlanner/friction"
,
mu
)
...
...
script/scenarios/sandbox/ANYmal/run.sh
0 → 100644
View file @
1500977e
#!/bin/bash
gepetto-gui &
hpp-rbprm-server &
ipython
-i
--no-confirm-exit
./
$1
pkill
-f
'gepetto-gui'
pkill
-f
'hpp-rbprm-server'
src/hpp/corbaserver/rbprm/rbprmfullbody.py
View file @
1500977e
...
...
@@ -285,7 +285,7 @@ class FullBody (Robot):
num_max_sample
=
1
for
(
limbName
,
normal
)
in
zip
(
contacts
,
normals
):
p
=
cl
.
getEffectorPosition
(
limbName
,
configuration
)[
0
]
cl
.
addNewContact
(
sId
,
limbName
,
p
,
normal
,
num_max_sample
)
cl
.
addNewContact
(
sId
,
limbName
,
p
,
normal
,
num_max_sample
,
True
)
return
cl
.
setStartStateId
(
sId
)
...
...
@@ -303,7 +303,7 @@ class FullBody (Robot):
num_max_sample
=
1
for
(
limbName
,
normal
)
in
zip
(
contacts
,
normals
):
p
=
cl
.
getEffectorPosition
(
limbName
,
configuration
)[
0
]
cl
.
addNewContact
(
sId
,
limbName
,
p
,
normal
,
num_max_sample
)
cl
.
addNewContact
(
sId
,
limbName
,
p
,
normal
,
num_max_sample
,
True
)
return
cl
.
setEndStateId
(
sId
)
## Initialize the first state of the path interpolation
...
...
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