From 4a846c8535599efc27ad62837b1031c8fa1d8957 Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Mon, 22 Jun 2020 13:03:57 +0200 Subject: [PATCH 01/23] Design the skeleton for the DCMPlanner --- src/Planners/CMakeLists.txt | 6 +- .../BipedalLocomotion/Planners/DCMPlanner.h | 88 +++++++++++++++++++ src/Planners/src/DCMPlanner.cpp | 34 +++++++ 3 files changed, 125 insertions(+), 3 deletions(-) create mode 100644 src/Planners/include/BipedalLocomotion/Planners/DCMPlanner.h create mode 100644 src/Planners/src/DCMPlanner.cpp diff --git a/src/Planners/CMakeLists.txt b/src/Planners/CMakeLists.txt index 0fd9b34dac..380165fc9c 100644 --- a/src/Planners/CMakeLists.txt +++ b/src/Planners/CMakeLists.txt @@ -8,9 +8,9 @@ if (FRAMEWORK_COMPILE_Planners) add_bipedal_locomotion_library( NAME Contact - PUBLIC_HEADERS ${H_PREFIX}/Contact.h ${H_PREFIX}/ContactList.h ${H_PREFIX}/ContactPhase.h ${H_PREFIX}/ContactPhaseList.h ${H_PREFIX}/ConvexHullHelper.h - SOURCES src/ContactList.cpp src/ContactPhase.cpp src/ContactPhaseList.cpp src/ConvexHullHelper.cpp - PUBLIC_LINK_LIBRARIES iDynTree::idyntree-core + PUBLIC_HEADERS ${H_PREFIX}/Contact.h ${H_PREFIX}/ContactList.h ${H_PREFIX}/ContactPhase.h ${H_PREFIX}/ContactPhaseList.h ${H_PREFIX}/ConvexHullHelper.h ${H_PREFIX}/DCMPlanner.h + SOURCES src/ContactList.cpp src/ContactPhase.cpp src/ContactPhaseList.cpp src/ConvexHullHelper.cpp src/DCMPlanner.cpp + PUBLIC_LINK_LIBRARIES iDynTree::idyntree-core BipedalLocomotion::System BipedalLocomotion::ParametersHandler PRIVATE_LINK_LIBRARIES Qhull::qhullcpp Qhull::qhullstatic_r INSTALLATION_FOLDER Planners SUBDIRECTORIES tests diff --git a/src/Planners/include/BipedalLocomotion/Planners/DCMPlanner.h b/src/Planners/include/BipedalLocomotion/Planners/DCMPlanner.h new file mode 100644 index 0000000000..a3eed398da --- /dev/null +++ b/src/Planners/include/BipedalLocomotion/Planners/DCMPlanner.h @@ -0,0 +1,88 @@ +/** + * @file DCMPlanner.h + * @authors Giulio Romualdi + * @copyright 2020 Istituto Italiano di Tecnologia (IIT). This software may be modified and + * distributed under the terms of the GNU Lesser General Public License v2.1 or any later version. + */ + +#ifndef BIPEDAL_LOCOMOTION_PLANNERS_DCM_PLANNER_H +#define BIPEDAL_LOCOMOTION_PLANNERS_DCM_PLANNER_H + +#include + +#include + +#include +#include +#include + +namespace BipedalLocomotion +{ +namespace Planners +{ + +/** + * DCMPlannerState describes the state of the Divergent Component of Motion (DCM) planner. + */ +struct DCMPlannerState +{ + iDynTree::Vector3 dcmPosition; /**< Position of the DCM expressed w.r.t. the inertial frame */ + iDynTree::Vector3 dcmVelocity; /**< Velocity of the DCM expressed w.r.t. the inertial frame */ + iDynTree::Vector3 vrpPosition; /**< Position of the virtual repellent point (VRP) expressed + w.r.t. the inertial frame */ + double omega;/**< Value of the parameter omega */ +}; + +/** + * DCMPlanner defines a trajectory generator for the variable height Divergent + * component of motion (DCM). Please inherit publicly from this class in order to define your + * planner. + */ +class DCMPlanner : public BipedalLocomotion::System::Advanceable +{ +protected: + std::weak_ptr m_contactPhaseList; /**< Pointer containing the contact + phases. */ + + DCMPlannerState m_initialState; /**< Initial state of the planner */ + +public: + /** + * Initialize the planner. + * @param handler pointer to the parameter handler. + * @return true in case of success/false otherwise. + */ + virtual bool initialize(std::weak_ptr handler); + + /** + * Set the initial state of the planner + * @param initialState the initial state of the planner + */ + void setInitialState(const DCMPlannerState& initialState); + + /** + * Set the contact phase list + * @note the contactPhaseList pointer should point to an already initialize ContactPhaseList. + * Please be sure that the memory pointed is reacheable for the entire life time of the + * DCMPlanner class. + * @param contactPhaseList pointer containing the list of the contact phases + * @return true in case of success, false otherwise. + */ + bool setContactPhaseList(std::weak_ptr contactPhaseList); + + /** + * Compute the DCM trajectory. + * @warning Please implement the function in your custom planner. + * @return true in case of success, false otherwise. + */ + virtual bool computeTrajectory() = 0; + + /** + * Destructor. + */ + virtual ~DCMPlanner() = default; +}; +} // namespace Planners +} // namespace BipedalLocomotion + +#endif // BIPEDAL_LOCOMOTION_PLANNERS_DCM_PLANNER_H diff --git a/src/Planners/src/DCMPlanner.cpp b/src/Planners/src/DCMPlanner.cpp new file mode 100644 index 0000000000..af1adce22a --- /dev/null +++ b/src/Planners/src/DCMPlanner.cpp @@ -0,0 +1,34 @@ +/** + * @file DCMPlanner.cpp + * @authors Giulio Romualdi + * @copyright 2020 Istituto Italiano di Tecnologia (IIT). This software may be modified and + * distributed under the terms of the GNU Lesser General Public License v2.1 or any later version. + */ + +#include + +using namespace BipedalLocomotion::Planners; + +bool DCMPlanner::initialize(std::weak_ptr handler) +{ + return true; +} + +bool DCMPlanner::setContactPhaseList(std::weak_ptr contactPhaseList) +{ + if(contactPhaseList.expired()) + { + std::cerr << "[DCMPlanner::setContactPhaseList] The contactPhaseList pointer is expired. " + "Please pass a valid pointer." + << std::endl; + return false; + } + + m_contactPhaseList = contactPhaseList; + return true; +} + +void DCMPlanner::setInitialState(const DCMPlannerState& initialState) +{ + m_initialState = initialState; +} From 72e42bab90c25ee2d208db8bb3aea0f4dcf69ea2 Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Mon, 6 Jul 2020 14:08:28 +0200 Subject: [PATCH 02/23] Implement the first version of the TimeVaryingDCMPlanner class --- .../Planners/TimeVaryingDCMPlanner.h | 80 ++ src/Planners/src/TimeVaryingDCMPlanner.cpp | 712 ++++++++++++++++++ 2 files changed, 792 insertions(+) create mode 100644 src/Planners/include/BipedalLocomotion/Planners/TimeVaryingDCMPlanner.h create mode 100644 src/Planners/src/TimeVaryingDCMPlanner.cpp diff --git a/src/Planners/include/BipedalLocomotion/Planners/TimeVaryingDCMPlanner.h b/src/Planners/include/BipedalLocomotion/Planners/TimeVaryingDCMPlanner.h new file mode 100644 index 0000000000..50d773e989 --- /dev/null +++ b/src/Planners/include/BipedalLocomotion/Planners/TimeVaryingDCMPlanner.h @@ -0,0 +1,80 @@ +/** + * @file TimeVaryingDCMPlanner.h + * @authors Giulio Romualdi + * @copyright 2020 Istituto Italiano di Tecnologia (IIT). This software may be modified and + * distributed under the terms of the GNU Lesser General Public License v2.1 or any later version. + */ + +#ifndef BIPEDAL_LOCOMOTION_PLANNERS_TIME_VARYING_DCM_PLANNER_H +#define BIPEDAL_LOCOMOTION_PLANNERS_TIME_VARYING_DCM_PLANNER_H + +#include + +#include + +namespace BipedalLocomotion +{ +namespace Planners +{ + +/** + * DCMPlanner defines a trajectory generator for the variable height Divergent component of motion + * (DCM). + */ +class TimeVaryingDCMPlanner : public DCMPlanner +{ + /** + * Private implementation + */ + struct Impl; + + std::unique_ptr m_pimpl; /**< Pointer to private implementation */ + +public: + /** + * Constructor. + */ + TimeVaryingDCMPlanner(); + + /** + * Destructor. + */ + ~TimeVaryingDCMPlanner(); + + /** + * Initialize the planner. + * @param handler pointer to the parameter handler. + * @return true in case of success/false otherwise. + */ + bool initialize(std::weak_ptr handler) override; + + /** + * Compute the DCM trajectory. + * @warning Please implement the function in your custom planner. + * @return true in case of success, false otherwise. + */ + bool computeTrajectory() final; + + /** + * @brief Get the object. + * @return a const reference of the requested object. + */ + const DCMPlannerState& get() const final; + + /** + * @brief Determines the validity of the object retrieved with get() + * @return True if the object is valid, false otherwise. + */ + bool isValid() const final; + + /** + * @brief Advance the internal state. This may change the value retrievable from get(). + * @return True if the advance is successfull. + */ + bool advance() final; + +}; +} // namespace Planners +} // namespace BipedalLocomotion + +#endif // BIPEDAL_LOCOMOTION_PLANNERS_TIME_VARYING_DCM_PLANNER_H diff --git a/src/Planners/src/TimeVaryingDCMPlanner.cpp b/src/Planners/src/TimeVaryingDCMPlanner.cpp new file mode 100644 index 0000000000..13dde62ad1 --- /dev/null +++ b/src/Planners/src/TimeVaryingDCMPlanner.cpp @@ -0,0 +1,712 @@ +/** + * @file TimeVaryingDCMPlanner.cpp + * @authors Giulio Romualdi + * @copyright 2020 Istituto Italiano di Tecnologia (IIT). This software may be modified and + * distributed under the terms of the GNU Lesser General Public License v2.1 or any later version. + */ + +#include + +#include +#include + +#include +#include + +using namespace BipedalLocomotion::Planners; + +/** + * Private implementation of the TimeVaryingDCMPlanner class + */ +struct TimeVaryingDCMPlanner::Impl +{ + static constexpr std::size_t dcmVectorSize = 3; + static constexpr std::size_t vrpVectorSize = 3; + + bool isTrajectoryComputed{false}; /**< True if the trajectory has been computed */ + bool isPlannerInitialized{false}; /**< True if the planner has been initialized */ + + ConvexHullHelper convexHullHelper; /**< Convex hull helper. It is used to compute the + constraints related to the position of the Zero Moment + Point (ZMP) */ + + iDynTree::CubicSpline dcmRefX; /**< Cubic spline for the x-coordinate of the reference DCM */ + iDynTree::CubicSpline dcmRefY; /**< Cubic spline for the y-coordinate of the reference DCM */ + iDynTree::CubicSpline dcmRefZ; /**< Cubic spline for the z-coordinate of the reference DCM */ + + casadi::Opti opti; /**< CasADi opti stack */ + casadi::MX costFunction; /**< Cost function of the optimization problem */ + + /** + * OptimizationVariables contains the optimization variables expressed as CasADi elements. + */ + struct OptimizationVariables + { + casadi::MX dcm; + casadi::MX omega; + casadi::MX vrp; + casadi::MX omegaDot; + }; + OptimizationVariables optiVariables; /**< Optimization variables */ + + /** + * OptimizationParameters contains some parameters used in the optimization problem. A parameter + * is a constant value considered in the optimization problem. Before solving the problem the + * parameters have to be filled with numerical values. + */ + struct OptimizationParameters + { + casadi::MX dcmInitialPosition; /**< Initial boundary condition on the DCM position */ + casadi::MX dcmFinalPosition; /**< Final boundary condition on the DCM position */ + casadi::MX dcmInitialVelocity; /**< Initial boundary condition on the DCM velocity */ + casadi::MX dcmFinalVelocity; /**< Final boundary condition on the DCM velocity */ + casadi::MX omegaInitialValue; /**< Initial boundary condition on the omega */ + casadi::MX omegaFinalValue; /**< Final boundary condition on the omega */ + casadi::MX dcmRefererenceTraj; /** DCM trajectory. This is trajectory is used as + regularization parameter while solving the optimization + problem */ + }; + OptimizationParameters optiParameters; /**< Optimization parameters */ + + /** + * Utilities function used in the optimization problem + */ + struct OptimizationFunctions + { + casadi::Function dcmDynamics; /** DCM dynamics */ + casadi::Function ecmp; /** Enhanced centroidal moment pivot (eCMP) equation */ + }; + OptimizationFunctions optiFunctions; /**< Collection of useful functions */ + + /** + * OptimizationSolution contains some elements required to retrieve the solution of the + * optimization problem. + */ + struct OptimizationSolution + { + std::unique_ptr solution; /**< Pointer to the solution of the optimization + problem */ + casadi::DM dcm; /**< Optimal DCM trajectory */ + casadi::DM omega; /**< Optimal omega trajectory */ + casadi::DM vrp; /**< Optimal VRP trajectory */ + casadi::DM omegaDot; /**< Optimal omega rate of change trajectory */ + }; + OptimizationSolution optiSolution; /**< Solution of the optimization problem */ + + DCMPlannerState trajectory; /**< State of the planner at the current state */ + std::size_t trajectoryIndex; /**< Current index of the trajectory */ + std::size_t numberOfTrajectorySamples; /**< Number of the samples considered by the optimization + problem */ + + /** + * Setting of the optimization problem + */ + struct OptimizationSettings + { + unsigned long solverVerbosity{0}; /**< Verbosity of ipopt */ + std::string ipoptLinearSolver{"mumps"}; /**< Linear solved used by ipopt */ + + double plannerSamplingTime; /**< Sampling time of the planner in seconds */ + std::vector footCorners; /**< Position of the corner of the foot + expressed in a frame placed in the center of the + foot. The X axis points forward and the z + upword.*/ + + double omegaDotWeight; /**< Weight related to the omega dot */ + double dcmTrackingWeight; /**< Weight to the tracking of the DCM */ + double omegaDotRateOfChangeWeight; /**< Weight related to rate of change of omega dot */ + double vrpRateOfChangeWeight; /**< Weight related to the rate of change of the VRP */ + double dcmRateOfChangeWeight; /**< Weight related to the rate of change of the DCM */ + }; + OptimizationSettings optiSettings; /**< Settings */ + + /** + * Get the DCM dynamics + */ + casadi::Function getDCMDynamics() + { + casadi::MX dcm = casadi::MX::sym("dcm", 3); + casadi::MX omega = casadi::MX::sym("omega"); + casadi::MX vrp = casadi::MX::sym("vrp", 3); + casadi::MX omegaDot = casadi::MX::sym("omega_dot"); + + casadi::MX rhs = casadi::MX::vertcat( + {(omega - omegaDot / omega) * (dcm - vrp), omegaDot}); + + return casadi::Function("DCMDynamics", {dcm, omega, vrp, omegaDot}, {rhs}); + } + + /** + * Get the eCMP from VRP and omega + */ + casadi::Function getECMP() + { + casadi::MX omega = casadi::MX::sym("omega"); + casadi::MX vrp = casadi::MX::sym("vrp", 3); + casadi::MX omegaDot = casadi::MX::sym("omega_dot"); + + casadi::MX rhs = casadi::MX::vertcat( + {vrp(casadi::Slice(0, 2)), vrp(2) - 9.81 / (casadi::MX::pow(omega, 2) - omegaDot)}); + + return casadi::Function("eCMP", {omega, vrp, omegaDot}, {rhs}); + } + + /** + * Clear the optimization problem + */ + void clear() + { + opti = casadi::Opti(); + } + + /** + * Setup the optimization problem options + */ + void setupOptiOptions() + { + casadi::Dict casadiOptions; + casadi::Dict ipoptOptions; + + if (this->optiSettings.solverVerbosity != 0) + { + casadi_int ipoptVerbosity = static_cast(optiSettings.solverVerbosity - 1); + ipoptOptions["print_level"] = ipoptVerbosity; + casadiOptions["print_time"] = true; + } else + { + ipoptOptions["print_level"] = 0; + casadiOptions["print_time"] = false; + } + ipoptOptions["linear_solver"] = this->optiSettings.ipoptLinearSolver; + + this->opti.solver("ipopt", casadiOptions, ipoptOptions); + } + + /** + * Setup opti stack + */ + void setupOpti(const std::size_t& horizonSampling) + { + using Sl = casadi::Slice; + + // define the size of the optimization variables + this->optiVariables.dcm = this->opti.variable(this->dcmVectorSize, horizonSampling + 1); + this->optiVariables.omega = this->opti.variable(1, horizonSampling + 1); + this->optiVariables.vrp = this->opti.variable(this->vrpVectorSize, horizonSampling); + this->optiVariables.omegaDot = this->opti.variable(1, horizonSampling); + + // set the initial constraints + this->optiParameters.dcmInitialPosition = this->opti.parameter(this->dcmVectorSize); + this->optiParameters.dcmFinalPosition = this->opti.parameter(this->dcmVectorSize); + this->optiParameters.dcmInitialVelocity = this->opti.parameter(this->dcmVectorSize); + this->optiParameters.dcmFinalVelocity = this->opti.parameter(this->dcmVectorSize); + this->optiParameters.omegaInitialValue = this->opti.parameter(); + this->optiParameters.omegaFinalValue = this->opti.parameter(); + + // set constraints and cost functions whose equations does not change in the optimization + // problem + auto& dcm = this->optiVariables.dcm; + auto& vrp = this->optiVariables.vrp; + auto& omega = this->optiVariables.omega; + auto& omegaDot = this->optiVariables.omegaDot; + + // initial values + this->opti.subject_to(omega(Sl(), 0) == this->optiParameters.omegaInitialValue); + this->opti.subject_to(dcm(Sl(), 0) == this->optiParameters.dcmInitialPosition); + this->opti.subject_to(casadi::MX::vertcat(this->optiFunctions.dcmDynamics( + {dcm(Sl(), 0), omega(Sl(), 0), vrp(Sl(), 0), omegaDot(Sl(), 0)})) + == casadi::MX::vertcat({this->optiParameters.dcmInitialVelocity, 0})); + + // final values + this->opti.subject_to(omega(Sl(), -1) == this->optiParameters.omegaFinalValue); + this->opti.subject_to(dcm(Sl(), -1) == this->optiParameters.dcmFinalPosition); + this->opti.subject_to(casadi::MX::vertcat(this->optiFunctions.dcmDynamics( + {dcm(Sl(), -1), omega(Sl(), -1), vrp(Sl(), -1), omegaDot(Sl(), -1)})) + == casadi::MX::vertcat({this->optiParameters.dcmFinalVelocity, 0})); + + for(std::size_t i = 0; i < horizonSampling - 1; i++) + { + // system dynamics + this->opti.subject_to( + casadi::MX::vertcat({dcm(Sl(), i + 1), omega(Sl(), i + 1)}) + == casadi::MX::vertcat({dcm(Sl(), i), omega(Sl(), i)}) + + this->optiSettings.plannerSamplingTime + * casadi::MX::vertcat(this->optiFunctions.dcmDynamics( + {dcm(Sl(), i), omega(Sl(), i), vrp(Sl(), i), omegaDot(Sl(), i)}))); + + // omega has to be positive + this->opti.subject_to(omega(Sl(), i) > 0); + this->opti.subject_to(casadi::MX::pow(omega(Sl(), i), 2) - omegaDot(Sl(), i) > 0); + } + + this->optiParameters.dcmRefererenceTraj = this->opti.parameter(this->dcmVectorSize, horizonSampling + 1); + + this->costFunction + = this->optiSettings.omegaDotWeight * casadi::MX::sumsqr(this->optiVariables.omegaDot) + + this->optiSettings.dcmTrackingWeight + * casadi::MX::sumsqr(this->optiVariables.dcm + - this->optiParameters.dcmRefererenceTraj) + + this->optiSettings.omegaDotRateOfChangeWeight + * casadi::MX::sumsqr(casadi::MX::diff(this->optiVariables.omegaDot)) + + this->optiSettings.vrpRateOfChangeWeight + * casadi::MX::sumsqr(casadi::MX::diff(this->optiVariables.vrp.T())) + + this->optiSettings.dcmRateOfChangeWeight + * casadi::MX::sumsqr(casadi::MX::diff(this->optiVariables.dcm.T())); + + this->opti.minimize(costFunction); + + // set the opti object options + this->setupOptiOptions(); + } + + /** + * Get the CasADi functions + */ + void setupCasADiFunctions() + { + optiFunctions.dcmDynamics = getDCMDynamics(); + optiFunctions.ecmp = getECMP(); + } + + bool computeConstraintElementsECMP(const ContactPhase& contactPhase, + casadi::DM& ecmpConstraintA, + casadi::DM& ecmpConstraintB) + { + // TODO we may optimize here (DYNAMIC MEMORY ALLOCATION) + std::vector points; + + // check if the points belongs to the same plane + bool samePlane = false; + if (contactPhase.activeContacts.size() == 1) + samePlane = true; + else if (contactPhase.activeContacts.size() == 2) + { + auto contactIt = contactPhase.activeContacts.cbegin(); + double foot1Height = contactIt->second->pose.getPosition()[2]; + std::advance(contactIt, 1); + double foot2Height = contactIt->second->pose.getPosition()[2]; + + if (foot2Height == foot1Height) + samePlane = true; + } + + // compute the convex hull only once + for (const auto& [contactName, activeContact] : contactPhase.activeContacts) + { + for (const auto& footCorner : this->optiSettings.footCorners) + { + iDynTree::Position pointPosition = activeContact->pose * footCorner; + + // TODO we may optimize here (DYNAMIC MEMORY ALLOCATION) + if (samePlane) + points.push_back(iDynTree::VectorDynSize(pointPosition.data(), 2)); + else + points.push_back(iDynTree::VectorDynSize(pointPosition.data(), 3)); + } + } + this->convexHullHelper.buildConvexHull(points); + const auto& A = this->convexHullHelper.getA(); + + // TODO PLEASE OPTIMIZE ME + // please check https://github.com/casadi/casadi/issues/2563 and + // https://groups.google.com/forum/#!topic/casadi-users/npPcKItdLN8 + // unfortunately iDynTree::MatrixDynSize store the matrix in row-major order, thus this + // trick is required + ecmpConstraintA.resize(A.rows(), A.cols()); + ecmpConstraintA = casadi::DM::zeros(A.rows(), A.cols()); + Eigen::MatrixXd AColumnMajour = iDynTree::toEigen(A); + std::memcpy(ecmpConstraintA.ptr(), + AColumnMajour.data(), + sizeof(double) * A.rows() * A.cols()); + + const auto& b = this->convexHullHelper.getB(); + ecmpConstraintB.resize(b.size(), 1); + ecmpConstraintB = casadi::DM::zeros(b.size(), 1); + std::memcpy(ecmpConstraintB.ptr(), b.data(), sizeof(double) * b.size()); + + return samePlane; + } + + bool setupOptimizationProblem(std::shared_ptr contactPhaseList, + const DCMPlannerState& initialState) + { + using Sl = casadi::Slice; + + // set the initial conditions + this->opti.set_value(this->optiParameters.dcmInitialPosition, + casadi::DM(std::vector(initialState.dcmPosition.data(), + initialState.dcmPosition.data() + + this->dcmVectorSize))); + this->opti.set_value(this->optiParameters.dcmInitialVelocity, + casadi::DM(std::vector(initialState.dcmVelocity.data(), + initialState.dcmVelocity.data() + + this->dcmVectorSize))); + this->opti.set_value(this->optiParameters.omegaInitialValue, initialState.omega); + + + double time = contactPhaseList->cbegin()->beginTime; + + std::vector dcmKnots; + std::vector timeKnots; + + // first point + timeKnots.push_back(time); + dcmKnots.push_back(initialState.dcmPosition); + + double averageDCMHeight = initialState.dcmPosition[2]; + + casadi::DM ecmpConstraintA; + casadi::DM ecmpConstraintB; + + const auto& dcm = this->optiVariables.dcm; + const auto& vrp = this->optiVariables.vrp; + const auto& omega = this->optiVariables.omega; + const auto& omegaDot = this->optiVariables.omegaDot; + + + auto contactPhaseListIt = contactPhaseList->cbegin(); + + + bool feetAreInSamePlane = computeConstraintElementsECMP(*contactPhaseListIt, + ecmpConstraintA, + ecmpConstraintB); + + for (std::size_t k = 0; k < this->optiVariables.vrp.columns(); k++) + { + // if the + if(k * this->optiSettings.plannerSamplingTime > contactPhaseListIt->endTime) + { + std::advance(contactPhaseListIt, 1); + feetAreInSamePlane = computeConstraintElementsECMP(*contactPhaseListIt, + ecmpConstraintA, + ecmpConstraintB); + + if (contactPhaseListIt->activeContacts.size() == 2 + && contactPhaseListIt != contactPhaseList->begin() + && contactPhaseListIt != contactPhaseList->lastPhase()) + { + timeKnots.emplace_back( + (contactPhaseListIt->endTime + contactPhaseListIt->beginTime) / 2); + + auto contactIt = contactPhaseListIt->activeContacts.cbegin(); + iDynTree::Position p1 = contactIt->second->pose.getPosition(); + std::advance(contactIt, 1); + iDynTree::Position p2 = contactIt->second->pose.getPosition(); + + iDynTree::Vector3 desiredDCMPosition; + iDynTree::toEigen(desiredDCMPosition) + = (iDynTree::toEigen(p1) + iDynTree::toEigen(p2)) / 2; + desiredDCMPosition(2) += averageDCMHeight; + + dcmKnots.emplace_back(desiredDCMPosition); + } + + // TODO please try to make it more versatile + // read it as if the robot is in the last double support phase + else if (contactPhaseListIt->activeContacts.size() == 2 + && contactPhaseListIt == contactPhaseList->lastPhase()) + { + timeKnots.push_back(contactPhaseListIt->endTime); + auto contactIt = contactPhaseListIt->activeContacts.cbegin(); + iDynTree::Position p1 = contactIt->second->pose.getPosition(); + std::advance(contactIt, 1); + iDynTree::Position p2 = contactIt->second->pose.getPosition(); + + iDynTree::Vector3 desiredDCMPosition; + iDynTree::toEigen(desiredDCMPosition) + = (iDynTree::toEigen(p1) + iDynTree::toEigen(p2)) / 2; + desiredDCMPosition(2) += averageDCMHeight; + + dcmKnots.emplace_back(desiredDCMPosition); + } + } + + if (!feetAreInSamePlane) + { + this->opti.subject_to( + casadi::MX::mtimes(ecmpConstraintA, + casadi::MX::vertcat(this->optiFunctions.ecmp( + {omega(Sl(), k), vrp(Sl(), k), omegaDot(Sl(), k)}))) + <= ecmpConstraintB); + } else + { + this->opti.subject_to(casadi::MX::mtimes(ecmpConstraintA, vrp(Sl(0, 2), k)) + <= ecmpConstraintB); + + this->opti.subject_to( + vrp(2, k) - 9.81 / (casadi::MX::pow(omega(Sl(), k), 2) - omegaDot(Sl(), k)) + == contactPhaseListIt->activeContacts.begin()->second->pose.getPosition()[2]); + } + } + + dcmRefX.setInitialConditions(initialState.dcmVelocity[0], 0); + dcmRefX.setFinalConditions(0, 0); + + dcmRefY.setInitialConditions(initialState.dcmVelocity[1], 0); + dcmRefY.setFinalConditions(0, 0); + + dcmRefZ.setInitialConditions(initialState.dcmVelocity[2], 0); + dcmRefZ.setFinalConditions(0, 0); + + iDynTree::VectorDynSize dcmKnotsX(dcmKnots.size()); + iDynTree::VectorDynSize dcmKnotsY(dcmKnots.size()); + iDynTree::VectorDynSize dcmKnotsZ(dcmKnots.size()); + iDynTree::VectorDynSize timeKnotsiDynTree(dcmKnots.size()); + + for(int i = 0; i < dcmKnots.size(); i++) + { + dcmKnotsX[i] = dcmKnots[i][0]; + dcmKnotsY[i] = dcmKnots[i][1]; + dcmKnotsZ[i] = dcmKnots[i][2]; + timeKnotsiDynTree[i] = timeKnots[i]; + } + + + // set last values + this->opti.set_value(this->optiParameters.dcmFinalPosition, + casadi::DM(std::vector(dcmKnots.back().data(), + dcmKnots.back().data() + + this->dcmVectorSize))); + + this->opti.set_value(this->optiParameters.dcmFinalVelocity, + casadi::DM::zeros(this->dcmVectorSize)); + + // TODO the final omega is equal to the initial omega + this->opti.set_value(this->optiParameters.omegaFinalValue, initialState.omega); + + dcmRefX.setData(timeKnotsiDynTree, dcmKnotsX); + dcmRefY.setData(timeKnotsiDynTree, dcmKnotsY); + dcmRefZ.setData(timeKnotsiDynTree, dcmKnotsZ); + + casadi::DM initialValueDCM(3,this->optiVariables.dcm.columns()); + casadi::DM initialValueOmega(1,this->optiVariables.dcm.columns()); + + for (int i = 0; i < this->optiVariables.dcm.columns(); i++) + { + + this->opti.set_value(this->optiParameters.dcmRefererenceTraj(0, i), + dcmRefX.evaluatePoint(i * this->optiSettings.plannerSamplingTime)); + this->opti.set_value(this->optiParameters.dcmRefererenceTraj(1, i), + dcmRefY.evaluatePoint(i * this->optiSettings.plannerSamplingTime)); + this->opti.set_value(this->optiParameters.dcmRefererenceTraj(2, i), + dcmRefZ.evaluatePoint(i * this->optiSettings.plannerSamplingTime)); + + initialValueDCM(0, i) + = dcmRefX.evaluatePoint(i * this->optiSettings.plannerSamplingTime); + initialValueDCM(1, i) + = dcmRefY.evaluatePoint(i * this->optiSettings.plannerSamplingTime); + initialValueDCM(2, i) + = dcmRefZ.evaluatePoint(i * this->optiSettings.plannerSamplingTime); + + initialValueOmega(Sl(), i) = std::sqrt( + 9.81 / dcmRefZ.evaluatePoint(i * this->optiSettings.plannerSamplingTime)); + } + + this->opti.set_initial(this->optiVariables.dcm, initialValueDCM); + this->opti.set_initial(this->optiVariables.omega, initialValueOmega); + this->opti.set_initial(this->optiVariables.omegaDot, + casadi::DM::zeros(1, omegaDot.columns())); + + return true; + } + + void prepareSolution() + { + using iDynTree::toEigen; + + std::size_t outputIndex = this->trajectoryIndex == numberOfTrajectorySamples + ? numberOfTrajectorySamples - 1 + : trajectoryIndex; + + for (std::size_t i = 0; i < dcmVectorSize; i++) + this->trajectory.dcmPosition(i) = static_cast(optiSolution.dcm(i, trajectoryIndex)); + + for (std::size_t i = 0; i < vrpVectorSize; i++) + trajectory.vrpPosition(i) = static_cast(optiSolution.vrp(i, outputIndex)); + + trajectory.omega = static_cast(optiSolution.omega(0, trajectoryIndex)); + + const double omegaDot = static_cast(optiSolution.omegaDot(0, outputIndex)); + + toEigen(trajectory.dcmVelocity) + = (trajectory.omega - omegaDot / trajectory.omega) + * (toEigen(trajectory.dcmPosition) - toEigen(trajectory.vrpPosition)); + } +}; + +TimeVaryingDCMPlanner::TimeVaryingDCMPlanner() +{ + m_pimpl = std::make_unique(); + assert(m_pimpl); +} + +TimeVaryingDCMPlanner::~TimeVaryingDCMPlanner() = default; + +bool TimeVaryingDCMPlanner::initialize(std::weak_ptr handler) +{ + assert(m_pimpl); + + // convert the weak_ptr into a shared_ptr + auto ptr = handler.lock(); + if (ptr == nullptr) + { + std::cerr << "[TimeVaryingDCMPlanner::initialize] The pointer to the parameter handler is " + "expired." + << std::endl; + return false; + } + + if (!ptr->getParameter("planner_sampling_time", m_pimpl->optiSettings.plannerSamplingTime)) + { + std::cerr << "[TimeVaryingDCMPlanner::initialize] Unable to load the sampling time of the " + "planner." + << std::endl; + return false; + } + + int numberOfFootCorners; + if (!ptr->getParameter("number_of_foot_corners", numberOfFootCorners)) + { + std::cerr << "[TimeVaryingDCMPlanner::initialize] Unable to load the number of foot " + "corners." + << std::endl; + return false; + } + + m_pimpl->optiSettings.footCorners.resize(numberOfFootCorners); + + for(std::size_t i = 0; i < numberOfFootCorners; i++) + { + if (!ptr->getParameter("foot_corner_" + std::to_string(i), + m_pimpl->optiSettings.footCorners[i])) + { + std::cerr << "[TimeVaryingDCMPlanner::initialize] Unable to load get the foot corner " + "number: " + << i << ". Please provide the foot corners having the following names: "; + for (std::size_t j = 0; j < numberOfFootCorners; j++) + { + std::cerr << "foot_corner_" + std::to_string(j) << " "; + } + std::cerr << "." << std::endl; + + return false; + } + } + + bool okCostFunctions = true; + okCostFunctions &= ptr->getParameter("omega_dot_weight", m_pimpl->optiSettings.omegaDotWeight); + okCostFunctions &= ptr->getParameter("dcm_tracking_weight", m_pimpl->optiSettings.dcmTrackingWeight); + okCostFunctions &= ptr->getParameter("omega_dot_rate_of_change_weight", + m_pimpl->optiSettings.omegaDotRateOfChangeWeight); + okCostFunctions &= ptr->getParameter("vrp_rate_of_change_weight", + m_pimpl->optiSettings.vrpRateOfChangeWeight); + + okCostFunctions &= ptr->getParameter("dcm_rate_of_change_weight", + m_pimpl->optiSettings.dcmRateOfChangeWeight); + + if (!okCostFunctions) + { + std::cerr << "[TimeVaryingDCMPlanner::initialize] Unable to load weights of the cost " + "function" + << std::endl; + return false; + } + + // the casadi functions are initialized only once + m_pimpl->setupCasADiFunctions(); + + m_pimpl->isPlannerInitialized = true; + return true; +} + +bool TimeVaryingDCMPlanner::computeTrajectory() +{ + assert(m_pimpl); + + if(!m_pimpl->isPlannerInitialized) + { + std::cerr << "[TimeVaryingDCMPlanner::initialize] Please initialize the planner before " + "computing the trajectory." + << std::endl; + return false; + } + + auto contacts = m_contactPhaseList.lock(); + if (contacts == nullptr) + { + std::cerr << "[TimeVaryingDCMPlanner::computeTrajectory] The pointer to the contact phase " + "list is expired." + << std::endl; + return false; + } + + // clear the solver and the solution computed + m_pimpl->clear(); + + const double& initialTrajectoryTime = contacts->cbegin()->beginTime; + const double& endTrajectoryTime = contacts->lastPhase()->endTime; + + m_pimpl->numberOfTrajectorySamples = std::ceil((endTrajectoryTime - initialTrajectoryTime) + / m_pimpl->optiSettings.plannerSamplingTime); + + m_pimpl->setupOpti(m_pimpl->numberOfTrajectorySamples); + + if (!m_pimpl->setupOptimizationProblem(contacts, m_initialState)) + { + std::cerr << "[TimeVaryingDCMPlanner::computeTrajectory] Unable to setup the optimization " + "problem." + << std::endl; + return false; + } + + try + { + m_pimpl->optiSolution.solution = std::make_unique(m_pimpl->opti.solve()); + } catch (const std::exception& e) + { + std::cerr << "[TimeVaryingDCMPlanner::computeTrajectory] Unable to solve the optimization " + "problem. The following exception has been thrown by the solver" + << e.what() << "." << std::endl; + return false; + } + + m_pimpl->optiSolution.dcm = m_pimpl->optiSolution.solution->value(m_pimpl->optiVariables.dcm); + m_pimpl->optiSolution.vrp = m_pimpl->optiSolution.solution->value(m_pimpl->optiVariables.vrp); + m_pimpl->optiSolution.omega = m_pimpl->optiSolution.solution->value(m_pimpl->optiVariables.omega); + m_pimpl->optiSolution.omegaDot = m_pimpl->optiSolution.solution->value(m_pimpl->optiVariables.omegaDot); + + m_pimpl->prepareSolution(); + + m_pimpl->isTrajectoryComputed = true; + + return true; +} + +const DCMPlannerState& TimeVaryingDCMPlanner::get() const +{ + assert(m_pimpl); + return m_pimpl->trajectory; +} + +bool TimeVaryingDCMPlanner::isValid() const +{ + assert(m_pimpl); + return m_pimpl->isTrajectoryComputed; +} + +bool TimeVaryingDCMPlanner::advance() +{ + if (!isValid()) + { + std::cerr << "[TimeVaryingDCMPlanner::advance] The data are not valid it is not possible " + "to advance." + << std::endl; + return false; + } + + m_pimpl->trajectoryIndex = std::min(m_pimpl->trajectoryIndex + 1, m_pimpl->numberOfTrajectorySamples); + + m_pimpl->prepareSolution(); + + return true; +} From bc4d2eb4f42a66e82d54d25513a1d3a18c087b63 Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Mon, 6 Jul 2020 14:10:55 +0200 Subject: [PATCH 03/23] Implement a simple test for the time-varying DCM planner --- .../tests/TimeVaryingDCMPlannerTest.cpp | 105 ++++++++++++++++++ 1 file changed, 105 insertions(+) create mode 100644 src/Planners/tests/TimeVaryingDCMPlannerTest.cpp diff --git a/src/Planners/tests/TimeVaryingDCMPlannerTest.cpp b/src/Planners/tests/TimeVaryingDCMPlannerTest.cpp new file mode 100644 index 0000000000..a303e2f1d5 --- /dev/null +++ b/src/Planners/tests/TimeVaryingDCMPlannerTest.cpp @@ -0,0 +1,105 @@ +/** + * @file TimeVaryingDCMPlannerTest.cpp + * @authors Giulio Romualdi + * @copyright 2020 Istituto Italiano di Tecnologia (IIT). This software may be modified and + * distributed under the terms of the GNU Lesser General Public License v2.1 or any later version. + */ + +// Catch2 +#include + +#include +#include +#include + +#include + +using namespace BipedalLocomotion::Planners; +using namespace BipedalLocomotion::ParametersHandler; + +TEST_CASE("TimeVaryingDCMPlanner") +{ + auto phaseList = std::make_shared(); + + ContactListMap contactListMap; + + iDynTree::Transform leftTransform = iDynTree::Transform::Identity(); + iDynTree::Position leftPosition; + + leftPosition[0] = 0; + leftPosition[1] = -0.8; + leftPosition[2] = 0; + leftTransform.setPosition(leftPosition); + REQUIRE(contactListMap["left"].addContact(leftTransform, 0.0, 1.0)); + + leftPosition[0] = 0.25; + leftPosition[2] = 0.2; + leftTransform.setPosition(leftPosition); + REQUIRE(contactListMap["left"].addContact(leftTransform, 2.0, 7.0)); + + iDynTree::Transform rightTransform = iDynTree::Transform::Identity(); + iDynTree::Position rightPosition; + + rightPosition[0] = 0; + rightPosition[1] = 0.8; + rightPosition[2] = 0; + rightTransform.setPosition(rightPosition); + REQUIRE(contactListMap["right"].addContact(rightTransform, 0.0, 3.0)); + + rightPosition[0] = 0.25; + rightPosition[2] = 0.2; + rightTransform.setPosition(rightPosition); + REQUIRE(contactListMap["right"].addContact(rightTransform, 4.0, 7.0)); + + phaseList->setLists(contactListMap); + + // Set the parameters + std::shared_ptr handler = std::make_shared(); + handler->setParameter("planner_sampling_time", 0.05); + handler->setParameter("number_of_foot_corners", 4); + + iDynTree::Vector3 footCorner; + footCorner[0] = 0.1; + footCorner[1] = 0.05; + footCorner[2] = 0; + handler->setParameter("foot_corner_0", footCorner); + + footCorner[1] = -0.05; + handler->setParameter("foot_corner_1", footCorner); + + footCorner[0] = -0.1; + footCorner[1] = -0.05; + handler->setParameter("foot_corner_2", footCorner); + + footCorner[1] = 0.05; + handler->setParameter("foot_corner_3", footCorner); + + // set the weight of the cost function + handler->setParameter("omega_dot_weight", 1.0); + handler->setParameter("dcm_tracking_weight", 1.0); + handler->setParameter("omega_dot_rate_of_change_weight", 10.0); + handler->setParameter("vrp_rate_of_change_weight", 100.0); + handler->setParameter("dcm_rate_of_change_weight", 1.0); + + // set the initial state + DCMPlannerState initialState; + initialState.dcmPosition.zero(); + initialState.dcmPosition[2] = 0.53; + initialState.dcmVelocity.zero(); + initialState.vrpPosition = initialState.dcmPosition; + initialState.omega = std::sqrt(9.81 / initialState.dcmPosition[2]); + + // Initialize the planner + TimeVaryingDCMPlanner planner; + REQUIRE(planner.initialize(handler)); + + REQUIRE(planner.setContactPhaseList(phaseList)); + planner.setInitialState(initialState); + + REQUIRE(planner.computeTrajectory()); + + for(int i = 0; i < 150; i++) + { + REQUIRE(planner.advance()); + } +} From 9e03490ae132c6d441bd6ebe59ca2440dde58f35 Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Mon, 6 Jul 2020 14:11:49 +0200 Subject: [PATCH 04/23] Require casADi for compiling the planners component --- cmake/BipedalLocomotionFrameworkFindDependencies.cmake | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/cmake/BipedalLocomotionFrameworkFindDependencies.cmake b/cmake/BipedalLocomotionFrameworkFindDependencies.cmake index 09189c7894..c1a1e64311 100644 --- a/cmake/BipedalLocomotionFrameworkFindDependencies.cmake +++ b/cmake/BipedalLocomotionFrameworkFindDependencies.cmake @@ -131,6 +131,9 @@ checkandset_dependency(Eigen3) find_package(Qhull 8.0.0 QUIET) checkandset_dependency(Qhull) +find_package(Casadi QUIET) +checkandset_dependency(Casadi) + framework_dependent_option(FRAMEWORK_COMPILE_YarpUtilities "Compile YarpHelper library?" ON "FRAMEWORK_HAS_YARP" OFF) @@ -145,7 +148,7 @@ framework_dependent_option(FRAMEWORK_COMPILE_Estimators framework_dependent_option(FRAMEWORK_COMPILE_Planners "Compile Planners libraries?" ON - "FRAMEWORK_HAS_Eigen3;FRAMEWORK_HAS_Qhull" OFF) + "FRAMEWORK_HAS_Eigen3;FRAMEWORK_HAS_Qhull;FRAMEWORK_HAS_Casadi" OFF) framework_dependent_option(FRAMEWORK_COMPILE_ContactModels "Compile ContactModels library?" ON From 5698dab5e648f7b36a6500e26ed1e50cb274b4c3 Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Mon, 6 Jul 2020 14:12:56 +0200 Subject: [PATCH 05/23] Enable the compilation of the time-varying dcm planner --- src/Planners/CMakeLists.txt | 6 +++--- src/Planners/tests/CMakeLists.txt | 5 +++++ 2 files changed, 8 insertions(+), 3 deletions(-) diff --git a/src/Planners/CMakeLists.txt b/src/Planners/CMakeLists.txt index 380165fc9c..a867f00c8f 100644 --- a/src/Planners/CMakeLists.txt +++ b/src/Planners/CMakeLists.txt @@ -8,10 +8,10 @@ if (FRAMEWORK_COMPILE_Planners) add_bipedal_locomotion_library( NAME Contact - PUBLIC_HEADERS ${H_PREFIX}/Contact.h ${H_PREFIX}/ContactList.h ${H_PREFIX}/ContactPhase.h ${H_PREFIX}/ContactPhaseList.h ${H_PREFIX}/ConvexHullHelper.h ${H_PREFIX}/DCMPlanner.h - SOURCES src/ContactList.cpp src/ContactPhase.cpp src/ContactPhaseList.cpp src/ConvexHullHelper.cpp src/DCMPlanner.cpp + PUBLIC_HEADERS ${H_PREFIX}/Contact.h ${H_PREFIX}/ContactList.h ${H_PREFIX}/ContactPhase.h ${H_PREFIX}/ContactPhaseList.h ${H_PREFIX}/ConvexHullHelper.h ${H_PREFIX}/DCMPlanner.h ${H_PREFIX}/TimeVaryingDCMPlanner.h + SOURCES src/ContactList.cpp src/ContactPhase.cpp src/ContactPhaseList.cpp src/ConvexHullHelper.cpp src/DCMPlanner.cpp src/TimeVaryingDCMPlanner.cpp PUBLIC_LINK_LIBRARIES iDynTree::idyntree-core BipedalLocomotion::System BipedalLocomotion::ParametersHandler - PRIVATE_LINK_LIBRARIES Qhull::qhullcpp Qhull::qhullstatic_r + PRIVATE_LINK_LIBRARIES Qhull::qhullcpp Qhull::qhullstatic_r casadi INSTALLATION_FOLDER Planners SUBDIRECTORIES tests DEPENDS_ON_EIGEN_PRIVATE) diff --git a/src/Planners/tests/CMakeLists.txt b/src/Planners/tests/CMakeLists.txt index c4e6e90538..9abbf91859 100644 --- a/src/Planners/tests/CMakeLists.txt +++ b/src/Planners/tests/CMakeLists.txt @@ -20,3 +20,8 @@ add_bipedal_test( NAME ConvexHullHelper SOURCES ConvexHullHelperTest.cpp LINKS BipedalLocomotion::Contact) + +add_bipedal_test( + NAME TimeVaryingDCMPlanner + SOURCES TimeVaryingDCMPlannerTest.cpp + LINKS BipedalLocomotion::Contact) From f5844cefdef585e5b9a7475812be00c70e9f82b4 Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Mon, 6 Jul 2020 14:13:40 +0200 Subject: [PATCH 06/23] Enable to compilation of casADi in workflows/ci.yml --- .github/workflows/ci.yml | 31 +++++++++++++++++++++++++++++-- 1 file changed, 29 insertions(+), 2 deletions(-) diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index 8a2b7be7a7..97471c9007 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -68,7 +68,7 @@ jobs: if: matrix.os == 'macOS-latest' run: | brew update - brew install ace boost eigen swig qt5 orocos-kdl catch2 qhull + brew install ace boost eigen swig qt5 orocos-kdl catch2 qhull ipopt - name: Dependencies [Ubuntu] if: matrix.os == 'ubuntu-latest' @@ -76,7 +76,7 @@ jobs: sudo apt-get update sudo apt-get install git build-essential cmake libace-dev coinor-libipopt-dev libboost-system-dev libboost-filesystem-dev \ libboost-thread-dev liborocos-kdl-dev libeigen3-dev swig qtbase5-dev qtdeclarative5-dev qtmultimedia5-dev \ - libxml2-dev liburdfdom-dev libtinyxml-dev liburdfdom-dev liboctave-dev python-dev valgrind + libxml2-dev liburdfdom-dev libtinyxml-dev liburdfdom-dev liboctave-dev python-dev valgrind coinor-libipopt-dev - name: Cache Source-based Dependencies id: cache-source-deps @@ -204,6 +204,33 @@ jobs: -DCMAKE_POSITION_INDEPENDENT_CODE=ON .. cmake --build . --config ${{ matrix.build_type }} --target install + - name: CasADi [Ubuntu/macOS] + if: matrix.os == 'ubuntu-latest' || matrix.os == 'macOS-latest' + shell: bash + run: | + cd ${GITHUB_WORKSPACE} + git clone https://github.com/casadi/casadi.git -b master casadi + cd casadi + mkdir build + cd build + cmake -DCMAKE_INSTALL_PREFIX=${GITHUB_WORKSPACE}/install/deps -DWITH_IPOPT=BOOL:ON .. + cmake --build . --config ${{ matrix.build_type }} --target install + + - name: CasADi [Windows] + if: matrix.os == 'windows-latest' + shell: bash + run: | + cd ${GITHUB_WORKSPACE} + git clone https://github.com/casadi/casadi.git -b master casadi + cd casadi + mkdir build + cd build + cmake -A x64 -DCMAKE_TOOLCHAIN_FILE=${VCPKG_INSTALLATION_ROOT}/scripts/buildsystems/vcpkg.cmake \ + -DCMAKE_PREFIX_PATH=${GITHUB_WORKSPACE}/install/deps \ + -DCMAKE_BUILD_TYPE=${{ matrix.build_type }} \ + -DCMAKE_INSTALL_PREFIX=${GITHUB_WORKSPACE}/install/deps \ + -DWITH_IPOPT=BOOL:ON .. + cmake --build . --config ${{ matrix.build_type }} --target install # =================== # CMAKE-BASED PROJECT From 68f9b02c8d27e867796a888ad335d62f45881717 Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Wed, 8 Jul 2020 15:47:48 +0200 Subject: [PATCH 07/23] Use Eigen instead iDynTree in the interface of DCMPlanners class --- src/Planners/CMakeLists.txt | 2 +- .../BipedalLocomotion/Planners/DCMPlanner.h | 10 +-- src/Planners/src/TimeVaryingDCMPlanner.cpp | 73 ++++++++++--------- .../tests/TimeVaryingDCMPlannerTest.cpp | 4 +- 4 files changed, 46 insertions(+), 43 deletions(-) diff --git a/src/Planners/CMakeLists.txt b/src/Planners/CMakeLists.txt index 81a7687f47..cee10f0cac 100644 --- a/src/Planners/CMakeLists.txt +++ b/src/Planners/CMakeLists.txt @@ -18,7 +18,7 @@ if (FRAMEWORK_COMPILE_Planners) NAME Planners PUBLIC_HEADERS ${H_PREFIX}/ConvexHullHelper.h ${H_PREFIX}/DCMPlanner.h ${H_PREFIX}/TimeVaryingDCMPlanner.h SOURCES src/ConvexHullHelper.cpp src/DCMPlanner.cpp src/TimeVaryingDCMPlanner.cpp - PUBLIC_LINK_LIBRARIES BipedalLocomotion::Contact BipedalLocomotion::System BipedalLocomotion::ParametersHandler + PUBLIC_LINK_LIBRARIES BipedalLocomotion::System BipedalLocomotion::ParametersHandler BipedalLocomotion::Contact PRIVATE_LINK_LIBRARIES Qhull::qhullcpp Qhull::qhullstatic_r casadi INSTALLATION_FOLDER Planners DEPENDS_ON_EIGEN_PUBLIC) diff --git a/src/Planners/include/BipedalLocomotion/Planners/DCMPlanner.h b/src/Planners/include/BipedalLocomotion/Planners/DCMPlanner.h index a3eed398da..c58bdb9981 100644 --- a/src/Planners/include/BipedalLocomotion/Planners/DCMPlanner.h +++ b/src/Planners/include/BipedalLocomotion/Planners/DCMPlanner.h @@ -10,7 +10,7 @@ #include -#include +#include #include #include @@ -26,10 +26,10 @@ namespace Planners */ struct DCMPlannerState { - iDynTree::Vector3 dcmPosition; /**< Position of the DCM expressed w.r.t. the inertial frame */ - iDynTree::Vector3 dcmVelocity; /**< Velocity of the DCM expressed w.r.t. the inertial frame */ - iDynTree::Vector3 vrpPosition; /**< Position of the virtual repellent point (VRP) expressed - w.r.t. the inertial frame */ + Eigen::Vector3d dcmPosition; /**< Position of the DCM expressed w.r.t. the inertial frame */ + Eigen::Vector3d dcmVelocity; /**< Velocity of the DCM expressed w.r.t. the inertial frame */ + Eigen::Vector3d vrpPosition; /**< Position of the virtual repellent point (VRP) expressed + w.r.t. the inertial frame */ double omega;/**< Value of the parameter omega */ }; diff --git a/src/Planners/src/TimeVaryingDCMPlanner.cpp b/src/Planners/src/TimeVaryingDCMPlanner.cpp index 13dde62ad1..5e4f8a13e9 100644 --- a/src/Planners/src/TimeVaryingDCMPlanner.cpp +++ b/src/Planners/src/TimeVaryingDCMPlanner.cpp @@ -103,8 +103,8 @@ struct TimeVaryingDCMPlanner::Impl */ struct OptimizationSettings { - unsigned long solverVerbosity{0}; /**< Verbosity of ipopt */ - std::string ipoptLinearSolver{"mumps"}; /**< Linear solved used by ipopt */ + unsigned long solverVerbosity{1}; /**< Verbosity of ipopt */ + std::string ipoptLinearSolver{"ma27"}; /**< Linear solved used by ipopt */ double plannerSamplingTime; /**< Sampling time of the planner in seconds */ std::vector footCorners; /**< Position of the corner of the foot @@ -273,12 +273,15 @@ struct TimeVaryingDCMPlanner::Impl casadi::DM& ecmpConstraintB) { // TODO we may optimize here (DYNAMIC MEMORY ALLOCATION) - std::vector points; + Eigen::MatrixXd points; // check if the points belongs to the same plane - bool samePlane = false; + int numberOfCoordinates = 3; + if (contactPhase.activeContacts.size() == 1) - samePlane = true; + { + numberOfCoordinates = 2; + } else if (contactPhase.activeContacts.size() == 2) { auto contactIt = contactPhase.activeContacts.cbegin(); @@ -287,36 +290,39 @@ struct TimeVaryingDCMPlanner::Impl double foot2Height = contactIt->second->pose.getPosition()[2]; if (foot2Height == foot1Height) - samePlane = true; + { + numberOfCoordinates = 2; + } } + const std::size_t numberOfPoints + = contactPhase.activeContacts.size() * this->optiSettings.footCorners.size(); + + points.resize(numberOfCoordinates, numberOfPoints); + // compute the convex hull only once + int columnIndex = 0; + iDynTree::Position point; for (const auto& [contactName, activeContact] : contactPhase.activeContacts) { for (const auto& footCorner : this->optiSettings.footCorners) { - iDynTree::Position pointPosition = activeContact->pose * footCorner; - - // TODO we may optimize here (DYNAMIC MEMORY ALLOCATION) - if (samePlane) - points.push_back(iDynTree::VectorDynSize(pointPosition.data(), 2)); - else - points.push_back(iDynTree::VectorDynSize(pointPosition.data(), 3)); + point = activeContact->pose * footCorner; + points.col(columnIndex) = iDynTree::toEigen(point).head(numberOfCoordinates); + columnIndex++; } } this->convexHullHelper.buildConvexHull(points); - const auto& A = this->convexHullHelper.getA(); // TODO PLEASE OPTIMIZE ME // please check https://github.com/casadi/casadi/issues/2563 and // https://groups.google.com/forum/#!topic/casadi-users/npPcKItdLN8 - // unfortunately iDynTree::MatrixDynSize store the matrix in row-major order, thus this - // trick is required + // Assumption: the matrices as stored as column-major + const auto& A = this->convexHullHelper.getA(); ecmpConstraintA.resize(A.rows(), A.cols()); ecmpConstraintA = casadi::DM::zeros(A.rows(), A.cols()); - Eigen::MatrixXd AColumnMajour = iDynTree::toEigen(A); std::memcpy(ecmpConstraintA.ptr(), - AColumnMajour.data(), + A.data(), sizeof(double) * A.rows() * A.cols()); const auto& b = this->convexHullHelper.getB(); @@ -324,7 +330,7 @@ struct TimeVaryingDCMPlanner::Impl ecmpConstraintB = casadi::DM::zeros(b.size(), 1); std::memcpy(ecmpConstraintB.ptr(), b.data(), sizeof(double) * b.size()); - return samePlane; + return numberOfCoordinates == 2; } bool setupOptimizationProblem(std::shared_ptr contactPhaseList, @@ -346,7 +352,7 @@ struct TimeVaryingDCMPlanner::Impl double time = contactPhaseList->cbegin()->beginTime; - std::vector dcmKnots; + std::vector dcmKnots; std::vector timeKnots; // first point @@ -393,9 +399,8 @@ struct TimeVaryingDCMPlanner::Impl std::advance(contactIt, 1); iDynTree::Position p2 = contactIt->second->pose.getPosition(); - iDynTree::Vector3 desiredDCMPosition; - iDynTree::toEigen(desiredDCMPosition) - = (iDynTree::toEigen(p1) + iDynTree::toEigen(p2)) / 2; + Eigen::Vector3d desiredDCMPosition; + desiredDCMPosition = (iDynTree::toEigen(p1) + iDynTree::toEigen(p2)) / 2; desiredDCMPosition(2) += averageDCMHeight; dcmKnots.emplace_back(desiredDCMPosition); @@ -412,9 +417,8 @@ struct TimeVaryingDCMPlanner::Impl std::advance(contactIt, 1); iDynTree::Position p2 = contactIt->second->pose.getPosition(); - iDynTree::Vector3 desiredDCMPosition; - iDynTree::toEigen(desiredDCMPosition) - = (iDynTree::toEigen(p1) + iDynTree::toEigen(p2)) / 2; + Eigen::Vector3d desiredDCMPosition; + desiredDCMPosition = (iDynTree::toEigen(p1) + iDynTree::toEigen(p2)) / 2; desiredDCMPosition(2) += averageDCMHeight; dcmKnots.emplace_back(desiredDCMPosition); @@ -512,25 +516,24 @@ struct TimeVaryingDCMPlanner::Impl void prepareSolution() { - using iDynTree::toEigen; - std::size_t outputIndex = this->trajectoryIndex == numberOfTrajectorySamples ? numberOfTrajectorySamples - 1 : trajectoryIndex; - for (std::size_t i = 0; i < dcmVectorSize; i++) - this->trajectory.dcmPosition(i) = static_cast(optiSolution.dcm(i, trajectoryIndex)); + std::memcpy(this->trajectory.dcmPosition.data(), + optiSolution.dcm.ptr(), + sizeof(double) * this->dcmVectorSize); - for (std::size_t i = 0; i < vrpVectorSize; i++) - trajectory.vrpPosition(i) = static_cast(optiSolution.vrp(i, outputIndex)); + std::memcpy(this->trajectory.vrpPosition.data(), + optiSolution.vrp.ptr(), + sizeof(double) * this->dcmVectorSize); trajectory.omega = static_cast(optiSolution.omega(0, trajectoryIndex)); const double omegaDot = static_cast(optiSolution.omegaDot(0, outputIndex)); - toEigen(trajectory.dcmVelocity) - = (trajectory.omega - omegaDot / trajectory.omega) - * (toEigen(trajectory.dcmPosition) - toEigen(trajectory.vrpPosition)); + trajectory.dcmVelocity = (trajectory.omega - omegaDot / trajectory.omega) + * (trajectory.dcmPosition - trajectory.vrpPosition); } }; diff --git a/src/Planners/tests/TimeVaryingDCMPlannerTest.cpp b/src/Planners/tests/TimeVaryingDCMPlannerTest.cpp index a303e2f1d5..8a36a21591 100644 --- a/src/Planners/tests/TimeVaryingDCMPlannerTest.cpp +++ b/src/Planners/tests/TimeVaryingDCMPlannerTest.cpp @@ -83,9 +83,9 @@ TEST_CASE("TimeVaryingDCMPlanner") // set the initial state DCMPlannerState initialState; - initialState.dcmPosition.zero(); + initialState.dcmPosition.setZero(); initialState.dcmPosition[2] = 0.53; - initialState.dcmVelocity.zero(); + initialState.dcmVelocity.setZero(); initialState.vrpPosition = initialState.dcmPosition; initialState.omega = std::sqrt(9.81 / initialState.dcmPosition[2]); From 630d5c0fcb52db81b430de4284f4340d54979f49 Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Wed, 8 Jul 2020 18:24:44 +0200 Subject: [PATCH 08/23] Store the ContactPhaseLists in a shared_ptr instead of weak_ptr in DCMPlanner class --- .../BipedalLocomotion/Planners/DCMPlanner.h | 4 ++-- src/Planners/src/DCMPlanner.cpp | 2 +- src/Planners/src/TimeVaryingDCMPlanner.cpp | 15 +++------------ 3 files changed, 6 insertions(+), 15 deletions(-) diff --git a/src/Planners/include/BipedalLocomotion/Planners/DCMPlanner.h b/src/Planners/include/BipedalLocomotion/Planners/DCMPlanner.h index c58bdb9981..59ac7f9084 100644 --- a/src/Planners/include/BipedalLocomotion/Planners/DCMPlanner.h +++ b/src/Planners/include/BipedalLocomotion/Planners/DCMPlanner.h @@ -41,8 +41,8 @@ struct DCMPlannerState class DCMPlanner : public BipedalLocomotion::System::Advanceable { protected: - std::weak_ptr m_contactPhaseList; /**< Pointer containing the contact - phases. */ + std::shared_ptr m_contactPhaseList; /**< Pointer containing the contact + phases. */ DCMPlannerState m_initialState; /**< Initial state of the planner */ diff --git a/src/Planners/src/DCMPlanner.cpp b/src/Planners/src/DCMPlanner.cpp index af1adce22a..7e1242d163 100644 --- a/src/Planners/src/DCMPlanner.cpp +++ b/src/Planners/src/DCMPlanner.cpp @@ -24,7 +24,7 @@ bool DCMPlanner::setContactPhaseList(std::weak_ptr conta return false; } - m_contactPhaseList = contactPhaseList; + m_contactPhaseList = contactPhaseList.lock(); return true; } diff --git a/src/Planners/src/TimeVaryingDCMPlanner.cpp b/src/Planners/src/TimeVaryingDCMPlanner.cpp index 5e4f8a13e9..0c9c4af550 100644 --- a/src/Planners/src/TimeVaryingDCMPlanner.cpp +++ b/src/Planners/src/TimeVaryingDCMPlanner.cpp @@ -634,27 +634,18 @@ bool TimeVaryingDCMPlanner::computeTrajectory() return false; } - auto contacts = m_contactPhaseList.lock(); - if (contacts == nullptr) - { - std::cerr << "[TimeVaryingDCMPlanner::computeTrajectory] The pointer to the contact phase " - "list is expired." - << std::endl; - return false; - } - // clear the solver and the solution computed m_pimpl->clear(); - const double& initialTrajectoryTime = contacts->cbegin()->beginTime; - const double& endTrajectoryTime = contacts->lastPhase()->endTime; + const double& initialTrajectoryTime = m_contactPhaseList->cbegin()->beginTime; + const double& endTrajectoryTime = m_contactPhaseList->lastPhase()->endTime; m_pimpl->numberOfTrajectorySamples = std::ceil((endTrajectoryTime - initialTrajectoryTime) / m_pimpl->optiSettings.plannerSamplingTime); m_pimpl->setupOpti(m_pimpl->numberOfTrajectorySamples); - if (!m_pimpl->setupOptimizationProblem(contacts, m_initialState)) + if (!m_pimpl->setupOptimizationProblem(m_contactPhaseList, m_initialState)) { std::cerr << "[TimeVaryingDCMPlanner::computeTrajectory] Unable to setup the optimization " "problem." From 98adeefcd53051cc63c372a5827f71d7825be8ae Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Wed, 8 Jul 2020 18:26:53 +0200 Subject: [PATCH 09/23] Remove outdated comment in TimeVaryingDCMPlanner documentation --- .../include/BipedalLocomotion/Planners/TimeVaryingDCMPlanner.h | 1 - 1 file changed, 1 deletion(-) diff --git a/src/Planners/include/BipedalLocomotion/Planners/TimeVaryingDCMPlanner.h b/src/Planners/include/BipedalLocomotion/Planners/TimeVaryingDCMPlanner.h index 50d773e989..e9e4be8513 100644 --- a/src/Planners/include/BipedalLocomotion/Planners/TimeVaryingDCMPlanner.h +++ b/src/Planners/include/BipedalLocomotion/Planners/TimeVaryingDCMPlanner.h @@ -50,7 +50,6 @@ class TimeVaryingDCMPlanner : public DCMPlanner /** * Compute the DCM trajectory. - * @warning Please implement the function in your custom planner. * @return true in case of success, false otherwise. */ bool computeTrajectory() final; From ec2c99c6384f330997b65a7b8c6308b50b1f49fb Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Wed, 8 Jul 2020 18:27:55 +0200 Subject: [PATCH 10/23] Use lowercase instead of cammelcase for finding casadi in cmake file --- cmake/BipedalLocomotionFrameworkFindDependencies.cmake | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/cmake/BipedalLocomotionFrameworkFindDependencies.cmake b/cmake/BipedalLocomotionFrameworkFindDependencies.cmake index 3bf90e52ee..0356c055fa 100644 --- a/cmake/BipedalLocomotionFrameworkFindDependencies.cmake +++ b/cmake/BipedalLocomotionFrameworkFindDependencies.cmake @@ -142,8 +142,8 @@ checkandset_dependency(Eigen3) find_package(Qhull 8.0.0 QUIET) checkandset_dependency(Qhull) -find_package(Casadi QUIET) -checkandset_dependency(Casadi) +find_package(casadi QUIET) +checkandset_dependency(casadi) framework_dependent_option(FRAMEWORK_COMPILE_YarpUtilities "Compile YarpHelper library?" ON @@ -159,7 +159,7 @@ framework_dependent_option(FRAMEWORK_COMPILE_Estimators framework_dependent_option(FRAMEWORK_COMPILE_Planners "Compile Planners libraries?" ON - "FRAMEWORK_USE_Eigen3;FRAMEWORK_USE_Qhull;FRAMEWORK_USE_Casadi" OFF) + "FRAMEWORK_USE_Eigen3;FRAMEWORK_USE_Qhull;FRAMEWORK_USE_casadi" OFF) framework_dependent_option(FRAMEWORK_COMPILE_ContactModels "Compile ContactModels library?" ON From f57218eb66ed354a331e1999f2c0e3c8b3df48c8 Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Wed, 8 Jul 2020 18:31:07 +0200 Subject: [PATCH 11/23] Apply suggestions from code review Co-authored-by: Stefano Dafarra --- src/Planners/include/BipedalLocomotion/Planners/DCMPlanner.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/Planners/include/BipedalLocomotion/Planners/DCMPlanner.h b/src/Planners/include/BipedalLocomotion/Planners/DCMPlanner.h index 59ac7f9084..b7086bac26 100644 --- a/src/Planners/include/BipedalLocomotion/Planners/DCMPlanner.h +++ b/src/Planners/include/BipedalLocomotion/Planners/DCMPlanner.h @@ -62,8 +62,8 @@ class DCMPlanner : public BipedalLocomotion::System::Advanceable Date: Wed, 8 Jul 2020 19:17:47 +0200 Subject: [PATCH 12/23] Use mumps as default solver for ipopt --- src/Planners/src/TimeVaryingDCMPlanner.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/Planners/src/TimeVaryingDCMPlanner.cpp b/src/Planners/src/TimeVaryingDCMPlanner.cpp index 0c9c4af550..1e811109f5 100644 --- a/src/Planners/src/TimeVaryingDCMPlanner.cpp +++ b/src/Planners/src/TimeVaryingDCMPlanner.cpp @@ -104,7 +104,7 @@ struct TimeVaryingDCMPlanner::Impl struct OptimizationSettings { unsigned long solverVerbosity{1}; /**< Verbosity of ipopt */ - std::string ipoptLinearSolver{"ma27"}; /**< Linear solved used by ipopt */ + std::string ipoptLinearSolver{"mumps"}; /**< Linear solved used by ipopt */ double plannerSamplingTime; /**< Sampling time of the planner in seconds */ std::vector footCorners; /**< Position of the corner of the foot From c86fd10467d064908d99aa1c8ea5870eb52f058d Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Tue, 14 Jul 2020 12:33:08 +0200 Subject: [PATCH 13/23] Install pkg-config in macOS continuous integration --- .github/workflows/ci.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index 97471c9007..a51d96e0c6 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -68,7 +68,7 @@ jobs: if: matrix.os == 'macOS-latest' run: | brew update - brew install ace boost eigen swig qt5 orocos-kdl catch2 qhull ipopt + brew install ace boost eigen swig qt5 orocos-kdl catch2 qhull ipopt pkg-config - name: Dependencies [Ubuntu] if: matrix.os == 'ubuntu-latest' From 6939e1359aa73aecfc02fc399575ecd2b6795023 Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Tue, 14 Jul 2020 14:25:15 +0200 Subject: [PATCH 14/23] Use DynamicalSystem and ForwardEuler integrator classes in TimeVaryingDCMPlanner --- src/Planners/src/TimeVaryingDCMPlanner.cpp | 98 +++++++++++++++------- 1 file changed, 70 insertions(+), 28 deletions(-) diff --git a/src/Planners/src/TimeVaryingDCMPlanner.cpp b/src/Planners/src/TimeVaryingDCMPlanner.cpp index 1e811109f5..72c8f1af42 100644 --- a/src/Planners/src/TimeVaryingDCMPlanner.cpp +++ b/src/Planners/src/TimeVaryingDCMPlanner.cpp @@ -12,8 +12,41 @@ #include #include +#include +#include using namespace BipedalLocomotion::Planners; +using namespace BipedalLocomotion::System; + +class TimeVaryingDCMPlannerDynamics : public DynamicalSystem, + std::tuple, + std::tuple> +{ +public: + /** + * Computes the floating based system dynamics. It return \f$f(x, u, t)\f$. + * @note The control input and the state have to be set separately with the methods + * setControlInput and setState. + * @param time the time at witch the dynamics is computed. + * @param stateDynamics tuple containing a reference to the element of the state derivative + * @return true in case of success, false otherwise. + */ + bool dynamics(const double& time, StateDerivativeType& stateDerivative) final + { + const auto& dcm = std::get<0>(m_state); + const auto& omega = std::get<1>(m_state); + + const auto& vrp = std::get<0>(m_controlInput); + const auto& omegaDotInput = std::get<1>(m_controlInput); + + auto& dcmDot = std::get<0>(stateDerivative); + auto& omegaDot = std::get<1>(stateDerivative); + + dcmDot = (omega - omegaDotInput / omega) * (dcm - vrp); + omegaDot = omegaDotInput; + return true; + } +}; /** * Private implementation of the TimeVaryingDCMPlanner class @@ -34,6 +67,8 @@ struct TimeVaryingDCMPlanner::Impl iDynTree::CubicSpline dcmRefY; /**< Cubic spline for the y-coordinate of the reference DCM */ iDynTree::CubicSpline dcmRefZ; /**< Cubic spline for the z-coordinate of the reference DCM */ + std::unique_ptr> dcmDynamicsIntegrator; + casadi::Opti opti; /**< CasADi opti stack */ casadi::MX costFunction; /**< Cost function of the optimization problem */ @@ -120,22 +155,6 @@ struct TimeVaryingDCMPlanner::Impl }; OptimizationSettings optiSettings; /**< Settings */ - /** - * Get the DCM dynamics - */ - casadi::Function getDCMDynamics() - { - casadi::MX dcm = casadi::MX::sym("dcm", 3); - casadi::MX omega = casadi::MX::sym("omega"); - casadi::MX vrp = casadi::MX::sym("vrp", 3); - casadi::MX omegaDot = casadi::MX::sym("omega_dot"); - - casadi::MX rhs = casadi::MX::vertcat( - {(omega - omegaDot / omega) * (dcm - vrp), omegaDot}); - - return casadi::Function("DCMDynamics", {dcm, omega, vrp, omegaDot}, {rhs}); - } - /** * Get the eCMP from VRP and omega */ @@ -189,6 +208,8 @@ struct TimeVaryingDCMPlanner::Impl { using Sl = casadi::Slice; + auto dcmDynamicalSystem = this->dcmDynamicsIntegrator->dynamicalSystem().lock(); + // define the size of the optimization variables this->optiVariables.dcm = this->opti.variable(this->dcmVectorSize, horizonSampling + 1); this->optiVariables.omega = this->opti.variable(1, horizonSampling + 1); @@ -213,26 +234,43 @@ struct TimeVaryingDCMPlanner::Impl // initial values this->opti.subject_to(omega(Sl(), 0) == this->optiParameters.omegaInitialValue); this->opti.subject_to(dcm(Sl(), 0) == this->optiParameters.dcmInitialPosition); - this->opti.subject_to(casadi::MX::vertcat(this->optiFunctions.dcmDynamics( - {dcm(Sl(), 0), omega(Sl(), 0), vrp(Sl(), 0), omegaDot(Sl(), 0)})) + + // initial value of the DCM dynamics + dcmDynamicalSystem->setState({dcm(Sl(), 0), omega(Sl(), 0)}); + dcmDynamicalSystem->setControlInput({vrp(Sl(), 0), omegaDot(Sl(), 0)}); + + std::tuple stateDerivative; + dcmDynamicalSystem->dynamics(0, stateDerivative); + + const auto& [initialDcmVelocity, initialOmegaVelocity] = stateDerivative; + this->opti.subject_to(casadi::MX::vertcat({initialDcmVelocity, initialOmegaVelocity}) == casadi::MX::vertcat({this->optiParameters.dcmInitialVelocity, 0})); // final values this->opti.subject_to(omega(Sl(), -1) == this->optiParameters.omegaFinalValue); this->opti.subject_to(dcm(Sl(), -1) == this->optiParameters.dcmFinalPosition); - this->opti.subject_to(casadi::MX::vertcat(this->optiFunctions.dcmDynamics( - {dcm(Sl(), -1), omega(Sl(), -1), vrp(Sl(), -1), omegaDot(Sl(), -1)})) - == casadi::MX::vertcat({this->optiParameters.dcmFinalVelocity, 0})); + + + dcmDynamicalSystem->setState({dcm(Sl(), -1), omega(Sl(), -1)}); + dcmDynamicalSystem->setControlInput({vrp(Sl(), -1), omegaDot(Sl(), -1)}); + + dcmDynamicalSystem->dynamics(0, stateDerivative); + const auto& [finalDcmVelocity, finalOmegaVelocity] = stateDerivative; + this->opti.subject_to(casadi::MX::vertcat({finalDcmVelocity, finalOmegaVelocity}) + == casadi::MX::vertcat({this->optiParameters.dcmFinalVelocity, 0})); for(std::size_t i = 0; i < horizonSampling - 1; i++) { + // initial value of the DCM dynamics + dcmDynamicalSystem->setState({dcm(Sl(), i), omega(Sl(), i)}); + dcmDynamicalSystem->setControlInput({vrp(Sl(), i), omegaDot(Sl(), i)}); + dcmDynamicsIntegrator->integrate(0, this->optiSettings.plannerSamplingTime); + + const auto& [dcmNextStep, omegaNextStep] = dcmDynamicsIntegrator->getSolution(); + // system dynamics - this->opti.subject_to( - casadi::MX::vertcat({dcm(Sl(), i + 1), omega(Sl(), i + 1)}) - == casadi::MX::vertcat({dcm(Sl(), i), omega(Sl(), i)}) - + this->optiSettings.plannerSamplingTime - * casadi::MX::vertcat(this->optiFunctions.dcmDynamics( - {dcm(Sl(), i), omega(Sl(), i), vrp(Sl(), i), omegaDot(Sl(), i)}))); + this->opti.subject_to(casadi::MX::vertcat({dcm(Sl(), i + 1), omega(Sl(), i + 1)}) + == casadi::MX::vertcat({dcmNextStep, omegaNextStep})); // omega has to be positive this->opti.subject_to(omega(Sl(), i) > 0); @@ -264,7 +302,6 @@ struct TimeVaryingDCMPlanner::Impl */ void setupCasADiFunctions() { - optiFunctions.dcmDynamics = getDCMDynamics(); optiFunctions.ecmp = getECMP(); } @@ -596,6 +633,11 @@ bool TimeVaryingDCMPlanner::initialize(std::weak_ptrdcmDynamicsIntegrator = std::make_unique>( + m_pimpl->optiSettings.plannerSamplingTime); + m_pimpl->dcmDynamicsIntegrator->setDynamicalSystem(std::make_shared()); + bool okCostFunctions = true; okCostFunctions &= ptr->getParameter("omega_dot_weight", m_pimpl->optiSettings.omegaDotWeight); okCostFunctions &= ptr->getParameter("dcm_tracking_weight", m_pimpl->optiSettings.dcmTrackingWeight); From 121a2a9dcd63a86a5e4341fbd5fba40f02448923 Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Tue, 14 Jul 2020 14:26:15 +0200 Subject: [PATCH 15/23] Bugfix in the TimeVaryingDCMPlanner::Impl::prepareSolution class --- src/Planners/src/TimeVaryingDCMPlanner.cpp | 17 +++++++++-------- 1 file changed, 9 insertions(+), 8 deletions(-) diff --git a/src/Planners/src/TimeVaryingDCMPlanner.cpp b/src/Planners/src/TimeVaryingDCMPlanner.cpp index 72c8f1af42..d355681347 100644 --- a/src/Planners/src/TimeVaryingDCMPlanner.cpp +++ b/src/Planners/src/TimeVaryingDCMPlanner.cpp @@ -553,21 +553,19 @@ struct TimeVaryingDCMPlanner::Impl void prepareSolution() { - std::size_t outputIndex = this->trajectoryIndex == numberOfTrajectorySamples - ? numberOfTrajectorySamples - 1 - : trajectoryIndex; + using Sl = casadi::Slice; std::memcpy(this->trajectory.dcmPosition.data(), - optiSolution.dcm.ptr(), + optiSolution.dcm(Sl(), this->trajectoryIndex).ptr(), sizeof(double) * this->dcmVectorSize); std::memcpy(this->trajectory.vrpPosition.data(), - optiSolution.vrp.ptr(), + optiSolution.vrp(Sl(), this->trajectoryIndex).ptr(), sizeof(double) * this->dcmVectorSize); - trajectory.omega = static_cast(optiSolution.omega(0, trajectoryIndex)); + trajectory.omega = static_cast(optiSolution.omega(0, this->trajectoryIndex)); - const double omegaDot = static_cast(optiSolution.omegaDot(0, outputIndex)); + const double omegaDot = static_cast(optiSolution.omegaDot(0, this->trajectoryIndex)); trajectory.dcmVelocity = (trajectory.omega - omegaDot / trajectory.omega) * (trajectory.dcmPosition - trajectory.vrpPosition); @@ -711,6 +709,9 @@ bool TimeVaryingDCMPlanner::computeTrajectory() m_pimpl->optiSolution.omega = m_pimpl->optiSolution.solution->value(m_pimpl->optiVariables.omega); m_pimpl->optiSolution.omegaDot = m_pimpl->optiSolution.solution->value(m_pimpl->optiVariables.omegaDot); + // reinitialize the trajectory + m_pimpl->trajectoryIndex = 0; + m_pimpl->prepareSolution(); m_pimpl->isTrajectoryComputed = true; @@ -740,7 +741,7 @@ bool TimeVaryingDCMPlanner::advance() return false; } - m_pimpl->trajectoryIndex = std::min(m_pimpl->trajectoryIndex + 1, m_pimpl->numberOfTrajectorySamples); + m_pimpl->trajectoryIndex = std::min(m_pimpl->trajectoryIndex + 1, m_pimpl->numberOfTrajectorySamples - 1); m_pimpl->prepareSolution(); From 6311dacb546dca9f7b1e98bcf6cb1e3d12a9b14e Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Tue, 14 Jul 2020 14:56:25 +0200 Subject: [PATCH 16/23] Add valgrind suppression for linux (related to casadi + ipopt) --- cmake/AddBipedalLocomotionUnitTest.cmake | 2 ++ cmake/valgrind-linux.supp | 18 ++++++++++++++++++ 2 files changed, 20 insertions(+) create mode 100644 cmake/valgrind-linux.supp diff --git a/cmake/AddBipedalLocomotionUnitTest.cmake b/cmake/AddBipedalLocomotionUnitTest.cmake index 92760370dd..82430fd823 100644 --- a/cmake/AddBipedalLocomotionUnitTest.cmake +++ b/cmake/AddBipedalLocomotionUnitTest.cmake @@ -25,6 +25,8 @@ if (FRAMEWORK_RUN_Valgrind_tests) set(MEMORYCHECK_COMMAND ${VALGRIND_PROGRAM}) if (APPLE) set(MEMORYCHECK_SUPPRESSIONS "--suppressions=${PROJECT_SOURCE_DIR}/cmake/valgrind-macos.supp") + elseif (UNIX AND NOT APPLE) + set(MEMORYCHECK_SUPPRESSIONS "--suppressions=${PROJECT_SOURCE_DIR}/cmake/valgrind-linux.supp") else () set(MEMORYCHECK_SUPPRESSIONS "") endif () diff --git a/cmake/valgrind-linux.supp b/cmake/valgrind-linux.supp new file mode 100644 index 0000000000..f88a12ad11 --- /dev/null +++ b/cmake/valgrind-linux.supp @@ -0,0 +1,18 @@ +{ + + Memcheck:Leak + match-leak-kinds: definite + fun:malloc + fun:get_num_procs + fun:blas_get_cpu_number + fun:gotoblas_init + fun:call_init + fun:_dl_init + fun:dl_open_worker + fun:_dl_catch_exception + fun:_dl_open + fun:dlopen_doit + fun:_dl_catch_exception + fun:_dl_catch_error + fun:_dlerror_run +} From 553489866c52871ddc52f82e36c75ccf4a6a042c Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Tue, 14 Jul 2020 21:26:10 +0200 Subject: [PATCH 17/23] Use vcpk_robotology v0.4.0 in GitHub action CI --- .github/workflows/ci.yml | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index a51d96e0c6..75446c1aae 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -9,7 +9,7 @@ on: - cron: '0 2 * * *' env: - vcpkg_robotology_TAG: v0.0.3 + vcpkg_robotology_TAG: v0.4.0 YCM_TAG: v0.11.0 YARP_TAG: 964bb26fa4791d83d72882711ea7509306248106 iDynTree_TAG: v1.1.0 @@ -53,10 +53,9 @@ jobs: # To avoid problems with non-relocatable packages, we unzip the archive exactly in the same C:/robotology/vcpkg # that has been used to create the pre-compiled archive cd C:/ - md C:/robotology - md C:/robotology/vcpkg wget https://github.com/robotology/robotology-superbuild-dependencies-vcpkg/releases/download/${env:vcpkg_robotology_TAG}/vcpkg-robotology.zip - unzip vcpkg-robotology.zip -d C:/robotology/vcpkg + unzip vcpkg-robotology.zip -d C:/ + rm vcpkg-robotology.zip # Overwrite the VCPKG_INSTALLATION_ROOT env variable defined by GitHub Actions to point to our vcpkg echo "::set-env name=VCPKG_INSTALLATION_ROOT::C:/robotology/vcpkg" From 1c05bb8397b92879b9ae0048a94f1293ee6f3b35 Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Wed, 15 Jul 2020 09:24:55 +0200 Subject: [PATCH 18/23] Attempt to fix GithHub action on Windows --- .github/workflows/ci.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index 75446c1aae..c92bbee27a 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -61,7 +61,7 @@ jobs: # Install Catch2 cd C:/robotology/vcpkg - ./vcpkg.exe install --triplet x64-windows catch2 + ./vcpkg.exe install --overlay-ports=C:/robotology/robotology-vcpkg-binary-ports catch2:x64-windows - name: Dependencies [macOS] if: matrix.os == 'macOS-latest' From a97e6dc52f49b320b5a8df54dcffcb2eaa245488 Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Wed, 15 Jul 2020 19:08:58 +0200 Subject: [PATCH 19/23] Add CasADi in the GitHub Action cache --- .github/workflows/ci.yml | 55 +++++++++++++++++++--------------------- 1 file changed, 26 insertions(+), 29 deletions(-) diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index c92bbee27a..12bdaff0e2 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -15,6 +15,7 @@ env: iDynTree_TAG: v1.1.0 Catch2_TAG: v2.11.3 Qhull_TAG: v8.0.0 + CasADi_TAG: 3.5.1 jobs: build: @@ -83,7 +84,7 @@ jobs: with: path: ${{ github.workspace }}/install/deps # Including ${{ runner.temp }} is a workaround taken from https://github.com/robotology/whole-body-estimators/pull/62 to fix macos configuration failure on https://github.com/dic-iit/bipedal-locomotion-framework/pull/45 - key: source-deps-${{ runner.os }}-${{runner.temp}}-vcpkg-robotology-${{ env.vcpkg_robotology_TAG }}-ycm-${{ env.YCM_TAG }}-yarp-${{ env.YARP_TAG }}-iDynTree-${{ env.iDynTree_TAG }}-catch2-${{ env.Catch2_TAG }}-qhull-${{ env.Qhull_TAG }} + key: source-deps-${{ runner.os }}-${{runner.temp}}-vcpkg-robotology-${{ env.vcpkg_robotology_TAG }}-ycm-${{ env.YCM_TAG }}-yarp-${{ env.YARP_TAG }}-iDynTree-${{ env.iDynTree_TAG }}-catch2-${{ env.Catch2_TAG }}-qhull-${{ env.Qhull_TAG }}-casADi-${{ env.CasADi_TAG }} - name: Source-based Dependencies [Windows] @@ -142,6 +143,20 @@ jobs: -DCMAKE_INSTALL_PREFIX=${GITHUB_WORKSPACE}/install/deps .. cmake --build . --config ${{ matrix.build_type }} --target install + # CasADi + cd ${GITHUB_WORKSPACE} + git clone https://github.com/casadi/casadi.git -b ${CasADi_TAG} casadi + cd casadi + mkdir build + cd build + cmake -A x64 -DCMAKE_TOOLCHAIN_FILE=${VCPKG_INSTALLATION_ROOT}/scripts/buildsystems/vcpkg.cmake \ + -DCMAKE_PREFIX_PATH=${GITHUB_WORKSPACE}/install/deps \ + -DCMAKE_BUILD_TYPE=${{ matrix.build_type }} \ + -DCMAKE_INSTALL_PREFIX=${GITHUB_WORKSPACE}/install/deps \ + -DWITH_IPOPT=BOOL:ON .. + cmake --build . --config ${{ matrix.build_type }} --target install + + - name: Source-based Dependencies [Ubuntu/macOS] if: steps.cache-source-deps.outputs.cache-hit != 'true' && (matrix.os == 'ubuntu-latest' || matrix.os == 'macOS-latest') shell: bash @@ -176,6 +191,16 @@ jobs: -DCMAKE_INSTALL_PREFIX=${GITHUB_WORKSPACE}/install/deps .. cmake --build . --config ${{ matrix.build_type }} --target install + # CasADi + cd ${GITHUB_WORKSPACE} + git clone https://github.com/casadi/casadi.git -b ${CasADi_TAG} casadi + cd casadi + mkdir build + cd build + cmake -DCMAKE_INSTALL_PREFIX=${GITHUB_WORKSPACE}/install/deps -DWITH_IPOPT=BOOL:ON .. + cmake --build . --config ${{ matrix.build_type }} --target install + + - name: Source-based Dependencies [Ubuntu] if: steps.cache-source-deps.outputs.cache-hit != 'true' && matrix.os == 'ubuntu-latest' shell: bash @@ -203,34 +228,6 @@ jobs: -DCMAKE_POSITION_INDEPENDENT_CODE=ON .. cmake --build . --config ${{ matrix.build_type }} --target install - - name: CasADi [Ubuntu/macOS] - if: matrix.os == 'ubuntu-latest' || matrix.os == 'macOS-latest' - shell: bash - run: | - cd ${GITHUB_WORKSPACE} - git clone https://github.com/casadi/casadi.git -b master casadi - cd casadi - mkdir build - cd build - cmake -DCMAKE_INSTALL_PREFIX=${GITHUB_WORKSPACE}/install/deps -DWITH_IPOPT=BOOL:ON .. - cmake --build . --config ${{ matrix.build_type }} --target install - - - name: CasADi [Windows] - if: matrix.os == 'windows-latest' - shell: bash - run: | - cd ${GITHUB_WORKSPACE} - git clone https://github.com/casadi/casadi.git -b master casadi - cd casadi - mkdir build - cd build - cmake -A x64 -DCMAKE_TOOLCHAIN_FILE=${VCPKG_INSTALLATION_ROOT}/scripts/buildsystems/vcpkg.cmake \ - -DCMAKE_PREFIX_PATH=${GITHUB_WORKSPACE}/install/deps \ - -DCMAKE_BUILD_TYPE=${{ matrix.build_type }} \ - -DCMAKE_INSTALL_PREFIX=${GITHUB_WORKSPACE}/install/deps \ - -DWITH_IPOPT=BOOL:ON .. - cmake --build . --config ${{ matrix.build_type }} --target install - # =================== # CMAKE-BASED PROJECT # =================== From 422e85734949a915d5eb4b51b4822dda72fbce06 Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Fri, 17 Jul 2020 13:02:06 +0200 Subject: [PATCH 20/23] Fix the compilation on Windows CI [casadi] --- .github/workflows/ci.yml | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index 12bdaff0e2..ae62fc04b0 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -144,6 +144,9 @@ jobs: cmake --build . --config ${{ matrix.build_type }} --target install # CasADi + # We override the casadi installation structure to be compliant with the folder structure used for + # the other dependencies. Please check: + # https://github.com/casadi/casadi/blob/6f122ca22e2a869903628c5738f154c8ac0f7455/CMakeLists.txt#L317 cd ${GITHUB_WORKSPACE} git clone https://github.com/casadi/casadi.git -b ${CasADi_TAG} casadi cd casadi @@ -153,6 +156,10 @@ jobs: -DCMAKE_PREFIX_PATH=${GITHUB_WORKSPACE}/install/deps \ -DCMAKE_BUILD_TYPE=${{ matrix.build_type }} \ -DCMAKE_INSTALL_PREFIX=${GITHUB_WORKSPACE}/install/deps \ + -DINCLUDE_PREFIX:PATH="include" \ + -DCMAKE_PREFIX:PATH="lib/cmake/casadi" \ + -DLIB_PREFIX:PATH="lib" \ + -DBIN_PREFIX:PATH="bin" \ -DWITH_IPOPT=BOOL:ON .. cmake --build . --config ${{ matrix.build_type }} --target install From 995a9256528dab15719d481293cd049d257ac94c Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Fri, 17 Jul 2020 14:12:51 +0200 Subject: [PATCH 21/23] Use the alias TimeVaryingDCMPlannerDynamics::StateDerivativeType instead of std::tuple<> in TimeVaryingDCMPlanner --- src/Planners/src/TimeVaryingDCMPlanner.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/Planners/src/TimeVaryingDCMPlanner.cpp b/src/Planners/src/TimeVaryingDCMPlanner.cpp index d355681347..2d2a5f08d0 100644 --- a/src/Planners/src/TimeVaryingDCMPlanner.cpp +++ b/src/Planners/src/TimeVaryingDCMPlanner.cpp @@ -239,7 +239,7 @@ struct TimeVaryingDCMPlanner::Impl dcmDynamicalSystem->setState({dcm(Sl(), 0), omega(Sl(), 0)}); dcmDynamicalSystem->setControlInput({vrp(Sl(), 0), omegaDot(Sl(), 0)}); - std::tuple stateDerivative; + TimeVaryingDCMPlannerDynamics::StateDerivativeType stateDerivative; dcmDynamicalSystem->dynamics(0, stateDerivative); const auto& [initialDcmVelocity, initialOmegaVelocity] = stateDerivative; From 6fb1de6774b16a94415c4a9daa0f692ed63840ce Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Fri, 17 Jul 2020 14:15:01 +0200 Subject: [PATCH 22/23] Use shared_ptr instead of weak_ptr in DCMPlanner::initialize() function --- .../BipedalLocomotion/Planners/DCMPlanner.h | 2 +- .../Planners/TimeVaryingDCMPlanner.h | 2 +- src/Planners/src/DCMPlanner.cpp | 2 +- src/Planners/src/TimeVaryingDCMPlanner.cpp | 40 +++++++++---------- 4 files changed, 22 insertions(+), 24 deletions(-) diff --git a/src/Planners/include/BipedalLocomotion/Planners/DCMPlanner.h b/src/Planners/include/BipedalLocomotion/Planners/DCMPlanner.h index b7086bac26..27506b8444 100644 --- a/src/Planners/include/BipedalLocomotion/Planners/DCMPlanner.h +++ b/src/Planners/include/BipedalLocomotion/Planners/DCMPlanner.h @@ -52,7 +52,7 @@ class DCMPlanner : public BipedalLocomotion::System::Advanceable handler); + virtual bool initialize(std::shared_ptr handler); /** * Set the initial state of the planner diff --git a/src/Planners/include/BipedalLocomotion/Planners/TimeVaryingDCMPlanner.h b/src/Planners/include/BipedalLocomotion/Planners/TimeVaryingDCMPlanner.h index e9e4be8513..578264b9c0 100644 --- a/src/Planners/include/BipedalLocomotion/Planners/TimeVaryingDCMPlanner.h +++ b/src/Planners/include/BipedalLocomotion/Planners/TimeVaryingDCMPlanner.h @@ -46,7 +46,7 @@ class TimeVaryingDCMPlanner : public DCMPlanner * @param handler pointer to the parameter handler. * @return true in case of success/false otherwise. */ - bool initialize(std::weak_ptr handler) override; + bool initialize(std::shared_ptr handler) override; /** * Compute the DCM trajectory. diff --git a/src/Planners/src/DCMPlanner.cpp b/src/Planners/src/DCMPlanner.cpp index 7e1242d163..eab73c4545 100644 --- a/src/Planners/src/DCMPlanner.cpp +++ b/src/Planners/src/DCMPlanner.cpp @@ -9,7 +9,7 @@ using namespace BipedalLocomotion::Planners; -bool DCMPlanner::initialize(std::weak_ptr handler) +bool DCMPlanner::initialize(std::shared_ptr handler) { return true; } diff --git a/src/Planners/src/TimeVaryingDCMPlanner.cpp b/src/Planners/src/TimeVaryingDCMPlanner.cpp index 2d2a5f08d0..62c8d5d562 100644 --- a/src/Planners/src/TimeVaryingDCMPlanner.cpp +++ b/src/Planners/src/TimeVaryingDCMPlanner.cpp @@ -580,30 +580,28 @@ TimeVaryingDCMPlanner::TimeVaryingDCMPlanner() TimeVaryingDCMPlanner::~TimeVaryingDCMPlanner() = default; -bool TimeVaryingDCMPlanner::initialize(std::weak_ptr handler) +bool TimeVaryingDCMPlanner::initialize(std::shared_ptr handler) { assert(m_pimpl); - // convert the weak_ptr into a shared_ptr - auto ptr = handler.lock(); - if (ptr == nullptr) + if (handler == nullptr) { - std::cerr << "[TimeVaryingDCMPlanner::initialize] The pointer to the parameter handler is " - "expired." + std::cerr << "[TimeVaryingDCMPlanner::initialize] The handler has to point to an already " + "initialized IParametershandler." << std::endl; return false; } - if (!ptr->getParameter("planner_sampling_time", m_pimpl->optiSettings.plannerSamplingTime)) + if (!handler->getParameter("planner_sampling_time", m_pimpl->optiSettings.plannerSamplingTime)) { - std::cerr << "[TimeVaryingDCMPlanner::initialize] Unable to load the sampling time of the " - "planner." + std::cerr << "[TimeVaryingDCMPlanner::initialize] Unable to load the sampling time of " + "the planner." << std::endl; return false; } int numberOfFootCorners; - if (!ptr->getParameter("number_of_foot_corners", numberOfFootCorners)) + if (!handler->getParameter("number_of_foot_corners", numberOfFootCorners)) { std::cerr << "[TimeVaryingDCMPlanner::initialize] Unable to load the number of foot " "corners." @@ -615,8 +613,8 @@ bool TimeVaryingDCMPlanner::initialize(std::weak_ptrgetParameter("foot_corner_" + std::to_string(i), - m_pimpl->optiSettings.footCorners[i])) + if (!handler->getParameter("foot_corner_" + std::to_string(i), + m_pimpl->optiSettings.footCorners[i])) { std::cerr << "[TimeVaryingDCMPlanner::initialize] Unable to load get the foot corner " "number: " @@ -637,15 +635,15 @@ bool TimeVaryingDCMPlanner::initialize(std::weak_ptrdcmDynamicsIntegrator->setDynamicalSystem(std::make_shared()); bool okCostFunctions = true; - okCostFunctions &= ptr->getParameter("omega_dot_weight", m_pimpl->optiSettings.omegaDotWeight); - okCostFunctions &= ptr->getParameter("dcm_tracking_weight", m_pimpl->optiSettings.dcmTrackingWeight); - okCostFunctions &= ptr->getParameter("omega_dot_rate_of_change_weight", - m_pimpl->optiSettings.omegaDotRateOfChangeWeight); - okCostFunctions &= ptr->getParameter("vrp_rate_of_change_weight", - m_pimpl->optiSettings.vrpRateOfChangeWeight); - - okCostFunctions &= ptr->getParameter("dcm_rate_of_change_weight", - m_pimpl->optiSettings.dcmRateOfChangeWeight); + okCostFunctions &= handler->getParameter("omega_dot_weight", m_pimpl->optiSettings.omegaDotWeight); + okCostFunctions &= handler->getParameter("dcm_tracking_weight", m_pimpl->optiSettings.dcmTrackingWeight); + okCostFunctions &= handler->getParameter("omega_dot_rate_of_change_weight", + m_pimpl->optiSettings.omegaDotRateOfChangeWeight); + okCostFunctions &= handler->getParameter("vrp_rate_of_change_weight", + m_pimpl->optiSettings.vrpRateOfChangeWeight); + + okCostFunctions &= handler->getParameter("dcm_rate_of_change_weight", + m_pimpl->optiSettings.dcmRateOfChangeWeight); if (!okCostFunctions) { From 7aaa1cb0786a9b01dcd3fcf9fc27b22d32a95bb2 Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Fri, 17 Jul 2020 14:36:59 +0200 Subject: [PATCH 23/23] Use shared_ptr instead of weak_ptr in DCMPlanner::setContactPhaseList() function --- .../include/BipedalLocomotion/Planners/DCMPlanner.h | 2 +- src/Planners/src/DCMPlanner.cpp | 6 +++--- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/src/Planners/include/BipedalLocomotion/Planners/DCMPlanner.h b/src/Planners/include/BipedalLocomotion/Planners/DCMPlanner.h index 27506b8444..22be37f455 100644 --- a/src/Planners/include/BipedalLocomotion/Planners/DCMPlanner.h +++ b/src/Planners/include/BipedalLocomotion/Planners/DCMPlanner.h @@ -68,7 +68,7 @@ class DCMPlanner : public BipedalLocomotion::System::Advanceable contactPhaseList); + bool setContactPhaseList(std::shared_ptr contactPhaseList); /** * Compute the DCM trajectory. diff --git a/src/Planners/src/DCMPlanner.cpp b/src/Planners/src/DCMPlanner.cpp index eab73c4545..ca660bd8d1 100644 --- a/src/Planners/src/DCMPlanner.cpp +++ b/src/Planners/src/DCMPlanner.cpp @@ -14,9 +14,9 @@ bool DCMPlanner::initialize(std::shared_ptr contactPhaseList) +bool DCMPlanner::setContactPhaseList(std::shared_ptr contactPhaseList) { - if(contactPhaseList.expired()) + if(contactPhaseList == nullptr) { std::cerr << "[DCMPlanner::setContactPhaseList] The contactPhaseList pointer is expired. " "Please pass a valid pointer." @@ -24,7 +24,7 @@ bool DCMPlanner::setContactPhaseList(std::weak_ptr conta return false; } - m_contactPhaseList = contactPhaseList.lock(); + m_contactPhaseList = contactPhaseList; return true; }