From d573ecf2b3fb7c1748d0397a1121d534e25d393d Mon Sep 17 00:00:00 2001 From: Arnaud TANGUY Date: Wed, 10 Jun 2020 16:34:17 +0900 Subject: [PATCH 01/43] [HMC] Import LinearTimeInvariantInvertedPendulum - Import implementation of LinearTimeInvariantInvertedPendulum from HMC2 - Adapt to mc_rtc - Remove custom types - clang-format code --- .../LinearTimeInvariantInvertedPendulum.h | 122 +++++++++ src/CMakeLists.txt | 2 + .../LinearTimeInvariantInvertedPendulum.cpp | 235 ++++++++++++++++++ 3 files changed, 359 insertions(+) create mode 100644 include/mc_planning/LinearTimeInvariantInvertedPendulum.h create mode 100644 src/mc_planning/LinearTimeInvariantInvertedPendulum.cpp diff --git a/include/mc_planning/LinearTimeInvariantInvertedPendulum.h b/include/mc_planning/LinearTimeInvariantInvertedPendulum.h new file mode 100644 index 0000000000..2a2157bbdf --- /dev/null +++ b/include/mc_planning/LinearTimeInvariantInvertedPendulum.h @@ -0,0 +1,122 @@ +/* + * Copyright (c) 2017, + * @author Mitsuharu Morisawa + * + * AIST + * + * All rights reserved. + * + * This program is made available under the terms of the Eclipse Public License + * v1.0 which accompanies this distribution, and is available at + * http://www.eclipse.org/legal/epl-v10.html + */ +#pragma once + +#include +#include +#include + +// XXX to get rid of +namespace hrp +{ +typedef Eigen::Vector2d Vector2; +typedef Eigen::Vector3d Vector3; +typedef Eigen::Matrix3d Matrix33; +typedef Eigen::MatrixXd dmatrix; +typedef Eigen::VectorXd dvector; +typedef Eigen::VectorXi ivector; +typedef Eigen::Matrix dvector6; +typedef Eigen::Quaternion dquaternion; +} // namespace hrp + +namespace mc_planning +{ + +class LinearTimeVariantInvertedPendulum +{ +protected: + using Matrix22 = Eigen::Matrix; + using Vector2 = Eigen::Matrix; + + double m_dt; + double m_dh; + int m_n_current; + int m_n_preview2; + + std::vector m_wk; + std::vector m_shk; + std::vector m_chk; + + boost::circular_buffer> m_A; + boost::circular_buffer> m_B; + std::vector> m_An; + std::vector> m_Bn; + + std::vector> m_X; + hrp::dvector m_p_ref; + hrp::dvector m_w2; + hrp::dvector m_w; + +private: + inline void init_m22(double waist_height); + inline void init_v2(double waist_height); + +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + hrp::dvector & p_ref(void) + { + return m_p_ref; + } + double & p_ref(int i) + { + return m_p_ref[i]; + } + hrp::dvector & w2(void) + { + return m_w2; + } + double & w2(int i) + { + return m_w2[i]; + } + hrp::dvector & w(void) + { + return m_w; + } + double & w(int i) + { + return m_w[i]; + } + + LinearTimeVariantInvertedPendulum(); + LinearTimeVariantInvertedPendulum(double dt, int n_preview = 0, int weight_resolution = 20000); + + virtual ~LinearTimeVariantInvertedPendulum(); + + void Initialize(double dt, int n_preview = 0, int weight_resolution = 20000); + + void initMatrices(double waist_height); + + void setOmega(int n_start, int n_end); + + void update(void); + + /*! @brief to get generated CoG/ZMP states + * @param[in] n_time is the time in range of[-n_preview:n_preview], current time is 0 + * @param[out] cog_pos is position of COG + * @param[out] cog_vel is velocity of COG + * @param[out] cog_acc is acceleration of COG + * @param[in] p is position of ZMP + * @param[in] pdot is velocity of ZMP + */ + void getState(int n_time, double & cog_pos, double & cog_vel, double & cog_acc, double & p, double & pdot); + void getState(int n_time, double & cog_pos, double & cog_vel, double & cog_acc, double & p); + void getState(int n_time, double & cog_pos, double & cog_vel, double & cog_acc); + void getState(int n_time, double & cog_pos, double & cog_vel); + void getState(int n_time, double & cog_pos); + + void generate(hrp::dvector & cog_pos, hrp::dvector & cog_vel, hrp::dvector & cog_acc, hrp::dvector & p_ref); +}; + +} // namespace mc_planning diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 88590c6686..b77642f8b0 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -408,11 +408,13 @@ install_mc_rtc_lib(mc_tasks) set(mc_planning_SRC mc_planning/Pendulum.cpp +mc_planning/LinearTimeInvariantInvertedPendulum.cpp ) set(mc_planning_HDR ../include/mc_planning/Pendulum.h ../include/mc_planning/api.h +../include/mc_planning/LinearTimeInvariantInvertedPendulum.h ) add_library(mc_planning SHARED ${mc_planning_SRC} ${mc_planning_HDR}) diff --git a/src/mc_planning/LinearTimeInvariantInvertedPendulum.cpp b/src/mc_planning/LinearTimeInvariantInvertedPendulum.cpp new file mode 100644 index 0000000000..5b1053025e --- /dev/null +++ b/src/mc_planning/LinearTimeInvariantInvertedPendulum.cpp @@ -0,0 +1,235 @@ +// g++ test_mat_mal.cpp -o test_mat -g -Wall -I.. -I../.. -I/usr/include/eigen3 +// -I/home/morisawa/openrtp/include/OpenHRP-3.1 -lm -lhrpUtil-3.1 + +#include +#include +#include +// #include +// #include + +namespace cst = mc_rtc::constants; + +namespace mc_planning +{ + +LinearTimeVariantInvertedPendulum::LinearTimeVariantInvertedPendulum(void) {} + +LinearTimeVariantInvertedPendulum::LinearTimeVariantInvertedPendulum(double dt, int n_preview, int weight_resolution) +{ + Initialize(dt, n_preview, weight_resolution); +} + +void LinearTimeVariantInvertedPendulum::Initialize(double dt, int n_preview, int weight_resolution) +{ + m_dt = dt; + m_n_current = n_preview; + if(n_preview == 0) + { + m_n_current = lround(1.6 / m_dt); + } + m_n_preview2 = n_preview * 2 + 1; + + if(weight_resolution == 0) + { + weight_resolution = 20000; + } + + m_wk.resize(weight_resolution); + m_shk.resize(weight_resolution); + m_chk.resize(weight_resolution); + + m_dh = (double)weight_resolution / 800.0; + for(int i = 0; i < weight_resolution; i++) + { + double w2 = 0.001 + (double)i / m_dh; + m_wk[i] = sqrt(w2); + m_shk[i] = sinh(m_wk[i] * dt); + m_chk[i] = cosh(m_wk[i] * dt); + } + + m_A.resize(m_n_preview2, Matrix22::Identity()); + m_An.resize(m_n_preview2, Matrix22::Identity()); + m_B.resize(m_n_preview2, Vector2::Zero()); + m_Bn.resize(m_n_preview2, Vector2::Zero()); + + m_X.resize(m_n_preview2, Vector2::Zero()); + m_p_ref.setZero(m_n_preview2); + m_w2.setZero(m_n_preview2); + m_w.setZero(m_n_preview2); +} + +LinearTimeVariantInvertedPendulum::~LinearTimeVariantInvertedPendulum() {} + +void LinearTimeVariantInvertedPendulum::init_m22(double waist_height) +{ + int h = lround(cst::GRAVITY * m_dh / waist_height); + + Matrix22 m; + m << m_chk[h], m_shk[h] / m_wk[h], m_wk[h] * m_shk[h], m_chk[h]; + + for(int i = 0; i < m_n_preview2; i++) m_A.push_front(m); +} + +void LinearTimeVariantInvertedPendulum::init_v2(double waist_height) +{ + int h = lround(cst::GRAVITY * m_dh / waist_height); + + Vector2 v; + v << 1 - m_chk[h], -m_wk[h] * m_shk[h]; + + for(int i = 0; i < m_n_preview2; i++) m_B.push_front(v); +} + +void LinearTimeVariantInvertedPendulum::initMatrices(double waist_height) +{ + init_m22(waist_height); + init_v2(waist_height); +} + +void LinearTimeVariantInvertedPendulum::update(void) +{ + // set newest matrices + { + int h = lround(m_w2[m_n_preview2 - 1] * m_dh); + const double & w = m_wk[h]; + const double & ch = m_chk[h]; + const double & sh = m_shk[h]; + m_w[m_n_preview2 - 1] = w; + Matrix22 Anew; + Anew << ch, sh / w, w * sh, ch; + Vector2 Bnew; + Bnew << 1 - ch, -w * sh; + m_A.push_back(Anew); + m_B.push_back(Bnew); + } + + for(int i = 0; i < m_n_current; i++) + { + int h = lround(m_w2[i] * m_dh); + m_w[i] = m_wk[h]; + } + + // update future matrices + for(int i = m_n_current; i < m_n_preview2 - 1; i++) + { + int h = lround(m_w2[i] * m_dh); + const double & w = m_wk[h]; + const double & ch = m_chk[h]; + const double & sh = m_shk[h]; + m_w[i] = w; + m_A[i] << ch, sh / w, w * sh, ch; + m_B[i] << 1 - ch, -w * sh; + } + + m_An[m_n_preview2 - 1] = m_A[m_n_preview2 - 1]; + m_Bn[m_n_preview2 - 1] = m_B[m_n_preview2 - 1]; + + Vector2 Bsum(m_Bn[m_n_preview2 - 1] * m_p_ref(m_n_preview2 - 1)); + for(int i = m_n_preview2 - 2; i >= 0; i--) + { + m_An[i].noalias() = m_An[i + 1] * m_A[i]; + m_Bn[i].noalias() = m_An[i + 1] * m_B[i]; + Bsum.noalias() += m_Bn[i] * m_p_ref(i); + } + + Vector2 Xg(m_p_ref(0), m_p_ref(m_n_preview2 - 1)); + Vector2 Vg; + Vg << -(m_An[0](0, 0) * Xg(0) - Xg(1) + Bsum(0)) / m_An[0](0, 1), + -m_An[0](1, 1) * (m_An[0](0, 0) * Xg(0) - Xg(1) + Bsum(0)) / m_An[0](0, 1) + m_An[0](1, 0) * Xg(0) + Bsum(1); + + m_X[0] << Xg(0), Vg(0); // set initial states + for(int i = 0; i < m_n_preview2 - 1; i++) m_X[i + 1] = (m_A[i] * m_X[i] + m_B[i] * m_p_ref(i)).eval(); +} + +void LinearTimeVariantInvertedPendulum::generate(hrp::dvector & cog_pos, + hrp::dvector & cog_vel, + hrp::dvector & cog_acc, + hrp::dvector & p_ref) +{ + // update future matrices + for(int i = 0; i < m_n_preview2; i++) + { + int h = lround(m_w2[i] * m_dh); + const double & w = m_wk[h]; + const double & ch = m_chk[h]; + const double & sh = m_shk[h]; + m_w[i] = w; + m_A[i] << ch, sh / w, w * sh, ch; + m_B[i] << 1 - ch, -w * sh; + } + + m_An[m_n_preview2 - 1] = m_A[m_n_preview2 - 1]; + m_Bn[m_n_preview2 - 1] = m_B[m_n_preview2 - 1]; + + Vector2 Bsum(m_Bn[m_n_preview2 - 1] * m_p_ref(m_n_preview2 - 1)); + for(int i = m_n_preview2 - 2; i >= 0; i--) + { + m_An[i].noalias() = m_An[i + 1] * m_A[i]; + m_Bn[i].noalias() = m_An[i + 1] * m_B[i]; + Bsum.noalias() += m_Bn[i] * m_p_ref(i); + } + + Vector2 Xg(m_p_ref(0), m_p_ref(m_n_preview2 - 1)); + Vector2 Vg; + Vg << -(m_An[0](0, 0) * Xg(0) - Xg(1) + Bsum(0)) / m_An[0](0, 1), + -m_An[0](1, 1) * (m_An[0](0, 0) * Xg(0) - Xg(1) + Bsum(0)) / m_An[0](0, 1) + m_An[0](1, 0) * Xg(0) + Bsum(1); + + Vector2 X(Xg(0), Vg(0)); + for(int i = 0; i < m_n_preview2 - 1; i++) + { + cog_pos(i) = X(0); + cog_vel(i) = X(1); + cog_acc(i) = m_w2[i] * (cog_pos(i) - m_p_ref(i)); + p_ref(i) = m_p_ref(i); + + X = (m_A[i] * X + m_B[i] * m_p_ref(i)).eval(); + } + cog_pos(m_n_preview2 - 1) = X(0); + cog_vel(m_n_preview2 - 1) = X(1); + cog_acc(m_n_preview2 - 1) = m_w2[m_n_preview2 - 1] * (cog_pos(m_n_preview2 - 1) - m_p_ref(m_n_preview2 - 1)); + p_ref(m_n_preview2 - 1) = m_p_ref(m_n_preview2 - 1); +} + +void LinearTimeVariantInvertedPendulum::getState(int n_time, + double & cog_pos, + double & cog_vel, + double & cog_acc, + double & p, + double & pdot) +{ + p = m_p_ref(m_n_current + n_time); + pdot = (p - m_p_ref[m_n_current + n_time - 1]) / m_dt; + cog_pos = m_X[m_n_current + n_time](0); + cog_vel = m_X[m_n_current + n_time](1); + cog_acc = m_w2[m_n_current + n_time] * (cog_pos - p); +} + +void LinearTimeVariantInvertedPendulum::getState(int n_time, + double & cog_pos, + double & cog_vel, + double & cog_acc, + double & p) +{ + double pdot; + getState(n_time, cog_pos, cog_vel, cog_acc, p, pdot); +} + +void LinearTimeVariantInvertedPendulum::getState(int n_time, double & cog_pos, double & cog_vel, double & cog_acc) +{ + double p, pdot; + getState(n_time, cog_pos, cog_vel, cog_acc, p, pdot); +} + +void LinearTimeVariantInvertedPendulum::getState(int n_time, double & cog_pos, double & cog_vel) +{ + double cog_acc, p, pdot; + getState(n_time, cog_pos, cog_vel, cog_acc, p, pdot); +} + +void LinearTimeVariantInvertedPendulum::getState(int n_time, double & cog_pos) +{ + double cog_vel, cog_acc, p, pdot; + getState(n_time, cog_pos, cog_vel, cog_acc, p, pdot); +} + +} // namespace mc_planning From 496c915b549055a5eeefbcac9213a5c114dfd4d9 Mon Sep 17 00:00:00 2001 From: Arnaud TANGUY Date: Wed, 10 Jun 2020 17:48:50 +0900 Subject: [PATCH 02/43] [HMC/LTVS] Cleanup and start documenting --- .../LinearTimeInvariantInvertedPendulum.h | 122 --------- .../LinearTimeVariantInvertedPendulum.h | 250 ++++++++++++++++++ src/CMakeLists.txt | 4 +- ... => LinearTimeVariantInvertedPendulum.cpp} | 133 +++++----- 4 files changed, 314 insertions(+), 195 deletions(-) delete mode 100644 include/mc_planning/LinearTimeInvariantInvertedPendulum.h create mode 100644 include/mc_planning/LinearTimeVariantInvertedPendulum.h rename src/mc_planning/{LinearTimeInvariantInvertedPendulum.cpp => LinearTimeVariantInvertedPendulum.cpp} (56%) diff --git a/include/mc_planning/LinearTimeInvariantInvertedPendulum.h b/include/mc_planning/LinearTimeInvariantInvertedPendulum.h deleted file mode 100644 index 2a2157bbdf..0000000000 --- a/include/mc_planning/LinearTimeInvariantInvertedPendulum.h +++ /dev/null @@ -1,122 +0,0 @@ -/* - * Copyright (c) 2017, - * @author Mitsuharu Morisawa - * - * AIST - * - * All rights reserved. - * - * This program is made available under the terms of the Eclipse Public License - * v1.0 which accompanies this distribution, and is available at - * http://www.eclipse.org/legal/epl-v10.html - */ -#pragma once - -#include -#include -#include - -// XXX to get rid of -namespace hrp -{ -typedef Eigen::Vector2d Vector2; -typedef Eigen::Vector3d Vector3; -typedef Eigen::Matrix3d Matrix33; -typedef Eigen::MatrixXd dmatrix; -typedef Eigen::VectorXd dvector; -typedef Eigen::VectorXi ivector; -typedef Eigen::Matrix dvector6; -typedef Eigen::Quaternion dquaternion; -} // namespace hrp - -namespace mc_planning -{ - -class LinearTimeVariantInvertedPendulum -{ -protected: - using Matrix22 = Eigen::Matrix; - using Vector2 = Eigen::Matrix; - - double m_dt; - double m_dh; - int m_n_current; - int m_n_preview2; - - std::vector m_wk; - std::vector m_shk; - std::vector m_chk; - - boost::circular_buffer> m_A; - boost::circular_buffer> m_B; - std::vector> m_An; - std::vector> m_Bn; - - std::vector> m_X; - hrp::dvector m_p_ref; - hrp::dvector m_w2; - hrp::dvector m_w; - -private: - inline void init_m22(double waist_height); - inline void init_v2(double waist_height); - -public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - - hrp::dvector & p_ref(void) - { - return m_p_ref; - } - double & p_ref(int i) - { - return m_p_ref[i]; - } - hrp::dvector & w2(void) - { - return m_w2; - } - double & w2(int i) - { - return m_w2[i]; - } - hrp::dvector & w(void) - { - return m_w; - } - double & w(int i) - { - return m_w[i]; - } - - LinearTimeVariantInvertedPendulum(); - LinearTimeVariantInvertedPendulum(double dt, int n_preview = 0, int weight_resolution = 20000); - - virtual ~LinearTimeVariantInvertedPendulum(); - - void Initialize(double dt, int n_preview = 0, int weight_resolution = 20000); - - void initMatrices(double waist_height); - - void setOmega(int n_start, int n_end); - - void update(void); - - /*! @brief to get generated CoG/ZMP states - * @param[in] n_time is the time in range of[-n_preview:n_preview], current time is 0 - * @param[out] cog_pos is position of COG - * @param[out] cog_vel is velocity of COG - * @param[out] cog_acc is acceleration of COG - * @param[in] p is position of ZMP - * @param[in] pdot is velocity of ZMP - */ - void getState(int n_time, double & cog_pos, double & cog_vel, double & cog_acc, double & p, double & pdot); - void getState(int n_time, double & cog_pos, double & cog_vel, double & cog_acc, double & p); - void getState(int n_time, double & cog_pos, double & cog_vel, double & cog_acc); - void getState(int n_time, double & cog_pos, double & cog_vel); - void getState(int n_time, double & cog_pos); - - void generate(hrp::dvector & cog_pos, hrp::dvector & cog_vel, hrp::dvector & cog_acc, hrp::dvector & p_ref); -}; - -} // namespace mc_planning diff --git a/include/mc_planning/LinearTimeVariantInvertedPendulum.h b/include/mc_planning/LinearTimeVariantInvertedPendulum.h new file mode 100644 index 0000000000..8e6705abab --- /dev/null +++ b/include/mc_planning/LinearTimeVariantInvertedPendulum.h @@ -0,0 +1,250 @@ +/* + * Copyright (c) 2017, + * @author Mitsuharu Morisawa + * + * AIST + * + * All rights reserved. + * + * This program is made available under the terms of the Eclipse Public License + * v1.0 which accompanies this distribution, and is available at + * http://www.eclipse.org/legal/epl-v10.html + */ +#pragma once + +#include +#include +#include +#include + +namespace mc_planning +{ + +/** + * @brief Reference CoM position and velocity trajectory generation. + * + * Implementation of the Linear Time-Varying System (LTVS) introduced in Sec III.A of: + * + * **Online 3D CoM Trajectory Generation for Multi-Contact Locomotion Synchronizing Contact**, *Mitsuharu Morisawa et + * al., IROS 2018* + * + * Computes the horizontal long-term trajectory of the CoM along either sagittal or frontal plane. + * + * The discretized system of the centroidal dynamics in the sagital plane is obtained as + * + * \f[ + * x_{k+1} = A_k x_k + B_k u_k + * \f] + * + * where + * + * - \f[A_k = \begin{bmatrix} + * cosh(\omega_k\delta T) & \frac{sinh(\omega_k\delta T)}{\omega_k} \\ + * \omega_k sinh(\omega_k\delta T) & cosh(\omega_k \delta T) + * \end{bmatrix} + * \f] + * - CoM state position/velocity along the sagital plane + * \f[ x_k = [x_{G,k}, \dot{x}_{G,k}] \f] + * - \f[ B_k = \begin{bmatrix} + * 1- cosh(\omega_k\delta T) \\ + * - \omega_k sinh(\omega_k\delta T) + * \end{bmatrix} + * \f] + * - Desired Centroidal Momentum Pivot (CMP) [equivalent to the ZMP]. \f$ L \f$ is the number of contact links. + * \f[ u_k = \sum^L_{i=1} \alpha_{z_i,k}p_{x_i,k} - \frac{\sigma_{y,k}}{m(g+\ddot{z}_{G,k})} \f] + * - \f[ + * \omega_k = + * \sqrt{\frac{g+\ddot{z}_{G,k}}{z_{G,k}-\sum_{i=1}^{L}\alpha_{x-i,k}p_{z_i,l}}} + * \f] + * + * This system is used to compute the reference position and velocity of the CoM + * for a preview window: + * - of size \f$ N_p = 2*N+1 \f$ centered around the current time \f$ [-N,\mbox{current},N] \f$ + * - this is represented internally by arrays indexed from \f$ [0...M] \f$ with \f$ M = 2*N \f$ with the current time + * being at index \f$ N \f$ + * + * **Expects the following to be defined externally**: + * - \f$ [\omega^2_0...\omega_M^2] \f$ defined by w2() + * - Desired CMP trajectory: \f$ [u_0...u_M] \f$ defined by p_ref() + * + * The CoM state \f$ x_F \f$ after the F-th future step is + * + * \f[ + * x_F = \Phi(F,0)x_0 + \sum_{i=0}^{F-1}\Phi(F,i+1)B_i u_i + * \f] + * + * where + * + * \f[ + * \Phi(k,j) = + * \left\{ + * \begin{array}{ll} + * A_{k-1}A_{k-2}...A_j &\mbox{if } k>j \\ + * I_2 & \mbox{otherwise }(k=j=F) + * \end{array} + * \right. + * \f] + * + * Those are computed by update() as m_An, m_Bn, and the corresponding trajectory as m_X + */ +struct MC_PLANNING_DLLAPI LinearTimeVariantInvertedPendulum +{ + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + LinearTimeVariantInvertedPendulum(); + /** + * @brief Initialization + * + * @param dt Timestep + * @param n_preview Number of future preview elements. The full size of the + * preview windows from past to future will be (2*n_preview+1) + * @param weight_resolution Number of data points to pre-compute for + * \f$ \omega, cosh(\omega), sinh(\omega) \f$ for height in a given range. + * This is used to optimize the otherwise costly computation of these + * constants. + */ + LinearTimeVariantInvertedPendulum(double dt, unsigned n_preview = 0, unsigned weight_resolution = 20000); + virtual ~LinearTimeVariantInvertedPendulum(); + + void Initialize(double dt, unsigned n_preview = 0, unsigned weight_resolution = 20000); + + /** XXX What does it initialize exactly? + * @param waist_height + */ + void initMatrices(double waist_height); + + /** + * @brief Computes the reference CoM state (position and velocity) + * + * Fills m_A, m_B, m_An, m_Bn, m_X + * Requires p_ref() and w2() to be filled first. + */ + void update(); + + /** + * @brief Generated CoM/ZMP state + */ + struct State + { + double cog_pos; ///< CoM position + double cog_vel; ///< CoM velocity + double cog_acc; ///< CoM acceleration + double p; ///< ZMP position + double pdot; ///< ZMP velocity + }; + + /*! @brief Get generated CoG/ZMP states + * @param n_time is the time in range of[-n_preview:n_preview], current time is 0 + */ + State getState(int n_time) const; + + /** XXX Unused by MultiContact COG generation + */ + void generate(Eigen::VectorXd & cog_pos, + Eigen::VectorXd & cog_vel, + Eigen::VectorXd & cog_acc, + Eigen::VectorXd & p_ref); + + /** Reference CMP along the trajectory \f$ [u_0..u_M] \f$ + * + * @return Reference to the CMP array + */ + Eigen::VectorXd & p_ref() + { + return m_p_ref; + } + + /** + * @brief Value of \f$ u_k \f$ + * + * @param k Index of the value in range \f$ [0, M] \f$ + */ + double p_ref(unsigned k) + { + return m_p_ref[k]; + } + + /** Squared values of \f$ \omega_k \f$ along the preview time: \f$ [\omega_0^2 ... \omega_M^2] \f$ + */ + Eigen::VectorXd & w2() + { + return m_w2; + } + + /** + * @brief Value of \f$ \omega_k^2 \f$ + * + * @param k Index of the value in range \f$ [0, M] \f$ + */ + double w2(unsigned k) const + { + return m_w2[k]; + } + const Eigen::VectorXd & w() const + { + return m_w; + } + /** + * @brief Value of \f$ \omega_k \f$ + * + * @param k Index of the value in range \f$ [0, M] \f$ + */ + double w(unsigned k) const + { + return m_w[k]; + } + +private: + /** Fill m_A + * + * XXX with what? unclear... + * + * @param waist_height + */ + inline void init_m22(double waist_height); + /** Fill m_B + * + * XXX with what? unclear... + * + * @param waist_height + */ + inline void init_v2(double waist_height); + +protected: + using Matrix22 = Eigen::Matrix; + using Vector2 = Eigen::Matrix; + + double m_dt; ///< Timstep + double m_dh; + unsigned m_n_current; ///< Index of current time (center of the preview window) + unsigned m_n_preview2; ///< Preview window size \f$ (2*\mbox{m_n_current})+1 \f$ + + /** @name Computation optimizations + * These values are pre-computed to speed-up computations. + * + * FIXME this makes the code hard to read, and too specialized for HRP use-case. + * Values are pre-computed assuming the typical range of heights likely for HRP + * robots. + * There is no check to see if we get outside of this range, this could break + * down for other use-cases + */ + ///@{ + std::vector m_wk; ///< \f$ \omega_k \f$ + std::vector m_shk; ///< \f$ sinh(\omega_k * dt) \f$ + std::vector m_chk; ///< \f$ cosh(\omega_k * dt) \f$ + ///@} + + boost::circular_buffer> m_A; ///< \f$ A_k \f$ + boost::circular_buffer> m_B; ///< \f$ B_k \f$ + std::vector> m_An; ///< \f$ \Phi(k, j) \f$ + std::vector> m_Bn; ///< \f$ \sum_{i=0}^{F-1}\Phi(F,i+1)B_i \f$ + std::vector> + m_X; ///< Generated trajectory for the CoM (position, velocity) + + Eigen::VectorXd m_p_ref; ///< \f$ [u_0,...,u_M] \f$ + + Eigen::VectorXd m_w2; ///< \f$ \omega_k^2 \f$ + Eigen::VectorXd m_w; ///< \f$ \omega \f$ +}; + +} // namespace mc_planning diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index b77642f8b0..73d272898c 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -408,13 +408,13 @@ install_mc_rtc_lib(mc_tasks) set(mc_planning_SRC mc_planning/Pendulum.cpp -mc_planning/LinearTimeInvariantInvertedPendulum.cpp +mc_planning/LinearTimeVariantInvertedPendulum.cpp ) set(mc_planning_HDR ../include/mc_planning/Pendulum.h ../include/mc_planning/api.h -../include/mc_planning/LinearTimeInvariantInvertedPendulum.h +../include/mc_planning/LinearTimeVariantInvertedPendulum.h ) add_library(mc_planning SHARED ${mc_planning_SRC} ${mc_planning_HDR}) diff --git a/src/mc_planning/LinearTimeInvariantInvertedPendulum.cpp b/src/mc_planning/LinearTimeVariantInvertedPendulum.cpp similarity index 56% rename from src/mc_planning/LinearTimeInvariantInvertedPendulum.cpp rename to src/mc_planning/LinearTimeVariantInvertedPendulum.cpp index 5b1053025e..52e4aa2c08 100644 --- a/src/mc_planning/LinearTimeInvariantInvertedPendulum.cpp +++ b/src/mc_planning/LinearTimeVariantInvertedPendulum.cpp @@ -1,46 +1,52 @@ -// g++ test_mat_mal.cpp -o test_mat -g -Wall -I.. -I../.. -I/usr/include/eigen3 -// -I/home/morisawa/openrtp/include/OpenHRP-3.1 -lm -lhrpUtil-3.1 - -#include +#include #include -#include -// #include -// #include +#include namespace cst = mc_rtc::constants; namespace mc_planning { -LinearTimeVariantInvertedPendulum::LinearTimeVariantInvertedPendulum(void) {} +LinearTimeVariantInvertedPendulum::LinearTimeVariantInvertedPendulum() {} -LinearTimeVariantInvertedPendulum::LinearTimeVariantInvertedPendulum(double dt, int n_preview, int weight_resolution) +LinearTimeVariantInvertedPendulum::LinearTimeVariantInvertedPendulum(double dt, + unsigned n_preview, + unsigned weight_resolution) { Initialize(dt, n_preview, weight_resolution); } -void LinearTimeVariantInvertedPendulum::Initialize(double dt, int n_preview, int weight_resolution) +void LinearTimeVariantInvertedPendulum::Initialize(double dt, unsigned n_preview, unsigned weight_resolution) { m_dt = dt; m_n_current = n_preview; if(n_preview == 0) { - m_n_current = lround(1.6 / m_dt); + m_n_current = static_cast(lround(1.6 / m_dt)); } m_n_preview2 = n_preview * 2 + 1; if(weight_resolution == 0) { weight_resolution = 20000; + mc_rtc::log::warning("[LinearTimeVariantInvertedPendulum::Initialize] Invalid weight resolution, using {} instead", + weight_resolution); } - m_wk.resize(weight_resolution); m_shk.resize(weight_resolution); m_chk.resize(weight_resolution); - m_dh = (double)weight_resolution / 800.0; - for(int i = 0; i < weight_resolution; i++) + /// XXX why divided by 800? + // 25 + m_dh = static_cast(weight_resolution) / 800.0; + for(unsigned i = 0; i < weight_resolution; i++) { + // Pendulum is h = w2/g + // Precomputes values from height between + // h min = 0.001 / G ~= 0.0001m + // to + // h max = (0.001+20000/800)/G = 2.5m + // FIXME Max height depends on weight resolution! double w2 = 0.001 + (double)i / m_dh; m_wk[i] = sqrt(w2); m_shk[i] = sinh(m_wk[i] * dt); @@ -62,22 +68,31 @@ LinearTimeVariantInvertedPendulum::~LinearTimeVariantInvertedPendulum() {} void LinearTimeVariantInvertedPendulum::init_m22(double waist_height) { - int h = lround(cst::GRAVITY * m_dh / waist_height); + // XXX why this index? + unsigned h = static_cast(lround(cst::GRAVITY * m_dh / waist_height)); + // A_k Matrix22 m; m << m_chk[h], m_shk[h] / m_wk[h], m_wk[h] * m_shk[h], m_chk[h]; - for(int i = 0; i < m_n_preview2; i++) m_A.push_front(m); + for(unsigned i = 0; i < m_n_preview2; i++) + { + m_A.push_front(m); + } } void LinearTimeVariantInvertedPendulum::init_v2(double waist_height) { - int h = lround(cst::GRAVITY * m_dh / waist_height); + // XXX why this indeThis is something x? + unsigned h = static_cast(lround(cst::GRAVITY * m_dh / waist_height)); Vector2 v; v << 1 - m_chk[h], -m_wk[h] * m_shk[h]; - for(int i = 0; i < m_n_preview2; i++) m_B.push_front(v); + for(unsigned i = 0; i < m_n_preview2; i++) + { + m_B.push_front(v); + } } void LinearTimeVariantInvertedPendulum::initMatrices(double waist_height) @@ -86,11 +101,14 @@ void LinearTimeVariantInvertedPendulum::initMatrices(double waist_height) init_v2(waist_height); } -void LinearTimeVariantInvertedPendulum::update(void) +void LinearTimeVariantInvertedPendulum::update() { // set newest matrices { - int h = lround(m_w2[m_n_preview2 - 1] * m_dh); + // XXX + // What does this index represent? + // Why? + unsigned h = static_cast(lround(m_w2[m_n_preview2 - 1] * m_dh)); const double & w = m_wk[h]; const double & ch = m_chk[h]; const double & sh = m_shk[h]; @@ -103,16 +121,16 @@ void LinearTimeVariantInvertedPendulum::update(void) m_B.push_back(Bnew); } - for(int i = 0; i < m_n_current; i++) + for(unsigned i = 0; i < m_n_current; i++) { - int h = lround(m_w2[i] * m_dh); + unsigned h = static_cast(lround(m_w2[i] * m_dh)); m_w[i] = m_wk[h]; } // update future matrices - for(int i = m_n_current; i < m_n_preview2 - 1; i++) + for(unsigned i = m_n_current; i < m_n_preview2 - 1; i++) { - int h = lround(m_w2[i] * m_dh); + auto h = static_cast(lround(m_w2[i] * m_dh)); const double & w = m_wk[h]; const double & ch = m_chk[h]; const double & sh = m_shk[h]; @@ -138,18 +156,21 @@ void LinearTimeVariantInvertedPendulum::update(void) -m_An[0](1, 1) * (m_An[0](0, 0) * Xg(0) - Xg(1) + Bsum(0)) / m_An[0](0, 1) + m_An[0](1, 0) * Xg(0) + Bsum(1); m_X[0] << Xg(0), Vg(0); // set initial states - for(int i = 0; i < m_n_preview2 - 1; i++) m_X[i + 1] = (m_A[i] * m_X[i] + m_B[i] * m_p_ref(i)).eval(); + for(unsigned i = 0; i < m_n_preview2 - 1; i++) + { + m_X[i + 1] = (m_A[i] * m_X[i] + m_B[i] * m_p_ref(i)).eval(); + } } -void LinearTimeVariantInvertedPendulum::generate(hrp::dvector & cog_pos, - hrp::dvector & cog_vel, - hrp::dvector & cog_acc, - hrp::dvector & p_ref) +void LinearTimeVariantInvertedPendulum::generate(Eigen::VectorXd & cog_pos, + Eigen::VectorXd & cog_vel, + Eigen::VectorXd & cog_acc, + Eigen::VectorXd & p_ref) { // update future matrices - for(int i = 0; i < m_n_preview2; i++) + for(unsigned i = 0; i < m_n_preview2; i++) { - int h = lround(m_w2[i] * m_dh); + auto h = static_cast(lround(m_w2[i] * m_dh)); const double & w = m_wk[h]; const double & ch = m_chk[h]; const double & sh = m_shk[h]; @@ -175,7 +196,7 @@ void LinearTimeVariantInvertedPendulum::generate(hrp::dvector & cog_pos, -m_An[0](1, 1) * (m_An[0](0, 0) * Xg(0) - Xg(1) + Bsum(0)) / m_An[0](0, 1) + m_An[0](1, 0) * Xg(0) + Bsum(1); Vector2 X(Xg(0), Vg(0)); - for(int i = 0; i < m_n_preview2 - 1; i++) + for(unsigned i = 0; i < m_n_preview2 - 1; i++) { cog_pos(i) = X(0); cog_vel(i) = X(1); @@ -190,46 +211,16 @@ void LinearTimeVariantInvertedPendulum::generate(hrp::dvector & cog_pos, p_ref(m_n_preview2 - 1) = m_p_ref(m_n_preview2 - 1); } -void LinearTimeVariantInvertedPendulum::getState(int n_time, - double & cog_pos, - double & cog_vel, - double & cog_acc, - double & p, - double & pdot) -{ - p = m_p_ref(m_n_current + n_time); - pdot = (p - m_p_ref[m_n_current + n_time - 1]) / m_dt; - cog_pos = m_X[m_n_current + n_time](0); - cog_vel = m_X[m_n_current + n_time](1); - cog_acc = m_w2[m_n_current + n_time] * (cog_pos - p); -} - -void LinearTimeVariantInvertedPendulum::getState(int n_time, - double & cog_pos, - double & cog_vel, - double & cog_acc, - double & p) -{ - double pdot; - getState(n_time, cog_pos, cog_vel, cog_acc, p, pdot); -} - -void LinearTimeVariantInvertedPendulum::getState(int n_time, double & cog_pos, double & cog_vel, double & cog_acc) -{ - double p, pdot; - getState(n_time, cog_pos, cog_vel, cog_acc, p, pdot); -} - -void LinearTimeVariantInvertedPendulum::getState(int n_time, double & cog_pos, double & cog_vel) -{ - double cog_acc, p, pdot; - getState(n_time, cog_pos, cog_vel, cog_acc, p, pdot); -} - -void LinearTimeVariantInvertedPendulum::getState(int n_time, double & cog_pos) +LinearTimeVariantInvertedPendulum::State LinearTimeVariantInvertedPendulum::getState(int n_time) const { - double cog_vel, cog_acc, p, pdot; - getState(n_time, cog_pos, cog_vel, cog_acc, p, pdot); + const auto t = static_cast(static_cast(m_n_current) + n_time); + State s; + s.p = m_p_ref(t); + s.pdot = (s.p - m_p_ref[t - 1]) / m_dt; + s.cog_pos = m_X[t](0); + s.cog_vel = m_X[t](1); + s.cog_acc = m_w2[t] * (s.cog_pos - s.p); + return s; } } // namespace mc_planning From 612bcdbc461c5c8507a3dacda830172e4fe669cf Mon Sep 17 00:00:00 2001 From: Arnaud TANGUY Date: Mon, 15 Jun 2020 10:26:58 +0900 Subject: [PATCH 03/43] LookupTable implementation (+test/benchmark) Adds a LookupTable implementation for convenient pre-computation, storing and access of heavy functions. --- benchmarks/CMakeLists.txt | 1 + benchmarks/benchHMC.cpp | 62 ++++++++++ .../LinearTimeVariantInvertedPendulum.h | 2 + include/mc_planning/LookupTable.h | 111 ++++++++++++++++++ tests/CMakeLists.txt | 5 + tests/test_hmc.cpp | 53 +++++++++ 6 files changed, 234 insertions(+) create mode 100644 benchmarks/benchHMC.cpp create mode 100644 include/mc_planning/LookupTable.h create mode 100644 tests/test_hmc.cpp diff --git a/benchmarks/CMakeLists.txt b/benchmarks/CMakeLists.txt index dcc7d19823..6a35e07f42 100644 --- a/benchmarks/CMakeLists.txt +++ b/benchmarks/CMakeLists.txt @@ -26,3 +26,4 @@ mc_rtc_benchmark(benchCompletionCriteria mc_control) mc_rtc_benchmark(benchSimulationContactSensor mc_control) mc_rtc_benchmark(benchRobotLoading mc_rbdyn) mc_rtc_benchmark(benchAllocTasks mc_tasks) +mc_rtc_benchmark(benchHMC mc_planning) diff --git a/benchmarks/benchHMC.cpp b/benchmarks/benchHMC.cpp new file mode 100644 index 0000000000..87626c81b1 --- /dev/null +++ b/benchmarks/benchHMC.cpp @@ -0,0 +1,62 @@ +/* + * Copyright 2015-2020 CNRS-UM LIRMM, CNRS-AIST JRL + */ + +#include +#include +#include "benchmark/benchmark.h" +#include + +static void BM_cosh_lookuptable(benchmark::State & state) +{ + // Perform setup here + auto omega = [](double h) { return std::sqrt(mc_rtc::constants::GRAVITY / h); }; + double dt = 0.005; + auto table = + mc_planning::LookupTable(20000, omega(2.5), omega(0.01), [dt](double x) { return cosh(x) * dt; }); + + static std::random_device rd; + static std::mt19937_64 gen(rd()); + static std::uniform_real_distribution dis(omega(2.5), omega(0.01)); + + double res = 0; + double val = 0; + for(auto _ : state) + { + val = dis(gen); + for(int i = 0; i < 10000; ++i) + { + // Prevent compiler from optimizing away the result + benchmark::DoNotOptimize(res = table(val)); + } + } + state.SetItemsProcessed(10000 * state.iterations()); +} + +static void BM_cosh(benchmark::State & state) +{ + // Perform setup here + auto omega = [](double h) { return std::sqrt(mc_rtc::constants::GRAVITY / h); }; + double dt = 0.005; + static std::random_device rd; + static std::mt19937_64 gen(rd()); + static std::uniform_real_distribution dis(omega(2.5), omega(0.01)); + + double res = 0; + double val = 0; + for(auto _ : state) + { + val = dis(gen); + for(int i = 0; i < 10000; ++i) + { + benchmark::DoNotOptimize(res = cosh(val) * dt); + } + } + state.SetItemsProcessed(10000 * state.iterations()); +} + +// Register the function as a benchmark +BENCHMARK(BM_cosh_lookuptable); +BENCHMARK(BM_cosh); +// Run the benchmark +BENCHMARK_MAIN(); diff --git a/include/mc_planning/LinearTimeVariantInvertedPendulum.h b/include/mc_planning/LinearTimeVariantInvertedPendulum.h index 8e6705abab..9bb7028693 100644 --- a/include/mc_planning/LinearTimeVariantInvertedPendulum.h +++ b/include/mc_planning/LinearTimeVariantInvertedPendulum.h @@ -12,7 +12,9 @@ */ #pragma once +#include #include +#include #include #include #include diff --git a/include/mc_planning/LookupTable.h b/include/mc_planning/LookupTable.h new file mode 100644 index 0000000000..2447976b93 --- /dev/null +++ b/include/mc_planning/LookupTable.h @@ -0,0 +1,111 @@ +/* + * Copyright 2015-2020 CNRS-UM LIRMM, CNRS-AIST JRL + */ + +#pragma once + +#include +#include +#include + +namespace mc_planning +{ +/** + * @brief Stores precomputed values of a function for fast lookup + * @tparam T Data type for the function arguments and result value + * @tparam CheckBounds Whether to check out-of-bounds access at runtime + * + * This class provides a lookup-table implementation intended for fast evaluation of + * computationally expensive functions. This is done by pre-generating a table + * of all possible function results in a given range/resolution, and only + * accessing the stored values instead of computing the function each time. + * + * On construction, the provided funciton is evaluated for values of `x` linearely distributed between + * `min` and `max`. \f[ [f(min), ..., f(x), ..., f(max)] \in T^\mbox{resolution} \f] + * Function values \f$ f(x) \f$ may be retrieved by calling the operator()(const T & value) + * + * @warning Notes regarding accuracy. + * - Due to the discretization, precision depends heavily on the chosen + * resolution and the function evaluated. Higher resolution improves accuracy + * at the expanse of memory usage (`resolution*sizeof(T)`). Conversely, lower + * resolution decreases accuracy and memory usage. + * - Accuracy may be lower near the upper bound due to numerical errors in + * linear generation of values in the min-max range. Whenever possible, it is + * recommended to choose a conservative upper boundary. + * - Always rigourously evaluate accuracy for each specific use-case. + */ +template +struct MC_PLANNING_DLLAPI LookupTable +{ + /** + * @brief Evaluate and store given function results + * + * \f[ [f(min), ..., f(max)] \in T^\mbox{resolution} \f] + * + * @param resolution Number of points to compute + * @param min Minimum argument value + * @param max Maximum argument value + * @param f Function to evaluate + */ + template + LookupTable(size_t resolution, const T & min, const T & max, MappingFunction f) + : table_(resolution), min_(min), max_(max) + { + if(min_ > max_) + { + mc_rtc::log::error_and_throw("[LookupTable] Invalid range (min {} > max {})", min, max); + } + if(resolution == 0) + { + mc_rtc::log::error_and_throw( + "[LookupTable] Resolution cannot be equal to zero (strictly positive)"); + } + T oldrange = static_cast(resolution); + T newrange = (max - min); + T fracRange = newrange / oldrange; + for(size_t i = 0; i < resolution; ++i) + { + table_[i] = f((static_cast(i) * fracRange) + min); + } + rangeConversion_ = static_cast(table_.size()) / (max_ - min_); + maxIndex_ = table_.size() - 1; + } + + /** + * @brief Retrieves the computed value \f$ f(x) \f$ + * + * @note Accuracy depends on the chosen resolution. + * + * @throws std::runtime_error + * If CheckBounds=true and x is out of bounds (x < min or x > max). + * + * @param x Value to be retrieved. x must be between \f$ [min, max] \f$. + * + * @return Closest pre-computed value of \f$ f(x) \f$ + */ + const T & operator()(const T & x) const + { + if(CheckBounds) + { + if(x < min_ || x > max_) + { + mc_rtc::log::error_and_throw("[LookupTable] Out of bound access ({} <= {} <= {})", min_, x, + max_); + } + } + else + { + assert(x < min_ && x > max_); + } + auto h = std::min(static_cast(lround((x - min_) * rangeConversion_)), maxIndex_); + return table_[h]; + } + +protected: + std::vector table_; + T min_, max_; + T rangeConversion_; + size_t maxIndex_; +}; + +} // namespace mc_planning diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt index 0b94f417c9..139753f16e 100644 --- a/tests/CMakeLists.txt +++ b/tests/CMakeLists.txt @@ -136,3 +136,8 @@ target_link_libraries(dummyServer PUBLIC mc_control) mc_rtc_test(test_filters mc_filter SpaceVecAlg::SpaceVecAlg) mc_rtc_test(test_interpolation mc_control) + +##################################### +# -- Temporary HMC-related tests -- # +##################################### +mc_rtc_test(test_hmc mc_planning) diff --git a/tests/test_hmc.cpp b/tests/test_hmc.cpp new file mode 100644 index 0000000000..a7a5a59f8e --- /dev/null +++ b/tests/test_hmc.cpp @@ -0,0 +1,53 @@ +/* + * Copyright 2015-2019 CNRS-UM LIRMM, CNRS-AIST JRL + */ + +#include +#include +#include + +#include + +template +void testLookupTable(Fun f, double min, double max, size_t resolution, size_t nbTest, double precision = 1e-6) +{ + using namespace mc_planning; + LookupTable lookupTable(resolution, min, max, f); + + double oldrange = static_cast(nbTest); + double newrange = max - min; + for(size_t i = 0; i < nbTest; ++i) + { + double v = ((static_cast(i) * newrange) / oldrange) + min; + BOOST_REQUIRE_CLOSE(lookupTable(v), f(v), precision); + } +} + +BOOST_AUTO_TEST_CASE(TestLookupTable) +{ + + { + // Test cosh loopup table in a realistic constext. + // + // Creates a lookup table from the value of omega to + // both cosh(w * dt) and sinh(w * dt) + // + // The range is chosen to be similar to that encountered for a humanoid + // robot (from 0.001m to 3m height) + auto omega = [](double h) { return std::sqrt(mc_rtc::constants::GRAVITY / h); }; + double dt = 0.005; + testLookupTable([dt](double x) { return cosh(x) * dt; }, omega(2.5), omega(0.01), 100000, 100000); + testLookupTable([](double x) { return sinh(x); }, omega(2.5), omega(0.01), 100000, 100000); + } + + { + // Other tests for loopup table + testLookupTable([](double x) { return cosh(x); }, -2., 3., 20000, 500); + + // Test lookuptable for sinh + testLookupTable([](double x) { return sinh(x); }, 0., 3., 20000, 500); + testLookupTable([](double x) { return sinh(x); }, -1.5, 3., 20000, 500); + + testLookupTable([](double x) { return sqrt(x); }, 0., 1000., 2000, 500); + } +} From 822c43384c78050488d615164bb2b29d9e249e51 Mon Sep 17 00:00:00 2001 From: Arnaud TANGUY Date: Mon, 15 Jun 2020 12:56:08 +0900 Subject: [PATCH 04/43] [hmc] Import code related to short-term com trajectory - Import LinearControl systems used for generating short-term com trajectories - Replace openhrp eigen types with Eigen - Cleanup formatting and unused commented-out code --- include/mc_planning/LIPMControlByPoleAssign.h | 33 ++++++++++ ...LIPMControlByPoleAssignWithExternalForce.h | 29 ++++++++ include/mc_planning/LinearControl3.h | 52 +++++++++++++++ src/CMakeLists.txt | 6 ++ src/mc_planning/LIPMControlByPoleAssign.cpp | 24 +++++++ ...PMControlByPoleAssignWithExternalForce.cpp | 49 ++++++++++++++ src/mc_planning/LinearControl3.cpp | 66 +++++++++++++++++++ 7 files changed, 259 insertions(+) create mode 100644 include/mc_planning/LIPMControlByPoleAssign.h create mode 100644 include/mc_planning/LIPMControlByPoleAssignWithExternalForce.h create mode 100644 include/mc_planning/LinearControl3.h create mode 100644 src/mc_planning/LIPMControlByPoleAssign.cpp create mode 100644 src/mc_planning/LIPMControlByPoleAssignWithExternalForce.cpp create mode 100644 src/mc_planning/LinearControl3.cpp diff --git a/include/mc_planning/LIPMControlByPoleAssign.h b/include/mc_planning/LIPMControlByPoleAssign.h new file mode 100644 index 0000000000..d0c34b9b7a --- /dev/null +++ b/include/mc_planning/LIPMControlByPoleAssign.h @@ -0,0 +1,33 @@ +#pragma once +#include + +namespace mc_planning +{ +namespace linear_control_system +{ + +class LIPMControlByPoleAssign : public LinearControl3 +{ +public: + void setStateVariables(double x_, double v_, double p_) + { + x << x_, v_, p_; + } + void getStateVariables(double & x_, double & v_, double & p_) + { + x_ = x(0); + v_ = x(1); + p_ = x(2); + } + void getStateVariables(double & x_, double & v_, double & p_, double & pdot) + { + x_ = x(0); + v_ = x(1); + p_ = x(2); + pdot = u; + } + void setSystemMatrices(const double alpha, const double beta, const double gamma, const double cog_height); +}; + +} // namespace linear_control_system +} // namespace mc_planning diff --git a/include/mc_planning/LIPMControlByPoleAssignWithExternalForce.h b/include/mc_planning/LIPMControlByPoleAssignWithExternalForce.h new file mode 100644 index 0000000000..0c3fc4d212 --- /dev/null +++ b/include/mc_planning/LIPMControlByPoleAssignWithExternalForce.h @@ -0,0 +1,29 @@ +#pragma once +#include "LIPMControlByPoleAssign.h" + +namespace mc_planning +{ +namespace linear_control_system +{ + +class LIPMControlByPoleAssignWithExternalForce : public LIPMControlByPoleAssign +{ +private: + Eigen::Vector3d B2; + +public: + LIPMControlByPoleAssignWithExternalForce(void); + + void Initialize(void); + + void update(const double Fext, const double dt); + void update(const Eigen::Vector3d & x_ref, const double Fext, const double dt); + void setSystemMatrices(const double alpha, + const double beta, + const double gamma, + const double omega, + const double total_mass); +}; + +} // namespace linear_control_system +} // namespace mc_planning diff --git a/include/mc_planning/LinearControl3.h b/include/mc_planning/LinearControl3.h new file mode 100644 index 0000000000..1bb697983d --- /dev/null +++ b/include/mc_planning/LinearControl3.h @@ -0,0 +1,52 @@ +/* + * Copyright (c) 2009, + * @author Mitsuharu Morisawa + * + * AIST + * + * All rights reserved. + * + * This program is made available under the terms of the Eclipse Public License + * v1.0 which accompanies this distribution, and is available at + * http://www.eclipse.org/legal/epl-v10.html + */ +#pragma once +#include +#include + +namespace mc_planning +{ +namespace linear_control_system +{ + +// linear control for siso system +struct MC_PLANNING_DLLAPI LinearControl3 +{ + LinearControl3(); + void Initialize(); + virtual void update(); + virtual void update(const Eigen::Vector3d & x_ref_); + virtual void update(const double dt); + virtual void update(const Eigen::Vector3d & x_ref_, const double dt); + void setReference(const Eigen::Vector3d & x_ref_) + { + x_ref = x_ref_; + }; + void setReference(double x1, double x2, double x3) + { + x_ref << x1, x2, x3; + }; + +protected: + // system matrices and feedback gain + Eigen::Matrix3d A; + Eigen::Vector3d B; + Eigen::RowVector3d C; + Eigen::Vector3d K; + Eigen::Vector3d x; ///< state variable + Eigen::Vector3d x_ref; ///< reference state variable + double y; ///< output + double u; ///< input +}; +} // namespace linear_control_system +} // namespace mc_planning diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 73d272898c..ba02470571 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -409,12 +409,18 @@ install_mc_rtc_lib(mc_tasks) set(mc_planning_SRC mc_planning/Pendulum.cpp mc_planning/LinearTimeVariantInvertedPendulum.cpp +mc_planning/LinearControl3.cpp +mc_planning/LIPMControlByPoleAssign.cpp +mc_planning/LIPMControlByPoleAssignWithExternalForce.cpp ) set(mc_planning_HDR ../include/mc_planning/Pendulum.h ../include/mc_planning/api.h ../include/mc_planning/LinearTimeVariantInvertedPendulum.h +../include/mc_planning/LinearControl3.h +../include/mc_planning/LIPMControlByPoleAssign.h +../include/mc_planning/LIPMControlByPoleAssignWithExternalForce.h ) add_library(mc_planning SHARED ${mc_planning_SRC} ${mc_planning_HDR}) diff --git a/src/mc_planning/LIPMControlByPoleAssign.cpp b/src/mc_planning/LIPMControlByPoleAssign.cpp new file mode 100644 index 0000000000..38a488de4c --- /dev/null +++ b/src/mc_planning/LIPMControlByPoleAssign.cpp @@ -0,0 +1,24 @@ +#include + +namespace mc_planning +{ +namespace linear_control_system +{ + +void LIPMControlByPoleAssign::setSystemMatrices(const double alpha, + const double beta, + const double gamma, + const double omega2) +{ + A << 0.0, 1.0, 0.0, omega2, 0.0, -omega2, 0.0, 0.0, 0.0; + + B << 0.0, 0.0, 1.0; + + C << 0.0, 0.0, 1.0; + + K << (alpha + beta + gamma) + alpha * beta * gamma / omega2, + (alpha * beta + beta * gamma + gamma * alpha) / omega2 + 1.0, -(alpha + beta + gamma); +} + +} // namespace linear_control_system +} // namespace mc_planning diff --git a/src/mc_planning/LIPMControlByPoleAssignWithExternalForce.cpp b/src/mc_planning/LIPMControlByPoleAssignWithExternalForce.cpp new file mode 100644 index 0000000000..7a5d35c004 --- /dev/null +++ b/src/mc_planning/LIPMControlByPoleAssignWithExternalForce.cpp @@ -0,0 +1,49 @@ +#include + +namespace mc_planning +{ +namespace linear_control_system +{ + +LIPMControlByPoleAssignWithExternalForce::LIPMControlByPoleAssignWithExternalForce(void) : B2(Eigen::Vector3d::Zero()) +{ + LinearControl3::Initialize(); +} + +void LIPMControlByPoleAssignWithExternalForce::Initialize(void) +{ + B2.setZero(); + LinearControl3::Initialize(); +} + +void LIPMControlByPoleAssignWithExternalForce::setSystemMatrices(const double alpha, + const double beta, + const double gamma, + const double omega2, + const double total_mass) +{ + LIPMControlByPoleAssign::setSystemMatrices(alpha, beta, gamma, omega2); + + B2 << 0.0, 1.0 / total_mass, 0.0; +} + +void LIPMControlByPoleAssignWithExternalForce::update(const double Fext, const double dt) +{ + y = (C * x)(0); + + double u = K.dot(x); + Eigen::Vector3d dx(A * x + B * u + B2 * Fext); + x += dx * dt; +} + +void LIPMControlByPoleAssignWithExternalForce::update(const Eigen::Vector3d & x_ref, const double Fext, const double dt) +{ + y = (C * x)(0); + + double u = K.dot(x - x_ref); + Eigen::Vector3d dx(A * x + B * u + B2 * Fext); + x += dx * dt; +} + +} // namespace linear_control_system +} // namespace mc_planning diff --git a/src/mc_planning/LinearControl3.cpp b/src/mc_planning/LinearControl3.cpp new file mode 100644 index 0000000000..a6b73859d7 --- /dev/null +++ b/src/mc_planning/LinearControl3.cpp @@ -0,0 +1,66 @@ +/* + * Copyright (c) 2009, + * @author Mitsuharu Morisawa + * + * AIST + * + * All rights reserved. + * + * This program is made available under the terms of the Eclipse Public License + * v1.0 which accompanies this distribution, and is available at + * http://www.eclipse.org/legal/epl-v10.html + */ +#include + +namespace mc_planning +{ + +namespace linear_control_system +{ + +LinearControl3::LinearControl3() +{ + LinearControl3::Initialize(); +} + +void LinearControl3::Initialize() +{ + A.setZero(); + B.setZero(); + C.setZero(); + K.setZero(); + x.setZero(); + x_ref.setZero(); + y = 0.0; + u = 0.0; +} + +void LinearControl3::update() +{ + u = -K.dot(x_ref - x); + x = A * x + B * u; + y = C.dot(x); +} + +void LinearControl3::update(const Eigen::Vector3d & x_ref_) +{ + x_ref = x_ref_; + update(); +} + +void LinearControl3::update(const double dt) +{ + u = -K.dot(x_ref - x); + Eigen::Vector3d dx(A * x + B * u); + x += dx * dt; + y = C.dot(x); +} + +void LinearControl3::update(const Eigen::Vector3d & x_ref_, const double dt) +{ + x_ref = x_ref_; + update(dt); +} + +} // namespace linear_control_system +} // namespace mc_planning From ec4fc5540d1187cc358f1e44a8f0c5334cc3347f Mon Sep 17 00:00:00 2001 From: Arnaud TANGUY Date: Mon, 15 Jun 2020 15:05:00 +0900 Subject: [PATCH 05/43] [HMC] Minimal running example of CoM trajectory generation - Import all necessary dependencies - Run HMC's sample generator.cpp as-is --- include/mc_planning/ClampedCubicSpline.h | 74 ++++ include/mc_planning/CubicSplineBase.h | 328 ++++++++++++++++++ include/mc_planning/InterpolatorBase.h | 298 ++++++++++++++++ include/mc_planning/LIPMControlByPoleAssign.h | 2 +- ...LIPMControlByPoleAssignWithExternalForce.h | 2 +- .../LinearTimeVariantInvertedPendulum.h | 17 +- include/mc_planning/MathFunction.h | 205 +++++++++++ include/mc_planning/State.h | 87 +++++ src/CMakeLists.txt | 5 + .../LinearTimeVariantInvertedPendulum.cpp | 19 + src/mc_planning/MathFunction.cpp | 72 ++++ src/mc_planning/State.cpp | 138 ++++++++ tests/CMakeLists.txt | 2 + tests/hmc/CMakeLists.txt | 2 + tests/hmc/generator.cpp | 193 +++++++++++ tests/hmc/generator.h | 135 +++++++ tests/hmc/testCoMGenerator.cpp | 81 +++++ 17 files changed, 1657 insertions(+), 3 deletions(-) create mode 100644 include/mc_planning/ClampedCubicSpline.h create mode 100644 include/mc_planning/CubicSplineBase.h create mode 100644 include/mc_planning/InterpolatorBase.h create mode 100644 include/mc_planning/MathFunction.h create mode 100644 include/mc_planning/State.h create mode 100644 src/mc_planning/MathFunction.cpp create mode 100644 src/mc_planning/State.cpp create mode 100644 tests/hmc/CMakeLists.txt create mode 100644 tests/hmc/generator.cpp create mode 100644 tests/hmc/generator.h create mode 100644 tests/hmc/testCoMGenerator.cpp diff --git a/include/mc_planning/ClampedCubicSpline.h b/include/mc_planning/ClampedCubicSpline.h new file mode 100644 index 0000000000..e1527faf3c --- /dev/null +++ b/include/mc_planning/ClampedCubicSpline.h @@ -0,0 +1,74 @@ +/* + * Copyright (c) 2009, + * @author Mitsuharu Morisawa + * + * AIST + * + * All rights reserved. + * + * This program is made available under the terms of the Eclipse Public License + * v1.0 which accompanies this distribution, and is available at + * http://www.eclipse.org/legal/epl-v10.html + */ +#pragma once +#include + +namespace mc_planning +{ +namespace motion_interpolator +{ + +template +class ClampedCubicSpline : public CubicSplineBase +{ +protected: + void setSplineCoeff(unsigned int n_base, unsigned int n_segment, unsigned int n_offset); + +public: + ClampedCubicSpline(const double & scale = 1.0, const double & eps = 1.0e-3); +}; + +template +ClampedCubicSpline::ClampedCubicSpline(const double & scale, const double & eps) +: CubicSplineBase(scale, eps, CLAMPED_CUBIC_SPLINE) +{ +} + +template +void ClampedCubicSpline::setSplineCoeff(unsigned int n_base, unsigned int n_size, unsigned int n_offset) +{ + using namespace motion_interpolator; + + typename std::deque>::const_iterator iter = InterpolatorBase::data.begin() + n_offset; + + const double & data_dot_ini = iter->vel; + const double & data_dot_end = (iter + n_size - 1)->vel; + + if(n_size <= 2) + { + CubicSplineBase::coeff[n_base].resize(n_size * 2, 0); + return; + } + + CubicSplineBase::coeff[n_base].resize(n_size * 2); + CubicSplineBase::coeff[n_base][0] = -0.5; + CubicSplineBase::coeff[n_base][n_size] = + (3.0 / ((double)((iter + 1)->t - iter->t) * InterpolatorBase::scale)) + * (((iter + 1)->pos - iter->pos) / ((double)((iter + 1)->t - iter->t) * InterpolatorBase::scale) + - data_dot_ini); + for(unsigned int i = 1; i < n_size; i++) + { + CubicSplineBase::coeff[n_base][i] = 0.0; + CubicSplineBase::coeff[n_base][i + n_size] = 0.0; + } + const double qn = 0.5; + const double un = + (3.0 / ((double)((iter + n_size - 1)->t - (iter + n_size - 2)->t) * InterpolatorBase::scale)) + * (data_dot_end + - ((iter + n_size - 1)->pos - (iter + n_size - 2)->pos) + / ((double)((iter + n_size - 1)->t - (iter + n_size - 2)->t) * InterpolatorBase::scale)); + + CubicSplineBase::makeSplineTable(n_base, n_size, n_offset, qn, un); +} +} // end of namespace motion_interpolator +} // namespace mc_planning diff --git a/include/mc_planning/CubicSplineBase.h b/include/mc_planning/CubicSplineBase.h new file mode 100644 index 0000000000..bd91daf9e4 --- /dev/null +++ b/include/mc_planning/CubicSplineBase.h @@ -0,0 +1,328 @@ +/* + * Copyright (c) 2009, + * @author Mitsuharu Morisawa + * + * AIST + * + * All rights reserved. + * + * This program is made available under the terms of the Eclipse Public License + * v1.0 which accompanies this distribution, and is available at + * http://www.eclipse.org/legal/epl-v10.html + */ +#pragma once +#include "InterpolatorBase.h" +#include +#include + +namespace mc_planning +{ +namespace motion_interpolator +{ + +template +class CubicSplineBase : public InterpolatorBase +{ +protected: + std::vector> coeff; + + void getPointToPoint(T t_, double & p, double & v, double & a, unsigned int n_segment); + + virtual void makeSplineTable(unsigned int n_base, + unsigned int n_segment, + unsigned int n_offset, + const double & qn, + const double & un); + + virtual void setSplineCoeff(unsigned int n_base, unsigned int n_segment, unsigned int n_offset) = 0; + +public: + CubicSplineBase(const double & scale = 1.0, const double & eps = 1.0e-3, interpolator_type ip_type_ = NOT_SELECTED); + + void get(T t_, double & p, double & v, double & a); + + void update(void); + + void update(T t_); + + void update(T t_from, T t_to); +}; + +template +CubicSplineBase::CubicSplineBase(const double & scale, const double & eps, interpolator_type ip_type_) +: InterpolatorBase(scale, eps, ip_type_) +{ +} + +template +void CubicSplineBase::getPointToPoint(T t_, double & p, double & v, double & a, unsigned int n_segment) +{ + using namespace motion_interpolator; + + typename std::deque>::const_iterator iter = InterpolatorBase::data.begin() + n_segment; + + const T ti = iter->t; + const T tf = (iter + 1)->t; + const double t1 = (double)(tf - ti) * InterpolatorBase::scale; + + if(t1 < InterpolatorBase::eps) + { + p = iter->pos; + v = iter->vel; + a = iter->acc; + } + else + { + const double t2 = t1 * t1; + const double t3 = t1 * t2; + const double t4 = t1 * t3; + const double t5 = t1 * t4; + + const double & pi = iter->pos; + const double & vi = iter->vel; + const double & ai = iter->acc; + const double & pf = (iter + 1)->pos; + const double & vf = (iter + 1)->vel; + const double & af = (iter + 1)->acc; + + const double & c0 = pi; + const double & c1 = vi; + const double c2 = ai / 2.0; + const double c3 = (20.0 * (pf - pi) - (8.0 * vf + 12.0 * vi) * t1 + (af - 3.0 * ai) * t2) / (2.0 * t3); + const double c4 = (-30.0 * (pf - pi) + (14.0 * vf + 16.0 * vi) * t1 - (2.0 * af - 3.0 * ai) * t2) / (2.0 * t4); + const double c5 = (12.0 * (pf - pi) - 6.0 * (vf + vi) * t1 + (af - ai) * t2) / (2.0 * t5); + + const double tn = (double)(t_ - ti) * InterpolatorBase::scale; + p = c0 + (c1 + (c2 + (c3 + (c4 + c5 * tn) * tn) * tn) * tn) * tn; + v = c1 + (2.0 * c2 + (3.0 * c3 + (4.0 * c4 + 5.0 * c5 * tn) * tn) * tn) * tn; + a = 2.0 * c2 + (6.0 * c3 + (12.0 * c4 + 20.0 * c5 * tn) * tn) * tn; + } +} + +template +void CubicSplineBase::get(T t_, double & p, double & v, double & a) +{ + using namespace motion_interpolator; + + if(InterpolatorBase::data.empty()) return; + + if(InterpolatorBase::data.size() == 1) + { + p = InterpolatorBase::data.front().pos; + v = 0.0; + a = 0.0; + return; + } + + unsigned int n_base = 0; + unsigned int n_segment_coeff = 0; + unsigned int n_segment_data = 0; + typename std::deque>::const_iterator iter = InterpolatorBase::data.begin(); + if(t_ <= iter->t) + { + t_ = iter->t; + } + else if(t_ > InterpolatorBase::data.back().t) + { + n_base = coeff.size() - 1; + n_segment_coeff = coeff[n_base].size() / 2 - 2; + n_segment_data = InterpolatorBase::data.size() - 2; + iter = InterpolatorBase::data.end() - 2; + t_ = InterpolatorBase::data.back().t; + } + else + { + while(t_ > (iter + 1)->t) + { + if((iter + 1)->boundary) + { + n_base++; + n_segment_coeff = 0; + } + else + n_segment_coeff++; + + n_segment_data++; + iter++; + } + } + + if(coeff[n_base].size() <= 4) + { + getPointToPoint(t_, p, v, a, n_segment_data); + } + else + { + double h = (double)((iter + 1)->t - iter->t) * InterpolatorBase::scale; + if(h <= 0.0) + throw "Bad input to routine splint"; + else + { + double dhi = (double)((iter + 1)->t - t_) * InterpolatorBase::scale / h; + double dlo = (double)(t_ - iter->t) * InterpolatorBase::scale / h; + + p = dhi * iter->pos + dlo * (iter + 1)->pos + + ((dhi * dhi * dhi - dhi) * coeff[n_base][n_segment_coeff] + + (dlo * dlo * dlo - dlo) * coeff[n_base][n_segment_coeff + 1]) + * (h * h) / 6.0; + v = (-iter->pos + (iter + 1)->pos) / h + + ((-3.0 * dhi * dhi + 1.0) * coeff[n_base][n_segment_coeff] + + (3.0 * dlo * dlo - 1.0) * coeff[n_base][n_segment_coeff + 1]) + * h / 6.0; + a = dhi * coeff[n_base][n_segment_coeff] + dlo * coeff[n_base][n_segment_coeff + 1]; + } + } +} + +template +void CubicSplineBase::makeSplineTable(unsigned int n_base, + unsigned int n_size, + unsigned int n_offset, + const double & qn, + const double & un) +{ + using namespace motion_interpolator; + + typename std::deque>::const_iterator iter = InterpolatorBase::data.begin() + n_offset; + for(unsigned int i = 1; i < n_size - 1; i++) + { + double sig = ((double)((iter + i)->t - (iter + i - 1)->t)) / ((double)((iter + i + 1)->t - (iter + i - 1)->t)); + double p = sig * coeff[n_base][i - 1] + 2.0; + coeff[n_base][i] = (sig - 1.0) / p; + coeff[n_base][i + n_size] = ((iter + i + 1)->pos - (iter + i)->pos) + / ((double)((iter + i + 1)->t - (iter + i)->t) * InterpolatorBase::scale) + - ((iter + i)->pos - (iter + i - 1)->pos) + / ((double)((iter + i)->t - (iter + i - 1)->t) * InterpolatorBase::scale); + coeff[n_base][i + n_size] = (6.0 * coeff[n_base][i + n_size] + / ((double)((iter + i + 1)->t - (iter + i - 1)->t) * InterpolatorBase::scale) + - sig * coeff[n_base][i - 1 + n_size]) + / p; + } + + coeff[n_base][n_size - 1] = (un - qn * coeff[n_base][n_size - 2 + n_size]) / (qn * coeff[n_base][n_size - 2] + 1.0); + for(int j = n_size - 2; j >= 0; j--) + coeff[n_base][j] = coeff[n_base][j] * coeff[n_base][j + 1] + coeff[n_base][j + n_size]; +} + +template +void CubicSplineBase::update(void) +{ + using namespace motion_interpolator; + + unsigned int n_base_max = 1; + for(typename std::deque>::const_iterator iter = InterpolatorBase::data.begin() + 1; + iter != InterpolatorBase::data.end() - 1; iter++) + { + if(iter->boundary) n_base_max++; + } + if(coeff.size() < n_base_max) coeff.resize(n_base_max); + + unsigned int n_base = 0; + unsigned int n_size = 2; + unsigned int n_data = 2; + for(typename std::deque>::const_iterator iter = InterpolatorBase::data.begin() + 1; + iter != InterpolatorBase::data.end(); iter++) + { + if(iter->boundary || (iter == InterpolatorBase::data.end() - 1)) + { + setSplineCoeff(n_base, n_size, n_data - n_size); + n_base++; + n_size = 2; + } + else + n_size++; + + n_data++; + } +} + +template +void CubicSplineBase::update(T t_) +{ + using namespace motion_interpolator; + + unsigned int n_base_max = 1; + for(typename std::deque>::const_iterator iter = InterpolatorBase::data.begin() + 1; + iter != InterpolatorBase::data.end() - 1; iter++) + { + if(iter->boundary) n_base_max++; + } + if(coeff.size() < n_base_max) coeff.resize(n_base_max); + + unsigned int n_base = 0; + unsigned int n_size = 0; + unsigned int n_data = 0; + typename std::deque>::const_iterator iter = InterpolatorBase::data.begin(); + while((iter + 2 != InterpolatorBase::data.end()) && ((t_ > (iter + 1)->t) || !(iter + 1)->boundary)) + { + if((iter + 1)->boundary) + { + n_base++; + n_size = 0; + } + else + n_size++; + + n_data++; + iter++; + } + + setSplineCoeff(n_base, n_size + 2, n_data - n_size); +} + +template +void CubicSplineBase::update(T t_from, T t_to) +{ + using namespace motion_interpolator; + + unsigned int n_base_max = 1; + for(typename std::deque>::const_iterator iter = InterpolatorBase::data.begin() + 1; + iter != InterpolatorBase::data.end() - 1; iter++) + { + if(iter->boundary) n_base_max++; + } + if(coeff.size() < n_base_max) coeff.resize(n_base_max); + + unsigned int n_base = 0; + unsigned int n_size = 0; + unsigned int n_data = 0; + typename std::deque>::const_iterator iter = InterpolatorBase::data.begin(); + while((iter + 2 != InterpolatorBase::data.end()) && ((t_from > (iter + 1)->t) || !(iter + 1)->boundary)) + { + if((iter + 1)->boundary) + { + n_base++; + n_size = 0; + } + else + n_size++; + + n_data++; + iter++; + } + + setSplineCoeff(n_base, n_size + 2, n_data - n_size); + + unsigned int n_base_to = n_base; + unsigned int n_size_to = n_size; + unsigned int n_data_to = n_data; + iter = InterpolatorBase::data.begin() + n_data; + while((iter + 2 != InterpolatorBase::data.end()) && ((t_to > (iter + 1)->t) || !(iter + 1)->boundary)) + { + if((iter + 1)->boundary) + { + if(n_base_to != n_base) setSplineCoeff(n_base_to, n_size_to + 2, n_data_to - n_size_to); + n_base_to++; + n_size_to = 0; + } + else + n_size_to++; + + n_data_to++; + iter++; + } + + if(n_base_to != n_base) setSplineCoeff(n_base_to, n_size_to + 2, n_data_to - n_size_to); +} +} // namespace motion_interpolator +} // namespace mc_planning diff --git a/include/mc_planning/InterpolatorBase.h b/include/mc_planning/InterpolatorBase.h new file mode 100644 index 0000000000..db0f62e710 --- /dev/null +++ b/include/mc_planning/InterpolatorBase.h @@ -0,0 +1,298 @@ +#pragma once +#include + +namespace mc_planning +{ +namespace motion_interpolator +{ + +typedef enum +{ + NOT_SELECTED, + // POINT_TO_POINT, + // POINT_TO_POINT_EXP, + // POINT_TO_POINT_CUBIC, + // POINT_TO_POINT_TRAPEZOID_VELOCITY_PROFILES, + // HOFFARBIB_MODEL, + // NATURAL_CUBIC_SPLINE, + CLAMPED_CUBIC_SPLINE, + // AKIMA_CUBIC_SPLINE, + // KOCHANEK_BARTELS_SPLINE, + // BSPLINE_OPEN_UNIFORM_KNOT, + // BSPLINE_OPEN_UNIFORM_KNOT_2ND, + // BSPLINE_OPEN_UNIFORM_KNOT_3RD, + // SINGLE_VIA_POINT, + // DOUBLE_VIA_POINTS, + // SEQUENTIAL_CUBIC_SPLINE, + // SEQUENTIAL_QUINTIC_SPLINE, + // TRAPEZOID_VELOCITY_PROFILES, + // STOP_EXP, + // STOP_QUINTIC_POLYNOMIALS, + // MONOTONIC_INCREASING_EXP +} interpolator_type; + +template +class InterpolatorDataType +{ +public: + bool boundary; + T t; + double pos; + double vel; + double acc; + + InterpolatorDataType(T t_ = 0.0, double pos_ = 0.0, double vel_ = 0.0, double acc_ = 0.0, bool boundary_ = false) + : boundary(boundary_), t(t_), pos(pos_), vel(vel_), acc(acc_) + { + } + + InterpolatorDataType(const InterpolatorDataType & obj) + : boundary(obj.boundary), t(obj.t), pos(obj.pos), vel(obj.vel), acc(obj.acc) + { + } +}; + +template +class InterpolatorBase +{ +private: + interpolator_type ip_type; + +protected: + const double scale; + const double eps; + + std::deque> data; + +public: + InterpolatorBase(const double & scale_ = 1.0, const double & eps = 0.001, interpolator_type ip_type = NOT_SELECTED); + + virtual ~InterpolatorBase() {} + + inline const interpolator_type & InterpolatorType(void) const + { + return ip_type; + } + + inline void clear(void) + { + data.clear(); + } + + inline unsigned int size(void) + { + return data.size(); + } + + inline void resize(unsigned int n) + { + data.resize(n); + } + + inline bool empty(void) + { + return data.empty(); + } + + inline std::deque> & getData(void) + { + return data; + } + + inline InterpolatorDataType & getData(int n) + { + return data[n]; + } + + inline InterpolatorDataType & front(void) + { + return data.front(); + } + + inline InterpolatorDataType & back(void) + { + return data.back(); + } + + inline virtual bool isFinish(const T & t_) + { + return (data.empty() || (data.back().t <= t_)) ? true : false; + } + + inline void pop_front(void) + { + if(!data.empty()) data.pop_front(); + } + + inline void pop_back(void) + { + if(!data.empty()) data.pop_back(); + } + + inline void push_back(const T & t_, const double & p) + { + data.push_back(InterpolatorDataType(t_, p)); + } + + inline void push_back(const T & t_, const double & p, const double & v) + { + data.push_back(InterpolatorDataType(t_, p, v)); + } + + inline void push_back(const T & t_, const double & p, const double & v, const double & a) + { + data.push_back(InterpolatorDataType(t_, p, v, a)); + } + + inline void push_back(const T & t_, const double & p, bool boundary) + { + data.push_back(InterpolatorDataType(t_, p, 0.0, 0.0, boundary)); + } + + inline void push_back(const T & t_, const double & p, const double & v, bool boundary) + { + data.push_back(InterpolatorDataType(t_, p, v, 0.0, boundary)); + } + + inline void push_back(const T & t_, const double & p, const double & v, const double & a, bool boundary) + { + data.push_back(InterpolatorDataType(t_, p, v, a, boundary)); + } + + inline void push_front(const T & t_, const double & p) + { + data.push_front(InterpolatorDataType(t_, p)); + } + + inline void push_front(const T & t_, const double & p, const double & v) + { + data.push_front(InterpolatorDataType(t_, p, v)); + } + + inline void push_front(const T & t_, const double & p, const double & v, const double & a) + { + data.push_front(InterpolatorDataType(t_, p, v, a)); + } + + inline void push_front(const T & t_, const double & p, bool boundary) + { + data.push_front(InterpolatorDataType(t_, p, 0.0, 0.0, boundary)); + } + + inline void push_front(const T & t_, const double & p, const double & v, bool boundary) + { + data.push_front(InterpolatorDataType(t_, p, v, 0.0, boundary)); + } + + inline void push_front(const T & t_, const double & p, const double & v, const double & a, bool boundary) + { + data.push_front(InterpolatorDataType(t_, p, v, a, boundary)); + } + + void set(const int n, const T & t, const double & p); + + void set(const int n, const T & t, const double & p, const double & v); + + void set(const int n, const T & t, const double & p, const double & v, const double & a); + + virtual void set(const int n, const T & t, const double & p, bool boundary); + + virtual void set(const int n, const T & t, const double & p, const double & v, bool boundary); + + virtual void set(const int n, const T & t, const double & p, const double & v, const double & a, bool boundary); + + double getValue(T t_); + + void get(T t_, double & p); + + void get(T t_, double & p, double & v); + + virtual void get(T t_, double & p, double & v, double & a) = 0; + + virtual void update(void){}; + + virtual void update(T t_){}; + + virtual void update(T t_from, T t_to){}; +}; + +template +InterpolatorBase::InterpolatorBase(const double & scale_, const double & eps_, interpolator_type ip_type_) +: ip_type(ip_type_), scale(scale_), eps(eps_) +{ +} + +template +void InterpolatorBase::set(int n, const T & t_, const double & p) +{ + data[n].t = t_; + data[n].pos = p; +} + +template +void InterpolatorBase::set(int n, const T & t_, const double & p, const double & v) +{ + data[n].t = t_; + data[n].pos = p; + data[n].vel = v; +} + +template +void InterpolatorBase::set(int n, const T & t_, const double & p, const double & v, const double & a) +{ + data[n].t = t_; + data[n].pos = p; + data[n].vel = v; + data[n].acc = a; +} + +template +void InterpolatorBase::set(int n, const T & t_, const double & p, bool boundary) +{ + data[n].t = t_; + data[n].pos = p; + data[n].boundary = boundary; +} + +template +void InterpolatorBase::set(int n, const T & t_, const double & p, const double & v, bool boundary) +{ + data[n].t = t_; + data[n].pos = p; + data[n].vel = v; + data[n].boundary = boundary; +} + +template +void InterpolatorBase::set(int n, const T & t_, const double & p, const double & v, const double & a, bool boundary) +{ + data[n].t = t_; + data[n].pos = p; + data[n].vel = v; + data[n].acc = a; + data[n].boundary = boundary; +} + +template +void InterpolatorBase::get(T t_, double & p) +{ + double v, a; + get(t_, p, v, a); +} + +template +void InterpolatorBase::get(T t_, double & p, double & v) +{ + double a; + get(t_, p, v, a); +} + +template +double InterpolatorBase::getValue(T t_) +{ + double p; + get(t_, p); + return p; +} +} // namespace motion_interpolator +} // namespace mc_planning diff --git a/include/mc_planning/LIPMControlByPoleAssign.h b/include/mc_planning/LIPMControlByPoleAssign.h index d0c34b9b7a..0d1fa46cd0 100644 --- a/include/mc_planning/LIPMControlByPoleAssign.h +++ b/include/mc_planning/LIPMControlByPoleAssign.h @@ -6,7 +6,7 @@ namespace mc_planning namespace linear_control_system { -class LIPMControlByPoleAssign : public LinearControl3 +class MC_PLANNING_DLLAPI LIPMControlByPoleAssign : public LinearControl3 { public: void setStateVariables(double x_, double v_, double p_) diff --git a/include/mc_planning/LIPMControlByPoleAssignWithExternalForce.h b/include/mc_planning/LIPMControlByPoleAssignWithExternalForce.h index 0c3fc4d212..00b33c3314 100644 --- a/include/mc_planning/LIPMControlByPoleAssignWithExternalForce.h +++ b/include/mc_planning/LIPMControlByPoleAssignWithExternalForce.h @@ -6,7 +6,7 @@ namespace mc_planning namespace linear_control_system { -class LIPMControlByPoleAssignWithExternalForce : public LIPMControlByPoleAssign +class MC_PLANNING_DLLAPI LIPMControlByPoleAssignWithExternalForce : public LIPMControlByPoleAssign { private: Eigen::Vector3d B2; diff --git a/include/mc_planning/LinearTimeVariantInvertedPendulum.h b/include/mc_planning/LinearTimeVariantInvertedPendulum.h index 9bb7028693..cdd4e84ab5 100644 --- a/include/mc_planning/LinearTimeVariantInvertedPendulum.h +++ b/include/mc_planning/LinearTimeVariantInvertedPendulum.h @@ -22,6 +22,9 @@ namespace mc_planning { +namespace linear_control_system +{ + /** * @brief Reference CoM position and velocity trajectory generation. * @@ -140,6 +143,16 @@ struct MC_PLANNING_DLLAPI LinearTimeVariantInvertedPendulum */ State getState(int n_time) const; + /*! @brief to get generated CoG/ZMP states + * @param[in] n_time is the time in range of[-n_preview:n_preview], current time is 0 + * @param[out] cog_pos is position of COG + * @param[out] cog_vel is velocity of COG + * @param[out] cog_acc is acceleration of COG + * @param[in] p is position of ZMP + * @param[in] pdot is velocity of ZMP + */ + void getState(int n_time, double & cog_pos, double & cog_vel, double & cog_acc, double & p, double & pdot); + /** XXX Unused by MultiContact COG generation */ void generate(Eigen::VectorXd & cog_pos, @@ -178,7 +191,7 @@ struct MC_PLANNING_DLLAPI LinearTimeVariantInvertedPendulum * * @param k Index of the value in range \f$ [0, M] \f$ */ - double w2(unsigned k) const + double & w2(unsigned k) { return m_w2[k]; } @@ -249,4 +262,6 @@ struct MC_PLANNING_DLLAPI LinearTimeVariantInvertedPendulum Eigen::VectorXd m_w; ///< \f$ \omega \f$ }; +} // namespace linear_control_system + } // namespace mc_planning diff --git a/include/mc_planning/MathFunction.h b/include/mc_planning/MathFunction.h new file mode 100644 index 0000000000..36c445d321 --- /dev/null +++ b/include/mc_planning/MathFunction.h @@ -0,0 +1,205 @@ +/* + * Copyright (c) 2009, + * @author Mitsuharu Morisawa + * + * AIST + * + * All rights reserved. + * + * This program is made available under the terms of the Eclipse Public License + * v1.0 which accompanies this distribution, and is available at + * http://www.eclipse.org/legal/epl-v10.html + */ +#pragma once +#include + +namespace mc_planning +{ + +const double epsilonAngle = 1e-16; + +template +inline T Max(const T & a, const T & b) +{ + return ((a) > (b) ? (a) : (b)); +} + +template +inline T Min(const T & a, const T & b) +{ + return ((a) < (b) ? (a) : (b)); +} + +template +inline int Sign(const T & x) +{ + return ((x < 0) ? (-1) : ((x > 0) ? 1 : 0)); +} + +template +inline T Pow2(const T & a) +{ + return dot((a), (a)); +} + +template +inline T polynomial3(const T & t) +{ + return ((3.0 - 2.0 * (t)) * (t) * (t)); +} + +template +inline T dpolynomial3(const T & t) +{ + return ((6.0 - 6.0 * (t)) * (t)); +} + +template +inline T ddpolynomial3(const T & t) +{ + return (6.0 - 12.0 * (t)); +} + +template +inline T polynomial5(const T & t) +{ + return (((6.0 * (t)-15.0) * (t) + 10.0) * (t) * (t) * (t)); +} + +template +inline T dpolynomial5(const T & t) +{ + return (((30.0 * (t)-60.0) * (t) + 30.0) * (t) * (t)); +} + +template +inline T ddpolynomial5(const T & t) +{ + return (((120.0 * (t)-180.0) * (t) + 60.0) * (t)); +} + +#if defined(__GNUC__) +# define __ATTRIBUTE_ALWAYS_INLINE__ __attribute__((always_inline)) +#else +# define __ATTRIBUTE_ALWAYS_INLINE__ +#endif + +inline Eigen::Matrix3d __ATTRIBUTE_ALWAYS_INLINE__ rotFromRoll(const double & roll) +{ + const double c = cos(roll); + const double s = sin(roll); + Eigen::Matrix3d r; + r << 1.0, 0, 0, 0, c, -s, 0, s, c; + return r; +} + +inline Eigen::Matrix3d __ATTRIBUTE_ALWAYS_INLINE__ rotFromPitch(const double & pitch) +{ + const double c = cos(pitch); + const double s = sin(pitch); + Eigen::Matrix3d r; + r << c, 0, s, 0, 1.0, 0, -s, 0, c; + return r; +} + +inline Eigen::Matrix3d __ATTRIBUTE_ALWAYS_INLINE__ rotFromYaw(const double & yaw) +{ + const double c = cos(yaw); + const double s = sin(yaw); + Eigen::Matrix3d r; + r << c, -s, 0, s, c, 0, 0, 0, 1.0; + return r; +} + +inline Eigen::Matrix3d __ATTRIBUTE_ALWAYS_INLINE__ rotFromYrp(const double & y, const double & r, const double & p) +{ + return (Eigen::AngleAxisd(y, Eigen::Vector3d::UnitZ()) * Eigen::AngleAxisd(r, Eigen::Vector3d::UnitX()) + * Eigen::AngleAxisd(p, Eigen::Vector3d::UnitY())) + .toRotationMatrix(); +} + +inline Eigen::Matrix3d __ATTRIBUTE_ALWAYS_INLINE__ rotFromAngleAxis(const double & angles, const Eigen::Vector3d & axis) +{ + return Eigen::AngleAxisd(angles, axis).toRotationMatrix(); + // return rodrigues(axis, angles); +} + +inline Eigen::Matrix3d __ATTRIBUTE_ALWAYS_INLINE__ rotFromAngleAxis(const Eigen::Vector3d & omega, + const double eps = 1.0e-6) +{ +#if 0 + Eigen::Vector3d w(omega); + for( int axis = 0 ; axis <= 2 ; axis++ ){ + while( w(axis) > M_PI ) w(axis) -= M_PI; + while( w(axis) < -M_PI ) w(axis) += M_PI; + } + double th = w.norm(); + return (th > eps) ? Eigen::AngleAxisd(th, w.normalized()).toRotationMatrix() : Eigen::Matrix3d::Identity(); +#else + double th = omega.norm(); + return (th > eps) ? Eigen::AngleAxisd(th, omega.normalized()).toRotationMatrix() : Eigen::Matrix3d::Identity(); + // return (th > eps) ? rodrigues(omega.normalized(), th) : Eigen::Matrix3d::Identity(); +#endif +} + +inline Eigen::Matrix3d __ATTRIBUTE_ALWAYS_INLINE__ rotFromAngleAxis(double e1, + double e2, + double e3, + const double eps = 1.0e-6) +{ + return rotFromAngleAxis(Eigen::Vector3d(e1, e2, e3), eps); +} + +/// !!! yrpFromRot can't check singularity (p = pi/2) +Eigen::Vector3d yrpFromRot(const Eigen::Matrix3d & m); +double yawFromRot(const Eigen::Matrix3d & r); +Eigen::Vector3d omegaFromRotApproximation(const Eigen::Matrix3d & r); + +inline Eigen::Matrix3d mergeTiltWithYaw(const Eigen::Vector3d & Rtez, const Eigen::Matrix3d & R2) +{ + + Eigen::Matrix3d R_temp1, R_temp2; + const Eigen::Vector3d & ez = Eigen::Vector3d::UnitZ(); + const Eigen::Vector3d & v1 = Rtez; + Eigen::Vector3d mlxv1 = (R2.transpose() * Eigen::Vector3d::UnitX()).cross(v1); + double n2 = mlxv1.squaredNorm(); + + if(n2 > epsilonAngle * epsilonAngle) + { + R_temp1 << -Eigen::Vector3d::UnitY(), Eigen::Vector3d::UnitX(), ez; + mlxv1 /= sqrt(n2); + R_temp2 << mlxv1.transpose(), v1.cross(mlxv1).transpose(), v1.transpose(); + return R_temp1 * R_temp2; + } + else + { + mlxv1 = (R2.transpose() * Eigen::Vector3d::UnitY()).cross(v1).normalized(); + R_temp2 << mlxv1.transpose(), v1.cross(mlxv1).transpose(), v1.transpose(); + return R_temp2.transpose(); + } +} + +inline Eigen::Matrix3d mergeRoll1Pitch1WithYaw2(const Eigen::Matrix3d & R1, const Eigen::Matrix3d & R2) +{ + return mergeTiltWithYaw(R1.transpose() * Eigen::Vector3d::UnitZ(), R2); +} + +double Saturation(const double & data, const double ulimit, const double llimit); +double Threshold(const double & data, const double ulimit, const double llimit); +double NormalizedTrapezoidCurve(int n_now, int n_ini, int n_acc, int n_dec, int n_end); + +template +T LowPassFilter(const T & data, T & data_lpf, const double gain) +{ + data_lpf += (data - data_lpf) * gain; + return data_lpf; +} + +template +T HighPassFilter(const T & data, T & data_hpf, const double gain) +{ + data_hpf += (data - data_hpf * (1.0 - gain)); + return data_hpf; +} + +} // namespace mc_planning diff --git a/include/mc_planning/State.h b/include/mc_planning/State.h new file mode 100644 index 0000000000..77ace42b9f --- /dev/null +++ b/include/mc_planning/State.h @@ -0,0 +1,87 @@ +/* + * Copyright (c) 2009, @author Mitsuharu Morisawa + * + * AIST + * + * All rights reserved. + * + * This program is made available under the terms of the Eclipse Public License + * v1.0 which accompanies this distribution, and is available at + * http://www.eclipse.org/legal/epl-v10.html + */ +#pragma once +#include +#include + +namespace mc_planning +{ + +class MC_PLANNING_DLLAPI StateP +{ +public: + Eigen::Vector3d P; ///< position + + /** + @brief constructor + */ + StateP(void); + + StateP(const Eigen::Vector3d & p); + + StateP(const StateP & i_state); + + StateP & operator=(const StateP & i_state); + + friend std::ostream & operator<<(std::ostream & os, const StateP & s); + + void Initialize(void); + + void Copy(const StateP & i_state); +}; + +class MC_PLANNING_DLLAPI StatePV : virtual public StateP +{ +public: + Eigen::Vector3d V; ///< velocity + + /** + @brief constructor + */ + StatePV(void); + + StatePV(const Eigen::Vector3d & p, const Eigen::Vector3d & v); + + StatePV(const StatePV & i_state); + + StatePV & operator=(const StatePV & i_state); + + friend std::ostream & operator<<(std::ostream & os, const StatePV & s); + + void Initialize(void); + + void Copy(const StatePV & i_state); +}; + +class MC_PLANNING_DLLAPI StatePVA : virtual public StatePV +{ +public: + Eigen::Vector3d Vdot; ///< acceleration + + /** + @brief constructor + */ + StatePVA(void); + + StatePVA(const Eigen::Vector3d & p, const Eigen::Vector3d & v, const Eigen::Vector3d & a); + + StatePVA(const StatePVA & i_state); + + StatePVA & operator=(const StatePVA & i_state); + + friend std::ostream & operator<<(std::ostream & os, const StatePVA & s); + + void Initialize(void); + + void Copy(const StatePVA & i_state); +}; +} // namespace mc_planning diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index ba02470571..4e4bd438b6 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -412,6 +412,8 @@ mc_planning/LinearTimeVariantInvertedPendulum.cpp mc_planning/LinearControl3.cpp mc_planning/LIPMControlByPoleAssign.cpp mc_planning/LIPMControlByPoleAssignWithExternalForce.cpp +mc_planning/MathFunction.cpp +mc_planning/State.cpp ) set(mc_planning_HDR @@ -421,6 +423,9 @@ set(mc_planning_HDR ../include/mc_planning/LinearControl3.h ../include/mc_planning/LIPMControlByPoleAssign.h ../include/mc_planning/LIPMControlByPoleAssignWithExternalForce.h +../include/mc_planning/InterpolatorBase.h +../include/mc_planning/CubicSplineBase.h +../include/mc_planning/ClampedCubicSpline.h ) add_library(mc_planning SHARED ${mc_planning_SRC} ${mc_planning_HDR}) diff --git a/src/mc_planning/LinearTimeVariantInvertedPendulum.cpp b/src/mc_planning/LinearTimeVariantInvertedPendulum.cpp index 52e4aa2c08..5d95839e6d 100644 --- a/src/mc_planning/LinearTimeVariantInvertedPendulum.cpp +++ b/src/mc_planning/LinearTimeVariantInvertedPendulum.cpp @@ -7,6 +7,9 @@ namespace cst = mc_rtc::constants; namespace mc_planning { +namespace linear_control_system +{ + LinearTimeVariantInvertedPendulum::LinearTimeVariantInvertedPendulum() {} LinearTimeVariantInvertedPendulum::LinearTimeVariantInvertedPendulum(double dt, @@ -223,4 +226,20 @@ LinearTimeVariantInvertedPendulum::State LinearTimeVariantInvertedPendulum::getS return s; } +void LinearTimeVariantInvertedPendulum::getState(int n_time, + double & cog_pos, + double & cog_vel, + double & cog_acc, + double & p, + double & pdot) +{ + p = m_p_ref(m_n_current + n_time); + pdot = (p - m_p_ref[m_n_current + n_time - 1]) / m_dt; + cog_pos = m_X[m_n_current + n_time](0); + cog_vel = m_X[m_n_current + n_time](1); + cog_acc = m_w2[m_n_current + n_time] * (cog_pos - p); +} + +} // namespace linear_control_system + } // namespace mc_planning diff --git a/src/mc_planning/MathFunction.cpp b/src/mc_planning/MathFunction.cpp new file mode 100644 index 0000000000..fb63f4d0d3 --- /dev/null +++ b/src/mc_planning/MathFunction.cpp @@ -0,0 +1,72 @@ +/* + * Copyright (c) 2009, + * @author Mitsuharu Morisawa + * + * AIST + * + * All rights reserved. + * + * This program is made available under the terms of the Eclipse Public License + * v1.0 which accompanies this distribution, and is available at + * http://www.eclipse.org/legal/epl-v10.html + */ +#include + +namespace mc_planning +{ + +Eigen::Vector3d yrpFromRot(const Eigen::Matrix3d & m) +{ + double roll = atan2(-m(1, 2), m(2, 2)); + double cr = cos(roll); + double pitch = atan2(cr * m(0, 2), m(2, 2)); + double cp = cos(pitch), sr = sin(roll); + double yaw = atan2(cp * (cr * m(1, 0) + sr * m(2, 0)), m(0, 0)); + + return Eigen::Vector3d(roll, pitch, yaw); +} + +double yawFromRot(const Eigen::Matrix3d & r) +{ + return atan2(r(1, 0), r(0, 0)); +} + +Eigen::Vector3d omegaFromRotApproximation(const Eigen::Matrix3d & r) +{ + const double k = -0.5; + return Eigen::Vector3d((r(1, 2) - r(2, 1)) * k, (r(2, 0) - r(0, 2)) * k, (r(0, 1) - r(1, 0)) * k); +} + +double Saturation(const double & data, const double ulimit, const double llimit) +{ + if(data > ulimit) + return ulimit; + else if(data < llimit) + return llimit; + return data; +} + +double Threshold(const double & data, const double ulimit, const double llimit) +{ + if(data > ulimit) + return data - ulimit; + else if(data < llimit) + return data - llimit; + return 0.0; +} + +double NormalizedTrapezoidCurve(int n_now, int n_ini, int n_acc, int n_dec, int n_end) +{ + if(n_now < n_ini) + return 0.0; + else if(n_now <= n_acc) + return polynomial3((double)((n_now - n_ini)) / (double)((n_acc - n_ini))); + else if(n_now <= n_dec) + return 1.0; + else if(n_now <= n_end) + return polynomial3((double)((n_end - n_now)) / (double)((n_end - n_dec))); + + return 0.0; +} + +} // namespace mc_planning diff --git a/src/mc_planning/State.cpp b/src/mc_planning/State.cpp new file mode 100644 index 0000000000..78c178db49 --- /dev/null +++ b/src/mc_planning/State.cpp @@ -0,0 +1,138 @@ +/* + * Copyright (c) 2009, + * @author Mitsuharu Morisawa + * + * AIST + * + * All rights reserved. + * + * This program is made available under the terms of the Eclipse Public License + * v1.0 which accompanies this distribution, and is available at + * http://www.eclipse.org/legal/epl-v10.html + */ +#include +#include +// #include +// #include +// + +namespace mc_planning +{ + +//////////////////////////////////////////////////////////// +StateP::StateP(void) +{ + StateP::Initialize(); +} + +StateP::StateP(const Eigen::Vector3d & p) : P(p) {} + +StateP::StateP(const StateP & i_state) +{ + StateP::Copy(i_state); +} + +StateP & StateP::operator=(const StateP & i_state) +{ + StateP::Copy(i_state); + return (*this); +} + +std::ostream & operator<<(std::ostream & os, const StateP & s) +{ + os << "P = " << s.P.transpose() << std::endl; + ; + return os; +} + +void StateP::Initialize(void) +{ + P.setZero(); +} + +void StateP::Copy(const StateP & i_state) +{ + P = i_state.P; +} + +//////////////////////////////////////////////////////////// +StatePV::StatePV(void) +{ + StatePV::Initialize(); +} + +StatePV::StatePV(const Eigen::Vector3d & p, const Eigen::Vector3d & v) : StateP(p), V(v) {} + +StatePV::StatePV(const StatePV & i_state) +{ + StatePV::Copy(i_state); +} + +StatePV & StatePV::operator=(const StatePV & i_state) +{ + StatePV::Copy(i_state); + return (*this); +} + +std::ostream & operator<<(std::ostream & os, const StatePV & s) +{ + os << "P = " << s.P.transpose() << std::endl; + os << "V = " << s.V.transpose() << std::endl; + ; + return os; +} + +void StatePV::Initialize(void) +{ + StateP::Initialize(); + V.setZero(); +} + +void StatePV::Copy(const StatePV & i_state) +{ + StateP::Copy(i_state); + V = i_state.V; +} + +//////////////////////////////////////////////////////////// +StatePVA::StatePVA(void) +{ + StatePVA::Initialize(); +} + +StatePVA::StatePVA(const Eigen::Vector3d & p, const Eigen::Vector3d & v, const Eigen::Vector3d & a) +: StatePV(p, v), Vdot(a) +{ +} + +StatePVA::StatePVA(const StatePVA & i_state) +{ + StatePVA::Copy(i_state); +} + +StatePVA & StatePVA::operator=(const StatePVA & i_state) +{ + StatePVA::Copy(i_state); + return (*this); +} + +std::ostream & operator<<(std::ostream & os, const StatePVA & s) +{ + os << "P = " << s.P.transpose() << std::endl; + os << "V = " << s.V.transpose() << std::endl; + os << "Vdot = " << s.Vdot.transpose() << std::endl; + return os; +} + +void StatePVA::Initialize(void) +{ + StatePV::Initialize(); + Vdot.setZero(); +} + +void StatePVA::Copy(const StatePVA & i_state) +{ + StatePV::Copy(i_state); + Vdot = i_state.Vdot; +} +} // namespace mc_planning diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt index 139753f16e..d1d6485237 100644 --- a/tests/CMakeLists.txt +++ b/tests/CMakeLists.txt @@ -141,3 +141,5 @@ mc_rtc_test(test_interpolation mc_control) # -- Temporary HMC-related tests -- # ##################################### mc_rtc_test(test_hmc mc_planning) + +add_subdirectory(hmc) diff --git a/tests/hmc/CMakeLists.txt b/tests/hmc/CMakeLists.txt new file mode 100644 index 0000000000..f2ffd35e88 --- /dev/null +++ b/tests/hmc/CMakeLists.txt @@ -0,0 +1,2 @@ +add_executable(generator testCoMGenerator.cpp generator.cpp) +target_link_libraries(generator PUBLIC mc_planning) diff --git a/tests/hmc/generator.cpp b/tests/hmc/generator.cpp new file mode 100644 index 0000000000..f5a6a97a3b --- /dev/null +++ b/tests/hmc/generator.cpp @@ -0,0 +1,193 @@ +#include "generator.h" +#include +#include +#include +// #include +// #include + +using namespace mc_planning::motion_interpolator; + +const double waist_height = 0.8; +constexpr int X = 0; +constexpr int Y = 1; +constexpr int Z = 2; + +namespace mc_planning +{ + +generator::generator(int n_preview, double dt) +: m_ComInterp(NULL), m_Pcalpha_ideal_pre(Eigen::Vector3d::Zero()), m_Pcalpha_ideal(Eigen::Vector3d::Zero()), + m_Pcalpha_cmp(Eigen::Vector3d::Zero()), m_Vcalpha_ideal_pre(Eigen::Vector3d::Zero()), + m_Vcalpha_ideal(Eigen::Vector3d::Zero()), m_Vcalpha_cmp(Eigen::Vector3d::Zero()), + m_Pcalpha_out(Eigen::Vector3d::Zero()), m_Pcalpha_motion_out(Eigen::Vector3d::Zero()), m_n_preview(n_preview), + m_n_steps(0), m_dt(dt), m_omega_valpha(0.0), m_mass(60.0) +{ + // setInterpolator(m_ComInterp, "clamped-cubic-spline", m_dt); + m_ComInterp = new ClampedCubicSpline(1.0, m_dt / 2); + + m_ipm_long[X].Initialize(m_dt, m_n_preview, 20000); + m_ipm_long[Y].Initialize(m_dt, m_n_preview, 20000); + m_ipm_short[X].Initialize(); + m_ipm_short[Y].Initialize(); + + m_poles[X] << 1.0, 1.0, 150.0; + m_poles[Y] << 1.0, 1.0, 150.0; + + m_virtual_height[X].setZero(m_n_preview * 2 + 1); + m_virtual_height[Y].setZero(m_n_preview * 2 + 1); + m_cog_height.setZero(m_n_preview * 2 + 1); + m_cog_dot_height.setZero(m_n_preview * 2 + 1); + m_cog_ddot_height.setZero(m_n_preview * 2 + 1); + + m_ipm_long[X].initMatrices(waist_height); + m_ipm_long[Y].initMatrices(waist_height); + + m_COG_ideal.P << 0.0, 0.0, waist_height; +} + +generator::~generator() +{ + delete m_ComInterp; +} + +void generator::setupCOGHeight(int n_current) +{ +#if 0 + for( int i = 0 ; i <= 25; i++ ){ + double t = (double)i * 36.0 * m_dt; + double waist = 0.2 * sin(2.0 * M_PI * t) + waist_height; + m_ComInterp->push_back(i * 40 + 620, waist); + } +#endif + if(n_current == 0) + { + m_ComInterp->push_back(0.0, waist_height); + m_ComInterp->push_back(lround(m_steps.back()(0) / m_dt), waist_height); + m_ComInterp->update(); + } + + for(int n_step = -m_n_preview; n_step <= m_n_preview; n_step++) + { + m_ComInterp->get(n_current + n_step + m_n_preview, m_cog_height[n_step + m_n_preview], + m_cog_dot_height[n_step + m_n_preview], m_cog_ddot_height[n_step + m_n_preview]); + } + + m_COG_ideal.P(Z) = m_cog_height[m_n_preview]; + m_COG_ideal.V(Z) = m_cog_dot_height[m_n_preview]; + m_COG_ideal.Vdot(Z) = m_cog_ddot_height[m_n_preview]; +} + +void generator::setupTimeTrajectories(int n_current) +{ + Eigen::VectorXd & px_ref = m_ipm_long[X].p_ref(); + Eigen::VectorXd & py_ref = m_ipm_long[Y].p_ref(); + int n_steps_loop = m_n_steps; + for(int i = 0; i < m_n_preview * 2 + 1; i++) + { + px_ref(i) = 0.0; + py_ref(i) = 0.0; + m_virtual_height[X](i) = 0.0; + m_virtual_height[Y](i) = 0.0; + + double tm = (double)(i + n_current) * m_dt; + if(tm >= m_steps[n_steps_loop](0)) + { + px_ref(i) = m_steps[n_steps_loop](1) + + (m_steps[n_steps_loop + 1](1) - m_steps[n_steps_loop](1)) + * polynomial3((tm - m_steps[n_steps_loop](0)) + / (m_steps[n_steps_loop + 1](0) - m_steps[n_steps_loop](0))); + py_ref(i) = m_steps[n_steps_loop](2) + + (m_steps[n_steps_loop + 1](2) - m_steps[n_steps_loop](2)) + * polynomial3((tm - m_steps[n_steps_loop](0)) + / (m_steps[n_steps_loop + 1](0) - m_steps[n_steps_loop](0))); + } + else + { + px_ref(i) = m_steps[n_steps_loop + 1](1); + py_ref(i) = m_steps[n_steps_loop + 1](2); + } + + m_ipm_long[X].w2(i) = + (mc_rtc::constants::GRAVITY + m_cog_ddot_height[i]) / (m_cog_height[i] - m_virtual_height[X](i)); + m_ipm_long[Y].w2(i) = + (mc_rtc::constants::GRAVITY + m_cog_ddot_height[i]) / (m_cog_height[i] - m_virtual_height[Y](i)); + + if(n_steps_loop < m_steps.size() - 1) + { + if(tm >= m_steps[n_steps_loop + 1](0)) n_steps_loop++; + } + } + + m_Pcalpha_ideal(Z) = (m_virtual_height[X](m_n_preview) + m_virtual_height[Y](m_n_preview)) / 2.0; + + double tm_cur = (double)n_current * m_dt; + if(m_n_steps < m_steps.size() - 1) + { + if(tm_cur > m_steps[m_n_steps + 1](0)) m_n_steps++; + } + +#if 0 + std::cout << "time=" << (double)n_current*m_dt << std::endl; + std::cout << "cog_height=" << m_cog_height.transpose() << std::endl; + std::cout << "vr_height=" << m_virtual_height[X].transpose() << std::endl; +#endif +#if 0 + std::cout << "time=" << (double)n_current*m_dt << std::endl; + std::cout << "cog_height=" << m_cog_height[m_n_preview] << std::endl; + std::cout << "vr_height=" << m_virtual_height[X][m_n_preview] << std::endl; + std::cout << "p_ref=" << px_ref[m_n_preview] << "," << py_ref[m_n_preview] << std::endl; +#endif +} + +void generator::generateTrajectories(void) +{ + StatePV COG_ideal_pre_next(m_COG_ideal.P, m_COG_ideal.V); + Eigen::Vector3d Pcalpha_ideal_pre_next(m_Pcalpha_ideal); + for(int axis = X; axis <= Y; axis++) + { + m_ipm_long[axis].update(); + + m_ipm_long[axis].getState(0, m_COG_ideal_pre.P(axis), m_COG_ideal_pre.V(axis), m_COG_ideal_pre.Vdot(axis), + m_Pcalpha_ideal_pre(axis), m_Vcalpha_ideal_pre(axis)); + m_ipm_long[axis].getState(1, m_COG_ideal.P(axis), m_COG_ideal.V(axis), m_COG_ideal.Vdot(axis), + m_Pcalpha_ideal(axis), m_Vcalpha_ideal(axis)); + + double omega2 = m_ipm_long[axis].w2(m_n_preview); + double omega = sqrt(omega2); + m_ipm_short[axis].setSystemMatrices(omega * m_poles[axis](0), omega * m_poles[axis](1), m_poles[axis](2), omega2, + m_mass); + + m_COG_cmp.P(axis) += COG_ideal_pre_next.P(axis) - m_COG_ideal_pre.P(axis); + m_COG_cmp.V(axis) += COG_ideal_pre_next.V(axis) - m_COG_ideal_pre.V(axis); + m_Pcalpha_cmp(axis) += Pcalpha_ideal_pre_next(axis) - m_Pcalpha_ideal(axis); + + m_ipm_short[axis].setStateVariables(m_COG_cmp.P(axis), m_COG_cmp.V(axis), m_Pcalpha_cmp(axis)); + double Fext = 0.0; + m_ipm_short[axis].update(Fext, m_dt); + m_ipm_short[axis].getStateVariables(m_COG_cmp.P(axis), m_COG_cmp.V(axis), m_Pcalpha_cmp(axis), m_Vcalpha_cmp(axis)); + } + + m_COG_out.P = m_COG_ideal.P + m_COG_cmp.P; + m_COG_out.V = m_COG_ideal.V + m_COG_cmp.V; + m_COG_out.Vdot = m_COG_ideal.Vdot + m_COG_cmp.Vdot; + + m_Pcalpha_out = m_Pcalpha_ideal + m_Pcalpha_cmp; + m_Pcalpha_motion_out = m_Pcalpha_out + m_Vcalpha_ideal * m_omega_valpha * m_dt; +} + +void generator::generate(int n_time) +{ + setupCOGHeight(n_time); + setupTimeTrajectories(n_time); + generateTrajectories(); + +#if 0 + std::cout << "COG_out = " << m_COG_out.P.transpose() << std::endl; + std::cout << "Pcalpha_out = " << m_Pcalpha_out.transpose() << std::endl; + + std::cout << "VCOGdot_ideal = " << m_COG_ideal.Vdot.transpose() << std::endl; + std::cout << "VCOGdot_cmp = " << m_COG_cmp.Vdot.transpose() << std::endl; +#endif +} + +} // namespace mc_planning diff --git a/tests/hmc/generator.h b/tests/hmc/generator.h new file mode 100644 index 0000000000..41e87bb7cd --- /dev/null +++ b/tests/hmc/generator.h @@ -0,0 +1,135 @@ +#pragma once +// #include +// #include +// #include +#include +#include +#include +#include + +namespace mc_planning +{ +class generator +{ +private: + motion_interpolator::InterpolatorBase * m_ComInterp; + + StatePVA m_COG_ideal_pre, m_COG_ideal, m_COG_cmp; + StatePVA m_COG_out; + + Eigen::Vector3d m_Pcalpha_ideal_pre, m_Pcalpha_ideal, m_Pcalpha_cmp; + Eigen::Vector3d m_Vcalpha_ideal_pre, m_Vcalpha_ideal, m_Vcalpha_cmp; + Eigen::Vector3d m_Pcalpha_out; + Eigen::Vector3d m_Pcalpha_motion_out; + + int m_n_preview; + int m_n_steps; + double m_dt; + double m_omega_valpha; + double m_mass; + + linear_control_system::LinearTimeVariantInvertedPendulum m_ipm_long[2]; + linear_control_system::LIPMControlByPoleAssignWithExternalForce m_ipm_short[2]; + Eigen::Vector3d m_poles[2]; + + Eigen::VectorXd m_virtual_height[2]; + Eigen::VectorXd m_cog_height; + Eigen::VectorXd m_cog_dot_height; + Eigen::VectorXd m_cog_ddot_height; + std::vector m_steps; + + void setupCOGHeight(int n_current); + + void setupTimeTrajectories(int n_current); + + void generateTrajectories(void); + +public: + const Eigen::Vector3d & IdealCOGPosition(void) const + { + return m_COG_ideal.P; + } + const Eigen::Vector3d & IdealCOGVelocity(void) const + { + return m_COG_ideal.V; + } + const Eigen::Vector3d & IdealCOGAcceleration(void) const + { + return m_COG_ideal.Vdot; + } + + const Eigen::Vector3d & CompensatedCOGPosition(void) const + { + return m_COG_cmp.P; + } + const Eigen::Vector3d & CompensatedCOGVelocity(void) const + { + return m_COG_cmp.V; + } + const Eigen::Vector3d & CompensatedCOGAcceleration(void) const + { + return m_COG_cmp.Vdot; + } + + const Eigen::Vector3d & OutputCOGPosition(void) const + { + return m_COG_out.P; + } + const Eigen::Vector3d & OutputCOGVelocity(void) const + { + return m_COG_out.V; + } + const Eigen::Vector3d & OutputCOGAcceleration(void) const + { + return m_COG_out.Vdot; + } + + const Eigen::Vector3d & IdealZMPPosition(void) const + { + return m_Pcalpha_ideal; + } + const Eigen::Vector3d & IdealZMPVelocity(void) const + { + return m_Vcalpha_ideal; + } + + const Eigen::Vector3d & CompensatedZMPPosition(void) const + { + return m_Pcalpha_cmp; + } + const Eigen::Vector3d & CompensatedZMPVelocity(void) const + { + return m_Vcalpha_cmp; + } + + const Eigen::Vector3d & OutputZMPPosition(void) const + { + return m_Pcalpha_out; + } + + const std::vector & Steps(void) const + { + return m_steps; + } + void setStesps(const std::vector & steps) + { + m_steps = steps; + } + + void push_back(double time, double com_x, double com_y) + { + m_steps.push_back(Eigen::Vector3d(time, com_x, com_y)); + } + void push_back(const Eigen::Vector3d & step) + { + m_steps.push_back(step); + } + + generator(int n_preview, double dt); + + ~generator(); + + void generate(int n_time); +}; + +} // namespace mc_planning diff --git a/tests/hmc/testCoMGenerator.cpp b/tests/hmc/testCoMGenerator.cpp new file mode 100644 index 0000000000..7738e5dc61 --- /dev/null +++ b/tests/hmc/testCoMGenerator.cpp @@ -0,0 +1,81 @@ +#include "generator.h" +#include +#include +#include +#include + +using namespace mc_planning; +using Vector3 = Eigen::Vector3d; + +const double dt = 0.005; +const int n_preview = lround(1.6 / dt); + +static const int X = 0; +static const int Y = 1; +static const int Z = 2; + +int main(void) +{ + + generator com_traj(n_preview, dt); + + com_traj.push_back(Vector3((double)n_preview * dt, -0.2, 0.0)); + com_traj.push_back(com_traj.Steps().back() + Vector3((double)n_preview * dt, 0.0, 0.0)); + com_traj.push_back(com_traj.Steps().back() + Vector3(0.1, 0.0, 0.0)); + com_traj.push_back(com_traj.Steps().back() + Vector3(1.6, 0.0, 0.0)); + com_traj.push_back(com_traj.Steps().back() + Vector3(0.1, 0.2, 0.095)); + com_traj.push_back(com_traj.Steps().back() + Vector3(1.6, 0.0, 0.0)); + com_traj.push_back(com_traj.Steps().back() + Vector3(0.1, 0.0, -0.19)); + com_traj.push_back(com_traj.Steps().back() + Vector3(1.6, 0.0, 0.0)); + com_traj.push_back(com_traj.Steps().back() + Vector3(0.1, -0.2, 0.095)); + com_traj.push_back(com_traj.Steps().back() + Vector3((double)n_preview * dt, 0.0, 0.0)); + com_traj.push_back(com_traj.Steps().back() + Vector3((double)n_preview * dt, 0.0, 0.0)); + + int n_loop = lround(com_traj.Steps().back()(0) / dt) - n_preview; + std::vector log_com_ideal(n_loop + 1, Vector3::Zero()); + std::vector log_com_cmp(n_loop + 1, Vector3::Zero()); + std::vector log_com_out(n_loop + 1, Vector3::Zero()); + std::vector log_zmp_ideal(n_loop + 1, Vector3::Zero()); + std::vector log_zmp_cmp(n_loop + 1, Vector3::Zero()); + std::vector log_zmp_out(n_loop + 1, Vector3::Zero()); + + struct timeval tv; + gettimeofday(&tv, NULL); + double t_start = tv.tv_sec + tv.tv_usec * 1.0e-6; + + int n_steps = 0; + for(int loop = 0; loop <= n_loop; loop++) + { + com_traj.generate(loop); + + log_com_ideal[loop] = com_traj.IdealCOGPosition(); + log_com_cmp[loop] = com_traj.CompensatedCOGPosition(); + log_com_out[loop] = com_traj.OutputCOGPosition(); + + log_zmp_ideal[loop] = com_traj.IdealZMPPosition(); + log_zmp_cmp[loop] = com_traj.CompensatedZMPPosition(); + log_zmp_out[loop] = com_traj.OutputZMPPosition(); + } + + gettimeofday(&tv, NULL); + double t_end = tv.tv_sec + tv.tv_usec * 1.0e-6; + std::cout << "calc time = " << t_end - t_start << std::endl; + std::cout << "ave. calc time = " << (t_end - t_start) / (double)n_loop << std::endl; + + std::ofstream ofs("test.dat"); + for(unsigned int k = 0; k < n_loop; k++) + { + ofs << (double)k * dt << " " << log_com_ideal[k](X) << " " << log_com_ideal[k](Y) << " " << log_com_ideal[k](Z) + << " " + << " " << log_com_cmp[k](X) << " " << log_com_cmp[k](Y) << " " << log_com_cmp[k](Z) << " " + << " " << log_com_out[k](X) << " " << log_com_out[k](Y) << " " << log_com_out[k](Z) << " " + << " " << log_zmp_ideal[k](X) << " " << log_zmp_ideal[k](Y) << " " << log_zmp_ideal[k](Z) << " " + << " " << log_zmp_cmp[k](X) << " " << log_zmp_cmp[k](Y) << " " << log_zmp_cmp[k](Z) << " " + << " " << log_zmp_out[k](X) << " " << log_zmp_out[k](Y) << " " << log_zmp_out[k](Z) << " " << std::endl; + } + ofs.close(); + + std::cout << "end of com trajectory generation" << std::endl; + + return 0; +} From b843e146ea720d4f9fe79aa094c627e93033d257 Mon Sep 17 00:00:00 2001 From: Arnaud TANGUY Date: Mon, 15 Jun 2020 17:16:25 +0900 Subject: [PATCH 06/43] [utils] Add utility to measure elapsed time of a function --- include/mc_rtc/time_utils.h | 58 +++++++++++++++++++++++++++++++++++++ 1 file changed, 58 insertions(+) create mode 100644 include/mc_rtc/time_utils.h diff --git a/include/mc_rtc/time_utils.h b/include/mc_rtc/time_utils.h new file mode 100644 index 0000000000..30dfd51702 --- /dev/null +++ b/include/mc_rtc/time_utils.h @@ -0,0 +1,58 @@ +/* + * Copyright 2015-2020 CNRS-UM LIRMM, CNRS-AIST JRL + */ + +#pragma once +#include + +namespace mc_rtc +{ +/** + * @ class measure + * @ brief Class to measure the execution time of a callable + */ +template, class ClockT = std::chrono::system_clock> +struct measure +{ + /** + * @brief Returns the quantity (count) of the elapsed time as TimeT units + */ + template + /** + * @brief Returns the execution time of the provide function + * + * @param func Function to evaluate + * @param args Arguments to the function + * + * @return Time elapsed (in the provided unit) + */ + static typename TimeT::rep execution(F && func, Args &&... args) + { + auto start = ClockT::now(); + func(std::forward(args)...); + auto duration = std::chrono::duration_cast(ClockT::now() - start); + return duration.count(); + } + + /** + * @brief Returns the exectution time of the provided function (in chrono + * type system) + * + * @param func Function to evaluate + * @param args Arguments to the function + * + * @return Time elapsed + */ + template + static TimeT duration(F && func, Args &&... args) + { + auto start = ClockT::now(); + func(std::forward(args)...); + return std::chrono::duration_cast(ClockT::now() - start); + } +}; +using measure_s = measure>; +using measure_ms = measure>; +using measure_ns = measure>; + +} // namespace mc_rtc From 136e1482a097fa32d8e57657d572f140a3f82f8d Mon Sep 17 00:00:00 2001 From: Arnaud TANGUY Date: Mon, 15 Jun 2020 17:47:15 +0900 Subject: [PATCH 07/43] [HMC/test] Use mc_rtc logging --- tests/hmc/generator.cpp | 17 +----- tests/hmc/generator.h | 6 +- tests/hmc/testCoMGenerator.cpp | 104 +++++++++++++-------------------- 3 files changed, 46 insertions(+), 81 deletions(-) diff --git a/tests/hmc/generator.cpp b/tests/hmc/generator.cpp index f5a6a97a3b..868b80ac1b 100644 --- a/tests/hmc/generator.cpp +++ b/tests/hmc/generator.cpp @@ -7,7 +7,7 @@ using namespace mc_planning::motion_interpolator; -const double waist_height = 0.8; +constexpr double waist_height = 0.8; constexpr int X = 0; constexpr int Y = 1; constexpr int Z = 2; @@ -22,8 +22,7 @@ generator::generator(int n_preview, double dt) m_Pcalpha_out(Eigen::Vector3d::Zero()), m_Pcalpha_motion_out(Eigen::Vector3d::Zero()), m_n_preview(n_preview), m_n_steps(0), m_dt(dt), m_omega_valpha(0.0), m_mass(60.0) { - // setInterpolator(m_ComInterp, "clamped-cubic-spline", m_dt); - m_ComInterp = new ClampedCubicSpline(1.0, m_dt / 2); + m_ComInterp = std::make_shared>(1.0, m_dt / 2); m_ipm_long[X].Initialize(m_dt, m_n_preview, 20000); m_ipm_long[Y].Initialize(m_dt, m_n_preview, 20000); @@ -45,20 +44,8 @@ generator::generator(int n_preview, double dt) m_COG_ideal.P << 0.0, 0.0, waist_height; } -generator::~generator() -{ - delete m_ComInterp; -} - void generator::setupCOGHeight(int n_current) { -#if 0 - for( int i = 0 ; i <= 25; i++ ){ - double t = (double)i * 36.0 * m_dt; - double waist = 0.2 * sin(2.0 * M_PI * t) + waist_height; - m_ComInterp->push_back(i * 40 + 620, waist); - } -#endif if(n_current == 0) { m_ComInterp->push_back(0.0, waist_height); diff --git a/tests/hmc/generator.h b/tests/hmc/generator.h index 41e87bb7cd..8a17195793 100644 --- a/tests/hmc/generator.h +++ b/tests/hmc/generator.h @@ -12,7 +12,7 @@ namespace mc_planning class generator { private: - motion_interpolator::InterpolatorBase * m_ComInterp; + std::shared_ptr> m_ComInterp; StatePVA m_COG_ideal_pre, m_COG_ideal, m_COG_cmp; StatePVA m_COG_out; @@ -36,7 +36,7 @@ class generator Eigen::VectorXd m_cog_height; Eigen::VectorXd m_cog_dot_height; Eigen::VectorXd m_cog_ddot_height; - std::vector m_steps; + std::vector m_steps; ///< (time, com_x, com_y) void setupCOGHeight(int n_current); @@ -127,8 +127,6 @@ class generator generator(int n_preview, double dt); - ~generator(); - void generate(int n_time); }; diff --git a/tests/hmc/testCoMGenerator.cpp b/tests/hmc/testCoMGenerator.cpp index 7738e5dc61..71f86faaf3 100644 --- a/tests/hmc/testCoMGenerator.cpp +++ b/tests/hmc/testCoMGenerator.cpp @@ -1,81 +1,61 @@ +#include +#include #include "generator.h" -#include -#include -#include -#include using namespace mc_planning; using Vector3 = Eigen::Vector3d; -const double dt = 0.005; +constexpr double dt = 0.005; const int n_preview = lround(1.6 / dt); -static const int X = 0; -static const int Y = 1; -static const int Z = 2; +constexpr int X = 0; +constexpr int Y = 1; +constexpr int Z = 2; int main(void) { generator com_traj(n_preview, dt); + // Trajectory of size 2*n_preview+1 + auto steps = com_traj.Steps(); com_traj.push_back(Vector3((double)n_preview * dt, -0.2, 0.0)); - com_traj.push_back(com_traj.Steps().back() + Vector3((double)n_preview * dt, 0.0, 0.0)); - com_traj.push_back(com_traj.Steps().back() + Vector3(0.1, 0.0, 0.0)); - com_traj.push_back(com_traj.Steps().back() + Vector3(1.6, 0.0, 0.0)); - com_traj.push_back(com_traj.Steps().back() + Vector3(0.1, 0.2, 0.095)); - com_traj.push_back(com_traj.Steps().back() + Vector3(1.6, 0.0, 0.0)); - com_traj.push_back(com_traj.Steps().back() + Vector3(0.1, 0.0, -0.19)); - com_traj.push_back(com_traj.Steps().back() + Vector3(1.6, 0.0, 0.0)); - com_traj.push_back(com_traj.Steps().back() + Vector3(0.1, -0.2, 0.095)); - com_traj.push_back(com_traj.Steps().back() + Vector3((double)n_preview * dt, 0.0, 0.0)); - com_traj.push_back(com_traj.Steps().back() + Vector3((double)n_preview * dt, 0.0, 0.0)); + com_traj.push_back(steps.back() + Vector3{(double)n_preview * dt, 0.0, 0.0}); + com_traj.push_back(steps.back() + Vector3{0.1, 0.0, 0.0}); + com_traj.push_back(steps.back() + Vector3{1.6, 0.0, 0.0}); + com_traj.push_back(steps.back() + Vector3{0.1, 0.2, 0.095}); + com_traj.push_back(steps.back() + Vector3{1.6, 0.0, 0.0}); + com_traj.push_back(steps.back() + Vector3{0.1, 0.0, -0.19}); + com_traj.push_back(steps.back() + Vector3{1.6, 0.0, 0.0}); + com_traj.push_back(steps.back() + Vector3{0.1, -0.2, 0.095}); + com_traj.push_back(steps.back() + Vector3{(double)n_preview * dt, 0.0, 0.0}); + com_traj.push_back(steps.back() + Vector3{(double)n_preview * dt, 0.0, 0.0}); + + mc_rtc::Logger logger(mc_rtc::Logger::Policy::NON_THREADED, "/tmp", "mc_rtc-test"); + logger.addLogEntry("IdealCOGPosition", [&com_traj]() { return com_traj.IdealCOGPosition(); }); + logger.addLogEntry("CompensatedCOGPosition", [&com_traj]() { return com_traj.CompensatedCOGPosition(); }); + logger.addLogEntry("OutputCOGPosition", [&com_traj]() { return com_traj.OutputCOGPosition(); }); + logger.addLogEntry("IdealZMPPosition", [&com_traj]() { return com_traj.IdealZMPPosition(); }); + logger.addLogEntry("CompensatedZMPPosition", [&com_traj]() { return com_traj.CompensatedZMPPosition(); }); + logger.addLogEntry("OutputZMPPosition", [&com_traj]() { return com_traj.OutputZMPPosition(); }); + logger.start("CoMGenerator", dt); int n_loop = lround(com_traj.Steps().back()(0) / dt) - n_preview; - std::vector log_com_ideal(n_loop + 1, Vector3::Zero()); - std::vector log_com_cmp(n_loop + 1, Vector3::Zero()); - std::vector log_com_out(n_loop + 1, Vector3::Zero()); - std::vector log_zmp_ideal(n_loop + 1, Vector3::Zero()); - std::vector log_zmp_cmp(n_loop + 1, Vector3::Zero()); - std::vector log_zmp_out(n_loop + 1, Vector3::Zero()); - - struct timeval tv; - gettimeofday(&tv, NULL); - double t_start = tv.tv_sec + tv.tv_usec * 1.0e-6; - - int n_steps = 0; - for(int loop = 0; loop <= n_loop; loop++) - { - com_traj.generate(loop); - - log_com_ideal[loop] = com_traj.IdealCOGPosition(); - log_com_cmp[loop] = com_traj.CompensatedCOGPosition(); - log_com_out[loop] = com_traj.OutputCOGPosition(); - - log_zmp_ideal[loop] = com_traj.IdealZMPPosition(); - log_zmp_cmp[loop] = com_traj.CompensatedZMPPosition(); - log_zmp_out[loop] = com_traj.OutputZMPPosition(); - } - - gettimeofday(&tv, NULL); - double t_end = tv.tv_sec + tv.tv_usec * 1.0e-6; - std::cout << "calc time = " << t_end - t_start << std::endl; - std::cout << "ave. calc time = " << (t_end - t_start) / (double)n_loop << std::endl; - - std::ofstream ofs("test.dat"); - for(unsigned int k = 0; k < n_loop; k++) - { - ofs << (double)k * dt << " " << log_com_ideal[k](X) << " " << log_com_ideal[k](Y) << " " << log_com_ideal[k](Z) - << " " - << " " << log_com_cmp[k](X) << " " << log_com_cmp[k](Y) << " " << log_com_cmp[k](Z) << " " - << " " << log_com_out[k](X) << " " << log_com_out[k](Y) << " " << log_com_out[k](Z) << " " - << " " << log_zmp_ideal[k](X) << " " << log_zmp_ideal[k](Y) << " " << log_zmp_ideal[k](Z) << " " - << " " << log_zmp_cmp[k](X) << " " << log_zmp_cmp[k](Y) << " " << log_zmp_cmp[k](Z) << " " - << " " << log_zmp_out[k](X) << " " << log_zmp_out[k](Y) << " " << log_zmp_out[k](Z) << " " << std::endl; - } - ofs.close(); - - std::cout << "end of com trajectory generation" << std::endl; + /* + * Generate the CoM trajectory based on the parameters in com_traj + */ + auto comGeneration = [&]() { + for(int loop = 0; loop <= n_loop; loop++) + { + com_traj.generate(loop); + logger.log(); + } + }; + + auto duration = mc_rtc::measure_ms::duration(comGeneration).count(); + mc_rtc::log::info("calc time = {} (ms)", duration); + mc_rtc::log::info("ave. calc time = {} (ms)", duration / static_cast(n_loop)); + mc_rtc::log::success("end of com trajectory generation"); return 0; } From 7020de9bb4d120e998471b2c079fb81cb9402d07 Mon Sep 17 00:00:00 2001 From: Arnaud TANGUY Date: Mon, 15 Jun 2020 21:14:54 +0900 Subject: [PATCH 08/43] [LookupTable] Fix debug assert --- include/mc_planning/LookupTable.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/include/mc_planning/LookupTable.h b/include/mc_planning/LookupTable.h index 2447976b93..8491f7e85a 100644 --- a/include/mc_planning/LookupTable.h +++ b/include/mc_planning/LookupTable.h @@ -95,7 +95,7 @@ struct MC_PLANNING_DLLAPI LookupTable } else { - assert(x < min_ && x > max_); + assert(x < min_ || x > max_); } auto h = std::min(static_cast(lround((x - min_) * rangeConversion_)), maxIndex_); return table_[h]; From d69affc605f3df323098d42351beab6bc3bac485 Mon Sep 17 00:00:00 2001 From: Arnaud TANGUY Date: Tue, 16 Jun 2020 09:22:20 +0900 Subject: [PATCH 09/43] [time_utils] Always pick a steady clock --- include/mc_rtc/time_utils.h | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/include/mc_rtc/time_utils.h b/include/mc_rtc/time_utils.h index 30dfd51702..bdd20b4bb3 100644 --- a/include/mc_rtc/time_utils.h +++ b/include/mc_rtc/time_utils.h @@ -11,7 +11,10 @@ namespace mc_rtc * @ class measure * @ brief Class to measure the execution time of a callable */ -template, class ClockT = std::chrono::system_clock> +template, + class ClockT = typename std::conditional::type> struct measure { /** From 5d5163eb7547061fc2fbcec5c65b8380892189fa Mon Sep 17 00:00:00 2001 From: Arnaud TANGUY Date: Tue, 16 Jun 2020 11:39:10 +0900 Subject: [PATCH 10/43] [HMC/LTVIP] Use LookupTable implementation Replace the old confusing and inaccurate implementation with the new generic LookupTable implementation: ``` [info] Omega2 = 12.258312499999999 [info] Original omega = 3.498714049475893 [info] New omega = 3.5033993502025997 [info] Actual omega = 3.5011872986174275 [info] Original cosh = 1.0003195295150686 [info] New cosh = 1.0001534265107093 [info] Actual cosh = 1.0001532328194729 [info] Original sinh = 0.02528163620591862 (previous code) [info] New sinh = 0.01751789259907999 (new lookup table implementation) [info] Actual sinh = 0.017506830645288594 (expected value) ``` Note better accuracy could be achieved using a non-uniform distribution. --- .../LinearTimeVariantInvertedPendulum.h | 74 ++++----- include/mc_planning/LookupTable.h | 25 ++- .../LinearTimeVariantInvertedPendulum.cpp | 146 +++++++++--------- tests/hmc/generator.h | 32 ++-- tests/hmc/testCoMGenerator.cpp | 25 +-- tests/test_hmc.cpp | 4 +- 6 files changed, 156 insertions(+), 150 deletions(-) diff --git a/include/mc_planning/LinearTimeVariantInvertedPendulum.h b/include/mc_planning/LinearTimeVariantInvertedPendulum.h index cdd4e84ab5..7b917cfcad 100644 --- a/include/mc_planning/LinearTimeVariantInvertedPendulum.h +++ b/include/mc_planning/LinearTimeVariantInvertedPendulum.h @@ -103,23 +103,39 @@ struct MC_PLANNING_DLLAPI LinearTimeVariantInvertedPendulum * @param dt Timestep * @param n_preview Number of future preview elements. The full size of the * preview windows from past to future will be (2*n_preview+1) + * @param minHeight for the pendulum (used for LookupTable optimizations) + * @param maxHeight for the pendulum * @param weight_resolution Number of data points to pre-compute for - * \f$ \omega, cosh(\omega), sinh(\omega) \f$ for height in a given range. - * This is used to optimize the otherwise costly computation of these + * \f$ \omega, cosh(\omega), sinh(\omega) \f$ for height in a given range + * [minHeight, maxHeight]. This is used to optimize the otherwise costly computation of these * constants. */ - LinearTimeVariantInvertedPendulum(double dt, unsigned n_preview = 0, unsigned weight_resolution = 20000); + LinearTimeVariantInvertedPendulum(double dt, + unsigned n_preview = 0, + unsigned weight_resolution = 20000, + double minHeight = 0.01, + double maxHeight = 2.5); virtual ~LinearTimeVariantInvertedPendulum(); - void Initialize(double dt, unsigned n_preview = 0, unsigned weight_resolution = 20000); + void Initialize(double dt, + unsigned n_preview = 0, + unsigned weight_resolution = 20000, + double minHeight = 0.01, + double maxHeight = 2.5); - /** XXX What does it initialize exactly? - * @param waist_height + /** + * @brief Initialize the discretized system matrices with constant pendulum height +Pre-computed table from . + + * + * Initalizes \f$ A_k \f$, \f$ B_k \f$ + * + * @param waist_height Pendulum height (constant over the preview window) */ void initMatrices(double waist_height); /** - * @brief Computes the reference CoM state (position and velocity) + * @brief Computes the reference CoM state online (position and velocity) * * Fills m_A, m_B, m_An, m_Bn, m_X * Requires p_ref() and w2() to be filled first. @@ -153,7 +169,8 @@ struct MC_PLANNING_DLLAPI LinearTimeVariantInvertedPendulum */ void getState(int n_time, double & cog_pos, double & cog_vel, double & cog_acc, double & p, double & pdot); - /** XXX Unused by MultiContact COG generation + /** + * Generate full trajectory (only used for tests) */ void generate(Eigen::VectorXd & cog_pos, Eigen::VectorXd & cog_vel, @@ -209,46 +226,14 @@ struct MC_PLANNING_DLLAPI LinearTimeVariantInvertedPendulum return m_w[k]; } -private: - /** Fill m_A - * - * XXX with what? unclear... - * - * @param waist_height - */ - inline void init_m22(double waist_height); - /** Fill m_B - * - * XXX with what? unclear... - * - * @param waist_height - */ - inline void init_v2(double waist_height); - protected: using Matrix22 = Eigen::Matrix; using Vector2 = Eigen::Matrix; double m_dt; ///< Timstep - double m_dh; unsigned m_n_current; ///< Index of current time (center of the preview window) unsigned m_n_preview2; ///< Preview window size \f$ (2*\mbox{m_n_current})+1 \f$ - /** @name Computation optimizations - * These values are pre-computed to speed-up computations. - * - * FIXME this makes the code hard to read, and too specialized for HRP use-case. - * Values are pre-computed assuming the typical range of heights likely for HRP - * robots. - * There is no check to see if we get outside of this range, this could break - * down for other use-cases - */ - ///@{ - std::vector m_wk; ///< \f$ \omega_k \f$ - std::vector m_shk; ///< \f$ sinh(\omega_k * dt) \f$ - std::vector m_chk; ///< \f$ cosh(\omega_k * dt) \f$ - ///@} - boost::circular_buffer> m_A; ///< \f$ A_k \f$ boost::circular_buffer> m_B; ///< \f$ B_k \f$ std::vector> m_An; ///< \f$ \Phi(k, j) \f$ @@ -260,6 +245,15 @@ struct MC_PLANNING_DLLAPI LinearTimeVariantInvertedPendulum Eigen::VectorXd m_w2; ///< \f$ \omega_k^2 \f$ Eigen::VectorXd m_w; ///< \f$ \omega \f$ + + /** + * @name Lookup tables for optimized computation of cosh, sinh and sqrt + **/ + ///@{ + LookupTable wTable_; ///< Pre-computed table from \f$ \omega^2 \rightarrow sqrt(\omega^2) \f$ + LookupTable chkTable_; ///< Pre-computed table from \f$ \omega^2 \rightarrow cosh(sqrt(\omega^2) * dt) \f$ + LookupTable shkTable_; ///< Pre-computed table from \f$ \omega^2 \rightarrow sinh(sqrt(\omega^2) * dt) \f$ + ///@} }; } // namespace linear_control_system diff --git a/include/mc_planning/LookupTable.h b/include/mc_planning/LookupTable.h index 8491f7e85a..86f6412b28 100644 --- a/include/mc_planning/LookupTable.h +++ b/include/mc_planning/LookupTable.h @@ -37,6 +37,23 @@ namespace mc_planning template struct MC_PLANNING_DLLAPI LookupTable { + /** + * @brief Evaluate and store given function results + * @see create(size_t resolution, const T & min, const T & max, MappingFunction f) + */ + template + LookupTable(size_t resolution, const T & min, const T & max, MappingFunction f) + { + create(resolution, min, max, f); + } + + /** + * @brief Default constructor + * + * To initialize the lookup table, call create(size_t resolution, const T & min, const T & max, MappingFunction f) + */ + LookupTable(){}; + /** * @brief Evaluate and store given function results * @@ -48,10 +65,9 @@ struct MC_PLANNING_DLLAPI LookupTable * @param f Function to evaluate */ template - LookupTable(size_t resolution, const T & min, const T & max, MappingFunction f) - : table_(resolution), min_(min), max_(max) + void create(size_t resolution, const T & min, const T & max, MappingFunction f) { - if(min_ > max_) + if(min > max) { mc_rtc::log::error_and_throw("[LookupTable] Invalid range (min {} > max {})", min, max); } @@ -60,6 +76,9 @@ struct MC_PLANNING_DLLAPI LookupTable mc_rtc::log::error_and_throw( "[LookupTable] Resolution cannot be equal to zero (strictly positive)"); } + table_.resize(resolution); + min_ = min; + max_ = max; T oldrange = static_cast(resolution); T newrange = (max - min); T fracRange = newrange / oldrange; diff --git a/src/mc_planning/LinearTimeVariantInvertedPendulum.cpp b/src/mc_planning/LinearTimeVariantInvertedPendulum.cpp index 5d95839e6d..91a01d2567 100644 --- a/src/mc_planning/LinearTimeVariantInvertedPendulum.cpp +++ b/src/mc_planning/LinearTimeVariantInvertedPendulum.cpp @@ -14,13 +14,26 @@ LinearTimeVariantInvertedPendulum::LinearTimeVariantInvertedPendulum() {} LinearTimeVariantInvertedPendulum::LinearTimeVariantInvertedPendulum(double dt, unsigned n_preview, - unsigned weight_resolution) + unsigned weight_resolution, + double minHeight, + double maxHeight) { - Initialize(dt, n_preview, weight_resolution); + Initialize(dt, n_preview, weight_resolution, minHeight, maxHeight); } -void LinearTimeVariantInvertedPendulum::Initialize(double dt, unsigned n_preview, unsigned weight_resolution) +void LinearTimeVariantInvertedPendulum::Initialize(double dt, + unsigned n_preview, + unsigned weight_resolution, + double minHeight, + double maxHeight) { + if(weight_resolution == 0) + { + mc_rtc::log::error_and_throw("[LinearTimeVariantInvertedPendulum::Initialize] Invalid " + "weight resolution {} (must be >0, recommended value 20000) ", + weight_resolution); + } + m_dt = dt; m_n_current = n_preview; if(n_preview == 0) @@ -29,32 +42,16 @@ void LinearTimeVariantInvertedPendulum::Initialize(double dt, unsigned n_preview } m_n_preview2 = n_preview * 2 + 1; - if(weight_resolution == 0) - { - weight_resolution = 20000; - mc_rtc::log::warning("[LinearTimeVariantInvertedPendulum::Initialize] Invalid weight resolution, using {} instead", - weight_resolution); - } - m_wk.resize(weight_resolution); - m_shk.resize(weight_resolution); - m_chk.resize(weight_resolution); - - /// XXX why divided by 800? - // 25 - m_dh = static_cast(weight_resolution) / 800.0; - for(unsigned i = 0; i < weight_resolution; i++) - { - // Pendulum is h = w2/g - // Precomputes values from height between - // h min = 0.001 / G ~= 0.0001m - // to - // h max = (0.001+20000/800)/G = 2.5m - // FIXME Max height depends on weight resolution! - double w2 = 0.001 + (double)i / m_dh; - m_wk[i] = sqrt(w2); - m_shk[i] = sinh(m_wk[i] * dt); - m_chk[i] = cosh(m_wk[i] * dt); - } + /** Create lookup tables for sqrt(w2), cosh(w*dt), sinh(w*dt) */ + auto omega2 = [](double h) { return mc_rtc::constants::GRAVITY / h; }; + auto omega = [](double w2) { return std::sqrt(w2); }; + auto chk = [this](double w2) { return cosh(wTable_(w2) * m_dt); }; + auto shk = [this](double w2) { return sinh(wTable_(w2) * m_dt); }; + double minOmega2 = omega2(maxHeight); + double maxOmega2 = omega2(minHeight); + wTable_.create(weight_resolution, minOmega2, maxOmega2, omega); + chkTable_.create(weight_resolution, minOmega2, maxOmega2, chk); + shkTable_.create(weight_resolution, minOmega2, maxOmega2, shk); m_A.resize(m_n_preview2, Matrix22::Identity()); m_An.resize(m_n_preview2, Matrix22::Identity()); @@ -69,52 +66,48 @@ void LinearTimeVariantInvertedPendulum::Initialize(double dt, unsigned n_preview LinearTimeVariantInvertedPendulum::~LinearTimeVariantInvertedPendulum() {} -void LinearTimeVariantInvertedPendulum::init_m22(double waist_height) -{ - // XXX why this index? - unsigned h = static_cast(lround(cst::GRAVITY * m_dh / waist_height)); - - // A_k - Matrix22 m; - m << m_chk[h], m_shk[h] / m_wk[h], m_wk[h] * m_shk[h], m_chk[h]; - - for(unsigned i = 0; i < m_n_preview2; i++) - { - m_A.push_front(m); - } -} - -void LinearTimeVariantInvertedPendulum::init_v2(double waist_height) -{ - // XXX why this indeThis is something x? - unsigned h = static_cast(lround(cst::GRAVITY * m_dh / waist_height)); - - Vector2 v; - v << 1 - m_chk[h], -m_wk[h] * m_shk[h]; - - for(unsigned i = 0; i < m_n_preview2; i++) - { - m_B.push_front(v); - } -} - void LinearTimeVariantInvertedPendulum::initMatrices(double waist_height) { - init_m22(waist_height); - init_v2(waist_height); + const double w2 = cst::GRAVITY / waist_height; + + // Initialize A matrix corresponding to the given pendulum height + const double w = wTable_(w2); + const double ch = chkTable_(w2); + const double sh = shkTable_(w2); + + auto init_m22 = [&]() { + Eigen::Matrix2d A; + A << ch, sh / w, w * sh, ch; + + // Initialize the whole preview window at constant height + for(unsigned i = 0; i < m_n_preview2; i++) + { + m_A.push_front(A); + } + }; + + auto init_v2 = [&]() { + Vector2 v; + v << 1 - ch, -w * sh; + + for(unsigned i = 0; i < m_n_preview2; i++) + { + m_B.push_front(v); + } + }; + + init_m22(); + init_v2(); } void LinearTimeVariantInvertedPendulum::update() { // set newest matrices { - // XXX - // What does this index represent? - // Why? - unsigned h = static_cast(lround(m_w2[m_n_preview2 - 1] * m_dh)); - const double & w = m_wk[h]; - const double & ch = m_chk[h]; - const double & sh = m_shk[h]; + double w2 = m_w2[m_n_preview2 - 1]; + const double w = wTable_(w2); + const double ch = chkTable_(w2); + const double sh = shkTable_(w2); m_w[m_n_preview2 - 1] = w; Matrix22 Anew; Anew << ch, sh / w, w * sh, ch; @@ -126,17 +119,16 @@ void LinearTimeVariantInvertedPendulum::update() for(unsigned i = 0; i < m_n_current; i++) { - unsigned h = static_cast(lround(m_w2[i] * m_dh)); - m_w[i] = m_wk[h]; + m_w[i] = wTable_(m_w2[i]); } // update future matrices for(unsigned i = m_n_current; i < m_n_preview2 - 1; i++) { - auto h = static_cast(lround(m_w2[i] * m_dh)); - const double & w = m_wk[h]; - const double & ch = m_chk[h]; - const double & sh = m_shk[h]; + const double w2 = m_w2[i]; + const double w = wTable_(w2); + const double ch = chkTable_(w2); + const double sh = shkTable_(w2); m_w[i] = w; m_A[i] << ch, sh / w, w * sh, ch; m_B[i] << 1 - ch, -w * sh; @@ -173,10 +165,10 @@ void LinearTimeVariantInvertedPendulum::generate(Eigen::VectorXd & cog_pos, // update future matrices for(unsigned i = 0; i < m_n_preview2; i++) { - auto h = static_cast(lround(m_w2[i] * m_dh)); - const double & w = m_wk[h]; - const double & ch = m_chk[h]; - const double & sh = m_shk[h]; + const double w2 = m_w2[i]; + const double w = wTable_(w2); + const double ch = chkTable_(w2); + const double sh = shkTable_(w2); m_w[i] = w; m_A[i] << ch, sh / w, w * sh, ch; m_B[i] << 1 - ch, -w * sh; diff --git a/tests/hmc/generator.h b/tests/hmc/generator.h index 8a17195793..f6e6d24254 100644 --- a/tests/hmc/generator.h +++ b/tests/hmc/generator.h @@ -42,72 +42,72 @@ class generator void setupTimeTrajectories(int n_current); - void generateTrajectories(void); + void generateTrajectories(); public: - const Eigen::Vector3d & IdealCOGPosition(void) const + const Eigen::Vector3d & IdealCOGPosition() const { return m_COG_ideal.P; } - const Eigen::Vector3d & IdealCOGVelocity(void) const + const Eigen::Vector3d & IdealCOGVelocity() const { return m_COG_ideal.V; } - const Eigen::Vector3d & IdealCOGAcceleration(void) const + const Eigen::Vector3d & IdealCOGAcceleration() const { return m_COG_ideal.Vdot; } - const Eigen::Vector3d & CompensatedCOGPosition(void) const + const Eigen::Vector3d & CompensatedCOGPosition() const { return m_COG_cmp.P; } - const Eigen::Vector3d & CompensatedCOGVelocity(void) const + const Eigen::Vector3d & CompensatedCOGVelocity() const { return m_COG_cmp.V; } - const Eigen::Vector3d & CompensatedCOGAcceleration(void) const + const Eigen::Vector3d & CompensatedCOGAcceleration() const { return m_COG_cmp.Vdot; } - const Eigen::Vector3d & OutputCOGPosition(void) const + const Eigen::Vector3d & OutputCOGPosition() const { return m_COG_out.P; } - const Eigen::Vector3d & OutputCOGVelocity(void) const + const Eigen::Vector3d & OutputCOGVelocity() const { return m_COG_out.V; } - const Eigen::Vector3d & OutputCOGAcceleration(void) const + const Eigen::Vector3d & OutputCOGAcceleration() const { return m_COG_out.Vdot; } - const Eigen::Vector3d & IdealZMPPosition(void) const + const Eigen::Vector3d & IdealZMPPosition() const { return m_Pcalpha_ideal; } - const Eigen::Vector3d & IdealZMPVelocity(void) const + const Eigen::Vector3d & IdealZMPVelocity() const { return m_Vcalpha_ideal; } - const Eigen::Vector3d & CompensatedZMPPosition(void) const + const Eigen::Vector3d & CompensatedZMPPosition() const { return m_Pcalpha_cmp; } - const Eigen::Vector3d & CompensatedZMPVelocity(void) const + const Eigen::Vector3d & CompensatedZMPVelocity() const { return m_Vcalpha_cmp; } - const Eigen::Vector3d & OutputZMPPosition(void) const + const Eigen::Vector3d & OutputZMPPosition() const { return m_Pcalpha_out; } - const std::vector & Steps(void) const + const std::vector & Steps() const { return m_steps; } diff --git a/tests/hmc/testCoMGenerator.cpp b/tests/hmc/testCoMGenerator.cpp index 71f86faaf3..56913ca601 100644 --- a/tests/hmc/testCoMGenerator.cpp +++ b/tests/hmc/testCoMGenerator.cpp @@ -18,18 +18,19 @@ int main(void) generator com_traj(n_preview, dt); // Trajectory of size 2*n_preview+1 - auto steps = com_traj.Steps(); - com_traj.push_back(Vector3((double)n_preview * dt, -0.2, 0.0)); - com_traj.push_back(steps.back() + Vector3{(double)n_preview * dt, 0.0, 0.0}); - com_traj.push_back(steps.back() + Vector3{0.1, 0.0, 0.0}); - com_traj.push_back(steps.back() + Vector3{1.6, 0.0, 0.0}); - com_traj.push_back(steps.back() + Vector3{0.1, 0.2, 0.095}); - com_traj.push_back(steps.back() + Vector3{1.6, 0.0, 0.0}); - com_traj.push_back(steps.back() + Vector3{0.1, 0.0, -0.19}); - com_traj.push_back(steps.back() + Vector3{1.6, 0.0, 0.0}); - com_traj.push_back(steps.back() + Vector3{0.1, -0.2, 0.095}); - com_traj.push_back(steps.back() + Vector3{(double)n_preview * dt, 0.0, 0.0}); - com_traj.push_back(steps.back() + Vector3{(double)n_preview * dt, 0.0, 0.0}); + std::vector steps; + steps.push_back(Vector3((double)n_preview * dt, -0.2, 0.0)); + steps.push_back(steps.back() + Vector3{(double)n_preview * dt, 0.0, 0.0}); + steps.push_back(steps.back() + Vector3{0.1, 0.0, 0.0}); + steps.push_back(steps.back() + Vector3{1.6, 0.0, 0.0}); + steps.push_back(steps.back() + Vector3{0.1, 0.2, 0.095}); + steps.push_back(steps.back() + Vector3{1.6, 0.0, 0.0}); + steps.push_back(steps.back() + Vector3{0.1, 0.0, -0.19}); + steps.push_back(steps.back() + Vector3{1.6, 0.0, 0.0}); + steps.push_back(steps.back() + Vector3{0.1, -0.2, 0.095}); + steps.push_back(steps.back() + Vector3{(double)n_preview * dt, 0.0, 0.0}); + steps.push_back(steps.back() + Vector3{(double)n_preview * dt, 0.0, 0.0}); + com_traj.setStesps(steps); mc_rtc::Logger logger(mc_rtc::Logger::Policy::NON_THREADED, "/tmp", "mc_rtc-test"); logger.addLogEntry("IdealCOGPosition", [&com_traj]() { return com_traj.IdealCOGPosition(); }); diff --git a/tests/test_hmc.cpp b/tests/test_hmc.cpp index a7a5a59f8e..d00b5392ce 100644 --- a/tests/test_hmc.cpp +++ b/tests/test_hmc.cpp @@ -36,8 +36,8 @@ BOOST_AUTO_TEST_CASE(TestLookupTable) // robot (from 0.001m to 3m height) auto omega = [](double h) { return std::sqrt(mc_rtc::constants::GRAVITY / h); }; double dt = 0.005; - testLookupTable([dt](double x) { return cosh(x) * dt; }, omega(2.5), omega(0.01), 100000, 100000); - testLookupTable([](double x) { return sinh(x); }, omega(2.5), omega(0.01), 100000, 100000); + testLookupTable([dt](double x) { return cosh(x * dt); }, omega(2.5), omega(0.01), 100000, 100000); + testLookupTable([dt](double x) { return sinh(x * dt); }, omega(2.5), omega(0.01), 100000, 100000); } { From ff9b4e016884a76ef0f8229916074c5deebda630 Mon Sep 17 00:00:00 2001 From: Arnaud TANGUY Date: Tue, 16 Jun 2020 11:54:39 +0900 Subject: [PATCH 11/43] [ci/tmp] Temporarely upload documentation of HMC branch --- .github/workflows/build.yml | 24 ++++++++++++++++++++++++ 1 file changed, 24 insertions(+) diff --git a/.github/workflows/build.yml b/.github/workflows/build.yml index b6d361e531..3e8a23a6ed 100644 --- a/.github/workflows/build.yml +++ b/.github/workflows/build.yml @@ -177,6 +177,30 @@ jobs: cd build cmake ../ && make -j2 && sudo make install || exit 1 popd + - name: Upload documentation (HMC fork) + # Only run on master branch and for one configuration + if: matrix.os == 'ubuntu-18.04' && matrix.build-type == 'RelWithDebInfo' && matrix.compiler == 'gcc' && github.ref == 'refs/heads/topic/hmc' && github.repository == 'arntanguy/mc_rtc' + run: | + set -x + pushd . + cd doc + cp -r /usr/local/share/doc/mc_rtc/doxygen-html . + find . -type f -print0 | xargs -0 sed -i -e's@/usr/local/share/doc/SpaceVecAlg/doxygen-html/@https://jrl-umi3218.github.io/SpaceVecAlg/doxygen/HEAD/@g' + find . -type f -print0 | xargs -0 sed -i -e's@/usr/local/share/doc/RBDyn/doxygen-html/@https://jrl-umi3218.github.io/RBDyn/doxygen/HEAD/@g' + find . -type f -print0 | xargs -0 sed -i -e's@/usr/local/share/doc/Tasks/doxygen-html/@https://jrl-umi3218.github.io/Tasks/doxygen/HEAD/@g' + find . -type f -print0 | xargs -0 sed -i -e's@/usr/local/share/doc/sch-core/doxygen-html/@https://jrl-umi3218.github.io/sch-core/doxygen/HEAD/@g' + find . -type f -print0 | xargs -0 sed -i -e's@/usr/local/share/doc/mc_rbdyn_urdf/doxygen-html/@https://jrl-umi3218.github.io/mc_rbdyn_urdf/doxygen/HEAD/@g' + sudo apt-get install -qq ruby-dev ruby-bundler libxml2-dev + bundle install --path vendor + git clone -b gh-pages https://arntanguy:${{ secrets.GH_PAGES_TOKEN }}@github.com/arntanguy/mc_rtc /tmp/website + bundle exec jekyll build --trace -b /mc_rtc -d /tmp/website + cd /tmp/website + git add . + git config --global user.email "arn.tanguy@gmail.com" + git config --global user.name "Arnaud TANGUY (Automated CI update)" + git commit -m "Website from commit ${GITHUB_SHA}" + git push origin gh-pages + popd - name: Upload documentation # Only run on master branch and for one configuration if: matrix.os == 'ubuntu-18.04' && matrix.build-type == 'RelWithDebInfo' && matrix.compiler == 'gcc' && github.ref == 'refs/heads/master' && github.repository == 'jrl-umi3218/mc_rtc' From 5485b13517bc9881d4eb6f71efd8bf58fff18509 Mon Sep 17 00:00:00 2001 From: Arnaud TANGUY Date: Fri, 19 Jun 2020 18:02:43 +0900 Subject: [PATCH 12/43] [CI/tmp] Disable slack notification for now --- .github/workflows/build.yml | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/.github/workflows/build.yml b/.github/workflows/build.yml index 3e8a23a6ed..fbe77af57c 100644 --- a/.github/workflows/build.yml +++ b/.github/workflows/build.yml @@ -225,11 +225,11 @@ jobs: git commit -m "Website from commit ${GITHUB_SHA}" git push origin gh-pages popd - - name: Slack Notification - if: failure() - uses: archive/github-actions-slack@master - with: - slack-bot-user-oauth-access-token: ${{ secrets.SLACK_BOT_TOKEN }} - slack-channel: '#ci' - slack-text: > - [mc_rtc] Build *${{ matrix.os }}/${{ matrix.build-type }} (${{ matrix.compiler }})* failed on ${{ github.ref }} + # - name: Slack Notification + # if: failure() + # uses: archive/github-actions-slack@master + # with: + # slack-bot-user-oauth-access-token: ${{ secrets.SLACK_BOT_TOKEN }} + # slack-channel: '#ci' + # slack-text: > + # [mc_rtc] Build *${{ matrix.os }}/${{ matrix.build-type }} (${{ matrix.compiler }})* failed on ${{ github.ref }} From 813d8c7a37f14f8af64c43946a8aa91efdd1cda3 Mon Sep 17 00:00:00 2001 From: Arnaud TANGUY Date: Tue, 16 Jun 2020 14:13:49 +0900 Subject: [PATCH 13/43] [HMC/bench] Use same cosh argument as in LTVIP --- benchmarks/benchHMC.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/benchmarks/benchHMC.cpp b/benchmarks/benchHMC.cpp index 87626c81b1..7dc05b1609 100644 --- a/benchmarks/benchHMC.cpp +++ b/benchmarks/benchHMC.cpp @@ -13,7 +13,7 @@ static void BM_cosh_lookuptable(benchmark::State & state) auto omega = [](double h) { return std::sqrt(mc_rtc::constants::GRAVITY / h); }; double dt = 0.005; auto table = - mc_planning::LookupTable(20000, omega(2.5), omega(0.01), [dt](double x) { return cosh(x) * dt; }); + mc_planning::LookupTable(20000, omega(2.5), omega(0.01), [dt](double x) { return cosh(x * dt); }); static std::random_device rd; static std::mt19937_64 gen(rd()); @@ -49,7 +49,7 @@ static void BM_cosh(benchmark::State & state) val = dis(gen); for(int i = 0; i < 10000; ++i) { - benchmark::DoNotOptimize(res = cosh(val) * dt); + benchmark::DoNotOptimize(res = cosh(val * dt)); } } state.SetItemsProcessed(10000 * state.iterations()); From 3b624eaa5a6029279d2ef99a1a2458ee6590a19c Mon Sep 17 00:00:00 2001 From: Arnaud TANGUY Date: Tue, 16 Jun 2020 17:40:47 +0900 Subject: [PATCH 14/43] [io_utils] Specializations for Eigen types --- include/mc_rtc/eigen_traits.h | 25 ++++++++ include/mc_rtc/io_utils.h | 110 ++++++++++++++++++++++++++++------ tests/test_io_utils.cpp | 11 ++++ 3 files changed, 129 insertions(+), 17 deletions(-) create mode 100644 include/mc_rtc/eigen_traits.h diff --git a/include/mc_rtc/eigen_traits.h b/include/mc_rtc/eigen_traits.h new file mode 100644 index 0000000000..88917beaf6 --- /dev/null +++ b/include/mc_rtc/eigen_traits.h @@ -0,0 +1,25 @@ +/* + * Copyright 2015-2020 CNRS-UM LIRMM, CNRS-AIST JRL + */ + +#pragma once +#include +#include + +namespace mc_rtc +{ +namespace internal +{ +namespace is_eigen_matrix_detail +{ +// These functions are never defined. +template +std::true_type test(const Eigen::MatrixBase *); +std::false_type test(...); +} // namespace is_eigen_matrix_detail +template +struct is_eigen_matrix : public decltype(is_eigen_matrix_detail::test(std::declval())) +{ +}; +} // namespace internal +} // namespace mc_rtc diff --git a/include/mc_rtc/io_utils.h b/include/mc_rtc/io_utils.h index 01a0541107..2a91dd609c 100644 --- a/include/mc_rtc/io_utils.h +++ b/include/mc_rtc/io_utils.h @@ -3,6 +3,7 @@ */ #pragma once +#include #include #include #include @@ -10,9 +11,9 @@ namespace mc_rtc { + namespace io { - /** Converts a container to a string * * @tparam Container A container whose underlying type is convertible to @@ -26,20 +27,23 @@ namespace io * @anchor to_string */ template::value, int>::type = 0> + typename std::enable_if::value + && !mc_rtc::internal::is_eigen_matrix::value, + int>::type = 0> std::string to_string(const Container & c, const std::string & delimiter = ", ") { if(c.cbegin() == c.cend()) { return ""; } - std::string out = *c.cbegin(); + std::stringstream out; + out << *c.cbegin(); for(auto it = std::next(c.cbegin()); it != c.cend(); ++it) { - out += delimiter; - out += *it; + out << delimiter; + out << *it; } - return out; + return out.str(); } /** @@ -69,40 +73,112 @@ std::string to_string(const Container & c, return out.str(); } -/** Converts a container to a string +/** + * @brief Converts a container to a string * * Example: - *\code{.cpp} + * \code{.cpp} * const auto availabeRobots = mc_rtc::io::to_string(robots(), [](const mc_rbdyn::Robot & r) -> const std::string & { - *return r.name(); }); - \endcode + * return r.name(); }); + * \endcode * * @tparam Container An iterable container whose unerlying type is convertible * to std::string. The container must define Container::value_type. * * @param c Container to convert - * @param get_value Lambda or functor that converts a single element from the container (of type Container::value_type) - to std::string + * @param get_value Lambda or functor that transforms each container element (of type Container::value_type) to an + * element convertible to std::string. This may be used to return elements of types non directly convertible to + * std::string, or to apply transformations to the data before returning it. * * @return A string of all the container elements. + * + * @anchor to_string_transform */ template::value, int>::type = 0> + typename std::enable_if::value + && !mc_rtc::internal::is_eigen_matrix::value, + int>::type = 0> std::string to_string(const Container & c, Callback && get_value, const std::string & delimiter = ", ") { if(c.cbegin() == c.cend()) { return ""; } - std::string out = get_value(*c.cbegin()); + std::stringstream out; + out << get_value(*c.cbegin()); for(auto it = std::next(c.cbegin()); it != c.cend(); ++it) { - out += delimiter; - out += get_value(*it); + out << delimiter; + out << get_value(*it); } - return out; + return out.str(); } +/** + * @name Specializations for Eigen types + */ +///@{ + +/** + * @brief Specialization of @ref to_string_transform for Eigen matrix/vector types + * @param fmt Eigen formatting options + * + * @see @ref to_string_transform + */ +template::value + && mc_rtc::internal::is_eigen_matrix::value, + int>::type = 0> +std::string to_string(const Container & c, + Callback && get_value, + const std::string & delimiter = ", ", + const Eigen::IOFormat & fmt = Eigen::IOFormat(Eigen::StreamPrecision, Eigen::DontAlignCols, "\t")) +{ + if(c.cbegin() == c.cend()) + { + return ""; + } + std::stringstream out; + out << get_value(*c.cbegin()).format(fmt); + for(auto it = std::next(c.cbegin()); it != c.cend(); ++it) + { + out << delimiter; + out << get_value(*it).format(fmt); + } + return out.str(); +} + +/** + * @brief Variant for Eigen matrix/vector types + * @param fmt Eigen formatting options + * + * @see @ref to_string + */ +template< + typename Container, + typename std::enable_if::value, int>::type = 0> +std::string to_string(const Container & c, + const std::string & delimiter = ", ", + const Eigen::IOFormat & fmt = + Eigen::IOFormat(Eigen::StreamPrecision, Eigen::DontAlignCols, "\t", " ", "", "", "", "")) +{ + if(c.cbegin() == c.cend()) + { + return ""; + } + std::stringstream out; + out << (*c.cbegin()).format(fmt); + for(auto it = std::next(c.cbegin()); it != c.cend(); ++it) + { + out << delimiter; + out << (*it).format(fmt); + } + return out.str(); +} + +///@} + } // namespace io } // namespace mc_rtc diff --git a/tests/test_io_utils.cpp b/tests/test_io_utils.cpp index d6f6b3e99f..b46890d90f 100644 --- a/tests/test_io_utils.cpp +++ b/tests/test_io_utils.cpp @@ -51,3 +51,14 @@ BOOST_AUTO_TEST_CASE(TestIOUtils) BOOST_CHECK(to_string(std::vector{"a", "aa", "aaa"}, lambda, " + ") == "1 + 2 + 3"); } + +BOOST_AUTO_TEST_CASE(TestIOUtilsEigen) +{ + using namespace mc_rtc::io; + // Trajectory of size 2*n_preview+1 + std::vector steps; + steps.push_back(Eigen::Vector3d::Random()); + steps.push_back(Eigen::Vector3d::Random()); + BOOST_REQUIRE(!to_string(steps, [](const Eigen::Vector3d & v) -> Eigen::RowVector3d { return v; }, "\n").empty()); + BOOST_REQUIRE(!to_string(steps).empty()); +} From 388e508ef8d6e9ae753bdb15aaa0e94168a2899a Mon Sep 17 00:00:00 2001 From: Arnaud TANGUY Date: Wed, 17 Jun 2020 11:37:50 +0900 Subject: [PATCH 15/43] [HMC/test] Clean generator example (+bugfix) Bugfix: out-of-bounds access to array m_steps --- include/mc_planning/State.h | 12 +- .../hmc => include/mc_planning}/generator.h | 160 ++++++++++++------ src/CMakeLists.txt | 2 + src/mc_planning/State.cpp | 12 +- {tests/hmc => src/mc_planning}/generator.cpp | 101 ++++++----- tests/hmc/CMakeLists.txt | 2 +- tests/hmc/testCoMGenerator.cpp | 9 +- 7 files changed, 193 insertions(+), 105 deletions(-) rename {tests/hmc => include/mc_planning}/generator.h (51%) rename {tests/hmc => src/mc_planning}/generator.cpp (71%) diff --git a/include/mc_planning/State.h b/include/mc_planning/State.h index 77ace42b9f..3729bb80ff 100644 --- a/include/mc_planning/State.h +++ b/include/mc_planning/State.h @@ -24,7 +24,7 @@ class MC_PLANNING_DLLAPI StateP /** @brief constructor */ - StateP(void); + StateP(); StateP(const Eigen::Vector3d & p); @@ -34,7 +34,7 @@ class MC_PLANNING_DLLAPI StateP friend std::ostream & operator<<(std::ostream & os, const StateP & s); - void Initialize(void); + void Initialize(); void Copy(const StateP & i_state); }; @@ -47,7 +47,7 @@ class MC_PLANNING_DLLAPI StatePV : virtual public StateP /** @brief constructor */ - StatePV(void); + StatePV(); StatePV(const Eigen::Vector3d & p, const Eigen::Vector3d & v); @@ -57,7 +57,7 @@ class MC_PLANNING_DLLAPI StatePV : virtual public StateP friend std::ostream & operator<<(std::ostream & os, const StatePV & s); - void Initialize(void); + void Initialize(); void Copy(const StatePV & i_state); }; @@ -70,7 +70,7 @@ class MC_PLANNING_DLLAPI StatePVA : virtual public StatePV /** @brief constructor */ - StatePVA(void); + StatePVA(); StatePVA(const Eigen::Vector3d & p, const Eigen::Vector3d & v, const Eigen::Vector3d & a); @@ -80,7 +80,7 @@ class MC_PLANNING_DLLAPI StatePVA : virtual public StatePV friend std::ostream & operator<<(std::ostream & os, const StatePVA & s); - void Initialize(void); + void Initialize(); void Copy(const StatePVA & i_state); }; diff --git a/tests/hmc/generator.h b/include/mc_planning/generator.h similarity index 51% rename from tests/hmc/generator.h rename to include/mc_planning/generator.h index f6e6d24254..2a10b0c01b 100644 --- a/tests/hmc/generator.h +++ b/include/mc_planning/generator.h @@ -9,42 +9,65 @@ namespace mc_planning { -class generator -{ -private: - std::shared_ptr> m_ComInterp; - - StatePVA m_COG_ideal_pre, m_COG_ideal, m_COG_cmp; - StatePVA m_COG_out; - Eigen::Vector3d m_Pcalpha_ideal_pre, m_Pcalpha_ideal, m_Pcalpha_cmp; - Eigen::Vector3d m_Vcalpha_ideal_pre, m_Vcalpha_ideal, m_Vcalpha_cmp; - Eigen::Vector3d m_Pcalpha_out; - Eigen::Vector3d m_Pcalpha_motion_out; - - int m_n_preview; - int m_n_steps; - double m_dt; - double m_omega_valpha; - double m_mass; - - linear_control_system::LinearTimeVariantInvertedPendulum m_ipm_long[2]; - linear_control_system::LIPMControlByPoleAssignWithExternalForce m_ipm_short[2]; - Eigen::Vector3d m_poles[2]; +/** + * @brief Utility class to generate long and short term trajectories for the + * CoM. + * + * Generate trajectories as introduced in: + * **Online 3D CoM Trajectory Generation for Multi-Contact Locomotion Synchronizing Contact**, *Mitsuharu Morisawa et + * al., IROS 2018* + */ +struct MC_PLANNING_DLLAPI generator +{ + /** + * @brief Construct a simple trajectory generator + * + * @param n_preview Size of the future preview window. The full preview window + * goes from future to past with size `2*n_preview+1` + * @param dt + */ + generator(int n_preview, double dt); - Eigen::VectorXd m_virtual_height[2]; - Eigen::VectorXd m_cog_height; - Eigen::VectorXd m_cog_dot_height; - Eigen::VectorXd m_cog_ddot_height; - std::vector m_steps; ///< (time, com_x, com_y) + /** + * @brief Generate the long and short term trajectories + * + * @param n_time current time + */ + void generate(int n_time); - void setupCOGHeight(int n_current); + /** + * @brief Desired steps + * @see setSteps(const std::vector & steps) + */ + const std::vector & Steps() const + { + return m_steps; + } - void setupTimeTrajectories(int n_current); + /** + * @brief Sets desired reference steps as (time, CoM X, CoM Y) + * + * @param steps Desired steps from which the trajectory will be generated + */ + void setSteps(const std::vector & steps) + { + m_steps = steps; + } - void generateTrajectories(); + /** + * @brief Add a step + * + * Step must be after the last existing timestep + * + * @param step + */ + void push_back(const Eigen::Vector3d & step); -public: + /** + * @name Accessors + */ + ///@{ const Eigen::Vector3d & IdealCOGPosition() const { return m_COG_ideal.P; @@ -106,28 +129,69 @@ class generator { return m_Pcalpha_out; } + ///@} - const std::vector & Steps() const - { - return m_steps; - } - void setStesps(const std::vector & steps) - { - m_steps = steps; - } +private: + /** + * @brief Setup CoM with constant height along the trajectory + * and iterpolate its position/velocity/acceleration + * + * Sets m_COG_ideal for the current time + * + * @param n_current Index of the start of the preview window + */ + void setupCOGHeight(int n_current); - void push_back(double time, double com_x, double com_y) - { - m_steps.push_back(Eigen::Vector3d(time, com_x, com_y)); - } - void push_back(const Eigen::Vector3d & step) - { - m_steps.push_back(step); - } + /** + * @brief Setup reference trajectories based on the desired steps + * + * Sets m_ipm_long.px_ref, m_ipm_long.py_ref, m_ipm_long.w2 and m_Pcalpha_ideal + * + * @param n_current Index of the start of the preview window + */ + void setupTimeTrajectories(int n_current); - generator(int n_preview, double dt); + /** + * @brief Solve long and short term trajectories from the reference time + * trajectories + * + * Computes m_COG_out, m_Pcalpha_out, m_Pcalpha_motion_out, m_COG_cmp + */ + void generateTrajectories(); - void generate(int n_time); +private: + std::shared_ptr> m_ComInterp; + + StatePVA m_COG_ideal_pre; + /** + * Ideal COG trajectory + * - X, Y components are computed by the long term trajectories m_ipm_long + * - Z component is interpolated by m_ComInterp + */ + StatePVA m_COG_ideal; + StatePVA m_COG_cmp; + StatePVA m_COG_out; + + Eigen::Vector3d m_Pcalpha_ideal_pre, m_Pcalpha_ideal, m_Pcalpha_cmp; + Eigen::Vector3d m_Vcalpha_ideal_pre, m_Vcalpha_ideal, m_Vcalpha_cmp; + Eigen::Vector3d m_Pcalpha_out; + Eigen::Vector3d m_Pcalpha_motion_out; + + int m_n_preview; ///< Size of the future preview window. The full preview is 2*m_n_preview+1 + int m_n_steps; ///< Current step + double m_dt; ///< Timestep + double m_omega_valpha; + double m_mass; ///< Robot mass + + linear_control_system::LinearTimeVariantInvertedPendulum m_ipm_long[2]; + linear_control_system::LIPMControlByPoleAssignWithExternalForce m_ipm_short[2]; + Eigen::Vector3d m_poles[2]; + + Eigen::VectorXd m_virtual_height[2]; + Eigen::VectorXd m_cog_height; ///< CoM height + Eigen::VectorXd m_cog_dot_height; ///< CoM velocity + Eigen::VectorXd m_cog_ddot_height; ///< CoM acceleration + std::vector m_steps; ///< (time, com_x, com_y) }; } // namespace mc_planning diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 4e4bd438b6..f57080039e 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -414,6 +414,7 @@ mc_planning/LIPMControlByPoleAssign.cpp mc_planning/LIPMControlByPoleAssignWithExternalForce.cpp mc_planning/MathFunction.cpp mc_planning/State.cpp +mc_planning/generator.cpp # XXX should it be there? ) set(mc_planning_HDR @@ -426,6 +427,7 @@ set(mc_planning_HDR ../include/mc_planning/InterpolatorBase.h ../include/mc_planning/CubicSplineBase.h ../include/mc_planning/ClampedCubicSpline.h +../include/mc_planning/generator.h ) add_library(mc_planning SHARED ${mc_planning_SRC} ${mc_planning_HDR}) diff --git a/src/mc_planning/State.cpp b/src/mc_planning/State.cpp index 78c178db49..f9ec0261cb 100644 --- a/src/mc_planning/State.cpp +++ b/src/mc_planning/State.cpp @@ -20,7 +20,7 @@ namespace mc_planning { //////////////////////////////////////////////////////////// -StateP::StateP(void) +StateP::StateP() { StateP::Initialize(); } @@ -45,7 +45,7 @@ std::ostream & operator<<(std::ostream & os, const StateP & s) return os; } -void StateP::Initialize(void) +void StateP::Initialize() { P.setZero(); } @@ -56,7 +56,7 @@ void StateP::Copy(const StateP & i_state) } //////////////////////////////////////////////////////////// -StatePV::StatePV(void) +StatePV::StatePV() { StatePV::Initialize(); } @@ -82,7 +82,7 @@ std::ostream & operator<<(std::ostream & os, const StatePV & s) return os; } -void StatePV::Initialize(void) +void StatePV::Initialize() { StateP::Initialize(); V.setZero(); @@ -95,7 +95,7 @@ void StatePV::Copy(const StatePV & i_state) } //////////////////////////////////////////////////////////// -StatePVA::StatePVA(void) +StatePVA::StatePVA() { StatePVA::Initialize(); } @@ -124,7 +124,7 @@ std::ostream & operator<<(std::ostream & os, const StatePVA & s) return os; } -void StatePVA::Initialize(void) +void StatePVA::Initialize() { StatePV::Initialize(); Vdot.setZero(); diff --git a/tests/hmc/generator.cpp b/src/mc_planning/generator.cpp similarity index 71% rename from tests/hmc/generator.cpp rename to src/mc_planning/generator.cpp index 868b80ac1b..55685cda22 100644 --- a/tests/hmc/generator.cpp +++ b/src/mc_planning/generator.cpp @@ -1,9 +1,7 @@ -#include "generator.h" #include +#include #include -#include -// #include -// #include +#include using namespace mc_planning::motion_interpolator; @@ -44,8 +42,12 @@ generator::generator(int n_preview, double dt) m_COG_ideal.P << 0.0, 0.0, waist_height; } +// n_current index of the start of the preview window void generator::setupCOGHeight(int n_current) { + // The first time, initialize the whole trajectory + // with constant waist height + // Ignores lateral sway during the trajectory? if(n_current == 0) { m_ComInterp->push_back(0.0, waist_height); @@ -53,12 +55,14 @@ void generator::setupCOGHeight(int n_current) m_ComInterp->update(); } + // Time window from past to future for(int n_step = -m_n_preview; n_step <= m_n_preview; n_step++) { m_ComInterp->get(n_current + n_step + m_n_preview, m_cog_height[n_step + m_n_preview], m_cog_dot_height[n_step + m_n_preview], m_cog_ddot_height[n_step + m_n_preview]); } + // Interpolated value at current time m_COG_ideal.P(Z) = m_cog_height[m_n_preview]; m_COG_ideal.V(Z) = m_cog_dot_height[m_n_preview]; m_COG_ideal.Vdot(Z) = m_cog_ddot_height[m_n_preview]; @@ -71,27 +75,29 @@ void generator::setupTimeTrajectories(int n_current) int n_steps_loop = m_n_steps; for(int i = 0; i < m_n_preview * 2 + 1; i++) { - px_ref(i) = 0.0; - py_ref(i) = 0.0; m_virtual_height[X](i) = 0.0; m_virtual_height[Y](i) = 0.0; - double tm = (double)(i + n_current) * m_dt; - if(tm >= m_steps[n_steps_loop](0)) + double tm = static_cast(i + n_current) * m_dt; + const auto & currStep = m_steps[n_steps_loop]; + const auto & nextStep = m_steps[n_steps_loop + 1]; + const double currTime = currStep(0); + const double nextTime = nextStep(0); + if(tm >= currTime) { - px_ref(i) = m_steps[n_steps_loop](1) - + (m_steps[n_steps_loop + 1](1) - m_steps[n_steps_loop](1)) - * polynomial3((tm - m_steps[n_steps_loop](0)) - / (m_steps[n_steps_loop + 1](0) - m_steps[n_steps_loop](0))); - py_ref(i) = m_steps[n_steps_loop](2) - + (m_steps[n_steps_loop + 1](2) - m_steps[n_steps_loop](2)) - * polynomial3((tm - m_steps[n_steps_loop](0)) - / (m_steps[n_steps_loop + 1](0) - m_steps[n_steps_loop](0))); + // 1: Step CoM x, 2: Step CoM Y + auto refXY = [&](const Eigen::Index axis) { + return currStep(axis) + + (nextStep(axis) - currStep(axis)) * polynomial3((tm - currTime) / (nextTime - currTime)); + }; + + px_ref(i) = refXY(1); + py_ref(i) = refXY(2); } else { - px_ref(i) = m_steps[n_steps_loop + 1](1); - py_ref(i) = m_steps[n_steps_loop + 1](2); + px_ref(i) = nextStep(1); + py_ref(i) = nextStep(2); } m_ipm_long[X].w2(i) = @@ -99,9 +105,12 @@ void generator::setupTimeTrajectories(int n_current) m_ipm_long[Y].w2(i) = (mc_rtc::constants::GRAVITY + m_cog_ddot_height[i]) / (m_cog_height[i] - m_virtual_height[Y](i)); - if(n_steps_loop < m_steps.size() - 1) + // We need one future step for computations, increment only + // till the second to last step + if(n_steps_loop < m_steps.size() - 2) { - if(tm >= m_steps[n_steps_loop + 1](0)) n_steps_loop++; + // If current time is after the next step time, move to next step + if(tm >= nextTime) n_steps_loop++; } } @@ -112,33 +121,32 @@ void generator::setupTimeTrajectories(int n_current) { if(tm_cur > m_steps[m_n_steps + 1](0)) m_n_steps++; } - -#if 0 - std::cout << "time=" << (double)n_current*m_dt << std::endl; - std::cout << "cog_height=" << m_cog_height.transpose() << std::endl; - std::cout << "vr_height=" << m_virtual_height[X].transpose() << std::endl; -#endif -#if 0 - std::cout << "time=" << (double)n_current*m_dt << std::endl; - std::cout << "cog_height=" << m_cog_height[m_n_preview] << std::endl; - std::cout << "vr_height=" << m_virtual_height[X][m_n_preview] << std::endl; - std::cout << "p_ref=" << px_ref[m_n_preview] << "," << py_ref[m_n_preview] << std::endl; -#endif } void generator::generateTrajectories(void) { StatePV COG_ideal_pre_next(m_COG_ideal.P, m_COG_ideal.V); Eigen::Vector3d Pcalpha_ideal_pre_next(m_Pcalpha_ideal); - for(int axis = X; axis <= Y; axis++) - { + + /** + * Computes long-term trajectory + */ + auto computeLongTerm = [&](int axis) { m_ipm_long[axis].update(); + // Current time m_ipm_long[axis].getState(0, m_COG_ideal_pre.P(axis), m_COG_ideal_pre.V(axis), m_COG_ideal_pre.Vdot(axis), m_Pcalpha_ideal_pre(axis), m_Vcalpha_ideal_pre(axis)); + // Next timestep m_ipm_long[axis].getState(1, m_COG_ideal.P(axis), m_COG_ideal.V(axis), m_COG_ideal.Vdot(axis), m_Pcalpha_ideal(axis), m_Vcalpha_ideal(axis)); + }; + /** + * Compute short term trajectory + * Depends on the long term trajectory results + **/ + auto computeShortTerm = [&](int axis) { double omega2 = m_ipm_long[axis].w2(m_n_preview); double omega = sqrt(omega2); m_ipm_short[axis].setSystemMatrices(omega * m_poles[axis](0), omega * m_poles[axis](1), m_poles[axis](2), omega2, @@ -152,7 +160,15 @@ void generator::generateTrajectories(void) double Fext = 0.0; m_ipm_short[axis].update(Fext, m_dt); m_ipm_short[axis].getStateVariables(m_COG_cmp.P(axis), m_COG_cmp.V(axis), m_Pcalpha_cmp(axis), m_Vcalpha_cmp(axis)); - } + }; + + auto computeTrajectories = [&](int axis) { + computeLongTerm(axis); + computeShortTerm(axis); + }; + + computeTrajectories(X); + computeTrajectories(Y); m_COG_out.P = m_COG_ideal.P + m_COG_cmp.P; m_COG_out.V = m_COG_ideal.V + m_COG_cmp.V; @@ -167,14 +183,15 @@ void generator::generate(int n_time) setupCOGHeight(n_time); setupTimeTrajectories(n_time); generateTrajectories(); +} -#if 0 - std::cout << "COG_out = " << m_COG_out.P.transpose() << std::endl; - std::cout << "Pcalpha_out = " << m_Pcalpha_out.transpose() << std::endl; - - std::cout << "VCOGdot_ideal = " << m_COG_ideal.Vdot.transpose() << std::endl; - std::cout << "VCOGdot_cmp = " << m_COG_cmp.Vdot.transpose() << std::endl; -#endif +void generator::push_back(const Eigen::Vector3d & step) +{ + if(step(0) <= m_steps.back()(0)) + { + mc_rtc::log::error("Invalid step time ({} <= {})", step(0), m_steps.back()(0)); + } + m_steps.push_back(step); } } // namespace mc_planning diff --git a/tests/hmc/CMakeLists.txt b/tests/hmc/CMakeLists.txt index f2ffd35e88..02e571ec17 100644 --- a/tests/hmc/CMakeLists.txt +++ b/tests/hmc/CMakeLists.txt @@ -1,2 +1,2 @@ -add_executable(generator testCoMGenerator.cpp generator.cpp) +add_executable(generator testCoMGenerator.cpp) target_link_libraries(generator PUBLIC mc_planning) diff --git a/tests/hmc/testCoMGenerator.cpp b/tests/hmc/testCoMGenerator.cpp index 56913ca601..409eec4654 100644 --- a/tests/hmc/testCoMGenerator.cpp +++ b/tests/hmc/testCoMGenerator.cpp @@ -1,6 +1,7 @@ +#include +#include #include #include -#include "generator.h" using namespace mc_planning; using Vector3 = Eigen::Vector3d; @@ -29,8 +30,12 @@ int main(void) steps.push_back(steps.back() + Vector3{1.6, 0.0, 0.0}); steps.push_back(steps.back() + Vector3{0.1, -0.2, 0.095}); steps.push_back(steps.back() + Vector3{(double)n_preview * dt, 0.0, 0.0}); + // Repeat last step for computations steps.push_back(steps.back() + Vector3{(double)n_preview * dt, 0.0, 0.0}); - com_traj.setStesps(steps); + com_traj.setSteps(steps); + mc_rtc::log::info( + "Desired steps:\nTime\tCoM X\tCoM Y\n{}", + mc_rtc::io::to_string(steps, [](const Eigen::Vector3d & v) -> Eigen::RowVector3d { return v; }, "\n")); mc_rtc::Logger logger(mc_rtc::Logger::Policy::NON_THREADED, "/tmp", "mc_rtc-test"); logger.addLogEntry("IdealCOGPosition", [&com_traj]() { return com_traj.IdealCOGPosition(); }); From 98e9e8f84413c5d7744c7db9603faf1f914b8667 Mon Sep 17 00:00:00 2001 From: Arnaud TANGUY Date: Thu, 18 Jun 2020 11:47:04 +0900 Subject: [PATCH 16/43] [time_utils] Add stopwatch --- include/mc_rtc/time_utils.h | 60 +++++++++++++++++++++++-------------- 1 file changed, 37 insertions(+), 23 deletions(-) diff --git a/include/mc_rtc/time_utils.h b/include/mc_rtc/time_utils.h index bdd20b4bb3..7a2f2ff559 100644 --- a/include/mc_rtc/time_utils.h +++ b/include/mc_rtc/time_utils.h @@ -4,12 +4,12 @@ #pragma once #include +#include namespace mc_rtc { /** - * @ class measure - * @ brief Class to measure the execution time of a callable + * @brief Class to measure the execution time of a callable */ template, class ClockT = typename std::conditional, std::chrono::steady_clock>::type> struct measure { - /** - * @brief Returns the quantity (count) of the elapsed time as TimeT units - */ - template - /** - * @brief Returns the execution time of the provide function - * - * @param func Function to evaluate - * @param args Arguments to the function - * - * @return Time elapsed (in the provided unit) - */ - static typename TimeT::rep execution(F && func, Args &&... args) - { - auto start = ClockT::now(); - func(std::forward(args)...); - auto duration = std::chrono::duration_cast(ClockT::now() - start); - return duration.count(); - } - /** * @brief Returns the exectution time of the provided function (in chrono * type system) @@ -47,7 +27,7 @@ struct measure * @return Time elapsed */ template - static TimeT duration(F && func, Args &&... args) + static TimeT execution(F && func, Args &&... args) { auto start = ClockT::now(); func(std::forward(args)...); @@ -56,6 +36,40 @@ struct measure }; using measure_s = measure>; using measure_ms = measure>; +using measure_us = measure>; using measure_ns = measure>; +/** + * @brief Class to measure execution time + */ +template, + class ClockT = typename std::conditional::type> +struct Stopwatch +{ + using TimeP = typename TimeT::time_point; + TimeP startTime_; + + Stopwatch() + { + startTime_ = ClockT::now(); + } + + inline void start() + { + startTime_ = ClockT::now(); + } + + inline TimeT elapsed() + { + auto elapsed = std::chrono::duration_cast(ClockT::now() - startTime_); + return elapsed; + } +}; +using Stopwatch_s = Stopwatch>; +using Stopwatch_ms = Stopwatch>; +using Stopwatch_us = Stopwatch>; +using Stopwatch_ns = Stopwatch>; + } // namespace mc_rtc From 452f2efcc330165dffeefbeaf60a25148bcd0b16 Mon Sep 17 00:00:00 2001 From: Arnaud TANGUY Date: Wed, 17 Jun 2020 19:16:16 +0900 Subject: [PATCH 17/43] [HMC/samples] Sample controller generating and tracking CoM/ZMP Trajectory Write a first simple example that: - Generates the CoM/ZMP trajectory (open-loop) - Tracks it with the LIPMStabilizer from Stephane - Adds GUI/live plot visualization --- CMakeLists.txt | 1 + include/mc_planning/generator.h | 19 ++- samples/CMakeLists.txt | 1 + .../CoMTrajectoryGeneration/CMakeLists.txt | 14 ++ .../etc/CoMTrajectoryGeneration.in.yaml | 74 +++++++++ .../src/CMakeLists.txt | 18 +++ .../src/CoMTrajectoryGeneration.cpp | 20 +++ .../src/CoMTrajectoryGeneration.h | 17 +++ samples/CoMTrajectoryGeneration/src/api.h | 35 +++++ samples/CoMTrajectoryGeneration/src/lib.cpp | 3 + .../src/states/CMakeLists.txt | 25 ++++ .../CoMTrajectoryGeneration_Initial.cpp | 141 ++++++++++++++++++ .../states/CoMTrajectoryGeneration_Initial.h | 43 ++++++ .../src/states/StabilizerStandingTrackCoM.cpp | 55 +++++++ .../src/states/StabilizerStandingTrackCoM.h | 32 ++++ .../src/states/data/states.json | 2 + src/mc_planning/generator.cpp | 38 ++++- src/mc_robots/jvrc1.cpp | 6 +- tests/hmc/testCoMGenerator.cpp | 10 +- 19 files changed, 535 insertions(+), 19 deletions(-) create mode 100644 samples/CMakeLists.txt create mode 100644 samples/CoMTrajectoryGeneration/CMakeLists.txt create mode 100644 samples/CoMTrajectoryGeneration/etc/CoMTrajectoryGeneration.in.yaml create mode 100644 samples/CoMTrajectoryGeneration/src/CMakeLists.txt create mode 100644 samples/CoMTrajectoryGeneration/src/CoMTrajectoryGeneration.cpp create mode 100644 samples/CoMTrajectoryGeneration/src/CoMTrajectoryGeneration.h create mode 100644 samples/CoMTrajectoryGeneration/src/api.h create mode 100644 samples/CoMTrajectoryGeneration/src/lib.cpp create mode 100644 samples/CoMTrajectoryGeneration/src/states/CMakeLists.txt create mode 100644 samples/CoMTrajectoryGeneration/src/states/CoMTrajectoryGeneration_Initial.cpp create mode 100644 samples/CoMTrajectoryGeneration/src/states/CoMTrajectoryGeneration_Initial.h create mode 100644 samples/CoMTrajectoryGeneration/src/states/StabilizerStandingTrackCoM.cpp create mode 100644 samples/CoMTrajectoryGeneration/src/states/StabilizerStandingTrackCoM.h create mode 100644 samples/CoMTrajectoryGeneration/src/states/data/states.json diff --git a/CMakeLists.txt b/CMakeLists.txt index ea8b35bf56..c6779b2432 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -156,6 +156,7 @@ include(CMakeModules/ListAllFiles.cmake) add_subdirectory(CMakeModules) add_subdirectory(doc) add_subdirectory(src) +add_subdirectory(samples) add_subdirectory(plugins) if(${BUILD_TESTING}) add_subdirectory(tests) diff --git a/include/mc_planning/generator.h b/include/mc_planning/generator.h index 2a10b0c01b..a5e3d555c8 100644 --- a/include/mc_planning/generator.h +++ b/include/mc_planning/generator.h @@ -6,13 +6,13 @@ #include #include #include +#include namespace mc_planning { /** - * @brief Utility class to generate long and short term trajectories for the - * CoM. + * @brief Utility class to generate long and short term trajectories for the CoM. * * Generate trajectories as introduced in: * **Online 3D CoM Trajectory Generation for Multi-Contact Locomotion Synchronizing Contact**, *Mitsuharu Morisawa et @@ -26,8 +26,10 @@ struct MC_PLANNING_DLLAPI generator * @param n_preview Size of the future preview window. The full preview window * goes from future to past with size `2*n_preview+1` * @param dt + * @mass Robot mass + * @waist_height Initial height of the waist (constant for now) */ - generator(int n_preview, double dt); + generator(int n_preview, double dt, double mass = 60, double waist_height = 0.8); /** * @brief Generate the long and short term trajectories @@ -36,6 +38,15 @@ struct MC_PLANNING_DLLAPI generator */ void generate(int n_time); + /** + * @brief Add to Logger + */ + void addToLogger(mc_rtc::Logger & logger); + /** + * @brief Remove from Logger + */ + void removeFromLogger(mc_rtc::Logger & logger); + /** * @brief Desired steps * @see setSteps(const std::vector & steps) @@ -53,6 +64,7 @@ struct MC_PLANNING_DLLAPI generator void setSteps(const std::vector & steps) { m_steps = steps; + m_n_steps = 0; } /** @@ -182,6 +194,7 @@ struct MC_PLANNING_DLLAPI generator double m_dt; ///< Timestep double m_omega_valpha; double m_mass; ///< Robot mass + double m_waist_height; ///< Height of the weight (constant for now) linear_control_system::LinearTimeVariantInvertedPendulum m_ipm_long[2]; linear_control_system::LIPMControlByPoleAssignWithExternalForce m_ipm_short[2]; diff --git a/samples/CMakeLists.txt b/samples/CMakeLists.txt new file mode 100644 index 0000000000..e0b765fde8 --- /dev/null +++ b/samples/CMakeLists.txt @@ -0,0 +1 @@ +add_subdirectory(CoMTrajectoryGeneration) diff --git a/samples/CoMTrajectoryGeneration/CMakeLists.txt b/samples/CoMTrajectoryGeneration/CMakeLists.txt new file mode 100644 index 0000000000..55f44e9372 --- /dev/null +++ b/samples/CoMTrajectoryGeneration/CMakeLists.txt @@ -0,0 +1,14 @@ +add_subdirectory(src) + +set(AROBASE "@") +set(CoMTrajectoryGeneration_STATES_INSTALL_PREFIX "${MC_CONTROLLER_INSTALL_PREFIX}/fsm/states") +set(CoMTrajectoryGeneration_STATES_DATA_INSTALL_PREFIX "${MC_CONTROLLER_INSTALL_PREFIX}/fsm/states/data") +set(CoMTrajectoryGeneration_INIT_STATE "Pause_2s") +set(MC_STATES_DEFAULT_INSTALL_PREFIX "${MC_CONTROLLER_INSTALL_PREFIX}/fsm/states") +set(MC_STATES_INSTALL_PREFIX "${MC_CONTROLLER_INSTALL_PREFIX}/${PROJECT_NAME}/states") +configure_file(etc/CoMTrajectoryGeneration.in.yaml "${CMAKE_CURRENT_BINARY_DIR}/etc/CoMTrajectoryGeneration.yaml") +install(FILES "${CMAKE_CURRENT_BINARY_DIR}/etc/CoMTrajectoryGeneration.yaml" DESTINATION "${MC_CONTROLLER_INSTALL_PREFIX}/etc") +unset(AROBASE) + +install(FILES "${CMAKE_CURRENT_BINARY_DIR}/etc/CoMTrajectoryGeneration.yaml" + DESTINATION "${MC_CONTROLLER_INSTALL_PREFIX}/etc") diff --git a/samples/CoMTrajectoryGeneration/etc/CoMTrajectoryGeneration.in.yaml b/samples/CoMTrajectoryGeneration/etc/CoMTrajectoryGeneration.in.yaml new file mode 100644 index 0000000000..58ce5720ec --- /dev/null +++ b/samples/CoMTrajectoryGeneration/etc/CoMTrajectoryGeneration.in.yaml @@ -0,0 +1,74 @@ +--- +# If true, the FSM transitions are managed by an external tool +Managed: false +# If true and the FSM is self-managed, transitions should be triggered +StepByStep: false +# Change idle behaviour, if true the state is kept until transition, +# otherwise the FSM holds the last state until transition +IdleKeepState: true +# Where to look for state libraries +StatesLibraries: +- "@MC_STATES_DEFAULT_INSTALL_PREFIX@" +- "@MC_STATES_INSTALL_PREFIX@" +# Where to look for state files +StatesFiles: +- "@MC_STATES_DEFAULT_INSTALL_PREFIX@/data" +- "@MC_STATES_INSTALL_PREFIX@/data" +# If true, state factory will be more verbose +VerboseStateFactory: false +# Additional robots to load +robots: + ground: + module: env/ground +# General constraints, always on +constraints: +- type: contact +- type: kinematics + damper: [0.1, 0.01, 0.5] +- type: compoundJoint +# Collision constraint +collisions: +- type: collision + useMinimal: true +# Initial set of contacts +contacts: [] + +# Implement some additional text states +states: + SmoothStart: + base: Parallel + states: [Stabilizer::Standing, Pause_2s] + CoMTrajectoryGenerationTracking: + base: Parallel + states: [CoMTrajectoryGeneration_Initial, StabilizerStandingTrackCoM] + configs: + StabilizerStandingTrackCoM: + StabilizerConfig: + type: lipm_stabilizer + leftFootSurface: LeftFootCenter + rightFootSurface: RightFootCenter + enabled: true + contacts: [Left, Right] + admittance: + maxVel: + linear: [0.3,0.3,0.3] + angular: [0.5,0.5,0.5] + Left: + rotation: [0,0,0] + height: 0 + Right: + rotation: [0,0,0] + height: 0 + +# Transitions map +transitions: +- [Pause_2s, OK, SmoothStart, Auto] +- [SmoothStart, OK, CoMTrajectoryGenerationTracking, Auto] +- [CoMTrajectoryGenerationTracking, OK, CoMTrajectoryGenerationTracking, Auto] + +# Initial state +init: Pause_2s + +# State observation +RunObservers: [Encoder, KinematicInertial] +UpdateObservers: [Encoder, KinematicInertial] diff --git a/samples/CoMTrajectoryGeneration/src/CMakeLists.txt b/samples/CoMTrajectoryGeneration/src/CMakeLists.txt new file mode 100644 index 0000000000..da4b8eb869 --- /dev/null +++ b/samples/CoMTrajectoryGeneration/src/CMakeLists.txt @@ -0,0 +1,18 @@ +set(controller_SRC + CoMTrajectoryGeneration.cpp +) + +set(controller_HDR + CoMTrajectoryGeneration.h +) + +set(CONTROLLER_NAME CoMTrajectoryGeneration) +add_library(${CONTROLLER_NAME} SHARED ${controller_SRC} ${controller_HDR}) +set_target_properties(${CONTROLLER_NAME} PROPERTIES COMPILE_FLAGS "-DCoMTrajectoryGeneration_EXPORTS") +target_link_libraries(${CONTROLLER_NAME} PUBLIC mc_rtc::mc_control_fsm) +install(TARGETS ${CONTROLLER_NAME} DESTINATION ${MC_CONTROLLER_INSTALL_PREFIX}/..) + +add_controller(${CONTROLLER_NAME}_controller lib.cpp "") +target_link_libraries(${CONTROLLER_NAME}_controller PUBLIC ${CONTROLLER_NAME}) + +add_subdirectory(states) diff --git a/samples/CoMTrajectoryGeneration/src/CoMTrajectoryGeneration.cpp b/samples/CoMTrajectoryGeneration/src/CoMTrajectoryGeneration.cpp new file mode 100644 index 0000000000..07a202a695 --- /dev/null +++ b/samples/CoMTrajectoryGeneration/src/CoMTrajectoryGeneration.cpp @@ -0,0 +1,20 @@ +#include "CoMTrajectoryGeneration.h" + +CoMTrajectoryGeneration::CoMTrajectoryGeneration(mc_rbdyn::RobotModulePtr rm, double dt, const mc_rtc::Configuration & config) +: mc_control::fsm::Controller(rm, dt, config) +{ + + mc_rtc::log::success("CoMTrajectoryGeneration init done "); +} + +bool CoMTrajectoryGeneration::run() +{ + return mc_control::fsm::Controller::run(); +} + +void CoMTrajectoryGeneration::reset(const mc_control::ControllerResetData & reset_data) +{ + mc_control::fsm::Controller::reset(reset_data); +} + + diff --git a/samples/CoMTrajectoryGeneration/src/CoMTrajectoryGeneration.h b/samples/CoMTrajectoryGeneration/src/CoMTrajectoryGeneration.h new file mode 100644 index 0000000000..0ed6293aa3 --- /dev/null +++ b/samples/CoMTrajectoryGeneration/src/CoMTrajectoryGeneration.h @@ -0,0 +1,17 @@ +#pragma once + +#include +#include + +#include "api.h" + +struct CoMTrajectoryGeneration_DLLAPI CoMTrajectoryGeneration : public mc_control::fsm::Controller +{ + CoMTrajectoryGeneration(mc_rbdyn::RobotModulePtr rm, double dt, const mc_rtc::Configuration & config); + + bool run() override; + + void reset(const mc_control::ControllerResetData & reset_data) override; +private: + mc_rtc::Configuration config_; +}; \ No newline at end of file diff --git a/samples/CoMTrajectoryGeneration/src/api.h b/samples/CoMTrajectoryGeneration/src/api.h new file mode 100644 index 0000000000..0f8d1c5c04 --- /dev/null +++ b/samples/CoMTrajectoryGeneration/src/api.h @@ -0,0 +1,35 @@ +#pragma once + +#if defined _WIN32 || defined __CYGWIN__ +# define CoMTrajectoryGeneration_DLLIMPORT __declspec(dllimport) +# define CoMTrajectoryGeneration_DLLEXPORT __declspec(dllexport) +# define CoMTrajectoryGeneration_DLLLOCAL +#else +// On Linux, for GCC >= 4, tag symbols using GCC extension. +# if __GNUC__ >= 4 +# define CoMTrajectoryGeneration_DLLIMPORT __attribute__((visibility("default"))) +# define CoMTrajectoryGeneration_DLLEXPORT __attribute__((visibility("default"))) +# define CoMTrajectoryGeneration_DLLLOCAL __attribute__((visibility("hidden"))) +# else +// Otherwise (GCC < 4 or another compiler is used), export everything. +# define CoMTrajectoryGeneration_DLLIMPORT +# define CoMTrajectoryGeneration_DLLEXPORT +# define CoMTrajectoryGeneration_DLLLOCAL +# endif // __GNUC__ >= 4 +#endif // defined _WIN32 || defined __CYGWIN__ + +#ifdef CoMTrajectoryGeneration_STATIC +// If one is using the library statically, get rid of +// extra information. +# define CoMTrajectoryGeneration_DLLAPI +# define CoMTrajectoryGeneration_LOCAL +#else +// Depending on whether one is building or using the +// library define DLLAPI to import or export. +# ifdef CoMTrajectoryGeneration_EXPORTS +# define CoMTrajectoryGeneration_DLLAPI CoMTrajectoryGeneration_DLLEXPORT +# else +# define CoMTrajectoryGeneration_DLLAPI CoMTrajectoryGeneration_DLLIMPORT +# endif // CoMTrajectoryGeneration_EXPORTS +# define CoMTrajectoryGeneration_LOCAL CoMTrajectoryGeneration_DLLLOCAL +#endif // CoMTrajectoryGeneration_STATIC \ No newline at end of file diff --git a/samples/CoMTrajectoryGeneration/src/lib.cpp b/samples/CoMTrajectoryGeneration/src/lib.cpp new file mode 100644 index 0000000000..9095d578f9 --- /dev/null +++ b/samples/CoMTrajectoryGeneration/src/lib.cpp @@ -0,0 +1,3 @@ +#include "CoMTrajectoryGeneration.h" + +CONTROLLER_CONSTRUCTOR("CoMTrajectoryGeneration", CoMTrajectoryGeneration) diff --git a/samples/CoMTrajectoryGeneration/src/states/CMakeLists.txt b/samples/CoMTrajectoryGeneration/src/states/CMakeLists.txt new file mode 100644 index 0000000000..bc1ae00932 --- /dev/null +++ b/samples/CoMTrajectoryGeneration/src/states/CMakeLists.txt @@ -0,0 +1,25 @@ +set(FSM_STATES_INSTALL_DIR "${MC_CONTROLLER_INSTALL_PREFIX}/fsm/states") +set(FSM_HDR_DIR "${PROJECT_SOURCE_DIR}/samples/mc_control/fsm/states") +set(FSM_HDR_INSTALL_DIR "${CMAKE_INSTALL_PREFIX}/include/mc_control/fsm/states") + +macro(add_fsm_state state_name state_SRC state_HDR) + add_library(${state_name} SHARED ${state_SRC} ${state_HDR} "${PROJECT_SOURCE_DIR}/include/mc_control/fsm/states/api.h") + set_target_properties(${state_name} PROPERTIES FOLDER controllers/fsm/states) + set_target_properties(${state_name} PROPERTIES OUTPUT_NAME ${state_name}) + set_target_properties(${state_name} PROPERTIES PREFIX "") + set_target_properties(${state_name} PROPERTIES COMPILE_FLAGS "-DMC_CONTROL_FSM_STATE_EXPORTS") + target_link_libraries(${state_name} PUBLIC mc_control_fsm) + install(TARGETS ${state_name} EXPORT mc_rtc_fsm_states DESTINATION ${FSM_STATES_INSTALL_DIR}) + install(FILES ${state_HDR} DESTINATION ${FSM_HDR_INSTALL_DIR}) +endmacro() + +macro(add_fsm_state_simple state_name) + add_fsm_state(${state_name} ${state_name}.cpp ${CMAKE_CURRENT_SOURCE_DIR}/${state_name}.h) +endmacro() + +add_fsm_state_simple(CoMTrajectoryGeneration_Initial) +target_link_libraries(CoMTrajectoryGeneration_Initial PUBLIC mc_planning) + +add_fsm_state_simple(StabilizerStandingTrackCoM) + +# add_fsm_data_directory(data) diff --git a/samples/CoMTrajectoryGeneration/src/states/CoMTrajectoryGeneration_Initial.cpp b/samples/CoMTrajectoryGeneration/src/states/CoMTrajectoryGeneration_Initial.cpp new file mode 100644 index 0000000000..ed10fe68de --- /dev/null +++ b/samples/CoMTrajectoryGeneration/src/states/CoMTrajectoryGeneration_Initial.cpp @@ -0,0 +1,141 @@ +#include "CoMTrajectoryGeneration_Initial.h" +#include "../CoMTrajectoryGeneration.h" +#include +#include + +namespace mc_samples +{ + +void CoMTrajectoryGeneration_Initial::configure(const mc_rtc::Configuration & config) +{ + config("previewTime", previewTime_); +} + +void CoMTrajectoryGeneration_Initial::start(mc_control::fsm::Controller & ctl) +{ + m_dt = ctl.timeStep; + previewSize_ = static_cast(lround(previewTime_ / m_dt)); + /// XXX should this be CoM height or waist height? + double waist_height = ctl.realRobot().com().z(); + + leftFootPos_ = ctl.robot().surfacePose("LeftFootCenter").translation(); + rightFootPos_ = ctl.robot().surfacePose("RightFootCenter").translation(); + feetCenterPos_ = (leftFootPos_ + rightFootPos_)/2; + + double leftY = leftFootPos_.y(); + double rightY = rightFootPos_.y(); + double centerY = feetCenterPos_.y(); + //clang-format off + m_steps = + { + {0.0, 0.0, 0.0}, + {2.0, 0.0, 0.0}, + {3.5, 0.0, rightY}, + {4, 0.0, rightY}, + {5.5, 0.0, leftY}, + {7, 0.0, rightY}, + {9, 0.0, centerY}, + {10, 0.0, centerY} + }; + //clang-format on + + comGenerator_ = std::make_shared(previewSize_, m_dt, ctl.robot().mass(), + waist_height); + comGenerator_->addToLogger(ctl.logger()); + comGenerator_->setSteps(m_steps); + mc_rtc::log::info( + "Desired steps:\nTime\tCoM X\tCoM Y\n{}", + mc_rtc::io::to_string(m_steps, [](const Eigen::Vector3d & v) -> Eigen::RowVector3d { return v; }, "\n")); + + using Color = mc_rtc::gui::Color; + using Style = mc_rtc::gui::plot::Style; + ctl.gui()->addPlot( + "Trajectory (Y)", + mc_rtc::gui::plot::X("t", [this]() { return t_; }), + mc_rtc::gui::plot::Y("Output CoM", [this]() { return comGenerator_->OutputCOGPosition().y(); }, Color::Green), + mc_rtc::gui::plot::Y("Ideal CoM", [this]() { return comGenerator_->IdealCOGPosition().y(); }, Color::Green, Style::Dashed), + mc_rtc::gui::plot::Y("Output ZMP", [this]() { return comGenerator_->OutputZMPPosition().y(); }, Color::Blue), + mc_rtc::gui::plot::Y("Ideal ZMP", [this]() { return comGenerator_->IdealZMPPosition().y(); }, Color::Blue, Style::Dashed)); + + ctl.gui()->addElement({"CoMTrajectoryGeneration"}, + mc_rtc::gui::Button("Left", + [this]() + { + const auto & curr = t_; + m_steps.clear(); + m_steps.push_back(Eigen::Vector3d{curr+3, 0, leftFootPos_.y()}); + comGenerator_->setSteps(m_steps); + updateSteps(); + }), + mc_rtc::gui::Button("Right", + [this]() + { + const auto & curr = t_; + m_steps.clear(); + m_steps.push_back(Eigen::Vector3d{curr+3, 0, rightFootPos_.y()}); + updateSteps(); + }), + mc_rtc::gui::Button("Center", + [this]() + { + const auto & curr = t_; + m_steps.clear(); + m_steps.push_back(Eigen::Vector3d{curr+3, 0, feetCenterPos_.y()}); + updateSteps(); + }) + ); + + ctl.logger().addLogEntry("perf_CoMGenerator[ms]", + [this]() + { + return generationTime_; + }); +} + +void CoMTrajectoryGeneration_Initial::updateSteps() +{ + // Repeat last element + m_steps.push_back(m_steps.back()); + m_steps.back()(0) += previewTime_; + comGenerator_->setSteps(m_steps); + mc_rtc::log::info("Current time: {}", t_); + mc_rtc::log::info( + "Desired steps:\nTime\tCoM X\tCoM Y\n{}", + mc_rtc::io::to_string(comGenerator_->Steps(), [](const Eigen::Vector3d & v) -> Eigen::RowVector3d { return v; }, "\n")); +} + +bool CoMTrajectoryGeneration_Initial::run(mc_control::fsm::Controller & ctl) +{ + if(t_ < m_steps.back()(0)) + { + auto generateTrajectory = [this]() + { + comGenerator_->generate(iter_); + }; + generationTime_ = mc_rtc::measure_ms::execution(generateTrajectory).count(); + + if(ctl.datastore().has("StabilizerStandingTrackCoM::target")) + { + ctl.datastore().call("StabilizerStandingTrackCoM::target", comGenerator_->OutputCOGPosition(), comGenerator_->OutputCOGVelocity(), comGenerator_->OutputCOGAcceleration(), comGenerator_->OutputZMPPosition()); + } + } + else + { + // mc_rtc::log::info("No more target"); + generationTime_ = 0; + } + + iter_++; + t_ += m_dt; + output("OK"); + return false; +} + +void CoMTrajectoryGeneration_Initial::teardown(mc_control::fsm::Controller & ctl) +{ + comGenerator_->removeFromLogger(ctl.logger()); +} + +} /* mc_samples */ + +EXPORT_SINGLE_STATE("CoMTrajectoryGeneration_Initial", mc_samples::CoMTrajectoryGeneration_Initial) diff --git a/samples/CoMTrajectoryGeneration/src/states/CoMTrajectoryGeneration_Initial.h b/samples/CoMTrajectoryGeneration/src/states/CoMTrajectoryGeneration_Initial.h new file mode 100644 index 0000000000..a5405ca7d3 --- /dev/null +++ b/samples/CoMTrajectoryGeneration/src/states/CoMTrajectoryGeneration_Initial.h @@ -0,0 +1,43 @@ +#pragma once + +#include +#include + +namespace mc_samples +{ + +struct CoMTrajectoryGeneration_Initial : mc_control::fsm::State +{ + + void configure(const mc_rtc::Configuration & config) override; + + void start(mc_control::fsm::Controller & ctl) override; + + bool run(mc_control::fsm::Controller & ctl) override; + + void teardown(mc_control::fsm::Controller & ctl) override; + + private: + void updateSteps(); + + private: + double previewTime_ = 1.6; ///< Preview horizon half-time [s] + Eigen::Vector3d polesX_ = {1.0, 1.0, 150.0}; + Eigen::Vector3d polesY_ = {1.0, 1.0, 150.0}; + + double m_dt = 0.005; ///< Timestep (for convenience) + unsigned previewSize_ = 0; ///< Size of the preview window + unsigned iter_ = 0; ///< Number of iterations elapsed since started + double t_ = 0; ///< Time elapsed since started + double generationTime_ = 0; ///< Time spent generating the trajectory + + Eigen::Vector3d leftFootPos_ = Eigen::Vector3d::Zero(); + Eigen::Vector3d rightFootPos_ = Eigen::Vector3d::Zero(); + Eigen::Vector3d feetCenterPos_ = Eigen::Vector3d::Zero(); + +private: + std::shared_ptr comGenerator_; + std::vector m_steps; ///< (time, com_x, com_y) +}; + +} /* mc_samples */ diff --git a/samples/CoMTrajectoryGeneration/src/states/StabilizerStandingTrackCoM.cpp b/samples/CoMTrajectoryGeneration/src/states/StabilizerStandingTrackCoM.cpp new file mode 100644 index 0000000000..cbcca605a1 --- /dev/null +++ b/samples/CoMTrajectoryGeneration/src/states/StabilizerStandingTrackCoM.cpp @@ -0,0 +1,55 @@ +#include "StabilizerStandingTrackCoM.h" +#include "../CoMTrajectoryGeneration.h" +#include + +#include +#include + +namespace mc_samples +{ + +void StabilizerStandingTrackCoM::configure(const mc_rtc::Configuration & config) +{ + config_.load(config); +} + +void StabilizerStandingTrackCoM::start(mc_control::fsm::Controller & ctl) +{ + // create stabilizer task from config + if(!config_.has("StabilizerConfig")) + { + config_.add("StabilizerConfig"); + } + config_("StabilizerConfig").add("type", "lipm_stabilizer"); + + stabilizerTask_ = mc_tasks::MetaTaskLoader::load( + ctl.solver(), config_("StabilizerConfig")); + // FIXME seems to hang when solution is not feasible + ctl.solver().addTask(stabilizerTask_); + stabilizerTask_->staticTarget(ctl.realRobot().com()); + + ctl.datastore().make_call("StabilizerStandingTrackCoM::target", + [this](const Eigen::Vector3d & com, const Eigen::Vector3d & comd, const Eigen::Vector3d & comdd, const Eigen::Vector3d & zmp) + { + stabilizerTask_->target(com, comd, comdd, zmp); + }); +} + + +bool StabilizerStandingTrackCoM::run(mc_control::fsm::Controller & ctl) +{ + // Update anchor frame for the KinematicInertial observer + ctl.anchorFrame(stabilizerTask_->anchorFrame()); + ctl.anchorFrameReal(stabilizerTask_->anchorFrameReal()); + output("OK"); + return false; +} + +void StabilizerStandingTrackCoM::teardown(mc_control::fsm::Controller & ctl) +{ + ctl.datastore().remove("StabilizerStandingTrackCoM::target"); +} + +} /* mc_samples */ + +EXPORT_SINGLE_STATE("StabilizerStandingTrackCoM", mc_samples::StabilizerStandingTrackCoM) diff --git a/samples/CoMTrajectoryGeneration/src/states/StabilizerStandingTrackCoM.h b/samples/CoMTrajectoryGeneration/src/states/StabilizerStandingTrackCoM.h new file mode 100644 index 0000000000..d6aee40e15 --- /dev/null +++ b/samples/CoMTrajectoryGeneration/src/states/StabilizerStandingTrackCoM.h @@ -0,0 +1,32 @@ +#pragma once + +#include + +namespace mc_tasks +{ +namespace lipm_stabilizer +{ +struct StabilizerTask; +} +} // namespace mc_tasks + +namespace mc_samples +{ + +struct StabilizerStandingTrackCoM : mc_control::fsm::State +{ + + void configure(const mc_rtc::Configuration & config) override; + + void start(mc_control::fsm::Controller & ctl) override; + + bool run(mc_control::fsm::Controller & ctl) override; + + void teardown(mc_control::fsm::Controller & ctl) override; + +private: + std::shared_ptr stabilizerTask_ = nullptr; + mc_rtc::Configuration config_; +}; + +} /* mc_samples */ diff --git a/samples/CoMTrajectoryGeneration/src/states/data/states.json b/samples/CoMTrajectoryGeneration/src/states/data/states.json new file mode 100644 index 0000000000..7a73a41bfd --- /dev/null +++ b/samples/CoMTrajectoryGeneration/src/states/data/states.json @@ -0,0 +1,2 @@ +{ +} \ No newline at end of file diff --git a/src/mc_planning/generator.cpp b/src/mc_planning/generator.cpp index 55685cda22..a4949fc26e 100644 --- a/src/mc_planning/generator.cpp +++ b/src/mc_planning/generator.cpp @@ -5,7 +5,6 @@ using namespace mc_planning::motion_interpolator; -constexpr double waist_height = 0.8; constexpr int X = 0; constexpr int Y = 1; constexpr int Z = 2; @@ -13,13 +12,14 @@ constexpr int Z = 2; namespace mc_planning { -generator::generator(int n_preview, double dt) +generator::generator(int n_preview, double dt, double mass, double waist_height) : m_ComInterp(NULL), m_Pcalpha_ideal_pre(Eigen::Vector3d::Zero()), m_Pcalpha_ideal(Eigen::Vector3d::Zero()), m_Pcalpha_cmp(Eigen::Vector3d::Zero()), m_Vcalpha_ideal_pre(Eigen::Vector3d::Zero()), m_Vcalpha_ideal(Eigen::Vector3d::Zero()), m_Vcalpha_cmp(Eigen::Vector3d::Zero()), m_Pcalpha_out(Eigen::Vector3d::Zero()), m_Pcalpha_motion_out(Eigen::Vector3d::Zero()), m_n_preview(n_preview), - m_n_steps(0), m_dt(dt), m_omega_valpha(0.0), m_mass(60.0) + m_n_steps(0), m_dt(dt), m_omega_valpha(0.0), m_mass(mass), m_waist_height(waist_height) { + mc_rtc::log::info("Creating generator with mass={}, waist_height={}", mass, waist_height); m_ComInterp = std::make_shared>(1.0, m_dt / 2); m_ipm_long[X].Initialize(m_dt, m_n_preview, 20000); @@ -50,8 +50,8 @@ void generator::setupCOGHeight(int n_current) // Ignores lateral sway during the trajectory? if(n_current == 0) { - m_ComInterp->push_back(0.0, waist_height); - m_ComInterp->push_back(lround(m_steps.back()(0) / m_dt), waist_height); + m_ComInterp->push_back(0.0, m_waist_height); + m_ComInterp->push_back(lround(m_steps.back()(0) / m_dt), m_waist_height); m_ComInterp->update(); } @@ -180,6 +180,10 @@ void generator::generateTrajectories(void) void generator::generate(int n_time) { + if(m_steps.empty()) + { + mc_rtc::log::error_and_throw("[generator] No reference provided, call setSteps()"); + } setupCOGHeight(n_time); setupTimeTrajectories(n_time); generateTrajectories(); @@ -194,4 +198,26 @@ void generator::push_back(const Eigen::Vector3d & step) m_steps.push_back(step); } -} // namespace mc_planning +void generator::addToLogger(mc_rtc::Logger & logger) +{ + // clang-format off + logger.addLogEntry("generator_IdealCOGPosition", [this]() { return this->IdealCOGPosition(); }); + logger.addLogEntry("generator_CompensatedCOGPosition", [this]() { return this->CompensatedCOGPosition(); }); + logger.addLogEntry("generator_OutputCOGPosition", [this]() { return this->OutputCOGPosition(); }); + logger.addLogEntry("generator_IdealZMPPosition", [this]() { return this->IdealZMPPosition(); }); + logger.addLogEntry("generator_CompensatedZMPPosition", [this]() { return this->CompensatedZMPPosition(); }); + logger.addLogEntry("generator_OutputZMPPosition", [this]() { return this->OutputZMPPosition(); }); + // clang-format on +} + +void generator::removeFromLogger(mc_rtc::Logger & logger) +{ + logger.removeLogEntry("generator_IdealCOGPosition"); + logger.removeLogEntry("generator_CompensatedCOGPosition"); + logger.removeLogEntry("generator_OutputCOGPosition"); + logger.removeLogEntry("generator_IdealZMPPosition"); + logger.removeLogEntry("generator_CompensatedZMPPosition"); + logger.removeLogEntry("generator_OutputZMPPosition"); +} + +} /* namespace mc_planning */ diff --git a/src/mc_robots/jvrc1.cpp b/src/mc_robots/jvrc1.cpp index 2f238bd6f1..01bb54426d 100644 --- a/src/mc_robots/jvrc1.cpp +++ b/src/mc_robots/jvrc1.cpp @@ -83,9 +83,9 @@ JVRC1RobotModule::JVRC1RobotModule(bool fixed) : RobotModule(std::string(JVRC_VA "L_KNEE", "L_ANKLE_P", "L_ANKLE_R"}; _lipmStabilizerConfig.torsoPitch = 0; _lipmStabilizerConfig.copAdmittance = Eigen::Vector2d{0.01, 0.01}; - _lipmStabilizerConfig.dcmPropGain = 5.0; - _lipmStabilizerConfig.dcmIntegralGain = 10; - _lipmStabilizerConfig.dcmDerivGain = 0.5; + _lipmStabilizerConfig.dcmPropGain = 2.0; + _lipmStabilizerConfig.dcmIntegralGain = 3.0; + _lipmStabilizerConfig.dcmDerivGain = 0.0; _lipmStabilizerConfig.dcmDerivatorTimeConstant = 1; _lipmStabilizerConfig.dcmIntegratorTimeConstant = 10; } diff --git a/tests/hmc/testCoMGenerator.cpp b/tests/hmc/testCoMGenerator.cpp index 409eec4654..e92e9266ed 100644 --- a/tests/hmc/testCoMGenerator.cpp +++ b/tests/hmc/testCoMGenerator.cpp @@ -38,13 +38,8 @@ int main(void) mc_rtc::io::to_string(steps, [](const Eigen::Vector3d & v) -> Eigen::RowVector3d { return v; }, "\n")); mc_rtc::Logger logger(mc_rtc::Logger::Policy::NON_THREADED, "/tmp", "mc_rtc-test"); - logger.addLogEntry("IdealCOGPosition", [&com_traj]() { return com_traj.IdealCOGPosition(); }); - logger.addLogEntry("CompensatedCOGPosition", [&com_traj]() { return com_traj.CompensatedCOGPosition(); }); - logger.addLogEntry("OutputCOGPosition", [&com_traj]() { return com_traj.OutputCOGPosition(); }); - logger.addLogEntry("IdealZMPPosition", [&com_traj]() { return com_traj.IdealZMPPosition(); }); - logger.addLogEntry("CompensatedZMPPosition", [&com_traj]() { return com_traj.CompensatedZMPPosition(); }); - logger.addLogEntry("OutputZMPPosition", [&com_traj]() { return com_traj.OutputZMPPosition(); }); logger.start("CoMGenerator", dt); + com_traj.addToLogger(logger); int n_loop = lround(com_traj.Steps().back()(0) / dt) - n_preview; /* @@ -57,8 +52,9 @@ int main(void) logger.log(); } }; + com_traj.removeFromLogger(logger); - auto duration = mc_rtc::measure_ms::duration(comGeneration).count(); + auto duration = mc_rtc::measure_ms::execution(comGeneration).count(); mc_rtc::log::info("calc time = {} (ms)", duration); mc_rtc::log::info("ave. calc time = {} (ms)", duration / static_cast(n_loop)); mc_rtc::log::success("end of com trajectory generation"); From a03ee7a90dc66b2035fb50b0481334672cf48a8a Mon Sep 17 00:00:00 2001 From: Arnaud TANGUY Date: Thu, 18 Jun 2020 16:05:26 +0900 Subject: [PATCH 18/43] [StabilizerTask] Log reference and stabilized targets --- .../mc_tasks/lipm_stabilizer/StabilizerTask.h | 30 +++++++++++++++++-- .../src/CoMTrajectoryGeneration.cpp | 7 +++++ .../src/states/StabilizerStandingTrackCoM.cpp | 23 ++++++++++++-- .../lipm_stabilizer/StabilizerTask.cpp | 17 +++++++++-- .../StabilizerTask_log_gui.cpp | 26 +++++++++------- 5 files changed, 86 insertions(+), 17 deletions(-) diff --git a/include/mc_tasks/lipm_stabilizer/StabilizerTask.h b/include/mc_tasks/lipm_stabilizer/StabilizerTask.h index 8f83e5114e..66e821bb4e 100644 --- a/include/mc_tasks/lipm_stabilizer/StabilizerTask.h +++ b/include/mc_tasks/lipm_stabilizer/StabilizerTask.h @@ -805,7 +805,14 @@ struct MC_TASKS_DLLAPI StabilizerTask : public MetaTask const mc_rbdyn::Robots & realRobots_; unsigned int robotIndex_; - /** Stabilizer targets */ + /** + * @name Computed stabilized targets + * + * - comTarget_, comdTarget_ and comddTarget_ are modified by ZMPCC + * - zmpTarget_ is the result of wrench distribution + * - dcmTarget_ is computed from the above values + * @{ + **/ Eigen::Vector3d comTargetRaw_ = Eigen::Vector3d::Zero(); Eigen::Vector3d comTarget_ = Eigen::Vector3d::Zero(); Eigen::Vector3d comdTarget_ = Eigen::Vector3d::Zero(); @@ -813,8 +820,24 @@ struct MC_TASKS_DLLAPI StabilizerTask : public MetaTask Eigen::Vector3d zmpTarget_ = Eigen::Vector3d::Zero(); Eigen::Vector3d zmpdTarget_ = Eigen::Vector3d::Zero(); Eigen::Vector3d dcmTarget_ = Eigen::Vector3d::Zero(); - double omega_; + /// @} + + /** + * @name Stabilizer desired reference (only used for logging) + * + * These values are set by calling target() or staticTarget(). + * @note This reference must follow a dynamically consistent trajectory + * @{ + */ + Eigen::Vector3d comRef_ = Eigen::Vector3d::Zero(); ///< Desired CoM position + Eigen::Vector3d comdRef_ = Eigen::Vector3d::Zero(); ///< Desired CoM velocity + Eigen::Vector3d comddRef_ = Eigen::Vector3d::Zero(); ///< Desired CoM acceleration + Eigen::Vector3d zmpRef_ = Eigen::Vector3d::Zero(); ///< Desired ZMP target + Eigen::Vector3d dcmRef_ = Eigen::Vector3d::Zero(); ///< Desired DCM + ///@} + + double omega_ = 0.; ///< Pendulum frequency double t_ = 0.; /**< Time elapsed since the task is running */ protected: @@ -841,6 +864,9 @@ struct MC_TASKS_DLLAPI StabilizerTask : public MetaTask Eigen::Vector3d measuredDCMUnbiased_ = Eigen::Vector3d::Zero(); /// DCM unbiased (only used for logging) sva::ForceVecd measuredNetWrench_ = sva::ForceVecd::Zero(); + Eigen::Vector3d desiredZMP_ = Eigen::Vector3d::Zero(); ///< Desired ZMP based on DCM error + Eigen::Vector3d distributedZMP_ = Eigen::Vector3d::Zero(); ///< Distributed desired ZMP (target) + bool zmpccOnlyDS_ = true; /**< Only apply ZMPCC in double support */ ZMPCC zmpcc_; /**< Compute CoM offset due to ZMPCC compensation */ diff --git a/samples/CoMTrajectoryGeneration/src/CoMTrajectoryGeneration.cpp b/samples/CoMTrajectoryGeneration/src/CoMTrajectoryGeneration.cpp index 07a202a695..bce9cccb55 100644 --- a/samples/CoMTrajectoryGeneration/src/CoMTrajectoryGeneration.cpp +++ b/samples/CoMTrajectoryGeneration/src/CoMTrajectoryGeneration.cpp @@ -15,6 +15,13 @@ bool CoMTrajectoryGeneration::run() void CoMTrajectoryGeneration::reset(const mc_control::ControllerResetData & reset_data) { mc_control::fsm::Controller::reset(reset_data); + datastore().make_call("KinematicAnchorFrame::" + robot().name(), + [](const mc_rbdyn::Robot & robot) + { + return sva::interpolate(robot.surfacePose("LeftFootCenter"), + robot.surfacePose("RightFootCenter"), + 0.5); + }); } diff --git a/samples/CoMTrajectoryGeneration/src/states/StabilizerStandingTrackCoM.cpp b/samples/CoMTrajectoryGeneration/src/states/StabilizerStandingTrackCoM.cpp index cbcca605a1..9384b2f417 100644 --- a/samples/CoMTrajectoryGeneration/src/states/StabilizerStandingTrackCoM.cpp +++ b/samples/CoMTrajectoryGeneration/src/states/StabilizerStandingTrackCoM.cpp @@ -39,8 +39,12 @@ void StabilizerStandingTrackCoM::start(mc_control::fsm::Controller & ctl) bool StabilizerStandingTrackCoM::run(mc_control::fsm::Controller & ctl) { // Update anchor frame for the KinematicInertial observer - ctl.anchorFrame(stabilizerTask_->anchorFrame()); - ctl.anchorFrameReal(stabilizerTask_->anchorFrameReal()); + ctl.datastore().remove("KinematicAnchorFrame::" + ctl.robot().name()); + ctl.datastore().make_call("KinematicAnchorFrame::" + ctl.robot().name(), + [this](const mc_rbdyn::Robot & robot) + { + return stabilizerTask_->anchorFrame(robot); + }); output("OK"); return false; } @@ -48,6 +52,21 @@ bool StabilizerStandingTrackCoM::run(mc_control::fsm::Controller & ctl) void StabilizerStandingTrackCoM::teardown(mc_control::fsm::Controller & ctl) { ctl.datastore().remove("StabilizerStandingTrackCoM::target"); + + ctl.datastore().remove("KinematicAnchorFrame::" + ctl.robot().name()); + + // Ensure that the anchor frame remains continuous even after this state has + // been destroyed + double leftFootRatio = stabilizerTask_->leftFootRatio(); + std::string leftSurface = stabilizerTask_->footSurface(mc_tasks::lipm_stabilizer::ContactState::Left); + std::string rightSurface = stabilizerTask_->footSurface(mc_tasks::lipm_stabilizer::ContactState::Right); + ctl.datastore().make_call("KinematicAnchorFrame::" + ctl.robot().name(), + [leftFootRatio, leftSurface, rightSurface](const mc_rbdyn::Robot & robot) + { + return sva::interpolate(robot.surfacePose(leftSurface), + robot.surfacePose(rightSurface), + leftFootRatio); + }); } } /* mc_samples */ diff --git a/src/mc_tasks/lipm_stabilizer/StabilizerTask.cpp b/src/mc_tasks/lipm_stabilizer/StabilizerTask.cpp index 60d9869eec..aa71666a41 100644 --- a/src/mc_tasks/lipm_stabilizer/StabilizerTask.cpp +++ b/src/mc_tasks/lipm_stabilizer/StabilizerTask.cpp @@ -484,6 +484,7 @@ void StabilizerTask::setContacts(const ContactDescriptionVector & contacts) "requires at least one contact to be set."); } contacts_.clear(); + addContacts_.clear(); // Reset support area boundaries supportMin_ = std::numeric_limits::max() * Eigen::Vector2d::Ones(); @@ -619,6 +620,13 @@ void StabilizerTask::target(const Eigen::Vector3d & com, double comHeight = comTarget_.z() - zmpTarget_.z(); omega_ = std::sqrt(constants::gravity.z() / comHeight); dcmTarget_ = comTarget_ + comdTarget_ / omega_; + + // Only stored for logging purposes + comRef_ = com; + comdRef_ = comd; + comddRef_ = comdd; + zmpRef_ = zmp; + dcmRef_ = dcmTarget_; } void StabilizerTask::setExternalWrenches(const std::vector & surfaceNames, @@ -758,8 +766,8 @@ void StabilizerTask::run() saturateWrench(desiredWrench_, footTasks[ContactState::Right], contacts_.at(ContactState::Right)); footTasks[ContactState::Left]->setZeroTargetWrench(); } - - distribZMP_ = mc_rbdyn::zmp(distribWrench_, zmpFrame_); + // Distributed ZMP + zmpTarget_ = mc_rbdyn::zmp(desiredWrench_, zmpFrame_, c_.safetyThresholds.MIN_NET_TOTAL_FORCE_ZMP); updateCoMTaskZMPCC(); updateFootForceDifferenceControl(); @@ -1102,9 +1110,12 @@ void StabilizerTask::updateCoMTaskZMPCC() { zmpcc_.configure(c_.zmpcc); zmpcc_.enabled(enabled_); - zmpcc_.update(distribZMP_, measuredZMP_, zmpFrame_, dt_); + // Distributed ZMP + zmpcc_.update(zmpTarget_, measuredZMP_, zmpFrame_, dt_); } zmpcc_.apply(comTarget_, comdTarget_, comddTarget_); + // Update the DCM target based on ZMPCC result + dcmTarget_ = comTarget_ + comdTarget_ / omega_; } void StabilizerTask::updateFootForceDifferenceControl() diff --git a/src/mc_tasks/lipm_stabilizer/StabilizerTask_log_gui.cpp b/src/mc_tasks/lipm_stabilizer/StabilizerTask_log_gui.cpp index 92943f6bb2..37f9159153 100644 --- a/src/mc_tasks/lipm_stabilizer/StabilizerTask_log_gui.cpp +++ b/src/mc_tasks/lipm_stabilizer/StabilizerTask_log_gui.cpp @@ -371,16 +371,22 @@ void StabilizerTask::addToLogger(mc_rtc::Logger & logger) MC_RTC_LOG_HELPER(name_ + "_left_foot_ratio", leftFootRatio_); // Stabilizer targets - MC_RTC_LOG_HELPER(name_ + "_target_pendulum_com", comTarget_); - MC_RTC_LOG_HELPER(name_ + "_target_pendulum_comd", comdTarget_); - MC_RTC_LOG_HELPER(name_ + "_target_pendulum_comdd", comddTarget_); - MC_RTC_LOG_HELPER(name_ + "_target_pendulum_dcm", dcmTarget_); - MC_RTC_LOG_HELPER(name_ + "_target_pendulum_omega", omega_); - MC_RTC_LOG_HELPER(name_ + "_target_pendulum_zmp", zmpTarget_); - MC_RTC_LOG_HELPER(name_ + "_target_pendulum_zmpd", zmpdTarget_); - MC_RTC_LOG_HELPER(name_ + "_target_stabilizer_zmp", distribZMP_); - - logger.addLogEntry(name_ + "_contactState", this, [this]() -> int { + MC_RTC_LOG_HELPER(name_ + "_reference_com", comRef_); + MC_RTC_LOG_HELPER(name_ + "_reference_comd", comdRef_); + MC_RTC_LOG_HELPER(name_ + "_reference_comdd", comddRef_); + MC_RTC_LOG_HELPER(name_ + "_reference_dcm", dcmRef_); + MC_RTC_LOG_HELPER(name_ + "_reference_omega", omega_); + MC_RTC_LOG_HELPER(name_ + "_reference_zmp", zmpRef_); + + // Stabilizer results + MC_RTC_LOG_HELPER(name_ + "_stabilized_com", comTarget_); + MC_RTC_LOG_HELPER(name_ + "_stabilized_comd", comdTarget_); + MC_RTC_LOG_HELPER(name_ + "_stabilized_comdd", comddTarget_); + MC_RTC_LOG_HELPER(name_ + "_stabilized_dcm", dcmTarget_); + MC_RTC_LOG_HELPER(name_ + "_stabilized_zmp", zmpTarget_); + + logger.addLogEntry(name_ + "_contactState", this, + [this]() -> double { if(inDoubleSupport()) return 0; else if(inContact(ContactState::Left)) From a2da9648cd50dd7e1e8a3568357c8e9de0be9fd9 Mon Sep 17 00:00:00 2001 From: Arnaud TANGUY Date: Thu, 18 Jun 2020 16:28:06 +0900 Subject: [PATCH 19/43] [StabilizerTask] Fix custom plots --- .../LIPMStabilizer/etc/custom_plot.json | 44 +++++++++---------- 1 file changed, 22 insertions(+), 22 deletions(-) diff --git a/src/mc_control/samples/LIPMStabilizer/etc/custom_plot.json b/src/mc_control/samples/LIPMStabilizer/etc/custom_plot.json index 754445140c..794252b140 100644 --- a/src/mc_control/samples/LIPMStabilizer/etc/custom_plot.json +++ b/src/mc_control/samples/LIPMStabilizer/etc/custom_plot.json @@ -1,24 +1,24 @@ [ - ["3D CoM", "t", ["lipm_stabilizer_jvrc1_target_pendulum_com_x", "lipm_stabilizer_jvrc1_controlRobot_com_x", "lipm_stabilizer_jvrc1_realRobot_com_x", "lipm_stabilizer_jvrc1_target_pendulum_com_y", "lipm_stabilizer_jvrc1_controlRobot_com_y", "lipm_stabilizer_jvrc1_realRobot_com_y", "lipm_stabilizer_jvrc1_target_pendulum_com_z", "lipm_stabilizer_jvrc1_controlRobot_com_z", "lipm_stabilizer_jvrc1_realRobot_com_z"], [], [], []], - ["3D DCM", "t", ["lipm_stabilizer_jvrc1_target_pendulum_dcm_x", "lipm_stabilizer_jvrc1_realRobot_dcm_x", "lipm_stabilizer_jvrc1_target_pendulum_dcm_y", "lipm_stabilizer_jvrc1_realRobot_dcm_y"], [], [], []], - ["Foot pressure both", "t", ["StabilizerTasks_cop_left_measured_wrench_fz", "StabilizerTasks_cop_left_target_wrench_fz", "StabilizerTasks_cop_right_measured_wrench_fz", "StabilizerTasks_cop_right_target_wrench_fz"], [], [], []], - ["Foot pressure left", "t", ["StabilizerTasks_cop_left_measured_wrench_fz", "StabilizerTasks_cop_left_target_wrench_fz"], [], [], []], - ["Foot pressure right", "t", ["StabilizerTasks_cop_right_measured_wrench_fz", "StabilizerTasks_cop_right_target_wrench_fz"], [], [], []], - ["Measured CoM-ZMP X", "t", ["lipm_stabilizer_jvrc1_realRobot_zmp_x", "lipm_stabilizer_jvrc1_support_min_x", "lipm_stabilizer_jvrc1_support_max_x", "lipm_stabilizer_jvrc1_realRobot_com_x"], [], [], []], - ["Measured CoM-ZMP Y", "t", ["lipm_stabilizer_jvrc1_realRobot_zmp_y", "lipm_stabilizer_jvrc1_support_min_y", "lipm_stabilizer_jvrc1_support_max_y", "lipm_stabilizer_jvrc1_realRobot_com_y"], [], [], []], - ["Measured DCM-ZMP X", "t", ["lipm_stabilizer_jvrc1_support_min_x", "lipm_stabilizer_jvrc1_support_max_x", "lipm_stabilizer_jvrc1_realRobot_dcm_x", "lipm_stabilizer_jvrc1_realRobot_zmp_x"], [], [], []], - ["Measured DCM-ZMP Y", "t", ["lipm_stabilizer_jvrc1_support_min_y", "lipm_stabilizer_jvrc1_support_max_y", "lipm_stabilizer_jvrc1_realRobot_dcm_y", "lipm_stabilizer_jvrc1_realRobot_zmp_y"], [], [], []], - ["Reference CoM-ZMP X", "t", ["lipm_stabilizer_jvrc1_target_pendulum_zmp_x", "lipm_stabilizer_jvrc1_support_min_x", "lipm_stabilizer_jvrc1_support_max_x", "lipm_stabilizer_jvrc1_target_pendulum_com_x"], [], [], []], - ["Reference CoM-ZMP Y", "t", ["lipm_stabilizer_jvrc1_target_pendulum_zmp_y", "lipm_stabilizer_jvrc1_support_min_y", "lipm_stabilizer_jvrc1_support_max_y", "lipm_stabilizer_jvrc1_target_pendulum_com_y"], [], [], []], - ["Reference DCM-ZMP X", "t", ["lipm_stabilizer_jvrc1_support_min_x", "lipm_stabilizer_jvrc1_support_max_x", "lipm_stabilizer_jvrc1_target_pendulum_dcm_x", "lipm_stabilizer_jvrc1_target_pendulum_zmp_x"], [], [], []], - ["Reference DCM-ZMP Y", "t", ["lipm_stabilizer_jvrc1_support_min_y", "lipm_stabilizer_jvrc1_support_max_y", "lipm_stabilizer_jvrc1_target_pendulum_dcm_y", "lipm_stabilizer_jvrc1_target_pendulum_zmp_y"], [], [], []], - ["Tracking CoM X", "t", ["lipm_stabilizer_jvrc1_target_pendulum_com_x", "lipm_stabilizer_jvrc1_controlRobot_com_x", "lipm_stabilizer_jvrc1_realRobot_com_x"], [], [], []], - ["Tracking CoM Y", "t", ["lipm_stabilizer_jvrc1_target_pendulum_com_y", "lipm_stabilizer_jvrc1_controlRobot_com_y", "lipm_stabilizer_jvrc1_realRobot_com_y"], [], [], []], - ["Tracking CoM Z", "t", ["lipm_stabilizer_jvrc1_target_pendulum_com_z", "lipm_stabilizer_jvrc1_controlRobot_com_z", "lipm_stabilizer_jvrc1_realRobot_com_z"], [], [], []], - ["Tracking DCM X", "t", ["lipm_stabilizer_jvrc1_target_pendulum_dcm_x", "lipm_stabilizer_jvrc1_realRobot_dcm_x"], [], [], []], - ["Tracking DCM Y", "t", ["lipm_stabilizer_jvrc1_target_pendulum_dcm_y", "lipm_stabilizer_jvrc1_realRobot_dcm_y"], [], [], []], - ["Tracking DCM-ZMP X", "t", ["lipm_stabilizer_jvrc1_support_min_x", "lipm_stabilizer_jvrc1_support_max_x", "lipm_stabilizer_jvrc1_target_pendulum_dcm_x", "lipm_stabilizer_jvrc1_realRobot_dcm_x", "lipm_stabilizer_jvrc1_target_pendulum_zmp_x", "lipm_stabilizer_jvrc1_realRobot_zmp_x"], [], [], []], - ["Tracking DCM-ZMP Y", "t", ["lipm_stabilizer_jvrc1_support_min_y", "lipm_stabilizer_jvrc1_support_max_y", "lipm_stabilizer_jvrc1_target_pendulum_dcm_y", "lipm_stabilizer_jvrc1_realRobot_dcm_y", "lipm_stabilizer_jvrc1_target_pendulum_zmp_y", "lipm_stabilizer_jvrc1_realRobot_zmp_y"], [], [], []], - ["Tracking ZMP X", "t", ["lipm_stabilizer_jvrc1_target_pendulum_zmp_x", "lipm_stabilizer_jvrc1_realRobot_zmp_x"], [], [], []], - ["Tracking ZMP Y", "t", ["lipm_stabilizer_jvrc1_target_pendulum_zmp_y", "lipm_stabilizer_jvrc1_realRobot_zmp_y"], [], [], []] + ["3D CoM", "t", ["lipm_stabilizer_jvrc1_stabilized_com_x", "lipm_stabilizer_jvrc1_robotControl_com_x", "lipm_stabilizer_jvrc1_robotReal_com_x", "lipm_stabilizer_jvrc1_stabilized_com_y", "lipm_stabilizer_jvrc1_robotControl_com_y", "lipm_stabilizer_jvrc1_robotReal_com_y", "lipm_stabilizer_jvrc1_stabilized_com_z", "lipm_stabilizer_jvrc1_robotControl_com_z", "lipm_stabilizer_jvrc1_robotReal_com_z"], [], [], []], + ["3D DCM", "t", ["lipm_stabilizer_jvrc1_stabilized_dcm_x", "lipm_stabilizer_jvrc1_robotReal_dcm_x", "lipm_stabilizer_jvrc1_stabilized_dcm_y", "lipm_stabilizer_jvrc1_robotReal_dcm_y"], [], [], []], + ["Foot pressure both", "t", ["lipm_stabilizer_jvrc1_tasks_cop_left_measured_wrench_fz", "lipm_stabilizer_jvrc1_tasks_cop_left_target_wrench_fz", "lipm_stabilizer_jvrc1_tasks_cop_right_measured_wrench_fz", "lipm_stabilizer_jvrc1_tasks_cop_right_target_wrench_fz"], [], [], []], + ["Foot pressure left", "t", ["lipm_stabilizer_jvrc1_tasks_cop_left_measured_wrench_fz", "lipm_stabilizer_jvrc1_tasks_cop_left_target_wrench_fz"], [], [], []], + ["Foot pressure right", "t", ["lipm_stabilizer_jvrc1_tasks_cop_right_measured_wrench_fz", "lipm_stabilizer_jvrc1_tasks_cop_right_target_wrench_fz"], [], [], []], + ["Measured CoM-ZMP X", "t", ["lipm_stabilizer_jvrc1_robotReal_zmp_x", "lipm_stabilizer_jvrc1_support_min_x", "lipm_stabilizer_jvrc1_support_max_x", "lipm_stabilizer_jvrc1_robotReal_com_x"], [], [], []], + ["Measured CoM-ZMP Y", "t", ["lipm_stabilizer_jvrc1_robotReal_zmp_y", "lipm_stabilizer_jvrc1_support_min_y", "lipm_stabilizer_jvrc1_support_max_y", "lipm_stabilizer_jvrc1_robotReal_com_y"], [], [], []], + ["Measured DCM-ZMP X", "t", ["lipm_stabilizer_jvrc1_support_min_x", "lipm_stabilizer_jvrc1_support_max_x", "lipm_stabilizer_jvrc1_robotReal_dcm_x", "lipm_stabilizer_jvrc1_robotReal_zmp_x"], [], [], []], + ["Measured DCM-ZMP Y", "t", ["lipm_stabilizer_jvrc1_support_min_y", "lipm_stabilizer_jvrc1_support_max_y", "lipm_stabilizer_jvrc1_robotReal_dcm_y", "lipm_stabilizer_jvrc1_robotReal_zmp_y"], [], [], []], + ["Reference CoM-ZMP X", "t", ["lipm_stabilizer_jvrc1_stabilized_zmp_x", "lipm_stabilizer_jvrc1_support_min_x", "lipm_stabilizer_jvrc1_support_max_x", "lipm_stabilizer_jvrc1_stabilized_com_x"], [], [], []], + ["Reference CoM-ZMP Y", "t", ["lipm_stabilizer_jvrc1_stabilized_zmp_y", "lipm_stabilizer_jvrc1_support_min_y", "lipm_stabilizer_jvrc1_support_max_y", "lipm_stabilizer_jvrc1_stabilized_com_y"], [], [], []], + ["Reference DCM-ZMP X", "t", ["lipm_stabilizer_jvrc1_support_min_x", "lipm_stabilizer_jvrc1_support_max_x", "lipm_stabilizer_jvrc1_stabilized_dcm_x", "lipm_stabilizer_jvrc1_stabilized_zmp_x"], [], [], []], + ["Reference DCM-ZMP Y", "t", ["lipm_stabilizer_jvrc1_support_min_y", "lipm_stabilizer_jvrc1_support_max_y", "lipm_stabilizer_jvrc1_stabilized_dcm_y", "lipm_stabilizer_jvrc1_stabilized_zmp_y"], [], [], []], + ["Tracking CoM X", "t", ["lipm_stabilizer_jvrc1_stabilized_com_x", "lipm_stabilizer_jvrc1_robotControl_com_x", "lipm_stabilizer_jvrc1_robotReal_com_x"], [], [], []], + ["Tracking CoM Y", "t", ["lipm_stabilizer_jvrc1_stabilized_com_y", "lipm_stabilizer_jvrc1_robotControl_com_y", "lipm_stabilizer_jvrc1_robotReal_com_y"], [], [], []], + ["Tracking CoM Z", "t", ["lipm_stabilizer_jvrc1_stabilized_com_z", "lipm_stabilizer_jvrc1_robotControl_com_z", "lipm_stabilizer_jvrc1_robotReal_com_z"], [], [], []], + ["Tracking DCM X", "t", ["lipm_stabilizer_jvrc1_stabilized_dcm_x", "lipm_stabilizer_jvrc1_robotReal_dcm_x"], [], [], []], + ["Tracking DCM Y", "t", ["lipm_stabilizer_jvrc1_stabilized_dcm_y", "lipm_stabilizer_jvrc1_robotReal_dcm_y"], [], [], []], + ["Tracking DCM-ZMP X", "t", ["lipm_stabilizer_jvrc1_support_min_x", "lipm_stabilizer_jvrc1_support_max_x", "lipm_stabilizer_jvrc1_stabilized_dcm_x", "lipm_stabilizer_jvrc1_robotReal_dcm_x", "lipm_stabilizer_jvrc1_stabilized_zmp_x", "lipm_stabilizer_jvrc1_robotReal_zmp_x"], [], [], []], + ["Tracking DCM-ZMP Y", "t", ["lipm_stabilizer_jvrc1_support_min_y", "lipm_stabilizer_jvrc1_support_max_y", "lipm_stabilizer_jvrc1_stabilized_dcm_y", "lipm_stabilizer_jvrc1_robotReal_dcm_y", "lipm_stabilizer_jvrc1_stabilized_zmp_y", "lipm_stabilizer_jvrc1_robotReal_zmp_y"], [], [], []], + ["Tracking ZMP X", "t", ["lipm_stabilizer_jvrc1_stabilized_zmp_x", "lipm_stabilizer_jvrc1_robotReal_zmp_x"], [], [], []], + ["Tracking ZMP Y", "t", ["lipm_stabilizer_jvrc1_stabilized_zmp_y", "lipm_stabilizer_jvrc1_robotReal_zmp_y"], [], [], []] ] From b5d713cf00be583901f377b795cbb476a4cf6e97 Mon Sep 17 00:00:00 2001 From: Arnaud TANGUY Date: Fri, 19 Jun 2020 11:29:27 +0900 Subject: [PATCH 20/43] [mc_log_ui] Display error when the log file is invalid If a log file contains one column or less, mc_log_ui would crash as it expects more entries to be available. This commit displays an error message instead. --- utils/mc_log_gui/mc_log_ui/mc_log_ui.py | 26 ++++++++++++++++++++++--- 1 file changed, 23 insertions(+), 3 deletions(-) diff --git a/utils/mc_log_gui/mc_log_ui/mc_log_ui.py b/utils/mc_log_gui/mc_log_ui/mc_log_ui.py index 4221964ee7..5a20b8bde2 100644 --- a/utils/mc_log_gui/mc_log_ui/mc_log_ui.py +++ b/utils/mc_log_gui/mc_log_ui/mc_log_ui.py @@ -1034,13 +1034,33 @@ def load_csv(self, fpath, clear = True): self.loaded_files = [] data = read_log(fpath) if not 't' in data: - print("This GUI assumes a time-entry named t is available in the log, failed loading {}".format(fpath)) - return + error = "This GUI assumes a time-entry named t is available in the log, failed loading {}".format(fpath) + if self.isVisible(): + err_diag = QtWidgets.QMessageBox(self) + err_diag.setModal(True) + err_diag.setText(error) + err_diag.exec_() + return + else: + print(error) + self.close() + exit(1) if 't' in self.data and not clear: dt = self.data['t'][1] - self.data['t'][0] ndt = data['t'][1] - data['t'][0] if abs(dt - ndt) > 1e-9: - print("This GUI assumes you are comparing logs with a similar timestep, already loaded dt = {} but attempted to load dt = {} from {}", dt, ndt, fpath) + print() + error = "This GUI assumes you are comparing logs with a similar timestep, already loaded dt = {} but attempted to load dt = {} from {}".format(dt, ndt, fpath) + if self.isVisible(): + err_diag = QtWidgets.QMessageBox(self) + err_diag.setModal(True) + err_diag.setText(error) + err_diag.exec_() + return + else: + print(error) + self.close() + exit(1) return pad_left = int(round((self.data['t'][0] - data['t'][0]) / dt)) pad_right = int(round((data['t'][-1] - self.data['t'][-1]) / dt)) From 0320d7fe427902d6dc7a17b2a1845f73b4e35099 Mon Sep 17 00:00:00 2001 From: Arnaud TANGUY Date: Fri, 19 Jun 2020 13:04:07 +0900 Subject: [PATCH 21/43] [HMC/generator] Cleanup, use unsigned values --- .../LinearTimeVariantInvertedPendulum.h | 4 +- include/mc_planning/generator.h | 93 +++++++++++++++---- .../CoMTrajectoryGeneration_Initial.cpp | 8 +- .../LinearTimeVariantInvertedPendulum.cpp | 6 +- src/mc_planning/generator.cpp | 41 ++++---- tests/hmc/testCoMGenerator.cpp | 15 ++- 6 files changed, 108 insertions(+), 59 deletions(-) diff --git a/include/mc_planning/LinearTimeVariantInvertedPendulum.h b/include/mc_planning/LinearTimeVariantInvertedPendulum.h index 7b917cfcad..1fba7c5162 100644 --- a/include/mc_planning/LinearTimeVariantInvertedPendulum.h +++ b/include/mc_planning/LinearTimeVariantInvertedPendulum.h @@ -157,7 +157,7 @@ Pre-computed table from . /*! @brief Get generated CoG/ZMP states * @param n_time is the time in range of[-n_preview:n_preview], current time is 0 */ - State getState(int n_time) const; + State getState(unsigned n_time) const; /*! @brief to get generated CoG/ZMP states * @param[in] n_time is the time in range of[-n_preview:n_preview], current time is 0 @@ -167,7 +167,7 @@ Pre-computed table from . * @param[in] p is position of ZMP * @param[in] pdot is velocity of ZMP */ - void getState(int n_time, double & cog_pos, double & cog_vel, double & cog_acc, double & p, double & pdot); + void getState(unsigned n_time, double & cog_pos, double & cog_vel, double & cog_acc, double & p, double & pdot); /** * Generate full trajectory (only used for tests) diff --git a/include/mc_planning/generator.h b/include/mc_planning/generator.h index a5e3d555c8..f31eac2bf2 100644 --- a/include/mc_planning/generator.h +++ b/include/mc_planning/generator.h @@ -17,6 +17,10 @@ namespace mc_planning * Generate trajectories as introduced in: * **Online 3D CoM Trajectory Generation for Multi-Contact Locomotion Synchronizing Contact**, *Mitsuharu Morisawa et * al., IROS 2018* + * + * @see mc_planning::linear_control_system::LinearTimeVariantInvertedPendulum long-term trajectory generation + * @see mc_planning::linear_control_system::LIPMControlByPoleAssignWithExternalForce short-term trajectory generation to + * continuously track the long-term trajectory */ struct MC_PLANNING_DLLAPI generator { @@ -29,14 +33,14 @@ struct MC_PLANNING_DLLAPI generator * @mass Robot mass * @waist_height Initial height of the waist (constant for now) */ - generator(int n_preview, double dt, double mass = 60, double waist_height = 0.8); + generator(unsigned n_preview, double dt, double mass = 60, double waist_height = 0.8); /** * @brief Generate the long and short term trajectories * * @param n_time current time */ - void generate(int n_time); + void generate(unsigned n_time); /** * @brief Add to Logger @@ -51,7 +55,7 @@ struct MC_PLANNING_DLLAPI generator * @brief Desired steps * @see setSteps(const std::vector & steps) */ - const std::vector & Steps() const + const std::vector & steps() const { return m_steps; } @@ -61,7 +65,7 @@ struct MC_PLANNING_DLLAPI generator * * @param steps Desired steps from which the trajectory will be generated */ - void setSteps(const std::vector & steps) + void steps(const std::vector & steps) { m_steps = steps; m_n_steps = 0; @@ -80,6 +84,43 @@ struct MC_PLANNING_DLLAPI generator * @name Accessors */ ///@{ + + /** + * @brief Sets the poles for the short-term trajectory + * + * Typical values range from \f$ [1,1,150] \f$ to \f$ [1,1,300] \f$. + * The bigger the poles are, the more the faster the short-term trajectory + * will converge to the long-term trajectory. Too high a value could lead to + * jerk when the long-term trajectory is not continuous + * + * @param poles Poles for the short-term trajectory + */ + void polesX(const Eigen::Vector3d & poles) + { + m_poles[0] = poles; + } + + /** @brief Gets the short-term trajectory poles */ + const Eigen::Vector3d & polesX() const + { + return m_poles[0]; + } + + /** + * @brief Sets the poles for the short-term trajectory + * + * @see polesX(const Eigen::Vector3d & poles) + */ + void polesY(const Eigen::Vector3d & poles) + { + m_poles[0] = poles; + } + + /** @brief Gets the short-term trajectory poles */ + const Eigen::Vector3d & polesY() const + { + return m_poles[0]; + } const Eigen::Vector3d & IdealCOGPosition() const { return m_COG_ideal.P; @@ -152,7 +193,7 @@ struct MC_PLANNING_DLLAPI generator * * @param n_current Index of the start of the preview window */ - void setupCOGHeight(int n_current); + void setupCOGHeight(unsigned n_current); /** * @brief Setup reference trajectories based on the desired steps @@ -161,7 +202,7 @@ struct MC_PLANNING_DLLAPI generator * * @param n_current Index of the start of the preview window */ - void setupTimeTrajectories(int n_current); + void setupTimeTrajectories(unsigned n_current); /** * @brief Solve long and short term trajectories from the reference time @@ -172,7 +213,7 @@ struct MC_PLANNING_DLLAPI generator void generateTrajectories(); private: - std::shared_ptr> m_ComInterp; + std::shared_ptr> m_ComInterp = nullptr; StatePVA m_COG_ideal_pre; /** @@ -184,23 +225,35 @@ struct MC_PLANNING_DLLAPI generator StatePVA m_COG_cmp; StatePVA m_COG_out; - Eigen::Vector3d m_Pcalpha_ideal_pre, m_Pcalpha_ideal, m_Pcalpha_cmp; - Eigen::Vector3d m_Vcalpha_ideal_pre, m_Vcalpha_ideal, m_Vcalpha_cmp; - Eigen::Vector3d m_Pcalpha_out; - Eigen::Vector3d m_Pcalpha_motion_out; + /** + * @name ZMP-related + * @{ + */ + Eigen::Vector3d m_Pcalpha_ideal_pre = Eigen::Vector3d::Zero(); + Eigen::Vector3d m_Pcalpha_ideal = Eigen::Vector3d::Zero(); + Eigen::Vector3d m_Pcalpha_cmp = Eigen::Vector3d::Zero(); + Eigen::Vector3d m_Vcalpha_ideal_pre = Eigen::Vector3d::Zero(); + Eigen::Vector3d m_Vcalpha_ideal = Eigen::Vector3d::Zero(); + Eigen::Vector3d m_Vcalpha_cmp = Eigen::Vector3d::Zero(); + Eigen::Vector3d m_Pcalpha_out = Eigen::Vector3d::Zero(); + Eigen::Vector3d m_Pcalpha_motion_out = Eigen::Vector3d::Zero(); + /// @} - int m_n_preview; ///< Size of the future preview window. The full preview is 2*m_n_preview+1 - int m_n_steps; ///< Current step - double m_dt; ///< Timestep - double m_omega_valpha; - double m_mass; ///< Robot mass + unsigned m_n_preview = 0; ///< Size of the future preview window. The full preview is 2*m_n_preview+1 + unsigned m_n_steps = 0; ///< Current step + double m_dt = 0.005; ///< Timestep + double m_omega_valpha = 0.0; + double m_mass = 60.; ///< Robot mass double m_waist_height; ///< Height of the weight (constant for now) - linear_control_system::LinearTimeVariantInvertedPendulum m_ipm_long[2]; - linear_control_system::LIPMControlByPoleAssignWithExternalForce m_ipm_short[2]; - Eigen::Vector3d m_poles[2]; + std::array m_ipm_long; + std::array m_ipm_short; + /**< Poles for the short-term trajectory. + * Typical range [1,1,150] ... [1,1,300] + */ + std::array m_poles = {Eigen::Vector3d{1., 1., 150.}, Eigen::Vector3d{1., 1., 150.}}; - Eigen::VectorXd m_virtual_height[2]; + std::array m_virtual_height; Eigen::VectorXd m_cog_height; ///< CoM height Eigen::VectorXd m_cog_dot_height; ///< CoM velocity Eigen::VectorXd m_cog_ddot_height; ///< CoM acceleration diff --git a/samples/CoMTrajectoryGeneration/src/states/CoMTrajectoryGeneration_Initial.cpp b/samples/CoMTrajectoryGeneration/src/states/CoMTrajectoryGeneration_Initial.cpp index ed10fe68de..ec3f5d9db9 100644 --- a/samples/CoMTrajectoryGeneration/src/states/CoMTrajectoryGeneration_Initial.cpp +++ b/samples/CoMTrajectoryGeneration/src/states/CoMTrajectoryGeneration_Initial.cpp @@ -42,7 +42,7 @@ void CoMTrajectoryGeneration_Initial::start(mc_control::fsm::Controller & ctl) comGenerator_ = std::make_shared(previewSize_, m_dt, ctl.robot().mass(), waist_height); comGenerator_->addToLogger(ctl.logger()); - comGenerator_->setSteps(m_steps); + comGenerator_->steps(m_steps); mc_rtc::log::info( "Desired steps:\nTime\tCoM X\tCoM Y\n{}", mc_rtc::io::to_string(m_steps, [](const Eigen::Vector3d & v) -> Eigen::RowVector3d { return v; }, "\n")); @@ -64,7 +64,7 @@ void CoMTrajectoryGeneration_Initial::start(mc_control::fsm::Controller & ctl) const auto & curr = t_; m_steps.clear(); m_steps.push_back(Eigen::Vector3d{curr+3, 0, leftFootPos_.y()}); - comGenerator_->setSteps(m_steps); + comGenerator_->steps(m_steps); updateSteps(); }), mc_rtc::gui::Button("Right", @@ -97,11 +97,11 @@ void CoMTrajectoryGeneration_Initial::updateSteps() // Repeat last element m_steps.push_back(m_steps.back()); m_steps.back()(0) += previewTime_; - comGenerator_->setSteps(m_steps); + comGenerator_->steps(m_steps); mc_rtc::log::info("Current time: {}", t_); mc_rtc::log::info( "Desired steps:\nTime\tCoM X\tCoM Y\n{}", - mc_rtc::io::to_string(comGenerator_->Steps(), [](const Eigen::Vector3d & v) -> Eigen::RowVector3d { return v; }, "\n")); + mc_rtc::io::to_string(comGenerator_->steps(), [](const Eigen::Vector3d & v) -> Eigen::RowVector3d { return v; }, "\n")); } bool CoMTrajectoryGeneration_Initial::run(mc_control::fsm::Controller & ctl) diff --git a/src/mc_planning/LinearTimeVariantInvertedPendulum.cpp b/src/mc_planning/LinearTimeVariantInvertedPendulum.cpp index 91a01d2567..36f1a12708 100644 --- a/src/mc_planning/LinearTimeVariantInvertedPendulum.cpp +++ b/src/mc_planning/LinearTimeVariantInvertedPendulum.cpp @@ -206,9 +206,9 @@ void LinearTimeVariantInvertedPendulum::generate(Eigen::VectorXd & cog_pos, p_ref(m_n_preview2 - 1) = m_p_ref(m_n_preview2 - 1); } -LinearTimeVariantInvertedPendulum::State LinearTimeVariantInvertedPendulum::getState(int n_time) const +LinearTimeVariantInvertedPendulum::State LinearTimeVariantInvertedPendulum::getState(unsigned n_time) const { - const auto t = static_cast(static_cast(m_n_current) + n_time); + const auto t = m_n_current + n_time; State s; s.p = m_p_ref(t); s.pdot = (s.p - m_p_ref[t - 1]) / m_dt; @@ -218,7 +218,7 @@ LinearTimeVariantInvertedPendulum::State LinearTimeVariantInvertedPendulum::getS return s; } -void LinearTimeVariantInvertedPendulum::getState(int n_time, +void LinearTimeVariantInvertedPendulum::getState(unsigned n_time, double & cog_pos, double & cog_vel, double & cog_acc, diff --git a/src/mc_planning/generator.cpp b/src/mc_planning/generator.cpp index a4949fc26e..7b57704f53 100644 --- a/src/mc_planning/generator.cpp +++ b/src/mc_planning/generator.cpp @@ -12,15 +12,11 @@ constexpr int Z = 2; namespace mc_planning { -generator::generator(int n_preview, double dt, double mass, double waist_height) -: m_ComInterp(NULL), m_Pcalpha_ideal_pre(Eigen::Vector3d::Zero()), m_Pcalpha_ideal(Eigen::Vector3d::Zero()), - m_Pcalpha_cmp(Eigen::Vector3d::Zero()), m_Vcalpha_ideal_pre(Eigen::Vector3d::Zero()), - m_Vcalpha_ideal(Eigen::Vector3d::Zero()), m_Vcalpha_cmp(Eigen::Vector3d::Zero()), - m_Pcalpha_out(Eigen::Vector3d::Zero()), m_Pcalpha_motion_out(Eigen::Vector3d::Zero()), m_n_preview(n_preview), - m_n_steps(0), m_dt(dt), m_omega_valpha(0.0), m_mass(mass), m_waist_height(waist_height) +generator::generator(unsigned n_preview, double dt, double mass, double waist_height) +: m_ComInterp(std::make_shared>(1.0, dt / 2.)), m_n_preview(n_preview), m_dt(dt), + m_mass(mass), m_waist_height(waist_height) { mc_rtc::log::info("Creating generator with mass={}, waist_height={}", mass, waist_height); - m_ComInterp = std::make_shared>(1.0, m_dt / 2); m_ipm_long[X].Initialize(m_dt, m_n_preview, 20000); m_ipm_long[Y].Initialize(m_dt, m_n_preview, 20000); @@ -42,24 +38,27 @@ generator::generator(int n_preview, double dt, double mass, double waist_height) m_COG_ideal.P << 0.0, 0.0, waist_height; } -// n_current index of the start of the preview window -void generator::setupCOGHeight(int n_current) +void generator::setupCOGHeight(unsigned n_current) { // The first time, initialize the whole trajectory // with constant waist height // Ignores lateral sway during the trajectory? if(n_current == 0) { - m_ComInterp->push_back(0.0, m_waist_height); - m_ComInterp->push_back(lround(m_steps.back()(0) / m_dt), m_waist_height); + m_ComInterp->push_back(0u, m_waist_height); + m_ComInterp->push_back(static_cast(std::lround(m_steps.back()(0) / m_dt)), m_waist_height); m_ComInterp->update(); } // Time window from past to future - for(int n_step = -m_n_preview; n_step <= m_n_preview; n_step++) + for(unsigned n_step = 0; n_step < 2 * m_n_preview + 1; ++n_step) { - m_ComInterp->get(n_current + n_step + m_n_preview, m_cog_height[n_step + m_n_preview], - m_cog_dot_height[n_step + m_n_preview], m_cog_ddot_height[n_step + m_n_preview]); + // clang-format off + m_ComInterp->get(n_current + n_step , + m_cog_height[n_step ], + m_cog_dot_height[n_step ], + m_cog_ddot_height[n_step]); + //clang-format on } // Interpolated value at current time @@ -68,12 +67,12 @@ void generator::setupCOGHeight(int n_current) m_COG_ideal.Vdot(Z) = m_cog_ddot_height[m_n_preview]; } -void generator::setupTimeTrajectories(int n_current) +void generator::setupTimeTrajectories(unsigned n_current) { Eigen::VectorXd & px_ref = m_ipm_long[X].p_ref(); Eigen::VectorXd & py_ref = m_ipm_long[Y].p_ref(); - int n_steps_loop = m_n_steps; - for(int i = 0; i < m_n_preview * 2 + 1; i++) + unsigned n_steps_loop = m_n_steps; + for(unsigned i = 0; i < m_n_preview * 2 + 1; i++) { m_virtual_height[X](i) = 0.0; m_virtual_height[Y](i) = 0.0; @@ -131,7 +130,7 @@ void generator::generateTrajectories(void) /** * Computes long-term trajectory */ - auto computeLongTerm = [&](int axis) { + auto computeLongTerm = [&](unsigned axis) { m_ipm_long[axis].update(); // Current time @@ -146,7 +145,7 @@ void generator::generateTrajectories(void) * Compute short term trajectory * Depends on the long term trajectory results **/ - auto computeShortTerm = [&](int axis) { + auto computeShortTerm = [&](unsigned axis) { double omega2 = m_ipm_long[axis].w2(m_n_preview); double omega = sqrt(omega2); m_ipm_short[axis].setSystemMatrices(omega * m_poles[axis](0), omega * m_poles[axis](1), m_poles[axis](2), omega2, @@ -162,7 +161,7 @@ void generator::generateTrajectories(void) m_ipm_short[axis].getStateVariables(m_COG_cmp.P(axis), m_COG_cmp.V(axis), m_Pcalpha_cmp(axis), m_Vcalpha_cmp(axis)); }; - auto computeTrajectories = [&](int axis) { + auto computeTrajectories = [&](unsigned axis) { computeLongTerm(axis); computeShortTerm(axis); }; @@ -178,7 +177,7 @@ void generator::generateTrajectories(void) m_Pcalpha_motion_out = m_Pcalpha_out + m_Vcalpha_ideal * m_omega_valpha * m_dt; } -void generator::generate(int n_time) +void generator::generate(unsigned n_time) { if(m_steps.empty()) { diff --git a/tests/hmc/testCoMGenerator.cpp b/tests/hmc/testCoMGenerator.cpp index e92e9266ed..80a6bb0c4d 100644 --- a/tests/hmc/testCoMGenerator.cpp +++ b/tests/hmc/testCoMGenerator.cpp @@ -7,11 +7,7 @@ using namespace mc_planning; using Vector3 = Eigen::Vector3d; constexpr double dt = 0.005; -const int n_preview = lround(1.6 / dt); - -constexpr int X = 0; -constexpr int Y = 1; -constexpr int Z = 2; +const unsigned n_preview = static_cast(std::lround(1.6 / dt)); int main(void) { @@ -32,7 +28,7 @@ int main(void) steps.push_back(steps.back() + Vector3{(double)n_preview * dt, 0.0, 0.0}); // Repeat last step for computations steps.push_back(steps.back() + Vector3{(double)n_preview * dt, 0.0, 0.0}); - com_traj.setSteps(steps); + com_traj.steps(steps); mc_rtc::log::info( "Desired steps:\nTime\tCoM X\tCoM Y\n{}", mc_rtc::io::to_string(steps, [](const Eigen::Vector3d & v) -> Eigen::RowVector3d { return v; }, "\n")); @@ -41,23 +37,24 @@ int main(void) logger.start("CoMGenerator", dt); com_traj.addToLogger(logger); - int n_loop = lround(com_traj.Steps().back()(0) / dt) - n_preview; + unsigned n_loop = static_cast(std::lround(com_traj.steps().back()(0) / dt) - n_preview); + mc_rtc::log::info("n_loop {}", n_loop); /* * Generate the CoM trajectory based on the parameters in com_traj */ auto comGeneration = [&]() { - for(int loop = 0; loop <= n_loop; loop++) + for(unsigned loop = 0; loop <= n_loop; loop++) { com_traj.generate(loop); logger.log(); } }; - com_traj.removeFromLogger(logger); auto duration = mc_rtc::measure_ms::execution(comGeneration).count(); mc_rtc::log::info("calc time = {} (ms)", duration); mc_rtc::log::info("ave. calc time = {} (ms)", duration / static_cast(n_loop)); mc_rtc::log::success("end of com trajectory generation"); + com_traj.removeFromLogger(logger); return 0; } From 878337eb081345a73d79b9d8c429410d0fe7a7d8 Mon Sep 17 00:00:00 2001 From: Arnaud TANGUY Date: Fri, 19 Jun 2020 18:01:47 +0900 Subject: [PATCH 22/43] [HMC/samples] WIP change footsteps online Bug: Does not interpolate the reference ZMP trajectory --- .../LinearTimeVariantInvertedPendulum.h | 31 +++-- include/mc_planning/generator.h | 33 +++++ .../CoMTrajectoryGeneration_Initial.cpp | 114 +++++++++++------- src/mc_planning/generator.cpp | 88 ++++++++------ 4 files changed, 176 insertions(+), 90 deletions(-) diff --git a/include/mc_planning/LinearTimeVariantInvertedPendulum.h b/include/mc_planning/LinearTimeVariantInvertedPendulum.h index 1fba7c5162..8a5e4de1fd 100644 --- a/include/mc_planning/LinearTimeVariantInvertedPendulum.h +++ b/include/mc_planning/LinearTimeVariantInvertedPendulum.h @@ -96,19 +96,16 @@ struct MC_PLANNING_DLLAPI LinearTimeVariantInvertedPendulum { EIGEN_MAKE_ALIGNED_OPERATOR_NEW + /** + * @brief Default constructor + * + * You must call Initialize() before use + */ LinearTimeVariantInvertedPendulum(); + /** * @brief Initialization - * - * @param dt Timestep - * @param n_preview Number of future preview elements. The full size of the - * preview windows from past to future will be (2*n_preview+1) - * @param minHeight for the pendulum (used for LookupTable optimizations) - * @param maxHeight for the pendulum - * @param weight_resolution Number of data points to pre-compute for - * \f$ \omega, cosh(\omega), sinh(\omega) \f$ for height in a given range - * [minHeight, maxHeight]. This is used to optimize the otherwise costly computation of these - * constants. + * Calls Initialize() */ LinearTimeVariantInvertedPendulum(double dt, unsigned n_preview = 0, @@ -117,6 +114,18 @@ struct MC_PLANNING_DLLAPI LinearTimeVariantInvertedPendulum double maxHeight = 2.5); virtual ~LinearTimeVariantInvertedPendulum(); + /** + * @brief Initialization + * + * @param dt Timestep @param n_preview Number of future preview elements. The full size of the + * preview windows from past to future will be (2*n_preview+1) + * @param minHeight for the pendulum (used for LookupTable optimizations) + * @param maxHeight for the pendulum + * @param weight_resolution Number of data points to pre-compute for + * \f$ \omega, cosh(\omega), sinh(\omega) \f$ for height in a given range + * [minHeight, maxHeight]. This is used to optimize the otherwise costly computation of these + * functions. + */ void Initialize(double dt, unsigned n_preview = 0, unsigned weight_resolution = 20000, @@ -125,8 +134,6 @@ struct MC_PLANNING_DLLAPI LinearTimeVariantInvertedPendulum /** * @brief Initialize the discretized system matrices with constant pendulum height -Pre-computed table from . - * * Initalizes \f$ A_k \f$, \f$ B_k \f$ * diff --git a/include/mc_planning/generator.h b/include/mc_planning/generator.h index f31eac2bf2..27e30cfae8 100644 --- a/include/mc_planning/generator.h +++ b/include/mc_planning/generator.h @@ -68,9 +68,42 @@ struct MC_PLANNING_DLLAPI generator void steps(const std::vector & steps) { m_steps = steps; + // Ensure that the trajectory is valid after the last step + m_steps.push_back(steps.back()); m_n_steps = 0; } + /** + * @brief WIP: Change future steps and ensures continuous trajectory + * + * @param n_time Start of the preview window + * @param futureSteps Future steps + */ + void changeFutureSteps(unsigned n_time, const std::vector & futureSteps) + { + // Find step corresponding to the start time + auto startTime = n_time * m_dt; + std::vector newSteps; + for(unsigned i = 0; i < m_steps.size(); ++i) + { + const auto & step = m_steps[i]; + if(step(0) < startTime + m_n_preview * m_dt) + { + // XXX should also discard old useless ones + newSteps.push_back(step); + } + } + // Repeat last step (ensures interpolation passes through current desired + // ZMP + newSteps.push_back({(n_time + m_n_preview) * m_dt, newSteps.back().y(), newSteps.back().z()}); + m_n_steps++; + // Add new future steps + std::copy(futureSteps.begin(), futureSteps.end(), std::back_inserter(newSteps)); + // Repeat last element to keep trajectory valid after time is elapsed + newSteps.push_back(newSteps.back()); + steps(newSteps); + } + /** * @brief Add a step * diff --git a/samples/CoMTrajectoryGeneration/src/states/CoMTrajectoryGeneration_Initial.cpp b/samples/CoMTrajectoryGeneration/src/states/CoMTrajectoryGeneration_Initial.cpp index ec3f5d9db9..6f34a91971 100644 --- a/samples/CoMTrajectoryGeneration/src/states/CoMTrajectoryGeneration_Initial.cpp +++ b/samples/CoMTrajectoryGeneration/src/states/CoMTrajectoryGeneration_Initial.cpp @@ -22,20 +22,23 @@ void CoMTrajectoryGeneration_Initial::start(mc_control::fsm::Controller & ctl) rightFootPos_ = ctl.robot().surfacePose("RightFootCenter").translation(); feetCenterPos_ = (leftFootPos_ + rightFootPos_)/2; - double leftY = leftFootPos_.y(); - double rightY = rightFootPos_.y(); + double leftY = leftFootPos_.y()-0.04; + double rightY = rightFootPos_.y()+0.04; double centerY = feetCenterPos_.y(); + const Eigen::Vector3d & currCoM = ctl.robot().com(); //clang-format off m_steps = { - {0.0, 0.0, 0.0}, - {2.0, 0.0, 0.0}, - {3.5, 0.0, rightY}, + {0.0, currCoM.x(), currCoM.y()}, + {0.2, 0.0, centerY}, + {2, 0.0, centerY}, + {2.2, 0.0, rightY}, {4, 0.0, rightY}, - {5.5, 0.0, leftY}, - {7, 0.0, rightY}, - {9, 0.0, centerY}, - {10, 0.0, centerY} + {4.2, 0.0, leftY}, + {6, 0.0, leftY}, + {6.2, 0.0, rightY}, + {8, 0.0, rightY}, + {8.2, 0.0, centerY} }; //clang-format on @@ -52,35 +55,72 @@ void CoMTrajectoryGeneration_Initial::start(mc_control::fsm::Controller & ctl) ctl.gui()->addPlot( "Trajectory (Y)", mc_rtc::gui::plot::X("t", [this]() { return t_; }), - mc_rtc::gui::plot::Y("Output CoM", [this]() { return comGenerator_->OutputCOGPosition().y(); }, Color::Green), - mc_rtc::gui::plot::Y("Ideal CoM", [this]() { return comGenerator_->IdealCOGPosition().y(); }, Color::Green, Style::Dashed), + mc_rtc::gui::plot::Y("Output CoM", [this]() { return comGenerator_->OutputCOGPosition().y(); }, Color::Magenta), + mc_rtc::gui::plot::Y("Ideal CoM", [this]() { return comGenerator_->IdealCOGPosition().y(); }, Color::Red, Style::Dashed), mc_rtc::gui::plot::Y("Output ZMP", [this]() { return comGenerator_->OutputZMPPosition().y(); }, Color::Blue), - mc_rtc::gui::plot::Y("Ideal ZMP", [this]() { return comGenerator_->IdealZMPPosition().y(); }, Color::Blue, Style::Dashed)); + mc_rtc::gui::plot::Y("Ideal ZMP", [this]() { return comGenerator_->IdealZMPPosition().y(); }, Color::Cyan, Style::Dashed)); ctl.gui()->addElement({"CoMTrajectoryGeneration"}, mc_rtc::gui::Button("Left", - [this]() + [this,leftY]() { const auto & curr = t_; - m_steps.clear(); - m_steps.push_back(Eigen::Vector3d{curr+3, 0, leftFootPos_.y()}); - comGenerator_->steps(m_steps); + comGenerator_->changeFutureSteps(iter_, + { + Eigen::Vector3d{curr + previewTime_ + 0.8, 0, leftY}, + }); updateSteps(); }), mc_rtc::gui::Button("Right", - [this]() + [this,rightY]() { const auto & curr = t_; - m_steps.clear(); - m_steps.push_back(Eigen::Vector3d{curr+3, 0, rightFootPos_.y()}); + comGenerator_->changeFutureSteps(iter_, + { + Eigen::Vector3d{curr + previewTime_ + 0.8, 0, rightY}, + }); updateSteps(); }), mc_rtc::gui::Button("Center", - [this]() + [this,centerY]() { const auto & curr = t_; - m_steps.clear(); - m_steps.push_back(Eigen::Vector3d{curr+3, 0, feetCenterPos_.y()}); + comGenerator_->changeFutureSteps(iter_, + { + Eigen::Vector3d{curr + previewTime_ + 0.8, 0, centerY}, + }); + updateSteps(); + }), + mc_rtc::gui::Button("Left-Right-Left", + [this,centerY,leftY,rightY]() + { + const auto & curr = t_; + comGenerator_->changeFutureSteps(iter_, + { + Eigen::Vector3d{curr + previewTime_ + 0.2, 0, leftY}, + Eigen::Vector3d{curr + previewTime_ + 2, 0, leftY}, + Eigen::Vector3d{curr + previewTime_ + 2.2, 0, rightY}, + Eigen::Vector3d{curr + previewTime_ + 4, 0, rightY}, + Eigen::Vector3d{curr + previewTime_ + 4.2, 0, centerY}, + }); + updateSteps(); + }), + mc_rtc::gui::Button("Left-Right-Left (x10)", + [this,centerY,leftY,rightY]() + { + const auto & curr = t_; + std::vector futureSteps; + auto startT = curr; + for (int i = 0; i < 10; ++i) + { + futureSteps.push_back(Eigen::Vector3d{startT + previewTime_ + 0.2, 0, leftY}); + futureSteps.push_back(Eigen::Vector3d{startT + previewTime_ + 2, 0, leftY}); + futureSteps.push_back(Eigen::Vector3d{startT + previewTime_ + 2.2, 0, rightY}); + futureSteps.push_back(Eigen::Vector3d{startT + previewTime_ + 4, 0, rightY}); + startT += 4; + } + futureSteps.push_back(Eigen::Vector3d{startT + previewTime_ + 0.2, 0, centerY}); + comGenerator_->changeFutureSteps(iter_, futureSteps); updateSteps(); }) ); @@ -94,10 +134,6 @@ void CoMTrajectoryGeneration_Initial::start(mc_control::fsm::Controller & ctl) void CoMTrajectoryGeneration_Initial::updateSteps() { - // Repeat last element - m_steps.push_back(m_steps.back()); - m_steps.back()(0) += previewTime_; - comGenerator_->steps(m_steps); mc_rtc::log::info("Current time: {}", t_); mc_rtc::log::info( "Desired steps:\nTime\tCoM X\tCoM Y\n{}", @@ -106,23 +142,19 @@ void CoMTrajectoryGeneration_Initial::updateSteps() bool CoMTrajectoryGeneration_Initial::run(mc_control::fsm::Controller & ctl) { - if(t_ < m_steps.back()(0)) + /* Generate trajectory starting at iter_ time (start of the past preview window) + * The trajectory result is computed for the current time (iter_+previewSize_) + */ + auto generateTrajectory = [this]() { - auto generateTrajectory = [this]() - { - comGenerator_->generate(iter_); - }; - generationTime_ = mc_rtc::measure_ms::execution(generateTrajectory).count(); - - if(ctl.datastore().has("StabilizerStandingTrackCoM::target")) - { - ctl.datastore().call("StabilizerStandingTrackCoM::target", comGenerator_->OutputCOGPosition(), comGenerator_->OutputCOGVelocity(), comGenerator_->OutputCOGAcceleration(), comGenerator_->OutputZMPPosition()); - } - } - else + comGenerator_->generate(iter_); + }; + generationTime_ = mc_rtc::measure_ms::execution(generateTrajectory).count(); + + // Provide targets to the stabilizer (running in another state) + if(ctl.datastore().has("StabilizerStandingTrackCoM::target")) { - // mc_rtc::log::info("No more target"); - generationTime_ = 0; + ctl.datastore().call("StabilizerStandingTrackCoM::target", comGenerator_->OutputCOGPosition(), comGenerator_->OutputCOGVelocity(), comGenerator_->OutputCOGAcceleration(), comGenerator_->OutputZMPPosition()); } iter_++; diff --git a/src/mc_planning/generator.cpp b/src/mc_planning/generator.cpp index 7b57704f53..4d87320206 100644 --- a/src/mc_planning/generator.cpp +++ b/src/mc_planning/generator.cpp @@ -18,23 +18,19 @@ generator::generator(unsigned n_preview, double dt, double mass, double waist_he { mc_rtc::log::info("Creating generator with mass={}, waist_height={}", mass, waist_height); - m_ipm_long[X].Initialize(m_dt, m_n_preview, 20000); - m_ipm_long[Y].Initialize(m_dt, m_n_preview, 20000); - m_ipm_short[X].Initialize(); - m_ipm_short[Y].Initialize(); - - m_poles[X] << 1.0, 1.0, 150.0; - m_poles[Y] << 1.0, 1.0, 150.0; + auto initialize = [this, &waist_height](unsigned X) { + m_ipm_long[X].Initialize(m_dt, m_n_preview, 20000); + m_ipm_short[X].Initialize(); + m_poles[X] << 1.0, 1.0, 150.0; + m_virtual_height[X].setZero(m_n_preview * 2 + 1); + m_ipm_long[X].initMatrices(waist_height); + }; + initialize(X); + initialize(Y); - m_virtual_height[X].setZero(m_n_preview * 2 + 1); - m_virtual_height[Y].setZero(m_n_preview * 2 + 1); m_cog_height.setZero(m_n_preview * 2 + 1); m_cog_dot_height.setZero(m_n_preview * 2 + 1); m_cog_ddot_height.setZero(m_n_preview * 2 + 1); - - m_ipm_long[X].initMatrices(waist_height); - m_ipm_long[Y].initMatrices(waist_height); - m_COG_ideal.P << 0.0, 0.0, waist_height; } @@ -54,9 +50,9 @@ void generator::setupCOGHeight(unsigned n_current) for(unsigned n_step = 0; n_step < 2 * m_n_preview + 1; ++n_step) { // clang-format off - m_ComInterp->get(n_current + n_step , - m_cog_height[n_step ], - m_cog_dot_height[n_step ], + m_ComInterp->get(n_current + n_step, + m_cog_height[n_step], + m_cog_dot_height[n_step], m_cog_ddot_height[n_step]); //clang-format on } @@ -77,24 +73,28 @@ void generator::setupTimeTrajectories(unsigned n_current) m_virtual_height[X](i) = 0.0; m_virtual_height[Y](i) = 0.0; - double tm = static_cast(i + n_current) * m_dt; - const auto & currStep = m_steps[n_steps_loop]; + const double currTime = static_cast(i + n_current) * m_dt; + const auto & prevStep = m_steps[n_steps_loop]; const auto & nextStep = m_steps[n_steps_loop + 1]; - const double currTime = currStep(0); + const double prevTime = prevStep(0); const double nextTime = nextStep(0); - if(tm >= currTime) + + if(currTime >= prevTime && currTime < nextTime) { - // 1: Step CoM x, 2: Step CoM Y - auto refXY = [&](const Eigen::Index axis) { - return currStep(axis) - + (nextStep(axis) - currStep(axis)) * polynomial3((tm - currTime) / (nextTime - currTime)); + /** + * @brief Interpolates the reference ZMP from one step to the next + * @param axis 1: Step CoM x, 2: Step CoM Y + */ + auto interpolateRefXY = [&](const Eigen::Index axis) { + return prevStep(axis) + + (nextStep(axis) - prevStep(axis)) * polynomial3((currTime - prevTime) / (nextTime - prevTime)); }; - px_ref(i) = refXY(1); - py_ref(i) = refXY(2); + px_ref(i) = interpolateRefXY(1); + py_ref(i) = interpolateRefXY(2); } else - { + { /* No previous step provided, start with the ZMP at the next step */ px_ref(i) = nextStep(1); py_ref(i) = nextStep(2); } @@ -109,20 +109,20 @@ void generator::setupTimeTrajectories(unsigned n_current) if(n_steps_loop < m_steps.size() - 2) { // If current time is after the next step time, move to next step - if(tm >= nextTime) n_steps_loop++; + if(currTime >= nextTime) n_steps_loop++; } } m_Pcalpha_ideal(Z) = (m_virtual_height[X](m_n_preview) + m_virtual_height[Y](m_n_preview)) / 2.0; double tm_cur = (double)n_current * m_dt; - if(m_n_steps < m_steps.size() - 1) + if(m_n_steps < m_steps.size() - 2) { if(tm_cur > m_steps[m_n_steps + 1](0)) m_n_steps++; } } -void generator::generateTrajectories(void) +void generator::generateTrajectories() { StatePV COG_ideal_pre_next(m_COG_ideal.P, m_COG_ideal.V); Eigen::Vector3d Pcalpha_ideal_pre_next(m_Pcalpha_ideal); @@ -144,6 +144,10 @@ void generator::generateTrajectories(void) /** * Compute short term trajectory * Depends on the long term trajectory results + * + * Generates the CoM and ZMP modification to be applied to the ideal long-term so that the short term + * - m_COG_cmp: Modification of the ideal CoG state (position and velocity) + * - m_Pcalpha_cmp: Modifiecation of the ideal ZMP state (position and velocity) **/ auto computeShortTerm = [&](unsigned axis) { double omega2 = m_ipm_long[axis].w2(m_n_preview); @@ -161,20 +165,30 @@ void generator::generateTrajectories(void) m_ipm_short[axis].getStateVariables(m_COG_cmp.P(axis), m_COG_cmp.V(axis), m_Pcalpha_cmp(axis), m_Vcalpha_cmp(axis)); }; + /** @brief Computes both long-term and short-term trajectories */ auto computeTrajectories = [&](unsigned axis) { computeLongTerm(axis); computeShortTerm(axis); }; - computeTrajectories(X); - computeTrajectories(Y); + /** + * Computes the desired output CoG and ZMP state composed + * of the ideal long-term trajectory output modified by the short-term + * trajectory to ensure continuity + */ + auto applyShortTermCompensation = [this]() + { + m_COG_out.P = m_COG_ideal.P + m_COG_cmp.P; + m_COG_out.V = m_COG_ideal.V + m_COG_cmp.V; + m_COG_out.Vdot = m_COG_ideal.Vdot + m_COG_cmp.Vdot; - m_COG_out.P = m_COG_ideal.P + m_COG_cmp.P; - m_COG_out.V = m_COG_ideal.V + m_COG_cmp.V; - m_COG_out.Vdot = m_COG_ideal.Vdot + m_COG_cmp.Vdot; + m_Pcalpha_out = m_Pcalpha_ideal + m_Pcalpha_cmp; + m_Pcalpha_motion_out = m_Pcalpha_out + m_Vcalpha_ideal * m_omega_valpha * m_dt; + }; - m_Pcalpha_out = m_Pcalpha_ideal + m_Pcalpha_cmp; - m_Pcalpha_motion_out = m_Pcalpha_out + m_Vcalpha_ideal * m_omega_valpha * m_dt; + computeTrajectories(X); + computeTrajectories(Y); + applyShortTermCompensation(); } void generator::generate(unsigned n_time) From 3255f5b00f7c9759e173aa7147c3b2f79a2f3d11 Mon Sep 17 00:00:00 2001 From: Arnaud TANGUY Date: Mon, 22 Jun 2020 14:16:34 +0900 Subject: [PATCH 23/43] [tests] Use in-source mc_planning headers for tests --- src/CMakeLists.txt | 23 +++++++++++++---------- 1 file changed, 13 insertions(+), 10 deletions(-) diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index f57080039e..edd2c823bc 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -417,17 +417,20 @@ mc_planning/State.cpp mc_planning/generator.cpp # XXX should it be there? ) +set(mc_planning_HDR_DIR + $$) + set(mc_planning_HDR -../include/mc_planning/Pendulum.h -../include/mc_planning/api.h -../include/mc_planning/LinearTimeVariantInvertedPendulum.h -../include/mc_planning/LinearControl3.h -../include/mc_planning/LIPMControlByPoleAssign.h -../include/mc_planning/LIPMControlByPoleAssignWithExternalForce.h -../include/mc_planning/InterpolatorBase.h -../include/mc_planning/CubicSplineBase.h -../include/mc_planning/ClampedCubicSpline.h -../include/mc_planning/generator.h + ${mc_planning_HDR_DIR}/Pendulum.h + ${mc_planning_HDR_DIR}/api.h + ${mc_planning_HDR_DIR}/LinearTimeVariantInvertedPendulum.h + ${mc_planning_HDR_DIR}/LinearControl3.h + ${mc_planning_HDR_DIR}/LIPMControlByPoleAssign.h + ${mc_planning_HDR_DIR}/LIPMControlByPoleAssignWithExternalForce.h + ${mc_planning_HDR_DIR}/InterpolatorBase.h + ${mc_planning_HDR_DIR}/CubicSplineBase.h + ${mc_planning_HDR_DIR}/ClampedCubicSpline.h + ${mc_planning_HDR_DIR}/generator.h ) add_library(mc_planning SHARED ${mc_planning_SRC} ${mc_planning_HDR}) From 811ed482627eb9d351180a73aef01974fdf21c1c Mon Sep 17 00:00:00 2001 From: Arnaud TANGUY Date: Mon, 22 Jun 2020 14:17:08 +0900 Subject: [PATCH 24/43] [HMC/generator] Discrete step logic implementation Implements helpers to handle the logic behind discrete reference provided as a step function: - Ensures that steps are ordered by time (using std::set) - Convenience to create steps, add step, find predecessors/successors - Implement efficient logic for sequentially exploiting footsteps in an online setting: removing old footsteps, updating the previous/next step, etc This leads to a more readable and safer implementation of the original generator class, and allows to cleanly change footsteps at any time. The sample state CoMTrajectoryGeneration has been updated accordingly. --- include/mc_planning/generator.h | 500 ++++++++++++++++-- include/mc_rtc/gui/types.h | 1 + .../CoMTrajectoryGeneration_Initial.cpp | 127 ++--- .../states/CoMTrajectoryGeneration_Initial.h | 5 +- src/mc_planning/generator.cpp | 68 +-- src/mc_rtc/gui/StateBuilder.cpp | 1 + tests/hmc/testCoMGenerator.cpp | 36 +- tests/test_hmc.cpp | 136 +++++ 8 files changed, 708 insertions(+), 166 deletions(-) diff --git a/include/mc_planning/generator.h b/include/mc_planning/generator.h index 27e30cfae8..e1e221c12f 100644 --- a/include/mc_planning/generator.h +++ b/include/mc_planning/generator.h @@ -7,10 +7,454 @@ #include #include #include +#include namespace mc_planning { +template +struct TimedStep +{ + TimedStep(double t, const Step & step) noexcept : t_(t), step_(step) {} + + const double & t() const noexcept + { + return t_; + } + + double & t() noexcept + { + return t_; + } + + const Step & step() const noexcept + { + return step_; + } + + Step & step() noexcept + { + return step_; + } + + // For std::set + bool operator<(const TimedStep & t) const noexcept + { + return (this->t_ < t.t_); + } + + bool operator<=(const TimedStep & t) const noexcept + { + return (this->t_ <= t.t_); + } + + void operator+(const TimedStep & t) noexcept + { + t_ += t.t(); + step_ += t.step(); + } + + TimedStep & operator+=(const TimedStep & rhs) noexcept + { + t_ += rhs.t(); + step_ += rhs.step(); + return *this; + } + + void operator-(const TimedStep & t) noexcept + { + t_ += t.t(); + step_ += t.step(); + } + + TimedStep & operator-=(const TimedStep & rhs) noexcept + { + t_ -= rhs.t(); + step_ -= rhs.step(); + return *this; + } + +protected: + double t_; + Step step_; +}; + +template +bool operator<(const TimedStep & s1, const TimedStep & s2) +{ + return s1 < s2; +} + +template +std::ostream & operator<<(std::ostream & os, const TimedStep & step) +{ + os << step.t() << " -> " << step.step().transpose(); + return os; +} + +/** + * @brief Helper class to manage a list of discrete steps and provide efficient + * sequential lookup of next/previous step. + * + * Steps have the following properties: + * - Each step is defined by + * - It's start time Step::t() + * - It's value Step::step() (may be any arbitrary type) + * - Steps are discrete events and do not have a specified duration + * - Steps are sorted from oldest to most recent + * + * Example + * \code{.cpp} + * PreviewSteps steps; + * steps.add( {1.6, {-0.2, 0.0}}); + * steps.addRelative({1.6, { 0.0, 0.0}}); + * steps.addRelative({0.1, { 0.0, 0.0}}); + * steps.addRelative({1.6, { 0.0, 0.0}}); + * steps.addRelative({0.1, { 0.2, 0.095}}); + * steps.addRelative({1.6, { 0.0, 0.0}}); + * steps.addRelative({0.1, { 0.0, -0.19}}); + * steps.addRelative({1.6, { 0.0, 0.0}}); + * steps.addRelative({0.1, {-0.2, 0.095}}); + * steps.addRelative({1.6, { 0.0, 0.0}}); + * steps.initialize(); + * mc_rtc::log::info("Desired steps:\nTime\tCoM X\tCoM Y\n{}", mc_rtc::io::to_string(steps.steps(), "\n")); + * + * double dt = 0.005; + * double previewTime = 1.6; + * const unsigned n_preview = static_cast(std::lround(previewTime / dt)); + * + * // Iterate over a preview window and perform computations + * // based on the previous and next steps + * // In a real-world use, this would most likely be used to compute a + * // smooth trajectory between the discrete steps. + * auto iteratePreview = [&](double startTime) + * { + * for(unsigned i = 0; i < n_preview * 2 + 1; i++) + * { + * double currTime = startTime + i*dt; + * // Update time within the preview window + * steps.update(currTime); + * if(!steps.isLastStep()) + * { + * const auto & nextStep = steps.next(); + * const auto & prevStep = steps.previous(); + * // Do something with next and previous steps, such as interpolate + * // between them to generate a smooth reference trajectory + * } + * else + * { + * // We are past the last step, handle this case + * const auto & lastStep = steps.previous(); + * } + * } + * } + * + * // Real-time control loop + * bool run() + * { + * // Start next preview window + * steps.nextWindow(t); + * iteratePreview(t); + * t+=dt; + * // Now you can modify future steps for the next preview window + * double transitionTime = 0.2; // 0.2 second to transition + * double delay = 0.5; // Transition in 0.5s in the future. The previous step + * will be extended until the transition starts. + * steps.changeFutureSteps( + * { + * t+previewTime+delay, // Extend the previous step until this time + * {t+previewTime+delay+transitionTime, {0., -0.15}} // Transition to the + * next steps + * }); + * } + * \endcode + */ +template +struct MC_PLANNING_DLLAPI PreviewSteps +{ + using value_type = TimedStep; + using PreviewStepsSet = std::set>; + using iterator = typename PreviewStepsSet::iterator; + using reverse_iterator = typename PreviewStepsSet::reverse_iterator; + + /** + * @brief Inserts a new step + * + * A step may be added at any time, including within the current preview + * window + * + * @param step Step to add + */ + void add(const TimedStep & step) + { + if(step.t() < (*steps_.begin()).t()) + { + mc_rtc::log::error("[PreviewSteps] can't create a step before the start of the preview window"); + return; + } + steps_.insert(step); + } + + void reset(const PreviewStepsSet & steps) + { + steps_ = steps; + } + + /** + * @brief Adds a step relative to the last step + * + * @throws std::runtime_error If steps() is empty + * + * @param step Relative step to add + */ + void addRelative(const TimedStep & step) + { + if(steps_.empty()) + { + mc_rtc::log::error_and_throw("[PreviewSteps] can't add a relative step to an empty set"); + return; + } + auto last = this->back(); + last += step; + add(last); + } + + /** + * @brief Replaces all footsteps following the first of the new provided steps + * + * @param nextSteps + */ + void replaceAfter(const PreviewStepsSet & nextSteps) + { + if(nextSteps.empty()) return; + deleteAfter(nextSteps.begin()->t()); + std::copy(nextSteps.begin(), nextSteps.end(), std::inserter(steps_, steps_.begin())); + } + + void deleteAfter(double t) + { + auto it = std::find_if(steps_.begin(), steps_.end(), [t](const value_type & step) { return step.t() >= t; }); + if(it != steps_.end()) + { + steps_.erase(it, steps_.end()); + } + } + + void changeFutureSteps(double startTransition, const std::vector & steps) + { + mc_rtc::log::info("Delete after {}", startTransition); + deleteAfter(startTransition); + // Extend previous step until start of transition + auto transitionStep = back(); + mc_rtc::log::info("Duplicating previous step {}", transitionStep); + transitionStep.t() = startTransition; + mc_rtc::log::info("Pre-Transition step {}", transitionStep); + steps_.insert(transitionStep); + mc_rtc::log::info("Inserting steps"); + for(const auto & step : steps) + { + mc_rtc::log::info("Step {}", step); + steps_.insert(step); + } + } + + /** + * @brief To be called after adding the initial footsteps + * + * Requires at least one footstep. + * + * @throws std::runtime_error if there isn't at least one footstep + */ + void initialize() + { + if(steps_.size() < 1) + { + mc_rtc::log::error_and_throw("[PreviewSteps] requires at least one step"); + } + else if(steps_.size() == 1) + { + previous_ = steps_.begin(); + next_ = previous_; + } + else + { + previous_ = steps_.begin(); + next_ = std::next(previous_); + mc_rtc::log::info("Init {}, {}", previous_->t(), next_->t()); + } + } + + /** + * @name Real-time functions + * These functions are intended to be called sequentially while iterating over the preview window in the following + * way:
  • nextWindow(double t): Sets up a new preview window starting at time t, updates the starting step, and + * removes old steps prior to t
  • for each time in the preview window:
    • next(), previous() provide the + * previous/next step for the current time. Warning: if the last step is reached (`isLastStep() == true`), then next() + * = previous()
    • update(double t): move along the preview window
      • + *
    + *
    • + *
  • Repeat with the next preview window
    • + *
+ * @{ + */ + + /** + * @brief Updates the time along the preview window + * + * Finds the previous and next step for the current time in the preview window + * + * @param t Curent time within the preview window + */ + void update(double t) noexcept + { + if(!isLastStep() && t >= next_->t()) + { + // mc_rtc::log::info("Change step"); + previous_ = next_; + ++next_; + // if(next_ == steps_.end()) + // { // only happens when the end of the step plan is in the current preview horizon (rare) + // next_ = previous_; + // } + // mc_rtc::log::info("After Change step: {}, {}", previous().t(), next().t()); + } + } + + /** + * @brief Whether we reached the last step + * + * @waning When on the last step, there is no next step available, and next() == previous() + */ + bool isLastStep() const noexcept + { + return next_ == steps_.end(); + } + + /** + * @brief Moves to the next preview window starting at time t + * + * Removes old footsteps outside of the window, and update previous/next + * steps accordingly + * + * @param t Time of the start of the preview window + */ + void nextWindow(double t) + { + // Check whether old footsteps need to be removed + if(steps_.size() > 2 && t > std::next(steps_.begin())->t()) + { + steps_.erase(steps_.begin()); + } + + previous_ = steps_.begin(); + next_ = std::next(previous_); + // mc_rtc::log::info("Rewinding for next window: prev {}, next {}", previous(), next()); + } + + /** + * @brief Returns the previous step w.r.t last call to update(double t) + * + * @warning If no next step is available, then next() = previous(). You can + * check whether this is the last step ahead of time by calling isLastStep() + * + * @return Next step + */ + const TimedStep & previous() const noexcept + { + return *previous_; + } + + /** + * @brief Returns the next step w.r.w last call to update(double t) + * + * @warning If no next step is available, then next() = previous(). You can + * check whether this is the last step ahead of time by calling isLastStep() + * + * @return Next step + */ + const TimedStep & next() const noexcept + { + return *next_; + } + /** @} */ // end of group real-time + + /** + * @brief Get the closest footstep prior to time t + * + * @param t Lookup time + * + * @note This funciton is not intended for real-time performace and has a + * complexity in `O(steps)` + * + * @return Nearest past footstep with time <= t + */ + const TimedStep & previous(double t) const noexcept + { + if(t >= back().t()) return back(); + auto it = + std::find_if(steps_.rbegin(), steps_.rend(), [&t](const TimedStep & step) { return step.t() <= t; }); + if(it != steps_.rend()) + { + return *it; + } + else + { + return *steps_.begin(); + } + } + + /** + * @brief Get the closest future footstep after time t + * + * @param t Lookup time + * + * @note This function is not intended for real-time performace and has a + * complexity in `O(steps)`. + * + * @return Nearest future footstep with time <= t + */ + const TimedStep & next(double t) const noexcept + { + auto it = std::find_if(steps_.begin(), steps_.end(), [&t](const TimedStep & step) { return step.t() >= t; }); + if(it != steps_.end()) + { + return *it; + } + return previous(); + } + + const PreviewStepsSet & steps() const noexcept + { + return steps_; + } + + const TimedStep & back() const noexcept + { + return *std::prev(steps_.end()); + } + + const TimedStep & front() const noexcept + { + return *steps_.begin(); + } + + bool isValid() const noexcept + { + return steps_.size() > 1; + } + +protected: + PreviewStepsSet steps_; // Ordered set of pairs of [time, Step] + + /** + * @name Real-time update + * @{ + */ + typename PreviewStepsSet::iterator previous_ = std::end(steps_); ///< Previous footstep + typename PreviewStepsSet::iterator next_ = std::end(steps_); ///< Next footstep + ///@} +}; + /** * @brief Utility class to generate long and short term trajectories for the CoM. * @@ -24,6 +468,8 @@ namespace mc_planning */ struct MC_PLANNING_DLLAPI generator { + using PreviewStepsSet = PreviewSteps::PreviewStepsSet; + /** * @brief Construct a simple trajectory generator * @@ -55,7 +501,7 @@ struct MC_PLANNING_DLLAPI generator * @brief Desired steps * @see setSteps(const std::vector & steps) */ - const std::vector & steps() const + const PreviewSteps & steps() const { return m_steps; } @@ -65,54 +511,26 @@ struct MC_PLANNING_DLLAPI generator * * @param steps Desired steps from which the trajectory will be generated */ - void steps(const std::vector & steps) + void steps(const PreviewSteps & steps) { m_steps = steps; - // Ensure that the trajectory is valid after the last step - m_steps.push_back(steps.back()); - m_n_steps = 0; } /** - * @brief WIP: Change future steps and ensures continuous trajectory + * @brief Change future steps and ensures continuous trajectory + * + * @note The previous footstep will be extended until `transitionTime`, and + * all future steps currently in the trajectory will be removed * - * @param n_time Start of the preview window + * @param transitionTime Time at which the transition should occur * @param futureSteps Future steps */ - void changeFutureSteps(unsigned n_time, const std::vector & futureSteps) + void changeFutureSteps(double transitionTime, + const std::vector> & futureSteps) { - // Find step corresponding to the start time - auto startTime = n_time * m_dt; - std::vector newSteps; - for(unsigned i = 0; i < m_steps.size(); ++i) - { - const auto & step = m_steps[i]; - if(step(0) < startTime + m_n_preview * m_dt) - { - // XXX should also discard old useless ones - newSteps.push_back(step); - } - } - // Repeat last step (ensures interpolation passes through current desired - // ZMP - newSteps.push_back({(n_time + m_n_preview) * m_dt, newSteps.back().y(), newSteps.back().z()}); - m_n_steps++; - // Add new future steps - std::copy(futureSteps.begin(), futureSteps.end(), std::back_inserter(newSteps)); - // Repeat last element to keep trajectory valid after time is elapsed - newSteps.push_back(newSteps.back()); - steps(newSteps); + m_steps.changeFutureSteps(transitionTime, futureSteps); } - /** - * @brief Add a step - * - * Step must be after the last existing timestep - * - * @param step - */ - void push_back(const Eigen::Vector3d & step); - /** * @name Accessors */ @@ -273,7 +691,6 @@ struct MC_PLANNING_DLLAPI generator /// @} unsigned m_n_preview = 0; ///< Size of the future preview window. The full preview is 2*m_n_preview+1 - unsigned m_n_steps = 0; ///< Current step double m_dt = 0.005; ///< Timestep double m_omega_valpha = 0.0; double m_mass = 60.; ///< Robot mass @@ -290,7 +707,10 @@ struct MC_PLANNING_DLLAPI generator Eigen::VectorXd m_cog_height; ///< CoM height Eigen::VectorXd m_cog_dot_height; ///< CoM velocity Eigen::VectorXd m_cog_ddot_height; ///< CoM acceleration - std::vector m_steps; ///< (time, com_x, com_y) + + // unsigned m_n_steps = 0; ///< Current step + // std::vector m_steps; ///< (time, com_x, com_y) + PreviewSteps m_steps; }; } // namespace mc_planning diff --git a/include/mc_rtc/gui/types.h b/include/mc_rtc/gui/types.h index 2e52599f6a..d66727e66b 100644 --- a/include/mc_rtc/gui/types.h +++ b/include/mc_rtc/gui/types.h @@ -120,6 +120,7 @@ struct MC_RTC_GUI_DLLAPI Color static const Color Yellow; static const Color Gray; static const Color LightGray; + static const Color Purple; static const std::map ColorMap; }; } // namespace gui diff --git a/samples/CoMTrajectoryGeneration/src/states/CoMTrajectoryGeneration_Initial.cpp b/samples/CoMTrajectoryGeneration/src/states/CoMTrajectoryGeneration_Initial.cpp index 6f34a91971..f58d8e233e 100644 --- a/samples/CoMTrajectoryGeneration/src/states/CoMTrajectoryGeneration_Initial.cpp +++ b/samples/CoMTrajectoryGeneration/src/states/CoMTrajectoryGeneration_Initial.cpp @@ -27,28 +27,27 @@ void CoMTrajectoryGeneration_Initial::start(mc_control::fsm::Controller & ctl) double centerY = feetCenterPos_.y(); const Eigen::Vector3d & currCoM = ctl.robot().com(); //clang-format off - m_steps = + steps_.reset( { - {0.0, currCoM.x(), currCoM.y()}, - {0.2, 0.0, centerY}, - {2, 0.0, centerY}, - {2.2, 0.0, rightY}, - {4, 0.0, rightY}, - {4.2, 0.0, leftY}, - {6, 0.0, leftY}, - {6.2, 0.0, rightY}, - {8, 0.0, rightY}, - {8.2, 0.0, centerY} - }; + {0.0, {currCoM.x(), currCoM.y()}}, + {0.2, {0.0, centerY}}, + {2, {0.0, centerY}}, + {2.2, {0.0, rightY}}, + {4, {0.0, rightY}}, + {4.2, {0.0, leftY}}, + {6, {0.0, leftY}}, + {6.2, {0.0, rightY}}, + {8, {0.0, rightY}}, + {8.2, {0.0, centerY}} + }); + steps_.initialize(); //clang-format on comGenerator_ = std::make_shared(previewSize_, m_dt, ctl.robot().mass(), waist_height); comGenerator_->addToLogger(ctl.logger()); - comGenerator_->steps(m_steps); - mc_rtc::log::info( - "Desired steps:\nTime\tCoM X\tCoM Y\n{}", - mc_rtc::io::to_string(m_steps, [](const Eigen::Vector3d & v) -> Eigen::RowVector3d { return v; }, "\n")); + comGenerator_->steps(steps_); + updateSteps(); using Color = mc_rtc::gui::Color; using Style = mc_rtc::gui::plot::Style; @@ -57,70 +56,73 @@ void CoMTrajectoryGeneration_Initial::start(mc_control::fsm::Controller & ctl) mc_rtc::gui::plot::X("t", [this]() { return t_; }), mc_rtc::gui::plot::Y("Output CoM", [this]() { return comGenerator_->OutputCOGPosition().y(); }, Color::Magenta), mc_rtc::gui::plot::Y("Ideal CoM", [this]() { return comGenerator_->IdealCOGPosition().y(); }, Color::Red, Style::Dashed), - mc_rtc::gui::plot::Y("Output ZMP", [this]() { return comGenerator_->OutputZMPPosition().y(); }, Color::Blue), - mc_rtc::gui::plot::Y("Ideal ZMP", [this]() { return comGenerator_->IdealZMPPosition().y(); }, Color::Cyan, Style::Dashed)); + mc_rtc::gui::plot::Y("Output ZMP", [this]() { return comGenerator_->OutputZMPPosition().y(); }, Color::Purple), + mc_rtc::gui::plot::Y("Ideal ZMP", [this]() { return comGenerator_->IdealZMPPosition().y(); }, Color::Blue, Style::Dashed)); ctl.gui()->addElement({"CoMTrajectoryGeneration"}, + mc_rtc::gui::NumberInput("Delay", + [this]() + { + return delay_; + }, + [this](double delay) + { + delay_ = delay; + }), + mc_rtc::gui::NumberInput("Transition time", + [this]() + { + return transition_; + }, + [this](double transition) + { + transition_ = transition; + }), mc_rtc::gui::Button("Left", [this,leftY]() { - const auto & curr = t_; - comGenerator_->changeFutureSteps(iter_, - { - Eigen::Vector3d{curr + previewTime_ + 0.8, 0, leftY}, - }); + auto curr = t_ + previewTime_ + delay_; + comGenerator_->changeFutureSteps( + curr, + { + {curr + transition_, {0, leftY}} + }); updateSteps(); }), mc_rtc::gui::Button("Right", [this,rightY]() { - const auto & curr = t_; - comGenerator_->changeFutureSteps(iter_, - { - Eigen::Vector3d{curr + previewTime_ + 0.8, 0, rightY}, - }); + auto curr= t_ + previewTime_ + delay_; + comGenerator_->changeFutureSteps( + curr, + { {curr + transition_, {0, rightY}} }); updateSteps(); }), mc_rtc::gui::Button("Center", [this,centerY]() { - const auto & curr = t_; - comGenerator_->changeFutureSteps(iter_, - { - Eigen::Vector3d{curr + previewTime_ + 0.8, 0, centerY}, - }); + auto curr= t_ + previewTime_ + delay_; + comGenerator_->changeFutureSteps( + curr, + { {curr + transition_, {0, centerY}} }); updateSteps(); }), mc_rtc::gui::Button("Left-Right-Left", [this,centerY,leftY,rightY]() { - const auto & curr = t_; - comGenerator_->changeFutureSteps(iter_, - { - Eigen::Vector3d{curr + previewTime_ + 0.2, 0, leftY}, - Eigen::Vector3d{curr + previewTime_ + 2, 0, leftY}, - Eigen::Vector3d{curr + previewTime_ + 2.2, 0, rightY}, - Eigen::Vector3d{curr + previewTime_ + 4, 0, rightY}, - Eigen::Vector3d{curr + previewTime_ + 4.2, 0, centerY}, - }); - updateSteps(); - }), - mc_rtc::gui::Button("Left-Right-Left (x10)", - [this,centerY,leftY,rightY]() - { - const auto & curr = t_; - std::vector futureSteps; - auto startT = curr; - for (int i = 0; i < 10; ++i) - { - futureSteps.push_back(Eigen::Vector3d{startT + previewTime_ + 0.2, 0, leftY}); - futureSteps.push_back(Eigen::Vector3d{startT + previewTime_ + 2, 0, leftY}); - futureSteps.push_back(Eigen::Vector3d{startT + previewTime_ + 2.2, 0, rightY}); - futureSteps.push_back(Eigen::Vector3d{startT + previewTime_ + 4, 0, rightY}); - startT += 4; - } - futureSteps.push_back(Eigen::Vector3d{startT + previewTime_ + 0.2, 0, centerY}); - comGenerator_->changeFutureSteps(iter_, futureSteps); + auto curr= t_ + previewTime_ + delay_; + std::vector> futureSteps; + auto t = curr + transition_; + futureSteps.push_back({t, {0, leftY}}); + t += 2; + futureSteps.push_back({t, {0, leftY}}); + t += transition_; + futureSteps.push_back({t, {0, rightY}}); + t += 2; + futureSteps.push_back({t, {0, rightY}}); + t += transition_; + futureSteps.push_back({t, {0, centerY}}); + comGenerator_->changeFutureSteps(curr, futureSteps); updateSteps(); }) ); @@ -134,10 +136,11 @@ void CoMTrajectoryGeneration_Initial::start(mc_control::fsm::Controller & ctl) void CoMTrajectoryGeneration_Initial::updateSteps() { - mc_rtc::log::info("Current time: {}", t_); + mc_rtc::log::info("Time start: {}", t_); + mc_rtc::log::info("Time curr : {}", t_+previewTime_); mc_rtc::log::info( "Desired steps:\nTime\tCoM X\tCoM Y\n{}", - mc_rtc::io::to_string(comGenerator_->steps(), [](const Eigen::Vector3d & v) -> Eigen::RowVector3d { return v; }, "\n")); + mc_rtc::io::to_string(comGenerator_->steps().steps(), "\n")); } bool CoMTrajectoryGeneration_Initial::run(mc_control::fsm::Controller & ctl) diff --git a/samples/CoMTrajectoryGeneration/src/states/CoMTrajectoryGeneration_Initial.h b/samples/CoMTrajectoryGeneration/src/states/CoMTrajectoryGeneration_Initial.h index a5405ca7d3..3c7802d08d 100644 --- a/samples/CoMTrajectoryGeneration/src/states/CoMTrajectoryGeneration_Initial.h +++ b/samples/CoMTrajectoryGeneration/src/states/CoMTrajectoryGeneration_Initial.h @@ -31,13 +31,16 @@ struct CoMTrajectoryGeneration_Initial : mc_control::fsm::State double t_ = 0; ///< Time elapsed since started double generationTime_ = 0; ///< Time spent generating the trajectory + double delay_ = 0.5; ///< Delay before moving manually + double transition_ = 0.2; ///< Transition time between steps (interpolation) + Eigen::Vector3d leftFootPos_ = Eigen::Vector3d::Zero(); Eigen::Vector3d rightFootPos_ = Eigen::Vector3d::Zero(); Eigen::Vector3d feetCenterPos_ = Eigen::Vector3d::Zero(); private: std::shared_ptr comGenerator_; - std::vector m_steps; ///< (time, com_x, com_y) + mc_planning::PreviewSteps steps_; ///< Foot steps defined as time, ZMP_x, ZMP_y }; } /* mc_samples */ diff --git a/src/mc_planning/generator.cpp b/src/mc_planning/generator.cpp index 4d87320206..0e0fcb6202 100644 --- a/src/mc_planning/generator.cpp +++ b/src/mc_planning/generator.cpp @@ -42,7 +42,7 @@ void generator::setupCOGHeight(unsigned n_current) if(n_current == 0) { m_ComInterp->push_back(0u, m_waist_height); - m_ComInterp->push_back(static_cast(std::lround(m_steps.back()(0) / m_dt)), m_waist_height); + m_ComInterp->push_back(static_cast(std::lround(m_steps.back().t() / m_dt)), m_waist_height); m_ComInterp->update(); } @@ -65,38 +65,38 @@ void generator::setupCOGHeight(unsigned n_current) void generator::setupTimeTrajectories(unsigned n_current) { + m_steps.nextWindow(n_current * m_dt); Eigen::VectorXd & px_ref = m_ipm_long[X].p_ref(); Eigen::VectorXd & py_ref = m_ipm_long[Y].p_ref(); - unsigned n_steps_loop = m_n_steps; for(unsigned i = 0; i < m_n_preview * 2 + 1; i++) { m_virtual_height[X](i) = 0.0; m_virtual_height[Y](i) = 0.0; const double currTime = static_cast(i + n_current) * m_dt; - const auto & prevStep = m_steps[n_steps_loop]; - const auto & nextStep = m_steps[n_steps_loop + 1]; - const double prevTime = prevStep(0); - const double nextTime = nextStep(0); + // Move to next step if necessary + m_steps.update(currTime); + const auto & prevStep = m_steps.previous(); - if(currTime >= prevTime && currTime < nextTime) + if(!m_steps.isLastStep()) { + const auto & nextStep = m_steps.next(); /** * @brief Interpolates the reference ZMP from one step to the next - * @param axis 1: Step CoM x, 2: Step CoM Y + * @param axis 0: Step CoM x, 0: Step CoM Y */ auto interpolateRefXY = [&](const Eigen::Index axis) { - return prevStep(axis) - + (nextStep(axis) - prevStep(axis)) * polynomial3((currTime - prevTime) / (nextTime - prevTime)); + return prevStep.step()(axis) + + (nextStep.step()(axis) - prevStep.step()(axis)) * polynomial3((currTime - prevStep.t()) / (nextStep.t() - prevStep.t())); }; - px_ref(i) = interpolateRefXY(1); - py_ref(i) = interpolateRefXY(2); + px_ref(i) = interpolateRefXY(0); + py_ref(i) = interpolateRefXY(1); } else { /* No previous step provided, start with the ZMP at the next step */ - px_ref(i) = nextStep(1); - py_ref(i) = nextStep(2); + px_ref(i) = prevStep.step().x(); + py_ref(i) = prevStep.step().y(); } m_ipm_long[X].w2(i) = @@ -104,22 +104,9 @@ void generator::setupTimeTrajectories(unsigned n_current) m_ipm_long[Y].w2(i) = (mc_rtc::constants::GRAVITY + m_cog_ddot_height[i]) / (m_cog_height[i] - m_virtual_height[Y](i)); - // We need one future step for computations, increment only - // till the second to last step - if(n_steps_loop < m_steps.size() - 2) - { - // If current time is after the next step time, move to next step - if(currTime >= nextTime) n_steps_loop++; - } } m_Pcalpha_ideal(Z) = (m_virtual_height[X](m_n_preview) + m_virtual_height[Y](m_n_preview)) / 2.0; - - double tm_cur = (double)n_current * m_dt; - if(m_n_steps < m_steps.size() - 2) - { - if(tm_cur > m_steps[m_n_steps + 1](0)) m_n_steps++; - } } void generator::generateTrajectories() @@ -193,44 +180,37 @@ void generator::generateTrajectories() void generator::generate(unsigned n_time) { - if(m_steps.empty()) + if(!m_steps.isValid()) { - mc_rtc::log::error_and_throw("[generator] No reference provided, call setSteps()"); + mc_rtc::log::error_and_throw("[generator] Invalid reference provided, call steps()"); } setupCOGHeight(n_time); setupTimeTrajectories(n_time); generateTrajectories(); } -void generator::push_back(const Eigen::Vector3d & step) -{ - if(step(0) <= m_steps.back()(0)) - { - mc_rtc::log::error("Invalid step time ({} <= {})", step(0), m_steps.back()(0)); - } - m_steps.push_back(step); -} - void generator::addToLogger(mc_rtc::Logger & logger) { // clang-format off + // Requested ideal trajectories logger.addLogEntry("generator_IdealCOGPosition", [this]() { return this->IdealCOGPosition(); }); - logger.addLogEntry("generator_CompensatedCOGPosition", [this]() { return this->CompensatedCOGPosition(); }); - logger.addLogEntry("generator_OutputCOGPosition", [this]() { return this->OutputCOGPosition(); }); logger.addLogEntry("generator_IdealZMPPosition", [this]() { return this->IdealZMPPosition(); }); - logger.addLogEntry("generator_CompensatedZMPPosition", [this]() { return this->CompensatedZMPPosition(); }); + // Generated trajectories + logger.addLogEntry("generator_OutputCOGPosition", [this]() { return this->OutputCOGPosition(); }); logger.addLogEntry("generator_OutputZMPPosition", [this]() { return this->OutputZMPPosition(); }); + logger.addLogEntry("generator_CompensatedCOGPosition", [this]() { return this->CompensatedCOGPosition(); }); + logger.addLogEntry("generator_CompensatedZMPPosition", [this]() { return this->CompensatedZMPPosition(); }); // clang-format on } void generator::removeFromLogger(mc_rtc::Logger & logger) { logger.removeLogEntry("generator_IdealCOGPosition"); - logger.removeLogEntry("generator_CompensatedCOGPosition"); - logger.removeLogEntry("generator_OutputCOGPosition"); logger.removeLogEntry("generator_IdealZMPPosition"); - logger.removeLogEntry("generator_CompensatedZMPPosition"); + logger.removeLogEntry("generator_OutputCOGPosition"); logger.removeLogEntry("generator_OutputZMPPosition"); + logger.removeLogEntry("generator_CompensatedCOGPosition"); + logger.removeLogEntry("generator_CompensatedZMPPosition"); } } /* namespace mc_planning */ diff --git a/src/mc_rtc/gui/StateBuilder.cpp b/src/mc_rtc/gui/StateBuilder.cpp index 9a541e90ad..628660b861 100644 --- a/src/mc_rtc/gui/StateBuilder.cpp +++ b/src/mc_rtc/gui/StateBuilder.cpp @@ -26,6 +26,7 @@ const Color Color::Magenta = Color(1, 0, 1, 1); const Color Color::Yellow = Color(1, 1, 0, 1); const Color Color::Gray = Color(0.6, 0.6, 0.6, 1); const Color Color::LightGray = Color(0.75, 0.75, 0.75, 1); +const Color Color::Purple = Color(0.5, 0, 1, 1); const std::map Color::ColorMap{ {"white", Color::White}, {"black", Color::Black}, {"red", Color::Red}, {"green", Color::Green}, {"blue", Color::Blue}, {"cyan", Color::Cyan}, {"magenta", Color::Magenta}, {"yellow", Color::Yellow}, diff --git a/tests/hmc/testCoMGenerator.cpp b/tests/hmc/testCoMGenerator.cpp index 80a6bb0c4d..24424d1da1 100644 --- a/tests/hmc/testCoMGenerator.cpp +++ b/tests/hmc/testCoMGenerator.cpp @@ -11,33 +11,30 @@ const unsigned n_preview = static_cast(std::lround(1.6 / dt)); int main(void) { - + // Trajectory of size 2*n_preview+1 generator com_traj(n_preview, dt); - // Trajectory of size 2*n_preview+1 - std::vector steps; - steps.push_back(Vector3((double)n_preview * dt, -0.2, 0.0)); - steps.push_back(steps.back() + Vector3{(double)n_preview * dt, 0.0, 0.0}); - steps.push_back(steps.back() + Vector3{0.1, 0.0, 0.0}); - steps.push_back(steps.back() + Vector3{1.6, 0.0, 0.0}); - steps.push_back(steps.back() + Vector3{0.1, 0.2, 0.095}); - steps.push_back(steps.back() + Vector3{1.6, 0.0, 0.0}); - steps.push_back(steps.back() + Vector3{0.1, 0.0, -0.19}); - steps.push_back(steps.back() + Vector3{1.6, 0.0, 0.0}); - steps.push_back(steps.back() + Vector3{0.1, -0.2, 0.095}); - steps.push_back(steps.back() + Vector3{(double)n_preview * dt, 0.0, 0.0}); - // Repeat last step for computations - steps.push_back(steps.back() + Vector3{(double)n_preview * dt, 0.0, 0.0}); + PreviewSteps steps; + steps.add({(double)n_preview * dt, {-0.2, 0.0}}); + steps.addRelative({(double)n_preview * dt, {0.0, 0.0}}); + steps.addRelative({0.1, {0.0, 0.0}}); + steps.addRelative({1.6, {0.0, 0.0}}); + steps.addRelative({0.1, {0.2, 0.095}}); + steps.addRelative({1.6, {0.0, 0.0}}); + steps.addRelative({0.1, {0.0, -0.19}}); + steps.addRelative({1.6, {0.0, 0.0}}); + steps.addRelative({0.1, {-0.2, 0.095}}); + steps.addRelative({(double)n_preview * dt, {0.0, 0.0}}); + steps.initialize(); + com_traj.steps(steps); - mc_rtc::log::info( - "Desired steps:\nTime\tCoM X\tCoM Y\n{}", - mc_rtc::io::to_string(steps, [](const Eigen::Vector3d & v) -> Eigen::RowVector3d { return v; }, "\n")); + mc_rtc::log::info("Desired steps:\nTime\tCoM X\tCoM Y\n{}", mc_rtc::io::to_string(steps.steps(), "\n")); mc_rtc::Logger logger(mc_rtc::Logger::Policy::NON_THREADED, "/tmp", "mc_rtc-test"); logger.start("CoMGenerator", dt); com_traj.addToLogger(logger); - unsigned n_loop = static_cast(std::lround(com_traj.steps().back()(0) / dt) - n_preview); + unsigned n_loop = static_cast(std::lround(com_traj.steps().back().t() / dt) - n_preview); mc_rtc::log::info("n_loop {}", n_loop); /* * Generate the CoM trajectory based on the parameters in com_traj @@ -51,6 +48,7 @@ int main(void) }; auto duration = mc_rtc::measure_ms::execution(comGeneration).count(); + mc_rtc::log::info("Desired steps:\nTime\tCoM X\tCoM Y\n{}", mc_rtc::io::to_string(com_traj.steps().steps(), "\n")); mc_rtc::log::info("calc time = {} (ms)", duration); mc_rtc::log::info("ave. calc time = {} (ms)", duration / static_cast(n_loop)); mc_rtc::log::success("end of com trajectory generation"); diff --git a/tests/test_hmc.cpp b/tests/test_hmc.cpp index d00b5392ce..ada69d1be9 100644 --- a/tests/test_hmc.cpp +++ b/tests/test_hmc.cpp @@ -8,6 +8,19 @@ #include +#include +#include + +template +bool allclose( + const Eigen::DenseBase & a, + const Eigen::DenseBase & b, + const typename DerivedA::RealScalar & rtol = Eigen::NumTraits::dummy_precision(), + const typename DerivedA::RealScalar & atol = Eigen::NumTraits::epsilon()) +{ + return ((a.derived() - b.derived()).array().abs() <= (atol + rtol * b.derived().array().abs())).all(); +} + template void testLookupTable(Fun f, double min, double max, size_t resolution, size_t nbTest, double precision = 1e-6) { @@ -51,3 +64,126 @@ BOOST_AUTO_TEST_CASE(TestLookupTable) testLookupTable([](double x) { return sqrt(x); }, 0., 1000., 2000, 500); } } + +BOOST_AUTO_TEST_CASE(TestPreviewSteps) +{ + using PreviewSteps = mc_planning::PreviewSteps; + PreviewSteps steps; + steps.add({0, {0.0, 0.0}}); + steps.add({1.5, {0.0, 1.5}}); + steps.add({1.6, {0.0, 1.6}}); + steps.add({2.9, {0.0, 2.9}}); + steps.add({3.0, {0.0, 3.0}}); + std::cout << "Steps:\n" << mc_rtc::io::to_string(steps.steps(), "\n") << std::endl; + + auto checkPrevious = [&](double t, double expected) { + std::cout << "Previous for t=" << t << ": " << steps.previous(t) << std::endl; + BOOST_REQUIRE_CLOSE(steps.previous(t).t(), expected, 1e-10); + }; + + checkPrevious(-0.5, 0.0); + checkPrevious(0, 0.0); + checkPrevious(1.6, 1.6); + checkPrevious(1.8, 1.6); + checkPrevious(2.9, 2.9); + checkPrevious(2.95, 2.9); + checkPrevious(3.0, 3.0); + checkPrevious(3.5, 3.0); + + auto checkNext = [&](double t, double expected) { + std::cout << "Next for t=" << t << ": " << steps.next(t) << std::endl; + BOOST_REQUIRE_CLOSE(steps.next(t).t(), expected, 1e-10); + }; + + checkNext(-0.5, 0); + checkNext(0.0, 1.5); + checkNext(0.5, 1.5); + checkNext(1.6, 2.9); + checkNext(1.8, 2.9); + checkNext(2.9, 3.0); + checkNext(2.95, 3.0); + checkNext(3.0, 3.0); + checkNext(3.5, 3.0); + + // Test replacing steps after a given value + { + auto steps2 = steps; + PreviewSteps::PreviewStepsSet newSteps; + newSteps.insert({1.6, {0.0, 11.6}}); + newSteps.insert({2.5, {0.0, 12.5}}); + newSteps.insert({3.0, {0.0, 13.0}}); + steps2.replaceAfter(newSteps); + std::cout << "New Steps:\n" << mc_rtc::io::to_string(steps2.steps(), "\n") << std::endl; + BOOST_REQUIRE(steps2.steps().size() == 5); + BOOST_REQUIRE(allclose((*std::next(steps2.steps().begin(), 0)).step(), Eigen::Vector2d{0.0, 0.0})); + BOOST_REQUIRE(allclose((*std::next(steps2.steps().begin(), 1)).step(), Eigen::Vector2d{0.0, 1.5})); + BOOST_REQUIRE(allclose((*std::next(steps2.steps().begin(), 2)).step(), Eigen::Vector2d{0.0, 11.6})); + BOOST_REQUIRE(allclose((*std::next(steps2.steps().begin(), 3)).step(), Eigen::Vector2d{0.0, 12.5})); + BOOST_REQUIRE(allclose((*std::next(steps2.steps().begin(), 4)).step(), Eigen::Vector2d{0.0, 13.0})); + } + + { + auto steps2 = steps; + PreviewSteps::PreviewStepsSet newSteps; + newSteps.insert({3.5, {0.0, 13.5}}); + steps2.replaceAfter(newSteps); + std::cout << "New Steps:\n" << mc_rtc::io::to_string(steps2.steps(), "\n") << std::endl; + BOOST_REQUIRE(steps2.steps().size() == steps.steps().size() + 1); + BOOST_REQUIRE(allclose((*std::next(steps2.steps().begin(), 0)).step(), Eigen::Vector2d{0.0, 0.0})); + BOOST_REQUIRE(allclose((*std::next(steps2.steps().begin(), 1)).step(), Eigen::Vector2d{0.0, 1.5})); + BOOST_REQUIRE(allclose((*std::next(steps2.steps().begin(), 2)).step(), Eigen::Vector2d{0.0, 1.6})); + BOOST_REQUIRE(allclose((*std::next(steps2.steps().begin(), 3)).step(), Eigen::Vector2d{0.0, 2.9})); + BOOST_REQUIRE(allclose((*std::next(steps2.steps().begin(), 4)).step(), Eigen::Vector2d{0.0, 3.0})); + BOOST_REQUIRE(allclose((*std::next(steps2.steps().begin(), 5)).step(), Eigen::Vector2d{0.0, 13.5})); + } +} + +BOOST_AUTO_TEST_CASE(TestPreviewStepsSequential) +{ + using PreviewSteps = mc_planning::PreviewSteps; + PreviewSteps steps; + steps.add({0, {0.0, 0.0}}); + steps.add({1.5, {0.0, 1.5}}); + steps.add({1.6, {0.0, 1.6}}); + steps.add({2.9, {0.0, 2.9}}); + steps.add({3.0, {0.0, 3.0}}); + steps.initialize(); + PreviewSteps stepsSeq = steps; + stepsSeq.initialize(); + std::cout << "Steps:\n" << mc_rtc::io::to_string(steps.steps(), "\n") << std::endl; + + double dt = 0.005; + unsigned n_loop = 5. / dt; + unsigned n_preview = 1.6 / dt; + for(unsigned loop = 0; loop <= n_loop; loop++) + { + auto previewStart = loop * dt; + mc_rtc::log::success("Start of new preview window at time: {:.3f}", previewStart); + stepsSeq.nextWindow(previewStart); + + for(unsigned previewWindow = loop; previewWindow < loop + 2 * n_preview + 1; ++previewWindow) + { + double t = previewWindow * dt; + // mc_rtc::log::info("t: {}", t); + stepsSeq.update(t); + + if(t >= steps.back().t()) + { + BOOST_REQUIRE(stepsSeq.isLastStep()); + } + else + { + BOOST_REQUIRE(!stepsSeq.isLastStep()); + } + + if(!stepsSeq.isLastStep()) + { + // mc_rtc::log::info("t: {}", t); + // mc_rtc::log::info("Steps:\n {}", mc_rtc::io::to_string(steps.steps(), "\n")); + // mc_rtc::log::info("prev(t): {:.3f}, prev(sew): {:.3f}", steps.previous(t).t(), stepsSeq.previous().t()); + BOOST_REQUIRE_CLOSE(steps.previous(t).t(), stepsSeq.previous().t(), 1e-10); + BOOST_REQUIRE_CLOSE(steps.next(t).t(), stepsSeq.next().t(), 1e-10); + } + } + } +} From a8596e14d5b10dd710370885280f411064cfb866 Mon Sep 17 00:00:00 2001 From: Arnaud TANGUY Date: Wed, 24 Jun 2020 18:10:30 +0900 Subject: [PATCH 25/43] [HMC/generator] Add benchmarks --- benchmarks/benchHMC.cpp | 110 +++++++++++++++++++++++++++++++- include/mc_planning/generator.h | 29 ++++----- 2 files changed, 120 insertions(+), 19 deletions(-) diff --git a/benchmarks/benchHMC.cpp b/benchmarks/benchHMC.cpp index 7dc05b1609..fcab787134 100644 --- a/benchmarks/benchHMC.cpp +++ b/benchmarks/benchHMC.cpp @@ -3,6 +3,7 @@ */ #include +#include #include #include "benchmark/benchmark.h" #include @@ -32,6 +33,7 @@ static void BM_cosh_lookuptable(benchmark::State & state) } state.SetItemsProcessed(10000 * state.iterations()); } +BENCHMARK(BM_cosh_lookuptable); static void BM_cosh(benchmark::State & state) { @@ -54,9 +56,111 @@ static void BM_cosh(benchmark::State & state) } state.SetItemsProcessed(10000 * state.iterations()); } - -// Register the function as a benchmark -BENCHMARK(BM_cosh_lookuptable); BENCHMARK(BM_cosh); + +class PreviewStepsFixture : public benchmark::Fixture +{ + using PreviewSteps = mc_planning::PreviewSteps; + +public: + void SetUp(const ::benchmark::State &) + { + steps = std::make_shared(); + steps->add({0, {0.0, 0.0}}); + steps->add({1.5, {0.0, 1.5}}); + steps->add({1.6, {0.0, 1.6}}); + steps->add({2.9, {0.0, 2.9}}); + steps->add({3.0, {0.0, 3.0}}); + steps->add({5.0, {0.0, 3.0}}); + steps->add({10.0, {0.0, 3.0}}); + steps->initialize(); + } + + void TearDown(const ::benchmark::State &) {} + + static constexpr unsigned nIter() + { + return n_loop * (2 * n_preview + 1); + } + + std::shared_ptr steps; + static constexpr double dt = 0.005; + static constexpr unsigned n_loop = 15. / dt; + static constexpr unsigned n_preview = 1.6 / dt; +}; + +BENCHMARK_DEFINE_F(PreviewStepsFixture, BM_PreviewSteps)(benchmark::State & state) +{ + while(state.KeepRunning()) + { + for(unsigned loop = 0; loop <= n_loop; loop++) + { + auto previewStart = loop * dt; + steps->nextWindow(previewStart); + + for(unsigned previewWindow = loop; previewWindow < loop + 2 * n_preview + 1; ++previewWindow) + { + double t = previewWindow * dt; + steps->update(t); + + if(!steps->isLastStep()) + { + mc_planning::TimedStep prev, next; + benchmark::DoNotOptimize(prev = steps->previous()); + benchmark::DoNotOptimize(next = steps->next()); + } + } + } + } + state.SetItemsProcessed(PreviewStepsFixture::nIter() * state.iterations()); +} +BENCHMARK_REGISTER_F(PreviewStepsFixture, BM_PreviewSteps)->Unit(benchmark::kMicrosecond); + +class GeneratorFixture : public benchmark::Fixture +{ +public: + using Generator = mc_planning::generator; + using PreviewSteps = mc_planning::PreviewSteps; + void SetUp(const ::benchmark::State &) + { + steps.add({(double)n_preview * dt, {-0.2, 0.0}}); + steps.addRelative({(double)n_preview * dt, {0.0, 0.0}}); + steps.addRelative({0.1, {0.0, 0.0}}); + steps.addRelative({1.6, {0.0, 0.0}}); + steps.addRelative({0.1, {0.2, 0.095}}); + steps.addRelative({1.6, {0.0, 0.0}}); + steps.addRelative({0.1, {0.0, -0.19}}); + steps.addRelative({1.6, {0.0, 0.0}}); + steps.addRelative({0.1, {-0.2, 0.095}}); + steps.addRelative({(double)n_preview * dt, {0.0, 0.0}}); + steps.initialize(); + generator = std::make_shared(n_preview, dt); + generator->steps(steps); + + n_loop = static_cast(std::lround(steps.back().t() / dt) - n_preview); + } + + void TearDown(const ::benchmark::State &) {} + + std::shared_ptr generator; + PreviewSteps steps; + static constexpr double dt = 0.005; + unsigned n_loop = 15. / dt; + static constexpr unsigned n_preview = 1.6 / dt; +}; + +BENCHMARK_DEFINE_F(GeneratorFixture, BM_Generator)(benchmark::State & state) +{ + for(auto _ : state) + { + for(unsigned loop = 0; loop <= n_loop; loop++) + { + generator->generate(loop); + } + } + state.SetItemsProcessed(n_loop * state.iterations()); +} +BENCHMARK_REGISTER_F(GeneratorFixture, BM_Generator)->Unit(benchmark::kMicrosecond); + // Run the benchmark BENCHMARK_MAIN(); diff --git a/include/mc_planning/generator.h b/include/mc_planning/generator.h index e1e221c12f..e14163583a 100644 --- a/include/mc_planning/generator.h +++ b/include/mc_planning/generator.h @@ -16,6 +16,7 @@ template struct TimedStep { TimedStep(double t, const Step & step) noexcept : t_(t), step_(step) {} + TimedStep() noexcept {} const double & t() const noexcept { @@ -75,7 +76,7 @@ struct TimedStep } protected: - double t_; + double t_ = 0; Step step_; }; @@ -280,7 +281,6 @@ struct MC_PLANNING_DLLAPI PreviewSteps { previous_ = steps_.begin(); next_ = std::next(previous_); - mc_rtc::log::info("Init {}, {}", previous_->t(), next_->t()); } } @@ -309,14 +309,8 @@ struct MC_PLANNING_DLLAPI PreviewSteps { if(!isLastStep() && t >= next_->t()) { - // mc_rtc::log::info("Change step"); previous_ = next_; ++next_; - // if(next_ == steps_.end()) - // { // only happens when the end of the step plan is in the current preview horizon (rare) - // next_ = previous_; - // } - // mc_rtc::log::info("After Change step: {}, {}", previous().t(), next().t()); } } @@ -348,15 +342,11 @@ struct MC_PLANNING_DLLAPI PreviewSteps previous_ = steps_.begin(); next_ = std::next(previous_); - // mc_rtc::log::info("Rewinding for next window: prev {}, next {}", previous(), next()); } /** * @brief Returns the previous step w.r.t last call to update(double t) * - * @warning If no next step is available, then next() = previous(). You can - * check whether this is the last step ahead of time by calling isLastStep() - * * @return Next step */ const TimedStep & previous() const noexcept @@ -367,14 +357,21 @@ struct MC_PLANNING_DLLAPI PreviewSteps /** * @brief Returns the next step w.r.w last call to update(double t) * - * @warning If no next step is available, then next() = previous(). You can - * check whether this is the last step ahead of time by calling isLastStep() + * @throws std::runtime_error If no next step is available. Use isLastStep() + * to check beforehand. * * @return Next step */ - const TimedStep & next() const noexcept + const TimedStep & next() const { - return *next_; + if(!isLastStep()) + { + return *next_; + } + else + { + mc_rtc::log::error_and_throw("Can't access a step past the end"); + } } /** @} */ // end of group real-time From eed398f842cd12aab597cc730b12249ab9177501 Mon Sep 17 00:00:00 2001 From: Arnaud TANGUY Date: Thu, 25 Jun 2020 10:07:01 +0900 Subject: [PATCH 26/43] [HMC/clean] Move PreviewSteps to its own header --- include/mc_planning/PreviewSteps.h | 454 +++++++++++++++++++++++++++++ include/mc_planning/generator.h | 447 +--------------------------- src/CMakeLists.txt | 1 + src/mc_planning/generator.cpp | 1 - 4 files changed, 457 insertions(+), 446 deletions(-) create mode 100644 include/mc_planning/PreviewSteps.h diff --git a/include/mc_planning/PreviewSteps.h b/include/mc_planning/PreviewSteps.h new file mode 100644 index 0000000000..34f46c6155 --- /dev/null +++ b/include/mc_planning/PreviewSteps.h @@ -0,0 +1,454 @@ +/* + * Copyright 2015-2020 CNRS-UM LIRMM, CNRS-AIST JRL + */ + +#pragma once +#include +#include +#include +#include + +namespace mc_planning +{ + +template +struct TimedStep +{ + TimedStep(double t, const Step & step) noexcept : t_(t), step_(step) {} + TimedStep() noexcept {} + + const double & t() const noexcept + { + return t_; + } + + double & t() noexcept + { + return t_; + } + + const Step & step() const noexcept + { + return step_; + } + + Step & step() noexcept + { + return step_; + } + + // For std::set + bool operator<(const TimedStep & t) const noexcept + { + return (this->t_ < t.t_); + } + + bool operator<=(const TimedStep & t) const noexcept + { + return (this->t_ <= t.t_); + } + + void operator+(const TimedStep & t) noexcept + { + t_ += t.t(); + step_ += t.step(); + } + + TimedStep & operator+=(const TimedStep & rhs) noexcept + { + t_ += rhs.t(); + step_ += rhs.step(); + return *this; + } + + void operator-(const TimedStep & t) noexcept + { + t_ += t.t(); + step_ += t.step(); + } + + TimedStep & operator-=(const TimedStep & rhs) noexcept + { + t_ -= rhs.t(); + step_ -= rhs.step(); + return *this; + } + +protected: + double t_ = 0; + Step step_; +}; + +template +bool operator<(const TimedStep & s1, const TimedStep & s2) +{ + return s1 < s2; +} + +template +std::ostream & operator<<(std::ostream & os, const TimedStep & step) +{ + os << step.t() << " -> " << step.step().transpose(); + return os; +} + +/** + * @brief Helper class to manage a list of discrete steps and provide efficient + * sequential lookup of next/previous step. + * + * Steps have the following properties: + * - Each step is defined by + * - It's start time Step::t() + * - It's value Step::step() (may be any arbitrary type) + * - Steps are discrete events and do not have a specified duration + * - Steps are sorted from oldest to most recent + * + * Example + * \code{.cpp} + * PreviewSteps steps; + * steps.add( {1.6, {-0.2, 0.0}}); + * steps.addRelative({1.6, { 0.0, 0.0}}); + * steps.addRelative({0.1, { 0.0, 0.0}}); + * steps.addRelative({1.6, { 0.0, 0.0}}); + * steps.addRelative({0.1, { 0.2, 0.095}}); + * steps.addRelative({1.6, { 0.0, 0.0}}); + * steps.addRelative({0.1, { 0.0, -0.19}}); + * steps.addRelative({1.6, { 0.0, 0.0}}); + * steps.addRelative({0.1, {-0.2, 0.095}}); + * steps.addRelative({1.6, { 0.0, 0.0}}); + * steps.initialize(); + * mc_rtc::log::info("Desired steps:\nTime\tCoM X\tCoM Y\n{}", mc_rtc::io::to_string(steps.steps(), "\n")); + * + * double dt = 0.005; + * double previewTime = 1.6; + * const unsigned n_preview = static_cast(std::lround(previewTime / dt)); + * + * // Iterate over a preview window and perform computations + * // based on the previous and next steps + * // In a real-world use, this would most likely be used to compute a + * // smooth trajectory between the discrete steps. + * auto iteratePreview = [&](double startTime) + * { + * for(unsigned i = 0; i < n_preview * 2 + 1; i++) + * { + * double currTime = startTime + i*dt; + * // Update time within the preview window + * steps.update(currTime); + * if(!steps.isLastStep()) + * { + * const auto & nextStep = steps.next(); + * const auto & prevStep = steps.previous(); + * // Do something with next and previous steps, such as interpolate + * // between them to generate a smooth reference trajectory + * } + * else + * { + * // We are past the last step, handle this case + * const auto & lastStep = steps.previous(); + * } + * } + * } + * + * // Real-time control loop + * bool run() + * { + * // Start next preview window + * steps.nextWindow(t); + * iteratePreview(t); + * t+=dt; + * // Now you can modify future steps for the next preview window + * double transitionTime = 0.2; // 0.2 second to transition + * double delay = 0.5; // Transition in 0.5s in the future. The previous step + * will be extended until the transition starts. + * steps.changeFutureSteps( + * { + * t+previewTime+delay, // Extend the previous step until this time + * {t+previewTime+delay+transitionTime, {0., -0.15}} // Transition to the + * next steps + * }); + * } + * \endcode + */ +template +struct MC_PLANNING_DLLAPI PreviewSteps +{ + using value_type = TimedStep; + using PreviewStepsSet = std::set>; + using iterator = typename PreviewStepsSet::iterator; + using reverse_iterator = typename PreviewStepsSet::reverse_iterator; + + /** + * @brief Inserts a new step + * + * A step may be added at any time, including within the current preview + * window + * + * @param step Step to add + */ + void add(const TimedStep & step) + { + if(step.t() < (*steps_.begin()).t()) + { + mc_rtc::log::error("[PreviewSteps] can't create a step before the start of the preview window"); + return; + } + steps_.insert(step); + } + + void reset(const PreviewStepsSet & steps) + { + steps_ = steps; + } + + /** + * @brief Adds a step relative to the last step + * + * @throws std::runtime_error If steps() is empty + * + * @param step Relative step to add + */ + void addRelative(const TimedStep & step) + { + if(steps_.empty()) + { + mc_rtc::log::error_and_throw("[PreviewSteps] can't add a relative step to an empty set"); + return; + } + auto last = this->back(); + last += step; + add(last); + } + + /** + * @brief Replaces all footsteps following the first of the new provided steps + * + * @param nextSteps + */ + void replaceAfter(const PreviewStepsSet & nextSteps) + { + if(nextSteps.empty()) return; + deleteAfter(nextSteps.begin()->t()); + std::copy(nextSteps.begin(), nextSteps.end(), std::inserter(steps_, steps_.begin())); + } + + void deleteAfter(double t) + { + auto it = std::find_if(steps_.begin(), steps_.end(), [t](const value_type & step) { return step.t() >= t; }); + if(it != steps_.end()) + { + steps_.erase(it, steps_.end()); + } + } + + void changeFutureSteps(double startTransition, const std::vector & steps) + { + mc_rtc::log::info("Delete after {}", startTransition); + deleteAfter(startTransition); + // Extend previous step until start of transition + auto transitionStep = back(); + mc_rtc::log::info("Duplicating previous step {}", transitionStep); + transitionStep.t() = startTransition; + mc_rtc::log::info("Pre-Transition step {}", transitionStep); + steps_.insert(transitionStep); + mc_rtc::log::info("Inserting steps"); + for(const auto & step : steps) + { + mc_rtc::log::info("Step {}", step); + steps_.insert(step); + } + } + + /** + * @brief To be called after adding the initial footsteps + * + * Requires at least one footstep. + * + * @throws std::runtime_error if there isn't at least one footstep + */ + void initialize() + { + if(steps_.size() < 1) + { + mc_rtc::log::error_and_throw("[PreviewSteps] requires at least one step"); + } + else if(steps_.size() == 1) + { + previous_ = steps_.begin(); + next_ = previous_; + } + else + { + previous_ = steps_.begin(); + next_ = std::next(previous_); + } + } + + /** + * @name Real-time functions + * These functions are intended to be called sequentially while iterating over the preview window in the following + * way:
  • nextWindow(double t): Sets up a new preview window starting at time t, updates the starting step, and + * removes old steps prior to t
  • for each time in the preview window:
    • next(), previous() provide the + * previous/next step for the current time. Warning: if the last step is reached (`isLastStep() == true`), then next() + * = previous()
    • update(double t): move along the preview window
      • + *
    + *
    • + *
  • Repeat with the next preview window
    • + *
+ * @{ + */ + + /** + * @brief Updates the time along the preview window + * + * Finds the previous and next step for the current time in the preview window + * + * @param t Curent time within the preview window + */ + void update(double t) noexcept + { + if(!isLastStep() && t >= next_->t()) + { + previous_ = next_; + ++next_; + } + } + + /** + * @brief Whether we reached the last step + * + * @waning When on the last step, there is no next step available, and next() == previous() + */ + bool isLastStep() const noexcept + { + return next_ == steps_.end(); + } + + /** + * @brief Moves to the next preview window starting at time t + * + * Removes old footsteps outside of the window, and update previous/next + * steps accordingly + * + * @param t Time of the start of the preview window + */ + void nextWindow(double t) + { + // Check whether old footsteps need to be removed + if(steps_.size() > 2 && t > std::next(steps_.begin())->t()) + { + steps_.erase(steps_.begin()); + } + + previous_ = steps_.begin(); + next_ = std::next(previous_); + } + + /** + * @brief Returns the previous step w.r.t last call to update(double t) + * + * @return Next step + */ + const TimedStep & previous() const noexcept + { + return *previous_; + } + + /** + * @brief Returns the next step w.r.w last call to update(double t) + * + * @throws std::runtime_error If no next step is available. Use isLastStep() + * to check beforehand. + * + * @return Next step + */ + const TimedStep & next() const + { + if(!isLastStep()) + { + return *next_; + } + else + { + mc_rtc::log::error_and_throw("Can't access a step past the end"); + } + } + /** @} */ // end of group real-time + + /** + * @brief Get the closest footstep prior to time t + * + * @param t Lookup time + * + * @note This funciton is not intended for real-time performace and has a + * complexity in `O(steps)` + * + * @return Nearest past footstep with time <= t + */ + const TimedStep & previous(double t) const noexcept + { + if(t >= back().t()) return back(); + auto it = + std::find_if(steps_.rbegin(), steps_.rend(), [&t](const TimedStep & step) { return step.t() <= t; }); + if(it != steps_.rend()) + { + return *it; + } + else + { + return *steps_.begin(); + } + } + + /** + * @brief Get the closest future footstep after time t + * + * @param t Lookup time + * + * @note This function is not intended for real-time performace and has a + * complexity in `O(steps)`. + * + * @return Nearest future footstep with time <= t + */ + const TimedStep & next(double t) const noexcept + { + auto it = std::find_if(steps_.begin(), steps_.end(), [&t](const TimedStep & step) { return step.t() >= t; }); + if(it != steps_.end()) + { + return *it; + } + return previous(); + } + + const PreviewStepsSet & steps() const noexcept + { + return steps_; + } + + const TimedStep & back() const noexcept + { + return *std::prev(steps_.end()); + } + + const TimedStep & front() const noexcept + { + return *steps_.begin(); + } + + bool isValid() const noexcept + { + return steps_.size() > 1; + } + +protected: + PreviewStepsSet steps_; // Ordered set of pairs of [time, Step] + + /** + * @name Real-time update + * @{ + */ + typename PreviewStepsSet::iterator previous_ = std::end(steps_); ///< Previous footstep + typename PreviewStepsSet::iterator next_ = std::end(steps_); ///< Next footstep + ///@} +}; + +} // namespace mc_planning diff --git a/include/mc_planning/generator.h b/include/mc_planning/generator.h index e14163583a..aa1e1c235f 100644 --- a/include/mc_planning/generator.h +++ b/include/mc_planning/generator.h @@ -1,10 +1,8 @@ #pragma once -// #include -// #include -// #include #include #include #include +#include #include #include #include @@ -12,446 +10,6 @@ namespace mc_planning { -template -struct TimedStep -{ - TimedStep(double t, const Step & step) noexcept : t_(t), step_(step) {} - TimedStep() noexcept {} - - const double & t() const noexcept - { - return t_; - } - - double & t() noexcept - { - return t_; - } - - const Step & step() const noexcept - { - return step_; - } - - Step & step() noexcept - { - return step_; - } - - // For std::set - bool operator<(const TimedStep & t) const noexcept - { - return (this->t_ < t.t_); - } - - bool operator<=(const TimedStep & t) const noexcept - { - return (this->t_ <= t.t_); - } - - void operator+(const TimedStep & t) noexcept - { - t_ += t.t(); - step_ += t.step(); - } - - TimedStep & operator+=(const TimedStep & rhs) noexcept - { - t_ += rhs.t(); - step_ += rhs.step(); - return *this; - } - - void operator-(const TimedStep & t) noexcept - { - t_ += t.t(); - step_ += t.step(); - } - - TimedStep & operator-=(const TimedStep & rhs) noexcept - { - t_ -= rhs.t(); - step_ -= rhs.step(); - return *this; - } - -protected: - double t_ = 0; - Step step_; -}; - -template -bool operator<(const TimedStep & s1, const TimedStep & s2) -{ - return s1 < s2; -} - -template -std::ostream & operator<<(std::ostream & os, const TimedStep & step) -{ - os << step.t() << " -> " << step.step().transpose(); - return os; -} - -/** - * @brief Helper class to manage a list of discrete steps and provide efficient - * sequential lookup of next/previous step. - * - * Steps have the following properties: - * - Each step is defined by - * - It's start time Step::t() - * - It's value Step::step() (may be any arbitrary type) - * - Steps are discrete events and do not have a specified duration - * - Steps are sorted from oldest to most recent - * - * Example - * \code{.cpp} - * PreviewSteps steps; - * steps.add( {1.6, {-0.2, 0.0}}); - * steps.addRelative({1.6, { 0.0, 0.0}}); - * steps.addRelative({0.1, { 0.0, 0.0}}); - * steps.addRelative({1.6, { 0.0, 0.0}}); - * steps.addRelative({0.1, { 0.2, 0.095}}); - * steps.addRelative({1.6, { 0.0, 0.0}}); - * steps.addRelative({0.1, { 0.0, -0.19}}); - * steps.addRelative({1.6, { 0.0, 0.0}}); - * steps.addRelative({0.1, {-0.2, 0.095}}); - * steps.addRelative({1.6, { 0.0, 0.0}}); - * steps.initialize(); - * mc_rtc::log::info("Desired steps:\nTime\tCoM X\tCoM Y\n{}", mc_rtc::io::to_string(steps.steps(), "\n")); - * - * double dt = 0.005; - * double previewTime = 1.6; - * const unsigned n_preview = static_cast(std::lround(previewTime / dt)); - * - * // Iterate over a preview window and perform computations - * // based on the previous and next steps - * // In a real-world use, this would most likely be used to compute a - * // smooth trajectory between the discrete steps. - * auto iteratePreview = [&](double startTime) - * { - * for(unsigned i = 0; i < n_preview * 2 + 1; i++) - * { - * double currTime = startTime + i*dt; - * // Update time within the preview window - * steps.update(currTime); - * if(!steps.isLastStep()) - * { - * const auto & nextStep = steps.next(); - * const auto & prevStep = steps.previous(); - * // Do something with next and previous steps, such as interpolate - * // between them to generate a smooth reference trajectory - * } - * else - * { - * // We are past the last step, handle this case - * const auto & lastStep = steps.previous(); - * } - * } - * } - * - * // Real-time control loop - * bool run() - * { - * // Start next preview window - * steps.nextWindow(t); - * iteratePreview(t); - * t+=dt; - * // Now you can modify future steps for the next preview window - * double transitionTime = 0.2; // 0.2 second to transition - * double delay = 0.5; // Transition in 0.5s in the future. The previous step - * will be extended until the transition starts. - * steps.changeFutureSteps( - * { - * t+previewTime+delay, // Extend the previous step until this time - * {t+previewTime+delay+transitionTime, {0., -0.15}} // Transition to the - * next steps - * }); - * } - * \endcode - */ -template -struct MC_PLANNING_DLLAPI PreviewSteps -{ - using value_type = TimedStep; - using PreviewStepsSet = std::set>; - using iterator = typename PreviewStepsSet::iterator; - using reverse_iterator = typename PreviewStepsSet::reverse_iterator; - - /** - * @brief Inserts a new step - * - * A step may be added at any time, including within the current preview - * window - * - * @param step Step to add - */ - void add(const TimedStep & step) - { - if(step.t() < (*steps_.begin()).t()) - { - mc_rtc::log::error("[PreviewSteps] can't create a step before the start of the preview window"); - return; - } - steps_.insert(step); - } - - void reset(const PreviewStepsSet & steps) - { - steps_ = steps; - } - - /** - * @brief Adds a step relative to the last step - * - * @throws std::runtime_error If steps() is empty - * - * @param step Relative step to add - */ - void addRelative(const TimedStep & step) - { - if(steps_.empty()) - { - mc_rtc::log::error_and_throw("[PreviewSteps] can't add a relative step to an empty set"); - return; - } - auto last = this->back(); - last += step; - add(last); - } - - /** - * @brief Replaces all footsteps following the first of the new provided steps - * - * @param nextSteps - */ - void replaceAfter(const PreviewStepsSet & nextSteps) - { - if(nextSteps.empty()) return; - deleteAfter(nextSteps.begin()->t()); - std::copy(nextSteps.begin(), nextSteps.end(), std::inserter(steps_, steps_.begin())); - } - - void deleteAfter(double t) - { - auto it = std::find_if(steps_.begin(), steps_.end(), [t](const value_type & step) { return step.t() >= t; }); - if(it != steps_.end()) - { - steps_.erase(it, steps_.end()); - } - } - - void changeFutureSteps(double startTransition, const std::vector & steps) - { - mc_rtc::log::info("Delete after {}", startTransition); - deleteAfter(startTransition); - // Extend previous step until start of transition - auto transitionStep = back(); - mc_rtc::log::info("Duplicating previous step {}", transitionStep); - transitionStep.t() = startTransition; - mc_rtc::log::info("Pre-Transition step {}", transitionStep); - steps_.insert(transitionStep); - mc_rtc::log::info("Inserting steps"); - for(const auto & step : steps) - { - mc_rtc::log::info("Step {}", step); - steps_.insert(step); - } - } - - /** - * @brief To be called after adding the initial footsteps - * - * Requires at least one footstep. - * - * @throws std::runtime_error if there isn't at least one footstep - */ - void initialize() - { - if(steps_.size() < 1) - { - mc_rtc::log::error_and_throw("[PreviewSteps] requires at least one step"); - } - else if(steps_.size() == 1) - { - previous_ = steps_.begin(); - next_ = previous_; - } - else - { - previous_ = steps_.begin(); - next_ = std::next(previous_); - } - } - - /** - * @name Real-time functions - * These functions are intended to be called sequentially while iterating over the preview window in the following - * way:
  • nextWindow(double t): Sets up a new preview window starting at time t, updates the starting step, and - * removes old steps prior to t
  • for each time in the preview window:
    • next(), previous() provide the - * previous/next step for the current time. Warning: if the last step is reached (`isLastStep() == true`), then next() - * = previous()
    • update(double t): move along the preview window
      • - *
    - *
    • - *
  • Repeat with the next preview window
    • - *
- * @{ - */ - - /** - * @brief Updates the time along the preview window - * - * Finds the previous and next step for the current time in the preview window - * - * @param t Curent time within the preview window - */ - void update(double t) noexcept - { - if(!isLastStep() && t >= next_->t()) - { - previous_ = next_; - ++next_; - } - } - - /** - * @brief Whether we reached the last step - * - * @waning When on the last step, there is no next step available, and next() == previous() - */ - bool isLastStep() const noexcept - { - return next_ == steps_.end(); - } - - /** - * @brief Moves to the next preview window starting at time t - * - * Removes old footsteps outside of the window, and update previous/next - * steps accordingly - * - * @param t Time of the start of the preview window - */ - void nextWindow(double t) - { - // Check whether old footsteps need to be removed - if(steps_.size() > 2 && t > std::next(steps_.begin())->t()) - { - steps_.erase(steps_.begin()); - } - - previous_ = steps_.begin(); - next_ = std::next(previous_); - } - - /** - * @brief Returns the previous step w.r.t last call to update(double t) - * - * @return Next step - */ - const TimedStep & previous() const noexcept - { - return *previous_; - } - - /** - * @brief Returns the next step w.r.w last call to update(double t) - * - * @throws std::runtime_error If no next step is available. Use isLastStep() - * to check beforehand. - * - * @return Next step - */ - const TimedStep & next() const - { - if(!isLastStep()) - { - return *next_; - } - else - { - mc_rtc::log::error_and_throw("Can't access a step past the end"); - } - } - /** @} */ // end of group real-time - - /** - * @brief Get the closest footstep prior to time t - * - * @param t Lookup time - * - * @note This funciton is not intended for real-time performace and has a - * complexity in `O(steps)` - * - * @return Nearest past footstep with time <= t - */ - const TimedStep & previous(double t) const noexcept - { - if(t >= back().t()) return back(); - auto it = - std::find_if(steps_.rbegin(), steps_.rend(), [&t](const TimedStep & step) { return step.t() <= t; }); - if(it != steps_.rend()) - { - return *it; - } - else - { - return *steps_.begin(); - } - } - - /** - * @brief Get the closest future footstep after time t - * - * @param t Lookup time - * - * @note This function is not intended for real-time performace and has a - * complexity in `O(steps)`. - * - * @return Nearest future footstep with time <= t - */ - const TimedStep & next(double t) const noexcept - { - auto it = std::find_if(steps_.begin(), steps_.end(), [&t](const TimedStep & step) { return step.t() >= t; }); - if(it != steps_.end()) - { - return *it; - } - return previous(); - } - - const PreviewStepsSet & steps() const noexcept - { - return steps_; - } - - const TimedStep & back() const noexcept - { - return *std::prev(steps_.end()); - } - - const TimedStep & front() const noexcept - { - return *steps_.begin(); - } - - bool isValid() const noexcept - { - return steps_.size() > 1; - } - -protected: - PreviewStepsSet steps_; // Ordered set of pairs of [time, Step] - - /** - * @name Real-time update - * @{ - */ - typename PreviewStepsSet::iterator previous_ = std::end(steps_); ///< Previous footstep - typename PreviewStepsSet::iterator next_ = std::end(steps_); ///< Next footstep - ///@} -}; - /** * @brief Utility class to generate long and short term trajectories for the CoM. * @@ -522,8 +80,7 @@ struct MC_PLANNING_DLLAPI generator * @param transitionTime Time at which the transition should occur * @param futureSteps Future steps */ - void changeFutureSteps(double transitionTime, - const std::vector> & futureSteps) + void changeFutureSteps(double transitionTime, const std::vector> & futureSteps) { m_steps.changeFutureSteps(transitionTime, futureSteps); } diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index edd2c823bc..b196514d38 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -431,6 +431,7 @@ set(mc_planning_HDR ${mc_planning_HDR_DIR}/CubicSplineBase.h ${mc_planning_HDR_DIR}/ClampedCubicSpline.h ${mc_planning_HDR_DIR}/generator.h + ${mc_planning_HDR_DIR}/PreviewSteps.h ) add_library(mc_planning SHARED ${mc_planning_SRC} ${mc_planning_HDR}) diff --git a/src/mc_planning/generator.cpp b/src/mc_planning/generator.cpp index 0e0fcb6202..52bdd9df96 100644 --- a/src/mc_planning/generator.cpp +++ b/src/mc_planning/generator.cpp @@ -103,7 +103,6 @@ void generator::setupTimeTrajectories(unsigned n_current) (mc_rtc::constants::GRAVITY + m_cog_ddot_height[i]) / (m_cog_height[i] - m_virtual_height[X](i)); m_ipm_long[Y].w2(i) = (mc_rtc::constants::GRAVITY + m_cog_ddot_height[i]) / (m_cog_height[i] - m_virtual_height[Y](i)); - } m_Pcalpha_ideal(Z) = (m_virtual_height[X](m_n_preview) + m_virtual_height[Y](m_n_preview)) / 2.0; From aa221b6a0713ab63824281b97e8b3117742ae35c Mon Sep 17 00:00:00 2001 From: Arnaud TANGUY Date: Thu, 25 Jun 2020 14:16:50 +0900 Subject: [PATCH 27/43] [HMC/bugfix] Fix bug in PreviewSteps::next --- include/mc_planning/PreviewSteps.h | 22 ++++++---------------- tests/test_hmc.cpp | 6 +++--- 2 files changed, 9 insertions(+), 19 deletions(-) diff --git a/include/mc_planning/PreviewSteps.h b/include/mc_planning/PreviewSteps.h index 34f46c6155..8e1ede8181 100644 --- a/include/mc_planning/PreviewSteps.h +++ b/include/mc_planning/PreviewSteps.h @@ -389,14 +389,7 @@ struct MC_PLANNING_DLLAPI PreviewSteps if(t >= back().t()) return back(); auto it = std::find_if(steps_.rbegin(), steps_.rend(), [&t](const TimedStep & step) { return step.t() <= t; }); - if(it != steps_.rend()) - { - return *it; - } - else - { - return *steps_.begin(); - } + return (it != steps_.rend()) ? *it : front(); } /** @@ -407,16 +400,13 @@ struct MC_PLANNING_DLLAPI PreviewSteps * @note This function is not intended for real-time performace and has a * complexity in `O(steps)`. * - * @return Nearest future footstep with time <= t + * @return Nearest future footstep with step.time > t, or last element if t is + * later than the last step */ - const TimedStep & next(double t) const noexcept + const TimedStep & next(double t) const { - auto it = std::find_if(steps_.begin(), steps_.end(), [&t](const TimedStep & step) { return step.t() >= t; }); - if(it != steps_.end()) - { - return *it; - } - return previous(); + auto it = std::find_if(steps_.begin(), steps_.end(), [&t](const TimedStep & step) { return step.t() > t; }); + return (it != steps_.end()) ? *it : back(); } const PreviewStepsSet & steps() const noexcept diff --git a/tests/test_hmc.cpp b/tests/test_hmc.cpp index ada69d1be9..e24d2c46a2 100644 --- a/tests/test_hmc.cpp +++ b/tests/test_hmc.cpp @@ -153,12 +153,12 @@ BOOST_AUTO_TEST_CASE(TestPreviewStepsSequential) std::cout << "Steps:\n" << mc_rtc::io::to_string(steps.steps(), "\n") << std::endl; double dt = 0.005; - unsigned n_loop = 5. / dt; - unsigned n_preview = 1.6 / dt; + unsigned n_loop = (unsigned)std::lround(5. / dt); + unsigned n_preview = (unsigned)std::lround(1.6 / dt); for(unsigned loop = 0; loop <= n_loop; loop++) { auto previewStart = loop * dt; - mc_rtc::log::success("Start of new preview window at time: {:.3f}", previewStart); + // mc_rtc::log::success("Start of new preview window at time: {:.3f}", previewStart); stepsSeq.nextWindow(previewStart); for(unsigned previewWindow = loop; previewWindow < loop + 2 * n_preview + 1; ++previewWindow) From ca8aaced7f862f44e2b4364518461dac3a9845c8 Mon Sep 17 00:00:00 2001 From: Arnaud TANGUY Date: Thu, 25 Jun 2020 16:50:16 +0900 Subject: [PATCH 28/43] [HMC/convenience] Iterable preview windows --- include/mc_planning/PreviewWindow.h | 200 +++++++++++++++++++++ include/mc_planning/RangeElementIterator.h | 73 ++++++++ src/CMakeLists.txt | 4 +- src/mc_planning/PreviewWindow.cpp | 15 ++ tests/test_hmc.cpp | 42 +++++ 5 files changed, 333 insertions(+), 1 deletion(-) create mode 100644 include/mc_planning/PreviewWindow.h create mode 100644 include/mc_planning/RangeElementIterator.h create mode 100644 src/mc_planning/PreviewWindow.cpp diff --git a/include/mc_planning/PreviewWindow.h b/include/mc_planning/PreviewWindow.h new file mode 100644 index 0000000000..aefa49aff6 --- /dev/null +++ b/include/mc_planning/PreviewWindow.h @@ -0,0 +1,200 @@ +/* + * Copyright 2015-2020 CNRS-UM LIRMM, CNRS-AIST JRL + */ + +#pragma once +#include +#include +#include + +namespace mc_planning +{ + +struct CenteredPreviewWindow; + +/** + * @brief Element returned by CenteredPreviewWindow::iterator providing the current + * index and allowing to conveniently get the corresponding time + */ +struct MC_PLANNING_DLLAPI PreviewElement +{ + explicit PreviewElement(const CenteredPreviewWindow & window, unsigned index) noexcept + : window_(window), index_(index) + { + } + + unsigned index() const noexcept + { + return index_; + } + + double time() const noexcept; + +protected: + const CenteredPreviewWindow & window_; + unsigned index_; +}; + +/** + * @brief Iterable range over a preview window with convenience time-management. + * + * The preview window is centered around the current element \f$ [-N, current, N] \f$ where `N` is the number of + * elements before and after the current value. The full preview window is of size `2*N+1`. The timestep is used to + * compute correspondances between discrete index of values stored in an (external) container and time. + * + * \code{.cpp} + * // Create an iterable preview window range with a window of time `2*1.6`, + * // timestep of 0.005 and starting at time `t=0.5` + * CenteredPreviewWindow window(1.6, 0.005, 0.5); + * mc_rtc::log::info("Start time: {:.3f}, index: {}\n" + * "End time: {:.3f}, index: {}\n" + * "Current time: {:.3f}, index: {}", + * window.startTime(), window.startIndex(), + * window.endTime(), window.endIndex(), + * window.currentTime(), window.currentIndex()); + * + * // Iterate over the preview window using a RangeElementIterator + * for(const auto & w : window) + * { + * mc_rtc::log::info("Preview element with index: {}, time: {:.3f}", w.index(), w.time()); + * // Example: Access some external container using the index + * // auto value = container[w.index()]; + * } + * \endcode + * + * \see For practical use, see: + * \see mc_planning::generator + * \see mc_planning::linear_control_system::LinearTimeVariantInvertedPendulum + */ +struct MC_PLANNING_DLLAPI CenteredPreviewWindow +{ + using iterator = RangeElementIterator; + using const_iterator = RangeElementIterator; + + /** + * @brief Constructs a window of duration `2*halfTime` with the current + * element at the center. + * + * @param halfTime Half-duration of the window + * @param dt Timesep + * @param startTime Time at which the window starts + */ + explicit CenteredPreviewWindow(double halfTime, double dt, const double startTime = 0.) noexcept + : halfSize_(static_cast(std::lround(halfTime / dt))), fullSize_(2 * halfSize_ + 1), dt_(dt) + { + startAt(startTime); + } + + /** + * @name Range iterators for computing index/time along the preview window + * @{ + */ + iterator begin() noexcept + { + return iterator{*this, startIndex()}; + } + iterator end() noexcept + { + return iterator{*this, endIndex()}; + } + + const_iterator rbegin() noexcept + { + return const_iterator{*this, startIndex()}; + } + const_iterator rend() noexcept + { + return const_iterator{*this, endIndex()}; + } + // @} + + /** + * @brief Start the preview window at the provided time + * + * @param t Time at which the window starts + */ + void startAt(double t) noexcept + { + start_ = indexFromTime(t); + } + + /** + * @brief Time at the start of the preview window + */ + inline double startTime() const noexcept + { + return start_ * dt_; + } + + /** + * @brief Index of the start of the preview window + */ + unsigned startIndex() const noexcept + { + return start_; + } + + /** + * @brief Time at the center of the preview window + */ + double currentTime() const noexcept + { + return (start_ + halfSize_) * dt_; + } + + /** + * @brief Index of the center of the preview window + */ + unsigned currentIndex() const noexcept + { + return start_ + halfSize_; + } + + /** + * @brief Index of the end of the preview window + */ + double endTime() const noexcept + { + return (start_ + fullSize_ - 1) * dt_; + } + + /** + * @brief Index of the end of the preview window + */ + unsigned endIndex() const noexcept + { + return start_ + fullSize_ - 1; + } + + /** + * @brief Converts between time and index + * + * @param t Time + * + * @return Index corresponding to the provided time + */ + unsigned indexFromTime(double t) const noexcept + { + return static_cast(std::lround(t / dt_)); + } + + /** + * @brief Converts between index and time + * + * @param index Index + * + * @return Time corresponding to the provided index + */ + double timeFromIndex(unsigned index) const noexcept + { + return index * dt_; + } + +protected: + unsigned halfSize_ = 0; ///< Number of future or past element + unsigned fullSize_ = 0; ///< 2*halfSize_+1 + unsigned start_ = 0; ///< Index at which the window starts + double dt_ = 0.005; ///< Timestep +}; + +} // namespace mc_planning diff --git a/include/mc_planning/RangeElementIterator.h b/include/mc_planning/RangeElementIterator.h new file mode 100644 index 0000000000..ce98e7b830 --- /dev/null +++ b/include/mc_planning/RangeElementIterator.h @@ -0,0 +1,73 @@ +/* + * Copyright 2015-2020 CNRS-UM LIRMM, CNRS-AIST JRL + */ + +#pragma once +#include +#include +#include + +namespace mc_planning +{ + +/** + * @brief Virtual iterator for iterating over a range of value + * and accessing elements of the parent class + * + * \see CenteredPreviewWindow for a practical use + */ +template +struct MC_PLANNING_DLLAPI RangeElementIterator +{ +private: + struct IncrementHolder + { + explicit IncrementHolder(unsigned value) noexcept : index_(value) {} + unsigned operator*() noexcept + { + return index_; + } + + private: + unsigned index_; + }; + +public: + using value_type = RetT; + /* deduce const qualifier from bool Const parameter */ + using reference = typename std::conditional::type; + using pointer = typename std::conditional::type; + using iterator_category = std::input_iterator_tag; + using difference_type = void; + + explicit RangeElementIterator(const ParentT & window, unsigned value) noexcept : window_(window), index_(value) {} + value_type operator*() const + { + return value_type(window_, index_); + } + bool operator==(const RangeElementIterator & other) const noexcept + { + return index_ == other.index_; + } + bool operator!=(const RangeElementIterator & other) const noexcept + { + return !(*this == other); + } + IncrementHolder operator++(int) noexcept + { + IncrementHolder ret(index_); + ++*this; + return ret; + } + RangeElementIterator & operator++() noexcept + { + ++index_; + return *this; + } + +private: + const ParentT & window_; + unsigned index_; +}; + +} // namespace mc_planning diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index b196514d38..12a92320fd 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -414,7 +414,8 @@ mc_planning/LIPMControlByPoleAssign.cpp mc_planning/LIPMControlByPoleAssignWithExternalForce.cpp mc_planning/MathFunction.cpp mc_planning/State.cpp -mc_planning/generator.cpp # XXX should it be there? +mc_planning/generator.cpp +mc_planning/PreviewWindow.cpp ) set(mc_planning_HDR_DIR @@ -432,6 +433,7 @@ set(mc_planning_HDR ${mc_planning_HDR_DIR}/ClampedCubicSpline.h ${mc_planning_HDR_DIR}/generator.h ${mc_planning_HDR_DIR}/PreviewSteps.h + ${mc_planning_HDR_DIR}/PreviewWindow.h ) add_library(mc_planning SHARED ${mc_planning_SRC} ${mc_planning_HDR}) diff --git a/src/mc_planning/PreviewWindow.cpp b/src/mc_planning/PreviewWindow.cpp new file mode 100644 index 0000000000..e3fa2be244 --- /dev/null +++ b/src/mc_planning/PreviewWindow.cpp @@ -0,0 +1,15 @@ +/* + * Copyright 2015-2020 CNRS-UM LIRMM, CNRS-AIST JRL + */ + +#include + +namespace mc_planning +{ + +double PreviewElement::time() const noexcept +{ + return window_.timeFromIndex(index_); +} + +} // namespace mc_planning diff --git a/tests/test_hmc.cpp b/tests/test_hmc.cpp index e24d2c46a2..533b526c35 100644 --- a/tests/test_hmc.cpp +++ b/tests/test_hmc.cpp @@ -8,6 +8,7 @@ #include +#include #include #include @@ -187,3 +188,44 @@ BOOST_AUTO_TEST_CASE(TestPreviewStepsSequential) } } } + +BOOST_AUTO_TEST_CASE(TestPreviewWindow) +{ + using namespace mc_planning; + + CenteredPreviewWindow window(1.6, 0.005); + BOOST_REQUIRE_EQUAL(window.startTime(), 0.); + BOOST_REQUIRE_EQUAL(window.endTime(), 3.2); + BOOST_REQUIRE_EQUAL(window.indexFromTime(0), 0); + BOOST_REQUIRE_EQUAL(window.timeFromIndex(0), 0); + BOOST_REQUIRE_EQUAL(window.timeFromIndex(1), 0.005); + BOOST_REQUIRE_EQUAL(window.indexFromTime(0.005), 1); + BOOST_REQUIRE_EQUAL(window.timeFromIndex(10), 0.05); + + { + CenteredPreviewWindow window(1.6, 0.005, 3.2); + CenteredPreviewWindow window2(1.6, 0.005); + window2.startAt(3.2); + BOOST_REQUIRE_EQUAL(window.startTime(), window2.startTime()); + } + + auto test = [&](double preview_time, double dt, double start) { + CenteredPreviewWindow window(preview_time, dt); + window.startAt(start); + unsigned i = window.indexFromTime(start); + BOOST_REQUIRE_EQUAL(i, std::lround(start / dt)); + for(const auto & w : window) + { + // mc_rtc::log::info("Window element with index: {}, time: {:.3f}", w.index(), w.time()); + BOOST_REQUIRE_EQUAL(w.index(), i); + BOOST_REQUIRE_EQUAL(w.time(), i * dt); + BOOST_REQUIRE_EQUAL(window.indexFromTime(window.timeFromIndex(w.index())), w.index()); + BOOST_REQUIRE_EQUAL(window.timeFromIndex(window.indexFromTime(w.time())), w.time()); + ++i; + } + }; + test(1.6, 0.005, 0); + test(1.6, 0.005, 1.5); + test(1.6, 0.002, 0); + test(1.6, 0.002, 1.5); +} From 7de1e893db7038139fb18d22c99b13de4004695b Mon Sep 17 00:00:00 2001 From: Arnaud TANGUY Date: Fri, 26 Jun 2020 00:54:25 +0900 Subject: [PATCH 29/43] [LookupTable] Human-readable function name --- include/mc_planning/LookupTable.h | 41 ++++++++++++++++++++++--------- 1 file changed, 29 insertions(+), 12 deletions(-) diff --git a/include/mc_planning/LookupTable.h b/include/mc_planning/LookupTable.h index 86f6412b28..2a52f2c8f3 100644 --- a/include/mc_planning/LookupTable.h +++ b/include/mc_planning/LookupTable.h @@ -33,6 +33,9 @@ namespace mc_planning * linear generation of values in the min-max range. Whenever possible, it is * recommended to choose a conservative upper boundary. * - Always rigourously evaluate accuracy for each specific use-case. + * - This implementation makes no assumption on the underlying structure of the + * function. Better results and perfomance could be achieved by exploiting + * these properties (such as symetry, non-uniform argument distribution, etc) */ template struct MC_PLANNING_DLLAPI LookupTable @@ -42,9 +45,9 @@ struct MC_PLANNING_DLLAPI LookupTable * @see create(size_t resolution, const T & min, const T & max, MappingFunction f) */ template - LookupTable(size_t resolution, const T & min, const T & max, MappingFunction f) + LookupTable(size_t resolution, const T & min, const T & max, MappingFunction f, const std::string & name = "") { - create(resolution, min, max, f); + create(resolution, min, max, f, name); } /** @@ -52,7 +55,7 @@ struct MC_PLANNING_DLLAPI LookupTable * * To initialize the lookup table, call create(size_t resolution, const T & min, const T & max, MappingFunction f) */ - LookupTable(){}; + LookupTable() noexcept {}; /** * @brief Evaluate and store given function results @@ -63,18 +66,22 @@ struct MC_PLANNING_DLLAPI LookupTable * @param min Minimum argument value * @param max Maximum argument value * @param f Function to evaluate + * @param name Human-readable description of the stored function (used for + * error reporting) */ template - void create(size_t resolution, const T & min, const T & max, MappingFunction f) + void create(size_t resolution, const T & min, const T & max, MappingFunction f, const std::string & name = "") { + name_ = name; + prefix_ = "[LookupTable]" + name_.size() ? "[" + name_ + "] " : " "; if(min > max) { - mc_rtc::log::error_and_throw("[LookupTable] Invalid range (min {} > max {})", min, max); + mc_rtc::log::error_and_throw(prefix_ + "Invalid range (min {} > max {})", min, max); } if(resolution == 0) { - mc_rtc::log::error_and_throw( - "[LookupTable] Resolution cannot be equal to zero (strictly positive)"); + mc_rtc::log::error_and_throw(prefix_ + + "Resolution cannot be equal to zero (strictly positive)"); } table_.resize(resolution); min_ = min; @@ -106,15 +113,23 @@ struct MC_PLANNING_DLLAPI LookupTable { if(CheckBounds) { - if(x < min_ || x > max_) + if(table_.empty() || x < min_ || x > max_) { - mc_rtc::log::error_and_throw("[LookupTable] Out of bound access ({} <= {} <= {})", min_, x, - max_); + if(table_.empty()) + { + mc_rtc::log::error_and_throw( + prefix_ + "Uninitialized table. Please call create() before use.", min_, x, max_); + } + else + { + mc_rtc::log::error_and_throw(prefix_ + "Out of bound access ({} <= {} <= {})", min_, x, + max_); + } } } else { - assert(x < min_ || x > max_); + assert(table_.empty() || x < min_ || x > max_); } auto h = std::min(static_cast(lround((x - min_) * rangeConversion_)), maxIndex_); return table_[h]; @@ -124,7 +139,9 @@ struct MC_PLANNING_DLLAPI LookupTable std::vector table_; T min_, max_; T rangeConversion_; - size_t maxIndex_; + size_t maxIndex_ = 0; + std::string name_ = {}; + std::string prefix_ = {}; }; } // namespace mc_planning From 6fe0ac19c325c77eae748b5933332e7493921948 Mon Sep 17 00:00:00 2001 From: Arnaud TANGUY Date: Fri, 26 Jun 2020 10:48:35 +0900 Subject: [PATCH 30/43] [HMC/generator] Use Iterable PreviewWindow --- benchmarks/benchHMC.cpp | 16 +- .../LinearTimeVariantInvertedPendulum.h | 3 +- include/mc_planning/PreviewWindow.h | 162 ++++++++++++++++-- include/mc_planning/generator.h | 11 +- .../CoMTrajectoryGeneration_Initial.cpp | 7 +- .../states/CoMTrajectoryGeneration_Initial.h | 5 +- .../LinearTimeVariantInvertedPendulum.cpp | 6 +- src/mc_planning/PreviewWindow.cpp | 16 ++ src/mc_planning/generator.cpp | 64 +++---- tests/hmc/testCoMGenerator.cpp | 17 +- tests/test_hmc.cpp | 79 +++++++-- 11 files changed, 301 insertions(+), 85 deletions(-) diff --git a/benchmarks/benchHMC.cpp b/benchmarks/benchHMC.cpp index fcab787134..3c367dbd98 100644 --- a/benchmarks/benchHMC.cpp +++ b/benchmarks/benchHMC.cpp @@ -121,10 +121,13 @@ class GeneratorFixture : public benchmark::Fixture public: using Generator = mc_planning::generator; using PreviewSteps = mc_planning::PreviewSteps; + using CenteredPreviewWindow = mc_planning::CenteredPreviewWindow; + GeneratorFixture() : preview(1.6, dt) {} + void SetUp(const ::benchmark::State &) { - steps.add({(double)n_preview * dt, {-0.2, 0.0}}); - steps.addRelative({(double)n_preview * dt, {0.0, 0.0}}); + steps.add({preview.halfDuration(), {-0.2, 0.0}}); + steps.addRelative({preview.halfDuration(), {0.0, 0.0}}); steps.addRelative({0.1, {0.0, 0.0}}); steps.addRelative({1.6, {0.0, 0.0}}); steps.addRelative({0.1, {0.2, 0.095}}); @@ -132,20 +135,21 @@ class GeneratorFixture : public benchmark::Fixture steps.addRelative({0.1, {0.0, -0.19}}); steps.addRelative({1.6, {0.0, 0.0}}); steps.addRelative({0.1, {-0.2, 0.095}}); - steps.addRelative({(double)n_preview * dt, {0.0, 0.0}}); + steps.addRelative({preview.halfDuration(), {0.0, 0.0}}); steps.initialize(); - generator = std::make_shared(n_preview, dt); + generator = std::make_shared(preview); generator->steps(steps); - n_loop = static_cast(std::lround(steps.back().t() / dt) - n_preview); + n_loop = preview.indexFromTime(steps.back().t()); } void TearDown(const ::benchmark::State &) {} std::shared_ptr generator; + CenteredPreviewWindow preview; PreviewSteps steps; static constexpr double dt = 0.005; - unsigned n_loop = 15. / dt; + unsigned n_loop = 0.; static constexpr unsigned n_preview = 1.6 / dt; }; diff --git a/include/mc_planning/LinearTimeVariantInvertedPendulum.h b/include/mc_planning/LinearTimeVariantInvertedPendulum.h index 8a5e4de1fd..a7302763ac 100644 --- a/include/mc_planning/LinearTimeVariantInvertedPendulum.h +++ b/include/mc_planning/LinearTimeVariantInvertedPendulum.h @@ -117,7 +117,8 @@ struct MC_PLANNING_DLLAPI LinearTimeVariantInvertedPendulum /** * @brief Initialization * - * @param dt Timestep @param n_preview Number of future preview elements. The full size of the + * @param dt Timestep + * @param n_preview Number of future preview elements. The full size of the * preview windows from past to future will be (2*n_preview+1) * @param minHeight for the pendulum (used for LookupTable optimizations) * @param maxHeight for the pendulum diff --git a/include/mc_planning/PreviewWindow.h b/include/mc_planning/PreviewWindow.h index aefa49aff6..13d1e63c08 100644 --- a/include/mc_planning/PreviewWindow.h +++ b/include/mc_planning/PreviewWindow.h @@ -18,21 +18,50 @@ struct CenteredPreviewWindow; */ struct MC_PLANNING_DLLAPI PreviewElement { + /** + * @brief Create an element (meant to be used by iterators) + * + * @param window Preview window to which this element refers to + * @param index Global index of the element + */ explicit PreviewElement(const CenteredPreviewWindow & window, unsigned index) noexcept : window_(window), index_(index) { } - unsigned index() const noexcept + /** + * @brief Returns the global index starting at `window.startIndex()` + */ + unsigned index() const noexcept; + /** + * @brief Returns the time corresponding to index() + */ + double time() const noexcept; + + /** + * @brief Returns the index local to the preview window + * + * @return Index between \f$ [0, 2*N] \f$ where `N` is the preview size + */ + unsigned localIndex() const noexcept; + + /** + * @brief Returns the time corresponding to localIndex() + */ + double localTime() const noexcept; + + /** + * @brief Reference to the preview window from which this element was + * generated + */ + const CenteredPreviewWindow & window() const noexcept { - return index_; + return window_; } - double time() const noexcept; - protected: - const CenteredPreviewWindow & window_; - unsigned index_; + const CenteredPreviewWindow & window_; ///< Window being iterated over + unsigned index_; ///< Global index }; /** @@ -42,6 +71,9 @@ struct MC_PLANNING_DLLAPI PreviewElement * elements before and after the current value. The full preview window is of size `2*N+1`. The timestep is used to * compute correspondances between discrete index of values stored in an (external) container and time. * + * Additionally, this class provides iterable ranges over the preview window. + * For example: + * * \code{.cpp} * // Create an iterable preview window range with a window of time `2*1.6`, * // timestep of 0.005 and starting at time `t=0.5` @@ -60,6 +92,18 @@ struct MC_PLANNING_DLLAPI PreviewElement * // Example: Access some external container using the index * // auto value = container[w.index()]; * } + * + * // Iterate over a copy of the preview window (cheap copy) starting at a specific time using a RangeElementIterator + * for(const auto & w : window.iterateFromTime(1.6)) + * { + * mc_rtc::log::info( + * "Preview element with global index: {}, time: {:.3f}\n" + * " local index: {}, time: {:.3f}\n" + * , w.index(), w.time() + * , w.localIndex(), w.localTime() + * ); + * // Global time starts at 1.6s, while local time is between [0, window.duration()] + * } * \endcode * * \see For practical use, see: @@ -82,7 +126,7 @@ struct MC_PLANNING_DLLAPI CenteredPreviewWindow explicit CenteredPreviewWindow(double halfTime, double dt, const double startTime = 0.) noexcept : halfSize_(static_cast(std::lround(halfTime / dt))), fullSize_(2 * halfSize_ + 1), dt_(dt) { - startAt(startTime); + startAtTime(startTime); } /** @@ -95,16 +139,58 @@ struct MC_PLANNING_DLLAPI CenteredPreviewWindow } iterator end() noexcept { - return iterator{*this, endIndex()}; + return iterator{*this, endIndex() + 1}; } - const_iterator rbegin() noexcept + const_iterator cbegin() noexcept { return const_iterator{*this, startIndex()}; } - const_iterator rend() noexcept + const_iterator cend() noexcept + { + return const_iterator{*this, endIndex() + 1}; + } + + /** + * @brief Iterate over a copy of the preview window + * + * The preview window constains only the window parameters and is cheap to + * copy. + * + * @param startTime Time from which to iterate + * + * @return A copy of the preview window starting at time startTime + */ + CenteredPreviewWindow iterateFromTime(double startTime) const noexcept + { + CenteredPreviewWindow w = *this; + w.startAtTime(startTime); + return w; + } + + /** + * @brief Iterate over a copy of the preview window + * + * The preview window constains only the window parameters and is cheap to + * copy. + * + * @param startIndex Index from which to iterate + * + * @return A copy of the preview window starting at index startIndex + */ + CenteredPreviewWindow iterateFromIndex(unsigned startIndex) const noexcept + { + CenteredPreviewWindow w = *this; + w.startAtIndex(startIndex); + return w; + } + + /** + * @brief Generate a copy of the current preview window + */ + CenteredPreviewWindow copy() const noexcept { - return const_iterator{*this, endIndex()}; + return *this; } // @} @@ -113,11 +199,19 @@ struct MC_PLANNING_DLLAPI CenteredPreviewWindow * * @param t Time at which the window starts */ - void startAt(double t) noexcept + void startAtTime(double t) noexcept { start_ = indexFromTime(t); } + /** + * @brief Start the preview window at the provided index + */ + void startAtIndex(unsigned index) noexcept + { + start_ = index; + } + /** * @brief Time at the start of the preview window */ @@ -155,7 +249,7 @@ struct MC_PLANNING_DLLAPI CenteredPreviewWindow */ double endTime() const noexcept { - return (start_ + fullSize_ - 1) * dt_; + return endIndex() * dt_; } /** @@ -190,6 +284,48 @@ struct MC_PLANNING_DLLAPI CenteredPreviewWindow return index * dt_; } + /** + * @brief Timestep + */ + double dt() const noexcept + { + return dt_; + } + + /** + * @brief Number of future or past elements. + * + * The preview window is of size `2*halfSize_+1` + */ + unsigned halfSize() const noexcept + { + return halfSize_; + } + + /** + * @brief Duration corresponding to halfSize() + */ + double halfDuration() const noexcept + { + return halfSize_ * dt_; + } + + /** + * @brief Full size of the preview window + */ + unsigned size() const noexcept + { + return fullSize_; + } + + /** + * @brief Full duration of the preview window + */ + double duration() const noexcept + { + return (size() - 1) * dt_; + } + protected: unsigned halfSize_ = 0; ///< Number of future or past element unsigned fullSize_ = 0; ///< 2*halfSize_+1 diff --git a/include/mc_planning/generator.h b/include/mc_planning/generator.h index aa1e1c235f..419b7883e7 100644 --- a/include/mc_planning/generator.h +++ b/include/mc_planning/generator.h @@ -3,6 +3,7 @@ #include #include #include +#include #include #include #include @@ -28,13 +29,11 @@ struct MC_PLANNING_DLLAPI generator /** * @brief Construct a simple trajectory generator * - * @param n_preview Size of the future preview window. The full preview window - * goes from future to past with size `2*n_preview+1` - * @param dt + * @param preview Preview window parameters (size, timestep) * @mass Robot mass * @waist_height Initial height of the waist (constant for now) */ - generator(unsigned n_preview, double dt, double mass = 60, double waist_height = 0.8); + generator(const CenteredPreviewWindow & preview, double mass = 60, double waist_height = 0.8); /** * @brief Generate the long and short term trajectories @@ -218,6 +217,8 @@ struct MC_PLANNING_DLLAPI generator void generateTrajectories(); private: + CenteredPreviewWindow preview_; ///< Iteratable preview window and parameters + CenteredPreviewWindow previewZero_; ///< Iteratable preview window and parameters std::shared_ptr> m_ComInterp = nullptr; StatePVA m_COG_ideal_pre; @@ -244,8 +245,6 @@ struct MC_PLANNING_DLLAPI generator Eigen::Vector3d m_Pcalpha_motion_out = Eigen::Vector3d::Zero(); /// @} - unsigned m_n_preview = 0; ///< Size of the future preview window. The full preview is 2*m_n_preview+1 - double m_dt = 0.005; ///< Timestep double m_omega_valpha = 0.0; double m_mass = 60.; ///< Robot mass double m_waist_height; ///< Height of the weight (constant for now) diff --git a/samples/CoMTrajectoryGeneration/src/states/CoMTrajectoryGeneration_Initial.cpp b/samples/CoMTrajectoryGeneration/src/states/CoMTrajectoryGeneration_Initial.cpp index f58d8e233e..74ec10b020 100644 --- a/samples/CoMTrajectoryGeneration/src/states/CoMTrajectoryGeneration_Initial.cpp +++ b/samples/CoMTrajectoryGeneration/src/states/CoMTrajectoryGeneration_Initial.cpp @@ -13,8 +13,7 @@ void CoMTrajectoryGeneration_Initial::configure(const mc_rtc::Configuration & co void CoMTrajectoryGeneration_Initial::start(mc_control::fsm::Controller & ctl) { - m_dt = ctl.timeStep; - previewSize_ = static_cast(lround(previewTime_ / m_dt)); + preview_ = mc_planning::CenteredPreviewWindow(previewTime_, ctl.timeStep); /// XXX should this be CoM height or waist height? double waist_height = ctl.realRobot().com().z(); @@ -43,7 +42,7 @@ void CoMTrajectoryGeneration_Initial::start(mc_control::fsm::Controller & ctl) steps_.initialize(); //clang-format on - comGenerator_ = std::make_shared(previewSize_, m_dt, ctl.robot().mass(), + comGenerator_ = std::make_shared(preview_, ctl.robot().mass(), waist_height); comGenerator_->addToLogger(ctl.logger()); comGenerator_->steps(steps_); @@ -161,7 +160,7 @@ bool CoMTrajectoryGeneration_Initial::run(mc_control::fsm::Controller & ctl) } iter_++; - t_ += m_dt; + t_ += ctl.timeStep; output("OK"); return false; } diff --git a/samples/CoMTrajectoryGeneration/src/states/CoMTrajectoryGeneration_Initial.h b/samples/CoMTrajectoryGeneration/src/states/CoMTrajectoryGeneration_Initial.h index 3c7802d08d..da9606af29 100644 --- a/samples/CoMTrajectoryGeneration/src/states/CoMTrajectoryGeneration_Initial.h +++ b/samples/CoMTrajectoryGeneration/src/states/CoMTrajectoryGeneration_Initial.h @@ -21,12 +21,11 @@ struct CoMTrajectoryGeneration_Initial : mc_control::fsm::State void updateSteps(); private: - double previewTime_ = 1.6; ///< Preview horizon half-time [s] + mc_planning::CenteredPreviewWindow preview_{0,0}; Eigen::Vector3d polesX_ = {1.0, 1.0, 150.0}; Eigen::Vector3d polesY_ = {1.0, 1.0, 150.0}; + double previewTime_ = 1.6; - double m_dt = 0.005; ///< Timestep (for convenience) - unsigned previewSize_ = 0; ///< Size of the preview window unsigned iter_ = 0; ///< Number of iterations elapsed since started double t_ = 0; ///< Time elapsed since started double generationTime_ = 0; ///< Time spent generating the trajectory diff --git a/src/mc_planning/LinearTimeVariantInvertedPendulum.cpp b/src/mc_planning/LinearTimeVariantInvertedPendulum.cpp index 36f1a12708..9b206f0f5b 100644 --- a/src/mc_planning/LinearTimeVariantInvertedPendulum.cpp +++ b/src/mc_planning/LinearTimeVariantInvertedPendulum.cpp @@ -49,9 +49,9 @@ void LinearTimeVariantInvertedPendulum::Initialize(double dt, auto shk = [this](double w2) { return sinh(wTable_(w2) * m_dt); }; double minOmega2 = omega2(maxHeight); double maxOmega2 = omega2(minHeight); - wTable_.create(weight_resolution, minOmega2, maxOmega2, omega); - chkTable_.create(weight_resolution, minOmega2, maxOmega2, chk); - shkTable_.create(weight_resolution, minOmega2, maxOmega2, shk); + wTable_.create(weight_resolution, minOmega2, maxOmega2, omega, "omega^2 -> sqrt(w2)"); + chkTable_.create(weight_resolution, minOmega2, maxOmega2, chk, "omega^2 -> cosh(sqrt(omega^2 * dt)"); + shkTable_.create(weight_resolution, minOmega2, maxOmega2, shk, "omega^2 -> sinh(sqrt(omega^2 * dt)"); m_A.resize(m_n_preview2, Matrix22::Identity()); m_An.resize(m_n_preview2, Matrix22::Identity()); diff --git a/src/mc_planning/PreviewWindow.cpp b/src/mc_planning/PreviewWindow.cpp index e3fa2be244..07248aad2a 100644 --- a/src/mc_planning/PreviewWindow.cpp +++ b/src/mc_planning/PreviewWindow.cpp @@ -7,9 +7,25 @@ namespace mc_planning { +unsigned PreviewElement::index() const noexcept +{ + return index_; +} + + double PreviewElement::time() const noexcept { return window_.timeFromIndex(index_); } +unsigned PreviewElement::localIndex() const noexcept +{ + return index_ - window_.startIndex(); +} + +double PreviewElement::localTime() const noexcept +{ + return window_.timeFromIndex(index_ - window_.startIndex()); +} + } // namespace mc_planning diff --git a/src/mc_planning/generator.cpp b/src/mc_planning/generator.cpp index 52bdd9df96..2157524d80 100644 --- a/src/mc_planning/generator.cpp +++ b/src/mc_planning/generator.cpp @@ -12,25 +12,27 @@ constexpr int Z = 2; namespace mc_planning { -generator::generator(unsigned n_preview, double dt, double mass, double waist_height) -: m_ComInterp(std::make_shared>(1.0, dt / 2.)), m_n_preview(n_preview), m_dt(dt), - m_mass(mass), m_waist_height(waist_height) +generator::generator(const CenteredPreviewWindow & preview, double mass, double waist_height) +: preview_(preview), previewZero_(preview), + m_ComInterp(std::make_shared>(1.0, preview_.dt() / 2.)), m_mass(mass), + m_waist_height(waist_height) { mc_rtc::log::info("Creating generator with mass={}, waist_height={}", mass, waist_height); - auto initialize = [this, &waist_height](unsigned X) { - m_ipm_long[X].Initialize(m_dt, m_n_preview, 20000); - m_ipm_short[X].Initialize(); + auto initialize = [this, &waist_height](unsigned axis) { + m_ipm_long[axis].Initialize(preview_.dt(), preview_.halfSize(), 20000); + m_ipm_short[axis].Initialize(); m_poles[X] << 1.0, 1.0, 150.0; - m_virtual_height[X].setZero(m_n_preview * 2 + 1); - m_ipm_long[X].initMatrices(waist_height); + m_virtual_height[axis].setZero(preview_.size()); + m_ipm_long[axis].initMatrices(waist_height); }; initialize(X); initialize(Y); - m_cog_height.setZero(m_n_preview * 2 + 1); - m_cog_dot_height.setZero(m_n_preview * 2 + 1); - m_cog_ddot_height.setZero(m_n_preview * 2 + 1); + auto previewSize = preview_.size(); + m_cog_height.setZero(previewSize); + m_cog_dot_height.setZero(previewSize); + m_cog_ddot_height.setZero(previewSize); m_COG_ideal.P << 0.0, 0.0, waist_height; } @@ -42,42 +44,43 @@ void generator::setupCOGHeight(unsigned n_current) if(n_current == 0) { m_ComInterp->push_back(0u, m_waist_height); - m_ComInterp->push_back(static_cast(std::lround(m_steps.back().t() / m_dt)), m_waist_height); + m_ComInterp->push_back(preview_.indexFromTime(m_steps.back().t()), m_waist_height); m_ComInterp->update(); } - // Time window from past to future - for(unsigned n_step = 0; n_step < 2 * m_n_preview + 1; ++n_step) + for(const auto & step : preview_.iterateFromIndex(0)) { // clang-format off - m_ComInterp->get(n_current + n_step, - m_cog_height[n_step], - m_cog_dot_height[n_step], - m_cog_ddot_height[n_step]); + auto n = step.index(); + m_ComInterp->get(n_current + n, m_cog_height[n], m_cog_dot_height[n], m_cog_ddot_height[n]); //clang-format on } // Interpolated value at current time - m_COG_ideal.P(Z) = m_cog_height[m_n_preview]; - m_COG_ideal.V(Z) = m_cog_dot_height[m_n_preview]; - m_COG_ideal.Vdot(Z) = m_cog_ddot_height[m_n_preview]; + unsigned current = previewZero_.currentIndex(); + m_COG_ideal.P(Z) = m_cog_height[current]; + m_COG_ideal.V(Z) = m_cog_dot_height[current]; + m_COG_ideal.Vdot(Z) = m_cog_ddot_height[current]; } void generator::setupTimeTrajectories(unsigned n_current) { - m_steps.nextWindow(n_current * m_dt); + m_steps.nextWindow(preview_.startTime()); Eigen::VectorXd & px_ref = m_ipm_long[X].p_ref(); Eigen::VectorXd & py_ref = m_ipm_long[Y].p_ref(); - for(unsigned i = 0; i < m_n_preview * 2 + 1; i++) + + // for(unsigned i = 0; i < preview_.size(); i++) + for(const auto & current : preview_) { + unsigned i = current.localIndex(); + const double currTime = current.time(); //preview_.timeFromIndex(i + n_current); + m_virtual_height[X](i) = 0.0; m_virtual_height[Y](i) = 0.0; - const double currTime = static_cast(i + n_current) * m_dt; // Move to next step if necessary m_steps.update(currTime); const auto & prevStep = m_steps.previous(); - if(!m_steps.isLastStep()) { const auto & nextStep = m_steps.next(); @@ -105,7 +108,8 @@ void generator::setupTimeTrajectories(unsigned n_current) (mc_rtc::constants::GRAVITY + m_cog_ddot_height[i]) / (m_cog_height[i] - m_virtual_height[Y](i)); } - m_Pcalpha_ideal(Z) = (m_virtual_height[X](m_n_preview) + m_virtual_height[Y](m_n_preview)) / 2.0; + unsigned current = previewZero_.currentIndex(); + m_Pcalpha_ideal(Z) = (m_virtual_height[X](current) + m_virtual_height[Y](current)) / 2.0; } void generator::generateTrajectories() @@ -136,7 +140,8 @@ void generator::generateTrajectories() * - m_Pcalpha_cmp: Modifiecation of the ideal ZMP state (position and velocity) **/ auto computeShortTerm = [&](unsigned axis) { - double omega2 = m_ipm_long[axis].w2(m_n_preview); + unsigned current = previewZero_.currentIndex(); + double omega2 = m_ipm_long[axis].w2(current); double omega = sqrt(omega2); m_ipm_short[axis].setSystemMatrices(omega * m_poles[axis](0), omega * m_poles[axis](1), m_poles[axis](2), omega2, m_mass); @@ -147,7 +152,7 @@ void generator::generateTrajectories() m_ipm_short[axis].setStateVariables(m_COG_cmp.P(axis), m_COG_cmp.V(axis), m_Pcalpha_cmp(axis)); double Fext = 0.0; - m_ipm_short[axis].update(Fext, m_dt); + m_ipm_short[axis].update(Fext, preview_.dt()); m_ipm_short[axis].getStateVariables(m_COG_cmp.P(axis), m_COG_cmp.V(axis), m_Pcalpha_cmp(axis), m_Vcalpha_cmp(axis)); }; @@ -169,7 +174,7 @@ void generator::generateTrajectories() m_COG_out.Vdot = m_COG_ideal.Vdot + m_COG_cmp.Vdot; m_Pcalpha_out = m_Pcalpha_ideal + m_Pcalpha_cmp; - m_Pcalpha_motion_out = m_Pcalpha_out + m_Vcalpha_ideal * m_omega_valpha * m_dt; + m_Pcalpha_motion_out = m_Pcalpha_out + m_Vcalpha_ideal * m_omega_valpha * preview_.dt(); }; computeTrajectories(X); @@ -183,6 +188,7 @@ void generator::generate(unsigned n_time) { mc_rtc::log::error_and_throw("[generator] Invalid reference provided, call steps()"); } + preview_.startAtIndex(n_time); setupCOGHeight(n_time); setupTimeTrajectories(n_time); generateTrajectories(); diff --git a/tests/hmc/testCoMGenerator.cpp b/tests/hmc/testCoMGenerator.cpp index 24424d1da1..da1a5cc991 100644 --- a/tests/hmc/testCoMGenerator.cpp +++ b/tests/hmc/testCoMGenerator.cpp @@ -6,17 +6,18 @@ using namespace mc_planning; using Vector3 = Eigen::Vector3d; -constexpr double dt = 0.005; -const unsigned n_preview = static_cast(std::lround(1.6 / dt)); - int main(void) { + double dt = 0.005; + CenteredPreviewWindow preview(1.6, dt); + // Trajectory of size 2*n_preview+1 - generator com_traj(n_preview, dt); + generator com_traj(preview); + auto prev_time = preview.halfDuration(); PreviewSteps steps; - steps.add({(double)n_preview * dt, {-0.2, 0.0}}); - steps.addRelative({(double)n_preview * dt, {0.0, 0.0}}); + steps.add({prev_time, {-0.2, 0.0}}); + steps.addRelative({prev_time, {0.0, 0.0}}); steps.addRelative({0.1, {0.0, 0.0}}); steps.addRelative({1.6, {0.0, 0.0}}); steps.addRelative({0.1, {0.2, 0.095}}); @@ -24,7 +25,7 @@ int main(void) steps.addRelative({0.1, {0.0, -0.19}}); steps.addRelative({1.6, {0.0, 0.0}}); steps.addRelative({0.1, {-0.2, 0.095}}); - steps.addRelative({(double)n_preview * dt, {0.0, 0.0}}); + steps.addRelative({prev_time, {0.0, 0.0}}); steps.initialize(); com_traj.steps(steps); @@ -34,7 +35,7 @@ int main(void) logger.start("CoMGenerator", dt); com_traj.addToLogger(logger); - unsigned n_loop = static_cast(std::lround(com_traj.steps().back().t() / dt) - n_preview); + unsigned n_loop = preview.indexFromTime(com_traj.steps().back().t()) - preview.halfSize(); mc_rtc::log::info("n_loop {}", n_loop); /* * Generate the CoM trajectory based on the parameters in com_traj diff --git a/tests/test_hmc.cpp b/tests/test_hmc.cpp index 533b526c35..71836c6675 100644 --- a/tests/test_hmc.cpp +++ b/tests/test_hmc.cpp @@ -5,7 +5,6 @@ #include #include #include - #include #include @@ -193,30 +192,86 @@ BOOST_AUTO_TEST_CASE(TestPreviewWindow) { using namespace mc_planning; - CenteredPreviewWindow window(1.6, 0.005); - BOOST_REQUIRE_EQUAL(window.startTime(), 0.); - BOOST_REQUIRE_EQUAL(window.endTime(), 3.2); - BOOST_REQUIRE_EQUAL(window.indexFromTime(0), 0); - BOOST_REQUIRE_EQUAL(window.timeFromIndex(0), 0); - BOOST_REQUIRE_EQUAL(window.timeFromIndex(1), 0.005); - BOOST_REQUIRE_EQUAL(window.indexFromTime(0.005), 1); - BOOST_REQUIRE_EQUAL(window.timeFromIndex(10), 0.05); + { + CenteredPreviewWindow window(0.015, 0.005); + BOOST_REQUIRE_EQUAL(window.duration(), 0.03); + BOOST_REQUIRE_EQUAL(window.halfDuration(), 0.015); + BOOST_REQUIRE_EQUAL(window.halfSize(), 3); // n + BOOST_REQUIRE_EQUAL(window.size(), 7); // 2*n+1 + BOOST_REQUIRE_EQUAL(window.startIndex(), 0); + BOOST_REQUIRE_EQUAL(window.startTime(), 0); + BOOST_REQUIRE_EQUAL(window.endIndex(), 6); // Array of size 7, last index is 6 + BOOST_REQUIRE_EQUAL(window.endTime(), 0.03); + auto end = *window.end(); + BOOST_REQUIRE_EQUAL(end.localIndex(), 7); // Iterator end is one element after the end + BOOST_REQUIRE_EQUAL(end.localTime(), 0.035); // Iterator end is one element after the end + BOOST_REQUIRE_EQUAL(end.index(), 7); // Iterator end is one element after the end + BOOST_REQUIRE_EQUAL(end.time(), 0.035); // Iterator end is one element after the end + auto start = *window.begin(); + BOOST_REQUIRE_EQUAL(start.localIndex(), 0); + BOOST_REQUIRE_EQUAL(start.localTime(), 0.0); + for(const auto w : window) + { + mc_rtc::log::info("index {}, local time {:.3f}, global index {}, global time {:.3f}", w.localIndex(), w.localTime(), w.index(), w.time()); + + BOOST_REQUIRE(w.localIndex() <= 6); + BOOST_REQUIRE(w.index() <= 6); + } + } + + { + CenteredPreviewWindow window(0.015, 0.005); + window.startAtTime(10); + BOOST_REQUIRE_EQUAL(window.duration(), 0.03); + BOOST_REQUIRE_EQUAL(window.halfDuration(), 0.015); + BOOST_REQUIRE_EQUAL(window.halfSize(), 3); // n + BOOST_REQUIRE_EQUAL(window.size(), 7); // 2*n+1 + BOOST_REQUIRE_EQUAL(window.startTime(), 10); + BOOST_REQUIRE_EQUAL(window.endTime(), 10.03); + auto end = *window.end(); + BOOST_REQUIRE_EQUAL(end.localIndex(), 7); // Iterator end is one element after the end + BOOST_REQUIRE_EQUAL(end.localTime(), 0.035); // Iterator end is one element after the end + auto start = *window.begin(); + BOOST_REQUIRE_EQUAL(start.localIndex(), 0); + BOOST_REQUIRE_EQUAL(start.localTime(), 0.0); + for(const auto w : window) + { + mc_rtc::log::info("local index {}, local time {:.3f}, global index {}, global time {:.3f}", w.localIndex(), w.localTime(), w.index(), w.time()); + + BOOST_REQUIRE(w.localIndex() <= 6); + } + } + + { + CenteredPreviewWindow window(1.6, 0.005); + BOOST_REQUIRE_EQUAL(window.startTime(), 0.); + BOOST_REQUIRE_EQUAL(window.endTime(), 3.2); + BOOST_REQUIRE_EQUAL(window.duration(), 3.2); + BOOST_REQUIRE_EQUAL(window.halfDuration(), 1.6); + BOOST_REQUIRE_EQUAL(window.halfSize(), (unsigned)std::lround(1.6 / 0.005)); + BOOST_REQUIRE_EQUAL(window.size(), (unsigned)std::lround(3.2 / 0.005) + 1); + BOOST_REQUIRE_EQUAL(window.indexFromTime(0), 0); + BOOST_REQUIRE_EQUAL(window.timeFromIndex(0), 0); + BOOST_REQUIRE_EQUAL(window.timeFromIndex(1), 0.005); + BOOST_REQUIRE_EQUAL(window.indexFromTime(0.005), 1); + BOOST_REQUIRE_EQUAL(window.timeFromIndex(10), 0.05); + } { CenteredPreviewWindow window(1.6, 0.005, 3.2); CenteredPreviewWindow window2(1.6, 0.005); - window2.startAt(3.2); + window2.startAtTime(3.2); BOOST_REQUIRE_EQUAL(window.startTime(), window2.startTime()); } auto test = [&](double preview_time, double dt, double start) { CenteredPreviewWindow window(preview_time, dt); - window.startAt(start); + window.startAtTime(start); unsigned i = window.indexFromTime(start); BOOST_REQUIRE_EQUAL(i, std::lround(start / dt)); for(const auto & w : window) { - // mc_rtc::log::info("Window element with index: {}, time: {:.3f}", w.index(), w.time()); + // mc_rtc::log::info("Window element with index: {}, time: {:.3f}", w.localIndex(), w.localTime()); BOOST_REQUIRE_EQUAL(w.index(), i); BOOST_REQUIRE_EQUAL(w.time(), i * dt); BOOST_REQUIRE_EQUAL(window.indexFromTime(window.timeFromIndex(w.index())), w.index()); From b9f95a1b21888e32c12017a436712816fe7e09cd Mon Sep 17 00:00:00 2001 From: Arnaud TANGUY Date: Fri, 26 Jun 2020 11:28:34 +0900 Subject: [PATCH 31/43] [HMC/doc] Include CoM generation example and figure --- .github/workflows/build.yml | 1 + doc/images/generator_com_generation.svg | 2304 +++++++++++++++++++++++ include/mc_planning/generator.h | 77 + src/mc_planning/PreviewWindow.cpp | 1 - tests/hmc/testCoMGenerator.cpp | 11 +- tests/test_hmc.cpp | 6 +- 6 files changed, 2393 insertions(+), 7 deletions(-) create mode 100644 doc/images/generator_com_generation.svg diff --git a/.github/workflows/build.yml b/.github/workflows/build.yml index fbe77af57c..01c1590e95 100644 --- a/.github/workflows/build.yml +++ b/.github/workflows/build.yml @@ -209,6 +209,7 @@ jobs: pushd . cd doc cp -r /usr/local/share/doc/mc_rtc/doxygen-html . + cp -r images doxygen-html find . -type f -print0 | xargs -0 sed -i -e's@/usr/local/share/doc/SpaceVecAlg/doxygen-html/@https://jrl-umi3218.github.io/SpaceVecAlg/doxygen/HEAD/@g' find . -type f -print0 | xargs -0 sed -i -e's@/usr/local/share/doc/RBDyn/doxygen-html/@https://jrl-umi3218.github.io/RBDyn/doxygen/HEAD/@g' find . -type f -print0 | xargs -0 sed -i -e's@/usr/local/share/doc/Tasks/doxygen-html/@https://jrl-umi3218.github.io/Tasks/doxygen/HEAD/@g' diff --git a/doc/images/generator_com_generation.svg b/doc/images/generator_com_generation.svg new file mode 100644 index 0000000000..8814985f96 --- /dev/null +++ b/doc/images/generator_com_generation.svg @@ -0,0 +1,2304 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/include/mc_planning/generator.h b/include/mc_planning/generator.h index 419b7883e7..9c6c027c8b 100644 --- a/include/mc_planning/generator.h +++ b/include/mc_planning/generator.h @@ -21,6 +21,83 @@ namespace mc_planning * @see mc_planning::linear_control_system::LinearTimeVariantInvertedPendulum long-term trajectory generation * @see mc_planning::linear_control_system::LIPMControlByPoleAssignWithExternalForce short-term trajectory generation to * continuously track the long-term trajectory + * + * + * Example + * Let's see with a simple example how this generator can be used in practice. + * We will generate a simple CoM trajectory in the sagittal and lateral + * direction by providing discrete desired ZMP positions along with their + * timing. + * See the corresponding mc_rtc sample testCoMGeneration and benchmark. + * + * \code{.cpp} + * #include + * #include + * #include + * #include + * + * using namespace mc_planning; + * + * int main(int, char **) + * { + * double dt = 0.005; + * CenteredPreviewWindow preview(1.6, dt); + * + * // Trajectory of size 2*n_preview+1 + * generator com_traj(preview); + * + * auto prev_time = preview.halfDuration(); + * PreviewSteps steps; + * steps.add({prev_time, {-0.2, 0.0}}); + * steps.addRelative({prev_time, {0.0, 0.0}}); + * steps.addRelative({0.1, {0.0, 0.0}}); + * steps.addRelative({1.6, {0.0, 0.0}}); + * steps.addRelative({0.1, {0.2, 0.095}}); + * steps.addRelative({1.6, {0.0, 0.0}}); + * steps.addRelative({0.1, {0.0, -0.19}}); + * steps.addRelative({1.6, {0.0, 0.0}}); + * steps.addRelative({0.1, {-0.2, 0.095}}); + * steps.addRelative({prev_time, {0.0, 0.0}}); + * steps.initialize(); + * + * com_traj.steps(steps); + * mc_rtc::log::info("Desired steps:\nTime\tZMP X\tZMP Y\n{}", mc_rtc::io::to_string(steps.steps(), "\n")); + * + * // Setup logging + * mc_rtc::Logger logger(mc_rtc::Logger::Policy::NON_THREADED, "/tmp", "mc_rtc-test"); + * logger.start("CoMGenerator", dt); + * com_traj.addToLogger(logger); + * + * unsigned n_loop = preview.indexFromTime(com_traj.steps().back().t()) - preview.halfSize(); + * + * + * // Generate the CoM trajectory based on the parameters in com_traj + * // In practice this would be done at every iteration of the controller, we + * // use a loop here to emulate this. + * auto comGeneration = [&]() { + * for(unsigned loop = 0; loop <= n_loop; loop++) + * { + * com_traj.generate(loop); + * mc_rtc::log::info("Generated CoM at current time: {}, com_traj.OutputCOGPosition().transpose()); + * // Note that you can change the desired steps at any time. + * // See changeFutureSteps() and related functions + * logger.log(); + * } + * }; + * + * // Run and measure the comGeneration loop + * auto duration = mc_rtc::measure_ms::execution(comGeneration).count(); + * mc_rtc::log::info("calc time = {} (ms)", duration); + * mc_rtc::log::info("ave. calc time = {} (ms)", duration / static_cast(n_loop)); + * mc_rtc::log::success("end of com trajectory generation"); + * + * com_traj.removeFromLogger(logger); + * return 0; + * } + * \endcode + * + * The generated trajectory should look like the figure below. + * @image html images/generator_com_generation.svg width=100% */ struct MC_PLANNING_DLLAPI generator { diff --git a/src/mc_planning/PreviewWindow.cpp b/src/mc_planning/PreviewWindow.cpp index 07248aad2a..bcfede5b50 100644 --- a/src/mc_planning/PreviewWindow.cpp +++ b/src/mc_planning/PreviewWindow.cpp @@ -12,7 +12,6 @@ unsigned PreviewElement::index() const noexcept return index_; } - double PreviewElement::time() const noexcept { return window_.timeFromIndex(index_); diff --git a/tests/hmc/testCoMGenerator.cpp b/tests/hmc/testCoMGenerator.cpp index da1a5cc991..051dba6c48 100644 --- a/tests/hmc/testCoMGenerator.cpp +++ b/tests/hmc/testCoMGenerator.cpp @@ -4,9 +4,8 @@ #include using namespace mc_planning; -using Vector3 = Eigen::Vector3d; -int main(void) +int main(int, char **) { double dt = 0.005; CenteredPreviewWindow preview(1.6, dt); @@ -29,16 +28,19 @@ int main(void) steps.initialize(); com_traj.steps(steps); - mc_rtc::log::info("Desired steps:\nTime\tCoM X\tCoM Y\n{}", mc_rtc::io::to_string(steps.steps(), "\n")); + mc_rtc::log::info("Desired steps:\nTime\tZMP X\tZMP Y\n{}", mc_rtc::io::to_string(steps.steps(), "\n")); + // Setup logging mc_rtc::Logger logger(mc_rtc::Logger::Policy::NON_THREADED, "/tmp", "mc_rtc-test"); logger.start("CoMGenerator", dt); com_traj.addToLogger(logger); unsigned n_loop = preview.indexFromTime(com_traj.steps().back().t()) - preview.halfSize(); - mc_rtc::log::info("n_loop {}", n_loop); + /* * Generate the CoM trajectory based on the parameters in com_traj + * In practice this would be done at every iteration of the controller, we + * use a loop here to emulate this. */ auto comGeneration = [&]() { for(unsigned loop = 0; loop <= n_loop; loop++) @@ -48,6 +50,7 @@ int main(void) } }; + // Run and measure the comGeneration loop auto duration = mc_rtc::measure_ms::execution(comGeneration).count(); mc_rtc::log::info("Desired steps:\nTime\tCoM X\tCoM Y\n{}", mc_rtc::io::to_string(com_traj.steps().steps(), "\n")); mc_rtc::log::info("calc time = {} (ms)", duration); diff --git a/tests/test_hmc.cpp b/tests/test_hmc.cpp index 71836c6675..cf76e22abb 100644 --- a/tests/test_hmc.cpp +++ b/tests/test_hmc.cpp @@ -212,7 +212,8 @@ BOOST_AUTO_TEST_CASE(TestPreviewWindow) BOOST_REQUIRE_EQUAL(start.localTime(), 0.0); for(const auto w : window) { - mc_rtc::log::info("index {}, local time {:.3f}, global index {}, global time {:.3f}", w.localIndex(), w.localTime(), w.index(), w.time()); + mc_rtc::log::info("index {}, local time {:.3f}, global index {}, global time {:.3f}", w.localIndex(), + w.localTime(), w.index(), w.time()); BOOST_REQUIRE(w.localIndex() <= 6); BOOST_REQUIRE(w.index() <= 6); @@ -236,7 +237,8 @@ BOOST_AUTO_TEST_CASE(TestPreviewWindow) BOOST_REQUIRE_EQUAL(start.localTime(), 0.0); for(const auto w : window) { - mc_rtc::log::info("local index {}, local time {:.3f}, global index {}, global time {:.3f}", w.localIndex(), w.localTime(), w.index(), w.time()); + mc_rtc::log::info("local index {}, local time {:.3f}, global index {}, global time {:.3f}", w.localIndex(), + w.localTime(), w.index(), w.time()); BOOST_REQUIRE(w.localIndex() <= 6); } From d1b46c8c207e119966d18d59cfa097afa87576f1 Mon Sep 17 00:00:00 2001 From: Arnaud TANGUY Date: Fri, 26 Jun 2020 14:04:20 +0900 Subject: [PATCH 32/43] [doc] Install documentation images --- .github/workflows/build.yml | 2 +- doc/CMakeLists.txt | 4 +++ include/mc_planning/generator.h | 50 +++++++++++++++++++++++++++------ 3 files changed, 47 insertions(+), 9 deletions(-) diff --git a/.github/workflows/build.yml b/.github/workflows/build.yml index 01c1590e95..f73f23ccfe 100644 --- a/.github/workflows/build.yml +++ b/.github/workflows/build.yml @@ -209,7 +209,7 @@ jobs: pushd . cd doc cp -r /usr/local/share/doc/mc_rtc/doxygen-html . - cp -r images doxygen-html + find . -type f -print0 | xargs -0 sed -i -e's@/usr/local/share/doc/SpaceVecAlg/doxygen-html/@https://jrl-umi3218.github.io/SpaceVecAlg/doxygen/HEAD/@g' find . -type f -print0 | xargs -0 sed -i -e's@/usr/local/share/doc/SpaceVecAlg/doxygen-html/@https://jrl-umi3218.github.io/SpaceVecAlg/doxygen/HEAD/@g' find . -type f -print0 | xargs -0 sed -i -e's@/usr/local/share/doc/RBDyn/doxygen-html/@https://jrl-umi3218.github.io/RBDyn/doxygen/HEAD/@g' find . -type f -print0 | xargs -0 sed -i -e's@/usr/local/share/doc/Tasks/doxygen-html/@https://jrl-umi3218.github.io/Tasks/doxygen/HEAD/@g' diff --git a/doc/CMakeLists.txt b/doc/CMakeLists.txt index 8d66832a1b..c88790712f 100644 --- a/doc/CMakeLists.txt +++ b/doc/CMakeLists.txt @@ -8,3 +8,7 @@ configure_file(_plugins/doxygen.rb.in "${CMAKE_CURRENT_SOURCE_DIR}/_plugins/doxy install(DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/_data/schemas DESTINATION ${JSON_DOC_INSTALL} FILES_MATCHING PATTERN "*.json") + +install(DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/images + DESTINATION ${CMAKE_INSTALL_DOCDIR}/doxygen-html +) diff --git a/include/mc_planning/generator.h b/include/mc_planning/generator.h index 9c6c027c8b..a210724263 100644 --- a/include/mc_planning/generator.h +++ b/include/mc_planning/generator.h @@ -202,53 +202,87 @@ struct MC_PLANNING_DLLAPI generator { return m_poles[0]; } + + /** + * @name Ideal CoG/ZMP generated by the long-term trajectory. + * @{ + */ + /** Ideal CoG position generated by the long-term trajectory. */ const Eigen::Vector3d & IdealCOGPosition() const { return m_COG_ideal.P; } + /** Ideal CoG velocity generated by the long-term trajectory. */ const Eigen::Vector3d & IdealCOGVelocity() const { return m_COG_ideal.V; } + /** Ideal CoG acceleration generated by the long-term trajectory. */ const Eigen::Vector3d & IdealCOGAcceleration() const { return m_COG_ideal.Vdot; } + /** Ideal ZMP position generated by the long-term trajectory. */ + const Eigen::Vector3d & IdealZMPPosition() const + { + return m_Pcalpha_ideal; + } + /** Ideal ZMP velocity generated by the long-term trajectory. */ + const Eigen::Vector3d & IdealZMPVelocity() const + { + return m_Vcalpha_ideal; + } + // @} + /** + * @name Compenstated CoG/ZMP such that the short-term trajectory tracks the + * long-term trajectory + * @{ + */ + /** + * @brief Compensation of the COG position due to the short-term trajectory + */ const Eigen::Vector3d & CompensatedCOGPosition() const { return m_COG_cmp.P; } + /** + * @brief Compensation of the COG velocity due to the short-term trajectory + */ const Eigen::Vector3d & CompensatedCOGVelocity() const { return m_COG_cmp.V; } + /** + * @brief Compensation of the COG acceleration due to the short-term trajectory + */ const Eigen::Vector3d & CompensatedCOGAcceleration() const { return m_COG_cmp.Vdot; } + /** + * @brief Output COG position at current time computed by the short-term trajectory to best track the long-term trajectory + */ const Eigen::Vector3d & OutputCOGPosition() const { return m_COG_out.P; } + /** + * @brief Output COG velocity at current time computed by the short-term trajectory to best track the long-term trajectory + */ const Eigen::Vector3d & OutputCOGVelocity() const { return m_COG_out.V; } + /** + * @brief Output COG acceleration at current time computed by the short-term trajectory to best track the long-term trajectory + */ const Eigen::Vector3d & OutputCOGAcceleration() const { return m_COG_out.Vdot; } - const Eigen::Vector3d & IdealZMPPosition() const - { - return m_Pcalpha_ideal; - } - const Eigen::Vector3d & IdealZMPVelocity() const - { - return m_Vcalpha_ideal; - } const Eigen::Vector3d & CompensatedZMPPosition() const { From 578680e8c4b57d01330d531a3003cc044cd17b3b Mon Sep 17 00:00:00 2001 From: Arnaud TANGUY Date: Fri, 26 Jun 2020 14:34:09 +0900 Subject: [PATCH 33/43] [HMC/doc] Complete generator doc --- include/mc_planning/generator.h | 92 ++++++++++++++++++++------------- 1 file changed, 57 insertions(+), 35 deletions(-) diff --git a/include/mc_planning/generator.h b/include/mc_planning/generator.h index a210724263..9f2d702eb9 100644 --- a/include/mc_planning/generator.h +++ b/include/mc_planning/generator.h @@ -128,6 +128,16 @@ struct MC_PLANNING_DLLAPI generator */ void removeFromLogger(mc_rtc::Logger & logger); + /** + * @name Step management + * Provide sagital and lateral ZMP reference as steps define by a set of (time, Vector2d). + * Steps can be changed at any time using these function. Note that in general steps + * should be changed in the future (after the middle of the preview window). + * If the next steps are suddently changed at the current time, this will + * cause a ZMP jerk (the CoM trajectory remains continuous). Prefer to change steps later in the future whenever + * possible to achieve smoother trajectories. + * @{ + */ /** * @brief Desired steps * @see setSteps(const std::vector & steps) @@ -160,19 +170,19 @@ struct MC_PLANNING_DLLAPI generator { m_steps.changeFutureSteps(transitionTime, futureSteps); } + ///@} /** - * @name Accessors + * @name Poles of the short-term trajectory + * Typical values range from \f$ [1,1,150] \f$ to \f$ [1,1,300] \f$. + * The bigger the poles are, the the faster the short-term trajectory will converge to the long-term trajectory. + * Too high a value could lead to jerk when the long-term trajectory is not continuous + * @{ */ - ///@{ /** * @brief Sets the poles for the short-term trajectory * - * Typical values range from \f$ [1,1,150] \f$ to \f$ [1,1,300] \f$. - * The bigger the poles are, the more the faster the short-term trajectory - * will converge to the long-term trajectory. Too high a value could lead to - * jerk when the long-term trajectory is not continuous * * @param poles Poles for the short-term trajectory */ @@ -202,6 +212,7 @@ struct MC_PLANNING_DLLAPI generator { return m_poles[0]; } + ///@} /** * @name Ideal CoG/ZMP generated by the long-term trajectory. @@ -232,67 +243,78 @@ struct MC_PLANNING_DLLAPI generator { return m_Vcalpha_ideal; } - // @} + ///@} /** - * @name Compenstated CoG/ZMP such that the short-term trajectory tracks the - * long-term trajectory + * @name Compenstated CoG/ZMP generated by the short-term trajectory + * + * The short-term trajectory is tracking the long-term trajectory. Usually, + * both long-term and short-term trajectories are close to each other, but + * when discontinuities of the long-term trajectory happen (mostly due to + * change of steps in the current preview window), then this compensation + * makes sure that the long-term trajectory is tracked smoothly without + * discontinuities. * @{ */ - /** - * @brief Compensation of the COG position due to the short-term trajectory - */ + /** @brief Compensation of the COG position due to the short-term trajectory */ const Eigen::Vector3d & CompensatedCOGPosition() const { return m_COG_cmp.P; } - /** - * @brief Compensation of the COG velocity due to the short-term trajectory - */ + /** @brief Compensation of the COG velocity due to the short-term trajectory */ const Eigen::Vector3d & CompensatedCOGVelocity() const { return m_COG_cmp.V; } - /** - * @brief Compensation of the COG acceleration due to the short-term trajectory - */ + /** @brief Compensation of the COG acceleration due to the short-term trajectory */ const Eigen::Vector3d & CompensatedCOGAcceleration() const { return m_COG_cmp.Vdot; } + /** @brief Compensation of the ZMP position due to the short-term trajectory */ + const Eigen::Vector3d & CompensatedZMPPosition() const + { + return m_Pcalpha_cmp; + } + /** @brief Compensation of the ZMP velocity due to the short-term trajectory */ + const Eigen::Vector3d & CompensatedZMPVelocity() const + { + return m_Vcalpha_cmp; + } + ///@} /** - * @brief Output COG position at current time computed by the short-term trajectory to best track the long-term trajectory + * @name Output CoG/ZMP such that the short-term trajectory tracks the long-term trajectory. + * + * This output is a combination of the ideal (long-term) and compensated + * (short-term) output that ensures continuous COG trajectories that will + * track the long-term trajectory. + * @{ */ + /** + * @brief Compensation of the COG position due to the short-term trajectory + */ + /** @brief Output COG position at current time computed by the short-term trajectory to best track the long-term + * trajectory */ const Eigen::Vector3d & OutputCOGPosition() const { return m_COG_out.P; } - /** - * @brief Output COG velocity at current time computed by the short-term trajectory to best track the long-term trajectory - */ + /** @brief Output COG velocity at current time computed by the short-term trajectory to best track the long-term + * trajectory */ const Eigen::Vector3d & OutputCOGVelocity() const { return m_COG_out.V; } - /** - * @brief Output COG acceleration at current time computed by the short-term trajectory to best track the long-term trajectory - */ + /** @brief Output COG acceleration at current time computed by the short-term trajectory to best track the long-term + * trajectory */ const Eigen::Vector3d & OutputCOGAcceleration() const { return m_COG_out.Vdot; } - - const Eigen::Vector3d & CompensatedZMPPosition() const - { - return m_Pcalpha_cmp; - } - const Eigen::Vector3d & CompensatedZMPVelocity() const - { - return m_Vcalpha_cmp; - } - + /** @brief Output ZMP acceleration at current time computed by the short-term trajectory to best track the long-term + * trajectory */ const Eigen::Vector3d & OutputZMPPosition() const { return m_Pcalpha_out; From 58e70d44edb52853b9829a55a8329e0b7a5ae0a4 Mon Sep 17 00:00:00 2001 From: Arnaud TANGUY Date: Fri, 26 Jun 2020 16:52:32 +0900 Subject: [PATCH 34/43] [doc] Include samples in doxygen, reorganize and install samples --- CMakeLists.txt | 1 + include/mc_planning/generator.h | 77 ++------ samples/CMakeLists.txt | 21 ++- samples/CoMGenerator/CMakeLists.txt | 1 + .../mc_sample_CoMGeneration.in.cpp | 4 + .../src/CoMTrajectoryGeneration.h | 17 -- .../CoMTrajectoryGeneration_Initial.cpp | 175 ------------------ .../states/CoMTrajectoryGeneration_Initial.h | 45 ----- src/mc_control/CMakeLists.txt | 1 + .../CoMTrajectoryGeneration/CMakeLists.txt | 5 +- .../etc/CoMTrajectoryGeneration.in.yaml | 6 +- .../src/CMakeLists.txt | 2 +- .../src/CoMTrajectoryGeneration.cpp | 6 +- .../src/CoMTrajectoryGeneration.h | 18 ++ .../CoMTrajectoryGeneration/src/api.h | 0 .../CoMTrajectoryGeneration/src/lib.cpp | 0 .../src/states/CMakeLists.txt | 0 .../CoMTrajectoryGeneration_Initial.cpp | 138 ++++++++++++++ .../states/CoMTrajectoryGeneration_Initial.h | 45 +++++ .../src/states/StabilizerStandingTrackCoM.cpp | 12 +- .../src/states/StabilizerStandingTrackCoM.h | 10 +- .../src/states/data/states.json | 0 tests/CMakeLists.txt | 2 - tests/hmc/CMakeLists.txt | 2 - 24 files changed, 258 insertions(+), 330 deletions(-) create mode 100644 samples/CoMGenerator/CMakeLists.txt rename tests/hmc/testCoMGenerator.cpp => samples/CoMGenerator/mc_sample_CoMGeneration.in.cpp (94%) delete mode 100644 samples/CoMTrajectoryGeneration/src/CoMTrajectoryGeneration.h delete mode 100644 samples/CoMTrajectoryGeneration/src/states/CoMTrajectoryGeneration_Initial.cpp delete mode 100644 samples/CoMTrajectoryGeneration/src/states/CoMTrajectoryGeneration_Initial.h rename {samples => src/mc_control/samples}/CoMTrajectoryGeneration/CMakeLists.txt (65%) rename {samples => src/mc_control/samples}/CoMTrajectoryGeneration/etc/CoMTrajectoryGeneration.in.yaml (92%) rename {samples => src/mc_control/samples}/CoMTrajectoryGeneration/src/CMakeLists.txt (93%) rename {samples => src/mc_control/samples}/CoMTrajectoryGeneration/src/CoMTrajectoryGeneration.cpp (84%) create mode 100644 src/mc_control/samples/CoMTrajectoryGeneration/src/CoMTrajectoryGeneration.h rename {samples => src/mc_control/samples}/CoMTrajectoryGeneration/src/api.h (100%) rename {samples => src/mc_control/samples}/CoMTrajectoryGeneration/src/lib.cpp (100%) rename {samples => src/mc_control/samples}/CoMTrajectoryGeneration/src/states/CMakeLists.txt (100%) create mode 100644 src/mc_control/samples/CoMTrajectoryGeneration/src/states/CoMTrajectoryGeneration_Initial.cpp create mode 100644 src/mc_control/samples/CoMTrajectoryGeneration/src/states/CoMTrajectoryGeneration_Initial.h rename {samples => src/mc_control/samples}/CoMTrajectoryGeneration/src/states/StabilizerStandingTrackCoM.cpp (91%) rename {samples => src/mc_control/samples}/CoMTrajectoryGeneration/src/states/StabilizerStandingTrackCoM.h (58%) rename {samples => src/mc_control/samples}/CoMTrajectoryGeneration/src/states/data/states.json (100%) delete mode 100644 tests/hmc/CMakeLists.txt diff --git a/CMakeLists.txt b/CMakeLists.txt index c6779b2432..c39c732ca4 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -33,6 +33,7 @@ set_property(GLOBAL PROPERTY USE_FOLDERS ON) project(${PROJECT_NAME} C CXX) set(MC_RTC_SOURCE_DIR "${PROJECT_SOURCE_DIR}") +set(DOXYGEN_EXAMPLE_PATH "${CMAKE_INSTALL_PREFIX}/${CMAKE_INSTALL_DOCDIR}/doxygen-html/examples") option(GENERATE_COVERAGE "Generate coverage data" FALSE) if("${CMAKE_CXX_FLAGS}" MATCHES "--coverage") diff --git a/include/mc_planning/generator.h b/include/mc_planning/generator.h index 9f2d702eb9..1fd303a664 100644 --- a/include/mc_planning/generator.h +++ b/include/mc_planning/generator.h @@ -30,71 +30,7 @@ namespace mc_planning * timing. * See the corresponding mc_rtc sample testCoMGeneration and benchmark. * - * \code{.cpp} - * #include - * #include - * #include - * #include - * - * using namespace mc_planning; - * - * int main(int, char **) - * { - * double dt = 0.005; - * CenteredPreviewWindow preview(1.6, dt); - * - * // Trajectory of size 2*n_preview+1 - * generator com_traj(preview); - * - * auto prev_time = preview.halfDuration(); - * PreviewSteps steps; - * steps.add({prev_time, {-0.2, 0.0}}); - * steps.addRelative({prev_time, {0.0, 0.0}}); - * steps.addRelative({0.1, {0.0, 0.0}}); - * steps.addRelative({1.6, {0.0, 0.0}}); - * steps.addRelative({0.1, {0.2, 0.095}}); - * steps.addRelative({1.6, {0.0, 0.0}}); - * steps.addRelative({0.1, {0.0, -0.19}}); - * steps.addRelative({1.6, {0.0, 0.0}}); - * steps.addRelative({0.1, {-0.2, 0.095}}); - * steps.addRelative({prev_time, {0.0, 0.0}}); - * steps.initialize(); - * - * com_traj.steps(steps); - * mc_rtc::log::info("Desired steps:\nTime\tZMP X\tZMP Y\n{}", mc_rtc::io::to_string(steps.steps(), "\n")); - * - * // Setup logging - * mc_rtc::Logger logger(mc_rtc::Logger::Policy::NON_THREADED, "/tmp", "mc_rtc-test"); - * logger.start("CoMGenerator", dt); - * com_traj.addToLogger(logger); - * - * unsigned n_loop = preview.indexFromTime(com_traj.steps().back().t()) - preview.halfSize(); - * - * - * // Generate the CoM trajectory based on the parameters in com_traj - * // In practice this would be done at every iteration of the controller, we - * // use a loop here to emulate this. - * auto comGeneration = [&]() { - * for(unsigned loop = 0; loop <= n_loop; loop++) - * { - * com_traj.generate(loop); - * mc_rtc::log::info("Generated CoM at current time: {}, com_traj.OutputCOGPosition().transpose()); - * // Note that you can change the desired steps at any time. - * // See changeFutureSteps() and related functions - * logger.log(); - * } - * }; - * - * // Run and measure the comGeneration loop - * auto duration = mc_rtc::measure_ms::execution(comGeneration).count(); - * mc_rtc::log::info("calc time = {} (ms)", duration); - * mc_rtc::log::info("ave. calc time = {} (ms)", duration / static_cast(n_loop)); - * mc_rtc::log::success("end of com trajectory generation"); - * - * com_traj.removeFromLogger(logger); - * return 0; - * } - * \endcode + * \include mc_sample_CoMGeneration.cpp * * The generated trajectory should look like the figure below. * @image html images/generator_com_generation.svg width=100% @@ -400,3 +336,14 @@ struct MC_PLANNING_DLLAPI generator }; } // namespace mc_planning + +/** + * @example mc_sample_CoMGeneration.cpp + * This sample generates a dynamic trajectory for the CoM and ZMP based on + * desired ZMP step inputs. + * + * The generated trajectory should look like the figure below. + * @image html images/generator_com_generation.svg width=100% + * + * \see mc_planning::generator + */ diff --git a/samples/CMakeLists.txt b/samples/CMakeLists.txt index e0b765fde8..5d4f67b0a8 100644 --- a/samples/CMakeLists.txt +++ b/samples/CMakeLists.txt @@ -1 +1,20 @@ -add_subdirectory(CoMTrajectoryGeneration) +macro(add_example NAME) + add_executable(${NAME} ${NAME}.cpp) + target_link_libraries(${NAME} PUBLIC ${ARGN}) +endmacro() + +macro(add_example_doc NAME) + file(RELATIVE_PATH MC_RTC_SAMPLE_PATH ${PROJECT_SOURCE_DIR} ${CMAKE_CURRENT_SOURCE_DIR}/${NAME}.in.cpp) + set(MC_RTC_SAMPLE_COMMAND ${NAME}) + configure_file(${NAME}.in.cpp + ${CMAKE_CURRENT_BINARY_DIR}/${NAME}.cpp) + add_example(${NAME} ${ARGN}) + + install(TARGETS ${NAME} DESTINATION bin) + + install(FILES ${CMAKE_CURRENT_BINARY_DIR}/${NAME}.cpp + DESTINATION ${CMAKE_INSTALL_DOCDIR}/doxygen-html/examples + ) +endmacro() + +add_subdirectory(CoMGenerator) diff --git a/samples/CoMGenerator/CMakeLists.txt b/samples/CoMGenerator/CMakeLists.txt new file mode 100644 index 0000000000..0141977a5c --- /dev/null +++ b/samples/CoMGenerator/CMakeLists.txt @@ -0,0 +1 @@ +add_example_doc(mc_sample_CoMGeneration mc_planning) diff --git a/tests/hmc/testCoMGenerator.cpp b/samples/CoMGenerator/mc_sample_CoMGeneration.in.cpp similarity index 94% rename from tests/hmc/testCoMGenerator.cpp rename to samples/CoMGenerator/mc_sample_CoMGeneration.in.cpp index 051dba6c48..4daacc793f 100644 --- a/tests/hmc/testCoMGenerator.cpp +++ b/samples/CoMGenerator/mc_sample_CoMGeneration.in.cpp @@ -1,3 +1,7 @@ +// This sample was generated from @MC_RTC_SAMPLE_PATH@ +// You can try it by running: +// $ @MC_RTC_SAMPLE_COMMAND@ + #include #include #include diff --git a/samples/CoMTrajectoryGeneration/src/CoMTrajectoryGeneration.h b/samples/CoMTrajectoryGeneration/src/CoMTrajectoryGeneration.h deleted file mode 100644 index 0ed6293aa3..0000000000 --- a/samples/CoMTrajectoryGeneration/src/CoMTrajectoryGeneration.h +++ /dev/null @@ -1,17 +0,0 @@ -#pragma once - -#include -#include - -#include "api.h" - -struct CoMTrajectoryGeneration_DLLAPI CoMTrajectoryGeneration : public mc_control::fsm::Controller -{ - CoMTrajectoryGeneration(mc_rbdyn::RobotModulePtr rm, double dt, const mc_rtc::Configuration & config); - - bool run() override; - - void reset(const mc_control::ControllerResetData & reset_data) override; -private: - mc_rtc::Configuration config_; -}; \ No newline at end of file diff --git a/samples/CoMTrajectoryGeneration/src/states/CoMTrajectoryGeneration_Initial.cpp b/samples/CoMTrajectoryGeneration/src/states/CoMTrajectoryGeneration_Initial.cpp deleted file mode 100644 index 74ec10b020..0000000000 --- a/samples/CoMTrajectoryGeneration/src/states/CoMTrajectoryGeneration_Initial.cpp +++ /dev/null @@ -1,175 +0,0 @@ -#include "CoMTrajectoryGeneration_Initial.h" -#include "../CoMTrajectoryGeneration.h" -#include -#include - -namespace mc_samples -{ - -void CoMTrajectoryGeneration_Initial::configure(const mc_rtc::Configuration & config) -{ - config("previewTime", previewTime_); -} - -void CoMTrajectoryGeneration_Initial::start(mc_control::fsm::Controller & ctl) -{ - preview_ = mc_planning::CenteredPreviewWindow(previewTime_, ctl.timeStep); - /// XXX should this be CoM height or waist height? - double waist_height = ctl.realRobot().com().z(); - - leftFootPos_ = ctl.robot().surfacePose("LeftFootCenter").translation(); - rightFootPos_ = ctl.robot().surfacePose("RightFootCenter").translation(); - feetCenterPos_ = (leftFootPos_ + rightFootPos_)/2; - - double leftY = leftFootPos_.y()-0.04; - double rightY = rightFootPos_.y()+0.04; - double centerY = feetCenterPos_.y(); - const Eigen::Vector3d & currCoM = ctl.robot().com(); - //clang-format off - steps_.reset( - { - {0.0, {currCoM.x(), currCoM.y()}}, - {0.2, {0.0, centerY}}, - {2, {0.0, centerY}}, - {2.2, {0.0, rightY}}, - {4, {0.0, rightY}}, - {4.2, {0.0, leftY}}, - {6, {0.0, leftY}}, - {6.2, {0.0, rightY}}, - {8, {0.0, rightY}}, - {8.2, {0.0, centerY}} - }); - steps_.initialize(); - //clang-format on - - comGenerator_ = std::make_shared(preview_, ctl.robot().mass(), - waist_height); - comGenerator_->addToLogger(ctl.logger()); - comGenerator_->steps(steps_); - updateSteps(); - - using Color = mc_rtc::gui::Color; - using Style = mc_rtc::gui::plot::Style; - ctl.gui()->addPlot( - "Trajectory (Y)", - mc_rtc::gui::plot::X("t", [this]() { return t_; }), - mc_rtc::gui::plot::Y("Output CoM", [this]() { return comGenerator_->OutputCOGPosition().y(); }, Color::Magenta), - mc_rtc::gui::plot::Y("Ideal CoM", [this]() { return comGenerator_->IdealCOGPosition().y(); }, Color::Red, Style::Dashed), - mc_rtc::gui::plot::Y("Output ZMP", [this]() { return comGenerator_->OutputZMPPosition().y(); }, Color::Purple), - mc_rtc::gui::plot::Y("Ideal ZMP", [this]() { return comGenerator_->IdealZMPPosition().y(); }, Color::Blue, Style::Dashed)); - - ctl.gui()->addElement({"CoMTrajectoryGeneration"}, - mc_rtc::gui::NumberInput("Delay", - [this]() - { - return delay_; - }, - [this](double delay) - { - delay_ = delay; - }), - mc_rtc::gui::NumberInput("Transition time", - [this]() - { - return transition_; - }, - [this](double transition) - { - transition_ = transition; - }), - mc_rtc::gui::Button("Left", - [this,leftY]() - { - auto curr = t_ + previewTime_ + delay_; - comGenerator_->changeFutureSteps( - curr, - { - {curr + transition_, {0, leftY}} - }); - updateSteps(); - }), - mc_rtc::gui::Button("Right", - [this,rightY]() - { - auto curr= t_ + previewTime_ + delay_; - comGenerator_->changeFutureSteps( - curr, - { {curr + transition_, {0, rightY}} }); - updateSteps(); - }), - mc_rtc::gui::Button("Center", - [this,centerY]() - { - auto curr= t_ + previewTime_ + delay_; - comGenerator_->changeFutureSteps( - curr, - { {curr + transition_, {0, centerY}} }); - updateSteps(); - }), - mc_rtc::gui::Button("Left-Right-Left", - [this,centerY,leftY,rightY]() - { - auto curr= t_ + previewTime_ + delay_; - std::vector> futureSteps; - auto t = curr + transition_; - futureSteps.push_back({t, {0, leftY}}); - t += 2; - futureSteps.push_back({t, {0, leftY}}); - t += transition_; - futureSteps.push_back({t, {0, rightY}}); - t += 2; - futureSteps.push_back({t, {0, rightY}}); - t += transition_; - futureSteps.push_back({t, {0, centerY}}); - comGenerator_->changeFutureSteps(curr, futureSteps); - updateSteps(); - }) - ); - - ctl.logger().addLogEntry("perf_CoMGenerator[ms]", - [this]() - { - return generationTime_; - }); -} - -void CoMTrajectoryGeneration_Initial::updateSteps() -{ - mc_rtc::log::info("Time start: {}", t_); - mc_rtc::log::info("Time curr : {}", t_+previewTime_); - mc_rtc::log::info( - "Desired steps:\nTime\tCoM X\tCoM Y\n{}", - mc_rtc::io::to_string(comGenerator_->steps().steps(), "\n")); -} - -bool CoMTrajectoryGeneration_Initial::run(mc_control::fsm::Controller & ctl) -{ - /* Generate trajectory starting at iter_ time (start of the past preview window) - * The trajectory result is computed for the current time (iter_+previewSize_) - */ - auto generateTrajectory = [this]() - { - comGenerator_->generate(iter_); - }; - generationTime_ = mc_rtc::measure_ms::execution(generateTrajectory).count(); - - // Provide targets to the stabilizer (running in another state) - if(ctl.datastore().has("StabilizerStandingTrackCoM::target")) - { - ctl.datastore().call("StabilizerStandingTrackCoM::target", comGenerator_->OutputCOGPosition(), comGenerator_->OutputCOGVelocity(), comGenerator_->OutputCOGAcceleration(), comGenerator_->OutputZMPPosition()); - } - - iter_++; - t_ += ctl.timeStep; - output("OK"); - return false; -} - -void CoMTrajectoryGeneration_Initial::teardown(mc_control::fsm::Controller & ctl) -{ - comGenerator_->removeFromLogger(ctl.logger()); -} - -} /* mc_samples */ - -EXPORT_SINGLE_STATE("CoMTrajectoryGeneration_Initial", mc_samples::CoMTrajectoryGeneration_Initial) diff --git a/samples/CoMTrajectoryGeneration/src/states/CoMTrajectoryGeneration_Initial.h b/samples/CoMTrajectoryGeneration/src/states/CoMTrajectoryGeneration_Initial.h deleted file mode 100644 index da9606af29..0000000000 --- a/samples/CoMTrajectoryGeneration/src/states/CoMTrajectoryGeneration_Initial.h +++ /dev/null @@ -1,45 +0,0 @@ -#pragma once - -#include -#include - -namespace mc_samples -{ - -struct CoMTrajectoryGeneration_Initial : mc_control::fsm::State -{ - - void configure(const mc_rtc::Configuration & config) override; - - void start(mc_control::fsm::Controller & ctl) override; - - bool run(mc_control::fsm::Controller & ctl) override; - - void teardown(mc_control::fsm::Controller & ctl) override; - - private: - void updateSteps(); - - private: - mc_planning::CenteredPreviewWindow preview_{0,0}; - Eigen::Vector3d polesX_ = {1.0, 1.0, 150.0}; - Eigen::Vector3d polesY_ = {1.0, 1.0, 150.0}; - double previewTime_ = 1.6; - - unsigned iter_ = 0; ///< Number of iterations elapsed since started - double t_ = 0; ///< Time elapsed since started - double generationTime_ = 0; ///< Time spent generating the trajectory - - double delay_ = 0.5; ///< Delay before moving manually - double transition_ = 0.2; ///< Transition time between steps (interpolation) - - Eigen::Vector3d leftFootPos_ = Eigen::Vector3d::Zero(); - Eigen::Vector3d rightFootPos_ = Eigen::Vector3d::Zero(); - Eigen::Vector3d feetCenterPos_ = Eigen::Vector3d::Zero(); - -private: - std::shared_ptr comGenerator_; - mc_planning::PreviewSteps steps_; ///< Foot steps defined as time, ZMP_x, ZMP_y -}; - -} /* mc_samples */ diff --git a/src/mc_control/CMakeLists.txt b/src/mc_control/CMakeLists.txt index aacc7f7e96..51487db9ab 100644 --- a/src/mc_control/CMakeLists.txt +++ b/src/mc_control/CMakeLists.txt @@ -52,6 +52,7 @@ if(${BUILD_CONTROLLER_SAMPLES}) add_subdirectory(samples/Admittance) add_subdirectory(samples/Impedance) add_subdirectory(samples/ExternalForces) + add_subdirectory(samples/CoMTrajectoryGeneration) endif() add_subdirectory(HalfSitPose) diff --git a/samples/CoMTrajectoryGeneration/CMakeLists.txt b/src/mc_control/samples/CoMTrajectoryGeneration/CMakeLists.txt similarity index 65% rename from samples/CoMTrajectoryGeneration/CMakeLists.txt rename to src/mc_control/samples/CoMTrajectoryGeneration/CMakeLists.txt index 55f44e9372..218a90be9d 100644 --- a/samples/CoMTrajectoryGeneration/CMakeLists.txt +++ b/src/mc_control/samples/CoMTrajectoryGeneration/CMakeLists.txt @@ -4,10 +4,9 @@ set(AROBASE "@") set(CoMTrajectoryGeneration_STATES_INSTALL_PREFIX "${MC_CONTROLLER_INSTALL_PREFIX}/fsm/states") set(CoMTrajectoryGeneration_STATES_DATA_INSTALL_PREFIX "${MC_CONTROLLER_INSTALL_PREFIX}/fsm/states/data") set(CoMTrajectoryGeneration_INIT_STATE "Pause_2s") -set(MC_STATES_DEFAULT_INSTALL_PREFIX "${MC_CONTROLLER_INSTALL_PREFIX}/fsm/states") -set(MC_STATES_INSTALL_PREFIX "${MC_CONTROLLER_INSTALL_PREFIX}/${PROJECT_NAME}/states") +# set(MC_STATES_DEFAULT_INSTALL_PREFIX "${MC_CONTROLLER_INSTALL_PREFIX}/fsm/states") +# set(MC_STATES_INSTALL_PREFIX "${MC_CONTROLLER_INSTALL_PREFIX}/${PROJECT_NAME}/states") configure_file(etc/CoMTrajectoryGeneration.in.yaml "${CMAKE_CURRENT_BINARY_DIR}/etc/CoMTrajectoryGeneration.yaml") -install(FILES "${CMAKE_CURRENT_BINARY_DIR}/etc/CoMTrajectoryGeneration.yaml" DESTINATION "${MC_CONTROLLER_INSTALL_PREFIX}/etc") unset(AROBASE) install(FILES "${CMAKE_CURRENT_BINARY_DIR}/etc/CoMTrajectoryGeneration.yaml" diff --git a/samples/CoMTrajectoryGeneration/etc/CoMTrajectoryGeneration.in.yaml b/src/mc_control/samples/CoMTrajectoryGeneration/etc/CoMTrajectoryGeneration.in.yaml similarity index 92% rename from samples/CoMTrajectoryGeneration/etc/CoMTrajectoryGeneration.in.yaml rename to src/mc_control/samples/CoMTrajectoryGeneration/etc/CoMTrajectoryGeneration.in.yaml index 58ce5720ec..65ee170372 100644 --- a/samples/CoMTrajectoryGeneration/etc/CoMTrajectoryGeneration.in.yaml +++ b/src/mc_control/samples/CoMTrajectoryGeneration/etc/CoMTrajectoryGeneration.in.yaml @@ -8,12 +8,10 @@ StepByStep: false IdleKeepState: true # Where to look for state libraries StatesLibraries: -- "@MC_STATES_DEFAULT_INSTALL_PREFIX@" -- "@MC_STATES_INSTALL_PREFIX@" +- "@CoMTrajectoryGeneration_STATES_INSTALL_PREFIX@" # Where to look for state files StatesFiles: -- "@MC_STATES_DEFAULT_INSTALL_PREFIX@/data" -- "@MC_STATES_INSTALL_PREFIX@/data" +- "@CoMTrajectoryGeneration_STATES_INSTALL_PREFIX@/data" # If true, state factory will be more verbose VerboseStateFactory: false # Additional robots to load diff --git a/samples/CoMTrajectoryGeneration/src/CMakeLists.txt b/src/mc_control/samples/CoMTrajectoryGeneration/src/CMakeLists.txt similarity index 93% rename from samples/CoMTrajectoryGeneration/src/CMakeLists.txt rename to src/mc_control/samples/CoMTrajectoryGeneration/src/CMakeLists.txt index da4b8eb869..eedc064e58 100644 --- a/samples/CoMTrajectoryGeneration/src/CMakeLists.txt +++ b/src/mc_control/samples/CoMTrajectoryGeneration/src/CMakeLists.txt @@ -8,7 +8,7 @@ set(controller_HDR set(CONTROLLER_NAME CoMTrajectoryGeneration) add_library(${CONTROLLER_NAME} SHARED ${controller_SRC} ${controller_HDR}) -set_target_properties(${CONTROLLER_NAME} PROPERTIES COMPILE_FLAGS "-DCoMTrajectoryGeneration_EXPORTS") +set_target_properties(${CONTROLLER_NAME} PROPERTIES COMPILE_FLAGS "-DMC_CONTROL_EXPORTS") target_link_libraries(${CONTROLLER_NAME} PUBLIC mc_rtc::mc_control_fsm) install(TARGETS ${CONTROLLER_NAME} DESTINATION ${MC_CONTROLLER_INSTALL_PREFIX}/..) diff --git a/samples/CoMTrajectoryGeneration/src/CoMTrajectoryGeneration.cpp b/src/mc_control/samples/CoMTrajectoryGeneration/src/CoMTrajectoryGeneration.cpp similarity index 84% rename from samples/CoMTrajectoryGeneration/src/CoMTrajectoryGeneration.cpp rename to src/mc_control/samples/CoMTrajectoryGeneration/src/CoMTrajectoryGeneration.cpp index bce9cccb55..0f48469894 100644 --- a/samples/CoMTrajectoryGeneration/src/CoMTrajectoryGeneration.cpp +++ b/src/mc_control/samples/CoMTrajectoryGeneration/src/CoMTrajectoryGeneration.cpp @@ -1,6 +1,8 @@ #include "CoMTrajectoryGeneration.h" -CoMTrajectoryGeneration::CoMTrajectoryGeneration(mc_rbdyn::RobotModulePtr rm, double dt, const mc_rtc::Configuration & config) +CoMTrajectoryGeneration::CoMTrajectoryGeneration(mc_rbdyn::RobotModulePtr rm, + double dt, + const mc_rtc::Configuration & config) : mc_control::fsm::Controller(rm, dt, config) { @@ -23,5 +25,3 @@ void CoMTrajectoryGeneration::reset(const mc_control::ControllerResetData & rese 0.5); }); } - - diff --git a/src/mc_control/samples/CoMTrajectoryGeneration/src/CoMTrajectoryGeneration.h b/src/mc_control/samples/CoMTrajectoryGeneration/src/CoMTrajectoryGeneration.h new file mode 100644 index 0000000000..904ca0de4d --- /dev/null +++ b/src/mc_control/samples/CoMTrajectoryGeneration/src/CoMTrajectoryGeneration.h @@ -0,0 +1,18 @@ +#pragma once + +#include +#include + +#include "api.h" + +struct CoMTrajectoryGeneration_DLLAPI CoMTrajectoryGeneration : public mc_control::fsm::Controller +{ + CoMTrajectoryGeneration(mc_rbdyn::RobotModulePtr rm, double dt, const mc_rtc::Configuration & config); + + bool run() override; + + void reset(const mc_control::ControllerResetData & reset_data) override; + +private: + mc_rtc::Configuration config_; +}; \ No newline at end of file diff --git a/samples/CoMTrajectoryGeneration/src/api.h b/src/mc_control/samples/CoMTrajectoryGeneration/src/api.h similarity index 100% rename from samples/CoMTrajectoryGeneration/src/api.h rename to src/mc_control/samples/CoMTrajectoryGeneration/src/api.h diff --git a/samples/CoMTrajectoryGeneration/src/lib.cpp b/src/mc_control/samples/CoMTrajectoryGeneration/src/lib.cpp similarity index 100% rename from samples/CoMTrajectoryGeneration/src/lib.cpp rename to src/mc_control/samples/CoMTrajectoryGeneration/src/lib.cpp diff --git a/samples/CoMTrajectoryGeneration/src/states/CMakeLists.txt b/src/mc_control/samples/CoMTrajectoryGeneration/src/states/CMakeLists.txt similarity index 100% rename from samples/CoMTrajectoryGeneration/src/states/CMakeLists.txt rename to src/mc_control/samples/CoMTrajectoryGeneration/src/states/CMakeLists.txt diff --git a/src/mc_control/samples/CoMTrajectoryGeneration/src/states/CoMTrajectoryGeneration_Initial.cpp b/src/mc_control/samples/CoMTrajectoryGeneration/src/states/CoMTrajectoryGeneration_Initial.cpp new file mode 100644 index 0000000000..ddb63596c4 --- /dev/null +++ b/src/mc_control/samples/CoMTrajectoryGeneration/src/states/CoMTrajectoryGeneration_Initial.cpp @@ -0,0 +1,138 @@ +#include "CoMTrajectoryGeneration_Initial.h" +#include +#include +#include "../CoMTrajectoryGeneration.h" + +namespace mc_samples +{ + +void CoMTrajectoryGeneration_Initial::configure(const mc_rtc::Configuration & config) +{ + config("previewTime", previewTime_); +} + +void CoMTrajectoryGeneration_Initial::start(mc_control::fsm::Controller & ctl) +{ + preview_ = mc_planning::CenteredPreviewWindow(previewTime_, ctl.timeStep); + /// XXX should this be CoM height or waist height? + double waist_height = ctl.realRobot().com().z(); + + leftFootPos_ = ctl.robot().surfacePose("LeftFootCenter").translation(); + rightFootPos_ = ctl.robot().surfacePose("RightFootCenter").translation(); + feetCenterPos_ = (leftFootPos_ + rightFootPos_) / 2; + + double leftY = leftFootPos_.y() - 0.04; + double rightY = rightFootPos_.y() + 0.04; + double centerY = feetCenterPos_.y(); + const Eigen::Vector3d & currCoM = ctl.robot().com(); + //clang-format off + steps_.reset({{0.0, {currCoM.x(), currCoM.y()}}, + {0.2, {0.0, centerY}}, + {2, {0.0, centerY}}, + {2.2, {0.0, rightY}}, + {4, {0.0, rightY}}, + {4.2, {0.0, leftY}}, + {6, {0.0, leftY}}, + {6.2, {0.0, rightY}}, + {8, {0.0, rightY}}, + {8.2, {0.0, centerY}}}); + steps_.initialize(); + //clang-format on + + comGenerator_ = std::make_shared(preview_, ctl.robot().mass(), waist_height); + comGenerator_->addToLogger(ctl.logger()); + comGenerator_->steps(steps_); + updateSteps(); + + using Color = mc_rtc::gui::Color; + using Style = mc_rtc::gui::plot::Style; + ctl.gui()->addPlot( + "Trajectory (Y)", mc_rtc::gui::plot::X("t", [this]() { return t_; }), + mc_rtc::gui::plot::Y("Output CoM", [this]() { return comGenerator_->OutputCOGPosition().y(); }, Color::Magenta), + mc_rtc::gui::plot::Y("Ideal CoM", [this]() { return comGenerator_->IdealCOGPosition().y(); }, Color::Red, + Style::Dashed), + mc_rtc::gui::plot::Y("Output ZMP", [this]() { return comGenerator_->OutputZMPPosition().y(); }, Color::Purple), + mc_rtc::gui::plot::Y("Ideal ZMP", [this]() { return comGenerator_->IdealZMPPosition().y(); }, Color::Blue, + Style::Dashed)); + + ctl.gui()->addElement( + {"CoMTrajectoryGeneration"}, + mc_rtc::gui::NumberInput("Delay", [this]() { return delay_; }, [this](double delay) { delay_ = delay; }), + mc_rtc::gui::NumberInput("Transition time", [this]() { return transition_; }, + [this](double transition) { transition_ = transition; }), + mc_rtc::gui::Button("Left", + [this, leftY]() { + auto curr = t_ + previewTime_ + delay_; + comGenerator_->changeFutureSteps(curr, {{curr + transition_, {0, leftY}}}); + updateSteps(); + }), + mc_rtc::gui::Button("Right", + [this, rightY]() { + auto curr = t_ + previewTime_ + delay_; + comGenerator_->changeFutureSteps(curr, {{curr + transition_, {0, rightY}}}); + updateSteps(); + }), + mc_rtc::gui::Button("Center", + [this, centerY]() { + auto curr = t_ + previewTime_ + delay_; + comGenerator_->changeFutureSteps(curr, {{curr + transition_, {0, centerY}}}); + updateSteps(); + }), + mc_rtc::gui::Button("Left-Right-Left", [this, centerY, leftY, rightY]() { + auto curr = t_ + previewTime_ + delay_; + std::vector> futureSteps; + auto t = curr + transition_; + futureSteps.push_back({t, {0, leftY}}); + t += 2; + futureSteps.push_back({t, {0, leftY}}); + t += transition_; + futureSteps.push_back({t, {0, rightY}}); + t += 2; + futureSteps.push_back({t, {0, rightY}}); + t += transition_; + futureSteps.push_back({t, {0, centerY}}); + comGenerator_->changeFutureSteps(curr, futureSteps); + updateSteps(); + })); + + ctl.logger().addLogEntry("perf_CoMGenerator[ms]", [this]() { return generationTime_; }); +} + +void CoMTrajectoryGeneration_Initial::updateSteps() +{ + mc_rtc::log::info("Time start: {}", t_); + mc_rtc::log::info("Time curr : {}", t_ + previewTime_); + mc_rtc::log::info("Desired steps:\nTime\tCoM X\tCoM Y\n{}", + mc_rtc::io::to_string(comGenerator_->steps().steps(), "\n")); +} + +bool CoMTrajectoryGeneration_Initial::run(mc_control::fsm::Controller & ctl) +{ + /* Generate trajectory starting at iter_ time (start of the past preview window) + * The trajectory result is computed for the current time (iter_+previewSize_) + */ + auto generateTrajectory = [this]() { comGenerator_->generate(iter_); }; + generationTime_ = mc_rtc::measure_ms::execution(generateTrajectory).count(); + + // Provide targets to the stabilizer (running in another state) + if(ctl.datastore().has("StabilizerStandingTrackCoM::target")) + { + ctl.datastore().call("StabilizerStandingTrackCoM::target", comGenerator_->OutputCOGPosition(), + comGenerator_->OutputCOGVelocity(), comGenerator_->OutputCOGAcceleration(), + comGenerator_->OutputZMPPosition()); + } + + iter_++; + t_ += ctl.timeStep; + output("OK"); + return false; +} + +void CoMTrajectoryGeneration_Initial::teardown(mc_control::fsm::Controller & ctl) +{ + comGenerator_->removeFromLogger(ctl.logger()); +} + +} // namespace mc_samples + +EXPORT_SINGLE_STATE("CoMTrajectoryGeneration_Initial", mc_samples::CoMTrajectoryGeneration_Initial) diff --git a/src/mc_control/samples/CoMTrajectoryGeneration/src/states/CoMTrajectoryGeneration_Initial.h b/src/mc_control/samples/CoMTrajectoryGeneration/src/states/CoMTrajectoryGeneration_Initial.h new file mode 100644 index 0000000000..93f2e1285b --- /dev/null +++ b/src/mc_control/samples/CoMTrajectoryGeneration/src/states/CoMTrajectoryGeneration_Initial.h @@ -0,0 +1,45 @@ +#pragma once + +#include +#include + +namespace mc_samples +{ + +struct CoMTrajectoryGeneration_Initial : mc_control::fsm::State +{ + + void configure(const mc_rtc::Configuration & config) override; + + void start(mc_control::fsm::Controller & ctl) override; + + bool run(mc_control::fsm::Controller & ctl) override; + + void teardown(mc_control::fsm::Controller & ctl) override; + +private: + void updateSteps(); + +private: + mc_planning::CenteredPreviewWindow preview_{0, 0}; + Eigen::Vector3d polesX_ = {1.0, 1.0, 150.0}; + Eigen::Vector3d polesY_ = {1.0, 1.0, 150.0}; + double previewTime_ = 1.6; + + unsigned iter_ = 0; ///< Number of iterations elapsed since started + double t_ = 0; ///< Time elapsed since started + double generationTime_ = 0; ///< Time spent generating the trajectory + + double delay_ = 0.5; ///< Delay before moving manually + double transition_ = 0.2; ///< Transition time between steps (interpolation) + + Eigen::Vector3d leftFootPos_ = Eigen::Vector3d::Zero(); + Eigen::Vector3d rightFootPos_ = Eigen::Vector3d::Zero(); + Eigen::Vector3d feetCenterPos_ = Eigen::Vector3d::Zero(); + +private: + std::shared_ptr comGenerator_; + mc_planning::PreviewSteps steps_; ///< Foot steps defined as time, ZMP_x, ZMP_y +}; + +} // namespace mc_samples diff --git a/samples/CoMTrajectoryGeneration/src/states/StabilizerStandingTrackCoM.cpp b/src/mc_control/samples/CoMTrajectoryGeneration/src/states/StabilizerStandingTrackCoM.cpp similarity index 91% rename from samples/CoMTrajectoryGeneration/src/states/StabilizerStandingTrackCoM.cpp rename to src/mc_control/samples/CoMTrajectoryGeneration/src/states/StabilizerStandingTrackCoM.cpp index 9384b2f417..891a14f1c3 100644 --- a/samples/CoMTrajectoryGeneration/src/states/StabilizerStandingTrackCoM.cpp +++ b/src/mc_control/samples/CoMTrajectoryGeneration/src/states/StabilizerStandingTrackCoM.cpp @@ -1,6 +1,6 @@ #include "StabilizerStandingTrackCoM.h" -#include "../CoMTrajectoryGeneration.h" #include +#include "../CoMTrajectoryGeneration.h" #include #include @@ -29,13 +29,11 @@ void StabilizerStandingTrackCoM::start(mc_control::fsm::Controller & ctl) stabilizerTask_->staticTarget(ctl.realRobot().com()); ctl.datastore().make_call("StabilizerStandingTrackCoM::target", - [this](const Eigen::Vector3d & com, const Eigen::Vector3d & comd, const Eigen::Vector3d & comdd, const Eigen::Vector3d & zmp) - { - stabilizerTask_->target(com, comd, comdd, zmp); - }); + [this](const Eigen::Vector3d & com, const Eigen::Vector3d & comd, + const Eigen::Vector3d & comdd, + const Eigen::Vector3d & zmp) { stabilizerTask_->target(com, comd, comdd, zmp); }); } - bool StabilizerStandingTrackCoM::run(mc_control::fsm::Controller & ctl) { // Update anchor frame for the KinematicInertial observer @@ -69,6 +67,6 @@ void StabilizerStandingTrackCoM::teardown(mc_control::fsm::Controller & ctl) }); } -} /* mc_samples */ +} // namespace mc_samples EXPORT_SINGLE_STATE("StabilizerStandingTrackCoM", mc_samples::StabilizerStandingTrackCoM) diff --git a/samples/CoMTrajectoryGeneration/src/states/StabilizerStandingTrackCoM.h b/src/mc_control/samples/CoMTrajectoryGeneration/src/states/StabilizerStandingTrackCoM.h similarity index 58% rename from samples/CoMTrajectoryGeneration/src/states/StabilizerStandingTrackCoM.h rename to src/mc_control/samples/CoMTrajectoryGeneration/src/states/StabilizerStandingTrackCoM.h index d6aee40e15..15838912be 100644 --- a/samples/CoMTrajectoryGeneration/src/states/StabilizerStandingTrackCoM.h +++ b/src/mc_control/samples/CoMTrajectoryGeneration/src/states/StabilizerStandingTrackCoM.h @@ -16,17 +16,17 @@ namespace mc_samples struct StabilizerStandingTrackCoM : mc_control::fsm::State { - void configure(const mc_rtc::Configuration & config) override; + void configure(const mc_rtc::Configuration & config) override; - void start(mc_control::fsm::Controller & ctl) override; + void start(mc_control::fsm::Controller & ctl) override; - bool run(mc_control::fsm::Controller & ctl) override; + bool run(mc_control::fsm::Controller & ctl) override; - void teardown(mc_control::fsm::Controller & ctl) override; + void teardown(mc_control::fsm::Controller & ctl) override; private: std::shared_ptr stabilizerTask_ = nullptr; mc_rtc::Configuration config_; }; -} /* mc_samples */ +} // namespace mc_samples diff --git a/samples/CoMTrajectoryGeneration/src/states/data/states.json b/src/mc_control/samples/CoMTrajectoryGeneration/src/states/data/states.json similarity index 100% rename from samples/CoMTrajectoryGeneration/src/states/data/states.json rename to src/mc_control/samples/CoMTrajectoryGeneration/src/states/data/states.json diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt index d1d6485237..139753f16e 100644 --- a/tests/CMakeLists.txt +++ b/tests/CMakeLists.txt @@ -141,5 +141,3 @@ mc_rtc_test(test_interpolation mc_control) # -- Temporary HMC-related tests -- # ##################################### mc_rtc_test(test_hmc mc_planning) - -add_subdirectory(hmc) diff --git a/tests/hmc/CMakeLists.txt b/tests/hmc/CMakeLists.txt deleted file mode 100644 index 02e571ec17..0000000000 --- a/tests/hmc/CMakeLists.txt +++ /dev/null @@ -1,2 +0,0 @@ -add_executable(generator testCoMGenerator.cpp) -target_link_libraries(generator PUBLIC mc_planning) From 268cdc829417c80900cb97dcb64543bca6abcb75 Mon Sep 17 00:00:00 2001 From: Arnaud TANGUY Date: Fri, 26 Jun 2020 22:17:58 +0900 Subject: [PATCH 35/43] [WIP/PreviewWindow] Implement views, strong typing for Time/Index --- include/mc_planning/PreviewWindow.h | 310 ++++++++++-------- include/mc_planning/generator.h | 1 - .../mc_sample_CoMGeneration.in.cpp | 2 +- src/mc_planning/PreviewWindow.cpp | 34 +- src/mc_planning/generator.cpp | 22 +- tests/test_hmc.cpp | 84 ++--- 6 files changed, 237 insertions(+), 216 deletions(-) diff --git a/include/mc_planning/PreviewWindow.h b/include/mc_planning/PreviewWindow.h index 13d1e63c08..a44dec8e40 100644 --- a/include/mc_planning/PreviewWindow.h +++ b/include/mc_planning/PreviewWindow.h @@ -11,6 +11,60 @@ namespace mc_planning { struct CenteredPreviewWindow; +struct PreviewWindowView; + +namespace internal +{ + +/** + * @brief Provides a stronger type to disallow implicit conversions + * + * See https://foonathan.net/2016/10/strong-typedefs/ + */ +template +class strong_typedef +{ +public: + strong_typedef() : value_() {} + + explicit strong_typedef(const T & value) : value_(value) {} + + explicit strong_typedef(T && value) noexcept(std::is_nothrow_move_constructible::value) : value_(std::move(value)) + { + } + + // Allow implicit conversion to T + operator T &() noexcept + { + return value_; + } + + // Allow implicit conversion to T const + operator const T &() const noexcept + { + return value_; + } + + friend void swap(strong_typedef & a, strong_typedef & b) noexcept + { + using std::swap; + swap(static_cast(a), static_cast(b)); + } + +private: + T value_; +}; + +} // namespace internal + +struct Time : internal::strong_typedef +{ + using strong_typedef::strong_typedef; // make constructors available +}; +struct Index : internal::strong_typedef +{ + using strong_typedef::strong_typedef; // make constructors available +}; /** * @brief Element returned by CenteredPreviewWindow::iterator providing the current @@ -24,44 +78,34 @@ struct MC_PLANNING_DLLAPI PreviewElement * @param window Preview window to which this element refers to * @param index Global index of the element */ - explicit PreviewElement(const CenteredPreviewWindow & window, unsigned index) noexcept - : window_(window), index_(index) - { - } + explicit PreviewElement(const PreviewWindowView & window, Index index) noexcept : window_(window), index_(index) {} + + explicit PreviewElement(const PreviewWindowView & window, unsigned index) noexcept : window_(window), index_(index) {} /** * @brief Returns the global index starting at `window.startIndex()` */ - unsigned index() const noexcept; + Index index() const noexcept; /** * @brief Returns the time corresponding to index() */ - double time() const noexcept; + Time time() const noexcept; /** * @brief Returns the index local to the preview window * * @return Index between \f$ [0, 2*N] \f$ where `N` is the preview size */ - unsigned localIndex() const noexcept; + Index localIndex() const noexcept; /** * @brief Returns the time corresponding to localIndex() */ - double localTime() const noexcept; - - /** - * @brief Reference to the preview window from which this element was - * generated - */ - const CenteredPreviewWindow & window() const noexcept - { - return window_; - } + Time localTime() const noexcept; protected: - const CenteredPreviewWindow & window_; ///< Window being iterated over - unsigned index_; ///< Global index + const PreviewWindowView & window_; ///< Window being iterated over + Index index_{0u}; ///< Global index }; /** @@ -112,9 +156,6 @@ struct MC_PLANNING_DLLAPI PreviewElement */ struct MC_PLANNING_DLLAPI CenteredPreviewWindow { - using iterator = RangeElementIterator; - using const_iterator = RangeElementIterator; - /** * @brief Constructs a window of duration `2*halfTime` with the current * element at the center. @@ -123,153 +164,113 @@ struct MC_PLANNING_DLLAPI CenteredPreviewWindow * @param dt Timesep * @param startTime Time at which the window starts */ - explicit CenteredPreviewWindow(double halfTime, double dt, const double startTime = 0.) noexcept + explicit CenteredPreviewWindow(Time halfTime, Time dt) noexcept : halfSize_(static_cast(std::lround(halfTime / dt))), fullSize_(2 * halfSize_ + 1), dt_(dt) { - startAtTime(startTime); } - /** - * @name Range iterators for computing index/time along the preview window - * @{ - */ - iterator begin() noexcept - { - return iterator{*this, startIndex()}; - } - iterator end() noexcept + explicit CenteredPreviewWindow(double halfTime, double dt) noexcept : CenteredPreviewWindow(Time{halfTime}, Time{dt}) { - return iterator{*this, endIndex() + 1}; - } - - const_iterator cbegin() noexcept - { - return const_iterator{*this, startIndex()}; - } - const_iterator cend() noexcept - { - return const_iterator{*this, endIndex() + 1}; } /** - * @brief Iterate over a copy of the preview window - * - * The preview window constains only the window parameters and is cheap to - * copy. - * - * @param startTime Time from which to iterate - * - * @return A copy of the preview window starting at time startTime + * @brief Timestep */ - CenteredPreviewWindow iterateFromTime(double startTime) const noexcept + Time dt() const noexcept { - CenteredPreviewWindow w = *this; - w.startAtTime(startTime); - return w; + return dt_; } /** - * @brief Iterate over a copy of the preview window - * - * The preview window constains only the window parameters and is cheap to - * copy. - * - * @param startIndex Index from which to iterate + * @brief Number of future or past elements. * - * @return A copy of the preview window starting at index startIndex + * The preview window is of size `2*halfSize_+1` */ - CenteredPreviewWindow iterateFromIndex(unsigned startIndex) const noexcept + Index halfSize() const noexcept { - CenteredPreviewWindow w = *this; - w.startAtIndex(startIndex); - return w; + return halfSize_; } /** - * @brief Generate a copy of the current preview window + * @brief Duration corresponding to halfSize() */ - CenteredPreviewWindow copy() const noexcept + Time halfDuration() const noexcept { - return *this; + return Time{halfSize_ * dt_}; } - // @} /** - * @brief Start the preview window at the provided time - * - * @param t Time at which the window starts + * @brief Full size of the preview window */ - void startAtTime(double t) noexcept + Index size() const noexcept { - start_ = indexFromTime(t); + return fullSize_; } /** - * @brief Start the preview window at the provided index + * @brief Full duration of the preview window */ - void startAtIndex(unsigned index) noexcept + Time duration() const noexcept { - start_ = index; + return Time{(size() - 1) * dt_}; } /** - * @brief Time at the start of the preview window + * @brief Converts between time and index + * + * @param t Time + * + * @return Index corresponding to the provided time */ - inline double startTime() const noexcept + Index index(Time t) const noexcept { - return start_ * dt_; + return Index{static_cast(std::lround(t / dt_))}; } /** - * @brief Index of the start of the preview window + * @brief Converts between index and time + * + * @param index Index + * + * @return Time corresponding to the provided index */ - unsigned startIndex() const noexcept + Time time(Index index) const noexcept { - return start_; + return Time{index * dt_}; } - /** - * @brief Time at the center of the preview window - */ - double currentTime() const noexcept + Index center() { - return (start_ + halfSize_) * dt_; + return halfSize_; } - /** - * @brief Index of the center of the preview window - */ - unsigned currentIndex() const noexcept - { - return start_ + halfSize_; - } + PreviewWindowView all(Index index) const noexcept; + PreviewWindowView all(Time startTime) const noexcept; + // PreviewWindowView past(Index startIndex) const noexcept; + // PreviewWindowView future(Index startIndex) const noexcept; - /** - * @brief Index of the end of the preview window - */ - double endTime() const noexcept - { - return endIndex() * dt_; - } +protected: + Index halfSize_{0u}; ///< Number of future or past element + Index fullSize_{0u}; ///< 2*halfSize_+1 + Time dt_{0.005}; ///< Timestep +}; - /** - * @brief Index of the end of the preview window - */ - unsigned endIndex() const noexcept +struct PreviewWindowView +{ + using iterator = RangeElementIterator; + using const_iterator = RangeElementIterator; + + explicit PreviewWindowView(const CenteredPreviewWindow & window, + Index absoluteStart, + Index localStart, + Index localEnd) + : window_(window), absoluteStart_(absoluteStart), localStart_(localStart), localEnd_(localEnd) { - return start_ + fullSize_ - 1; } - /** - * @brief Converts between time and index - * - * @param t Time - * - * @return Index corresponding to the provided time - */ - unsigned indexFromTime(double t) const noexcept + Index index(Time t) const noexcept { - return static_cast(std::lround(t / dt_)); + return window_.index(t); } /** @@ -279,58 +280,87 @@ struct MC_PLANNING_DLLAPI CenteredPreviewWindow * * @return Time corresponding to the provided index */ - double timeFromIndex(unsigned index) const noexcept + Time time(Index index) const noexcept { - return index * dt_; + return window_.time(index); } /** - * @brief Timestep + * @name Range iterators for computing index/time along the preview window + * @{ */ - double dt() const noexcept + iterator begin() noexcept { - return dt_; + return iterator{*this, startIndex()}; + } + iterator end() noexcept + { + return iterator{*this, endIndex() + 1}; } + const_iterator cbegin() noexcept + { + return const_iterator{*this, startIndex()}; + } + const_iterator cend() noexcept + { + return const_iterator{*this, endIndex() + 1}; + } + // @} + /** - * @brief Number of future or past elements. - * - * The preview window is of size `2*halfSize_+1` + * @brief Time at the start of the preview window */ - unsigned halfSize() const noexcept + inline Time startTime() const noexcept { - return halfSize_; + return Time{absoluteStart_ * window_.dt()}; } /** - * @brief Duration corresponding to halfSize() + * @brief Index of the start of the preview window */ - double halfDuration() const noexcept + Index startIndex() const noexcept { - return halfSize_ * dt_; + return absoluteStart_; } + // /** + // * @brief Time at the center of the preview window + // */ + // Time currentTime() const noexcept + // { + // return (start_ + window_.halfSize()) * window_.dt(); + // } + + // /** + // * @brief Index of the center of the preview window + // */ + // Index currentIndex() const noexcept + // { + // return start_ + window_.halfSize(); + // } + /** - * @brief Full size of the preview window + * @brief Index of the end of the preview window */ - unsigned size() const noexcept + Time endTime() const noexcept { - return fullSize_; + return Time{endIndex() * window_.dt()}; } /** - * @brief Full duration of the preview window + * @brief Index of the end of the preview window */ - double duration() const noexcept + Index endIndex() const noexcept { - return (size() - 1) * dt_; + return Index{absoluteStart_ + window_.size() - 1}; } protected: - unsigned halfSize_ = 0; ///< Number of future or past element - unsigned fullSize_ = 0; ///< 2*halfSize_+1 - unsigned start_ = 0; ///< Index at which the window starts - double dt_ = 0.005; ///< Timestep + const CenteredPreviewWindow & window_; ///< Preview window parameters + Index absoluteStart_{0}; ///< Index at which the view starts + Index localStart_{0}; ///< Index within the preview window where it starts + Index localEnd_{0}; ///< Index past end of the view }; } // namespace mc_planning diff --git a/include/mc_planning/generator.h b/include/mc_planning/generator.h index 1fd303a664..0c4ca4705f 100644 --- a/include/mc_planning/generator.h +++ b/include/mc_planning/generator.h @@ -287,7 +287,6 @@ struct MC_PLANNING_DLLAPI generator private: CenteredPreviewWindow preview_; ///< Iteratable preview window and parameters - CenteredPreviewWindow previewZero_; ///< Iteratable preview window and parameters std::shared_ptr> m_ComInterp = nullptr; StatePVA m_COG_ideal_pre; diff --git a/samples/CoMGenerator/mc_sample_CoMGeneration.in.cpp b/samples/CoMGenerator/mc_sample_CoMGeneration.in.cpp index 4daacc793f..e81fc2af57 100644 --- a/samples/CoMGenerator/mc_sample_CoMGeneration.in.cpp +++ b/samples/CoMGenerator/mc_sample_CoMGeneration.in.cpp @@ -39,7 +39,7 @@ int main(int, char **) logger.start("CoMGenerator", dt); com_traj.addToLogger(logger); - unsigned n_loop = preview.indexFromTime(com_traj.steps().back().t()) - preview.halfSize(); + unsigned n_loop = preview.index(Time(com_traj.steps().back().t())) - preview.halfSize(); /* * Generate the CoM trajectory based on the parameters in com_traj diff --git a/src/mc_planning/PreviewWindow.cpp b/src/mc_planning/PreviewWindow.cpp index bcfede5b50..80f032a132 100644 --- a/src/mc_planning/PreviewWindow.cpp +++ b/src/mc_planning/PreviewWindow.cpp @@ -7,24 +7,44 @@ namespace mc_planning { -unsigned PreviewElement::index() const noexcept +PreviewWindowView CenteredPreviewWindow::all(Index startIndex) const noexcept +{ + return PreviewWindowView{*this, startIndex, Index{0}, fullSize_}; +} + +PreviewWindowView CenteredPreviewWindow::all(Time startTime) const noexcept +{ + return PreviewWindowView{*this, index(startTime), Index{0}, fullSize_}; +} + +// PreviewWindowView CenteredPreviewWindow::past(Index startIndex) const noexcept +// { +// return PreviewWindowView{*this, startIndex, halfSize_}; +// } + +// PreviewWindowView CenteredPreviewWindow::future(Index startIndex) const noexcept +// { +// return PreviewWindowView{*this, startIndex+halfSize_, startIndex+fullSize_}; +// } + +Index PreviewElement::index() const noexcept { return index_; } -double PreviewElement::time() const noexcept +Time PreviewElement::time() const noexcept { - return window_.timeFromIndex(index_); + return window_.time(Index(index_)); } -unsigned PreviewElement::localIndex() const noexcept +Index PreviewElement::localIndex() const noexcept { - return index_ - window_.startIndex(); + return Index{index_ - window_.startIndex()}; } -double PreviewElement::localTime() const noexcept +Time PreviewElement::localTime() const noexcept { - return window_.timeFromIndex(index_ - window_.startIndex()); + return window_.time(Index(index_ - window_.startIndex())); } } // namespace mc_planning diff --git a/src/mc_planning/generator.cpp b/src/mc_planning/generator.cpp index 2157524d80..33a5e7a4a5 100644 --- a/src/mc_planning/generator.cpp +++ b/src/mc_planning/generator.cpp @@ -13,8 +13,7 @@ namespace mc_planning { generator::generator(const CenteredPreviewWindow & preview, double mass, double waist_height) -: preview_(preview), previewZero_(preview), - m_ComInterp(std::make_shared>(1.0, preview_.dt() / 2.)), m_mass(mass), +: preview_(preview), m_ComInterp(std::make_shared>(1.0, preview_.dt() / 2.)), m_mass(mass), m_waist_height(waist_height) { mc_rtc::log::info("Creating generator with mass={}, waist_height={}", mass, waist_height); @@ -44,11 +43,11 @@ void generator::setupCOGHeight(unsigned n_current) if(n_current == 0) { m_ComInterp->push_back(0u, m_waist_height); - m_ComInterp->push_back(preview_.indexFromTime(m_steps.back().t()), m_waist_height); + m_ComInterp->push_back(preview_.index(Time(m_steps.back().t())), m_waist_height); m_ComInterp->update(); } - for(const auto & step : preview_.iterateFromIndex(0)) + for(const auto & step : preview_.all(Index(0))) { // clang-format off auto n = step.index(); @@ -57,7 +56,7 @@ void generator::setupCOGHeight(unsigned n_current) } // Interpolated value at current time - unsigned current = previewZero_.currentIndex(); + unsigned current = preview_.center(); m_COG_ideal.P(Z) = m_cog_height[current]; m_COG_ideal.V(Z) = m_cog_dot_height[current]; m_COG_ideal.Vdot(Z) = m_cog_ddot_height[current]; @@ -65,15 +64,15 @@ void generator::setupCOGHeight(unsigned n_current) void generator::setupTimeTrajectories(unsigned n_current) { - m_steps.nextWindow(preview_.startTime()); + auto currentPreview = preview_.all(Index(n_current)); + m_steps.nextWindow(currentPreview.startTime()); Eigen::VectorXd & px_ref = m_ipm_long[X].p_ref(); Eigen::VectorXd & py_ref = m_ipm_long[Y].p_ref(); - // for(unsigned i = 0; i < preview_.size(); i++) - for(const auto & current : preview_) + for(const auto & current : currentPreview) { unsigned i = current.localIndex(); - const double currTime = current.time(); //preview_.timeFromIndex(i + n_current); + const double currTime = current.time(); m_virtual_height[X](i) = 0.0; m_virtual_height[Y](i) = 0.0; @@ -108,7 +107,7 @@ void generator::setupTimeTrajectories(unsigned n_current) (mc_rtc::constants::GRAVITY + m_cog_ddot_height[i]) / (m_cog_height[i] - m_virtual_height[Y](i)); } - unsigned current = previewZero_.currentIndex(); + unsigned current = preview_.center(); m_Pcalpha_ideal(Z) = (m_virtual_height[X](current) + m_virtual_height[Y](current)) / 2.0; } @@ -140,7 +139,7 @@ void generator::generateTrajectories() * - m_Pcalpha_cmp: Modifiecation of the ideal ZMP state (position and velocity) **/ auto computeShortTerm = [&](unsigned axis) { - unsigned current = previewZero_.currentIndex(); + unsigned current = preview_.center(); double omega2 = m_ipm_long[axis].w2(current); double omega = sqrt(omega2); m_ipm_short[axis].setSystemMatrices(omega * m_poles[axis](0), omega * m_poles[axis](1), m_poles[axis](2), omega2, @@ -188,7 +187,6 @@ void generator::generate(unsigned n_time) { mc_rtc::log::error_and_throw("[generator] Invalid reference provided, call steps()"); } - preview_.startAtIndex(n_time); setupCOGHeight(n_time); setupTimeTrajectories(n_time); generateTrajectories(); diff --git a/tests/test_hmc.cpp b/tests/test_hmc.cpp index cf76e22abb..55881e69f9 100644 --- a/tests/test_hmc.cpp +++ b/tests/test_hmc.cpp @@ -194,23 +194,24 @@ BOOST_AUTO_TEST_CASE(TestPreviewWindow) { CenteredPreviewWindow window(0.015, 0.005); + auto view = window.all(Index(0)); // View on the preview window starting at time 0 BOOST_REQUIRE_EQUAL(window.duration(), 0.03); BOOST_REQUIRE_EQUAL(window.halfDuration(), 0.015); BOOST_REQUIRE_EQUAL(window.halfSize(), 3); // n BOOST_REQUIRE_EQUAL(window.size(), 7); // 2*n+1 - BOOST_REQUIRE_EQUAL(window.startIndex(), 0); - BOOST_REQUIRE_EQUAL(window.startTime(), 0); - BOOST_REQUIRE_EQUAL(window.endIndex(), 6); // Array of size 7, last index is 6 - BOOST_REQUIRE_EQUAL(window.endTime(), 0.03); - auto end = *window.end(); + BOOST_REQUIRE_EQUAL(view.startIndex(), 0); + BOOST_REQUIRE_EQUAL(view.startTime(), 0); + BOOST_REQUIRE_EQUAL(view.endIndex(), 6); // Array of size 7, last index is 6 + BOOST_REQUIRE_EQUAL(view.endTime(), 0.03); + auto end = *view.end(); BOOST_REQUIRE_EQUAL(end.localIndex(), 7); // Iterator end is one element after the end BOOST_REQUIRE_EQUAL(end.localTime(), 0.035); // Iterator end is one element after the end BOOST_REQUIRE_EQUAL(end.index(), 7); // Iterator end is one element after the end BOOST_REQUIRE_EQUAL(end.time(), 0.035); // Iterator end is one element after the end - auto start = *window.begin(); + auto start = *view.begin(); BOOST_REQUIRE_EQUAL(start.localIndex(), 0); BOOST_REQUIRE_EQUAL(start.localTime(), 0.0); - for(const auto w : window) + for(const auto w : view) { mc_rtc::log::info("index {}, local time {:.3f}, global index {}, global time {:.3f}", w.localIndex(), w.localTime(), w.index(), w.time()); @@ -218,69 +219,42 @@ BOOST_AUTO_TEST_CASE(TestPreviewWindow) BOOST_REQUIRE(w.localIndex() <= 6); BOOST_REQUIRE(w.index() <= 6); } - } - { - CenteredPreviewWindow window(0.015, 0.005); - window.startAtTime(10); - BOOST_REQUIRE_EQUAL(window.duration(), 0.03); - BOOST_REQUIRE_EQUAL(window.halfDuration(), 0.015); - BOOST_REQUIRE_EQUAL(window.halfSize(), 3); // n - BOOST_REQUIRE_EQUAL(window.size(), 7); // 2*n+1 - BOOST_REQUIRE_EQUAL(window.startTime(), 10); - BOOST_REQUIRE_EQUAL(window.endTime(), 10.03); - auto end = *window.end(); - BOOST_REQUIRE_EQUAL(end.localIndex(), 7); // Iterator end is one element after the end - BOOST_REQUIRE_EQUAL(end.localTime(), 0.035); // Iterator end is one element after the end - auto start = *window.begin(); - BOOST_REQUIRE_EQUAL(start.localIndex(), 0); - BOOST_REQUIRE_EQUAL(start.localTime(), 0.0); - for(const auto w : window) - { - mc_rtc::log::info("local index {}, local time {:.3f}, global index {}, global time {:.3f}", w.localIndex(), - w.localTime(), w.index(), w.time()); + { // Test a view starting at time 10 + auto view10 = window.all(Time(10)); + BOOST_REQUIRE_EQUAL(view10.startTime(), 10); + BOOST_REQUIRE_EQUAL(view10.endTime(), 10.03); + auto end = *view10.end(); + BOOST_REQUIRE_EQUAL(end.localIndex(), 7); // Iterator end is one element after the end + BOOST_REQUIRE_EQUAL(end.localTime(), 0.035); // Iterator end is one element after the end + auto start = *view10.begin(); + BOOST_REQUIRE_EQUAL(start.localIndex(), 0); + BOOST_REQUIRE_EQUAL(start.localTime(), 0.0); + for(const auto w : view10) + { + mc_rtc::log::info("local index {}, local time {:.3f}, global index {}, global time {:.3f}", w.localIndex(), + w.localTime(), w.index(), w.time()); - BOOST_REQUIRE(w.localIndex() <= 6); + BOOST_REQUIRE(w.localIndex() <= 6); + } } } - { - CenteredPreviewWindow window(1.6, 0.005); - BOOST_REQUIRE_EQUAL(window.startTime(), 0.); - BOOST_REQUIRE_EQUAL(window.endTime(), 3.2); - BOOST_REQUIRE_EQUAL(window.duration(), 3.2); - BOOST_REQUIRE_EQUAL(window.halfDuration(), 1.6); - BOOST_REQUIRE_EQUAL(window.halfSize(), (unsigned)std::lround(1.6 / 0.005)); - BOOST_REQUIRE_EQUAL(window.size(), (unsigned)std::lround(3.2 / 0.005) + 1); - BOOST_REQUIRE_EQUAL(window.indexFromTime(0), 0); - BOOST_REQUIRE_EQUAL(window.timeFromIndex(0), 0); - BOOST_REQUIRE_EQUAL(window.timeFromIndex(1), 0.005); - BOOST_REQUIRE_EQUAL(window.indexFromTime(0.005), 1); - BOOST_REQUIRE_EQUAL(window.timeFromIndex(10), 0.05); - } - - { - CenteredPreviewWindow window(1.6, 0.005, 3.2); - CenteredPreviewWindow window2(1.6, 0.005); - window2.startAtTime(3.2); - BOOST_REQUIRE_EQUAL(window.startTime(), window2.startTime()); - } - auto test = [&](double preview_time, double dt, double start) { CenteredPreviewWindow window(preview_time, dt); - window.startAtTime(start); - unsigned i = window.indexFromTime(start); + unsigned i = window.index(Time(start)); BOOST_REQUIRE_EQUAL(i, std::lround(start / dt)); - for(const auto & w : window) + for(const auto & w : window.all(Time(start))) { // mc_rtc::log::info("Window element with index: {}, time: {:.3f}", w.localIndex(), w.localTime()); BOOST_REQUIRE_EQUAL(w.index(), i); BOOST_REQUIRE_EQUAL(w.time(), i * dt); - BOOST_REQUIRE_EQUAL(window.indexFromTime(window.timeFromIndex(w.index())), w.index()); - BOOST_REQUIRE_EQUAL(window.timeFromIndex(window.indexFromTime(w.time())), w.time()); + BOOST_REQUIRE_EQUAL(window.index(window.time(w.index())), w.index()); + BOOST_REQUIRE_EQUAL(window.time(window.index(w.time())), w.time()); ++i; } }; + test(1.6, 0.005, 0); test(1.6, 0.005, 1.5); test(1.6, 0.002, 0); From 2b29fc964ac206bd53d822c3bca9e524da200c74 Mon Sep 17 00:00:00 2001 From: Arnaud TANGUY Date: Wed, 1 Jul 2020 11:23:13 +0900 Subject: [PATCH 36/43] [cmake] Option to force colored output for Clang/GCC --- CMakeLists.txt | 11 +++++++++++ 1 file changed, 11 insertions(+) diff --git a/CMakeLists.txt b/CMakeLists.txt index c39c732ca4..a0abbce94c 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -57,6 +57,17 @@ option(MC_RTC_DISABLE_NETWORK "Build without network support" OFF) option(DISABLE_ROS "Build without ROS support (even if ROS was found)" OFF) +# By default, neither clang nor gcc produce color output if they detect the output medium +# is not a terminal (e.g Ninja). +option (FORCE_COLORED_OUTPUT "Always produce ANSI-colored output (GNU/Clang only)." OFF) +if (${FORCE_COLORED_OUTPUT}) + if ("${CMAKE_CXX_COMPILER_ID}" STREQUAL "GNU") + add_compile_options (-fdiagnostics-color=always) + elseif ("${CMAKE_CXX_COMPILER_ID}" STREQUAL "Clang") + add_compile_options (-fcolor-diagnostics) + endif() +endif() + add_project_dependency(Boost REQUIRED COMPONENTS filesystem timer) add_project_dependency(Tasks REQUIRED) add_project_dependency(mc_rbdyn_urdf REQUIRED) From 0eb3b77516c927f6e53950f4dd06a2ec86d757a2 Mon Sep 17 00:00:00 2001 From: Arnaud TANGUY Date: Wed, 1 Jul 2020 14:33:12 +0900 Subject: [PATCH 37/43] [WIP/PreviewWindow] Continue views implementation --- benchmarks/benchHMC.cpp | 2 +- include/mc_planning/PreviewWindow.h | 232 +++++++++++++++++++--------- src/mc_planning/PreviewWindow.cpp | 50 ++++-- tests/test_hmc.cpp | 66 +++++--- 4 files changed, 241 insertions(+), 109 deletions(-) diff --git a/benchmarks/benchHMC.cpp b/benchmarks/benchHMC.cpp index 3c367dbd98..84706bbf82 100644 --- a/benchmarks/benchHMC.cpp +++ b/benchmarks/benchHMC.cpp @@ -140,7 +140,7 @@ class GeneratorFixture : public benchmark::Fixture generator = std::make_shared(preview); generator->steps(steps); - n_loop = preview.indexFromTime(steps.back().t()); + n_loop = preview.index(mc_planning::Time(steps.back().t())); } void TearDown(const ::benchmark::State &) {} diff --git a/include/mc_planning/PreviewWindow.h b/include/mc_planning/PreviewWindow.h index a44dec8e40..401cc29f37 100644 --- a/include/mc_planning/PreviewWindow.h +++ b/include/mc_planning/PreviewWindow.h @@ -45,23 +45,53 @@ class strong_typedef return value_; } + T operator()() const noexcept + { + return value_; + } + friend void swap(strong_typedef & a, strong_typedef & b) noexcept { using std::swap; swap(static_cast(a), static_cast(b)); } -private: +protected: T value_; }; +template +struct strong_typedef_operators +{ + friend T operator+(const T & lhs, const T & rhs) + { + return T{lhs() + rhs()}; + } + friend T operator-(const T & lhs, const T & rhs) + { + return T{lhs() - rhs()}; + } + friend T operator+(const T & lhs, const underlying_t & rhs) + { + return T{lhs() + rhs}; + } + friend T operator-(const T & lhs, const underlying_t & rhs) + { + return T{lhs() - rhs}; + } +}; + } // namespace internal -struct Time : internal::strong_typedef +struct Time : internal::strong_typedef, internal::strong_typedef_operators { using strong_typedef::strong_typedef; // make constructors available + Time operator+(const Time & t) + { + return Time{value_ + t}; + } }; -struct Index : internal::strong_typedef +struct Index : internal::strong_typedef, internal::strong_typedef_operators { using strong_typedef::strong_typedef; // make constructors available }; @@ -109,48 +139,18 @@ struct MC_PLANNING_DLLAPI PreviewElement }; /** - * @brief Iterable range over a preview window with convenience time-management. + * @brief Stores parameters related to a preview window * - * The preview window is centered around the current element \f$ [-N, current, N] \f$ where `N` is the number of + * This preview window is centered around the current element \f$ [-N, current, N] \f$ where `N` is the number of * elements before and after the current value. The full preview window is of size `2*N+1`. The timestep is used to * compute correspondances between discrete index of values stored in an (external) container and time. * - * Additionally, this class provides iterable ranges over the preview window. - * For example: + * The @ref range_views functions can be used to obtain PreviewWindowView iterable ranges over past/future/all preview + * elements for a window starting at any time. * - * \code{.cpp} - * // Create an iterable preview window range with a window of time `2*1.6`, - * // timestep of 0.005 and starting at time `t=0.5` - * CenteredPreviewWindow window(1.6, 0.005, 0.5); - * mc_rtc::log::info("Start time: {:.3f}, index: {}\n" - * "End time: {:.3f}, index: {}\n" - * "Current time: {:.3f}, index: {}", - * window.startTime(), window.startIndex(), - * window.endTime(), window.endIndex(), - * window.currentTime(), window.currentIndex()); + * \see PreviewWindowView iterable ranges over a floating preview window * - * // Iterate over the preview window using a RangeElementIterator - * for(const auto & w : window) - * { - * mc_rtc::log::info("Preview element with index: {}, time: {:.3f}", w.index(), w.time()); - * // Example: Access some external container using the index - * // auto value = container[w.index()]; - * } - * - * // Iterate over a copy of the preview window (cheap copy) starting at a specific time using a RangeElementIterator - * for(const auto & w : window.iterateFromTime(1.6)) - * { - * mc_rtc::log::info( - * "Preview element with global index: {}, time: {:.3f}\n" - * " local index: {}, time: {:.3f}\n" - * , w.index(), w.time() - * , w.localIndex(), w.localTime() - * ); - * // Global time starts at 1.6s, while local time is between [0, window.duration()] - * } - * \endcode - * - * \see For practical use, see: + * For practical use, see: * \see mc_planning::generator * \see mc_planning::linear_control_system::LinearTimeVariantInvertedPendulum */ @@ -212,7 +212,7 @@ struct MC_PLANNING_DLLAPI CenteredPreviewWindow */ Time duration() const noexcept { - return Time{(size() - 1) * dt_}; + return Time{(size() - 1u) * dt_}; } /** @@ -239,15 +239,42 @@ struct MC_PLANNING_DLLAPI CenteredPreviewWindow return Time{index * dt_}; } - Index center() + Index center() const noexcept { return halfSize_; } + /** + * @defgoup range_views + * Returns predefined views to the past/future of the preview window starting at the specified time/index + * - all: all elements in the preview window + * - past: all past elements, present excluded + * - pastInclusive: all past elements, present included + * - future: all future elements, present excluded + * - futureInclusive: all future elements, present included + * @{ + */ + /** All preview elements with a preview window starting at provided index */ PreviewWindowView all(Index index) const noexcept; + /** All preview elements with a preview window starting at provided time */ PreviewWindowView all(Time startTime) const noexcept; - // PreviewWindowView past(Index startIndex) const noexcept; - // PreviewWindowView future(Index startIndex) const noexcept; + /** Past preview elements (excluding present) with a preview window starting at provided index */ + PreviewWindowView past(Index index) const noexcept; + /** Past preview elements (excluding present) with a preview window starting at provided time */ + PreviewWindowView past(Time startTime) const noexcept; + /** Past preview elements (including present) with a preview window starting at provided index */ + PreviewWindowView pastInclusive(Index index) const noexcept; + /** Past preview elements (including present) with a preview window starting at provided index */ + PreviewWindowView pastInclusive(Time startTime) const noexcept; + /** Future preview elements (excluding present) with a preview window starting at provided index */ + PreviewWindowView future(Index index) const noexcept; + /** Future preview elements (excluding present) with a preview window starting at provided time */ + PreviewWindowView future(Time startTime) const noexcept; + /** Future preview elements (including present) with a preview window starting at provided index */ + PreviewWindowView futureInclusive(Index index) const noexcept; + /** Future preview elements (including present) with a preview window starting at provided time */ + PreviewWindowView futureInclusive(Time startTime) const noexcept; + /// @} protected: Index halfSize_{0u}; ///< Number of future or past element @@ -255,6 +282,45 @@ struct MC_PLANNING_DLLAPI CenteredPreviewWindow Time dt_{0.005}; ///< Timestep }; +/** + * Provides an iterable view on a preview window for convenient safe access to + * ranges of elements + * + * For example: + * \code{.cpp} + * // Create an iterable preview window range with a window of time `2*1.6`, + * // timestep of 0.005 + * CenteredPreviewWindow window(1.6, 0.005); + * mc_rtc::log::info("Start time: {:.3f}, index: {}\n" + * "End time: {:.3f}, index: {}\n" + * "Current time: {:.3f}, index: {}", + * window.startTime(), window.startIndex(), + * window.endTime(), window.endIndex(), + * window.currentTime(), window.currentIndex()); + * + * // Iterate over a view to the preview window starting at time t=0.5 and + * // containing all past elements in the preview window + * for(const auto & w : window.past(Time{0.5})) + * { + * mc_rtc::log::info("Preview element with index: {}, time: {:.3f}", w.index(), w.time()); + * // Example: Access some external container using the index + * // auto value = container[w.index()]; + * } + * + * // Iterate over a view to the preview window starting at time t=1.6 and + * // containing all elements (past, present and future) in the preview window + * for(const auto & w : window.all(1.6)) + * { + * mc_rtc::log::info( + * "Preview element with global index: {}, time: {:.3f}\n" + * " local index: {}, time: {:.3f}\n" + * , w.index(), w.time() + * , w.localIndex(), w.localTime() + * ); + * // Global time starts at 1.6s, while local time is between [0, window.duration()] + * } + * \endcode + **/ struct PreviewWindowView { using iterator = RangeElementIterator; @@ -268,18 +334,13 @@ struct PreviewWindowView { } + /** @brief Index corresponding to time t */ Index index(Time t) const noexcept { return window_.index(t); } - /** - * @brief Converts between index and time - * - * @param index Index - * - * @return Time corresponding to the provided index - */ + /** @brief Time corresponding to index */ Time time(Index index) const noexcept { return window_.time(index); @@ -295,7 +356,7 @@ struct PreviewWindowView } iterator end() noexcept { - return iterator{*this, endIndex() + 1}; + return iterator{*this, endIndex() + 1u}; } const_iterator cbegin() noexcept @@ -304,44 +365,28 @@ struct PreviewWindowView } const_iterator cend() noexcept { - return const_iterator{*this, endIndex() + 1}; + return const_iterator{*this, endIndex() + 1u}; } // @} /** - * @brief Time at the start of the preview window + * @brief Time at the start of the preview window view */ inline Time startTime() const noexcept { - return Time{absoluteStart_ * window_.dt()}; + return Time{(absoluteStart_ + localStart_) * window_.dt()}; } /** - * @brief Index of the start of the preview window + * @brief Index of the start of the preview window view */ Index startIndex() const noexcept { - return absoluteStart_; + return absoluteStart_ + localStart_; } - // /** - // * @brief Time at the center of the preview window - // */ - // Time currentTime() const noexcept - // { - // return (start_ + window_.halfSize()) * window_.dt(); - // } - - // /** - // * @brief Index of the center of the preview window - // */ - // Index currentIndex() const noexcept - // { - // return start_ + window_.halfSize(); - // } - /** - * @brief Index of the end of the preview window + * @brief Index of the end of the preview window view */ Time endTime() const noexcept { @@ -349,16 +394,51 @@ struct PreviewWindowView } /** - * @brief Index of the end of the preview window + * @brief Index of the end of the preview window view */ Index endIndex() const noexcept { - return Index{absoluteStart_ + window_.size() - 1}; + return absoluteStart_ + localEnd_; + } + + /** + * @brief Full size of the preview window view + */ + Index size() const noexcept + { + return Index{localEnd_ - localStart_ + 1u}; + } + + /** + * @brief Full duration of the preview window view + */ + Time duration() const noexcept + { + return Time{(size() - 1u) * window_.dt()}; + } + + /** + * @brief Index of the center of the preview window + * @note May be outside of this view + */ + Index nowIndex() const noexcept + { + return absoluteStart_ + window_.halfSize(); + } + + /** + * @brief Time of the center of the preview window + * @note May be outside of this view + */ + Time nowTime() const noexcept + { + return Time{nowIndex() * window_.dt()}; } protected: const CenteredPreviewWindow & window_; ///< Preview window parameters - Index absoluteStart_{0}; ///< Index at which the view starts + Index absoluteStart_{ + 0}; ///< Index at which the preview window starts. The view is defined relative to the start of the preivew window Index localStart_{0}; ///< Index within the preview window where it starts Index localEnd_{0}; ///< Index past end of the view }; diff --git a/src/mc_planning/PreviewWindow.cpp b/src/mc_planning/PreviewWindow.cpp index 80f032a132..2369d6c6e2 100644 --- a/src/mc_planning/PreviewWindow.cpp +++ b/src/mc_planning/PreviewWindow.cpp @@ -9,23 +9,53 @@ namespace mc_planning PreviewWindowView CenteredPreviewWindow::all(Index startIndex) const noexcept { - return PreviewWindowView{*this, startIndex, Index{0}, fullSize_}; + return PreviewWindowView{*this, startIndex, Index{0}, fullSize_ - 1u}; } PreviewWindowView CenteredPreviewWindow::all(Time startTime) const noexcept { - return PreviewWindowView{*this, index(startTime), Index{0}, fullSize_}; + return PreviewWindowView{*this, index(startTime), Index{0}, fullSize_ - 1u}; } -// PreviewWindowView CenteredPreviewWindow::past(Index startIndex) const noexcept -// { -// return PreviewWindowView{*this, startIndex, halfSize_}; -// } +PreviewWindowView CenteredPreviewWindow::past(Index startIndex) const noexcept +{ + return PreviewWindowView{*this, startIndex, Index{0}, halfSize() - 1u}; +} + +PreviewWindowView CenteredPreviewWindow::past(Time startTime) const noexcept +{ + return PreviewWindowView{*this, index(startTime), Index{0}, halfSize() - 1u}; +} + +PreviewWindowView CenteredPreviewWindow::pastInclusive(Index startIndex) const noexcept +{ + return PreviewWindowView{*this, startIndex, Index{0}, Index{halfSize()}}; +} + +PreviewWindowView CenteredPreviewWindow::pastInclusive(Time startTime) const noexcept +{ + return PreviewWindowView{*this, index(startTime), Index{0}, Index{halfSize()}}; +} -// PreviewWindowView CenteredPreviewWindow::future(Index startIndex) const noexcept -// { -// return PreviewWindowView{*this, startIndex+halfSize_, startIndex+fullSize_}; -// } +PreviewWindowView CenteredPreviewWindow::future(Index startIndex) const noexcept +{ + return PreviewWindowView{*this, startIndex, halfSize() + 1u, fullSize_ - 1u}; +} + +PreviewWindowView CenteredPreviewWindow::future(Time startTime) const noexcept +{ + return PreviewWindowView{*this, index(startTime), halfSize() + 1u, fullSize_ - 1u}; +} + +PreviewWindowView CenteredPreviewWindow::futureInclusive(Index startIndex) const noexcept +{ + return PreviewWindowView{*this, startIndex, halfSize(), fullSize_ - 1u}; +} + +PreviewWindowView CenteredPreviewWindow::futureInclusive(Time startTime) const noexcept +{ + return PreviewWindowView{*this, index(startTime), halfSize(), fullSize_ - 1u}; +} Index PreviewElement::index() const noexcept { diff --git a/tests/test_hmc.cpp b/tests/test_hmc.cpp index 55881e69f9..4c159d99cc 100644 --- a/tests/test_hmc.cpp +++ b/tests/test_hmc.cpp @@ -194,30 +194,52 @@ BOOST_AUTO_TEST_CASE(TestPreviewWindow) { CenteredPreviewWindow window(0.015, 0.005); - auto view = window.all(Index(0)); // View on the preview window starting at time 0 - BOOST_REQUIRE_EQUAL(window.duration(), 0.03); - BOOST_REQUIRE_EQUAL(window.halfDuration(), 0.015); - BOOST_REQUIRE_EQUAL(window.halfSize(), 3); // n - BOOST_REQUIRE_EQUAL(window.size(), 7); // 2*n+1 - BOOST_REQUIRE_EQUAL(view.startIndex(), 0); - BOOST_REQUIRE_EQUAL(view.startTime(), 0); - BOOST_REQUIRE_EQUAL(view.endIndex(), 6); // Array of size 7, last index is 6 - BOOST_REQUIRE_EQUAL(view.endTime(), 0.03); - auto end = *view.end(); - BOOST_REQUIRE_EQUAL(end.localIndex(), 7); // Iterator end is one element after the end - BOOST_REQUIRE_EQUAL(end.localTime(), 0.035); // Iterator end is one element after the end - BOOST_REQUIRE_EQUAL(end.index(), 7); // Iterator end is one element after the end - BOOST_REQUIRE_EQUAL(end.time(), 0.035); // Iterator end is one element after the end - auto start = *view.begin(); - BOOST_REQUIRE_EQUAL(start.localIndex(), 0); - BOOST_REQUIRE_EQUAL(start.localTime(), 0.0); - for(const auto w : view) { - mc_rtc::log::info("index {}, local time {:.3f}, global index {}, global time {:.3f}", w.localIndex(), - w.localTime(), w.index(), w.time()); + auto view = window.all(Index(0)); // View on the preview window starting at time 0 + BOOST_REQUIRE_EQUAL(window.duration(), 0.03); + BOOST_REQUIRE_EQUAL(window.halfDuration(), 0.015); + BOOST_REQUIRE_EQUAL(window.halfSize(), 3); // n + BOOST_REQUIRE_EQUAL(window.size(), 7); // 2*n+1 + BOOST_REQUIRE_EQUAL(view.startIndex(), 0); + BOOST_REQUIRE_EQUAL(view.startTime(), 0); + BOOST_REQUIRE_EQUAL(view.endIndex(), 6); // Array of size 7, last index is 6 + BOOST_REQUIRE_EQUAL(view.endTime(), 0.03); + BOOST_REQUIRE_EQUAL(view.nowIndex(), 3); + BOOST_REQUIRE_EQUAL(view.nowTime(), 0.015); + auto end = *view.end(); + BOOST_REQUIRE_EQUAL(end.localIndex(), 7); // Iterator end is one element after the end + BOOST_REQUIRE_EQUAL(end.localTime(), 0.035); // Iterator end is one element after the end + BOOST_REQUIRE_EQUAL(end.index(), 7); // Iterator end is one element after the end + BOOST_REQUIRE_EQUAL(end.time(), 0.035); // Iterator end is one element after the end + auto start = *view.begin(); + BOOST_REQUIRE_EQUAL(start.localIndex(), 0); + BOOST_REQUIRE_EQUAL(start.localTime(), 0.0); + for(const auto w : view) + { + mc_rtc::log::info("index {}, local time {:.3f}, global index {}, global time {:.3f}", w.localIndex(), + w.localTime(), w.index(), w.time()); - BOOST_REQUIRE(w.localIndex() <= 6); - BOOST_REQUIRE(w.index() <= 6); + BOOST_REQUIRE(w.localIndex() <= 6); + BOOST_REQUIRE(w.index() <= 6); + } + } + { + auto past = window.past(Time(10)); + auto pastInclusive = window.pastInclusive(Time(10)); + auto future = window.future(Time(10)); + auto futureInclusive = window.futureInclusive(Time(10)); + BOOST_REQUIRE_EQUAL(past.startTime(), 10); + BOOST_REQUIRE_EQUAL(pastInclusive.startTime(), 10); + BOOST_REQUIRE_EQUAL(past.endTime(), 10.01); + BOOST_REQUIRE_EQUAL(pastInclusive.endTime(), 10.015); + BOOST_REQUIRE_EQUAL(past.nowTime(), 10.015); + BOOST_REQUIRE_EQUAL(pastInclusive.nowTime(), 10.015); + BOOST_REQUIRE_EQUAL(future.startTime(), 10.020); + BOOST_REQUIRE_EQUAL(futureInclusive.startTime(), 10.015); + BOOST_REQUIRE_EQUAL(future.endTime(), 10.03); + BOOST_REQUIRE_EQUAL(futureInclusive.endTime(), 10.03); + BOOST_REQUIRE_EQUAL(future.nowTime(), 10.015); + BOOST_REQUIRE_EQUAL(futureInclusive.nowTime(), 10.015); } { // Test a view starting at time 10 From 4c84a3d33163586e029166cb19be13c98085ddef Mon Sep 17 00:00:00 2001 From: Arnaud TANGUY Date: Thu, 2 Jul 2020 16:03:36 +0900 Subject: [PATCH 38/43] [log/utils] Get compile-time type from enum value --- include/mc_rtc/log/utils.h | 81 +++++++++++++++++++++++++++----------- 1 file changed, 57 insertions(+), 24 deletions(-) diff --git a/include/mc_rtc/log/utils.h b/include/mc_rtc/log/utils.h index 7a7f0107a7..7cf4358b00 100644 --- a/include/mc_rtc/log/utils.h +++ b/include/mc_rtc/log/utils.h @@ -57,36 +57,69 @@ struct GetLogType static constexpr mc_rtc::log::LogType type = mc_rtc::log::LogType::None; }; -#define IMPL_MAPPING(CPPT, ENUMV) \ +#define IMPL_TYPE_TO_ENUM_MAPPING(CPPT, ENUMV) \ template<> \ struct GetLogType \ { \ static constexpr mc_rtc::log::LogType type = mc_rtc::log::LogType::ENUMV; \ } -IMPL_MAPPING(bool, Bool); -IMPL_MAPPING(int8_t, Int8_t); -IMPL_MAPPING(int16_t, Int16_t); -IMPL_MAPPING(int32_t, Int32_t); -IMPL_MAPPING(int64_t, Int64_t); -IMPL_MAPPING(uint8_t, Uint8_t); -IMPL_MAPPING(uint16_t, Uint16_t); -IMPL_MAPPING(uint32_t, Uint32_t); -IMPL_MAPPING(uint64_t, Uint64_t); -IMPL_MAPPING(float, Float); -IMPL_MAPPING(double, Double); -IMPL_MAPPING(std::string, String); -IMPL_MAPPING(Eigen::Vector2d, Vector2d); -IMPL_MAPPING(Eigen::Vector3d, Vector3d); -IMPL_MAPPING(Eigen::Vector6d, Vector6d); -IMPL_MAPPING(Eigen::VectorXd, VectorXd); -IMPL_MAPPING(Eigen::Quaterniond, Quaterniond); -IMPL_MAPPING(sva::PTransformd, PTransformd); -IMPL_MAPPING(sva::ForceVecd, ForceVecd); -IMPL_MAPPING(sva::MotionVecd, MotionVecd); -IMPL_MAPPING(sva::ImpedanceVecd, MotionVecd); - -#undef IMPL_MAPPING +IMPL_TYPE_TO_ENUM_MAPPING(bool, Bool); +IMPL_TYPE_TO_ENUM_MAPPING(int8_t, Int8_t); +IMPL_TYPE_TO_ENUM_MAPPING(int16_t, Int16_t); +IMPL_TYPE_TO_ENUM_MAPPING(int32_t, Int32_t); +IMPL_TYPE_TO_ENUM_MAPPING(int64_t, Int64_t); +IMPL_TYPE_TO_ENUM_MAPPING(uint8_t, Uint8_t); +IMPL_TYPE_TO_ENUM_MAPPING(uint16_t, Uint16_t); +IMPL_TYPE_TO_ENUM_MAPPING(uint32_t, Uint32_t); +IMPL_TYPE_TO_ENUM_MAPPING(uint64_t, Uint64_t); +IMPL_TYPE_TO_ENUM_MAPPING(float, Float); +IMPL_TYPE_TO_ENUM_MAPPING(double, Double); +IMPL_TYPE_TO_ENUM_MAPPING(std::string, String); +IMPL_TYPE_TO_ENUM_MAPPING(Eigen::Vector2d, Vector2d); +IMPL_TYPE_TO_ENUM_MAPPING(Eigen::Vector3d, Vector3d); +IMPL_TYPE_TO_ENUM_MAPPING(Eigen::Vector6d, Vector6d); +IMPL_TYPE_TO_ENUM_MAPPING(Eigen::VectorXd, VectorXd); +IMPL_TYPE_TO_ENUM_MAPPING(Eigen::Quaterniond, Quaterniond); +IMPL_TYPE_TO_ENUM_MAPPING(sva::PTransformd, PTransformd); +IMPL_TYPE_TO_ENUM_MAPPING(sva::ForceVecd, ForceVecd); +IMPL_TYPE_TO_ENUM_MAPPING(sva::MotionVecd, MotionVecd); +IMPL_TYPE_TO_ENUM_MAPPING(sva::ImpedanceVecd, MotionVecd); +IMPL_TYPE_TO_ENUM_MAPPING(std::vector, VectorDouble); +#undef IMPL_TYPE_TO_ENUM_MAPPING + +template +struct GetType; + +#define IMPL_ENUM_TO_TYPE_MAPPING(CPPT, ENUMV) \ + template<> \ + struct GetType \ + { \ + using type = CPPT; \ + }; +IMPL_ENUM_TO_TYPE_MAPPING(bool, Bool); +IMPL_ENUM_TO_TYPE_MAPPING(int8_t, Int8_t); +IMPL_ENUM_TO_TYPE_MAPPING(int16_t, Int16_t); +IMPL_ENUM_TO_TYPE_MAPPING(int32_t, Int32_t); +IMPL_ENUM_TO_TYPE_MAPPING(int64_t, Int64_t); +IMPL_ENUM_TO_TYPE_MAPPING(uint8_t, Uint8_t); +IMPL_ENUM_TO_TYPE_MAPPING(uint16_t, Uint16_t); +IMPL_ENUM_TO_TYPE_MAPPING(uint32_t, Uint32_t); +IMPL_ENUM_TO_TYPE_MAPPING(uint64_t, Uint64_t); +IMPL_ENUM_TO_TYPE_MAPPING(float, Float); +IMPL_ENUM_TO_TYPE_MAPPING(double, Double); +IMPL_ENUM_TO_TYPE_MAPPING(std::string, String); +IMPL_ENUM_TO_TYPE_MAPPING(Eigen::Vector2d, Vector2d); +IMPL_ENUM_TO_TYPE_MAPPING(Eigen::Vector3d, Vector3d); +IMPL_ENUM_TO_TYPE_MAPPING(Eigen::Vector6d, Vector6d); +IMPL_ENUM_TO_TYPE_MAPPING(Eigen::VectorXd, VectorXd); +IMPL_ENUM_TO_TYPE_MAPPING(Eigen::Quaterniond, Quaterniond); +IMPL_ENUM_TO_TYPE_MAPPING(sva::PTransformd, PTransformd); +IMPL_ENUM_TO_TYPE_MAPPING(sva::ForceVecd, ForceVecd); +IMPL_ENUM_TO_TYPE_MAPPING(sva::MotionVecd, MotionVecd); +IMPL_ENUM_TO_TYPE_MAPPING(sva::ImpedanceVecd, MotionVecd); +IMPL_ENUM_TO_TYPE_MAPPING(std::vector, VectorDouble); +#undef IMPL_ENUM_TO_TYPE_MAPPING template struct GetLogType> From 81f44d8de2e15b5d33a5394895ef1f2bdbc59bad Mon Sep 17 00:00:00 2001 From: Arnaud TANGUY Date: Wed, 1 Jul 2020 20:45:54 +0900 Subject: [PATCH 39/43] [utils] Add mc_bin_compare for comparison between logs --- utils/CMakeLists.txt | 3 + utils/mc_bin_compare.cpp | 284 +++++++++++++++++++++++++++++++++++++++ 2 files changed, 287 insertions(+) create mode 100644 utils/mc_bin_compare.cpp diff --git a/utils/CMakeLists.txt b/utils/CMakeLists.txt index 4089a1f2fc..9949b5bd10 100644 --- a/utils/CMakeLists.txt +++ b/utils/CMakeLists.txt @@ -25,6 +25,9 @@ add_mc_rtc_utils(mc_old_bin_to_flat mc_old_bin_to_flat.cpp) add_mc_rtc_utils(mc_json_to_yaml mc_json_to_yaml.cpp) +add_mc_rtc_utils(mc_bin_compare mc_bin_compare.cpp) +target_link_libraries(mc_bin_compare PUBLIC Boost::program_options Boost::disable_autolinking) + configure_file(mc_bin_utils.in.cpp "${CMAKE_CURRENT_BINARY_DIR}/mc_bin_utils.cpp") set(mc_bin_utils_SRC "${CMAKE_CURRENT_BINARY_DIR}/mc_bin_utils.cpp" diff --git a/utils/mc_bin_compare.cpp b/utils/mc_bin_compare.cpp new file mode 100644 index 0000000000..4909a74ca9 --- /dev/null +++ b/utils/mc_bin_compare.cpp @@ -0,0 +1,284 @@ +/* + * Copyright 2015-2020 CNRS-UM LIRMM, CNRS-AIST JRL + */ + +#include +#include +#include +#include +#include +#include +#include + +#include +namespace bfs = boost::filesystem; +#include +namespace po = boost::program_options; + +void usage(char * name) +{ + std::cerr << name << " log1.bin log2.bin [entry1 entry2 ...]\n"; +} + +template +bool allclose( + const Eigen::DenseBase & a, + const Eigen::DenseBase & b, + const typename DerivedA::RealScalar & rtol = Eigen::NumTraits::dummy_precision(), + const typename DerivedA::RealScalar & atol = Eigen::NumTraits::epsilon()) +{ + return ((a.derived() - b.derived()).array().abs() <= (atol + rtol * b.derived().array().abs())).all(); +} + +template< + typename T, + typename std::enable_if::value && !mc_rtc::internal::is_eigen_matrix::value + && !std::is_same::value && !std::is_same::value, + int>::type = 0> +bool compare_elem(const T &, const T &, double) +{ + mc_rtc::log::error_and_throw("Comparison for type {} hasn't been implemented", + mc_rtc::type_name()); +} + +template::value && !mc_rtc::internal::is_eigen_matrix::value, + int>::type = 0> +bool compare_elem(const T & e1, const T & e2, double tolerance) +{ + return std::fabs(e1 - e2) < tolerance; +} + +template::value && !std::is_arithmetic::value, + int>::type = 0> +bool compare_elem(const T & e1, const T & e2, double tolerance) +{ + return allclose(e1, e2, tolerance, tolerance); +} + +template<> +bool compare_elem(const Eigen::Quaterniond & e1, const Eigen::Quaterniond & e2, double tolerance) +{ + return allclose(e1.matrix(), e2.matrix(), tolerance, tolerance); +} + +template<> +bool compare_elem(const bool & e1, const bool & e2, double) +{ + return e1 == e2; +} + +template<> +bool compare_elem(const std::string & e1, const std::string & e2, double) +{ + return e1 == e2; +} + +template::value || std::is_same::value, + int>::type = 0> +bool compare_elem(const T & e1, const T & e2, double tolerance) +{ + return allclose(e1.vector(), e2.vector(), tolerance, tolerance); +} + +template<> +bool compare_elem(const sva::PTransformd & e1, const sva::PTransformd & e2, double tolerance) +{ + return sva::transformError(e1, e2).vector().norm() < 6 * tolerance; +} + +template<> +bool compare_elem(const std::vector & e1, const std::vector & e2, double tolerance) +{ + if(e1.size() != e2.size()) + { + return false; + } + for(size_t i = 0; i < e1.size(); ++i) + { + if(!compare_elem(e1[i], e2[i], tolerance)) + { + return false; + } + } + return true; +} + +template +bool compare(const std::string & entry, + const std::vector & v1, + const std::vector & v2, + double tolerance = 1e-10) +{ + if(v1.size() != v2.size()) + { + mc_rtc::log::error("Entry {} requires the same number of elements", entry); + return false; + } + bool res = true; + for(size_t i = 0; i < v1.size(); ++i) + { + const T * elem1 = v1[i]; + const T * elem2 = v2[i]; + if(elem1 == nullptr || elem2 == nullptr) + { + res = res && (elem1 == nullptr && elem2 == nullptr); + } + else + { + bool equal = compare_elem(*elem1, *elem2, tolerance); + if(!equal) + { + // TODO implement fmt::formatter for all types including eigen to + // display the element value generically + // mc_rtc::log::info("Comparation failed for entry {} at element {}: {} != {} (tolerance {})", entry, i, *elem1, + // *elem2, tolerance); + mc_rtc::log::info("Comparaison failed for entry {} at index {} (tolerance {})", entry, i, tolerance); + } + res = res && equal; + } + if(res == false) return false; + } + return res; +} + +bool compare_entry(const mc_rtc::log::FlatLog & log1, const mc_rtc::log::FlatLog & log2, const std::string & entry) +{ + if(!log1.has(entry) || !log2.has(entry)) + { + mc_rtc::log::error("Entry {} must exist in both log files", entry); + return false; + } + auto types1 = log1.type(entry); + auto types2 = log2.type(entry); + if(types1 != types2) + { + mc_rtc::log::error("Types for entry {} must match", entry); + return false; + } + + bool res = false; +#define COMPARE(LOGT) \ + case mc_rtc::log::LogType::LOGT: \ + { \ + using type = mc_rtc::log::GetType::type; \ + res = compare(entry, log1.getRaw(entry), log2.getRaw(entry)); \ + break; \ + } + switch(types1) + { + COMPARE(Quaterniond) + COMPARE(Vector2d) + COMPARE(Vector3d) + COMPARE(Vector6d) + COMPARE(VectorXd) + COMPARE(Int8_t) + COMPARE(Int16_t) + COMPARE(Int32_t) + COMPARE(Int64_t) + COMPARE(Uint8_t) + COMPARE(Uint16_t) + COMPARE(Uint32_t) + COMPARE(Uint64_t) + COMPARE(Float) + COMPARE(Double) + COMPARE(Bool) + COMPARE(String) + COMPARE(MotionVecd) + COMPARE(ForceVecd) + COMPARE(PTransformd) + COMPARE(VectorDouble) + case mc_rtc::log::LogType::None: + default: + mc_rtc::log::error("Comparison of unsupported type {} requested for entry {}", mc_rtc::log::LogTypeName(types1), + entry); + res = false; + break; + } +#undef COMPARE + + if(res == false) + { + mc_rtc::log::error("Comparison for entry {} failed", entry); + } + return res; +} + +int main(int argc, char * argv[]) +{ + po::variables_map vm; + po::options_description tool("mc_bin_compare"); + // clang-format off + tool.add_options() + ("help", "Produce this message") + ("log1", po::value(), "First log file") + ("log2", po::value(), "Second log file") + ("entries", po::value>(), "Entries") + ("tolerance", po::value()->default_value(1e-10), "Tolerance threshold"); + // clang-format on + po::positional_options_description pos; + pos.add("log1", 1); + pos.add("log2", 1); + pos.add("entries", -1); + po::store(po::command_line_parser(argc, argv).options(tool).positional(pos).run(), vm); + po::notify(vm); + if(vm.count("help") || !vm.count("log1") || !vm.count("log2")) + { + std::cout << "Usage: mc_bin_compare [options] log1 log2 [entries...]\n\n"; + std::cout << tool << "\n"; + return 1; + } + std::vector entries; + if(vm.count("entries")) + { + entries = vm["entries"].as>(); + } + auto file1 = vm["log1"].as(); + auto file2 = vm["log2"].as(); + auto tolerance = vm["tolerance"].as(); + + if(!bfs::exists(file1)) + { + mc_rtc::log::error("File {} does not exist", file1); + return 1; + } + if(!bfs::exists(file2)) + { + mc_rtc::log::error("File {} does not exist", file2); + return 1; + } + mc_rtc::log::FlatLog log1(file1); + mc_rtc::log::FlatLog log2(file2); + if(entries.empty()) + { + mc_rtc::log::info("No entry specified, assuming comparison between all entries"); + for(const auto & entry : log1.entries()) + { + entries.push_back(entry); + } + } + mc_rtc::log::info("Comparing:\n" + "- Logs : {} and {} with:\n" + "- Tolerance: {}\n" + "- Entries : {}", + file1, file2, tolerance, mc_rtc::io::to_string(entries)); + + bool success = true; + for(const auto & entry : entries) + { + success = success && compare_entry(log1, log2, entry); + } + + if(success) + { + mc_rtc::log::success("Comparison successful"); + return 0; + } + else + { + mc_rtc::log::error("Comparison failed: one or more log entries differ"); + return 1; + } +} From 9b1bd15f62b7945b20d71fe1bc0b35bc394f8c38 Mon Sep 17 00:00:00 2001 From: Arnaud TANGUY Date: Fri, 3 Jul 2020 17:27:20 +0900 Subject: [PATCH 40/43] [tests] Add regression tests based on logs --- CMakeLists.txt | 1 + tests/CMakeLists.txt | 11 +++++++++++ tests/log_tests/CMakeLists.txt | 34 ++++++++++++++++++++++++++++++++++ 3 files changed, 46 insertions(+) create mode 100644 tests/log_tests/CMakeLists.txt diff --git a/CMakeLists.txt b/CMakeLists.txt index a0abbce94c..2794af7920 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -47,6 +47,7 @@ option(TREAT_WARNINGS_AS_ERRORS "Treat warnings as errors" OFF) option(BUILD_CONTROLLER_SAMPLES "Build sample controllers in the project" ON) option(DISABLE_CONTROLLER_TESTS "Disable controller unit tests" OFF) +option(DISABLE_LOG_REGRESSION_TESTS "Disable tests checking regressions against downloaded logs" ON) option(DISABLE_ROBOT_TESTS "Disable RobotModule unit tests" OFF) option(ENABLE_FAST_TESTS "Run controllers tests for a few iterations" OFF) diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt index 139753f16e..92e6acbc6f 100644 --- a/tests/CMakeLists.txt +++ b/tests/CMakeLists.txt @@ -141,3 +141,14 @@ mc_rtc_test(test_interpolation mc_control) # -- Temporary HMC-related tests -- # ##################################### mc_rtc_test(test_hmc mc_planning) + +####################################### +# -- Regression tests against logs -- # +####################################### +# Compares excecutable samples against logs of their expected exection +# These tests require network access to download the log files +# Failing of these tests may occur if the methods are changed on purpose. In that case the +# new expected log should be updated. +if(NOT ${DISABLE_LOG_REGRESSION_TESTS}) + add_subdirectory(log_tests) +endif() diff --git a/tests/log_tests/CMakeLists.txt b/tests/log_tests/CMakeLists.txt new file mode 100644 index 0000000000..6cf3ebab8c --- /dev/null +++ b/tests/log_tests/CMakeLists.txt @@ -0,0 +1,34 @@ +# - Dowloads log file from LOG_URL +# - Run the corresponding target EXECUTABLE_TARGET that was used to generate this log file (with optional parameters ARGN). +# This will generates a new log file whose path should be specified with GENERATED_LOG +# - Uses mc_bin_compare tool to compare the entries in both logs +macro(mc_rtc_log_test NAME LOG_URL EXECUTABLE_TARGET GENERATED_LOG) + set(REFERENCE_LOG ${CMAKE_CURRENT_BINARY_DIR}/reference_log.bin) + message(STATUS "Generating ${CMAKE_CURRENT_BINARY_DIR}/test_log_${NAME}.sh") + +file(GENERATE + OUTPUT "${CMAKE_CURRENT_BINARY_DIR}/test_log_${NAME}.sh" + CONTENT +" +#!/bin/sh +wget -O ${REFERENCE_LOG} ${LOG_URL} +$ ${ARGN} +$ ${REFERENCE_LOG} ${GENERATED_LOG} +") + +### file(GENERATE...) does not support permissions, and the files are not generated until GENERATE time (e.g after all the cmake scripts have been processed). Thus the only way to set permissions would be at build time, which involves creating extra custom targets + +# Compare logs (will probably only work on linux) + add_test(NAME TestRegressionCoMGeneration + COMMAND "/bin/sh ${CMAKE_CURRENT_BINARY_DIR}/test_log_${NAME}.sh") +endmacro() + +if(TARGET mc_sample_CoMGeneration) + # Test regression of the CoM trajectory generation using the mc_sample_CoMGeneration sample + mc_rtc_log_test( + TestRegressionCoMGeneration + "https://seafile.lirmm.fr/f/75870c0ca45f469591c1/?dl=1" + mc_sample_CoMGeneration + "/tmp/mc_rtc-test-CoMGenerator-latest.bin" + ) +endif() From 9ecadaa9d15968072ee74b21ce18e1dfcaaeacfe Mon Sep 17 00:00:00 2001 From: Arnaud TANGUY Date: Fri, 3 Jul 2020 17:34:34 +0900 Subject: [PATCH 41/43] [HMC/LTVIP] Use preview windows --- .../LinearTimeVariantInvertedPendulum.h | 33 +---- include/mc_planning/PreviewWindow.h | 17 +++ include/mc_planning/generator.h | 13 +- .../LinearTimeVariantInvertedPendulum.cpp | 125 ++++++++---------- src/mc_planning/PreviewWindow.cpp | 25 ++++ src/mc_planning/generator.cpp | 21 +-- tests/test_hmc.cpp | 1 + 7 files changed, 122 insertions(+), 113 deletions(-) diff --git a/include/mc_planning/LinearTimeVariantInvertedPendulum.h b/include/mc_planning/LinearTimeVariantInvertedPendulum.h index a7302763ac..6044fc967e 100644 --- a/include/mc_planning/LinearTimeVariantInvertedPendulum.h +++ b/include/mc_planning/LinearTimeVariantInvertedPendulum.h @@ -13,6 +13,7 @@ #pragma once #include +#include #include #include #include @@ -96,24 +97,6 @@ struct MC_PLANNING_DLLAPI LinearTimeVariantInvertedPendulum { EIGEN_MAKE_ALIGNED_OPERATOR_NEW - /** - * @brief Default constructor - * - * You must call Initialize() before use - */ - LinearTimeVariantInvertedPendulum(); - - /** - * @brief Initialization - * Calls Initialize() - */ - LinearTimeVariantInvertedPendulum(double dt, - unsigned n_preview = 0, - unsigned weight_resolution = 20000, - double minHeight = 0.01, - double maxHeight = 2.5); - virtual ~LinearTimeVariantInvertedPendulum(); - /** * @brief Initialization * @@ -127,11 +110,10 @@ struct MC_PLANNING_DLLAPI LinearTimeVariantInvertedPendulum * [minHeight, maxHeight]. This is used to optimize the otherwise costly computation of these * functions. */ - void Initialize(double dt, - unsigned n_preview = 0, - unsigned weight_resolution = 20000, - double minHeight = 0.01, - double maxHeight = 2.5); + LinearTimeVariantInvertedPendulum(const CenteredPreviewWindow & window, + unsigned weight_resolution = 20000, + double minHeight = 0.01, + double maxHeight = 2.5); /** * @brief Initialize the discretized system matrices with constant pendulum height @@ -235,13 +217,10 @@ struct MC_PLANNING_DLLAPI LinearTimeVariantInvertedPendulum } protected: + const CenteredPreviewWindow window_; using Matrix22 = Eigen::Matrix; using Vector2 = Eigen::Matrix; - double m_dt; ///< Timstep - unsigned m_n_current; ///< Index of current time (center of the preview window) - unsigned m_n_preview2; ///< Preview window size \f$ (2*\mbox{m_n_current})+1 \f$ - boost::circular_buffer> m_A; ///< \f$ A_k \f$ boost::circular_buffer> m_B; ///< \f$ B_k \f$ std::vector> m_An; ///< \f$ \Phi(k, j) \f$ diff --git a/include/mc_planning/PreviewWindow.h b/include/mc_planning/PreviewWindow.h index 401cc29f37..091272f8a7 100644 --- a/include/mc_planning/PreviewWindow.h +++ b/include/mc_planning/PreviewWindow.h @@ -239,11 +239,18 @@ struct MC_PLANNING_DLLAPI CenteredPreviewWindow return Time{index * dt_}; } + /** Index of the central element */ Index center() const noexcept { return halfSize_; } + /** Index of last element */ + Index last() const noexcept + { + return fullSize_ - 1u; + } + /** * @defgoup range_views * Returns predefined views to the past/future of the preview window starting at the specified time/index @@ -258,22 +265,32 @@ struct MC_PLANNING_DLLAPI CenteredPreviewWindow PreviewWindowView all(Index index) const noexcept; /** All preview elements with a preview window starting at provided time */ PreviewWindowView all(Time startTime) const noexcept; + /** All preview elements with a preview window starting at index 0 */ + PreviewWindowView all() const noexcept; /** Past preview elements (excluding present) with a preview window starting at provided index */ PreviewWindowView past(Index index) const noexcept; /** Past preview elements (excluding present) with a preview window starting at provided time */ PreviewWindowView past(Time startTime) const noexcept; + /** Past preview elements (excluding present) with a preview window starting at index 0 */ + PreviewWindowView past() const noexcept; /** Past preview elements (including present) with a preview window starting at provided index */ PreviewWindowView pastInclusive(Index index) const noexcept; /** Past preview elements (including present) with a preview window starting at provided index */ PreviewWindowView pastInclusive(Time startTime) const noexcept; + /** Past preview elements (including present) with a preview window starting at index 0 */ + PreviewWindowView pastInclusive() const noexcept; /** Future preview elements (excluding present) with a preview window starting at provided index */ PreviewWindowView future(Index index) const noexcept; /** Future preview elements (excluding present) with a preview window starting at provided time */ PreviewWindowView future(Time startTime) const noexcept; + /** Future preview elements (excluding present) with a preview window starting at index 0 */ + PreviewWindowView future() const noexcept; /** Future preview elements (including present) with a preview window starting at provided index */ PreviewWindowView futureInclusive(Index index) const noexcept; /** Future preview elements (including present) with a preview window starting at provided time */ PreviewWindowView futureInclusive(Time startTime) const noexcept; + /** Future preview elements (including present) with a preview window starting at index 0 */ + PreviewWindowView futureInclusive() const noexcept; /// @} protected: diff --git a/include/mc_planning/generator.h b/include/mc_planning/generator.h index 0c4ca4705f..c4d84256d5 100644 --- a/include/mc_planning/generator.h +++ b/include/mc_planning/generator.h @@ -288,6 +288,12 @@ struct MC_PLANNING_DLLAPI generator private: CenteredPreviewWindow preview_; ///< Iteratable preview window and parameters std::shared_ptr> m_ComInterp = nullptr; + std::array m_ipm_long; + std::array m_ipm_short; + /**< Poles for the short-term trajectory. + * Typical range [1,1,150] ... [1,1,300] + */ + std::array m_poles = {Eigen::Vector3d{1., 1., 150.}, Eigen::Vector3d{1., 1., 150.}}; StatePVA m_COG_ideal_pre; /** @@ -317,13 +323,6 @@ struct MC_PLANNING_DLLAPI generator double m_mass = 60.; ///< Robot mass double m_waist_height; ///< Height of the weight (constant for now) - std::array m_ipm_long; - std::array m_ipm_short; - /**< Poles for the short-term trajectory. - * Typical range [1,1,150] ... [1,1,300] - */ - std::array m_poles = {Eigen::Vector3d{1., 1., 150.}, Eigen::Vector3d{1., 1., 150.}}; - std::array m_virtual_height; Eigen::VectorXd m_cog_height; ///< CoM height Eigen::VectorXd m_cog_dot_height; ///< CoM velocity diff --git a/src/mc_planning/LinearTimeVariantInvertedPendulum.cpp b/src/mc_planning/LinearTimeVariantInvertedPendulum.cpp index 9b206f0f5b..5640cd53ce 100644 --- a/src/mc_planning/LinearTimeVariantInvertedPendulum.cpp +++ b/src/mc_planning/LinearTimeVariantInvertedPendulum.cpp @@ -10,22 +10,11 @@ namespace mc_planning namespace linear_control_system { -LinearTimeVariantInvertedPendulum::LinearTimeVariantInvertedPendulum() {} - -LinearTimeVariantInvertedPendulum::LinearTimeVariantInvertedPendulum(double dt, - unsigned n_preview, +LinearTimeVariantInvertedPendulum::LinearTimeVariantInvertedPendulum(const CenteredPreviewWindow & window, unsigned weight_resolution, double minHeight, double maxHeight) -{ - Initialize(dt, n_preview, weight_resolution, minHeight, maxHeight); -} - -void LinearTimeVariantInvertedPendulum::Initialize(double dt, - unsigned n_preview, - unsigned weight_resolution, - double minHeight, - double maxHeight) +: window_(window) { if(weight_resolution == 0) { @@ -34,38 +23,29 @@ void LinearTimeVariantInvertedPendulum::Initialize(double dt, weight_resolution); } - m_dt = dt; - m_n_current = n_preview; - if(n_preview == 0) - { - m_n_current = static_cast(lround(1.6 / m_dt)); - } - m_n_preview2 = n_preview * 2 + 1; - /** Create lookup tables for sqrt(w2), cosh(w*dt), sinh(w*dt) */ auto omega2 = [](double h) { return mc_rtc::constants::GRAVITY / h; }; auto omega = [](double w2) { return std::sqrt(w2); }; - auto chk = [this](double w2) { return cosh(wTable_(w2) * m_dt); }; - auto shk = [this](double w2) { return sinh(wTable_(w2) * m_dt); }; + auto chk = [this](double w2) { return cosh(wTable_(w2) * window_.dt()); }; + auto shk = [this](double w2) { return sinh(wTable_(w2) * window_.dt()); }; double minOmega2 = omega2(maxHeight); double maxOmega2 = omega2(minHeight); wTable_.create(weight_resolution, minOmega2, maxOmega2, omega, "omega^2 -> sqrt(w2)"); chkTable_.create(weight_resolution, minOmega2, maxOmega2, chk, "omega^2 -> cosh(sqrt(omega^2 * dt)"); shkTable_.create(weight_resolution, minOmega2, maxOmega2, shk, "omega^2 -> sinh(sqrt(omega^2 * dt)"); - m_A.resize(m_n_preview2, Matrix22::Identity()); - m_An.resize(m_n_preview2, Matrix22::Identity()); - m_B.resize(m_n_preview2, Vector2::Zero()); - m_Bn.resize(m_n_preview2, Vector2::Zero()); + unsigned previewSize = window_.size(); + m_A.resize(previewSize, Matrix22::Identity()); + m_An.resize(previewSize, Matrix22::Identity()); + m_B.resize(previewSize, Vector2::Zero()); + m_Bn.resize(previewSize, Vector2::Zero()); - m_X.resize(m_n_preview2, Vector2::Zero()); - m_p_ref.setZero(m_n_preview2); - m_w2.setZero(m_n_preview2); - m_w.setZero(m_n_preview2); + m_X.resize(previewSize, Vector2::Zero()); + m_p_ref.setZero(previewSize); + m_w2.setZero(previewSize); + m_w.setZero(previewSize); } -LinearTimeVariantInvertedPendulum::~LinearTimeVariantInvertedPendulum() {} - void LinearTimeVariantInvertedPendulum::initMatrices(double waist_height) { const double w2 = cst::GRAVITY / waist_height; @@ -79,8 +59,7 @@ void LinearTimeVariantInvertedPendulum::initMatrices(double waist_height) Eigen::Matrix2d A; A << ch, sh / w, w * sh, ch; - // Initialize the whole preview window at constant height - for(unsigned i = 0; i < m_n_preview2; i++) + for(unsigned i = 0; i < window_.size(); i++) { m_A.push_front(A); } @@ -90,12 +69,13 @@ void LinearTimeVariantInvertedPendulum::initMatrices(double waist_height) Vector2 v; v << 1 - ch, -w * sh; - for(unsigned i = 0; i < m_n_preview2; i++) + for(unsigned i = 0; i < window_.size(); i++) { m_B.push_front(v); } }; + // Initialize the whole preview window at constant height init_m22(); init_v2(); } @@ -104,11 +84,11 @@ void LinearTimeVariantInvertedPendulum::update() { // set newest matrices { - double w2 = m_w2[m_n_preview2 - 1]; + double w2 = m_w2[window_.last()]; const double w = wTable_(w2); const double ch = chkTable_(w2); const double sh = shkTable_(w2); - m_w[m_n_preview2 - 1] = w; + m_w[window_.last()] = w; Matrix22 Anew; Anew << ch, sh / w, w * sh, ch; Vector2 Bnew; @@ -117,13 +97,13 @@ void LinearTimeVariantInvertedPendulum::update() m_B.push_back(Bnew); } - for(unsigned i = 0; i < m_n_current; i++) + for(unsigned i = 0; i < window_.halfSize(); i++) { m_w[i] = wTable_(m_w2[i]); } // update future matrices - for(unsigned i = m_n_current; i < m_n_preview2 - 1; i++) + for(unsigned i = window_.center(); i < window_.last(); i++) { const double w2 = m_w2[i]; const double w = wTable_(w2); @@ -134,24 +114,26 @@ void LinearTimeVariantInvertedPendulum::update() m_B[i] << 1 - ch, -w * sh; } - m_An[m_n_preview2 - 1] = m_A[m_n_preview2 - 1]; - m_Bn[m_n_preview2 - 1] = m_B[m_n_preview2 - 1]; + unsigned lastIdx = window_.last(); + m_An[lastIdx] = m_A[lastIdx]; + m_Bn[lastIdx] = m_B[lastIdx]; - Vector2 Bsum(m_Bn[m_n_preview2 - 1] * m_p_ref(m_n_preview2 - 1)); - for(int i = m_n_preview2 - 2; i >= 0; i--) + Vector2 Bsum(m_Bn[lastIdx] * m_p_ref(lastIdx)); + for(int i = static_cast(lastIdx) - 1; i >= 0; i--) { - m_An[i].noalias() = m_An[i + 1] * m_A[i]; - m_Bn[i].noalias() = m_An[i + 1] * m_B[i]; - Bsum.noalias() += m_Bn[i] * m_p_ref(i); + unsigned idx = static_cast(i); + m_An[idx].noalias() = m_An[idx + 1u] * m_A[idx]; + m_Bn[idx].noalias() = m_An[idx + 1u] * m_B[idx]; + Bsum.noalias() += m_Bn[idx] * m_p_ref(idx); } - Vector2 Xg(m_p_ref(0), m_p_ref(m_n_preview2 - 1)); + Vector2 Xg(m_p_ref(0), m_p_ref(lastIdx)); Vector2 Vg; Vg << -(m_An[0](0, 0) * Xg(0) - Xg(1) + Bsum(0)) / m_An[0](0, 1), -m_An[0](1, 1) * (m_An[0](0, 0) * Xg(0) - Xg(1) + Bsum(0)) / m_An[0](0, 1) + m_An[0](1, 0) * Xg(0) + Bsum(1); m_X[0] << Xg(0), Vg(0); // set initial states - for(unsigned i = 0; i < m_n_preview2 - 1; i++) + for(unsigned i = 0; i < window_.last(); i++) { m_X[i + 1] = (m_A[i] * m_X[i] + m_B[i] * m_p_ref(i)).eval(); } @@ -163,7 +145,7 @@ void LinearTimeVariantInvertedPendulum::generate(Eigen::VectorXd & cog_pos, Eigen::VectorXd & p_ref) { // update future matrices - for(unsigned i = 0; i < m_n_preview2; i++) + for(unsigned i = 0; i < window_.size(); i++) { const double w2 = m_w2[i]; const double w = wTable_(w2); @@ -174,24 +156,26 @@ void LinearTimeVariantInvertedPendulum::generate(Eigen::VectorXd & cog_pos, m_B[i] << 1 - ch, -w * sh; } - m_An[m_n_preview2 - 1] = m_A[m_n_preview2 - 1]; - m_Bn[m_n_preview2 - 1] = m_B[m_n_preview2 - 1]; + auto lastIdx = window_.last(); + m_An[lastIdx] = m_A[lastIdx]; + m_Bn[lastIdx] = m_B[lastIdx]; - Vector2 Bsum(m_Bn[m_n_preview2 - 1] * m_p_ref(m_n_preview2 - 1)); - for(int i = m_n_preview2 - 2; i >= 0; i--) + Vector2 Bsum(m_Bn[lastIdx] * m_p_ref(lastIdx)); + for(int i = static_cast(lastIdx) - 1; i >= 0; i--) { - m_An[i].noalias() = m_An[i + 1] * m_A[i]; - m_Bn[i].noalias() = m_An[i + 1] * m_B[i]; - Bsum.noalias() += m_Bn[i] * m_p_ref(i); + unsigned idx = static_cast(i); + m_An[idx].noalias() = m_An[idx + 1u] * m_A[idx]; + m_Bn[idx].noalias() = m_An[idx + 1u] * m_B[idx]; + Bsum.noalias() += m_Bn[idx] * m_p_ref(idx); } - Vector2 Xg(m_p_ref(0), m_p_ref(m_n_preview2 - 1)); + Vector2 Xg(m_p_ref(0), m_p_ref(lastIdx)); Vector2 Vg; Vg << -(m_An[0](0, 0) * Xg(0) - Xg(1) + Bsum(0)) / m_An[0](0, 1), -m_An[0](1, 1) * (m_An[0](0, 0) * Xg(0) - Xg(1) + Bsum(0)) / m_An[0](0, 1) + m_An[0](1, 0) * Xg(0) + Bsum(1); Vector2 X(Xg(0), Vg(0)); - for(unsigned i = 0; i < m_n_preview2 - 1; i++) + for(unsigned i = 0; i < lastIdx; i++) { cog_pos(i) = X(0); cog_vel(i) = X(1); @@ -200,18 +184,18 @@ void LinearTimeVariantInvertedPendulum::generate(Eigen::VectorXd & cog_pos, X = (m_A[i] * X + m_B[i] * m_p_ref(i)).eval(); } - cog_pos(m_n_preview2 - 1) = X(0); - cog_vel(m_n_preview2 - 1) = X(1); - cog_acc(m_n_preview2 - 1) = m_w2[m_n_preview2 - 1] * (cog_pos(m_n_preview2 - 1) - m_p_ref(m_n_preview2 - 1)); - p_ref(m_n_preview2 - 1) = m_p_ref(m_n_preview2 - 1); + cog_pos(lastIdx) = X(0); + cog_vel(lastIdx) = X(1); + cog_acc(lastIdx) = m_w2[lastIdx] * (cog_pos(lastIdx) - m_p_ref(lastIdx)); + p_ref(lastIdx) = m_p_ref(lastIdx); } LinearTimeVariantInvertedPendulum::State LinearTimeVariantInvertedPendulum::getState(unsigned n_time) const { - const auto t = m_n_current + n_time; + const auto t = window_.center() + n_time; State s; s.p = m_p_ref(t); - s.pdot = (s.p - m_p_ref[t - 1]) / m_dt; + s.pdot = (s.p - m_p_ref[t - 1u]) / window_.dt(); s.cog_pos = m_X[t](0); s.cog_vel = m_X[t](1); s.cog_acc = m_w2[t] * (s.cog_pos - s.p); @@ -225,11 +209,12 @@ void LinearTimeVariantInvertedPendulum::getState(unsigned n_time, double & p, double & pdot) { - p = m_p_ref(m_n_current + n_time); - pdot = (p - m_p_ref[m_n_current + n_time - 1]) / m_dt; - cog_pos = m_X[m_n_current + n_time](0); - cog_vel = m_X[m_n_current + n_time](1); - cog_acc = m_w2[m_n_current + n_time] * (cog_pos - p); + const auto t = window_.center() + n_time; + p = m_p_ref(t); + pdot = (p - m_p_ref[t - 1u]) / window_.dt(); + cog_pos = m_X[t](0); + cog_vel = m_X[t](1); + cog_acc = m_w2[t] * (cog_pos - p); } } // namespace linear_control_system diff --git a/src/mc_planning/PreviewWindow.cpp b/src/mc_planning/PreviewWindow.cpp index 2369d6c6e2..e5834c6624 100644 --- a/src/mc_planning/PreviewWindow.cpp +++ b/src/mc_planning/PreviewWindow.cpp @@ -17,6 +17,11 @@ PreviewWindowView CenteredPreviewWindow::all(Time startTime) const noexcept return PreviewWindowView{*this, index(startTime), Index{0}, fullSize_ - 1u}; } +PreviewWindowView CenteredPreviewWindow::all() const noexcept +{ + return all(Index{0}); +} + PreviewWindowView CenteredPreviewWindow::past(Index startIndex) const noexcept { return PreviewWindowView{*this, startIndex, Index{0}, halfSize() - 1u}; @@ -27,6 +32,11 @@ PreviewWindowView CenteredPreviewWindow::past(Time startTime) const noexcept return PreviewWindowView{*this, index(startTime), Index{0}, halfSize() - 1u}; } +PreviewWindowView CenteredPreviewWindow::past() const noexcept +{ + return past(Index{0}); +} + PreviewWindowView CenteredPreviewWindow::pastInclusive(Index startIndex) const noexcept { return PreviewWindowView{*this, startIndex, Index{0}, Index{halfSize()}}; @@ -37,6 +47,11 @@ PreviewWindowView CenteredPreviewWindow::pastInclusive(Time startTime) const noe return PreviewWindowView{*this, index(startTime), Index{0}, Index{halfSize()}}; } +PreviewWindowView CenteredPreviewWindow::pastInclusive() const noexcept +{ + return pastInclusive(Index{0}); +} + PreviewWindowView CenteredPreviewWindow::future(Index startIndex) const noexcept { return PreviewWindowView{*this, startIndex, halfSize() + 1u, fullSize_ - 1u}; @@ -47,6 +62,11 @@ PreviewWindowView CenteredPreviewWindow::future(Time startTime) const noexcept return PreviewWindowView{*this, index(startTime), halfSize() + 1u, fullSize_ - 1u}; } +PreviewWindowView CenteredPreviewWindow::future() const noexcept +{ + return future(Index{0}); +} + PreviewWindowView CenteredPreviewWindow::futureInclusive(Index startIndex) const noexcept { return PreviewWindowView{*this, startIndex, halfSize(), fullSize_ - 1u}; @@ -57,6 +77,11 @@ PreviewWindowView CenteredPreviewWindow::futureInclusive(Time startTime) const n return PreviewWindowView{*this, index(startTime), halfSize(), fullSize_ - 1u}; } +PreviewWindowView CenteredPreviewWindow::futureInclusive() const noexcept +{ + return futureInclusive(Index{0}); +} + Index PreviewElement::index() const noexcept { return index_; diff --git a/src/mc_planning/generator.cpp b/src/mc_planning/generator.cpp index 33a5e7a4a5..afd3aa923e 100644 --- a/src/mc_planning/generator.cpp +++ b/src/mc_planning/generator.cpp @@ -13,13 +13,17 @@ namespace mc_planning { generator::generator(const CenteredPreviewWindow & preview, double mass, double waist_height) -: preview_(preview), m_ComInterp(std::make_shared>(1.0, preview_.dt() / 2.)), m_mass(mass), - m_waist_height(waist_height) +: preview_(preview), m_ComInterp(std::make_shared>(1.0, preview_.dt() / 2.)), + // XXX Should be only one computation for both to speed up computation + m_ipm_long{ + linear_control_system::LinearTimeVariantInvertedPendulum(preview, 20000), + linear_control_system::LinearTimeVariantInvertedPendulum(preview, 20000), + }, + m_mass(mass), m_waist_height(waist_height) { mc_rtc::log::info("Creating generator with mass={}, waist_height={}", mass, waist_height); auto initialize = [this, &waist_height](unsigned axis) { - m_ipm_long[axis].Initialize(preview_.dt(), preview_.halfSize(), 20000); m_ipm_short[axis].Initialize(); m_poles[X] << 1.0, 1.0, 150.0; m_virtual_height[axis].setZero(preview_.size()); @@ -29,6 +33,7 @@ generator::generator(const CenteredPreviewWindow & preview, double mass, double initialize(Y); auto previewSize = preview_.size(); + mc_rtc::log::info("Initializing with size {} ", previewSize); m_cog_height.setZero(previewSize); m_cog_dot_height.setZero(previewSize); m_cog_ddot_height.setZero(previewSize); @@ -47,12 +52,10 @@ void generator::setupCOGHeight(unsigned n_current) m_ComInterp->update(); } - for(const auto & step : preview_.all(Index(0))) + for(const auto & step : preview_.all()) { - // clang-format off auto n = step.index(); m_ComInterp->get(n_current + n, m_cog_height[n], m_cog_dot_height[n], m_cog_ddot_height[n]); - //clang-format on } // Interpolated value at current time @@ -89,7 +92,8 @@ void generator::setupTimeTrajectories(unsigned n_current) */ auto interpolateRefXY = [&](const Eigen::Index axis) { return prevStep.step()(axis) - + (nextStep.step()(axis) - prevStep.step()(axis)) * polynomial3((currTime - prevStep.t()) / (nextStep.t() - prevStep.t())); + + (nextStep.step()(axis) - prevStep.step()(axis)) + * polynomial3((currTime - prevStep.t()) / (nextStep.t() - prevStep.t())); }; px_ref(i) = interpolateRefXY(0); @@ -166,8 +170,7 @@ void generator::generateTrajectories() * of the ideal long-term trajectory output modified by the short-term * trajectory to ensure continuity */ - auto applyShortTermCompensation = [this]() - { + auto applyShortTermCompensation = [this]() { m_COG_out.P = m_COG_ideal.P + m_COG_cmp.P; m_COG_out.V = m_COG_ideal.V + m_COG_cmp.V; m_COG_out.Vdot = m_COG_ideal.Vdot + m_COG_cmp.Vdot; diff --git a/tests/test_hmc.cpp b/tests/test_hmc.cpp index 4c159d99cc..c88d51f92d 100644 --- a/tests/test_hmc.cpp +++ b/tests/test_hmc.cpp @@ -200,6 +200,7 @@ BOOST_AUTO_TEST_CASE(TestPreviewWindow) BOOST_REQUIRE_EQUAL(window.halfDuration(), 0.015); BOOST_REQUIRE_EQUAL(window.halfSize(), 3); // n BOOST_REQUIRE_EQUAL(window.size(), 7); // 2*n+1 + BOOST_REQUIRE_EQUAL(window.last(), 6); // 2*n+1 BOOST_REQUIRE_EQUAL(view.startIndex(), 0); BOOST_REQUIRE_EQUAL(view.startTime(), 0); BOOST_REQUIRE_EQUAL(view.endIndex(), 6); // Array of size 7, last index is 6 From 704ebd5f49688b33f2cab1d96c7c8d26ecc15c3a Mon Sep 17 00:00:00 2001 From: Arnaud TANGUY Date: Sat, 12 Sep 2020 10:28:33 +0900 Subject: [PATCH 42/43] [samples/HMC] Adapt CoMTrajectoryGeneration to mc_rtc 1.5 --- .../etc/CoMTrajectoryGeneration.in.yaml | 7 +++++-- .../src/states/StabilizerStandingTrackCoM.cpp | 3 +++ 2 files changed, 8 insertions(+), 2 deletions(-) diff --git a/src/mc_control/samples/CoMTrajectoryGeneration/etc/CoMTrajectoryGeneration.in.yaml b/src/mc_control/samples/CoMTrajectoryGeneration/etc/CoMTrajectoryGeneration.in.yaml index 65ee170372..676e93f687 100644 --- a/src/mc_control/samples/CoMTrajectoryGeneration/etc/CoMTrajectoryGeneration.in.yaml +++ b/src/mc_control/samples/CoMTrajectoryGeneration/etc/CoMTrajectoryGeneration.in.yaml @@ -68,5 +68,8 @@ transitions: init: Pause_2s # State observation -RunObservers: [Encoder, KinematicInertial] -UpdateObservers: [Encoder, KinematicInertial] +ObserverPipelines: +- name: MainPipeline + observers: + - type: Encoder + - type: KinematicInertial diff --git a/src/mc_control/samples/CoMTrajectoryGeneration/src/states/StabilizerStandingTrackCoM.cpp b/src/mc_control/samples/CoMTrajectoryGeneration/src/states/StabilizerStandingTrackCoM.cpp index 891a14f1c3..679efd36f5 100644 --- a/src/mc_control/samples/CoMTrajectoryGeneration/src/states/StabilizerStandingTrackCoM.cpp +++ b/src/mc_control/samples/CoMTrajectoryGeneration/src/states/StabilizerStandingTrackCoM.cpp @@ -32,6 +32,9 @@ void StabilizerStandingTrackCoM::start(mc_control::fsm::Controller & ctl) [this](const Eigen::Vector3d & com, const Eigen::Vector3d & comd, const Eigen::Vector3d & comdd, const Eigen::Vector3d & zmp) { stabilizerTask_->target(com, comd, comdd, zmp); }); + // Update anchor frame for the KinematicInertial observer + ctl.datastore().make_call("KinematicAnchorFrame::" + ctl.robot().name(), + [this](const mc_rbdyn::Robot & robot) { return stabilizerTask_->anchorFrame(robot); }); } bool StabilizerStandingTrackCoM::run(mc_control::fsm::Controller & ctl) From 39396a47149784ba58dc09203ce234a6233aafc7 Mon Sep 17 00:00:00 2001 From: Arnaud TANGUY Date: Tue, 30 Mar 2021 16:26:25 +0900 Subject: [PATCH 43/43] [hmc] Fix rebase to mc_rtc 1.7 --- include/mc_rtc/log/utils.h | 1 - .../mc_tasks/lipm_stabilizer/StabilizerTask.h | 1 - .../src/CoMTrajectoryGeneration.cpp | 10 +++------- .../src/states/StabilizerStandingTrackCoM.cpp | 19 +++++-------------- .../StabilizerTask_log_gui.cpp | 3 +-- 5 files changed, 9 insertions(+), 25 deletions(-) diff --git a/include/mc_rtc/log/utils.h b/include/mc_rtc/log/utils.h index 7cf4358b00..4e5da1d297 100644 --- a/include/mc_rtc/log/utils.h +++ b/include/mc_rtc/log/utils.h @@ -117,7 +117,6 @@ IMPL_ENUM_TO_TYPE_MAPPING(Eigen::Quaterniond, Quaterniond); IMPL_ENUM_TO_TYPE_MAPPING(sva::PTransformd, PTransformd); IMPL_ENUM_TO_TYPE_MAPPING(sva::ForceVecd, ForceVecd); IMPL_ENUM_TO_TYPE_MAPPING(sva::MotionVecd, MotionVecd); -IMPL_ENUM_TO_TYPE_MAPPING(sva::ImpedanceVecd, MotionVecd); IMPL_ENUM_TO_TYPE_MAPPING(std::vector, VectorDouble); #undef IMPL_ENUM_TO_TYPE_MAPPING diff --git a/include/mc_tasks/lipm_stabilizer/StabilizerTask.h b/include/mc_tasks/lipm_stabilizer/StabilizerTask.h index 66e821bb4e..9c1335a02a 100644 --- a/include/mc_tasks/lipm_stabilizer/StabilizerTask.h +++ b/include/mc_tasks/lipm_stabilizer/StabilizerTask.h @@ -836,7 +836,6 @@ struct MC_TASKS_DLLAPI StabilizerTask : public MetaTask Eigen::Vector3d dcmRef_ = Eigen::Vector3d::Zero(); ///< Desired DCM ///@} - double omega_ = 0.; ///< Pendulum frequency double t_ = 0.; /**< Time elapsed since the task is running */ diff --git a/src/mc_control/samples/CoMTrajectoryGeneration/src/CoMTrajectoryGeneration.cpp b/src/mc_control/samples/CoMTrajectoryGeneration/src/CoMTrajectoryGeneration.cpp index 0f48469894..a7493577d4 100644 --- a/src/mc_control/samples/CoMTrajectoryGeneration/src/CoMTrajectoryGeneration.cpp +++ b/src/mc_control/samples/CoMTrajectoryGeneration/src/CoMTrajectoryGeneration.cpp @@ -17,11 +17,7 @@ bool CoMTrajectoryGeneration::run() void CoMTrajectoryGeneration::reset(const mc_control::ControllerResetData & reset_data) { mc_control::fsm::Controller::reset(reset_data); - datastore().make_call("KinematicAnchorFrame::" + robot().name(), - [](const mc_rbdyn::Robot & robot) - { - return sva::interpolate(robot.surfacePose("LeftFootCenter"), - robot.surfacePose("RightFootCenter"), - 0.5); - }); + datastore().make_call("KinematicAnchorFrame::" + robot().name(), [](const mc_rbdyn::Robot & robot) { + return sva::interpolate(robot.surfacePose("LeftFootCenter"), robot.surfacePose("RightFootCenter"), 0.5); + }); } diff --git a/src/mc_control/samples/CoMTrajectoryGeneration/src/states/StabilizerStandingTrackCoM.cpp b/src/mc_control/samples/CoMTrajectoryGeneration/src/states/StabilizerStandingTrackCoM.cpp index 679efd36f5..3965fe53ea 100644 --- a/src/mc_control/samples/CoMTrajectoryGeneration/src/states/StabilizerStandingTrackCoM.cpp +++ b/src/mc_control/samples/CoMTrajectoryGeneration/src/states/StabilizerStandingTrackCoM.cpp @@ -33,19 +33,13 @@ void StabilizerStandingTrackCoM::start(mc_control::fsm::Controller & ctl) const Eigen::Vector3d & comdd, const Eigen::Vector3d & zmp) { stabilizerTask_->target(com, comd, comdd, zmp); }); // Update anchor frame for the KinematicInertial observer + ctl.datastore().remove("KinematicAnchorFrame::" + ctl.robot().name()); ctl.datastore().make_call("KinematicAnchorFrame::" + ctl.robot().name(), [this](const mc_rbdyn::Robot & robot) { return stabilizerTask_->anchorFrame(robot); }); } bool StabilizerStandingTrackCoM::run(mc_control::fsm::Controller & ctl) { - // Update anchor frame for the KinematicInertial observer - ctl.datastore().remove("KinematicAnchorFrame::" + ctl.robot().name()); - ctl.datastore().make_call("KinematicAnchorFrame::" + ctl.robot().name(), - [this](const mc_rbdyn::Robot & robot) - { - return stabilizerTask_->anchorFrame(robot); - }); output("OK"); return false; } @@ -61,13 +55,10 @@ void StabilizerStandingTrackCoM::teardown(mc_control::fsm::Controller & ctl) double leftFootRatio = stabilizerTask_->leftFootRatio(); std::string leftSurface = stabilizerTask_->footSurface(mc_tasks::lipm_stabilizer::ContactState::Left); std::string rightSurface = stabilizerTask_->footSurface(mc_tasks::lipm_stabilizer::ContactState::Right); - ctl.datastore().make_call("KinematicAnchorFrame::" + ctl.robot().name(), - [leftFootRatio, leftSurface, rightSurface](const mc_rbdyn::Robot & robot) - { - return sva::interpolate(robot.surfacePose(leftSurface), - robot.surfacePose(rightSurface), - leftFootRatio); - }); + ctl.datastore().make_call("KinematicAnchorFrame::" + ctl.robot().name(), [leftFootRatio, leftSurface, rightSurface]( + const mc_rbdyn::Robot & robot) { + return sva::interpolate(robot.surfacePose(leftSurface), robot.surfacePose(rightSurface), leftFootRatio); + }); } } // namespace mc_samples diff --git a/src/mc_tasks/lipm_stabilizer/StabilizerTask_log_gui.cpp b/src/mc_tasks/lipm_stabilizer/StabilizerTask_log_gui.cpp index 37f9159153..ebae16d980 100644 --- a/src/mc_tasks/lipm_stabilizer/StabilizerTask_log_gui.cpp +++ b/src/mc_tasks/lipm_stabilizer/StabilizerTask_log_gui.cpp @@ -385,8 +385,7 @@ void StabilizerTask::addToLogger(mc_rtc::Logger & logger) MC_RTC_LOG_HELPER(name_ + "_stabilized_dcm", dcmTarget_); MC_RTC_LOG_HELPER(name_ + "_stabilized_zmp", zmpTarget_); - logger.addLogEntry(name_ + "_contactState", this, - [this]() -> double { + logger.addLogEntry(name_ + "_contactState", this, [this]() -> double { if(inDoubleSupport()) return 0; else if(inContact(ContactState::Left))