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
b1d39ed9
Commit
b1d39ed9
authored
Aug 29, 2019
by
stevet
Committed by
Pierre Fernbach
Sep 20, 2019
Browse files
anymal box scenario functional
parent
16ea2bf6
Changes
7
Hide whitespace changes
Inline
Side-by-side
script/scenarios/sandbox/anymal_one_step/anymal_box.py
0 → 100644
View file @
b1d39ed9
import
setup_one_step
as
sos
fsp
=
sos
.
fewStepPlanner
#planner instance
q_init
=
sos
.
q_init
#initial configuration
initState
=
sos
.
initState
#initial state.
viewer
=
sos
.
v
### Go somewhere
n_goal
=
q_init
[:
7
][:]
n_goal
[
0
]
+=
.
5
n_goal
[
1
]
+=
0
#~ n_goal[2] = [0.,0.,0.7071,0.7071]
n_goal
[
3
:
7
]
=
[
0.
,
0.
,
0.7071
,
0.7071
]
#~ sos.fullBody.toggleNonContactingLimb(sos.fullBody.prongFrontId)
states
,
configs
=
fsp
.
goToQuasiStatic
(
initState
,
n_goal
,
stepsize
=
0.001
,
displayGuidePath
=
False
,
erasePreviousStates
=
True
)
#~ pId = fsp.guidePath(initState.q()[:7], n_goal, displayPath = True)
#~ fsp.setPlanningContext()
#~ fsp.pathPlayer(pId)
#equivalent to
# fsp.goToQuasiStatic(self, initState, n_goal, stepsize = 0.002, goalLimbsInContact = None, goalNormals = None, displayGuidePath = False)
#display computed States:
#~ n_goal[3:7] = [0.,0.,0.7071,0.7071]
#~ s = states[-1] #last state computed
#~ states, configs = fsp.goToQuasiStatic(s,n_goal, displayGuidePath = False, erasePreviousStates = True, stepsize = 0.001)
#display configuration
#~ viewer(s.q())
#~ states+=states2
#~ configs+=configs2
configs
=
[
q
+
[
0.
for
_
in
range
(
6
)]
for
q
in
configs
]
fullBody
=
sos
.
fullBody
v
=
viewer
#~ sos.dispContactPlan(states,0.051) #2nd argument is frame rateue
#some helpers:
#~ s.q() # configuration associated to state
#~ initState. # configuration associated to state
#~ from hpp.corbaserver.rbprm import rbprmstate
#~ target = sos.fullBody.getJointPosition(sos.fullBody.prongFrontId)[:3]
#~ target[2] = 0.
#~ s = rbprmstate.StateHelper.cloneState(states[-1])[0]
#~ fb = sos.fullBody
#~ sos.fullBody.toggleNonContactingLimb(sos.fullBody.prongFrontId)
#~ rbprmstate.StateHelper.addNewContact(s,sos.fullBody.prongFrontId,target,[0.,0.,1.])
fullBody
.
resetJointsBounds
()
script/scenarios/sandbox/anymal_one_step/anymal_box.sh
0 → 100644
View file @
b1d39ed9
#! /bin/bash
gepetto-gui &
hpp-rbprm-server &
cp
*
.py /media/data/dev/linux/hpp/src/hpp-rbprm-corba/script/scenarios/memmo
#~ ipython -i --no-confirm-exit $DEVEL_HPP_DIR/src/multicontact-locomotion-planning/scripts/run_mlp.py talos_circle
ipython
-i
--no-confirm-exit
$DEVEL_HPP_DIR
/src/multicontact-locomotion-planning/scripts/run_mlp.py anymal_box.py
pkill
-f
'gepetto-gui'
pkill
-f
'hpp-rbprm-server'
script/scenarios/sandbox/anymal_one_step/config.py
View file @
b1d39ed9
SCENE
=
"multicontact/
ground
"
SCENE
=
"multicontact/
anymal_box
"
INIT_CONFIG_ROOT
=
[
0
,
0
,
0.465
,
0
,
0
,
0
,
1
]
INIT_CONFIG_WB
=
None
ROOT_BOUNDS
=
[
-
20
,
20
,
-
20
,
20
,
0.1
,
0.6
]
script/scenarios/sandbox/anymal_one_step/demo.py
View file @
b1d39ed9
...
...
@@ -9,24 +9,36 @@ viewer = sos.v
### Go somewhere
n_goal
=
q_init
[:
7
][:]
n_goal
[
0
]
+=
2
n_goal
[
1
]
+=
1
n_goal
[
3
:
7
]
=
[
0.
,
0.
,
0.7071
,
0.7071
]
n_goal
[
0
]
+=
3
n_goal
[
1
]
+=
0
#~ n_goal[2] = [0.,0.,0.7071,0.7071]
#~ n_goal[3:7] = [0.,0.,0.7071,0.7071]
#~ sos.fullBody.toggleNonContactingLimb(sos.fullBody.prongFrontId)
states
,
configs
=
fsp
.
goToQuasiStatic
(
initState
,
n_goal
,
displayGuidePath
=
True
)
states
,
configs
=
fsp
.
goToQuasiStatic
(
initState
,
n_goal
,
displayGuidePath
=
False
)
#~ pId = fsp.guidePath(initState.q()[:7], n_goal, displayPath = True)
#~ fsp.setPlanningContext()
#~ fsp.pathPlayer(pId)
#equivalent to
# fsp.goToQuasiStatic(self, initState, n_goal, stepsize = 0.002, goalLimbsInContact = None, goalNormals = None, displayGuidePath = False)
#display computed States:
#~ sos.dispContactPlan(states,0.051) #2nd argument is frame rateue
n_goal
[
3
:
7
]
=
[
0.
,
0.
,
0.7071
,
0.7071
]
s
=
states
[
-
1
]
#last state computed
states2
,
configs2
=
fsp
.
goToQuasiStatic
(
s
,
n_goal
,
displayGuidePath
=
False
)
#display configuration
viewer
(
s
.
q
())
states
+=
states2
configs
+=
configs2
configs
=
[
q
+
[
0.
for
_
in
range
(
6
)]
for
q
in
configs
]
sos
.
dispContactPlan
(
states
,
0.051
)
#2nd argument is frame rateue
#some helpers:
s
.
q
()
# configuration associated to state
#~ initState. # configuration associated to state
...
...
src/CMakeLists.txt
View file @
b1d39ed9
...
...
@@ -85,6 +85,7 @@ INSTALL(
${
CMAKE_CURRENT_SOURCE_DIR
}
/hpp/corbaserver/rbprm/rbprmfullbody.py
${
CMAKE_CURRENT_SOURCE_DIR
}
/hpp/corbaserver/rbprm/rbprmstate.py
${
CMAKE_CURRENT_SOURCE_DIR
}
/hpp/corbaserver/rbprm/state_alg.py
${
CMAKE_CURRENT_SOURCE_DIR
}
/hpp/corbaserver/rbprm/fewstepsplanner.py
DESTINATION
${
PYTHON_SITELIB
}
/hpp/corbaserver/rbprm
)
INSTALL
(
...
...
src/hpp/corbaserver/rbprm/fewstepsplanner.py
View file @
b1d39ed9
...
...
@@ -100,7 +100,7 @@ class FewStepPlanner(object):
def
interpolateStates
(
self
,
stepsize
,
pathId
=
1
,
robustnessTreshold
=
0
,
filterStates
=
False
,
testReachability
=
True
,
quasiStatic
=
False
,
erasePreviousStates
=
True
):
return
_interpolateState
(
self
.
problemSolver
,
self
.
fullBody
,
stepsize
,
pathId
,
robustnessTreshold
,
filterStates
,
testReachability
,
quasiStatic
,
erasePreviousStates
)
def
goToQuasiStatic
(
self
,
currentState
,
toPos
,
stepsize
=
0.002
,
goalLimbsInContact
=
None
,
goalNormals
=
None
,
displayGuidePath
=
False
):
def
goToQuasiStatic
(
self
,
currentState
,
toPos
,
stepsize
=
0.002
,
goalLimbsInContact
=
None
,
goalNormals
=
None
,
displayGuidePath
=
False
,
erasePreviousStates
=
False
):
pId
=
self
.
guidePath
(
currentState
.
q
()[:
7
],
toPos
,
displayPath
=
displayGuidePath
)
self
.
fullBody
.
setStartStateId
(
currentState
.
sId
)
targetState
=
currentState
.
q
()[:];
targetState
[:
7
]
=
toPos
...
...
@@ -109,7 +109,7 @@ class FewStepPlanner(object):
if
goalNormals
is
None
:
goalNormals
=
[[
0.
,
0.
,
1.
]
for
_
in
range
(
len
(
goalLimbsInContact
))]
self
.
fullBody
.
setEndState
(
targetState
,
goalLimbsInContact
,
goalNormals
)
return
self
.
interpolateStates
(
stepsize
,
pathId
=
pId
,
robustnessTreshold
=
1
,
filterStates
=
True
,
quasiStatic
=
True
,
erasePreviousStates
=
False
)
return
self
.
interpolateStates
(
stepsize
,
pathId
=
pId
,
robustnessTreshold
=
1
,
filterStates
=
True
,
quasiStatic
=
True
,
erasePreviousStates
=
erasePreviousStates
)
src/hpp/corbaserver/rbprm/state_alg.py
View file @
b1d39ed9
...
...
@@ -23,7 +23,7 @@ try:
from
CWC_methods
import
compute_CWC
,
is_stable
from
lp_find_point
import
find_valid_c_cwc
,
find_valid_c_cwc_qp
,
lp_ineq_4D
except
:
print
"WARNING: in state_alg, some optinal dependencies were not found"
#~
print "WARNING: in state_alg, some optinal dependencies were not found"
pass
## algorithmic methods on state
...
...
@@ -81,8 +81,8 @@ def computeIntermediateState(sfrom, sto):
# \param p 3d position of the point
# \param n 3d normal of the contact location center
# \return (State, success) whether the creation was successful, as well as the new state
def
addNewContact
(
state
,
limbName
,
p
,
n
,
num_max_sample
=
0
):
sId
=
state
.
cl
.
addNewContact
(
state
.
sId
,
limbName
,
p
,
n
,
num_max_sample
)
def
addNewContact
(
state
,
limbName
,
p
,
n
,
num_max_sample
=
0
,
lockOtherJoints
=
False
):
sId
=
state
.
cl
.
addNewContact
(
state
.
sId
,
limbName
,
p
,
n
,
num_max_sample
,
lockOtherJoints
)
if
(
sId
!=
-
1
):
return
State
(
state
.
fullBody
,
sId
=
sId
),
True
return
state
,
False
...
...
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