From 7124575dc431bb78ffce772763f090c95fc3be05 Mon Sep 17 00:00:00 2001
From: paleziart <paleziart@laas.fr>
Date: Mon, 6 Sep 2021 14:07:37 +0200
Subject: [PATCH] Formatting Controller.py
---
scripts/Controller.py | 40 ++++++++++++++++++++++------------------
1 file changed, 22 insertions(+), 18 deletions(-)
diff --git a/scripts/Controller.py b/scripts/Controller.py
index ce733396..19c80ee2 100644
--- a/scripts/Controller.py
+++ b/scripts/Controller.py
@@ -13,6 +13,7 @@ from solopython.utils.viewerClient import viewerClient, NonBlockingViewerFromRob
import libquadruped_reactive_walking as lqrw
from example_robot_data.robots_loader import Solo12Loader
+
class Result:
"""Object to store the result of the control loop
It contains what is sent to the robot (gains, desired positions and velocities,
@@ -137,8 +138,8 @@ class Controller:
# Wrapper that makes the link with the solver that you want to use for the MPC
self.mpc_wrapper = MPC_Wrapper.MPC_Wrapper(params, self.q)
- self.o_targetFootstep = np.zeros((3,4)) # Store result for MPC_planner
-
+ self.o_targetFootstep = np.zeros((3, 4)) # Store result for MPC_planner
+
# ForceMonitor to display contact forces in PyBullet with red lines
# import ForceMonitor
# myForceMonitor = ForceMonitor.ForceMonitor(pyb_sim.robotId, pyb_sim.planeId)
@@ -235,7 +236,7 @@ class Controller:
self.q[:, 0] = self.estimator.getQUpdated()
self.v[:, 0] = self.estimator.getVUpdated()
self.yaw_estim = self.estimator.getYawEstim()
- # TODO: Understand why using Python or C++ h_v leads to a slightly different result since the
+ # TODO: Understand why using Python or C++ h_v leads to a slightly different result since the
# difference between them at each time step is 1e-16 at max (butterfly effect?)
t_filter = time.time()
@@ -275,12 +276,12 @@ class Controller:
# Solve MPC problem once every k_mpc iterations of the main loop
if (self.k % self.k_mpc) == 0:
try:
- if self.type_MPC == 3 :
+ if self.type_MPC == 3:
# Compute the target foostep in local frame, to stop the optimisation around it when t_lock overpass
l_targetFootstep = oRh.transpose() @ (self.o_targetFootstep - oTh)
self.mpc_wrapper.solve(self.k, xref, fsteps, cgait, l_targetFootstep, oRh, oTh)
- else :
- self.mpc_wrapper.solve(self.k, xref, fsteps, cgait, np.zeros((3,4)))
+ else:
+ self.mpc_wrapper.solve(self.k, xref, fsteps, cgait, np.zeros((3, 4)))
except ValueError:
print("MPC Problem")
@@ -294,14 +295,14 @@ class Controller:
t_mpc = time.time()
# If the MPC optimizes footsteps positions then we use them
- if self.k > 100 and self.type_MPC == 3 :
- for foot in range(4):
- if cgait[0,foot] == 0 :
+ if self.k > 100 and self.type_MPC == 3:
+ for foot in range(4):
+ if cgait[0, foot] == 0:
id = 0
- while cgait[id,foot] == 0 :
+ while cgait[id, foot] == 0:
id += 1
- self.o_targetFootstep[:2,foot] = self.x_f_mpc[24 + 2*foot:24+2*foot+2, id+1]
-
+ self.o_targetFootstep[:2, foot] = self.x_f_mpc[24 + 2*foot:24+2*foot+2, id+1]
+
# Update pos, vel and acc references for feet
self.footTrajectoryGenerator.update(self.k, self.o_targetFootstep)
@@ -310,23 +311,25 @@ class Controller:
if (not self.error) and (not self.joystick.stop):
# Update configuration vector for wbc
- self.q_wbc[2, 0] = self.q_filt_mpc[2, 0] #Â Height
+ self.q_wbc[2, 0] = self.q_filt_mpc[2, 0] # Height
self.q_wbc[3, 0] = self.q_filt_mpc[3, 0] # Roll
self.q_wbc[4, 0] = self.q_filt_mpc[4, 0] # Pitch
self.q_wbc[6:, 0] = self.wbcWrapper.qdes[:] # with reference angular positions of previous loop
# Update velocity vector for wbc
- self.dq_wbc[:6, 0] = self.estimator.getVFilt()[:6] #Â Velocities in base frame (not horizontal frame!)
+ self.dq_wbc[:6, 0] = self.estimator.getVFilt()[:6] # Â Velocities in base frame (not horizontal frame!)
self.dq_wbc[6:, 0] = self.wbcWrapper.vdes[:] # with reference angular velocities of previous loop
# Feet command position, velocity and acceleration in base frame
- self.feet_a_cmd = self.footTrajectoryGenerator.getFootAccelerationBaseFrame(oRh.transpose(), np.zeros((3, 1)), np.zeros((3, 1)))
- self.feet_v_cmd = self.footTrajectoryGenerator.getFootVelocityBaseFrame(oRh.transpose(), np.zeros((3, 1)), np.zeros((3, 1)))
+ self.feet_a_cmd = self.footTrajectoryGenerator.getFootAccelerationBaseFrame(
+ oRh.transpose(), np.zeros((3, 1)), np.zeros((3, 1)))
+ self.feet_v_cmd = self.footTrajectoryGenerator.getFootVelocityBaseFrame(
+ oRh.transpose(), np.zeros((3, 1)), np.zeros((3, 1)))
self.feet_p_cmd = self.footTrajectoryGenerator.getFootPositionBaseFrame(oRh.transpose(), oTh)
#Â Desired position, orientation and velocities of the base
if not self.gait.getIsStatic():
- self.xgoals[2:5, 0] = [self.h_ref, 0.0, 0.0] #Â Height (in horizontal frame!)
+ self.xgoals[2:5, 0] = [self.h_ref, 0.0, 0.0] # Â Height (in horizontal frame!)
else:
self.xgoals[2:5, 0] += self.vref_filt_mpc[2:5, 0] * self.dt_wbc
@@ -361,7 +364,8 @@ class Controller:
self.security_check()
# Update PyBullet camera
- self.pyb_camera(device, 0.0) # to have yaw update in simu: utils_mpc.quaternionToRPY(self.estimator.q_filt[3:7, 0])[2, 0]
+ # to have yaw update in simu: utils_mpc.quaternionToRPY(self.estimator.q_filt[3:7, 0])[2, 0]
+ self.pyb_camera(device, 0.0)
# Logs
self.log_misc(t_start, t_filter, t_planner, t_mpc, t_wbc)
--
GitLab