From cab73271280f7593a36bfea4a4c4616cdd04d204 Mon Sep 17 00:00:00 2001
From: paleziart <paleziart@laas.fr>
Date: Tue, 20 Apr 2021 15:48:54 +0200
Subject: [PATCH] Small fix after merging
---
src/StatePlanner.cpp | 4 +++-
1 file changed, 3 insertions(+), 1 deletion(-)
diff --git a/src/StatePlanner.cpp b/src/StatePlanner.cpp
index 978f7326..15c4e269 100644
--- a/src/StatePlanner.cpp
+++ b/src/StatePlanner.cpp
@@ -46,11 +46,13 @@ void StatePlanner::computeReferenceStates(VectorN const& q, Vector6 const& v, Ve
referenceStates_(2, 1 + i) = h_ref_ + z_average;
- referenceStates_(5, 1 + i) = vref(5) * dt_vector_(i) + RPY_(2);
+ referenceStates_(5, 1 + i) = vref(5) * dt_vector_(i);
referenceStates_(6, 1 + i) = vref(0) * std::cos(referenceStates_(5, 1 + i)) - vref(1) * std::sin(referenceStates_(5, 1 + i));
referenceStates_(7, 1 + i) = vref(0) * std::sin(referenceStates_(5, 1 + i)) + vref(1) * std::cos(referenceStates_(5, 1 + i));
+ referenceStates_(5, 1 + i) += RPY_(2);
+
referenceStates_(11, 1 + i) = vref(5);
}
}
--
GitLab