diff --git a/scripts/QP_WBC.py b/scripts/QP_WBC.py
index c9f2a6b80c275235a0d46884f5433ee0c699a2d4..9e5530665179ab68ec3f06592f5ab887e0717f6a 100644
--- a/scripts/QP_WBC.py
+++ b/scripts/QP_WBC.py
@@ -17,8 +17,11 @@ class controller():
self.invKin = Solo12InvKin(dt) # Inverse Kinematics object
self.qp_wbc = QP_WBC() # QP of the WBC
- # self.box_qp_wbc = BOX_QP_WBC() # QP of the WBC
+
self.box_qp = lrw.QPWBC()
+ self.M = np.zeros((18, 18))
+ self.Jc = np.zeros((12, 18))
+
self.x = np.zeros(18) # solution of WBC QP
self.error = False # Set to True when an error happens in the controller
@@ -89,61 +92,37 @@ class controller():
#self.tac = time()
- """self.box_qp_wbc.compute_matrices(self.invKin.rmodel, self.invKin.rdata, q, dq, ddq_cmd, np.array([f_cmd]).T, contacts)
- self.box_qp_wbc.create_weight_matrices()"""
- # Solve QP problem
- # self.box_qp_wbc.compute(self.invKin.rmodel, self.invKin.rdata,
- # q.copy(), dq.copy(), ddq_cmd, np.array([f_cmd]).T, contacts)
-
- # Solve QP problem
+ # Solve QP problem with Python version
"""self.qp_wbc.compute(self.invKin.rmodel, self.invKin.rdata,
q.copy(), dq.copy(), ddq_cmd, np.array([f_cmd]).T, contacts)"""
# Compute the joint space inertia matrix M by using the Composite Rigid Body Algorithm
- M = pin.crba(self.invKin.rmodel, self.invKin.rdata, q)
- # M = np.zeros(Mfull.shape)
- # M[:6, :6] = Mfull[:6, :6]
+ self.M = pin.crba(self.invKin.rmodel, self.invKin.rdata, q)
- # Contact Jacobian
+ # Compute Jacobian of contact points
# Indexes of feet frames in this order: [FL, FR, HL, HR]
indexes = [10, 18, 26, 34]
- Jc = np.zeros((12, 18))
+ self.Jc = np.zeros((12, 18))
for i in range(4):
if contacts[i]:
- Jc[(3*i):(3*(i+1)), :] = pin.getFrameJacobian(self.invKin.rmodel, self.invKin.rdata, indexes[i],
+ self.Jc[(3*i):(3*(i+1)), :] = pin.getFrameJacobian(self.invKin.rmodel, self.invKin.rdata, indexes[i],
pin.LOCAL_WORLD_ALIGNED)[:3, :]
- ddq_cmd_tmp = np.zeros((18, 1))
- ddq_cmd_tmp[:6, 0] = ddq_cmd[:6, 0]
+
+ # Compute joint torques according to the current state of the system and the desired joint accelerations
RNEA = pin.rnea(self.invKin.rmodel, self.invKin.rdata, q, dq, ddq_cmd)[:6]
- self.box_qp.run(M.copy(), Jc.copy(), f_cmd.reshape((-1, 1)).copy(), RNEA.reshape((-1, 1)).copy())
+ # Solve the QP problem with C++ bindings
+ self.box_qp.run(self.M, self.Jc, f_cmd.reshape((-1, 1)), RNEA.reshape((-1, 1)))
+ # Add deltas found by the QP problem to reference quantities
deltaddq = self.box_qp.get_ddq_res()
f_with_delta = self.box_qp.get_f_res().reshape((-1, 1))
ddq_with_delta = ddq_cmd.copy()
ddq_with_delta[:6, 0] += deltaddq
- RNEA_delta = pin.rnea(self.invKin.rmodel, self.invKin.rdata, q, dq, ddq_with_delta)[6:]
- Ja = Jc[:, 6:].transpose()
- tauff = RNEA_delta - (Ja @ f_with_delta).ravel()
-
- """ta = pin.rnea(self.invKin.rmodel, self.invKin.rdata, q, dq, ddq_with_delta)[:6]
- tb = Jc[:, :6].transpose()
- tc = ta - (tb @ f_with_delta).ravel()
- print(tc)"""
-
-
- #return (self.A @ (self.ddq_cmd + self.delta_ddq) + np.array([self.NLE]).transpose()
- # - self.JcT @ (self.f_cmd + np.array([self.x[6:]]).transpose()))[6:, ]
-
- """if dq[0, 0] > 0.4:
- from IPython import embed
- embed()"""
-
-
- # Retrieve joint torques
- # tmp_res = self.box_qp_wbc.get_joint_torques(self.invKin.rmodel, self.invKin.rdata, q, dq, ddq_cmd)
- self.tau_ff[:] = tauff[:] # self.qp_wbc.get_joint_torques().ravel()
+ # Compute joint torques from contact forces and desired accelerations
+ RNEA_delta = pin.rnea(self.invKin.rmodel, self.invKin.rdata, q, dq, ddq_with_delta)[6:]
+ self.tau_ff[:] = RNEA_delta - ((self.Jc[:, 6:].transpose()) @ f_with_delta).ravel()
"""print("f python:", self.qp_wbc.f_cmd.ravel() + np.array(self.qp_wbc.x[6:]))
print("f cpp :", f_with_delta.ravel())
@@ -151,15 +130,8 @@ class controller():
print("ddq python:", (self.qp_wbc.ddq_cmd + self.qp_wbc.delta_ddq).ravel())
print("ddq cpp :", ddq_with_delta.ravel())"""
-
- """from IPython import embed
- embed()"""
-
#self.toc = time()
- """from IPython import embed
- embed()"""
-
# Retrieve desired positions and velocities
self.vdes[:, 0] = self.invKin.dq_cmd # (dq + ddq_cmd * self.dt).ravel() # v des in world frame
self.qdes[:] = self.invKin.q_cmd # pin.integrate(self.invKin.robot.model, q, self.vdes * self.dt)
@@ -198,9 +170,8 @@ class controller():
self.log_tstamps[self.k_log] = clock()"""
- """if dq[0, 0] > 0.02:
- from IPython import embed
- embed()"""
+ """from IPython import embed
+ embed()"""
self.k_log += 1
diff --git a/src/QPWBC.cpp b/src/QPWBC.cpp
index 42f8d6bf0b0170303b93109e63b392b69355bcfe..38d42e409c50593f41b052d5cbff8b555f95630c 100644
--- a/src/QPWBC.cpp
+++ b/src/QPWBC.cpp
@@ -485,4 +485,4 @@ void QPWBC::update_PQ() {
/*char t_char[1] = {'P'};
my_print_csc_matrix(P, t_char);*/
-}
\ No newline at end of file
+}