From 923f6d05f37f09061209695462080f1722a6fd3e Mon Sep 17 00:00:00 2001
From: odri <odri@furano.laas.fr>
Date: Wed, 25 Aug 2021 16:30:51 +0200
Subject: [PATCH] Adapt Kp Kd for new vector form + Tweak corba viewer
---
scripts/Controller.py | 10 +++++-----
1 file changed, 5 insertions(+), 5 deletions(-)
diff --git a/scripts/Controller.py b/scripts/Controller.py
index 4e0f7a1a..d9b9bdfa 100644
--- a/scripts/Controller.py
+++ b/scripts/Controller.py
@@ -327,8 +327,8 @@ class Controller:
self.feet_a_cmd)
# Quantities sent to the control board
- self.result.P = self.Kp_main * np.ones(12)
- self.result.D = self.Kd_main * np.ones(12)
+ self.result.P = np.array(self.Kp_main.tolist() * 4)
+ self.result.D = np.array(self.Kd_main.tolist() * 4)
self.result.q_des[:] = self.wbcWrapper.qdes[:]
self.result.v_des[:] = self.wbcWrapper.vdes[:]
self.result.FF = self.Kff_main * np.ones(12)
@@ -336,9 +336,9 @@ class Controller:
# Display robot in Gepetto corba viewer
if self.enable_corba_viewer and (self.k % 5 == 0):
- self.q_display[:3, 0] = self.q[:3, 0]
- self.q_display[3:7, 0] = pin.Quaternion(pin.rpy.rpyToMatrix(self.q[3:6, 0])).coeffs()
- self.q_display[7:, 0] = self.q[6:, 0]
+ self.q_display[:3, 0] = np.array([0.0, 0.0, self.h_ref])
+ self.q_display[3:7, 0] = np.array([0.0, 0.0, 0.0, 1.0])
+ self.q_display[7:, 0] = self.wbcWrapper.qdes[:]
self.solo.display(self.q_display)
t_wbc = time.time()
--
GitLab