diff --git a/scripts/Controller.py b/scripts/Controller.py
index 59b0dd7d2638d20e3d90d780e9e2afa8926498fa..7bf9041a9c4fde0f6d896fc5b9b24d8f5dc98f0a 100644
--- a/scripts/Controller.py
+++ b/scripts/Controller.py
@@ -5,8 +5,8 @@ import utils_mpc
import time
import math
-from QP_WBC import wbc_controller
import MPC_Wrapper
+import Joystick
import pybullet as pyb
import pinocchio as pin
from solopython.utils.viewerClient import viewerClient, NonBlockingViewerFromRobot
@@ -125,7 +125,7 @@ class Controller:
self.solo = utils_mpc.init_robot(q_init, params, self.enable_gepetto_viewer)
# Create Joystick object
- self.joystick = utils_mpc.init_objects(params)
+ self.joystick = Joystick.Joystick(params)
# Enable/Disable hybrid control
self.enable_hybrid_control = True
@@ -283,9 +283,6 @@ class Controller:
else:
self.x_f_mpc[12:, :] = self.x_save.copy()"""
- """from IPython import embed
- embed()"""
-
t_mpc = time.time()
# If the MPC optimizes footsteps positions then we use them
diff --git a/scripts/QP_WBC.py b/scripts/QP_WBC.py
deleted file mode 100644
index 427f03bd45611e750694129688daffe42936498d..0000000000000000000000000000000000000000
--- a/scripts/QP_WBC.py
+++ /dev/null
@@ -1,136 +0,0 @@
-# coding: utf8
-
-import numpy as np
-import pinocchio as pin
-from time import perf_counter as clock
-from time import time
-import libquadruped_reactive_walking as lrw
-from example_robot_data.robots_loader import Solo12Loader
-
-class wbc_controller():
- """Whole body controller which contains an Inverse Kinematics step and a BoxQP step
-
- Args:
- dt (float): time step of the whole body control
- """
-
- def __init__(self, params):
-
- Solo12Loader.free_flyer = True
- self.robot = Solo12Loader().robot
-
- self.dt = params.dt_wbc # Time step
-
- self.invKin = lrw.InvKin() # Inverse Kinematics solver in C++
- self.invKin.initialize(params)
-
- self.box_qp = lrw.QPWBC() # Box Quadratic Programming solver
- self.box_qp.initialize(params)
-
- self.M = np.zeros((18, 18))
- self.Jc = np.zeros((12, 18))
-
- self.error = False # Set to True when an error happens in the controller
-
- self.k_since_contact = np.zeros((1, 4))
-
- # Logging
- self.k_log = 0
- self.log_feet_pos = np.zeros((3, 4, params.N_SIMULATION))
- self.log_feet_err = np.zeros((3, 4, params.N_SIMULATION))
- self.log_feet_vel = np.zeros((3, 4, params.N_SIMULATION))
- self.log_feet_pos_target = np.zeros((3, 4, params.N_SIMULATION))
- self.log_feet_vel_target = np.zeros((3, 4, params.N_SIMULATION))
- self.log_feet_acc_target = np.zeros((3, 4, params.N_SIMULATION))
-
- # Arrays to store results (for solo12)
- self.qdes = np.zeros(12)
- self.vdes = np.zeros(12)
- self.tau_ff = np.zeros(12)
-
- # Indexes of feet frames in this order: [FL, FR, HL, HR]
- self.indexes = [10, 18, 26, 34]
-
- def compute(self, q, dq, f_cmd, contacts, pgoals, vgoals, agoals):
- """ Call Inverse Kinematics to get an acceleration command then
- solve a QP problem to get the feedforward torques
-
- Args:
- q (19x1): Current state of the base
- dq (18x1): Current velocity of the base (in base frame)
- f_cmd (1x12): Contact forces references from the mpc
- contacts (1x4): Contact status of feet
- planner (object): Object that contains the pos, vel and acc references for feet
- """
-
- # Update nb of iterations since contact
- self.k_since_contact += contacts # Increment feet in stance phase
- self.k_since_contact *= contacts # Reset feet in swing phase
-
- self.tic = time()
-
- # Compute Inverse Kinematics
- self.invKin.run_InvKin(q[7:, 0:1], dq[6:, 0:1], np.array([contacts]), pgoals.transpose(), vgoals.transpose(), agoals.transpose())
- ddq_cmd = np.zeros((18, 1))
- ddq_cmd[6:, 0] = self.invKin.get_ddq_cmd()
-
- #Â TODO: Adapt logging
- """for i in range(4):
- self.log_feet_pos[:, i, self.k_log] = self.invKin.robot.data.oMf[self.indexes[i]].translation
- self.log_feet_err[:, i, self.k_log] = pgoals[:, i] - self.invKin.robot.data.oMf[self.indexes[i]].translation # self.invKin.pfeet_err[i]
- self.log_feet_vel[:, i, self.k_log] = pin.getFrameVelocity(self.invKin.robot.model, self.invKin.robot.data,
- self.indexes[i], pin.LOCAL_WORLD_ALIGNED).linear"""
- self.feet_pos = self.log_feet_pos[:, :, self.k_log]
- self.feet_err = self.log_feet_err[:, :, self.k_log]
- self.feet_vel = self.log_feet_vel[:, :, self.k_log]
-
- self.log_feet_pos_target[:, :, self.k_log] = pgoals[:, :]
- self.log_feet_vel_target[:, :, self.k_log] = vgoals[:, :]
- self.log_feet_acc_target[:, :, self.k_log] = agoals[:, :]
-
- self.tac = time()
-
- # Compute the joint space inertia matrix M by using the Composite Rigid Body Algorithm
- q_tmp = np.zeros((19, 1))
- q_tmp[6, 0] = 1.0
- self.M = pin.crba(self.robot.model, self.robot.data, q_tmp)
-
- # self.M[:6, :6] = self.M[:6, :6] * (np.eye(6) == 1) # (self.M[:6, :6] > 1e-3)
-
- # Compute Jacobian of contact points
- pin.computeJointJacobians(self.robot.model, self.robot.data, q)
- self.Jc = np.zeros((12, 18))
- for i_ee in range(4):
- if contacts[i_ee]:
- idx = self.invKin.get_foot_id(i_ee)
- self.Jc[(3*i_ee):(3*(i_ee+1)), :] = pin.getFrameJacobian(self.robot.model, self.robot.data, idx, pin.LOCAL_WORLD_ALIGNED)[:3]
-
- # Compute joint torques according to the current state of the system and the desired joint accelerations
- RNEA = pin.rnea(self.robot.model, self.robot.data, q, dq, ddq_cmd)[:6]
-
- # Solve the QP problem with C++ bindings
- self.box_qp.run(self.M, self.Jc, f_cmd.reshape((-1, 1)), RNEA.reshape((-1, 1)), self.k_since_contact)
-
- # Add deltas found by the QP problem to reference quantities
- deltaddq = self.box_qp.get_ddq_res()
- self.f_with_delta = self.box_qp.get_f_res().reshape((-1, 1))
- ddq_with_delta = ddq_cmd.copy()
- ddq_with_delta[:6, 0] += deltaddq
-
- # Compute joint torques from contact forces and desired accelerations
- RNEA_delta = pin.rnea(self.robot.model, self.robot.data, q, dq, ddq_with_delta)[6:]
- self.tau_ff[:] = RNEA_delta - ((self.Jc[:, 6:].transpose()) @ self.f_with_delta).ravel()
-
- # Retrieve desired positions and velocities
- self.vdes[:] = self.invKin.get_dq_cmd()
- self.qdes[:] = self.invKin.get_q_cmd()
-
- self.toc = time()
-
- """self.tic = 0.0
- self.tac = 0.0
- self.toc = 0.0"""
-
- self.k_log += 1
-
- return 0