Add a weight vector for the InvKin tasks

include/qrw/InvKin.hpp

@@ -138,6 +138,7 @@ class InvKin {
   Vector3 Kd_base_position;
   Vector3 Kp_base_orientation;
   Vector3 Kd_base_orientation;
+  Vector7 w_tasks;
 
   Vector18 ddq_cmd_;  // Actuator command accelerations
   Vector18 dq_cmd_;   // Actuator command velocities

include/qrw/Params.hpp

@@ -99,6 +99,7 @@ class Params {
   std::vector<double> Kd_base_position;  // Derivative gains for the base position task
   std::vector<double> Kp_base_orientation;  // Proportional gains for the base orientation task
   std::vector<double> Kd_base_orientation;  // Derivative gains for the base orientation task
+  std::vector<double> w_tasks;  // Tasks weights: [feet/base, vx, vy, vz, roll+wroll, pitch+wpitch, wyaw]
 
   // Parameters of WBC QP problem
   double Q1;      // Weights for the "delta articular accelerations" optimization variables

src/InvKin.cpp

@@ -66,6 +66,7 @@ void InvKin::initialize(Params& params) {
   Kd_base_position = Vector3(params_->Kd_base_position.data());
   Kp_base_orientation = Vector3(params_->Kp_base_orientation.data());
   Kd_base_orientation = Vector3(params_->Kd_base_orientation.data());
+  w_tasks = Vector7(params_->w_tasks.data());
 
 }
 
@@ -139,6 +140,30 @@ void InvKin::refreshAndCompute(Matrix14 const& contacts, Matrix43 const& pgoals,
   // Base angular task
   dx_r.block(0, 15, 1, 3) = wb_ref_.transpose();
 
+  // Product with tasks weights for Jacobian
+  J_.block(0, 0, 12, 18) *= w_tasks(0, 0);
+  for (int i = 0; i < 6; i++) {
+    J_.row(12 + i) *= w_tasks(1 + i, 0);
+  }
+
+  // Product with tasks weights for acc references
+  acc.block(0, 0, 1, 12) *= w_tasks(0, 0);
+  for (int i = 0; i < 6; i++) {
+    acc(0, 12 + i) *= w_tasks(1 + i, 0);
+  }
+
+  // Product with tasks weights for vel references
+  dx_r.block(0, 0, 1, 12) *= w_tasks(0, 0);
+  for (int i = 0; i < 6; i++) {
+    dx_r(0, 12 + i) *= w_tasks(1 + i, 0);
+  }
+
+  // Product with tasks weights for pos references
+  x_err.block(0, 0, 1, 12) *= w_tasks(0, 0);
+  for (int i = 0; i < 6; i++) {
+    x_err(0, 12 + i) *= w_tasks(1 + i, 0);
+  }
+
   // Jacobian inversion using damped pseudo inverse
   invJ_ = pseudoInverse(J_);
 
@@ -171,7 +196,7 @@ void InvKin::refreshAndCompute(Matrix14 const& contacts, Matrix43 const& pgoals,
 void InvKin::run_InvKin(VectorN const& q, VectorN const& dq, MatrixN const& contacts, MatrixN const& pgoals,
                         MatrixN const& vgoals, MatrixN const& agoals, MatrixN const& x_cmd) {
   // std::cout << "run invkin q: " << q << std::endl;
-  
+
   // Update model and data of the robot
   pinocchio::forwardKinematics(model_, data_, q, dq, VectorN::Zero(model_.nv));
   pinocchio::computeJointJacobians(model_, data_);

src/Params.cpp

@@ -45,6 +45,7 @@ Params::Params()
       Kd_base_position(3, 0.0),   // Fill with zeros, will be filled with values later
       Kp_base_orientation(3, 0.0),   // Fill with zeros, will be filled with values later
       Kd_base_orientation(3, 0.0),   // Fill with zeros, will be filled with values later
+      w_tasks(7, 0.0),   // Fill with zeros, will be filled with values later
 
       Q1(0.0),
       Q2(0.0),
@@ -182,6 +183,9 @@ void Params::initialize(const std::string& file_path) {
   assert_yaml_parsing(robot_node, "robot", "Kd_base_orientation");
   Kd_base_orientation = robot_node["Kd_base_orientation"].as<std::vector<double> >();
   
+  assert_yaml_parsing(robot_node, "robot", "w_tasks");
+  w_tasks = robot_node["w_tasks"].as<std::vector<double> >();
+
   assert_yaml_parsing(robot_node, "robot", "Q1");
   Q1 = robot_node["Q1"].as<double>();
 

src/config_solo12.yaml

@@ -6,12 +6,12 @@ robot:
     PLOTTING: true  # Enable/disable automatic plotting at the end of the experiment
     envID: 0  # Identifier of the environment to choose in which one the simulation will happen
     use_flat_plane: true  # If True the ground is flat, otherwise it has bumps
-    predefined_vel: false  # If we are using a predefined reference velocity (True) or a joystick (False)
+    predefined_vel: true  # If we are using a predefined reference velocity (True) or a joystick (False)
     velID: 10  # Identifier of the reference velocity profile to choose which one will be sent to the robot
     N_SIMULATION: 30000  # Number of simulated wbc time steps
-    enable_pyb_GUI: true  # Enable/disable PyBullet GUI
+    enable_pyb_GUI: true # Enable/disable PyBullet GUI
     enable_corba_viewer: false  # Enable/disable Corba Viewer
-    enable_multiprocessing: true  # Enable/disable running the MPC in another process in parallel of the main loop
+    enable_multiprocessing: false  # 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
@@ -25,7 +25,7 @@ robot:
 
     # Parameters of Gait
     N_periods: 2
-    gait: [12, 1, 0, 0, 1
+    gait: [12, 1, 0, 0, 1,
            12, 0, 1, 1, 0]  # Initial gait matrix
 
     # Parameters of Estimator
@@ -51,6 +51,7 @@ robot:
     Kd_base_position: [100.0, 100.0, 100.0]  # Derivative gains for the base position task
     Kp_base_orientation: [1000.0, 1000.0, 1000.0]  # Proportional gains for the base orientation task
     Kd_base_orientation: [100.0, 100.0, 100.0]  # Derivative gains for the base orientation task
+    w_tasks: [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]
 
     # Parameters of WBC QP problem
     Q1: 0.1  # Weights for the "delta articular accelerations" optimization variables