From d8e1e562b61cafbedc1c5c3ff3b6e6927160d30f Mon Sep 17 00:00:00 2001
From: paleziart <paleziart@laas.fr>
Date: Wed, 21 Oct 2020 14:56:02 +0200
Subject: [PATCH] Fix rotation error + Return next state expected by the MPC
---
CMakeLists.txt | 5 +++++
src/MPC.cpp | 6 +++---
2 files changed, 8 insertions(+), 3 deletions(-)
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 25589315..48bdd8c6 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -1,5 +1,7 @@
cmake_minimum_required(VERSION 3.1)
+set (CMAKE_CXX_STANDARD 11)
+
# Project properties
set(PROJECT_NAMESPACE gepetto)
set(PROJECT_NAME example-adder)
@@ -77,6 +79,9 @@ endif()
# target_compile_options(${PROJECT_NAME} PUBLIC PRINTING)
# target_compile_definitions(${PROJECT_NAME} PUBLIC PRINTING)
+target_compile_options(${PROJECT_NAME} PUBLIC -DNDEBUG -O3)
+
+
# Main Executable
add_executable(${PROJECT_NAMESPACE}-${PROJECT_NAME} src/main.cpp)
target_link_libraries(${PROJECT_NAMESPACE}-${PROJECT_NAME} ${PROJECT_NAME})
diff --git a/src/MPC.cpp b/src/MPC.cpp
index b67fec7b..0ac6563f 100644
--- a/src/MPC.cpp
+++ b/src/MPC.cpp
@@ -267,7 +267,7 @@ int MPC::create_ML() {
double s = sin(xref(5, k));
Eigen::Matrix<double, 3, 3> R;
R << c, -s, 0.0, s, c, 0.0, 0.0, 0.0, 1.0;
- Eigen::Matrix<double, 3, 3> R_gI = R * gI;
+ Eigen::Matrix<double, 3, 3> R_gI = R.transpose() * gI * R;
Eigen::Matrix<double, 3, 3> I_inv = R_gI.inverse();
// Get skew-symetric matrix for each foothold
@@ -420,7 +420,7 @@ int MPC::create_weight_matrices() {
// Define weights for the x-x_ref components of the optimization vector
// Hand-tuning of parameters if you want to give more weight to specific components
- double w[12] = {0.5f, 0.5f, 2.0f, 0.11f, 0.11f, 0.11f};
+ double w[12] = {0.5f, 0.5f, 2.0f, 0.55f, 0.55f, 0.11f};
w[6] = 2.0f * sqrt(w[0]);
w[7] = 2.0f * sqrt(w[1]);
w[8] = 2.0f * sqrt(w[2]);
@@ -522,7 +522,7 @@ int MPC::update_ML(Eigen::MatrixXd fsteps) {
double s = sin(xref(5, k));
Eigen::Matrix<double, 3, 3> R;
R << c, -s, 0.0, s, c, 0.0, 0.0, 0.0, 1.0;
- Eigen::Matrix<double, 3, 3> R_gI = R * gI;
+ Eigen::Matrix<double, 3, 3> R_gI = R.transpose() * gI * R;
Eigen::Matrix<double, 3, 3> I_inv = R_gI.inverse();
// Get skew-symetric matrix for each foothold
--
GitLab