Skip to content
GitLab
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
Hide whitespace changes
Inline Side-by-side
script/scenarios/anymal_python_script/darpa_anymal.py deleted 100644 → 0
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
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
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
View file @ f522a1dd
......@@ -42,7 +42,6 @@ cType = "_3_DOF"
rLegId = 'rfleg'
rLeg = 'rf_haa_joint'
rfoot = 'rf_foot_joint'
#~ offset = [0.,-0.021,0.]
offset = [0.,-0.021,0.]
normal = [0,1,0]
legx = 0.02; legy = 0.02
......@@ -72,11 +71,6 @@ fullBody.addLimb(larmId,larm,lHand,offset,normal, legx, legy, nbSamples, "random
#~ fullBody.runLimbSampleAnalysis(rarmId, "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,
0.0,
0.6838277139631803,
......@@ -96,15 +90,10 @@ q_init = [-2.0,
0.03995660287873871,
-0.9577096766517215,
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
fullBody.setStartState(q_init,[rLegId,lLegId,rarmId,larmId])
......@@ -169,90 +158,7 @@ def contactPlan(step = 0.5):
r(configs[i]);
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."
#~ d();e()
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)
genPlan(0.01);contactPlan(0.01)
script/scenarios/run.sh deleted 100755 → 0
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
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
View file @ 7947aa71
path computation 24
path computation 2
path computation 3
path computation 2
path computation 2
path computation 2
path computation 2
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