diff --git a/scripts/Controller.py b/scripts/Controller.py index 93dae208ce7ef4be2c1f5ee1f8da599bfc2353c1..c9c2ec9ed4e0f33a01f43a7c80d1a4242f2454c2 100644 --- a/scripts/Controller.py +++ b/scripts/Controller.py @@ -106,9 +106,8 @@ class Controller: self.solo, self.fsteps_init, self.h_init = utils_mpc.init_robot(q_init, self.enable_gepetto_viewer) # Create Joystick, FootstepPlanner, Logger and Interface objects - self.joystick, self.logger, self.estimator = utils_mpc.init_objects( - dt_wbc, dt_mpc, N_SIMULATION, k_mpc, T_gait, type_MPC, predefined_vel, self.h_init, kf_enabled, - perfectEstimator) + self.joystick, self.estimator = utils_mpc.init_objects( + dt_wbc, N_SIMULATION, predefined_vel, self.h_init, kf_enabled, perfectEstimator) # Enable/Disable hybrid control self.enable_hybrid_control = True @@ -127,8 +126,6 @@ class Controller: self.gait = lqrw.Gait() self.gait.initialize(dt_mpc, T_gait, T_mpc, N_gait) - """from IPython import embed - embed()""" shoulders = np.zeros((3, 4)) shoulders[0, :] = [0.1946, 0.1946, -0.1946, -0.1946] diff --git a/scripts/utils_mpc.py b/scripts/utils_mpc.py index 0fea358d113e0ea237b3c7e728fe59e9d7e60f6f..108592f8fc707fba9c41f51603bfc2a30796443e 100644 --- a/scripts/utils_mpc.py +++ b/scripts/utils_mpc.py @@ -129,16 +129,12 @@ def init_robot(q_init, enable_viewer): return solo, fsteps_init, h_init -def init_objects(dt_tsid, dt_mpc, k_max_loop, k_mpc, T_mpc, type_MPC, predefined, h_init, kf_enabled, perfectEstimator): +def init_objects(dt_tsid, k_max_loop, predefined, h_init, kf_enabled, perfectEstimator): """Create several objects that are used in the control loop Args: dt_tsid (float): time step of TSID - dt_mpc (float): time step of the MPC k_max_loop (int): maximum number of iterations of the simulation - k_mpc (int): number of tsid iterations for one iteration of the mpc - T_mpc (float): duration of mpc prediction horizon - type_MPC (bool): which MPC you want to use (PA's or Thomas') predefined (bool): if we are using a predefined reference velocity (True) or a joystick (False) h_init (float): initial height of the robot base kf_enabled (bool): complementary filter (False) or kalman filter (True) @@ -148,13 +144,10 @@ def init_objects(dt_tsid, dt_mpc, k_max_loop, k_mpc, T_mpc, type_MPC, predefined # Create Joystick object joystick = Joystick.Joystick(predefined) - # Create logger object - logger = Logger.Logger(k_max_loop, dt_tsid, dt_mpc, k_mpc, T_mpc, type_MPC) - # Create Estimator object estimator = Estimator.Estimator(dt_tsid, k_max_loop, h_init, kf_enabled, perfectEstimator) - return joystick, logger, estimator + return joystick, estimator def display_all(solo, k, sequencer, fstep_planner, ftraj_gen, mpc):