Unverified Commit 802c3198 authored by stonneau's avatar stonneau Committed by GitHub
Browse files

Merge pull request #47 from pFernbach/devel

addNewContact with rotation, add tools/scenario scripts 
parents e7bbf3db 99eef4b8
......@@ -778,8 +778,9 @@ module hpp
/// \param max_num_sample number of times it will try to randomly sample a configuration before failing
/// \param lockOtherJoints : if True, the values of all the joints exepct the ones of 'limbName' are constrained to be constant.
/// This only affect the first projection, if max_num_sample > 0 and the first projection was unsuccessfull, the parameter is ignored
/// \param rotation : desired rotation of the end-effector in contact, expressed as Quaternion (x,y,z,w). If different from zero, the normal is ignored. Otherwise the rotation is automatically computed from the normal (with one axis of freedom)
/// \return (success,NState) whether the creation was successful, as well as the new state
short addNewContact(in unsigned short stateId, in string limbName, in floatSeq position, in floatSeq normal, in unsigned short max_num_sample, in boolean lockOtherJoints) raises (Error);
short addNewContact(in unsigned short stateId, in string limbName, in floatSeq position, in floatSeq normal, in unsigned short max_num_sample, in boolean lockOtherJoints,in floatSeq rotation) raises (Error);
/// removes a contact from the state
/// if the limb is not already in contact, does nothing
......
......@@ -6,7 +6,7 @@ print "Plan guide trajectory ..."
import talos_plateformes_path as tp
print "Done."
import time
Robot.urdfSuffix+="_safeFeet"
pId = tp.ps.numberPaths() -1
fullBody = Robot ()
......
......@@ -36,11 +36,11 @@ q_init = q_ref[::]
fullBody.setReferenceConfig(q_ref)
fullBody.setCurrentConfig (q_init)
fullBody.setPostureWeights(fullBody.postureWeights[::]+[0]*6)
fullBody.usePosturalTaskContactCreation(True)
#fullBody.usePosturalTaskContactCreation(True)
print "Generate limb DB ..."
tStart = time.time()
fullBody.loadAllLimbs("fixedStep04","ReferenceConfiguration")
fullBody.loadAllLimbs("fixedStep04","ReferenceConfiguration",nbSamples=100000,disableEffectorCollision=False)
tGenerate = time.time() - tStart
print "Done."
print "Databases generated in : "+str(tGenerate)+" s"
......@@ -51,12 +51,12 @@ configSize = fullBody.getConfigSize() -fullBody.client.robot.getDimensionExtraCo
q_init[0:7] = tp.ps.configAtParam(pId,0.)[0:7] # use this to get the correct orientation
q_goal = q_init[::]; q_goal[0:7] = tp.ps.configAtParam(pId,tp.ps.pathLength(pId))[0:7]
dir_init = tp.ps.configAtParam(pId,0.)[tp.indexECS:tp.indexECS+3]
dir_init = [0,0,0]
acc_init = tp.ps.configAtParam(pId,0.)[tp.indexECS+3:tp.indexECS+6]
dir_goal = tp.ps.configAtParam(pId,tp.ps.pathLength(pId))[tp.indexECS:tp.indexECS+3]
acc_goal = [0,0,0]
robTreshold = 2
robTreshold = 5
# copy extraconfig for start and init configurations
q_init[configSize:configSize+3] = dir_init[::]
q_init[configSize+3:configSize+6] = acc_init[::]
......@@ -66,30 +66,30 @@ q_goal[configSize+3:configSize+6] = [0,0,0]
q_init[2] = q_ref[2]
q_goal[2] = q_ref[2]
normals = [[0.,0.,1.] for _ in range(4)]
fullBody.setStaticStability(True)
fullBody.setCurrentConfig (q_init)
v(q_init)
fullBody.setStartState(q_init,Robot.limbs_names,normals)
fullBody.setEndState(q_goal,Robot.limbs_names,normals)
fullBody.setCurrentConfig (q_goal)
v(q_goal)
v.addLandmark('anymal/base',0.3)
v(q_init)
z = [0.,0.,1]
# specify the full body configurations as start and goal state of the problem
fullBody.setStartState(q_init,fullBody.limbs_names, [z for _ in range(4)])
fullBody.setEndState(q_goal,fullBody.limbs_names, [z for _ in range(4)])
print "Generate contact plan ..."
tStart = time.time()
configs = fullBody.interpolate(0.002,pathId=pId,robustnessTreshold = robTreshold, filterStates = True,testReachability=True,quasiStatic=True)
configs = fullBody.interpolate(0.001,pathId=pId,robustnessTreshold = robTreshold, filterStates = True,testReachability=True,quasiStatic=True)
tInterpolateConfigs = time.time() - tStart
print "Done. "
print "Contact sequence computed in "+str(tInterpolateConfigs)+" s."
print "Done."
print "Contact plan generated in : "+str(tInterpolateConfigs)+" s"
print "number of configs :", len(configs)
#raw_input("Press Enter to display the contact sequence ...")
#displayContactSequence(v,configs)
fullBody.resetJointsBounds()
......@@ -36,7 +36,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/forceAllOrientation",True)
ps.setParameter("DynamicPlanner/sizeFootX",0.01)
ps.setParameter("DynamicPlanner/sizeFootY",0.01)
ps.setParameter("DynamicPlanner/friction",mu)
......
from hpp.corbaserver.rbprm.anymal import Robot
from hpp.gepetto import Viewer
from tools.display_tools import *
import time
print "Plan guide trajectory ..."
import scenarios.sandbox.ANYmal.anymal_modular_palet_low_path as tp
#Robot.urdfSuffix += "_safeFeet"
statusFilename = tp.statusFilename
pId = tp.pid
f = open(statusFilename,"a")
if tp.ps.numberPaths() > 0 :
print "Path planning OK."
f.write("Planning_success: True"+"\n")
f.close()
else :
print "Error during path planning"
f.write("Planning_success: False"+"\n")
f.close()
import sys
sys.exit(1)
fullBody = Robot ()
# Set the bounds for the root
rootBounds = tp.rootBounds[::]
rootBounds[0] -= 0.2
rootBounds[1] += 0.2
rootBounds[2] -= 0.5
rootBounds[3] += 0.5
rootBounds[4] -= 0.2
rootBounds[5] += 0.2
fullBody.setJointBounds ("root_joint", rootBounds)
fullBody.setVeryConstrainedJointsBounds()
# add the 6 extraDof for velocity and acceleration (see *_path.py script)
fullBody.client.robot.setDimensionExtraConfigSpace(tp.extraDof)
fullBody.client.robot.setExtraConfigSpaceBounds([-tp.vMax,tp.vMax,-tp.vMax,tp.vMax,-tp.vMax,tp.vMax,-tp.aMax,tp.aMax,-tp.aMax,tp.aMax,-tp.aMaxZ,tp.aMaxZ])
ps = tp.ProblemSolver( fullBody )
ps.setParameter("Kinodynamic/velocityBound",tp.vMax)
ps.setParameter("Kinodynamic/accelerationBound",tp.aMax)
ps.setParameter("FullBody/frictionCoefficient",tp.mu)
#load the viewer
try :
v = tp.Viewer (ps,viewerClient=tp.v.client, displayCoM = True)
except Exception:
print "No viewer started !"
class FakeViewer():
def __init__(self):
return
def __call__(self,q):
return
def addLandmark(self,a,b):
return
v = FakeViewer()
# load a reference configuration
q_ref = fullBody.referenceConfig[::]+[0]*6
q_init = q_ref[::]
fullBody.setReferenceConfig(q_ref)
fullBody.setPostureWeights(fullBody.postureWeights[::]+[0]*6)
#fullBody.usePosturalTaskContactCreation(True)
fullBody.setCurrentConfig (q_init)
print "Generate limb DB ..."
tStart = time.time()
# generate databases :
fullBody.loadAllLimbs("static","ReferenceConfiguration",nbSamples=100000,disableEffectorCollision=False)
tGenerate = time.time() - tStart
print "Done."
print "Databases generated in : "+str(tGenerate)+" s"
v.addLandmark('anymal/base_0',0.2)
#define initial and final configurations :
configSize = fullBody.getConfigSize() -fullBody.client.robot.getDimensionExtraConfigSpace()
q_init[0:7] = tp.ps.configAtParam(pId,0)[0:7] # use this to get the correct orientation
q_goal = q_init[::]; q_goal[0:7] = tp.ps.configAtParam(pId,tp.ps.pathLength(pId))[0:7]
vel_init = tp.ps.configAtParam(pId,0)[tp.indexECS:tp.indexECS+3]
acc_init = tp.ps.configAtParam(pId,0)[tp.indexECS+3:tp.indexECS+6]
vel_goal = tp.ps.configAtParam(pId,tp.ps.pathLength(pId))[tp.indexECS:tp.indexECS+3]
acc_goal = [0,0,0]
vel_init = [0,0,0]
robTreshold = 5
# copy extraconfig for start and init configurations
q_init[configSize:configSize+3] = vel_init[::]
q_init[configSize+3:configSize+6] = acc_init[::]
q_goal[configSize:configSize+3] = vel_goal[::]
q_goal[configSize+3:configSize+6] = [0,0,0]
q_init[2] = q_ref[2]
q_goal[2] = q_ref[2]
normals = [[0.,0.,1.] for _ in range(4)]
fullBody.setStaticStability(True)
fullBody.setCurrentConfig (q_init)
v(q_init)
fullBody.setStartState(q_init,Robot.limbs_names,normals)
fullBody.setEndState(q_goal,Robot.limbs_names,normals)
fullBody.setCurrentConfig (q_goal)
v(q_goal)
print "Generate contact plan ..."
tStart = time.time()
configs = fullBody.interpolate(0.001,pathId=pId,robustnessTreshold = robTreshold, filterStates = True,testReachability=True,quasiStatic=True)
tInterpolateConfigs = time.time() - tStart
print "Done."
print "Contact plan generated in : "+str(tInterpolateConfigs)+" s"
print "number of configs :", len(configs)
#raw_input("Press Enter to display the contact sequence ...")
#displayContactSequence(v,configs)
if len(configs) < 2 :
cg_success = False
print "Error during contact generation."
else:
cg_success = True
print "Contact generation Done."
if abs(configs[-1][0] - tp.q_goal[0]) < 0.01 and abs(configs[-1][1]- tp.q_goal[1]) < 0.01 and (len(fullBody.getContactsVariations(len(configs)-2,len(configs)-1))==1):
print "Contact generation successful."
cg_reach_goal = True
else:
print "Contact generation failed to reach the goal."
cg_reach_goal = False
f = open(statusFilename,"a")
f.write("cg_success: "+str(cg_success)+"\n")
f.write("cg_reach_goal: "+str(cg_reach_goal)+"\n")
f.close()
if (not cg_success):
import sys
sys.exit(1)
#beginId = 2
# put back original bounds for wholebody methods
fullBody.resetJointsBounds()
#displayContactSequence(v,configs)
from hpp.corbaserver.rbprm.anymal_abstract import Robot
Robot.urdfName += "_large"
from hpp.gepetto import Viewer
from hpp.corbaserver import ProblemSolver
from pinocchio import Quaternion
import numpy as np
import time
import math
statusFilename = "infos.log"
Z_VALUE = 0.465
Y_VALUE = -0.1
vInit = 0.3
vMax = 0.5# linear velocity bound for the root
aMax = 0.5# linear acceleration bound for the root
aMaxZ = 5.
extraDof = 6
mu=0.3# coefficient of friction
# Creating an instance of the helper class, and loading the robot
# Creating an instance of the helper class, and loading the robot
rbprmBuilder = Robot ()
# Define bounds for the root : bounding box of the scenario
rootBounds = [-1.7,1.7, Y_VALUE-0.001, Y_VALUE+0.001, Z_VALUE-0.001, Z_VALUE+0.231]
rbprmBuilder.setJointBounds ("root_joint", rootBounds)
# The following lines set constraint on the valid configurations:
# a configuration is valid only if all limbs can create a contact with the corresponding afforcances type
rbprmBuilder.setFilter(Robot.urdfNameRom)
for rom in rbprmBuilder.urdfNameRom :
rbprmBuilder.setAffordanceFilter(rom, ['Support'])
# We also bound the rotations of the torso. (z, y, x)
rbprmBuilder.boundSO3([-3.14,3.14,-0.1,0.1,-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)
# 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()
# Creating an instance of HPP problem solver
ps = ProblemSolver( rbprmBuilder )
# define parameters used by various methods :
ps.setParameter("Kinodynamic/velocityBound",vMax)
ps.setParameter("Kinodynamic/accelerationBound",aMax)
ps.setParameter("Kinodynamic/verticalAccelerationBound",aMaxZ)
ps.setParameter("Kinodynamic/forceAllOrientation",True)
ps.setParameter("DynamicPlanner/sizeFootX",0.2)
ps.setParameter("DynamicPlanner/sizeFootY",0.12)
ps.setParameter("DynamicPlanner/friction",mu)
# sample only configuration with null velocity and acceleration :
ps.setParameter("ConfigurationShooter/sampleExtraDOF",False)
ps.setParameter("PathOptimization/RandomShortcut/NumberOfLoops",100)
# initialize the viewer :
from hpp.gepetto import ViewerFactory
vf = ViewerFactory (ps)
# load the module to analyse the environnement and compute the possible contact surfaces
from hpp.corbaserver.affordance.affordance import AffordanceTool
afftool = AffordanceTool ()
afftool.setAffordanceConfig('Support', [0.5, 0.03, 0.00005])
afftool.setMinimumArea('Support',0.03)
afftool.loadObstacleModel ("hpp_environments", "ori/modular_palet_low_collision", "planning", vf, reduceSizes=[0.1,0,0])
#afftool.loadObstacleModel ("hpp_environments", "ori/modular_palet_low", "planning", vf)
try :
v = vf.createViewer(displayArrows = True)
except Exception:
print "No viewer started !"
class FakeViewer():
def __init__(self):
return
def __call__(self,q):
return
v = FakeViewer()
afftool.visualiseAffordances('Support', v, v.color.lightBrown)
#v.addLandmark(v.sceneName,1)
q_init = rbprmBuilder.getCurrentConfig ();
q_init[2] = Z_VALUE
q_init[0:2] = [-1.6,Y_VALUE]
q_init[-6] = vInit
v(q_init)
q_goal=q_init[::]
q_goal[0] = 1.6
ps.setInitialConfig (q_init)
ps.addGoalConfig (q_goal)
# write problem in files :
f = open(statusFilename,"w")
f.write("q_init= "+str(q_init)+"\n")
f.write("q_goal= "+str(q_goal)+"\n")
f.close()
# Choosing RBPRM shooter and path validation methods.
ps.selectConfigurationShooter("RbprmShooter")
ps.selectPathValidation("RbprmPathValidation",0.05)
# Choosing kinodynamic methods :
ps.selectSteeringMethod("RBPRMKinodynamic")
ps.selectDistance("Kinodynamic")
ps.selectPathPlanner("DynamicPlanner")
ps.addPathOptimizer ("RandomShortcutDynamic")
# Solve the planning problem :
t = ps.solve()
print "Guide planning done in "+str(t)+", optimizing trajectory ..."
pid = ps.numberPaths()-1
for i in range(20):
ps.optimizePath(pid)
pid = ps.numberPaths()-1
try :
# display solution :
from hpp.gepetto import PathPlayer
pp = PathPlayer (v)
pp.dt=0.1
pp.displayPath(pid)#pp.displayVelocityPath(0) #
v.client.gui.setVisibility("path_"+str(pid)+"_root","ALWAYS_ON_TOP")
pp.dt=0.01
pp(pid)
except Exception:
pass
# move the robot out of the view before computing the contacts
q_far = q_init[::]
q_far[2] = -2
v(q_far)
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
......
from hpp.corbaserver.rbprm.talos_abstract import Robot
from hpp.gepetto import Viewer
from hpp.corbaserver import ProblemSolver
import numpy as np
from pinocchio import Quaternion
import time
Robot.urdfName += "_large"
vMax = 0.5# linear velocity bound for the root
aMax = 0.7# linear acceleration bound for the root
extraDof = 6
mu=0.5# coefficient of friction
# Creating an instance of the helper class, and loading the robot
# Creating an instance of the helper class, and loading the robot
rbprmBuilder = Robot ()
# Define bounds for the root : bounding box of the scenario
rbprmBuilder.setJointBounds ("root_joint", [0,18.5, 0, 24., rbprmBuilder.ref_height, rbprmBuilder.ref_height])
rbprmBuilder.setJointBounds ('torso_1_joint', [0,0])
rbprmBuilder.setJointBounds ('torso_2_joint', [0,0])
# The following lines set constraint on the valid configurations:
# a configuration is valid only if all limbs can create a contact with the corresponding afforcances type
rbprmBuilder.setFilter(['talos_lleg_rom','talos_rleg_rom'])
rbprmBuilder.setAffordanceFilter('talos_lleg_rom', ['Support',])
rbprmBuilder.setAffordanceFilter('talos_rleg_rom', ['Support'])
# We also bound the rotations of the torso. (z, y, x)
rbprmBuilder.boundSO3([-4,4,-0.1,0.1,-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)
# We set the bounds of this extraDof with velocity and acceleration bounds (expect on z axis)
rbprmBuilder.client.robot.setExtraConfigSpaceBounds([-vMax,vMax,-vMax,vMax,0,0,-aMax,aMax,-aMax,aMax,0,0])
indexECS = rbprmBuilder.getConfigSize() - rbprmBuilder.client.robot.getDimensionExtraConfigSpace()
# Creating an instance of HPP problem solver
ps = ProblemSolver( rbprmBuilder )
# define parameters used by various methods :
ps.setParameter("Kinodynamic/velocityBound",vMax)
ps.setParameter("Kinodynamic/accelerationBound",aMax)
ps.setParameter("DynamicPlanner/sizeFootX",0.2)
ps.setParameter("DynamicPlanner/sizeFootY",0.12)
ps.setParameter("DynamicPlanner/friction",mu)
ps.setParameter("Kinodynamic/forceYawOrientation",True)
# sample only configuration with null velocity and acceleration :
ps.setParameter("ConfigurationShooter/sampleExtraDOF",False)
ps.setParameter("PathOptimization/RandomShortcut/NumberOfLoops",500)
# initialize the viewer :
from hpp.gepetto import ViewerFactory
vf = ViewerFactory (ps)
# load the module to analyse the environnement and compute the possible contact surfaces
from hpp.corbaserver.affordance.affordance import AffordanceTool
afftool = AffordanceTool ()
afftool.setAffordanceConfig('Support', [0.5, 0.03, 5.])
afftool.loadObstacleModel ("hpp_environments", "multicontact/maze_hard", "planning", vf)
#load the viewer
v = vf.createViewer(displayArrows = True)
v.addLandmark(v.sceneName,1)
afftool.visualiseAffordances('Support', v, v.color.lightBrown)