Skip to content
Snippets Groups Projects
Unverified Commit f522a1dd authored by stonneau's avatar stonneau Committed by GitHub
Browse files

Merge pull request #15 from stonneau/cleaning 

I take the liberty of merging this pull request that only moves / delete demo scripts not meant to be used by anyone other than me
parents 7947aa71 b28053d8
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
Showing
with 3 additions and 512 deletions
script/scenarios/anymal_python_script/darpa_anymal.py deleted 100644 → 0
+ 0
− 211
View file @ 7947aa71
#Importing helper class for RBPRM
from hpp.corbaserver.rbprm.rbprmbuilder import Builder
from hpp.corbaserver.rbprm.rbprmfullbody import FullBody
from hpp.corbaserver.rbprm.problem_solver import ProblemSolver
from hpp.gepetto import Viewer
#calling script darpa_hyq_path to compute root path
import darpa_anymal_path as tp
from os import environ
ins_dir = environ['DEVEL_DIR']
db_dir = ins_dir+"/install/share/hyq-rbprm/database/hyq_"
from hpp.corbaserver import Client
packageName = "anymal_description"
meshPackageName = "anymal_description"
rootJointType = "freeflyer"
# Information to retrieve urdf and srdf files.
urdfName = "anymal"
urdfSuffix = ""
srdfSuffix = ""
# This time we load the full body model of HyQ
fullBody = FullBody ()
print("alive -1")
fullBody.loadFullBodyModel(urdfName, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
print("alive 0")
fullBody.setJointBounds ("base_joint_xyz", [-2.5,5, -1, 1, 0.3, 4]) #originally [-2,5, -1, 1, 0.3, 4]
print("alive 1")
# Setting a number of sample configurations used
nbSamples = 50000 #used to be 20000
ps = tp.ProblemSolver(fullBody)
r = tp.Viewer (ps, viewerClient=tp.r.client)
rootName = 'base_joint_xyz'
cType = "_3_DOF"
rLegId = 'rfleg'
rLeg = 'RF_HAA'
rfoot = 'RF_ADAPTER_TO_FOOT'
offset = [0.,-0.031,0.] #originally [0.,-0.031,0.]
normal = [0,1,0] #hyq needs [0,1,0], also for anymal
legx = 0.03; legy = 0.03
def addLimbDb(limbId, heuristicName, loadValues = True, disableEffectorCollision = False):
fullBody.addLimbDatabase(str(db_dir+limbId+'.db'), limbId, heuristicName,loadValues, disableEffectorCollision)
fullBody.addLimb(rLegId,rLeg,rfoot,offset,normal, legx, legy, nbSamples, "forward", 0.1, cType)
lLegId = 'lhleg'
lLeg = 'LH_HAA'
lfoot = 'LH_ADAPTER_TO_FOOT'
fullBody.addLimb(lLegId,lLeg,lfoot,offset,normal, legx, legy, nbSamples, "random", 0.1, cType)
#~
rarmId = 'rhleg'
rarm = 'RH_HAA'
rHand = 'RH_ADAPTER_TO_FOOT'
fullBody.addLimb(rarmId,rarm,rHand,offset,normal, legx, legy, nbSamples, "random", 0.1, cType)
larmId = 'lfleg'
larm = 'LF_HAA'
lHand = 'LF_ADAPTER_TO_FOOT'
fullBody.addLimb(larmId,larm,lHand,offset,normal, legx, legy, nbSamples, "forward", 0.1, cType)
#~ fullBody.runLimbSampleAnalysis(rLegId, "jointLimitsDistance", True)
#~ fullBody.runLimbSampleAnalysis(lLegId, "jointLimitsDistance", True)
#~ fullBody.runLimbSampleAnalysis(rarmId, "manipulability", True)
#~ fullBody.runLimbSampleAnalysis(larmId, "manipulability", True)
q_0 = fullBody.getCurrentConfig();
q_init = fullBody.getCurrentConfig(); q_init[0:7] = tp.q_init[0:7]
q_goal = fullBody.getCurrentConfig(); q_goal[0:7] = tp.q_goal[0:7]
# Randomly generating a contact configuration at q_init
fullBody.setCurrentConfig (q_init)
q_init = fullBody.generateContacts(q_init, [0,0,1]) #originally [0,0,1]
#q_init[7:] = [0.0, 0.0, 0.0, \
# 0.0, 0.0, 0.0, \
# 0.0, 0.0, 0.0, \
# 0.0, 0.0, 0.0]
print "q_init"
print q_init
# Randomly generating a contact configuration at q_end
fullBody.setCurrentConfig (q_goal)
q_goal = fullBody.generateContacts(q_goal, [0,0,1])
# specifying the full body configurations as start and goal state of the problem
fullBody.setStartState(q_init,[rLegId,lLegId,rarmId,larmId])
#~ fullBody.setStartState(q_init,[rLegId])
#~ fullBody.setStartState(q_init,[rLegId,rarmId,larmId])
fullBody.setEndState(q_goal,[rLegId,lLegId,rarmId,larmId])
r(q_init)
#~ configs = fullBody.interpolate(0.12, 10, 10, True)
configs = []
#~ r.loadObstacleModel ('hpp-rbprm-corba', "darpa", "contact")
# calling draw with increasing i will display the sequence
i = 0;
#~ fullBody.draw(configs[i],r); i=i+1; i-1
from hpp.gepetto import PathPlayer
#pp = PathPlayer (fullBody.client.basic, r)
import time
#DEMO METHODS
def initConfig():
r.client.gui.setVisibility("anymal", "ON")
tp.cl.problem.selectProblem("default")
tp.r.client.gui.setVisibility("toto", "OFF")
tp.r.client.gui.setVisibility("hyq_trunk_large", "OFF")
r(q_init)
def endConfig():
r.client.gui.setVisibility("anymal", "ON")
tp.cl.problem.selectProblem("default")
tp.r.client.gui.setVisibility("toto", "OFF")
tp.r.client.gui.setVisibility("hyq_trunk_large", "OFF")
r(q_goal)
def rootPath():
r.client.gui.setVisibility("hyq", "OFF")
tp.cl.problem.selectProblem("rbprm_path")
tp.r.client.gui.setVisibility("toto", "OFF")
r.client.gui.setVisibility("hyq", "OFF")
tp.r.client.gui.setVisibility("hyq_trunk_large", "ON")
tp.pp(0)
tp.r.client.gui.setVisibility("hyq_trunk_large", "OFF")
r.client.gui.setVisibility("anymal", "ON")
tp.cl.problem.selectProblem("default")
def genPlan(step = 0.06, robustness = 18, cut = True):
tp.cl.problem.selectProblem("default")
r.client.gui.setVisibility("anymal", "ON")
tp.r.client.gui.setVisibility("toto", "OFF")
tp.r.client.gui.setVisibility("hyq_trunk_large", "OFF")
global configs
start = time.clock()
configs = fullBody.interpolate(step, 10, robustness, filterStates = cut, testReachability=False, quasiStatic=False)
#~ configs = fullBody.interpolate(step, 10, robustness, cut) #originally (0.12, 10, 10, True)
end = time.clock()
print "Last element in interpolated configs"
print configs[-1]
print "Contact plan generated in " + str(end-start) + "seconds"
r(configs[-1])
def contactPlan(t=0.5):
r.client.gui.setVisibility("anymal", "ON")
tp.cl.problem.selectProblem("default")
tp.r.client.gui.setVisibility("toto", "OFF")
tp.r.client.gui.setVisibility("hyq_trunk_large", "OFF")
for i in range(1,len(configs)):
r(configs[i]);
time.sleep(t)
def a():
print "initial configuration"
initConfig()
def b():
print "end configuration"
endConfig()
def c():
print "displaying root path"
rootPath()
def d(step = 0.06, robustness = 18, cut = True):
print "computing contact plan"
genPlan(step, cut)
def e(t = 0.2):
print "displaying contact plan"
contactPlan(t)
print "Root path generated in " + str(tp.t) + " ms."
d(0.005,20)
#print "Chcek collisions"
#print fullBody.isConfigValid(fullBody.getCurrentConfig())
#print " "
#print "Check candidate configurations"
#print fullBody.getNumSamples(rLegId)
#print fullBody.getNumSamples(lLegId)
#print fullBody.getNumSamples(larmId)
#print fullBody.getNumSamples(rarmId)
#print "Visualise Voxels"
#print "Current Configuration"
#print fullBody.getCurrentConfig()
#fullBody.createOctreeBoxes(r.client.gui,0,rLegId,fullBody.getCurrentConfig())
#fullBody.createOctreeBoxes(r.client.gui,0,lLegId,fullBody.getCurrentConfig())
#fullBody.createOctreeBoxes(r.client.gui,0,larmId,fullBody.getCurrentConfig())
#fullBody.createOctreeBoxes(r.client.gui,0,rarmId,fullBody.getCurrentConfig())
script/scenarios/anymal_python_script/darpa_anymal_path.py deleted 100644 → 0
+ 0
− 100
View file @ 7947aa71
# Importing helper class for setting up a reachability planning problem
from hpp.corbaserver.rbprm.rbprmbuilder import Builder
# Importing Gepetto viewer helper class
from hpp.gepetto import Viewer
rootJointType = 'freeflyer'
packageName = 'hpp-rbprm-corba'
meshPackageName = 'hpp-rbprm-corba'
# URDF file describing the trunk of the robot HyQ
urdfName = 'anymal_all'
# URDF files describing the reachable workspace of each limb of HyQ
urdfNameRom = ['anymal_lhleg_rom','anymal_lfleg_rom','anymal_rfleg_rom','anymal_rhleg_rom']
urdfSuffix = ""
srdfSuffix = ""
# Creating an instance of the helper class, and loading the robot
rbprmBuilder = Builder ()
rbprmBuilder.loadModel(urdfName, urdfNameRom, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
rbprmBuilder.setJointBounds ("base_joint_xyz", [-3,5, -1, 1, 0.3, 4]) #[-2,5, -1, 1, 0.3, 4])
# The following lines set constraint on the valid configurations:
# a configuration is valid only if all limbs can create a contact ...
rbprmBuilder.setFilter(['anymal_rhleg_rom', 'anymal_lfleg_rom', 'anymal_rfleg_rom','anymal_lhleg_rom'])
rbprmBuilder.setAffordanceFilter('anymal_rhleg_rom', ['Support'])
rbprmBuilder.setAffordanceFilter('anymal_rfleg_rom', ['Support',])
rbprmBuilder.setAffordanceFilter('anymal_lhleg_rom', ['Support'])
rbprmBuilder.setAffordanceFilter('anymal_lfleg_rom', ['Support',])
# We also bound the rotations of the torso.
rbprmBuilder.boundSO3([-0.4,0.4,-0.4,0.4,-0.4,0.4]) #original [-0.4,0.4,-3,3,-3,3]
# Creating an instance of HPP problem solver and the viewer
from hpp.corbaserver.rbprm.problem_solver import ProblemSolver
ps = ProblemSolver( rbprmBuilder )
r = Viewer (ps)
# Setting initial and goal configurations
q_init = rbprmBuilder.getCurrentConfig ();
q_init [0:3] = [-2.5, 0, 0.49]; rbprmBuilder.setCurrentConfig (q_init); r (q_init)
q_goal = q_init [::]
q_goal [0:3] = [2, 0, 0.48]; r (q_goal) #[3, 0, 0.5]
# Choosing a path optimizer
ps.addPathOptimizer("RandomShortcut")
ps.setInitialConfig (q_init)
ps.addGoalConfig (q_goal)
from hpp.corbaserver.affordance.affordance import AffordanceTool
afftool = AffordanceTool ()
afftool.loadObstacleModel (packageName, "darpa", "planning", r, reduceSizes=[0.05,0.,0.])
#~ afftool.visualiseAffordances('Support', r, [0.25, 0.5, 0.5])
# Choosing RBPRM shooter and path validation methods.
# Note that the standard RRT algorithm is used.
ps.client.problem.selectConFigurationShooter("RbprmShooter")
ps.client.problem.selectPathValidation("RbprmPathValidation",0.05)
# Solve the problem
t = ps.solve ()
if isinstance(t, list):
t = t[0]* 3600000 + t[1] * 60000 + t[2] * 1000 + t[3]
# Playing the computed path
from hpp.gepetto import PathPlayer
pp = PathPlayer (rbprmBuilder.client.basic, r)
q_far = q_init [::]
q_far [0:3] = [-2, -3, 0.63];
r(q_far)
for i in range(1,10):
rbprmBuilder.client.basic.problem.optimizePath(i)
from hpp.corbaserver import Client
from hpp.corbaserver.robot import Robot as Parent
class Robot (Parent):
rootJointType = 'freeflyer'
packageName = 'hpp-rbprm-corba'
meshPackageName = 'hpp-rbprm-corba'
# URDF file describing the trunk of the robot HyQ
urdfName = 'anymal_all'
urdfSuffix = ""
srdfSuffix = ""
def __init__ (self, robotName, load = True):
Parent.__init__ (self, robotName, self.rootJointType, load)
self.tf_root = "base_footprint"
self.client.basic = Client ()
self.load = load
#DEMO code to play root path and final contact plan
cl = Client()
cl.problem.selectProblem("rbprm_path")
rbprmBuilder2 = Robot ("toto")
ps2 = ProblemSolver( rbprmBuilder2 )
cl.problem.selectProblem("default")
cl.problem.movePathToProblem(3,"rbprm_path",rbprmBuilder.getAllJointNames())
r2 = Viewer (ps2, viewerClient=r.client)
r2(q_far)
script/scenarios/darpa_hyq_path.py deleted 100755 → 0
+ 0
− 83
View file @ 7947aa71
# Importing helper class for setting up a reachability planning problem
from hpp.corbaserver.rbprm.rbprmbuilder import Builder
# Importing Gepetto viewer helper class
from hpp.gepetto import Viewer
rootJointType = 'freeflyer'
packageName = 'hpp-rbprm-corba'
meshPackageName = 'hpp-rbprm-corba'
# URDF file describing the trunk of the robot HyQ
urdfName = 'hyq_trunk_large'
# URDF files describing the reachable workspace of each limb of HyQ
urdfNameRom = ['hyq_lhleg_rom','hyq_lfleg_rom','hyq_rfleg_rom','hyq_rhleg_rom']
urdfSuffix = ""
srdfSuffix = ""
# Creating an instance of the helper class, and loading the robot
rbprmBuilder = Builder ()
rbprmBuilder.loadModel(urdfName, urdfNameRom, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
rbprmBuilder.setJointBounds ("base_joint_xyz", [-2,5, -1, 1, 0.3, 4])
# The following lines set constraint on the valid configurations:
# a configuration is valid only if all limbs can create a contact ...
rbprmBuilder.setFilter(['hyq_rhleg_rom', 'hyq_lfleg_rom', 'hyq_rfleg_rom','hyq_lhleg_rom'])
rbprmBuilder.setAffordanceFilter('hyq_rhleg_rom', ['Support'])
rbprmBuilder.setAffordanceFilter('hyq_rfleg_rom', ['Support',])
rbprmBuilder.setAffordanceFilter('hyq_lhleg_rom', ['Support'])
rbprmBuilder.setAffordanceFilter('hyq_lfleg_rom', ['Support',])
# We also bound the rotations of the torso.
rbprmBuilder.boundSO3([-0.4,0.4,-3,3,-3,3])
# Creating an instance of HPP problem solver and the viewer
from hpp.corbaserver.rbprm.problem_solver import ProblemSolver
ps = ProblemSolver( rbprmBuilder )
r = Viewer (ps)
# Setting initial and goal configurations
q_init = rbprmBuilder.getCurrentConfig ();
q_init [0:3] = [-2, 0, 0.63]; rbprmBuilder.setCurrentConfig (q_init); r (q_init)
q_goal = q_init [::]
q_goal [0:3] = [3, 0, 0.63]; r (q_goal)
#~ q_goal [0:3] = [-1.5, 0, 0.63]; r (q_goal)
# Choosing a path optimizer
ps.addPathOptimizer("RandomShortcut")
ps.setInitialConfig (q_init)
ps.addGoalConfig (q_goal)
from hpp.corbaserver.affordance.affordance import AffordanceTool
afftool = AffordanceTool ()
afftool.loadObstacleModel (packageName, "darpa", "planning", r)
afftool.visualiseAffordances('Support', r, [0.25, 0.5, 0.5])
# Choosing RBPRM shooter and path validation methods.
# Note that the standard RRT algorithm is used.
ps.client.problem.selectConFigurationShooter("RbprmShooter")
ps.client.problem.selectPathValidation("RbprmPathValidation",0.05)
r.loadObstacleModel (packageName, "darpa", "planning")
r(q_init)
#~ ps.solve ()
t = ps.solve ()
#~ print t;
if isinstance(t, list):
t = t[0]* 3600000 + t[1] * 60000 + t[2] * 1000 + t[3]
f = open('log.txt', 'a')
f.write("path computation " + str(t) + "\n")
f.close()
# Solve the problem
t = ps.solve ()
# Playing the computed path
from hpp.gepetto import PathPlayer
pp = PathPlayer (rbprmBuilder.client.basic, r)
#~ pp (1)
q_far = q_init [::]
q_far [0:3] = [-2, -3, 0.63];
r(q_far)
for i in range(1,10):
rbprmBuilder.client.basic.problem.optimizePath(i)
script/scenarios/demos/darpa_hyq.py
+ 3
− 97
View file @ f522a1dd
...@@ -42,7 +42,6 @@ cType = "_3_DOF" ...@@ -42,7 +42,6 @@ cType = "_3_DOF"
rLegId = 'rfleg' rLegId = 'rfleg'
rLeg = 'rf_haa_joint' rLeg = 'rf_haa_joint'
rfoot = 'rf_foot_joint' rfoot = 'rf_foot_joint'
#~ offset = [0.,-0.021,0.]
offset = [0.,-0.021,0.] offset = [0.,-0.021,0.]
normal = [0,1,0] normal = [0,1,0]
legx = 0.02; legy = 0.02 legx = 0.02; legy = 0.02
...@@ -72,11 +71,6 @@ fullBody.addLimb(larmId,larm,lHand,offset,normal, legx, legy, nbSamples, "random ...@@ -72,11 +71,6 @@ fullBody.addLimb(larmId,larm,lHand,offset,normal, legx, legy, nbSamples, "random
#~ fullBody.runLimbSampleAnalysis(rarmId, "jointLimitsDistance", True) #~ fullBody.runLimbSampleAnalysis(rarmId, "jointLimitsDistance", True)
#~ fullBody.runLimbSampleAnalysis(larmId, "jointLimitsDistance", True) #~ fullBody.runLimbSampleAnalysis(larmId, "jointLimitsDistance", True)
#~ q_init = hyq_ref[:]; q_init[0:7] = tp.q_init[0:7];
#~ q_goal = hyq_ref[:]; q_goal[0:7] = tp.q_goal[0:7];
q_init = hyq_ref[:]; q_init[0:7] = tp.q_init[0:7]; q_init[2]=hyq_ref[2]
q_goal = hyq_ref[:]; q_goal[0:7] = tp.q_goal[0:7]; q_goal[2]=hyq_ref[2]+0.02
q_init = [-2.0, q_init = [-2.0,
0.0, 0.0,
0.6838277139631803, 0.6838277139631803,
...@@ -96,15 +90,10 @@ q_init = [-2.0, ...@@ -96,15 +90,10 @@ q_init = [-2.0,
0.03995660287873871, 0.03995660287873871,
-0.9577096766517215, -0.9577096766517215,
0.9384602821326071] 0.9384602821326071]
q_goal = hyq_ref[:]; q_goal[0:7] = tp.q_goal[0:7]; q_goal[2]=hyq_ref[2]+0.02
# Randomly generating a contact configuration at q_init
#~ fullBody.setCurrentConfig (q_init)
#~ q_init = fullBody.generateContacts(q_init, [0,0,1])
# Randomly generating a contact configuration at q_end
#~ fullBody.setCurrentConfig (q_goal)
#~ q_goal = fullBody.generateContacts(q_goal, [0,0,1])
# specifying the full body configurations as start and goal state of the problem # specifying the full body configurations as start and goal state of the problem
fullBody.setStartState(q_init,[rLegId,lLegId,rarmId,larmId]) fullBody.setStartState(q_init,[rLegId,lLegId,rarmId,larmId])
...@@ -169,90 +158,7 @@ def contactPlan(step = 0.5): ...@@ -169,90 +158,7 @@ def contactPlan(step = 0.5):
r(configs[i]); r(configs[i]);
time.sleep(step) time.sleep(step)
def contactPlanDontMove(step = 0.5):
r.client.gui.setVisibility("hyq", "ON")
tp.cl.problem.selectProblem("default")
tp.r.client.gui.setVisibility("toto", "OFF")
tp.r.client.gui.setVisibility("hyq_trunk_large", "OFF")
for i in range(0,len(configs)):
a = configs[i]
a[:6] = [0 for _ in range(6)]
a[6] = 1
#~ r(configs[i]);
r(a);
time.sleep(step)
def a():
print "initial configuration"
initConfig()
def b():
print "end configuration"
endConfig()
def c():
print "displaying root path"
rootPath()
def d(step=0.06):
print "computing contact plan"
genPlan(step)
def e(step = 0.5):
print "displaying contact plan"
contactPlan(step)
def f(step = 0.5):
print "displaying static contact plan"
contactPlanDontMove(step)
print "Root path generated in " + str(tp.t) + " ms." print "Root path generated in " + str(tp.t) + " ms."
#~ d();e() genPlan(0.01);contactPlan(0.01)
d(0.01);e(0.01)
#~ d(0.004);e(0.01)
from hpp.corbaserver.rbprm.rbprmstate import *
com = fullBody.getCenterOfMass()
s = None
def d1():
global s
s = State(fullBody,q = q_init, limbsIncontact = [larmId])
a = s.q()
a[2]=a[2]+0.01
s.setQ(a)
return s.projectToCOM([0.01,0.,0.], maxNumSample = 0)
def d2():
global s
s = State(fullBody,q = q_init, limbsIncontact = [larmId, rarmId])
a = s.q()
a[2]=a[2]+0.05
a[0]=a[0]+0.05
s.setQ(a)
return s.projectToCOM([0.01,0.,0.], maxNumSample = 0)
def d3():
global s
s = State(fullBody,q = q_init, limbsIncontact = [rarmId])
a = s.q()
a[2]=a[2]+0.01
s.setQ(a)
return s.projectToCOM([0.01,0.,0.], maxNumSample = 0)
def d4():
global s
s = State(fullBody,q = q_init, limbsIncontact = [rarmId])
a = s.q()
a[2]=a[2]-0.01
s.setQ(a)
print s.projectToCOM([0.01,0.,0.], maxNumSample = 0)
s = State(fullBody,q = s.q(), limbsIncontact = [larmId])
return s.projectToCOM([0.01,0.,0.], maxNumSample = 0)
script/scenarios/run.sh deleted 100755 → 0
+ 0
− 8
View file @ 7947aa71
#!/bin/bash
gepetto-viewer-server &
hpp-rbprm-server &
ipython -i --no-confirm-exit ./$1
pkill -f 'gepetto-viewer-server'
pkill -f 'hpp-rbprm-server'
script/scenarios/rund.sh deleted 100755 → 0
+ 0
− 6
View file @ 7947aa71
#!/bin/bash
gepetto-viewer-server &
ipython -i --no-confirm-exit ./$1
pkill -f 'gepetto-viewer-server'
script/scenarios/sandbox/log.txt deleted 100644 → 0
+ 0
− 7
View file @ 7947aa71
path computation 24
path computation 2
path computation 3
path computation 2
path computation 2
path computation 2
path computation 2
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