diff --git a/script/scenarios/sandbox/dynamic/tools.py b/script/scenarios/sandbox/dynamic/tools.py
deleted file mode 100644
index 7205cd367cd5cb3d0e29226ef7416d807afdd0d6..0000000000000000000000000000000000000000
--- a/script/scenarios/sandbox/dynamic/tools.py
+++ /dev/null
@@ -1,43 +0,0 @@
-
-def addSphere(viewer,color, pos,rotation = None,name=None,radius=0.01):
- gui = viewer.client.gui
- if name==None:
- i=0
- name='sphere_'+str(i)
- while name in gui.getNodeList():
- i=i+1
- name='sphere_'+str(i)
- gui.addSphere(name,radius,color)
- gui.setVisibility(name,"ALWAYS_ON_TOP")
- gui.addToGroup(name,viewer.sceneName)
- if len(pos)==7:
- rotation=pos[3:7]
- pos=pos[0:3]
- if rotation==None:
- rotation=[1,0,0,0]
- else:
- viewer.addLandmark(name,0.1)
- gui.applyConfiguration(name,pos+rotation)
- gui.refresh()
-
-def moveObject(viewer,pos,rotation=[1,0,0,0]):
- viewer.client.gui.applyConfiguration(name,pos+rotation)
- viewer.client.gui.refresh()
-
-def addVector(viewer,rbprmBuilder,color,v,name=None):
- gui = viewer.client.gui
- if name==None:
- i=0
- name='vector_'+str(i)
- while name in gui.getNodeList():
- i=i+1
- name='sphere_'+str(i)
- quat = rbprmBuilder.quaternionFromVector(v[3:6])
- v[3:7] = quat[::]
- gui.addArrow(name,0.02,1,color)
- gui.addToGroup(name,viewer.sceneName)
- gui.setVisibility(name,"ON")
- gui.applyConfiguration(name,v)
- gui.refresh()
-
-
diff --git a/script/tools/constraint_to_dae.py b/script/tools/constraint_to_dae.py
index 2776f71cee59e893d6576c493083408fadc3702d..985024b377d4d898090d01a788ad279e688c4424 100644
--- a/script/tools/constraint_to_dae.py
+++ b/script/tools/constraint_to_dae.py
@@ -1,6 +1,6 @@
import subprocess
import os
-DIR = "/local/fernbac/bench_iros18/constraints_obj/"
+DIR = "/local/fernbach/qhull/constraints_obj/"
STAB_NAME = "stability"
CONS_NAME = "constraints"
KIN_NAME = "kinematics"
@@ -10,8 +10,9 @@ BEZIER_NAME = "bezier_wp"
def generate_off_file(name):
os.remove(DIR+name+"_.off") if os.path.isfile(DIR+name+"_.off") else None
os.remove(DIR+name+".off") if os.path.isfile(DIR+name+".off") else None
-
- cmd = "cat "+DIR+name+".txt | qhalf Fp | qconvex o >> "+DIR+name+"_.off"
+
+ #cmd = "cat "+DIR+name+".txt | qhalf Fp | qconvex o >> "+DIR+name+"_.off"
+ cmd = "cat "+DIR+name+".txt | qhalf FP | qconvex Ft >> "+DIR+name+"_.off"
try :
subprocess.check_output(cmd,shell=True)
print "qHull OK for file : "+name+".txt"
diff --git a/script/tools/createActionDRP.py b/script/tools/createActionDRP.py
new file mode 100644
index 0000000000000000000000000000000000000000..36d5cb2bbd0d368fe96b69320a95b93a13411e96
--- /dev/null
+++ b/script/tools/createActionDRP.py
@@ -0,0 +1,138 @@
+# Create actions YAML file
+
+import yaml
+#import anymal_darpa_Asil as drp
+
+def autoBaseFootstep(configs,fullbody,filename):
+ initial = """\
+steps:
+
+ - step:
+ - base_auto:
+ height:
+ average_linear_velocity: 0.5
+
+ - step:
+ - base_auto:
+ average_linear_velocity: 0.5
+ - footstep:
+ name: RF_LEG
+ target:
+ frame: odom
+ position:
+
+ - step:
+ - base_auto:
+ average_linear_velocity: 0.5
+
+ - step:
+ - base_auto:
+ average_linear_velocity: 0.5
+ - footstep:
+ name: LH_LEG
+ target:
+ frame: odom
+ position:
+
+ - step:
+ - base_auto:
+ average_linear_velocity: 0.5
+ - step:
+ - base_auto:
+ average_linear_velocity: 0.5
+ - footstep:
+ name: LF_LEG
+ target:
+ frame: odom
+ position:
+
+ - step:
+ - base_auto:
+ average_linear_velocity: 0.5
+
+ - step:
+ - base_auto:
+ average_linear_velocity: 0.5
+ - footstep:
+ name: RH_LEG
+ target:
+ frame: odom
+ position:
+
+ - step:
+ - base_auto:
+ average_linear_velocity: 0.5
+ """
+ data = yaml.load(initial)
+
+ LF_footPos = fullbody.getEffectorPosition("LFleg", configs[0])
+ LH_footPos = fullbody.getEffectorPosition("LHleg", configs[0])
+ RF_footPos = fullbody.getEffectorPosition("RFleg", configs[0])
+ RH_footPos = fullbody.getEffectorPosition("RHleg", configs[0])
+
+ data['steps'][0]['step'][0]['base_auto']['height'] = configs[0][2]
+ data['steps'][1]['step'][1]['footstep']['target']['position'] = RF_footPos[0]
+ data['steps'][3]['step'][1]['footstep']['target']['position'] = LH_footPos[0]
+ data['steps'][5]['step'][1]['footstep']['target']['position'] = LF_footPos[0]
+ data['steps'][7]['step'][1]['footstep']['target']['position'] = RH_footPos[0]
+
+
+ for i in range(1, len(configs)):
+ step = """\
+ - step:
+ - base_auto:
+ average_linear_velocity: 0.5
+ - footstep:
+ name:
+ target:
+ frame: odom
+ position:
+
+ - step:
+ - base_auto:
+ average_linear_velocity: 0.5
+ """
+ step_data = yaml.load(step)
+
+ if i == (len(configs)-1):
+ footPos = fullbody.getEffectorPosition("RFleg", configs[i])
+ step_data[0]['step'][1]['footstep']['name'] = 'RF_LEG'
+ step_data[0]['step'][1]['footstep']['target']['position'] = footPos[0]
+
+ footPos = fullbody.getEffectorPosition("LHleg", configs[i])
+ step_data[0]['step'][1]['footstep']['name'] = 'LH_LEG'
+ step_data[0]['step'][1]['footstep']['target']['position'] = footPos[0]
+
+ footPos = fullbody.getEffectorPosition("LFleg", configs[i])
+ step_data[0]['step'][1]['footstep']['name'] = 'LF_LEG'
+ step_data[0]['step'][1]['footstep']['target']['position'] = footPos[0]
+
+ footPos = fullbody.getEffectorPosition("RHleg", configs[i])
+ step_data[0]['step'][1]['footstep']['name'] = 'RH_LEG'
+ step_data[0]['step'][1]['footstep']['target']['position'] = footPos[0]
+
+ movedLeg = fullbody.getContactsVariations(i-1, i)
+ if movedLeg[0] == 'LFleg':
+ footPos = fullbody.getEffectorPosition("LFleg", configs[i])
+ step_data[0]['step'][1]['footstep']['name'] = 'LF_LEG'
+ step_data[0]['step'][1]['footstep']['target']['position'] = footPos[0]
+ elif movedLeg[0] == 'LHleg':
+ footPos = fullbody.getEffectorPosition("LHleg", configs[i])
+ step_data[0]['step'][1]['footstep']['name'] = 'LH_LEG'
+ step_data[0]['step'][1]['footstep']['target']['position'] = footPos[0]
+ elif movedLeg[0] == 'RFleg':
+ footPos = fullbody.getEffectorPosition("RFleg", configs[i])
+ step_data[0]['step'][1]['footstep']['name'] = 'RF_LEG'
+ step_data[0]['step'][1]['footstep']['target']['position'] = footPos[0]
+ elif movedLeg[0] == 'RHleg':
+ footPos = fullbody.getEffectorPosition("RHleg", configs[i])
+ step_data[0]['step'][1]['footstep']['name'] = 'RH_LEG'
+ step_data[0]['step'][1]['footstep']['target']['position'] = footPos[0]
+
+
+
+ data['steps'].extend(step_data)
+ stream = file(filename+'.yaml', 'w')
+ yaml.dump(data, stream, default_flow_style=False)
+ print('saved .yaml file as '+filename+'.yaml')
+
diff --git a/script/tools/display_tools.py b/script/tools/display_tools.py
index 0dac0b5bcf00bddebbb289c12237c0a8ce916179..918ceaecf09e2404aca3a35e781d1ad8ac0d867d 100644
--- a/script/tools/display_tools.py
+++ b/script/tools/display_tools.py
@@ -16,3 +16,25 @@ def displayContactSequence(r,configs,pause=1.):
for i in range(0,len(configs)):
r(configs[i])
time.sleep(pause)
+
+def moveObject(viewer,pos,rotation=[1,0,0,0]):
+ viewer.client.gui.applyConfiguration(name,pos+rotation)
+ viewer.client.gui.refresh()
+
+def addVector(viewer,rbprmBuilder,color,v,name=None):
+ gui = viewer.client.gui
+ if name==None:
+ i=0
+ name='vector_'+str(i)
+ while name in gui.getNodeList():
+ i=i+1
+ name='sphere_'+str(i)
+ quat = rbprmBuilder.quaternionFromVector(v[3:6])
+ v[3:7] = quat[::]
+ gui.addArrow(name,0.02,1,color)
+ gui.addToGroup(name,viewer.sceneName)
+ gui.setVisibility(name,"ON")
+ gui.applyConfiguration(name,v)
+ gui.refresh()
+
+