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

Merge pull request #27 from pFernbach/topic/tools_scripts

Update and add tools script
parents 403f992a 9a6d529c
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()
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"
......
# 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')
......@@ -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()
Markdown is supported
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