Merge pull request #27 from pFernbach/topic/tools_scripts

Update and add tools script

script/scenarios/sandbox/dynamic/tools.py

-
-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()
-
-

script/tools/constraint_to_dae.py

 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"

script/tools/createActionDRP.py

+# 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')
+

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()
+
+