From fef360d6b26083ae59fe5fde0e3b9f42c3ff3da5 Mon Sep 17 00:00:00 2001
From: odri <odri@furano.laas.fr>
Date: Fri, 17 Sep 2021 17:31:05 +0200
Subject: [PATCH] Add a weight vector for the InvKin tasks
---
include/qrw/InvKin.hpp | 1 +
include/qrw/Params.hpp | 1 +
src/InvKin.cpp | 27 ++++++++++++++++++++++++++-
src/Params.cpp | 4 ++++
src/config_solo12.yaml | 9 +++++----
5 files changed, 37 insertions(+), 5 deletions(-)
diff --git a/include/qrw/InvKin.hpp b/include/qrw/InvKin.hpp
index e89a7c2d..80f9c895 100644
--- a/include/qrw/InvKin.hpp
+++ b/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
diff --git a/include/qrw/Params.hpp b/include/qrw/Params.hpp
index d3993c80..6857dcd7 100644
--- a/include/qrw/Params.hpp
+++ b/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
diff --git a/src/InvKin.cpp b/src/InvKin.cpp
index e74dce84..d0a0e9a3 100644
--- a/src/InvKin.cpp
+++ b/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_);
diff --git a/src/Params.cpp b/src/Params.cpp
index f673e36d..66a529a9 100644
--- a/src/Params.cpp
+++ b/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>();
diff --git a/src/config_solo12.yaml b/src/config_solo12.yaml
index 189c5d24..81c98113 100644
--- a/src/config_solo12.yaml
+++ b/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
--
GitLab