From 95ce7dae4153f6bcae32b5411e29c1018be5765c Mon Sep 17 00:00:00 2001
From: Ale <alessandroassirell98@gmail.com>
Date: Wed, 27 Jul 2022 11:27:20 +0200
Subject: [PATCH] 1KHz mpc (reduced model) Closed loop EndEff tracking cost
Impedance + Feedforward control
---
.../main_solo12_control.py | 14 ++++++++++----
.../tools/LoggerControl.py | 13 +++++++------
2 files changed, 17 insertions(+), 10 deletions(-)
diff --git a/python/quadruped_reactive_walking/main_solo12_control.py b/python/quadruped_reactive_walking/main_solo12_control.py
index 57a054bc..ba51a625 100644
--- a/python/quadruped_reactive_walking/main_solo12_control.py
+++ b/python/quadruped_reactive_walking/main_solo12_control.py
@@ -3,6 +3,7 @@ import time
from pathlib import Path
import numpy as np
import git
+from datetime import datetime
import quadruped_reactive_walking as qrw
from .Controller import Controller
@@ -18,7 +19,7 @@ target.update(0)
repo = git.Repo(search_parent_directories=True)
sha = repo.head.object.hexsha
-msg = repo.head.object.message
+msg = repo.head.object.message + "\nCommit: " + sha
if params.SIMULATION:
from .tools.PyBulletSimulator import PyBulletSimulator
@@ -227,11 +228,16 @@ def control_loop():
print("Masterboard timeout detected.")
if params.LOGGING:
- log_path = Path("/tmp") / "logs"
+ date_str = datetime.now().strftime("%Y_%m_%d_%H_%M")
+ log_path = Path("/tmp") / "logs" / date_str
+ log_path.mkdir(parents=True)
loggerControl.save(str(log_path))
+ with open(str(log_path / 'readme.txt') , 'w') as f:
+ f.write(msg)
- if params.PLOTTING:
- loggerControl.plot()
+ if params.PLOTTING:
+ loggerControl.plot(save=True, fileName=str(log_path))
+ print("Plots saved in ", str(log_path) + "/")
if params.SIMULATION and params.enable_pyb_GUI:
device.Stop()
diff --git a/python/quadruped_reactive_walking/tools/LoggerControl.py b/python/quadruped_reactive_walking/tools/LoggerControl.py
index 4258d900..4270e6ee 100644
--- a/python/quadruped_reactive_walking/tools/LoggerControl.py
+++ b/python/quadruped_reactive_walking/tools/LoggerControl.py
@@ -202,7 +202,7 @@ class LoggerControl:
plt.legend(legend)
plt.draw()
if save:
- plt.savefig(fileName + "_joint_positions")
+ plt.savefig(fileName + "/joint_positions")
plt.figure(figsize=(12, 6), dpi=90)
i = 0
@@ -218,7 +218,7 @@ class LoggerControl:
plt.legend(legend)
plt.draw()
if save:
- plt.savefig(fileName + "_joint_velocities")
+ plt.savefig(fileName + "/joint_velocities")
plt.figure(figsize=(12, 6), dpi=90)
i = 0
@@ -234,7 +234,7 @@ class LoggerControl:
plt.legend(legend)
plt.draw()
if save:
- plt.savefig(fileName + "_joint_torques")
+ plt.savefig(fileName + "/joint_torques")
legend = ["x", "y", "z"]
plt.figure(figsize=(12, 18), dpi = 90)
@@ -245,6 +245,8 @@ class LoggerControl:
plt.plot(m_feet_p_log[self.pd.rfFootId][:, p])
plt.plot(feet_p_log[self.pd.rfFootId][:, p])
plt.legend(["Target", "Measured", "Predicted"])
+ if save:
+ plt.savefig(fileName + "/target")
self.plot_controller_times()
self.plot_OCP_times()
@@ -307,8 +309,7 @@ class LoggerControl:
plt.ylabel("Time [s]")
def save(self, fileName="data"):
- date_str = datetime.now().strftime("_%Y_%m_%d_%H_%M")
- name = fileName + date_str + ".npz"
+ name = fileName + "/data.npz"
np.savez_compressed(
name,
@@ -349,7 +350,7 @@ class LoggerControl:
voltage=self.voltage,
energy=self.energy,
)
- print("Logs and plots saved in " + name)
+ print("Logs saved in " + name)
def load(self):
if self.data is None:
--
GitLab