diff --git a/include/qrw/MPC.hpp b/include/qrw/MPC.hpp
index 002eaa9a29cdc4a16988084a42d6d78e0aa24c39..d42ae05dce07d1dd751b7993ca4a725b7b894ce6 100644
--- a/include/qrw/MPC.hpp
+++ b/include/qrw/MPC.hpp
@@ -50,7 +50,7 @@ class MPC {
/// \param[in] fsteps_in Footsteps location over the prediction horizon stored in a Nx12 matrix
///
////////////////////////////////////////////////////////////////////////////////////////////////
- int run(int num_iter, const Eigen::MatrixXd &xref_in, const Eigen::MatrixXd &fsteps_in, const Eigen::MatrixXd &nle);
+ int run(int num_iter, const Eigen::MatrixXd &xref_in, const Eigen::MatrixXd &fsteps_in);
// Getters
Eigen::MatrixXd get_latest_result(); // Return the latest desired contact forces that have been computed
@@ -157,7 +157,7 @@ class MPC {
/// \param[in] fsteps Footsteps location over the prediction horizon stored in a Nx12 matrix
///
////////////////////////////////////////////////////////////////////////////////////////////////
- int update_matrices(Eigen::MatrixXd fsteps, const Eigen::Matrix<double, 6, 1> &nle);
+ int update_matrices(Eigen::MatrixXd fsteps);
////////////////////////////////////////////////////////////////////////////////////////////////
///
@@ -173,7 +173,7 @@ class MPC {
/// \brief Update the N and K matrices involved in the MPC constraint equations M.X = N and L.X <= K
///
////////////////////////////////////////////////////////////////////////////////////////////////
- int update_NK(const Eigen::Matrix<double, 6, 1> &nle);
+ int update_NK();
////////////////////////////////////////////////////////////////////////////////////////////////
///
diff --git a/scripts/Controller.py b/scripts/Controller.py
index e41b192ea1e2942ba898172e6357dc0fe27241b1..9aae51fec7ac86ead1586a96b1c7d5e24a2fef6b 100644
--- a/scripts/Controller.py
+++ b/scripts/Controller.py
@@ -283,7 +283,7 @@ class Controller:
self.footTrajectoryGenerator.getFootJerk(),
self.footTrajectoryGenerator.getTswing() - self.footTrajectoryGenerator.getT0s())
else :
- self.mpc_wrapper.solve(self.k, xref, fsteps, cgait, np.zeros((3,4)), nle=self.nle)
+ self.mpc_wrapper.solve(self.k, xref, fsteps, cgait, np.zeros((3,4)))
except ValueError:
print("MPC Problem")
@@ -350,7 +350,7 @@ class Controller:
self.xgoals[6:, 0] = self.vref_filt_mpc[:, 0] # Velocities (in horizontal frame!)
- print(" ###### ")
+ #print(" ###### ")
# Run InvKin + WBC QP
self.wbcWrapper.compute(self.q_wbc, self.dq_wbc,
@@ -385,7 +385,7 @@ class Controller:
device.imu.attitude_euler[2])).coeffs().tolist()
pyb.resetBasePositionAndOrientation(device.pyb_sim.robotId, oTh_pyb, q_oRb_pyb)"""
- if self.k >= 8220 and (self.k % self.k_mpc == 0):
+ """if self.k >= 8220 and (self.k % self.k_mpc == 0):
print(self.k)
print("x_f_mpc: ", self.x_f_mpc[:, 0])
print("ddq delta: ", self.wbcWrapper.ddq_with_delta)
@@ -395,7 +395,7 @@ class Controller:
plt.plot(self.x_f_mpc[6, :])
plt.show(block=True)
- print("f delta: ", self.wbcWrapper.f_with_delta)
+ print("f delta: ", self.wbcWrapper.f_with_delta)"""
"""if self.k == 1:
quit()"""
@@ -410,7 +410,7 @@ class Controller:
self.pyb_camera(device, 0.0)
# Update debug display (spheres, ...)
- self.pyb_debug(device, fsteps, cgait, xref)
+ # self.pyb_debug(device, fsteps, cgait, xref)
# Logs
self.log_misc(t_start, t_filter, t_planner, t_mpc, t_wbc)
diff --git a/scripts/ForceMonitor.py b/scripts/ForceMonitor.py
index 3cd0e498df93719d156a6a863a63378faf1bfb3b..bb573625c3b02abd7632f379b36a2615fb49ea13 100644
--- a/scripts/ForceMonitor.py
+++ b/scripts/ForceMonitor.py
@@ -67,7 +67,7 @@ class ForceMonitor:
f_x += f_tmp[0]
f_y += f_tmp[1]
f_z += f_tmp[2]
- print("Contact: ", -f_tmp[0], -f_tmp[1], -f_tmp[2])
+ # print("Contact: ", -f_tmp[0], -f_tmp[1], -f_tmp[2])
if (i_line+1) > len(self.lines): # If not enough existing lines in line storage a new item is created
lineID = pyb.addUserDebugLine(start, end, lineColorRGB=[1.0, 0.0, 0.0], lineWidth=8)
@@ -78,7 +78,7 @@ class ForceMonitor:
i_line += 1
# Should be around 21,5 (2.2 kg * 9.81 m^2/s)
- print("Total ground reaction force: (", -f_x, ", ", -f_y, ", ", -f_z, ")")
+ # print("Total ground reaction force: (", -f_x, ", ", -f_y, ", ", -f_z, ")")
for i_zero in range(i_line, len(self.lines)):
self.lines[i_zero] = pyb.addUserDebugLine([0.0, 0.0, 0.0], [0.0, 0.0, 0.0], lineColorRGB=[
diff --git a/scripts/Joystick.py b/scripts/Joystick.py
index 1f559a71f4b5c643b2b171ba0dccd548f7666b24..c1705269c6d3913f096bd8dc1f94b34d4ffc94f3 100644
--- a/scripts/Joystick.py
+++ b/scripts/Joystick.py
@@ -40,8 +40,8 @@ class Joystick:
self.vX = 0.
self.vY = 0.
self.vYaw = 0.
- self.VxScale = 0.5
- self.VyScale = 0.8
+ self.VxScale = 0.6
+ self.VyScale = 1.0
self.vYawScale = 1.2
self.Vx_ref = 0.3
@@ -89,11 +89,14 @@ class Joystick:
# Create the gamepad client
if k_loop == 0:
self.gp = gC.GamepadClient()
+ self.gp.leftJoystickX.value = 0.00390625
+ self.gp.leftJoystickY.value = 0.00390625
+ self.gp.rightJoystickX.value = 0.00390625
# Get the velocity command based on the position of joysticks
- self.vX = self.gp.leftJoystickX.value * self.VxScale
- self.vY = self.gp.leftJoystickY.value * self.VyScale
- self.vYaw = self.gp.rightJoystickX.value * self.vYawScale
+ self.vX = (self.gp.leftJoystickX.value / 0.00778 * 2. - 1.) * self.VxScale
+ self.vY = (self.gp.leftJoystickY.value / 0.00778 * 2. - 1.) * self.VyScale
+ self.vYaw = (self.gp.rightJoystickX.value / 0.00778 * 2. - 1.) * self.vYawScale
if self.gp.L1Button.value: # If L1 is pressed the orientation of the base is controlled
self.v_gp = np.array(
@@ -265,7 +268,7 @@ class Joystick:
[0.0, 0.0, 0.0, 0.0],
[0.0, 0.0, 0.0, 0.0]])
elif velID == 10: # FORWAAAAAAAAAARD
- self.t_switch = np.array([0, 2, 4, 6, 8, 10, 15])
+ self.t_switch = np.array([0, 2, 4, 6, 8, 10, 15]) * 2
self.v_switch = np.array([[0.0, 0.4, 0.8, 1.0, 1.0, 1.0, 1.0],
[0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
[0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
diff --git a/scripts/LoggerControl.py b/scripts/LoggerControl.py
index 64e04f765964309b035de23b8075e582ddc90fc2..d7413038aa04efd75eaf0d81a9f5e4bf9ea2c81c 100644
--- a/scripts/LoggerControl.py
+++ b/scripts/LoggerControl.py
@@ -365,6 +365,8 @@ class LoggerControl():
else:
plt.legend(["State", "Ground truth", "Ref state"], prop={'size': 8})
plt.ylabel(lgd[i])
+ if i == 0:
+ plt.ylim([-0.05, 1.25])
self.custom_suptitle("Linear and angular velocities")
print("RMSE: ", np.sqrt(((self.joy_v_ref[:-1000, 0] - self.mocap_h_v[:-1000, 0])**2).mean()))
diff --git a/scripts/MPC_Wrapper.py b/scripts/MPC_Wrapper.py
index 80fc23ec828ab49cb0d169d4dacdbe9cae120109..2ef221d8455ba585864a027940c8f3c4c960f176 100644
--- a/scripts/MPC_Wrapper.py
+++ b/scripts/MPC_Wrapper.py
@@ -131,7 +131,7 @@ class MPC_Wrapper:
def solve(self, k, xref, fsteps, gait, l_fsteps_target, oRh = np.eye(3), oTh = np.zeros((3,1)),
position = np.zeros((3,4)), velocity = np.zeros((3,4)) , acceleration = np.zeros((3,4)), jerk = np.zeros((3,4)) ,
- dt_flying = np.zeros(4), nle = np.zeros((6, 1))):
+ dt_flying = np.zeros(4)):
"""Call either the asynchronous MPC or the synchronous MPC depending on the value of multiprocessing during
the creation of the wrapper
@@ -148,7 +148,7 @@ class MPC_Wrapper:
if self.multiprocessing: # Run in parallel process
self.run_MPC_asynchronous(k, xref, fsteps, l_fsteps_target, oRh, oTh, position, velocity, acceleration, jerk, dt_flying)
else: # Run in the same process than main loop
- self.run_MPC_synchronous(k, xref, fsteps, l_fsteps_target, oRh, oTh, position, velocity, acceleration, jerk, dt_flying, nle)
+ self.run_MPC_synchronous(k, xref, fsteps, l_fsteps_target, oRh, oTh, position, velocity, acceleration, jerk, dt_flying)
if not np.allclose(gait[0, :], self.gait_past): # If gait status has changed
if np.allclose(gait[0, :], self.gait_next): # If we're still doing what was planned the last time MPC was solved
@@ -189,7 +189,7 @@ class MPC_Wrapper:
self.not_first_iter = True
return self.last_available_result
- def run_MPC_synchronous(self, k, xref, fsteps, l_fsteps_target, oRh, oTh, position, velocity, acceleration, jerk, dt_flying, nle):
+ def run_MPC_synchronous(self, k, xref, fsteps, l_fsteps_target, oRh, oTh, position, velocity, acceleration, jerk, dt_flying):
"""Run the MPC (synchronous version) to get the desired contact forces for the feet currently in stance phase
Args:
@@ -204,7 +204,7 @@ class MPC_Wrapper:
if self.mpc_type == MPC_type.OSQP:
# OSQP MPC
- self.mpc.run(np.int(k), xref.copy(), fsteps.copy(), nle.copy())
+ self.mpc.run(np.int(k), xref.copy(), fsteps.copy())
elif self.mpc_type == MPC_type.CROCODDYL_PLANNER: # Add goal position to stop the optimisation
# Crocoddyl MPC
self.mpc.solve(k, xref.copy(), fsteps.copy(), l_fsteps_target, position, velocity, acceleration, jerk, oRh, oTh, dt_flying)
diff --git a/scripts/config_solo12.yaml b/scripts/config_solo12.yaml
index d31b0d01d74e0b4b29860844fae0316bb7103f35..8ebc482d01770abd552ee94641d6409d609fd81a 100644
--- a/scripts/config_solo12.yaml
+++ b/scripts/config_solo12.yaml
@@ -44,4 +44,4 @@ joint_calibrator:
Kp: 1.
Kd: 0.05
T: 1.
- dt: 0.001
+ dt: 0.002
diff --git a/scripts/main_solo12_control.py b/scripts/main_solo12_control.py
index 8647ed908c90d7dcbca26d407d75b0754af8612e..08bfb377aca0a4cebbfbf7361bc80aa6a79f0937 100644
--- a/scripts/main_solo12_control.py
+++ b/scripts/main_solo12_control.py
@@ -226,7 +226,7 @@ def control_loop(name_interface_clone=None, des_vel_analysis=None):
t_end_whole = time.time()
- myForceMonitor.display_contact_forces()
+ # myForceMonitor.display_contact_forces()
# Send command to the robot
for i in range(1):
@@ -322,7 +322,7 @@ def control_loop(name_interface_clone=None, des_vel_analysis=None):
print("Masterboard timeout detected.")
print("Either the masterboard has been shut down or there has been a connection issue with the cable/wifi.")
- from matplotlib import pyplot as plt
+ """from matplotlib import pyplot as plt
N = loggerControl.tstamps.shape[0]
t_range = np.array([k*loggerControl.dt for k in range(N)])
plt.figure()
@@ -335,7 +335,7 @@ def control_loop(name_interface_clone=None, des_vel_analysis=None):
plt.plot(t_range, log_JcTf_out[:-3, 0], "mediumblue")
plt.plot(t_range, loggerControl.planner_gait[:, 0, 0], "k", linewidth=3)
plt.legend(["Mddq", "NLE", "Mddq+NLE", "JcT f", "Mddq out", "Mddq out + NLE", "JcT f out", "Contact"])
- plt.show(block=True)
+ plt.show(block=True)"""
# Plot recorded data
if params.PLOTTING:
@@ -375,7 +375,7 @@ def main():
help='Name of the clone interface that will reproduce the movement of the first one \
(use ifconfig in a terminal), for instance "enp1s0"')
- # os.nice(-20) # Â Set the process to highest priority (from -20 highest to +20 lowest)
+ os.nice(-20) # Â Set the process to highest priority (from -20 highest to +20 lowest)
f, v = control_loop(parser.parse_args().clone) # , np.array([1.5, 0.0, 0.0, 0.0, 0.0, 0.0]))
print(f, v)
quit()
diff --git a/src/InvKin.cpp b/src/InvKin.cpp
index 858f443fc77221abc5f6cb89a179c24e9e4207ad..b30c6aeb92b13f76d522636539346cb7bb859839 100644
--- a/src/InvKin.cpp
+++ b/src/InvKin.cpp
@@ -79,13 +79,14 @@ void InvKin::initialize(Params& params) {
void InvKin::refreshAndCompute(Matrix14 const& contacts, Matrix43 const& pgoals, Matrix43 const& vgoals,
Matrix43 const& agoals) {
- std::cout << std::fixed;
+ /*std::cout << std::fixed;
std::cout << std::setprecision(2);
std::cout << "pgoals:" << std::endl;
std::cout << pgoals << std::endl;
std::cout << "posf_" << std::endl;
- std::cout << posf_ << std::endl;
+ std::cout << posf_ << std::endl;*/
+
/*std::cout << "vf_" << std::endl;
std::cout << vf_ << std::endl;*/
@@ -103,20 +104,20 @@ void InvKin::refreshAndCompute(Matrix14 const& contacts, Matrix43 const& pgoals,
+ params_->Kd_flyingfeet * (vfeet_ref.row(i) - vf_.row(i))
+ agoals.row(i);
- std::cout << "1: " << afeet.row(i) << std::endl;
+ //std::cout << "1: " << afeet.row(i) << std::endl;
afeet.row(i) -= af_.row(i) + (wf_.row(i)).cross(vf_.row(i));
- std::cout << "2: " << afeet.row(i) << std::endl;
+ //std::cout << "2: " << afeet.row(i) << std::endl;
// Subtract base acceleration
afeet.row(i) -= (params_->Kd_flyingfeet * (vb_ref_ - vb_) - (ab_.head(3) + wb_.cross(vb_))).transpose();
- std::cout << "3: " << afeet.row(i) << std::endl;
+ //std::cout << "3: " << afeet.row(i) << std::endl;
/*std::cout << vb_ref_.transpose() << std::endl;
std::cout << vb_.transpose() << std::endl;
std::cout << wb_.transpose() << std::endl;
std::cout << ab_.head(3).transpose() << std::endl;
std::cout << (vb_ref_ - vb_).transpose() << std::endl;
std::cout << (wb_.cross(vb_)).transpose() << std::endl;*/
- std::cout << "---" << std::endl;
+ //std::cout << "---" << std::endl;
}
@@ -295,9 +296,9 @@ void InvKin::refreshAndCompute(Matrix14 const& contacts, Matrix43 const& pgoals,
/*std::cout << "J" << std::endl << J_ << std::endl;
std::cout << "invJ" << std::endl << invJ_ << std::endl;*/
- std::cout << "acc" << std::endl << acc << std::endl;
+ /*std::cout << "acc" << std::endl << acc << std::endl;
std::cout << "dx_r" << std::endl << dx_r << std::endl;
- std::cout << "x_err" << std::endl << x_err << std::endl;
+ std::cout << "x_err" << std::endl << x_err << std::endl;*/
/*std::cout << "ddq_cmd" << std::endl << ddq_cmd_ << std::endl;
std::cout << "dq_cmd" << std::endl << dq_cmd_ << std::endl;
std::cout << "q_step" << std::endl << q_step_ << std::endl;*/
diff --git a/src/MPC.cpp b/src/MPC.cpp
index 6a21a3baf021f8ade0c4b8e7998930a95be526bd..fec0b57acf9d787583f05842d1da3b4a129f153e 100644
--- a/src/MPC.cpp
+++ b/src/MPC.cpp
@@ -377,7 +377,7 @@ int MPC::create_weight_matrices() {
return 0;
}
-int MPC::update_matrices(Eigen::MatrixXd fsteps, const Eigen::Matrix<double, 6, 1> &nle) {
+int MPC::update_matrices(Eigen::MatrixXd fsteps) {
/* M need to be updated between each iteration:
- lever_arms changes since the robot moves
- I_inv changes if the reference velocity vector is modified
@@ -387,7 +387,7 @@ int MPC::update_matrices(Eigen::MatrixXd fsteps, const Eigen::Matrix<double, 6,
/* N need to be updated between each iteration:
- X0 changes since the robot moves
- Xk* changes since X0 is not the same */
- update_NK(nle);
+ update_NK();
// L matrix is constant
// K matrix is constant
@@ -441,7 +441,7 @@ int MPC::update_ML(Eigen::MatrixXd fsteps) {
return 0;
}
-int MPC::update_NK(const Eigen::Matrix<double, 6, 1> &nle) {
+int MPC::update_NK() {
// Matrix g is already created and not changed
//std::cout << "NLE MPC: " << std::endl << nle.transpose() << std::endl;
@@ -587,7 +587,7 @@ Return the next predicted state of the base
*/
double *MPC::get_x_next() { return x_next; }
-int MPC::run(int num_iter, const Eigen::MatrixXd &xref_in, const Eigen::MatrixXd &fsteps_in, const Eigen::MatrixXd &nle) {
+int MPC::run(int num_iter, const Eigen::MatrixXd &xref_in, const Eigen::MatrixXd &fsteps_in) {
// Recontruct the gait based on the computed footsteps
construct_gait(fsteps_in);
@@ -600,7 +600,7 @@ int MPC::run(int num_iter, const Eigen::MatrixXd &xref_in, const Eigen::MatrixXd
if (num_iter == 0) {
create_matrices();
} else {
- update_matrices(fsteps_in, nle);
+ update_matrices(fsteps_in);
}
// Create an initial guess and call the solver to solve the QP problem
diff --git a/src/QPWBC.cpp b/src/QPWBC.cpp
index a97f070394f111eec291e10a6d7d974c44db0516..fd4e6555ae9fac2bb009515510e696afa1e1d35a 100644
--- a/src/QPWBC.cpp
+++ b/src/QPWBC.cpp
@@ -238,7 +238,7 @@ int QPWBC::update_matrices(const Eigen::Matrix<double, 6, 6> &M, const Eigen::Ma
my_print_csc_matrix(P, t_char);
t_char[0] = 'M';
- my_print_csc_matrix(ML, t_char);*/
+ my_print_csc_matrix(ML, t_char);
std::cout << "Q" << std::endl;
for (int j = 0; j < 18; j++) {
@@ -256,7 +256,7 @@ int QPWBC::update_matrices(const Eigen::Matrix<double, 6, 6> &M, const Eigen::Ma
for (int j = 0; j < 26; j++) {
std::cout << v_NK_up[j] << " ";
}
- std::cout << std::endl;
+ std::cout << std::endl;*/
return 0;
}
@@ -368,10 +368,10 @@ int QPWBC::retrieve_result(const Eigen::Matrix<double, 6, 1> &ddq_cmd, const Eig
// Adding reference acceleration to delta ddq
// ddq_res += ddq_cmd;
- std::cout << "f_cmd: " << std::endl << f_cmd.transpose() << std::endl;
+ /*std::cout << "f_cmd: " << std::endl << f_cmd.transpose() << std::endl;
std::cout << "f_res: " << std::endl << f_res.transpose() << std::endl;
std::cout << "ddq_cmd: " << std::endl << ddq_cmd.transpose() << std::endl;
- std::cout << "ddq_res: " << std::endl << ddq_res.transpose() << std::endl;
+ std::cout << "ddq_res: " << std::endl << ddq_res.transpose() << std::endl;*/
return 0;
}
@@ -708,8 +708,8 @@ void WbcWrapper::compute(VectorN const &q, VectorN const &dq, VectorN const &f_c
pinocchio::rnea(model_, data_, q_wbc_, dq_wbc_, ddq_cmd_);
- std::cout << "M inertia" << std::endl;
- std::cout << data_.M << std::endl;
+ /*std::cout << "M inertia" << std::endl;
+ std::cout << data_.M << std::endl;*/
/*std::cout << "Jc" << std::endl;
std::cout << Jc_ << std::endl;
std::cout << "f_cmd" << std::endl;
@@ -719,15 +719,15 @@ void WbcWrapper::compute(VectorN const &q, VectorN const &dq, VectorN const &f_c
std::cout << "k_since" << std::endl;
std::cout << k_since_contact_ << std::endl;*/
- std::cout << "Force compensation " << std::endl;
+ //std::cout << "Force compensation " << std::endl;
Vector12 f_compensation = pseudoInverse(Jc_.transpose()) * (data_.tau.head(6) - RNEA_without_joints + RNEA_NLE);
/*for (int i = 0; i < 4; i++) {
f_compensation(3*i+2, 0) = 0.0;
}*/
- std::cout << f_compensation << std::endl;
+ //std::cout << f_compensation << std::endl;
//std::cout << "agoals " << std::endl << agoals << std::endl;
- std::cout << "ddq_cmd_bis " << std::endl << ddq_cmd_.transpose() << std::endl;
+ //std::cout << "ddq_cmd_bis " << std::endl << ddq_cmd_.transpose() << std::endl;
// Solve the QP problem
box_qp_->run(data_.M, Jc_, ddq_cmd_, f_cmd + f_compensation, data_.tau.head(6),
@@ -740,7 +740,7 @@ void WbcWrapper::compute(VectorN const &q, VectorN const &dq, VectorN const &f_c
Vector6 left = data_.M.block(0, 0, 6, 6) * box_qp_->get_ddq_res() - Jc_.transpose() * box_qp_->get_f_res();
Vector6 right = - data_.tau.head(6) + Jc_.transpose() * (f_cmd + f_compensation);
- std::cout << "RNEA: " << std::endl << data_.tau.head(6).transpose() << std::endl;
+ //std::cout << "RNEA: " << std::endl << data_.tau.head(6).transpose() << std::endl;
Vector6 tmp_RNEA = data_.tau.head(6);
//std::cout << "left: " << std::endl << left.transpose() << std::endl;
//std::cout << "right: " << std::endl << right.transpose() << std::endl;
@@ -751,9 +751,9 @@ void WbcWrapper::compute(VectorN const &q, VectorN const &dq, VectorN const &f_c
pinocchio::rnea(model_, data_, q_wbc_, dq_wbc_, VectorN::Zero(model_.nv));
Vector6 tmp_NLE = data_.tau.head(6);
- std::cout << "NLE: " << std::endl << data_.tau.head(6).transpose() << std::endl;
+ /*std::cout << "NLE: " << std::endl << data_.tau.head(6).transpose() << std::endl;
std::cout << "M DDQ: " << std::endl << (tmp_RNEA - data_.tau.head(6)).transpose() << std::endl;
- std::cout << "JcT f_cmd: " << std::endl << (Jc_.transpose() * (f_cmd + f_compensation)).transpose() << std::endl;
+ std::cout << "JcT f_cmd: " << std::endl << (Jc_.transpose() * (f_cmd + f_compensation)).transpose() << std::endl;*/
// std::cout << "Gravity ?: " << std::endl << (data_.M * ddq_cmd_ - data_.tau.head(6)).transpose() << std::endl;
@@ -766,9 +766,9 @@ void WbcWrapper::compute(VectorN const &q, VectorN const &dq, VectorN const &f_c
// Compute joint torques from contact forces and desired accelerations
pinocchio::rnea(model_, data_, q_wbc_, dq_wbc_, ddq_with_delta_);
- std::cout << "NLE Delta: " << std::endl << tmp_NLE.transpose() << std::endl;
+ /*std::cout << "NLE Delta: " << std::endl << tmp_NLE.transpose() << std::endl;
std::cout << "M DDQ Delta: " << std::endl << (data_.tau.head(6) - tmp_NLE).transpose() << std::endl;
- std::cout << "JcT f_cmd Delta: " << std::endl << (Jc_.transpose() * f_with_delta_).transpose() << std::endl;
+ std::cout << "JcT f_cmd Delta: " << std::endl << (Jc_.transpose() * f_with_delta_).transpose() << std::endl;*/
/*std::cout << "rnea delta" << std::endl;
std::cout << data_.tau.tail(12) << std::endl;
diff --git a/src/config_solo12.yaml b/src/config_solo12.yaml
index 14b67a7ad028035ed4aa32fb9f3e377965285b97..a578c201efe6bb6454aea07ed44a04d3f6902d8c 100644
--- a/src/config_solo12.yaml
+++ b/src/config_solo12.yaml
@@ -12,14 +12,14 @@ robot:
N_SIMULATION: 9000 # Number of simulated wbc time steps
enable_pyb_GUI: false # Enable/disable PyBullet GUI
enable_corba_viewer: true # Enable/disable Corba Viewer
- enable_multiprocessing: false # Enable/disable running the MPC in another process in parallel of the main loop
+ enable_multiprocessing: true # Enable/disable running the MPC in another process in parallel of the main loop
perfect_estimator: false # Enable/disable perfect estimator by using data directly from PyBullet
# General control parameters
# [0.0, 0.865, -1.583, 0.0, 0.865, -1.583, 0.0, 0.865, -1.583, 0.0, 0.865, -1.583] # h_com = 0.2
- # q_init: [0.0, 0.764, -1.407, 0.0, 0.76407, -1.4, 0.0, 0.76407, -1.407, 0.0, 0.764, -1.407] #Â h_com = 0.218
- q_init: [0.0, 0.7, -1.4, 0.0, 0.7, -1.4, 0.0, -0.7, 1.4, 0.0, -0.7, 1.4] # Initial articular positions
- dt_wbc: 0.001 # Time step of the whole body control
+ q_init: [0.0, 0.764, -1.407, 0.0, 0.76407, -1.4, 0.0, 0.76407, -1.407, 0.0, 0.764, -1.407] #Â h_com = 0.218
+ # q_init: [0.0, 0.7, -1.4, 0.0, 0.7, -1.4, 0.0, -0.7, 1.4, 0.0, -0.7, 1.4] # Initial articular positions
+ dt_wbc: 0.002 # Time step of the whole body control
dt_mpc: 0.02 # Time step of the model predictive control
type_MPC: 0 # Which MPC solver you want to use: 0 for OSQP MPC, 1, 2, 3 for Crocoddyl MPCs
Kp_main: [3.0, 3.0, 3.0] # Proportional gains for the PD+
@@ -50,16 +50,16 @@ robot:
osqp_Nz_lim: 35.0 # Maximum vertical force that can be applied at contact points
# Parameters of InvKin
- Kp_flyingfeet: 50.0 # Proportional gain for feet position tasks
- Kd_flyingfeet: 14.0 # Derivative gain for feet position tasks
- Kp_base_position: [50.0, 50.0, 50.0] # Proportional gains for the base position task
- Kd_base_position: [14.0, 14.0, 14.0] # Derivative gains for the base position task
- Kp_base_orientation: [0.1, 0.1, 0.1] # Proportional gains for the base orientation task
- Kd_base_orientation: [0.6, 0.6, 0.6] # Derivative gains for the base orientation task
+ Kp_flyingfeet: 10.0 # Proportional gain for feet position tasks
+ Kd_flyingfeet: 6.3 # Derivative gain for feet position tasks
+ Kp_base_position: [10.0, 10.0, 10.0] # Proportional gains for the base position task
+ Kd_base_position: [6.3, 6.3, 6.3] # Derivative gains for the base position task
+ Kp_base_orientation: [10.0, 10.0, 10.0] # Proportional gains for the base orientation task
+ Kd_base_orientation: [6.3, 6.3, 6.3] # Derivative gains for the base orientation task
w_tasks: [1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0] #Â Tasks weights: [feet/base, vx, vy, vz, roll+wroll, pitch+wpitch, wyaw, contacts]
# Parameters of WBC QP problem
Q1: 0.1 # Weights for the "delta articular accelerations" optimization variables
- Q2: 1.0 # Weights for the "delta contact forces" optimization variables
+ Q2: 10.0 # Weights for the "delta contact forces" optimization variables
Fz_max: 35.0 # Maximum vertical contact force [N]
Fz_min: 0.0 # Minimal vertical contact force [N]
\ No newline at end of file