Commit b1d39ed9 authored by stevet's avatar stevet Committed by Pierre Fernbach
Browse files

anymal box scenario functional

parent 16ea2bf6
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()
#! /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'
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]
......@@ -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
......
......@@ -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(
......
......@@ -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)
......@@ -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
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment