From 9a6d529c694bbe329f7299ae76167e9a7a29fb03 Mon Sep 17 00:00:00 2001
From: pFernbach <pierre.fernbach@gmail.com>
Date: Mon, 18 Feb 2019 17:39:52 +0100
Subject: [PATCH] [tools] add script to export CS to free gait
---
script/tools/createActionDRP.py | 138 ++++++++++++++++++++++++++++++++
1 file changed, 138 insertions(+)
create mode 100644 script/tools/createActionDRP.py
diff --git a/script/tools/createActionDRP.py b/script/tools/createActionDRP.py
new file mode 100644
index 00000000..36d5cb2b
--- /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')
+
--
GitLab