From 27aada11546e6be4734ec74a4d69e456992a3127 Mon Sep 17 00:00:00 2001 From: Emilio Date: Tue, 3 Mar 2020 17:57:58 +0100 Subject: [PATCH 1/5] Added weight retrieval function --- .../TrajectoryPlanner/TrajectoryGenerator.h | 428 ++++--- .../src/TrajectoryGenerator.cpp | 1070 +++++++++-------- 2 files changed, 786 insertions(+), 712 deletions(-) diff --git a/src/TrajectoryPlanner/include/WalkingControllers/TrajectoryPlanner/TrajectoryGenerator.h b/src/TrajectoryPlanner/include/WalkingControllers/TrajectoryPlanner/TrajectoryGenerator.h index bab70335..a603e030 100644 --- a/src/TrajectoryPlanner/include/WalkingControllers/TrajectoryPlanner/TrajectoryGenerator.h +++ b/src/TrajectoryPlanner/include/WalkingControllers/TrajectoryPlanner/TrajectoryGenerator.h @@ -6,14 +6,13 @@ * @date 2018 */ - #ifndef WALKING_CONTROLLERS_TRAJECTORY_PLANNER_TRAJECTORY_GENERATOR_H #define WALKING_CONTROLLERS_TRAJECTORY_PLANNER_TRAJECTORY_GENERATOR_H // std -#include #include #include +#include // YARP #include @@ -23,203 +22,248 @@ #include -namespace WalkingControllers -{ +namespace WalkingControllers { /** * Enumerator useful to track the state of the trajectory generator */ - enum class GeneratorState {NotConfigured, Configured, FirstStep, Called, Returned, Closing}; +enum class GeneratorState { + NotConfigured, + Configured, + FirstStep, + Called, + Returned, + Closing +}; /** - * TrajectoryGenerator class is used to handle the UnicycleTrajectoryGenerator library. + * TrajectoryGenerator class is used to handle the UnicycleTrajectoryGenerator + * library. */ - class TrajectoryGenerator - { - UnicycleGenerator m_trajectoryGenerator; /**< UnicycleTrajectoryGenerator object. */ - std::shared_ptr m_dcmGenerator; - std::shared_ptr m_heightGenerator; - std::shared_ptr m_feetGenerator; - bool m_useMinimumJerk; - - bool m_swingLeft; /**< True if the first swing foot is the left. */ - - double m_dT; /**< Sampling time of the planner. */ - double m_plannerHorizon; /**< Horizon of the planner. */ - - double m_nominalWidth; /**< Nominal width between two feet. */ - double m_initTime; /**< Init time of the current trajectory. */ - - iDynTree::Vector2 m_referencePointDistance; /**< Vector between the center of the unicycle and the point that has to be reach the goal. */ - - GeneratorState m_generatorState{GeneratorState::NotConfigured}; /**< Useful to track the generator state. */ - - std::thread m_generatorThread; /**< Main trajectory thread. */ - std::condition_variable m_conditionVariable; /**< Synchronizer. */ - - bool m_correctLeft; /**< The left foot has to be corrected. */ - iDynTree::Transform m_measuredTransformLeft; /**< Measured transformation between the left foot and the world frame. (w_H_lf) */ - iDynTree::Transform m_measuredTransformRight; /**< Measured transformation between the right foot and the world frame. (w_H_rf) */ - - iDynTree::Vector2 m_desiredPoint; /**< Desired final position of the x-y projection of the CoM. */ - - iDynTree::Vector2 m_DCMBoundaryConditionAtMergePointPosition; /**< DCM position at the merge point. */ - iDynTree::Vector2 m_DCMBoundaryConditionAtMergePointVelocity; /**< DCM velocity at the merge point. */ - - std::mutex m_mutex; /**< Mutex. */ - - /** - * Main thread method. - */ - void computeThread(); - - public: - - /** - * Deconstructor. - */ - ~TrajectoryGenerator(); - - /** - * Initialize the trajectory generator - * @param config yarp searchable object. - * @return true/false in case of success/failure. - */ - bool initialize(const yarp::os::Searchable& config); - - /** - * Configure the planner. - * @param config yarp searchable object. - * @return true/false in case of success/failure. - */ - bool configurePlanner(const yarp::os::Searchable& config); - - /** - * Generate the first trajectory. - * This method has to be called before updateTrajectories() method - * @return true/false in case of success/failure. - */ - bool generateFirstTrajectories(); - - /** - * Generate the first trajectory. - * This method has to be called before only by the ontTheFly method. - * @param leftToRightTransform transformation between from the left foot to the right foot; - * @return true/false in case of success/failure. - */ - bool generateFirstTrajectories(const iDynTree::Transform &leftToRightTransform); - // const iDynTree::Position &initialCOMPosition); - - /** - * Update the trajectory. - * The old trajectory will be deleted and a new one is evaluated. The boundary condition of the new trajectory is given by - * the position and the velocity of the DCM at the merge point. - * This method allows you to take into account the real position one foot at the beginning of the trajectory. - * @param initTime is the initial time of the trajectory; - * @param DCMBoundaryConditionAtMergePointPosition is the position of the DCM at the merge point; - * @param DCMBoundaryConditionAtMergePointVelocity is the velocity of the DCM at the merge point; - * @param correctLeft todo; - * @param measured Measured transformation between the stance foot and the world frame. (w_H_{stancefoot}); - * @param desiredPosition final desired position of the projection of the CoM. - * @return true/false in case of success/failure. - */ - bool updateTrajectories(double initTime, const iDynTree::Vector2& DCMBoundaryConditionAtMergePointPosition, - const iDynTree::Vector2& DCMBoundaryConditionAtMergePointVelocity, bool correctLeft, - const iDynTree::Transform& measured, const iDynTree::Vector2& desiredPosition); - - /** - * Return if the trajectory was computed - * @return true if the trajectory has been computed false otherwise. - */ - bool isTrajectoryComputed(); - - /** - * Configure the planner in order to add or not the terminal step - * @param terminalStep if it true the terminal step will be added - */ - void addTerminalStep(bool terminalStep); - - /** - * Return if a new trajectory is asked. - * @return true if the trajectory has already asked. - */ - bool isTrajectoryAsked(); - - /** - * Get the desired 2D-DCM position trajectory - * @param DCMPositionTrajectory desired trajectory of the DCM. - * @return true/false in case of success/failure. - */ - bool getDCMPositionTrajectory(std::vector& DCMPositionTrajectory); - - /** - * Get the desired 2D-DCM velocity trajectory - * @param DCMVelocityTrajectory desired trajectory of the DCM. - * @return true/false in case of success/failure. - */ - bool getDCMVelocityTrajectory(std::vector& DCMVelocityTrajectory); - - /** - * Get the feet trajectory - * @param lFootTrajectory vector containing the left foot trajectory; - * @param rFootTrajectory vector containing the right foot trajectory. - * @return true/false in case of success/failure. - */ - bool getFeetTrajectories(std::vector& lFootTrajectory, - std::vector& rFootTrajectory); - - - /** - * Get the feet twist - * @param lFootTwist vector containing the left foot twists; - * @param rFootTwist vector containing the right foot twists. - * @return true/false in case of success/failure. - */ - bool getFeetTwist(std::vector& lFootTwist, - std::vector& rFootTwist); - /** - * Get the when the main frame of the left foot is the fix frame. - * @param isLeftFixedFrame vector containing when the main frame of - * the left foot is the fix frame. - * @return true/false in case of success/failure. - */ - bool getWhenUseLeftAsFixed(std::vector& isLeftFixedFrame); - - /** - * Get the feet phases - * @param lFootContacts vector containing the state of the left foot (true = in contact); - * @param rFootContacts vector containing the state of the right foot (true = in contact). - * @return true/false in case of success/failure. - */ - bool getFeetStandingPeriods(std::vector& lFootContacts, - std::vector& rFootContacts); - - /** - * Get the CoM height trajectory - * @param CoMHeightTrajectory vector containing trajectory of the COM on the z axis. - * @return true/false in case of success/failure. - */ - bool getCoMHeightTrajectory(std::vector& CoMHeightTrajectory); - - /** - * Get the CoM height velocity - * @param CoMHeightVelocity vector containing the velocity of the COM on the z axis. - * @return true/false in case of success/failure. - */ - bool getCoMHeightVelocity(std::vector& CoMHeightVelocity); - - /** - * Get the merge points along the trajectory - * @param mergePoints vector containing all the merge points of the trajectory. - * @return true/false in case of success/failure. - */ - bool getMergePoints(std::vector& mergePoints); - - /** - * Reset the planner - */ - void reset(); - }; +class TrajectoryGenerator { + UnicycleGenerator + m_trajectoryGenerator; /**< UnicycleTrajectoryGenerator object. */ + std::shared_ptr m_dcmGenerator; + std::shared_ptr m_heightGenerator; + std::shared_ptr m_feetGenerator; + bool m_useMinimumJerk; + + bool m_swingLeft; /**< True if the first swing foot is the left. */ + + double m_dT; /**< Sampling time of the planner. */ + double m_plannerHorizon; /**< Horizon of the planner. */ + + double m_nominalWidth; /**< Nominal width between two feet. */ + double m_initTime; /**< Init time of the current trajectory. */ + + iDynTree::Vector2 m_referencePointDistance; /**< Vector between the center of + the unicycle and the point that + has to be reach the goal. */ + + GeneratorState m_generatorState{ + GeneratorState::NotConfigured}; /**< Useful to track the generator state. + */ + + std::thread m_generatorThread; /**< Main trajectory thread. */ + std::condition_variable m_conditionVariable; /**< Synchronizer. */ + + bool m_correctLeft; /**< The left foot has to be corrected. */ + iDynTree::Transform + m_measuredTransformLeft; /**< Measured transformation between the left + foot and the world frame. (w_H_lf) */ + iDynTree::Transform + m_measuredTransformRight; /**< Measured transformation between the right + foot and the world frame. (w_H_rf) */ + + iDynTree::Vector2 m_desiredPoint; /**< Desired final position of the x-y + projection of the CoM. */ + + iDynTree::Vector2 + m_DCMBoundaryConditionAtMergePointPosition; /**< DCM position at the merge + point. */ + iDynTree::Vector2 + m_DCMBoundaryConditionAtMergePointVelocity; /**< DCM velocity at the merge + point. */ + + std::mutex m_mutex; /**< Mutex. */ + + /** + * Main thread method. + */ + void computeThread(); + +public: + /** + * Deconstructor. + */ + ~TrajectoryGenerator(); + + /** + * Initialize the trajectory generator + * @param config yarp searchable object. + * @return true/false in case of success/failure. + */ + bool initialize(const yarp::os::Searchable &config); + + /** + * Configure the planner. + * @param config yarp searchable object. + * @return true/false in case of success/failure. + */ + bool configurePlanner(const yarp::os::Searchable &config); + + /** + * Generate the first trajectory. + * This method has to be called before updateTrajectories() method + * @return true/false in case of success/failure. + */ + bool generateFirstTrajectories(); + + /** + * Generate the first trajectory. + * This method has to be called before only by the ontTheFly method. + * @param leftToRightTransform transformation between from the left foot to + * the right foot; + * @return true/false in case of success/failure. + */ + bool + generateFirstTrajectories(const iDynTree::Transform &leftToRightTransform); + // const iDynTree::Position &initialCOMPosition); + + /** + * Update the trajectory. + * The old trajectory will be deleted and a new one is evaluated. The boundary + * condition of the new trajectory is given by the position and the velocity + * of the DCM at the merge point. This method allows you to take into account + * the real position one foot at the beginning of the trajectory. + * @param initTime is the initial time of the trajectory; + * @param DCMBoundaryConditionAtMergePointPosition is the position of the DCM + * at the merge point; + * @param DCMBoundaryConditionAtMergePointVelocity is the velocity of the DCM + * at the merge point; + * @param correctLeft todo; + * @param measured Measured transformation between the stance foot and the + * world frame. (w_H_{stancefoot}); + * @param desiredPosition final desired position of the projection of the CoM. + * @return true/false in case of success/failure. + */ + bool updateTrajectories( + double initTime, + const iDynTree::Vector2 &DCMBoundaryConditionAtMergePointPosition, + const iDynTree::Vector2 &DCMBoundaryConditionAtMergePointVelocity, + bool correctLeft, const iDynTree::Transform &measured, + const iDynTree::Vector2 &desiredPosition); + + /** + * Return if the trajectory was computed + * @return true if the trajectory has been computed false otherwise. + */ + bool isTrajectoryComputed(); + + /** + * Configure the planner in order to add or not the terminal step + * @param terminalStep if it true the terminal step will be added + */ + void addTerminalStep(bool terminalStep); + + /** + * Return if a new trajectory is asked. + * @return true if the trajectory has already asked. + */ + bool isTrajectoryAsked(); + + /** + * Get the desired 2D-DCM position trajectory + * @param DCMPositionTrajectory desired trajectory of the DCM. + * @return true/false in case of success/failure. + */ + bool getDCMPositionTrajectory( + std::vector &DCMPositionTrajectory); + + /** + * Get the desired 2D-DCM velocity trajectory + * @param DCMVelocityTrajectory desired trajectory of the DCM. + * @return true/false in case of success/failure. + */ + bool getDCMVelocityTrajectory( + std::vector &DCMVelocityTrajectory); + + /** + * Get the feet trajectory + * @param lFootTrajectory vector containing the left foot trajectory; + * @param rFootTrajectory vector containing the right foot trajectory. + * @return true/false in case of success/failure. + */ + bool getFeetTrajectories(std::vector &lFootTrajectory, + std::vector &rFootTrajectory); + + /** + * Get the feet twist + * @param lFootTwist vector containing the left foot twists; + * @param rFootTwist vector containing the right foot twists. + * @return true/false in case of success/failure. + */ + bool getFeetTwist(std::vector &lFootTwist, + std::vector &rFootTwist); + /** + * Get the when the main frame of the left foot is the fix frame. + * @param isLeftFixedFrame vector containing when the main frame of + * the left foot is the fix frame. + * @return true/false in case of success/failure. + */ + bool getWhenUseLeftAsFixed(std::vector &isLeftFixedFrame); + + /** + * Get the feet phases + * @param lFootContacts vector containing the state of the left foot (true = + * in contact); + * @param rFootContacts vector containing the state of the right foot (true = + * in contact). + * @return true/false in case of success/failure. + */ + bool getFeetStandingPeriods(std::vector &lFootContacts, + std::vector &rFootContacts); + + /** + * Get the CoM height trajectory + * @param CoMHeightTrajectory vector containing trajectory of the COM on the z + * axis. + * @return true/false in case of success/failure. + */ + bool getCoMHeightTrajectory(std::vector &CoMHeightTrajectory); + + /** + * Get the CoM height velocity + * @param CoMHeightVelocity vector containing the velocity of the COM on the z + * axis. + * @return true/false in case of success/failure. + */ + bool getCoMHeightVelocity(std::vector &CoMHeightVelocity); + + /** + * Get the merge points along the trajectory + * @param mergePoints vector containing all the merge points of the + * trajectory. + * @return true/false in case of success/failure. + */ + bool getMergePoints(std::vector &mergePoints); + + /** + * Get the weight percentage for the left and right foot + * @param weightInLeft vector containing the weight on the left foot (0 in + * case in case of stance foot during SS, 1 in case of swing foot) + * @param weightInRight vector containing the weight on the right foot (0 in + * case in case of stance foot during SS, 1 in case of swing foot) + * @return true/false in case of success/failure. + */ + bool getWeightPercentage(std::vector &weightInLeft, + std::vector &weightInRight); + + /** + * Reset the planner + */ + void reset(); }; +}; // namespace WalkingControllers #endif diff --git a/src/TrajectoryPlanner/src/TrajectoryGenerator.cpp b/src/TrajectoryPlanner/src/TrajectoryGenerator.cpp index debbd022..4f3372fb 100644 --- a/src/TrajectoryPlanner/src/TrajectoryGenerator.cpp +++ b/src/TrajectoryPlanner/src/TrajectoryGenerator.cpp @@ -18,603 +18,633 @@ using namespace WalkingControllers; -TrajectoryGenerator::~TrajectoryGenerator() -{ - { - std::lock_guard guard(m_mutex); - m_generatorState = GeneratorState::Closing; - m_conditionVariable.notify_one(); - } +TrajectoryGenerator::~TrajectoryGenerator() { + { + std::lock_guard guard(m_mutex); + m_generatorState = GeneratorState::Closing; + m_conditionVariable.notify_one(); + } - if(m_generatorThread.joinable()) - { - m_generatorThread.join(); - m_generatorThread = std::thread(); - } + if (m_generatorThread.joinable()) { + m_generatorThread.join(); + m_generatorThread = std::thread(); + } } -bool TrajectoryGenerator::initialize(const yarp::os::Searchable& config) -{ - if(!configurePlanner(config)) - { - yError() << "[initialize] Failed to configure the unicycle trajectory generator."; - return false; - } - return true; +bool TrajectoryGenerator::initialize(const yarp::os::Searchable &config) { + if (!configurePlanner(config)) { + yError() << "[initialize] Failed to configure the unicycle trajectory " + "generator."; + return false; + } + return true; } -bool TrajectoryGenerator::configurePlanner(const yarp::os::Searchable& config) -{ - if(config.isNull()) - { - yError() << "[configurePlanner] Empty configuration for unicycle planner."; - return false; - } - - - m_dT = config.check("sampling_time", yarp::os::Value(0.016)).asDouble(); - m_plannerHorizon = config.check("plannerHorizon", yarp::os::Value(20.0)).asDouble(); - double unicycleGain = config.check("unicycleGain", yarp::os::Value(10.0)).asDouble(); - - if(!YarpUtilities::getVectorFromSearchable(config, "referencePosition", m_referencePointDistance)) - { - yError() << "[configurePlanner] Initialization failed while reading referencePosition vector."; - return false; - } - - // get left and right ZMP delta - iDynTree::Vector2 leftZMPDelta; - if(!YarpUtilities::getVectorFromSearchable(config, "leftZMPDelta", leftZMPDelta)) - { - yError() << "[configurePlanner] Initialization failed while reading rStancePosition vector."; - return false; - } - - iDynTree::Vector2 rightZMPDelta; - if(!YarpUtilities::getVectorFromSearchable(config, "rightZMPDelta", rightZMPDelta)) - { - yError() << "[configurePlanner] Initialization failed while reading rStancePosition vector."; - return false; - } - - double timeWeight = config.check("timeWeight", yarp::os::Value(2.5)).asDouble(); - double positionWeight = config.check("positionWeight", yarp::os::Value(1.0)).asDouble(); - double slowWhenTurningGain = config.check("slowWhenTurningGain", yarp::os::Value(0.0)).asDouble(); - double maxStepLength = config.check("maxStepLength", yarp::os::Value(0.05)).asDouble(); - double minStepLength = config.check("minStepLength", yarp::os::Value(0.005)).asDouble(); - double minWidth = config.check("minWidth", yarp::os::Value(0.03)).asDouble(); - double maxAngleVariation = iDynTree::deg2rad(config.check("maxAngleVariation", - yarp::os::Value(40.0)).asDouble()); - double minAngleVariation = iDynTree::deg2rad(config.check("minAngleVariation", - yarp::os::Value(5.0)).asDouble()); - double maxStepDuration = config.check("maxStepDuration", yarp::os::Value(8.0)).asDouble(); - double minStepDuration = config.check("minStepDuration", yarp::os::Value(2.9)).asDouble(); - double stepHeight = config.check("stepHeight", yarp::os::Value(0.005)).asDouble(); - double landingVelocity = config.check("stepLandingVelocity", yarp::os::Value(0.0)).asDouble(); - double apexTime = config.check("footApexTime", yarp::os::Value(0.5)).asDouble(); - double comHeight = config.check("com_height", yarp::os::Value(0.49)).asDouble(); - double comHeightDelta = config.check("comHeightDelta", yarp::os::Value(0.01)).asDouble(); - double nominalDuration = config.check("nominalDuration", yarp::os::Value(4.0)).asDouble(); - double lastStepSwitchTime = config.check("lastStepSwitchTime", yarp::os::Value(0.5)).asDouble(); - double switchOverSwingRatio = config.check("switchOverSwingRatio", - yarp::os::Value(0.4)).asDouble(); - double mergePointRatio = config.check("mergePointRatio", yarp::os::Value(0.5)).asDouble(); - - m_nominalWidth = config.check("nominalWidth", yarp::os::Value(0.04)).asDouble(); - - m_swingLeft = config.check("swingLeft", yarp::os::Value(true)).asBool(); - bool startWithSameFoot = config.check("startAlwaysSameFoot", yarp::os::Value(false)).asBool(); - m_useMinimumJerk = config.check("useMinimumJerkFootTrajectory", - yarp::os::Value(false)).asBool(); - double pitchDelta = config.check("pitchDelta", yarp::os::Value(0.0)).asDouble(); - - // try to configure the planner - std::shared_ptr unicyclePlanner = m_trajectoryGenerator.unicyclePlanner(); - bool ok = true; - ok = ok && unicyclePlanner->setDesiredPersonDistance(m_referencePointDistance(0), - m_referencePointDistance(1)); - ok = ok && unicyclePlanner->setControllerGain(unicycleGain); - ok = ok && unicyclePlanner->setMaximumIntegratorStepSize(m_dT); - ok = ok && unicyclePlanner->setMaxStepLength(maxStepLength); - ok = ok && unicyclePlanner->setWidthSetting(minWidth, m_nominalWidth); - ok = ok && unicyclePlanner->setMaxAngleVariation(maxAngleVariation); - ok = ok && unicyclePlanner->setCostWeights(positionWeight, timeWeight); - ok = ok && unicyclePlanner->setStepTimings(minStepDuration, - maxStepDuration, nominalDuration); - ok = ok && unicyclePlanner->setPlannerPeriod(m_dT); - ok = ok && unicyclePlanner->setMinimumAngleForNewSteps(minAngleVariation); - ok = ok && unicyclePlanner->setMinimumStepLength(minStepLength); - ok = ok && unicyclePlanner->setSlowWhenTurnGain(slowWhenTurningGain); - unicyclePlanner->addTerminalStep(false); - unicyclePlanner->startWithLeft(m_swingLeft); - unicyclePlanner->resetStartingFootIfStill(startWithSameFoot); - - ok = ok && m_trajectoryGenerator.setSwitchOverSwingRatio(switchOverSwingRatio); - ok = ok && m_trajectoryGenerator.setTerminalHalfSwitchTime(lastStepSwitchTime); - ok = ok && m_trajectoryGenerator.setPauseConditions(maxStepDuration, nominalDuration); - - if (m_useMinimumJerk) { - m_feetGenerator = m_trajectoryGenerator.addFeetMinimumJerkGenerator(); - } else { - m_feetGenerator = m_trajectoryGenerator.addFeetCubicSplineGenerator(); - } - ok = ok && m_feetGenerator->setStepHeight(stepHeight); - ok = ok && m_feetGenerator->setFootLandingVelocity(landingVelocity); - ok = ok && m_feetGenerator->setFootApexTime(apexTime); - ok = ok && m_feetGenerator->setPitchDelta(pitchDelta); - - m_heightGenerator = m_trajectoryGenerator.addCoMHeightTrajectoryGenerator(); - ok = ok && m_heightGenerator->setCoMHeightSettings(comHeight, comHeightDelta); - ok = ok && m_trajectoryGenerator.setMergePointRatio(mergePointRatio); +bool TrajectoryGenerator::configurePlanner(const yarp::os::Searchable &config) { + if (config.isNull()) { + yError() << "[configurePlanner] Empty configuration for unicycle planner."; + return false; + } + + m_dT = config.check("sampling_time", yarp::os::Value(0.016)).asDouble(); + m_plannerHorizon = + config.check("plannerHorizon", yarp::os::Value(20.0)).asDouble(); + double unicycleGain = + config.check("unicycleGain", yarp::os::Value(10.0)).asDouble(); + + if (!YarpUtilities::getVectorFromSearchable(config, "referencePosition", + m_referencePointDistance)) { + yError() << "[configurePlanner] Initialization failed while reading " + "referencePosition vector."; + return false; + } + + // get left and right ZMP delta + iDynTree::Vector2 leftZMPDelta; + if (!YarpUtilities::getVectorFromSearchable(config, "leftZMPDelta", + leftZMPDelta)) { + yError() << "[configurePlanner] Initialization failed while reading " + "rStancePosition vector."; + return false; + } + + iDynTree::Vector2 rightZMPDelta; + if (!YarpUtilities::getVectorFromSearchable(config, "rightZMPDelta", + rightZMPDelta)) { + yError() << "[configurePlanner] Initialization failed while reading " + "rStancePosition vector."; + return false; + } + + double timeWeight = + config.check("timeWeight", yarp::os::Value(2.5)).asDouble(); + double positionWeight = + config.check("positionWeight", yarp::os::Value(1.0)).asDouble(); + double slowWhenTurningGain = + config.check("slowWhenTurningGain", yarp::os::Value(0.0)).asDouble(); + double maxStepLength = + config.check("maxStepLength", yarp::os::Value(0.05)).asDouble(); + double minStepLength = + config.check("minStepLength", yarp::os::Value(0.005)).asDouble(); + double minWidth = config.check("minWidth", yarp::os::Value(0.03)).asDouble(); + double maxAngleVariation = iDynTree::deg2rad( + config.check("maxAngleVariation", yarp::os::Value(40.0)).asDouble()); + double minAngleVariation = iDynTree::deg2rad( + config.check("minAngleVariation", yarp::os::Value(5.0)).asDouble()); + double maxStepDuration = + config.check("maxStepDuration", yarp::os::Value(8.0)).asDouble(); + double minStepDuration = + config.check("minStepDuration", yarp::os::Value(2.9)).asDouble(); + double stepHeight = + config.check("stepHeight", yarp::os::Value(0.005)).asDouble(); + double landingVelocity = + config.check("stepLandingVelocity", yarp::os::Value(0.0)).asDouble(); + double apexTime = + config.check("footApexTime", yarp::os::Value(0.5)).asDouble(); + double comHeight = + config.check("com_height", yarp::os::Value(0.49)).asDouble(); + double comHeightDelta = + config.check("comHeightDelta", yarp::os::Value(0.01)).asDouble(); + double nominalDuration = + config.check("nominalDuration", yarp::os::Value(4.0)).asDouble(); + double lastStepSwitchTime = + config.check("lastStepSwitchTime", yarp::os::Value(0.5)).asDouble(); + double switchOverSwingRatio = + config.check("switchOverSwingRatio", yarp::os::Value(0.4)).asDouble(); + double mergePointRatio = + config.check("mergePointRatio", yarp::os::Value(0.5)).asDouble(); + + m_nominalWidth = + config.check("nominalWidth", yarp::os::Value(0.04)).asDouble(); + + m_swingLeft = config.check("swingLeft", yarp::os::Value(true)).asBool(); + bool startWithSameFoot = + config.check("startAlwaysSameFoot", yarp::os::Value(false)).asBool(); + m_useMinimumJerk = + config.check("useMinimumJerkFootTrajectory", yarp::os::Value(false)) + .asBool(); + double pitchDelta = + config.check("pitchDelta", yarp::os::Value(0.0)).asDouble(); + + // try to configure the planner + std::shared_ptr unicyclePlanner = + m_trajectoryGenerator.unicyclePlanner(); + bool ok = true; + ok = ok && unicyclePlanner->setDesiredPersonDistance( + m_referencePointDistance(0), m_referencePointDistance(1)); + ok = ok && unicyclePlanner->setControllerGain(unicycleGain); + ok = ok && unicyclePlanner->setMaximumIntegratorStepSize(m_dT); + ok = ok && unicyclePlanner->setMaxStepLength(maxStepLength); + ok = ok && unicyclePlanner->setWidthSetting(minWidth, m_nominalWidth); + ok = ok && unicyclePlanner->setMaxAngleVariation(maxAngleVariation); + ok = ok && unicyclePlanner->setCostWeights(positionWeight, timeWeight); + ok = ok && unicyclePlanner->setStepTimings(minStepDuration, maxStepDuration, + nominalDuration); + ok = ok && unicyclePlanner->setPlannerPeriod(m_dT); + ok = ok && unicyclePlanner->setMinimumAngleForNewSteps(minAngleVariation); + ok = ok && unicyclePlanner->setMinimumStepLength(minStepLength); + ok = ok && unicyclePlanner->setSlowWhenTurnGain(slowWhenTurningGain); + unicyclePlanner->addTerminalStep(false); + unicyclePlanner->startWithLeft(m_swingLeft); + unicyclePlanner->resetStartingFootIfStill(startWithSameFoot); + + ok = + ok && m_trajectoryGenerator.setSwitchOverSwingRatio(switchOverSwingRatio); + ok = + ok && m_trajectoryGenerator.setTerminalHalfSwitchTime(lastStepSwitchTime); + ok = ok && m_trajectoryGenerator.setPauseConditions(maxStepDuration, + nominalDuration); + + if (m_useMinimumJerk) { + m_feetGenerator = m_trajectoryGenerator.addFeetMinimumJerkGenerator(); + } else { + m_feetGenerator = m_trajectoryGenerator.addFeetCubicSplineGenerator(); + } + ok = ok && m_feetGenerator->setStepHeight(stepHeight); + ok = ok && m_feetGenerator->setFootLandingVelocity(landingVelocity); + ok = ok && m_feetGenerator->setFootApexTime(apexTime); + ok = ok && m_feetGenerator->setPitchDelta(pitchDelta); + + m_heightGenerator = m_trajectoryGenerator.addCoMHeightTrajectoryGenerator(); + ok = ok && m_heightGenerator->setCoMHeightSettings(comHeight, comHeightDelta); + ok = ok && m_trajectoryGenerator.setMergePointRatio(mergePointRatio); + + m_dcmGenerator = m_trajectoryGenerator.addDCMTrajectoryGenerator(); + m_dcmGenerator->setFootOriginOffset(leftZMPDelta, rightZMPDelta); + m_dcmGenerator->setOmega(sqrt(9.81 / comHeight)); + + m_correctLeft = true; + + if (ok) { + // the mutex is automatically released when lock_guard goes out of its scope + std::lock_guard guard(m_mutex); - m_dcmGenerator = m_trajectoryGenerator.addDCMTrajectoryGenerator(); - m_dcmGenerator->setFootOriginOffset(leftZMPDelta, rightZMPDelta); - m_dcmGenerator->setOmega(sqrt(9.81/comHeight)); + // change the state of the generator + m_generatorState = GeneratorState::FirstStep; - m_correctLeft = true; + // start the thread + m_generatorThread = std::thread(&TrajectoryGenerator::computeThread, this); + } - if(ok) - { - // the mutex is automatically released when lock_guard goes out of its scope - std::lock_guard guard(m_mutex); + return ok; +} - // change the state of the generator - m_generatorState = GeneratorState::FirstStep; +void TrajectoryGenerator::addTerminalStep(bool terminalStep) { + m_trajectoryGenerator.unicyclePlanner()->addTerminalStep(terminalStep); +} - // start the thread - m_generatorThread = std::thread(&TrajectoryGenerator::computeThread, this); - } +void TrajectoryGenerator::computeThread() { + while (true) { + double initTime; + double endTime; + double dT; - return ok; -} + bool correctLeft; -void TrajectoryGenerator::addTerminalStep(bool terminalStep) -{ - m_trajectoryGenerator.unicyclePlanner()->addTerminalStep(terminalStep); -} + iDynTree::Vector2 desiredPoint; + iDynTree::Vector2 measuredPositionLeft, measuredPositionRight; + double measuredAngleLeft, measuredAngleRight; -void TrajectoryGenerator::computeThread() -{ - while (true) - { - double initTime; - double endTime; - double dT; - - bool correctLeft; - - iDynTree::Vector2 desiredPoint; - iDynTree::Vector2 measuredPositionLeft, measuredPositionRight; - double measuredAngleLeft, measuredAngleRight; - - iDynTree::Vector2 DCMBoundaryConditionAtMergePointPosition; - iDynTree::Vector2 DCMBoundaryConditionAtMergePointVelocity; - - // wait until a new trajectory has to be evaluated. - { - std::unique_lock lock(m_mutex); - m_conditionVariable.wait(lock, [&]{return ((m_generatorState == GeneratorState::Called) - || (m_generatorState == GeneratorState::Closing));}); - - if(m_generatorState == GeneratorState::Closing) - break; - - // set timings - dT = m_dT ; - initTime = m_initTime; - endTime = initTime + m_plannerHorizon; - - // set desired point - desiredPoint = m_desiredPoint; - - // dcm boundary conditions - DCMBoundaryConditionAtMergePointPosition = m_DCMBoundaryConditionAtMergePointPosition; - DCMBoundaryConditionAtMergePointVelocity = m_DCMBoundaryConditionAtMergePointVelocity; - - // left foot - measuredPositionLeft(0) = m_measuredTransformLeft.getPosition()(0); - measuredPositionLeft(1) = m_measuredTransformLeft.getPosition()(1); - measuredAngleLeft = m_measuredTransformLeft.getRotation().asRPY()(2); - - // right foot - measuredPositionRight(0) = m_measuredTransformRight.getPosition()(0); - measuredPositionRight(1) = m_measuredTransformRight.getPosition()(1); - measuredAngleRight = m_measuredTransformRight.getRotation().asRPY()(2); - - correctLeft = m_correctLeft; - } - - // clear the old trajectory - std::shared_ptr unicyclePlanner = m_trajectoryGenerator.unicyclePlanner(); - unicyclePlanner->clearDesiredTrajectory(); - - // add new point - if(!unicyclePlanner->addDesiredTrajectoryPoint(endTime, desiredPoint)) - { - // something goes wrong - std::lock_guard guard(m_mutex); - m_generatorState = GeneratorState::Configured; - yError() << "[TrajectoryGenerator_Thread] Error while setting the new reference."; - break; - } - - iDynTree::Vector2 measuredPosition; - double measuredAngle; - measuredPosition = correctLeft ? measuredPositionLeft : measuredPositionRight; - measuredAngle = correctLeft ? measuredAngleLeft : measuredAngleRight; - - DCMInitialState initialState; - initialState.initialPosition = DCMBoundaryConditionAtMergePointPosition; - initialState.initialVelocity = DCMBoundaryConditionAtMergePointVelocity; - - if (!m_dcmGenerator->setDCMInitialState(initialState)) { - // something goes wrong - std::lock_guard guard(m_mutex); - m_generatorState = GeneratorState::Configured; - yError() << "[TrajectoryGenerator_Thread] Failed to set the initial state."; - break; - } - - if(m_trajectoryGenerator.reGenerate(initTime, dT, endTime, - correctLeft, measuredPosition, measuredAngle)) - { - std::lock_guard guard(m_mutex); - m_generatorState = GeneratorState::Returned; - continue; - } - else - { - std::lock_guard guard(m_mutex); - m_generatorState = GeneratorState::Configured; - yError() << "[TrajectoryGenerator_Thread] Failed in computing new trajectory."; - continue; - } - } -} + iDynTree::Vector2 DCMBoundaryConditionAtMergePointPosition; + iDynTree::Vector2 DCMBoundaryConditionAtMergePointVelocity; -bool TrajectoryGenerator::generateFirstTrajectories() -{ - // check if this step is the first one + // wait until a new trajectory has to be evaluated. { - std::lock_guard guard(m_mutex); - if(m_generatorState != GeneratorState::FirstStep) - { - yError() << "[generateFirstTrajectories] This is not the first step! How can I generate the first step?"; - return false; - } + std::unique_lock lock(m_mutex); + m_conditionVariable.wait(lock, [&] { + return ((m_generatorState == GeneratorState::Called) || + (m_generatorState == GeneratorState::Closing)); + }); + + if (m_generatorState == GeneratorState::Closing) + break; + + // set timings + dT = m_dT; + initTime = m_initTime; + endTime = initTime + m_plannerHorizon; + + // set desired point + desiredPoint = m_desiredPoint; + + // dcm boundary conditions + DCMBoundaryConditionAtMergePointPosition = + m_DCMBoundaryConditionAtMergePointPosition; + DCMBoundaryConditionAtMergePointVelocity = + m_DCMBoundaryConditionAtMergePointVelocity; + + // left foot + measuredPositionLeft(0) = m_measuredTransformLeft.getPosition()(0); + measuredPositionLeft(1) = m_measuredTransformLeft.getPosition()(1); + measuredAngleLeft = m_measuredTransformLeft.getRotation().asRPY()(2); + + // right foot + measuredPositionRight(0) = m_measuredTransformRight.getPosition()(0); + measuredPositionRight(1) = m_measuredTransformRight.getPosition()(1); + measuredAngleRight = m_measuredTransformRight.getRotation().asRPY()(2); + + correctLeft = m_correctLeft; } - // clear the all trajectory - std::shared_ptr unicyclePlanner = m_trajectoryGenerator.unicyclePlanner(); + // clear the old trajectory + std::shared_ptr unicyclePlanner = + m_trajectoryGenerator.unicyclePlanner(); unicyclePlanner->clearDesiredTrajectory(); - // clear left and right footsteps - m_trajectoryGenerator.getLeftFootPrint()->clearSteps(); - m_trajectoryGenerator.getRightFootPrint()->clearSteps(); - - // set initial and final times - double initTime = 0; - double endTime = initTime + m_plannerHorizon; - - // at the beginning iCub has to stop - m_desiredPoint(0) = m_referencePointDistance(0); - m_desiredPoint(1) = m_referencePointDistance(1); - - // add the initial point - if(!unicyclePlanner->addDesiredTrajectoryPoint(initTime, m_referencePointDistance)) - { - yError() << "[generateFirstTrajectories] Error while setting the first reference."; - return false; + // add new point + if (!unicyclePlanner->addDesiredTrajectoryPoint(endTime, desiredPoint)) { + // something goes wrong + std::lock_guard guard(m_mutex); + m_generatorState = GeneratorState::Configured; + yError() << "[TrajectoryGenerator_Thread] Error while setting the new " + "reference."; + break; } - // add the final point - if(!unicyclePlanner->addDesiredTrajectoryPoint(endTime, m_desiredPoint)) - { - yError() << "[generateFirstTrajectories] Error while setting the new reference."; - return false; + iDynTree::Vector2 measuredPosition; + double measuredAngle; + measuredPosition = + correctLeft ? measuredPositionLeft : measuredPositionRight; + measuredAngle = correctLeft ? measuredAngleLeft : measuredAngleRight; + + DCMInitialState initialState; + initialState.initialPosition = DCMBoundaryConditionAtMergePointPosition; + initialState.initialVelocity = DCMBoundaryConditionAtMergePointVelocity; + + if (!m_dcmGenerator->setDCMInitialState(initialState)) { + // something goes wrong + std::lock_guard guard(m_mutex); + m_generatorState = GeneratorState::Configured; + yError() + << "[TrajectoryGenerator_Thread] Failed to set the initial state."; + break; } - // generate the first trajectories - if(!m_trajectoryGenerator.generate(initTime, m_dT, endTime)) - { - yError() << "[generateFirstTrajectories] Error while computing the first trajectories."; - return false; + if (m_trajectoryGenerator.reGenerate(initTime, dT, endTime, correctLeft, + measuredPosition, measuredAngle)) { + std::lock_guard guard(m_mutex); + m_generatorState = GeneratorState::Returned; + continue; + } else { + std::lock_guard guard(m_mutex); + m_generatorState = GeneratorState::Configured; + yError() + << "[TrajectoryGenerator_Thread] Failed in computing new trajectory."; + continue; } - - m_generatorState = GeneratorState::Returned; - return true; + } } -bool TrajectoryGenerator::generateFirstTrajectories(const iDynTree::Transform &leftToRightTransform) - // const iDynTree::Position &initialCOMPosition) -{ - // check if this step is the first one - { - std::lock_guard guard(m_mutex); - if(m_generatorState != GeneratorState::FirstStep) - { - yError() << "[generateFirstTrajectories] This is not the first step! How can I generate the first step?"; - return false; - } - } - - // clear the all trajectory - std::shared_ptr unicyclePlanner = m_trajectoryGenerator.unicyclePlanner(); - unicyclePlanner->clearDesiredTrajectory(); - - // clear left and right footsteps - m_trajectoryGenerator.getLeftFootPrint()->clearSteps(); - m_trajectoryGenerator.getRightFootPrint()->clearSteps(); - - // set initial and final times - double initTime = 0; - double endTime = initTime + m_plannerHorizon; - - // at the beginning iCub has to stop - m_desiredPoint(0) = m_referencePointDistance(0); - m_desiredPoint(1) = m_referencePointDistance(1); - - // add the initial point - if(!unicyclePlanner->addDesiredTrajectoryPoint(initTime, m_referencePointDistance)) - { - yError() << "[generateFirstTrajectories] Error while setting the first reference."; - return false; +bool TrajectoryGenerator::generateFirstTrajectories() { + // check if this step is the first one + { + std::lock_guard guard(m_mutex); + if (m_generatorState != GeneratorState::FirstStep) { + yError() << "[generateFirstTrajectories] This is not the first step! How " + "can I generate the first step?"; + return false; } + } + + // clear the all trajectory + std::shared_ptr unicyclePlanner = + m_trajectoryGenerator.unicyclePlanner(); + unicyclePlanner->clearDesiredTrajectory(); + + // clear left and right footsteps + m_trajectoryGenerator.getLeftFootPrint()->clearSteps(); + m_trajectoryGenerator.getRightFootPrint()->clearSteps(); + + // set initial and final times + double initTime = 0; + double endTime = initTime + m_plannerHorizon; + + // at the beginning iCub has to stop + m_desiredPoint(0) = m_referencePointDistance(0); + m_desiredPoint(1) = m_referencePointDistance(1); + + // add the initial point + if (!unicyclePlanner->addDesiredTrajectoryPoint(initTime, + m_referencePointDistance)) { + yError() << "[generateFirstTrajectories] Error while setting the first " + "reference."; + return false; + } + + // add the final point + if (!unicyclePlanner->addDesiredTrajectoryPoint(endTime, m_desiredPoint)) { + yError() + << "[generateFirstTrajectories] Error while setting the new reference."; + return false; + } + + // generate the first trajectories + if (!m_trajectoryGenerator.generate(initTime, m_dT, endTime)) { + yError() << "[generateFirstTrajectories] Error while computing the first " + "trajectories."; + return false; + } + + m_generatorState = GeneratorState::Returned; + return true; +} - // add the final point - if(!unicyclePlanner->addDesiredTrajectoryPoint(endTime, m_desiredPoint)) - { - yError() << "[generateFirstTrajectories] Error while setting the new reference."; - return false; +bool TrajectoryGenerator::generateFirstTrajectories( + const iDynTree::Transform &leftToRightTransform) +// const iDynTree::Position &initialCOMPosition) +{ + // check if this step is the first one + { + std::lock_guard guard(m_mutex); + if (m_generatorState != GeneratorState::FirstStep) { + yError() << "[generateFirstTrajectories] This is not the first step! How " + "can I generate the first step?"; + return false; } + } + + // clear the all trajectory + std::shared_ptr unicyclePlanner = + m_trajectoryGenerator.unicyclePlanner(); + unicyclePlanner->clearDesiredTrajectory(); + + // clear left and right footsteps + m_trajectoryGenerator.getLeftFootPrint()->clearSteps(); + m_trajectoryGenerator.getRightFootPrint()->clearSteps(); + + // set initial and final times + double initTime = 0; + double endTime = initTime + m_plannerHorizon; + + // at the beginning iCub has to stop + m_desiredPoint(0) = m_referencePointDistance(0); + m_desiredPoint(1) = m_referencePointDistance(1); + + // add the initial point + if (!unicyclePlanner->addDesiredTrajectoryPoint(initTime, + m_referencePointDistance)) { + yError() << "[generateFirstTrajectories] Error while setting the first " + "reference."; + return false; + } + + // add the final point + if (!unicyclePlanner->addDesiredTrajectoryPoint(endTime, m_desiredPoint)) { + yError() + << "[generateFirstTrajectories] Error while setting the new reference."; + return false; + } + + // add real position of the feet + std::shared_ptr left, right; + + left = m_trajectoryGenerator.getLeftFootPrint(); + left->clearSteps(); + right = m_trajectoryGenerator.getRightFootPrint(); + right->clearSteps(); + + iDynTree::Vector2 leftPosition, rightPosition; + double leftAngle, rightAngle; + + // if the standing foot is the right -> the unicycle starts parallel to the + // right foot + if (m_swingLeft) { + rightPosition(0) = 0.0; + rightPosition(1) = -leftToRightTransform.inverse().getPosition()(1) / 2; + rightAngle = 0; + right->addStep(rightPosition, rightAngle, 0.0); + + leftPosition(0) = leftToRightTransform.inverse().getPosition()(0); + leftPosition(1) = leftToRightTransform.inverse().getPosition()(1) / 2; + leftAngle = leftToRightTransform.inverse().getRotation().asRPY()(2); + left->addStep(leftPosition, leftAngle, 0.0); + } else { + leftPosition(0) = 0.0; + leftPosition(1) = -leftToRightTransform.getPosition()(1) / 2; + leftAngle = 0; + left->addStep(leftPosition, leftAngle, 0.0); + + rightPosition(0) = leftToRightTransform.getPosition()(0); + rightPosition(1) = leftToRightTransform.getPosition()(1) / 2; + rightAngle = leftToRightTransform.getRotation().asRPY()(2); + right->addStep(rightPosition, rightAngle, 0.0); + } + + // generate the first trajectories + if (!m_trajectoryGenerator.generate(initTime, m_dT, endTime)) { + yError() << "[generateFirstTrajectories] Error while computing the first " + "trajectories."; + return false; + } + + m_generatorState = GeneratorState::Returned; + return true; +} - // add real position of the feet - std::shared_ptr left, right; - - left = m_trajectoryGenerator.getLeftFootPrint(); - left->clearSteps(); - right = m_trajectoryGenerator.getRightFootPrint(); - right->clearSteps(); - - iDynTree::Vector2 leftPosition, rightPosition; - double leftAngle, rightAngle; +bool TrajectoryGenerator::updateTrajectories( + double initTime, + const iDynTree::Vector2 &DCMBoundaryConditionAtMergePointPosition, + const iDynTree::Vector2 &DCMBoundaryConditionAtMergePointVelocity, + bool correctLeft, const iDynTree::Transform &measured, + const iDynTree::Vector2 &desiredPosition) { + { + std::lock_guard guard(m_mutex); - // if the standing foot is the right -> the unicycle starts parallel to the right foot - if(m_swingLeft) - { - rightPosition(0) = 0.0; - rightPosition(1) = -leftToRightTransform.inverse().getPosition()(1)/2; - rightAngle = 0; - right->addStep(rightPosition, rightAngle, 0.0); - - leftPosition(0) = leftToRightTransform.inverse().getPosition()(0); - leftPosition(1) = leftToRightTransform.inverse().getPosition()(1)/2; - leftAngle = leftToRightTransform.inverse().getRotation().asRPY()(2); - left->addStep(leftPosition, leftAngle, 0.0); - } - else - { - leftPosition(0) = 0.0; - leftPosition(1) = -leftToRightTransform.getPosition()(1)/2; - leftAngle = 0; - left->addStep(leftPosition, leftAngle, 0.0); - - rightPosition(0) = leftToRightTransform.getPosition()(0); - rightPosition(1) = leftToRightTransform.getPosition()(1)/2; - rightAngle = leftToRightTransform.getRotation().asRPY()(2); - right->addStep(rightPosition, rightAngle, 0.0); + if (m_generatorState == GeneratorState::Called) { + yError() << "[updateTrajectories_one correction] Cannot launch the " + "generator twice. " + << "Please wait until the trajectory has be evaluated ."; + return false; } - // generate the first trajectories - if(!m_trajectoryGenerator.generate(initTime, m_dT, endTime)) - { - yError() << "[generateFirstTrajectories] Error while computing the first trajectories."; - return false; + if (m_generatorState != GeneratorState::Returned) { + yError() << "[updateTrajectories_one correction] The trajectory " + "generator has not computed any trajectory yet. " + << "Please call 'generateFirstTrajectories()' method."; + return false; } + } + // if correctLeft is true the stance foot is the true. + // The vector (expressed in the unicycle reference frame from the left foot to + // the center of the unicycle is [0, width/2]') + iDynTree::Vector2 unicyclePositionFromStanceFoot; + unicyclePositionFromStanceFoot(0) = 0.0; + unicyclePositionFromStanceFoot(1) = + correctLeft ? -m_nominalWidth / 2 : m_nominalWidth / 2; + + iDynTree::Vector2 desredPositionFromStanceFoot; + iDynTree::toEigen(desredPositionFromStanceFoot) = + iDynTree::toEigen(unicyclePositionFromStanceFoot) + + iDynTree::toEigen(m_referencePointDistance) + + iDynTree::toEigen(desiredPosition); + + // prepare the rotation matrix w_R_{unicycle} + double theta = measured.getRotation().asRPY()(2); + double s_theta = std::sin(theta); + double c_theta = std::cos(theta); + + // save the data + { + std::lock_guard guard(m_mutex); - m_generatorState = GeneratorState::Returned; - return true; -} - -bool TrajectoryGenerator::updateTrajectories(double initTime, const iDynTree::Vector2& DCMBoundaryConditionAtMergePointPosition, - const iDynTree::Vector2& DCMBoundaryConditionAtMergePointVelocity, bool correctLeft, - const iDynTree::Transform& measured, const iDynTree::Vector2& desiredPosition) -{ - { - std::lock_guard guard(m_mutex); - - if(m_generatorState == GeneratorState::Called) - { - yError() << "[updateTrajectories_one correction] Cannot launch the generator twice. " - << "Please wait until the trajectory has be evaluated ."; - return false; - } - - if(m_generatorState != GeneratorState::Returned) - { - yError() << "[updateTrajectories_one correction] The trajectory generator has not computed any trajectory yet. " - << "Please call 'generateFirstTrajectories()' method."; - return false; - } - } - // if correctLeft is true the stance foot is the true. - // The vector (expressed in the unicycle reference frame from the left foot to the center of the - // unicycle is [0, width/2]') - iDynTree::Vector2 unicyclePositionFromStanceFoot; - unicyclePositionFromStanceFoot(0) = 0.0; - unicyclePositionFromStanceFoot(1) = correctLeft ? -m_nominalWidth/2 : m_nominalWidth/2; - - iDynTree::Vector2 desredPositionFromStanceFoot; - iDynTree::toEigen(desredPositionFromStanceFoot) = iDynTree::toEigen(unicyclePositionFromStanceFoot) - + iDynTree::toEigen(m_referencePointDistance) + iDynTree::toEigen(desiredPosition); - - // prepare the rotation matrix w_R_{unicycle} - double theta = measured.getRotation().asRPY()(2); - double s_theta = std::sin(theta); - double c_theta = std::cos(theta); - - // save the data - { - std::lock_guard guard(m_mutex); - - // apply the homogeneous transformation w_H_{unicycle} - m_desiredPoint(0) = c_theta * desredPositionFromStanceFoot(0) - - s_theta * desredPositionFromStanceFoot(1) + measured.getPosition()(0); - m_desiredPoint(1) = s_theta * desredPositionFromStanceFoot(0) - + c_theta * desredPositionFromStanceFoot(1) + measured.getPosition()(1); + // apply the homogeneous transformation w_H_{unicycle} + m_desiredPoint(0) = c_theta * desredPositionFromStanceFoot(0) - + s_theta * desredPositionFromStanceFoot(1) + + measured.getPosition()(0); + m_desiredPoint(1) = s_theta * desredPositionFromStanceFoot(0) + + c_theta * desredPositionFromStanceFoot(1) + + measured.getPosition()(1); - m_initTime = initTime; + m_initTime = initTime; - // Boundary condition - m_DCMBoundaryConditionAtMergePointPosition = DCMBoundaryConditionAtMergePointPosition; - m_DCMBoundaryConditionAtMergePointVelocity = DCMBoundaryConditionAtMergePointVelocity; + // Boundary condition + m_DCMBoundaryConditionAtMergePointPosition = + DCMBoundaryConditionAtMergePointPosition; + m_DCMBoundaryConditionAtMergePointVelocity = + DCMBoundaryConditionAtMergePointVelocity; - m_correctLeft = correctLeft; + m_correctLeft = correctLeft; - if(correctLeft) - m_measuredTransformLeft = measured; - else - m_measuredTransformRight = measured; + if (correctLeft) + m_measuredTransformLeft = measured; + else + m_measuredTransformRight = measured; - m_generatorState = GeneratorState::Called; - } + m_generatorState = GeneratorState::Called; + } - m_conditionVariable.notify_one(); + m_conditionVariable.notify_one(); - return true; + return true; } -bool TrajectoryGenerator::isTrajectoryComputed() -{ - std::lock_guard guard(m_mutex); - return m_generatorState == GeneratorState::Returned; +bool TrajectoryGenerator::isTrajectoryComputed() { + std::lock_guard guard(m_mutex); + return m_generatorState == GeneratorState::Returned; } -bool TrajectoryGenerator::isTrajectoryAsked() -{ - std::lock_guard guard(m_mutex); - return m_generatorState == GeneratorState::Called; +bool TrajectoryGenerator::isTrajectoryAsked() { + std::lock_guard guard(m_mutex); + return m_generatorState == GeneratorState::Called; } -bool TrajectoryGenerator::getDCMPositionTrajectory(std::vector& DCMPositionTrajectory) -{ - if(!isTrajectoryComputed()) - { - yError() << "[getDCMPositionTrajectory] No trajectories are available"; - return false; - } +bool TrajectoryGenerator::getDCMPositionTrajectory( + std::vector &DCMPositionTrajectory) { + if (!isTrajectoryComputed()) { + yError() << "[getDCMPositionTrajectory] No trajectories are available"; + return false; + } - DCMPositionTrajectory = m_dcmGenerator->getDCMPosition(); - return true; + DCMPositionTrajectory = m_dcmGenerator->getDCMPosition(); + return true; } -bool TrajectoryGenerator::getDCMVelocityTrajectory(std::vector& DCMVelocityTrajectory) -{ - if(!isTrajectoryComputed()) - { - yError() << "[getDCMVelocityTrajectory] No trajectories are available"; - return false; - } +bool TrajectoryGenerator::getDCMVelocityTrajectory( + std::vector &DCMVelocityTrajectory) { + if (!isTrajectoryComputed()) { + yError() << "[getDCMVelocityTrajectory] No trajectories are available"; + return false; + } - DCMVelocityTrajectory = m_dcmGenerator->getDCMVelocity(); - return true; + DCMVelocityTrajectory = m_dcmGenerator->getDCMVelocity(); + return true; } -bool TrajectoryGenerator::getFeetTrajectories(std::vector& lFootTrajectory, - std::vector& rFootTrajectory) -{ - if(!isTrajectoryComputed()) - { - yError() << "[getFeetTrajectories] No trajectories are available"; - return false; - } +bool TrajectoryGenerator::getFeetTrajectories( + std::vector &lFootTrajectory, + std::vector &rFootTrajectory) { + if (!isTrajectoryComputed()) { + yError() << "[getFeetTrajectories] No trajectories are available"; + return false; + } - m_feetGenerator->getFeetTrajectories(lFootTrajectory, rFootTrajectory); + m_feetGenerator->getFeetTrajectories(lFootTrajectory, rFootTrajectory); - return true; + return true; } -bool TrajectoryGenerator::getFeetTwist(std::vector& lFootTwist, - std::vector& rFootTwist) -{ - if(!isTrajectoryComputed()) - { - yError() << "[getFeetTwist] No trajectories are available"; - return false; - } +bool TrajectoryGenerator::getFeetTwist( + std::vector &lFootTwist, + std::vector &rFootTwist) { + if (!isTrajectoryComputed()) { + yError() << "[getFeetTwist] No trajectories are available"; + return false; + } - m_feetGenerator->getFeetTwistsInMixedRepresentation(lFootTwist, rFootTwist); + m_feetGenerator->getFeetTwistsInMixedRepresentation(lFootTwist, rFootTwist); - return true; + return true; } -bool TrajectoryGenerator::getWhenUseLeftAsFixed(std::vector& isLeftFixedFrame) -{ - if(!isTrajectoryComputed()) - { - yError() << "[getWhenUseLeftAsFixed] No trajectories are available"; - return false; - } +bool TrajectoryGenerator::getWhenUseLeftAsFixed( + std::vector &isLeftFixedFrame) { + if (!isTrajectoryComputed()) { + yError() << "[getWhenUseLeftAsFixed] No trajectories are available"; + return false; + } - m_trajectoryGenerator.getWhenUseLeftAsFixed(isLeftFixedFrame); - return true; + m_trajectoryGenerator.getWhenUseLeftAsFixed(isLeftFixedFrame); + return true; } +bool TrajectoryGenerator::getFeetStandingPeriods( + std::vector &lFootContacts, std::vector &rFootContacts) { + if (!isTrajectoryComputed()) { + yError() << "[getFeetStandingPeriods] No trajectories are available"; + return false; + } -bool TrajectoryGenerator::getFeetStandingPeriods(std::vector& lFootContacts, - std::vector& rFootContacts) -{ - if(!isTrajectoryComputed()) - { - yError() << "[getFeetStandingPeriods] No trajectories are available"; - return false; - } + m_trajectoryGenerator.getFeetStandingPeriods(lFootContacts, rFootContacts); + return true; +} - m_trajectoryGenerator.getFeetStandingPeriods(lFootContacts, rFootContacts); - return true; +bool TrajectoryGenerator::getCoMHeightTrajectory( + std::vector &CoMHeightTrajectory) { + if (!isTrajectoryComputed()) { + yError() << "[getCoMHeightTrajectory] No trajectories are available"; + return false; + } + + m_heightGenerator->getCoMHeightTrajectory(CoMHeightTrajectory); + return true; } -bool TrajectoryGenerator::getCoMHeightTrajectory(std::vector& CoMHeightTrajectory) -{ - if(!isTrajectoryComputed()) - { - yError() << "[getCoMHeightTrajectory] No trajectories are available"; - return false; - } +bool TrajectoryGenerator::getCoMHeightVelocity( + std::vector &CoMHeightVelocity) { + if (!isTrajectoryComputed()) { + yError() << "[getCoMHeightVelocity] No trajectories are available"; + return false; + } - m_heightGenerator->getCoMHeightTrajectory(CoMHeightTrajectory); - return true; + m_heightGenerator->getCoMHeightVelocity(CoMHeightVelocity); + return true; } -bool TrajectoryGenerator::getCoMHeightVelocity(std::vector& CoMHeightVelocity) -{ - if(!isTrajectoryComputed()) - { - yError() << "[getCoMHeightVelocity] No trajectories are available"; - return false; - } +bool TrajectoryGenerator::getMergePoints(std::vector &mergePoints) { + if (!isTrajectoryComputed()) { + yError() << "[getMergePoints] No trajectories are available"; + return false; + } - m_heightGenerator->getCoMHeightVelocity(CoMHeightVelocity); - return true; + m_trajectoryGenerator.getMergePoints(mergePoints); + return true; } -bool TrajectoryGenerator::getMergePoints(std::vector& mergePoints) -{ - if(!isTrajectoryComputed()) - { - yError() << "[getMergePoints] No trajectories are available"; - return false; - } +bool TrajectoryGenerator::getWeightPercentage( + std::vector &weightInLeft, std::vector &weightInRight) { + if (!isTrajectoryComputed()) { + yError() << "[getWeightPercentage] No trajectories are available"; + return false; + } - m_trajectoryGenerator.getMergePoints(mergePoints); - return true; + m_dcmGenerator->getWeightPercentage(weightInLeft, weightInRight); + return true; } -void TrajectoryGenerator::reset() -{ - // the mutex is automatically released when lock_guard goes out of its scope - std::lock_guard guard(m_mutex); +void TrajectoryGenerator::reset() { + // the mutex is automatically released when lock_guard goes out of its scope + std::lock_guard guard(m_mutex); - // change the state of the generator - m_generatorState = GeneratorState::FirstStep; + // change the state of the generator + m_generatorState = GeneratorState::FirstStep; } From e07370a903addd579401bdb2b132325ccb7efa1a Mon Sep 17 00:00:00 2001 From: Emilio Date: Wed, 4 Mar 2020 11:03:54 +0100 Subject: [PATCH 2/5] Removed auto-formatting modifications --- .../TrajectoryPlanner/TrajectoryGenerator.h | 439 ++++--- .../src/TrajectoryGenerator.cpp | 1081 ++++++++--------- 2 files changed, 735 insertions(+), 785 deletions(-) diff --git a/src/TrajectoryPlanner/include/WalkingControllers/TrajectoryPlanner/TrajectoryGenerator.h b/src/TrajectoryPlanner/include/WalkingControllers/TrajectoryPlanner/TrajectoryGenerator.h index a603e030..1401fc7b 100644 --- a/src/TrajectoryPlanner/include/WalkingControllers/TrajectoryPlanner/TrajectoryGenerator.h +++ b/src/TrajectoryPlanner/include/WalkingControllers/TrajectoryPlanner/TrajectoryGenerator.h @@ -6,13 +6,14 @@ * @date 2018 */ + #ifndef WALKING_CONTROLLERS_TRAJECTORY_PLANNER_TRAJECTORY_GENERATOR_H #define WALKING_CONTROLLERS_TRAJECTORY_PLANNER_TRAJECTORY_GENERATOR_H // std +#include #include #include -#include // YARP #include @@ -22,248 +23,214 @@ #include -namespace WalkingControllers { +namespace WalkingControllers +{ /** * Enumerator useful to track the state of the trajectory generator */ -enum class GeneratorState { - NotConfigured, - Configured, - FirstStep, - Called, - Returned, - Closing -}; + enum class GeneratorState {NotConfigured, Configured, FirstStep, Called, Returned, Closing}; /** - * TrajectoryGenerator class is used to handle the UnicycleTrajectoryGenerator - * library. + * TrajectoryGenerator class is used to handle the UnicycleTrajectoryGenerator library. */ -class TrajectoryGenerator { - UnicycleGenerator - m_trajectoryGenerator; /**< UnicycleTrajectoryGenerator object. */ - std::shared_ptr m_dcmGenerator; - std::shared_ptr m_heightGenerator; - std::shared_ptr m_feetGenerator; - bool m_useMinimumJerk; - - bool m_swingLeft; /**< True if the first swing foot is the left. */ - - double m_dT; /**< Sampling time of the planner. */ - double m_plannerHorizon; /**< Horizon of the planner. */ - - double m_nominalWidth; /**< Nominal width between two feet. */ - double m_initTime; /**< Init time of the current trajectory. */ - - iDynTree::Vector2 m_referencePointDistance; /**< Vector between the center of - the unicycle and the point that - has to be reach the goal. */ - - GeneratorState m_generatorState{ - GeneratorState::NotConfigured}; /**< Useful to track the generator state. - */ - - std::thread m_generatorThread; /**< Main trajectory thread. */ - std::condition_variable m_conditionVariable; /**< Synchronizer. */ - - bool m_correctLeft; /**< The left foot has to be corrected. */ - iDynTree::Transform - m_measuredTransformLeft; /**< Measured transformation between the left - foot and the world frame. (w_H_lf) */ - iDynTree::Transform - m_measuredTransformRight; /**< Measured transformation between the right - foot and the world frame. (w_H_rf) */ - - iDynTree::Vector2 m_desiredPoint; /**< Desired final position of the x-y - projection of the CoM. */ - - iDynTree::Vector2 - m_DCMBoundaryConditionAtMergePointPosition; /**< DCM position at the merge - point. */ - iDynTree::Vector2 - m_DCMBoundaryConditionAtMergePointVelocity; /**< DCM velocity at the merge - point. */ - - std::mutex m_mutex; /**< Mutex. */ - - /** - * Main thread method. - */ - void computeThread(); - -public: - /** - * Deconstructor. - */ - ~TrajectoryGenerator(); - - /** - * Initialize the trajectory generator - * @param config yarp searchable object. - * @return true/false in case of success/failure. - */ - bool initialize(const yarp::os::Searchable &config); - - /** - * Configure the planner. - * @param config yarp searchable object. - * @return true/false in case of success/failure. - */ - bool configurePlanner(const yarp::os::Searchable &config); - - /** - * Generate the first trajectory. - * This method has to be called before updateTrajectories() method - * @return true/false in case of success/failure. - */ - bool generateFirstTrajectories(); - - /** - * Generate the first trajectory. - * This method has to be called before only by the ontTheFly method. - * @param leftToRightTransform transformation between from the left foot to - * the right foot; - * @return true/false in case of success/failure. - */ - bool - generateFirstTrajectories(const iDynTree::Transform &leftToRightTransform); - // const iDynTree::Position &initialCOMPosition); - - /** - * Update the trajectory. - * The old trajectory will be deleted and a new one is evaluated. The boundary - * condition of the new trajectory is given by the position and the velocity - * of the DCM at the merge point. This method allows you to take into account - * the real position one foot at the beginning of the trajectory. - * @param initTime is the initial time of the trajectory; - * @param DCMBoundaryConditionAtMergePointPosition is the position of the DCM - * at the merge point; - * @param DCMBoundaryConditionAtMergePointVelocity is the velocity of the DCM - * at the merge point; - * @param correctLeft todo; - * @param measured Measured transformation between the stance foot and the - * world frame. (w_H_{stancefoot}); - * @param desiredPosition final desired position of the projection of the CoM. - * @return true/false in case of success/failure. - */ - bool updateTrajectories( - double initTime, - const iDynTree::Vector2 &DCMBoundaryConditionAtMergePointPosition, - const iDynTree::Vector2 &DCMBoundaryConditionAtMergePointVelocity, - bool correctLeft, const iDynTree::Transform &measured, - const iDynTree::Vector2 &desiredPosition); - - /** - * Return if the trajectory was computed - * @return true if the trajectory has been computed false otherwise. - */ - bool isTrajectoryComputed(); - - /** - * Configure the planner in order to add or not the terminal step - * @param terminalStep if it true the terminal step will be added - */ - void addTerminalStep(bool terminalStep); - - /** - * Return if a new trajectory is asked. - * @return true if the trajectory has already asked. - */ - bool isTrajectoryAsked(); - - /** - * Get the desired 2D-DCM position trajectory - * @param DCMPositionTrajectory desired trajectory of the DCM. - * @return true/false in case of success/failure. - */ - bool getDCMPositionTrajectory( - std::vector &DCMPositionTrajectory); - - /** - * Get the desired 2D-DCM velocity trajectory - * @param DCMVelocityTrajectory desired trajectory of the DCM. - * @return true/false in case of success/failure. - */ - bool getDCMVelocityTrajectory( - std::vector &DCMVelocityTrajectory); - - /** - * Get the feet trajectory - * @param lFootTrajectory vector containing the left foot trajectory; - * @param rFootTrajectory vector containing the right foot trajectory. - * @return true/false in case of success/failure. - */ - bool getFeetTrajectories(std::vector &lFootTrajectory, - std::vector &rFootTrajectory); - - /** - * Get the feet twist - * @param lFootTwist vector containing the left foot twists; - * @param rFootTwist vector containing the right foot twists. - * @return true/false in case of success/failure. - */ - bool getFeetTwist(std::vector &lFootTwist, - std::vector &rFootTwist); - /** - * Get the when the main frame of the left foot is the fix frame. - * @param isLeftFixedFrame vector containing when the main frame of - * the left foot is the fix frame. - * @return true/false in case of success/failure. - */ - bool getWhenUseLeftAsFixed(std::vector &isLeftFixedFrame); - - /** - * Get the feet phases - * @param lFootContacts vector containing the state of the left foot (true = - * in contact); - * @param rFootContacts vector containing the state of the right foot (true = - * in contact). - * @return true/false in case of success/failure. - */ - bool getFeetStandingPeriods(std::vector &lFootContacts, - std::vector &rFootContacts); - - /** - * Get the CoM height trajectory - * @param CoMHeightTrajectory vector containing trajectory of the COM on the z - * axis. - * @return true/false in case of success/failure. - */ - bool getCoMHeightTrajectory(std::vector &CoMHeightTrajectory); - - /** - * Get the CoM height velocity - * @param CoMHeightVelocity vector containing the velocity of the COM on the z - * axis. - * @return true/false in case of success/failure. - */ - bool getCoMHeightVelocity(std::vector &CoMHeightVelocity); - - /** - * Get the merge points along the trajectory - * @param mergePoints vector containing all the merge points of the - * trajectory. - * @return true/false in case of success/failure. - */ - bool getMergePoints(std::vector &mergePoints); - - /** - * Get the weight percentage for the left and right foot - * @param weightInLeft vector containing the weight on the left foot (0 in - * case in case of stance foot during SS, 1 in case of swing foot) - * @param weightInRight vector containing the weight on the right foot (0 in - * case in case of stance foot during SS, 1 in case of swing foot) - * @return true/false in case of success/failure. - */ - bool getWeightPercentage(std::vector &weightInLeft, - std::vector &weightInRight); - - /** - * Reset the planner - */ - void reset(); + class TrajectoryGenerator + { + UnicycleGenerator m_trajectoryGenerator; /**< UnicycleTrajectoryGenerator object. */ + std::shared_ptr m_dcmGenerator; + std::shared_ptr m_heightGenerator; + std::shared_ptr m_feetGenerator; + bool m_useMinimumJerk; + + bool m_swingLeft; /**< True if the first swing foot is the left. */ + + double m_dT; /**< Sampling time of the planner. */ + double m_plannerHorizon; /**< Horizon of the planner. */ + + double m_nominalWidth; /**< Nominal width between two feet. */ + double m_initTime; /**< Init time of the current trajectory. */ + + iDynTree::Vector2 m_referencePointDistance; /**< Vector between the center of the unicycle and the point that has to be reach the goal. */ + + GeneratorState m_generatorState{GeneratorState::NotConfigured}; /**< Useful to track the generator state. */ + + std::thread m_generatorThread; /**< Main trajectory thread. */ + std::condition_variable m_conditionVariable; /**< Synchronizer. */ + + bool m_correctLeft; /**< The left foot has to be corrected. */ + iDynTree::Transform m_measuredTransformLeft; /**< Measured transformation between the left foot and the world frame. (w_H_lf) */ + iDynTree::Transform m_measuredTransformRight; /**< Measured transformation between the right foot and the world frame. (w_H_rf) */ + + iDynTree::Vector2 m_desiredPoint; /**< Desired final position of the x-y projection of the CoM. */ + + iDynTree::Vector2 m_DCMBoundaryConditionAtMergePointPosition; /**< DCM position at the merge point. */ + iDynTree::Vector2 m_DCMBoundaryConditionAtMergePointVelocity; /**< DCM velocity at the merge point. */ + + std::mutex m_mutex; /**< Mutex. */ + + /** + * Main thread method. + */ + void computeThread(); + + public: + + /** + * Deconstructor. + */ + ~TrajectoryGenerator(); + + /** + * Initialize the trajectory generator + * @param config yarp searchable object. + * @return true/false in case of success/failure. + */ + bool initialize(const yarp::os::Searchable& config); + + /** + * Configure the planner. + * @param config yarp searchable object. + * @return true/false in case of success/failure. + */ + bool configurePlanner(const yarp::os::Searchable& config); + + /** + * Generate the first trajectory. + * This method has to be called before updateTrajectories() method + * @return true/false in case of success/failure. + */ + bool generateFirstTrajectories(); + + /** + * Generate the first trajectory. + * This method has to be called before only by the ontTheFly method. + * @param leftToRightTransform transformation between from the left foot to the right foot; + * @return true/false in case of success/failure. + */ + bool generateFirstTrajectories(const iDynTree::Transform &leftToRightTransform); + // const iDynTree::Position &initialCOMPosition); + + /** + * Update the trajectory. + * The old trajectory will be deleted and a new one is evaluated. The boundary condition of the new trajectory is given by + * the position and the velocity of the DCM at the merge point. + * This method allows you to take into account the real position one foot at the beginning of the trajectory. + * @param initTime is the initial time of the trajectory; + * @param DCMBoundaryConditionAtMergePointPosition is the position of the DCM at the merge point; + * @param DCMBoundaryConditionAtMergePointVelocity is the velocity of the DCM at the merge point; + * @param correctLeft todo; + * @param measured Measured transformation between the stance foot and the world frame. (w_H_{stancefoot}); + * @param desiredPosition final desired position of the projection of the CoM. + * @return true/false in case of success/failure. + */ + bool updateTrajectories(double initTime, const iDynTree::Vector2& DCMBoundaryConditionAtMergePointPosition, + const iDynTree::Vector2& DCMBoundaryConditionAtMergePointVelocity, bool correctLeft, + const iDynTree::Transform& measured, const iDynTree::Vector2& desiredPosition); + + /** + * Return if the trajectory was computed + * @return true if the trajectory has been computed false otherwise. + */ + bool isTrajectoryComputed(); + + /** + * Configure the planner in order to add or not the terminal step + * @param terminalStep if it true the terminal step will be added + */ + void addTerminalStep(bool terminalStep); + + /** + * Return if a new trajectory is asked. + * @return true if the trajectory has already asked. + */ + bool isTrajectoryAsked(); + + /** + * Get the desired 2D-DCM position trajectory + * @param DCMPositionTrajectory desired trajectory of the DCM. + * @return true/false in case of success/failure. + */ + bool getDCMPositionTrajectory(std::vector& DCMPositionTrajectory); + + /** + * Get the desired 2D-DCM velocity trajectory + * @param DCMVelocityTrajectory desired trajectory of the DCM. + * @return true/false in case of success/failure. + */ + bool getDCMVelocityTrajectory(std::vector& DCMVelocityTrajectory); + + /** + * Get the feet trajectory + * @param lFootTrajectory vector containing the left foot trajectory; + * @param rFootTrajectory vector containing the right foot trajectory. + * @return true/false in case of success/failure. + */ + bool getFeetTrajectories(std::vector& lFootTrajectory, + std::vector& rFootTrajectory); + + + /** + * Get the feet twist + * @param lFootTwist vector containing the left foot twists; + * @param rFootTwist vector containing the right foot twists. + * @return true/false in case of success/failure. + */ + bool getFeetTwist(std::vector& lFootTwist, + std::vector& rFootTwist); + /** + * Get the when the main frame of the left foot is the fix frame. + * @param isLeftFixedFrame vector containing when the main frame of + * the left foot is the fix frame. + * @return true/false in case of success/failure. + */ + bool getWhenUseLeftAsFixed(std::vector& isLeftFixedFrame); + + /** + * Get the feet phases + * @param lFootContacts vector containing the state of the left foot (true = in contact); + * @param rFootContacts vector containing the state of the right foot (true = in contact). + * @return true/false in case of success/failure. + */ + bool getFeetStandingPeriods(std::vector& lFootContacts, + std::vector& rFootContacts); + + /** + * Get the CoM height trajectory + * @param CoMHeightTrajectory vector containing trajectory of the COM on the z axis. + * @return true/false in case of success/failure. + */ + bool getCoMHeightTrajectory(std::vector& CoMHeightTrajectory); + + /** + * Get the CoM height velocity + * @param CoMHeightVelocity vector containing the velocity of the COM on the z axis. + * @return true/false in case of success/failure. + */ + bool getCoMHeightVelocity(std::vector& CoMHeightVelocity); + + /** + * Get the merge points along the trajectory + * @param mergePoints vector containing all the merge points of the trajectory. + * @return true/false in case of success/failure. + */ + bool getMergePoints(std::vector& mergePoints); + + /** + * Get the weight percentage for the left and right foot + * @param weightInLeft vector containing the weight on the left foot (0 in case in case of + * stance foot during SS, 1 in case of swing foot) + * @param weightInRight vector containing the weight on the right foot (0 in case in case of + * stance foot during SS, 1 in case of swing foot) + * @return true/false in case of success/failure. + */ + bool getWeightPercentage(std::vector &weightInLeft, std::vector &weightInRight); + + + /** + * Reset the planner + */ + void reset(); + }; }; -}; // namespace WalkingControllers #endif diff --git a/src/TrajectoryPlanner/src/TrajectoryGenerator.cpp b/src/TrajectoryPlanner/src/TrajectoryGenerator.cpp index 4f3372fb..6eb9d75c 100644 --- a/src/TrajectoryPlanner/src/TrajectoryGenerator.cpp +++ b/src/TrajectoryPlanner/src/TrajectoryGenerator.cpp @@ -18,633 +18,616 @@ using namespace WalkingControllers; -TrajectoryGenerator::~TrajectoryGenerator() { - { - std::lock_guard guard(m_mutex); - m_generatorState = GeneratorState::Closing; - m_conditionVariable.notify_one(); - } +TrajectoryGenerator::~TrajectoryGenerator() +{ + { + std::lock_guard guard(m_mutex); + m_generatorState = GeneratorState::Closing; + m_conditionVariable.notify_one(); + } - if (m_generatorThread.joinable()) { - m_generatorThread.join(); - m_generatorThread = std::thread(); - } + if(m_generatorThread.joinable()) + { + m_generatorThread.join(); + m_generatorThread = std::thread(); + } } -bool TrajectoryGenerator::initialize(const yarp::os::Searchable &config) { - if (!configurePlanner(config)) { - yError() << "[initialize] Failed to configure the unicycle trajectory " - "generator."; - return false; - } - return true; +bool TrajectoryGenerator::initialize(const yarp::os::Searchable& config) +{ + if(!configurePlanner(config)) + { + yError() << "[initialize] Failed to configure the unicycle trajectory generator."; + return false; + } + return true; } -bool TrajectoryGenerator::configurePlanner(const yarp::os::Searchable &config) { - if (config.isNull()) { - yError() << "[configurePlanner] Empty configuration for unicycle planner."; - return false; - } - - m_dT = config.check("sampling_time", yarp::os::Value(0.016)).asDouble(); - m_plannerHorizon = - config.check("plannerHorizon", yarp::os::Value(20.0)).asDouble(); - double unicycleGain = - config.check("unicycleGain", yarp::os::Value(10.0)).asDouble(); - - if (!YarpUtilities::getVectorFromSearchable(config, "referencePosition", - m_referencePointDistance)) { - yError() << "[configurePlanner] Initialization failed while reading " - "referencePosition vector."; - return false; - } - - // get left and right ZMP delta - iDynTree::Vector2 leftZMPDelta; - if (!YarpUtilities::getVectorFromSearchable(config, "leftZMPDelta", - leftZMPDelta)) { - yError() << "[configurePlanner] Initialization failed while reading " - "rStancePosition vector."; - return false; - } - - iDynTree::Vector2 rightZMPDelta; - if (!YarpUtilities::getVectorFromSearchable(config, "rightZMPDelta", - rightZMPDelta)) { - yError() << "[configurePlanner] Initialization failed while reading " - "rStancePosition vector."; - return false; - } - - double timeWeight = - config.check("timeWeight", yarp::os::Value(2.5)).asDouble(); - double positionWeight = - config.check("positionWeight", yarp::os::Value(1.0)).asDouble(); - double slowWhenTurningGain = - config.check("slowWhenTurningGain", yarp::os::Value(0.0)).asDouble(); - double maxStepLength = - config.check("maxStepLength", yarp::os::Value(0.05)).asDouble(); - double minStepLength = - config.check("minStepLength", yarp::os::Value(0.005)).asDouble(); - double minWidth = config.check("minWidth", yarp::os::Value(0.03)).asDouble(); - double maxAngleVariation = iDynTree::deg2rad( - config.check("maxAngleVariation", yarp::os::Value(40.0)).asDouble()); - double minAngleVariation = iDynTree::deg2rad( - config.check("minAngleVariation", yarp::os::Value(5.0)).asDouble()); - double maxStepDuration = - config.check("maxStepDuration", yarp::os::Value(8.0)).asDouble(); - double minStepDuration = - config.check("minStepDuration", yarp::os::Value(2.9)).asDouble(); - double stepHeight = - config.check("stepHeight", yarp::os::Value(0.005)).asDouble(); - double landingVelocity = - config.check("stepLandingVelocity", yarp::os::Value(0.0)).asDouble(); - double apexTime = - config.check("footApexTime", yarp::os::Value(0.5)).asDouble(); - double comHeight = - config.check("com_height", yarp::os::Value(0.49)).asDouble(); - double comHeightDelta = - config.check("comHeightDelta", yarp::os::Value(0.01)).asDouble(); - double nominalDuration = - config.check("nominalDuration", yarp::os::Value(4.0)).asDouble(); - double lastStepSwitchTime = - config.check("lastStepSwitchTime", yarp::os::Value(0.5)).asDouble(); - double switchOverSwingRatio = - config.check("switchOverSwingRatio", yarp::os::Value(0.4)).asDouble(); - double mergePointRatio = - config.check("mergePointRatio", yarp::os::Value(0.5)).asDouble(); - - m_nominalWidth = - config.check("nominalWidth", yarp::os::Value(0.04)).asDouble(); - - m_swingLeft = config.check("swingLeft", yarp::os::Value(true)).asBool(); - bool startWithSameFoot = - config.check("startAlwaysSameFoot", yarp::os::Value(false)).asBool(); - m_useMinimumJerk = - config.check("useMinimumJerkFootTrajectory", yarp::os::Value(false)) - .asBool(); - double pitchDelta = - config.check("pitchDelta", yarp::os::Value(0.0)).asDouble(); - - // try to configure the planner - std::shared_ptr unicyclePlanner = - m_trajectoryGenerator.unicyclePlanner(); - bool ok = true; - ok = ok && unicyclePlanner->setDesiredPersonDistance( - m_referencePointDistance(0), m_referencePointDistance(1)); - ok = ok && unicyclePlanner->setControllerGain(unicycleGain); - ok = ok && unicyclePlanner->setMaximumIntegratorStepSize(m_dT); - ok = ok && unicyclePlanner->setMaxStepLength(maxStepLength); - ok = ok && unicyclePlanner->setWidthSetting(minWidth, m_nominalWidth); - ok = ok && unicyclePlanner->setMaxAngleVariation(maxAngleVariation); - ok = ok && unicyclePlanner->setCostWeights(positionWeight, timeWeight); - ok = ok && unicyclePlanner->setStepTimings(minStepDuration, maxStepDuration, - nominalDuration); - ok = ok && unicyclePlanner->setPlannerPeriod(m_dT); - ok = ok && unicyclePlanner->setMinimumAngleForNewSteps(minAngleVariation); - ok = ok && unicyclePlanner->setMinimumStepLength(minStepLength); - ok = ok && unicyclePlanner->setSlowWhenTurnGain(slowWhenTurningGain); - unicyclePlanner->addTerminalStep(false); - unicyclePlanner->startWithLeft(m_swingLeft); - unicyclePlanner->resetStartingFootIfStill(startWithSameFoot); - - ok = - ok && m_trajectoryGenerator.setSwitchOverSwingRatio(switchOverSwingRatio); - ok = - ok && m_trajectoryGenerator.setTerminalHalfSwitchTime(lastStepSwitchTime); - ok = ok && m_trajectoryGenerator.setPauseConditions(maxStepDuration, - nominalDuration); - - if (m_useMinimumJerk) { - m_feetGenerator = m_trajectoryGenerator.addFeetMinimumJerkGenerator(); - } else { - m_feetGenerator = m_trajectoryGenerator.addFeetCubicSplineGenerator(); - } - ok = ok && m_feetGenerator->setStepHeight(stepHeight); - ok = ok && m_feetGenerator->setFootLandingVelocity(landingVelocity); - ok = ok && m_feetGenerator->setFootApexTime(apexTime); - ok = ok && m_feetGenerator->setPitchDelta(pitchDelta); - - m_heightGenerator = m_trajectoryGenerator.addCoMHeightTrajectoryGenerator(); - ok = ok && m_heightGenerator->setCoMHeightSettings(comHeight, comHeightDelta); - ok = ok && m_trajectoryGenerator.setMergePointRatio(mergePointRatio); - - m_dcmGenerator = m_trajectoryGenerator.addDCMTrajectoryGenerator(); - m_dcmGenerator->setFootOriginOffset(leftZMPDelta, rightZMPDelta); - m_dcmGenerator->setOmega(sqrt(9.81 / comHeight)); - - m_correctLeft = true; - - if (ok) { - // the mutex is automatically released when lock_guard goes out of its scope - std::lock_guard guard(m_mutex); +bool TrajectoryGenerator::configurePlanner(const yarp::os::Searchable& config) +{ + if(config.isNull()) + { + yError() << "[configurePlanner] Empty configuration for unicycle planner."; + return false; + } - // change the state of the generator - m_generatorState = GeneratorState::FirstStep; - // start the thread - m_generatorThread = std::thread(&TrajectoryGenerator::computeThread, this); - } + m_dT = config.check("sampling_time", yarp::os::Value(0.016)).asDouble(); + m_plannerHorizon = config.check("plannerHorizon", yarp::os::Value(20.0)).asDouble(); + double unicycleGain = config.check("unicycleGain", yarp::os::Value(10.0)).asDouble(); - return ok; -} + if(!YarpUtilities::getVectorFromSearchable(config, "referencePosition", m_referencePointDistance)) + { + yError() << "[configurePlanner] Initialization failed while reading referencePosition vector."; + return false; + } -void TrajectoryGenerator::addTerminalStep(bool terminalStep) { - m_trajectoryGenerator.unicyclePlanner()->addTerminalStep(terminalStep); -} + // get left and right ZMP delta + iDynTree::Vector2 leftZMPDelta; + if(!YarpUtilities::getVectorFromSearchable(config, "leftZMPDelta", leftZMPDelta)) + { + yError() << "[configurePlanner] Initialization failed while reading rStancePosition vector."; + return false; + } + + iDynTree::Vector2 rightZMPDelta; + if(!YarpUtilities::getVectorFromSearchable(config, "rightZMPDelta", rightZMPDelta)) + { + yError() << "[configurePlanner] Initialization failed while reading rStancePosition vector."; + return false; + } -void TrajectoryGenerator::computeThread() { - while (true) { - double initTime; - double endTime; - double dT; + double timeWeight = config.check("timeWeight", yarp::os::Value(2.5)).asDouble(); + double positionWeight = config.check("positionWeight", yarp::os::Value(1.0)).asDouble(); + double slowWhenTurningGain = config.check("slowWhenTurningGain", yarp::os::Value(0.0)).asDouble(); + double maxStepLength = config.check("maxStepLength", yarp::os::Value(0.05)).asDouble(); + double minStepLength = config.check("minStepLength", yarp::os::Value(0.005)).asDouble(); + double minWidth = config.check("minWidth", yarp::os::Value(0.03)).asDouble(); + double maxAngleVariation = iDynTree::deg2rad(config.check("maxAngleVariation", + yarp::os::Value(40.0)).asDouble()); + double minAngleVariation = iDynTree::deg2rad(config.check("minAngleVariation", + yarp::os::Value(5.0)).asDouble()); + double maxStepDuration = config.check("maxStepDuration", yarp::os::Value(8.0)).asDouble(); + double minStepDuration = config.check("minStepDuration", yarp::os::Value(2.9)).asDouble(); + double stepHeight = config.check("stepHeight", yarp::os::Value(0.005)).asDouble(); + double landingVelocity = config.check("stepLandingVelocity", yarp::os::Value(0.0)).asDouble(); + double apexTime = config.check("footApexTime", yarp::os::Value(0.5)).asDouble(); + double comHeight = config.check("com_height", yarp::os::Value(0.49)).asDouble(); + double comHeightDelta = config.check("comHeightDelta", yarp::os::Value(0.01)).asDouble(); + double nominalDuration = config.check("nominalDuration", yarp::os::Value(4.0)).asDouble(); + double lastStepSwitchTime = config.check("lastStepSwitchTime", yarp::os::Value(0.5)).asDouble(); + double switchOverSwingRatio = config.check("switchOverSwingRatio", + yarp::os::Value(0.4)).asDouble(); + double mergePointRatio = config.check("mergePointRatio", yarp::os::Value(0.5)).asDouble(); + + m_nominalWidth = config.check("nominalWidth", yarp::os::Value(0.04)).asDouble(); + + m_swingLeft = config.check("swingLeft", yarp::os::Value(true)).asBool(); + bool startWithSameFoot = config.check("startAlwaysSameFoot", yarp::os::Value(false)).asBool(); + m_useMinimumJerk = config.check("useMinimumJerkFootTrajectory", + yarp::os::Value(false)).asBool(); + double pitchDelta = config.check("pitchDelta", yarp::os::Value(0.0)).asDouble(); + + // try to configure the planner + std::shared_ptr unicyclePlanner = m_trajectoryGenerator.unicyclePlanner(); + bool ok = true; + ok = ok && unicyclePlanner->setDesiredPersonDistance(m_referencePointDistance(0), + m_referencePointDistance(1)); + ok = ok && unicyclePlanner->setControllerGain(unicycleGain); + ok = ok && unicyclePlanner->setMaximumIntegratorStepSize(m_dT); + ok = ok && unicyclePlanner->setMaxStepLength(maxStepLength); + ok = ok && unicyclePlanner->setWidthSetting(minWidth, m_nominalWidth); + ok = ok && unicyclePlanner->setMaxAngleVariation(maxAngleVariation); + ok = ok && unicyclePlanner->setCostWeights(positionWeight, timeWeight); + ok = ok && unicyclePlanner->setStepTimings(minStepDuration, + maxStepDuration, nominalDuration); + ok = ok && unicyclePlanner->setPlannerPeriod(m_dT); + ok = ok && unicyclePlanner->setMinimumAngleForNewSteps(minAngleVariation); + ok = ok && unicyclePlanner->setMinimumStepLength(minStepLength); + ok = ok && unicyclePlanner->setSlowWhenTurnGain(slowWhenTurningGain); + unicyclePlanner->addTerminalStep(false); + unicyclePlanner->startWithLeft(m_swingLeft); + unicyclePlanner->resetStartingFootIfStill(startWithSameFoot); + + ok = ok && m_trajectoryGenerator.setSwitchOverSwingRatio(switchOverSwingRatio); + ok = ok && m_trajectoryGenerator.setTerminalHalfSwitchTime(lastStepSwitchTime); + ok = ok && m_trajectoryGenerator.setPauseConditions(maxStepDuration, nominalDuration); + + if (m_useMinimumJerk) { + m_feetGenerator = m_trajectoryGenerator.addFeetMinimumJerkGenerator(); + } else { + m_feetGenerator = m_trajectoryGenerator.addFeetCubicSplineGenerator(); + } + ok = ok && m_feetGenerator->setStepHeight(stepHeight); + ok = ok && m_feetGenerator->setFootLandingVelocity(landingVelocity); + ok = ok && m_feetGenerator->setFootApexTime(apexTime); + ok = ok && m_feetGenerator->setPitchDelta(pitchDelta); - bool correctLeft; + m_heightGenerator = m_trajectoryGenerator.addCoMHeightTrajectoryGenerator(); + ok = ok && m_heightGenerator->setCoMHeightSettings(comHeight, comHeightDelta); + ok = ok && m_trajectoryGenerator.setMergePointRatio(mergePointRatio); - iDynTree::Vector2 desiredPoint; - iDynTree::Vector2 measuredPositionLeft, measuredPositionRight; - double measuredAngleLeft, measuredAngleRight; + m_dcmGenerator = m_trajectoryGenerator.addDCMTrajectoryGenerator(); + m_dcmGenerator->setFootOriginOffset(leftZMPDelta, rightZMPDelta); + m_dcmGenerator->setOmega(sqrt(9.81/comHeight)); - iDynTree::Vector2 DCMBoundaryConditionAtMergePointPosition; - iDynTree::Vector2 DCMBoundaryConditionAtMergePointVelocity; + m_correctLeft = true; - // wait until a new trajectory has to be evaluated. + if(ok) { - std::unique_lock lock(m_mutex); - m_conditionVariable.wait(lock, [&] { - return ((m_generatorState == GeneratorState::Called) || - (m_generatorState == GeneratorState::Closing)); - }); + // the mutex is automatically released when lock_guard goes out of its scope + std::lock_guard guard(m_mutex); - if (m_generatorState == GeneratorState::Closing) - break; + // change the state of the generator + m_generatorState = GeneratorState::FirstStep; - // set timings - dT = m_dT; - initTime = m_initTime; - endTime = initTime + m_plannerHorizon; - - // set desired point - desiredPoint = m_desiredPoint; + // start the thread + m_generatorThread = std::thread(&TrajectoryGenerator::computeThread, this); + } - // dcm boundary conditions - DCMBoundaryConditionAtMergePointPosition = - m_DCMBoundaryConditionAtMergePointPosition; - DCMBoundaryConditionAtMergePointVelocity = - m_DCMBoundaryConditionAtMergePointVelocity; + return ok; +} - // left foot - measuredPositionLeft(0) = m_measuredTransformLeft.getPosition()(0); - measuredPositionLeft(1) = m_measuredTransformLeft.getPosition()(1); - measuredAngleLeft = m_measuredTransformLeft.getRotation().asRPY()(2); +void TrajectoryGenerator::addTerminalStep(bool terminalStep) +{ + m_trajectoryGenerator.unicyclePlanner()->addTerminalStep(terminalStep); +} - // right foot - measuredPositionRight(0) = m_measuredTransformRight.getPosition()(0); - measuredPositionRight(1) = m_measuredTransformRight.getPosition()(1); - measuredAngleRight = m_measuredTransformRight.getRotation().asRPY()(2); +void TrajectoryGenerator::computeThread() +{ + while (true) + { + double initTime; + double endTime; + double dT; + + bool correctLeft; + + iDynTree::Vector2 desiredPoint; + iDynTree::Vector2 measuredPositionLeft, measuredPositionRight; + double measuredAngleLeft, measuredAngleRight; + + iDynTree::Vector2 DCMBoundaryConditionAtMergePointPosition; + iDynTree::Vector2 DCMBoundaryConditionAtMergePointVelocity; + + // wait until a new trajectory has to be evaluated. + { + std::unique_lock lock(m_mutex); + m_conditionVariable.wait(lock, [&]{return ((m_generatorState == GeneratorState::Called) + || (m_generatorState == GeneratorState::Closing));}); + + if(m_generatorState == GeneratorState::Closing) + break; + + // set timings + dT = m_dT ; + initTime = m_initTime; + endTime = initTime + m_plannerHorizon; + + // set desired point + desiredPoint = m_desiredPoint; + + // dcm boundary conditions + DCMBoundaryConditionAtMergePointPosition = m_DCMBoundaryConditionAtMergePointPosition; + DCMBoundaryConditionAtMergePointVelocity = m_DCMBoundaryConditionAtMergePointVelocity; + + // left foot + measuredPositionLeft(0) = m_measuredTransformLeft.getPosition()(0); + measuredPositionLeft(1) = m_measuredTransformLeft.getPosition()(1); + measuredAngleLeft = m_measuredTransformLeft.getRotation().asRPY()(2); + + // right foot + measuredPositionRight(0) = m_measuredTransformRight.getPosition()(0); + measuredPositionRight(1) = m_measuredTransformRight.getPosition()(1); + measuredAngleRight = m_measuredTransformRight.getRotation().asRPY()(2); + + correctLeft = m_correctLeft; + } + + // clear the old trajectory + std::shared_ptr unicyclePlanner = m_trajectoryGenerator.unicyclePlanner(); + unicyclePlanner->clearDesiredTrajectory(); + + // add new point + if(!unicyclePlanner->addDesiredTrajectoryPoint(endTime, desiredPoint)) + { + // something goes wrong + std::lock_guard guard(m_mutex); + m_generatorState = GeneratorState::Configured; + yError() << "[TrajectoryGenerator_Thread] Error while setting the new reference."; + break; + } + + iDynTree::Vector2 measuredPosition; + double measuredAngle; + measuredPosition = correctLeft ? measuredPositionLeft : measuredPositionRight; + measuredAngle = correctLeft ? measuredAngleLeft : measuredAngleRight; + + DCMInitialState initialState; + initialState.initialPosition = DCMBoundaryConditionAtMergePointPosition; + initialState.initialVelocity = DCMBoundaryConditionAtMergePointVelocity; + + if (!m_dcmGenerator->setDCMInitialState(initialState)) { + // something goes wrong + std::lock_guard guard(m_mutex); + m_generatorState = GeneratorState::Configured; + yError() << "[TrajectoryGenerator_Thread] Failed to set the initial state."; + break; + } + + if(m_trajectoryGenerator.reGenerate(initTime, dT, endTime, + correctLeft, measuredPosition, measuredAngle)) + { + std::lock_guard guard(m_mutex); + m_generatorState = GeneratorState::Returned; + continue; + } + else + { + std::lock_guard guard(m_mutex); + m_generatorState = GeneratorState::Configured; + yError() << "[TrajectoryGenerator_Thread] Failed in computing new trajectory."; + continue; + } + } +} - correctLeft = m_correctLeft; +bool TrajectoryGenerator::generateFirstTrajectories() +{ + // check if this step is the first one + { + std::lock_guard guard(m_mutex); + if(m_generatorState != GeneratorState::FirstStep) + { + yError() << "[generateFirstTrajectories] This is not the first step! How can I generate the first step?"; + return false; + } } - // clear the old trajectory - std::shared_ptr unicyclePlanner = - m_trajectoryGenerator.unicyclePlanner(); + // clear the all trajectory + std::shared_ptr unicyclePlanner = m_trajectoryGenerator.unicyclePlanner(); unicyclePlanner->clearDesiredTrajectory(); - // add new point - if (!unicyclePlanner->addDesiredTrajectoryPoint(endTime, desiredPoint)) { - // something goes wrong - std::lock_guard guard(m_mutex); - m_generatorState = GeneratorState::Configured; - yError() << "[TrajectoryGenerator_Thread] Error while setting the new " - "reference."; - break; - } - - iDynTree::Vector2 measuredPosition; - double measuredAngle; - measuredPosition = - correctLeft ? measuredPositionLeft : measuredPositionRight; - measuredAngle = correctLeft ? measuredAngleLeft : measuredAngleRight; - - DCMInitialState initialState; - initialState.initialPosition = DCMBoundaryConditionAtMergePointPosition; - initialState.initialVelocity = DCMBoundaryConditionAtMergePointVelocity; - - if (!m_dcmGenerator->setDCMInitialState(initialState)) { - // something goes wrong - std::lock_guard guard(m_mutex); - m_generatorState = GeneratorState::Configured; - yError() - << "[TrajectoryGenerator_Thread] Failed to set the initial state."; - break; - } - - if (m_trajectoryGenerator.reGenerate(initTime, dT, endTime, correctLeft, - measuredPosition, measuredAngle)) { - std::lock_guard guard(m_mutex); - m_generatorState = GeneratorState::Returned; - continue; - } else { - std::lock_guard guard(m_mutex); - m_generatorState = GeneratorState::Configured; - yError() - << "[TrajectoryGenerator_Thread] Failed in computing new trajectory."; - continue; + // clear left and right footsteps + m_trajectoryGenerator.getLeftFootPrint()->clearSteps(); + m_trajectoryGenerator.getRightFootPrint()->clearSteps(); + + // set initial and final times + double initTime = 0; + double endTime = initTime + m_plannerHorizon; + + // at the beginning iCub has to stop + m_desiredPoint(0) = m_referencePointDistance(0); + m_desiredPoint(1) = m_referencePointDistance(1); + + // add the initial point + if(!unicyclePlanner->addDesiredTrajectoryPoint(initTime, m_referencePointDistance)) + { + yError() << "[generateFirstTrajectories] Error while setting the first reference."; + return false; } - } -} -bool TrajectoryGenerator::generateFirstTrajectories() { - // check if this step is the first one - { - std::lock_guard guard(m_mutex); - if (m_generatorState != GeneratorState::FirstStep) { - yError() << "[generateFirstTrajectories] This is not the first step! How " - "can I generate the first step?"; - return false; - } - } - - // clear the all trajectory - std::shared_ptr unicyclePlanner = - m_trajectoryGenerator.unicyclePlanner(); - unicyclePlanner->clearDesiredTrajectory(); - - // clear left and right footsteps - m_trajectoryGenerator.getLeftFootPrint()->clearSteps(); - m_trajectoryGenerator.getRightFootPrint()->clearSteps(); - - // set initial and final times - double initTime = 0; - double endTime = initTime + m_plannerHorizon; - - // at the beginning iCub has to stop - m_desiredPoint(0) = m_referencePointDistance(0); - m_desiredPoint(1) = m_referencePointDistance(1); - - // add the initial point - if (!unicyclePlanner->addDesiredTrajectoryPoint(initTime, - m_referencePointDistance)) { - yError() << "[generateFirstTrajectories] Error while setting the first " - "reference."; - return false; - } - - // add the final point - if (!unicyclePlanner->addDesiredTrajectoryPoint(endTime, m_desiredPoint)) { - yError() - << "[generateFirstTrajectories] Error while setting the new reference."; - return false; - } - - // generate the first trajectories - if (!m_trajectoryGenerator.generate(initTime, m_dT, endTime)) { - yError() << "[generateFirstTrajectories] Error while computing the first " - "trajectories."; - return false; - } - - m_generatorState = GeneratorState::Returned; - return true; + // add the final point + if(!unicyclePlanner->addDesiredTrajectoryPoint(endTime, m_desiredPoint)) + { + yError() << "[generateFirstTrajectories] Error while setting the new reference."; + return false; + } + + // generate the first trajectories + if(!m_trajectoryGenerator.generate(initTime, m_dT, endTime)) + { + yError() << "[generateFirstTrajectories] Error while computing the first trajectories."; + return false; + } + + m_generatorState = GeneratorState::Returned; + return true; } -bool TrajectoryGenerator::generateFirstTrajectories( - const iDynTree::Transform &leftToRightTransform) -// const iDynTree::Position &initialCOMPosition) +bool TrajectoryGenerator::generateFirstTrajectories(const iDynTree::Transform &leftToRightTransform) + // const iDynTree::Position &initialCOMPosition) { - // check if this step is the first one - { - std::lock_guard guard(m_mutex); - if (m_generatorState != GeneratorState::FirstStep) { - yError() << "[generateFirstTrajectories] This is not the first step! How " - "can I generate the first step?"; - return false; - } - } - - // clear the all trajectory - std::shared_ptr unicyclePlanner = - m_trajectoryGenerator.unicyclePlanner(); - unicyclePlanner->clearDesiredTrajectory(); - - // clear left and right footsteps - m_trajectoryGenerator.getLeftFootPrint()->clearSteps(); - m_trajectoryGenerator.getRightFootPrint()->clearSteps(); - - // set initial and final times - double initTime = 0; - double endTime = initTime + m_plannerHorizon; - - // at the beginning iCub has to stop - m_desiredPoint(0) = m_referencePointDistance(0); - m_desiredPoint(1) = m_referencePointDistance(1); - - // add the initial point - if (!unicyclePlanner->addDesiredTrajectoryPoint(initTime, - m_referencePointDistance)) { - yError() << "[generateFirstTrajectories] Error while setting the first " - "reference."; - return false; - } - - // add the final point - if (!unicyclePlanner->addDesiredTrajectoryPoint(endTime, m_desiredPoint)) { - yError() - << "[generateFirstTrajectories] Error while setting the new reference."; - return false; - } - - // add real position of the feet - std::shared_ptr left, right; - - left = m_trajectoryGenerator.getLeftFootPrint(); - left->clearSteps(); - right = m_trajectoryGenerator.getRightFootPrint(); - right->clearSteps(); - - iDynTree::Vector2 leftPosition, rightPosition; - double leftAngle, rightAngle; - - // if the standing foot is the right -> the unicycle starts parallel to the - // right foot - if (m_swingLeft) { - rightPosition(0) = 0.0; - rightPosition(1) = -leftToRightTransform.inverse().getPosition()(1) / 2; - rightAngle = 0; - right->addStep(rightPosition, rightAngle, 0.0); - - leftPosition(0) = leftToRightTransform.inverse().getPosition()(0); - leftPosition(1) = leftToRightTransform.inverse().getPosition()(1) / 2; - leftAngle = leftToRightTransform.inverse().getRotation().asRPY()(2); - left->addStep(leftPosition, leftAngle, 0.0); - } else { - leftPosition(0) = 0.0; - leftPosition(1) = -leftToRightTransform.getPosition()(1) / 2; - leftAngle = 0; - left->addStep(leftPosition, leftAngle, 0.0); - - rightPosition(0) = leftToRightTransform.getPosition()(0); - rightPosition(1) = leftToRightTransform.getPosition()(1) / 2; - rightAngle = leftToRightTransform.getRotation().asRPY()(2); - right->addStep(rightPosition, rightAngle, 0.0); - } - - // generate the first trajectories - if (!m_trajectoryGenerator.generate(initTime, m_dT, endTime)) { - yError() << "[generateFirstTrajectories] Error while computing the first " - "trajectories."; - return false; - } - - m_generatorState = GeneratorState::Returned; - return true; -} + // check if this step is the first one + { + std::lock_guard guard(m_mutex); + if(m_generatorState != GeneratorState::FirstStep) + { + yError() << "[generateFirstTrajectories] This is not the first step! How can I generate the first step?"; + return false; + } + } -bool TrajectoryGenerator::updateTrajectories( - double initTime, - const iDynTree::Vector2 &DCMBoundaryConditionAtMergePointPosition, - const iDynTree::Vector2 &DCMBoundaryConditionAtMergePointVelocity, - bool correctLeft, const iDynTree::Transform &measured, - const iDynTree::Vector2 &desiredPosition) { - { - std::lock_guard guard(m_mutex); + // clear the all trajectory + std::shared_ptr unicyclePlanner = m_trajectoryGenerator.unicyclePlanner(); + unicyclePlanner->clearDesiredTrajectory(); - if (m_generatorState == GeneratorState::Called) { - yError() << "[updateTrajectories_one correction] Cannot launch the " - "generator twice. " - << "Please wait until the trajectory has be evaluated ."; - return false; - } - - if (m_generatorState != GeneratorState::Returned) { - yError() << "[updateTrajectories_one correction] The trajectory " - "generator has not computed any trajectory yet. " - << "Please call 'generateFirstTrajectories()' method."; - return false; - } - } - // if correctLeft is true the stance foot is the true. - // The vector (expressed in the unicycle reference frame from the left foot to - // the center of the unicycle is [0, width/2]') - iDynTree::Vector2 unicyclePositionFromStanceFoot; - unicyclePositionFromStanceFoot(0) = 0.0; - unicyclePositionFromStanceFoot(1) = - correctLeft ? -m_nominalWidth / 2 : m_nominalWidth / 2; - - iDynTree::Vector2 desredPositionFromStanceFoot; - iDynTree::toEigen(desredPositionFromStanceFoot) = - iDynTree::toEigen(unicyclePositionFromStanceFoot) + - iDynTree::toEigen(m_referencePointDistance) + - iDynTree::toEigen(desiredPosition); - - // prepare the rotation matrix w_R_{unicycle} - double theta = measured.getRotation().asRPY()(2); - double s_theta = std::sin(theta); - double c_theta = std::cos(theta); - - // save the data - { - std::lock_guard guard(m_mutex); + // clear left and right footsteps + m_trajectoryGenerator.getLeftFootPrint()->clearSteps(); + m_trajectoryGenerator.getRightFootPrint()->clearSteps(); + + // set initial and final times + double initTime = 0; + double endTime = initTime + m_plannerHorizon; - // apply the homogeneous transformation w_H_{unicycle} - m_desiredPoint(0) = c_theta * desredPositionFromStanceFoot(0) - - s_theta * desredPositionFromStanceFoot(1) + - measured.getPosition()(0); - m_desiredPoint(1) = s_theta * desredPositionFromStanceFoot(0) + - c_theta * desredPositionFromStanceFoot(1) + - measured.getPosition()(1); + // at the beginning iCub has to stop + m_desiredPoint(0) = m_referencePointDistance(0); + m_desiredPoint(1) = m_referencePointDistance(1); - m_initTime = initTime; + // add the initial point + if(!unicyclePlanner->addDesiredTrajectoryPoint(initTime, m_referencePointDistance)) + { + yError() << "[generateFirstTrajectories] Error while setting the first reference."; + return false; + } - // Boundary condition - m_DCMBoundaryConditionAtMergePointPosition = - DCMBoundaryConditionAtMergePointPosition; - m_DCMBoundaryConditionAtMergePointVelocity = - DCMBoundaryConditionAtMergePointVelocity; + // add the final point + if(!unicyclePlanner->addDesiredTrajectoryPoint(endTime, m_desiredPoint)) + { + yError() << "[generateFirstTrajectories] Error while setting the new reference."; + return false; + } - m_correctLeft = correctLeft; + // add real position of the feet + std::shared_ptr left, right; - if (correctLeft) - m_measuredTransformLeft = measured; + left = m_trajectoryGenerator.getLeftFootPrint(); + left->clearSteps(); + right = m_trajectoryGenerator.getRightFootPrint(); + right->clearSteps(); + + iDynTree::Vector2 leftPosition, rightPosition; + double leftAngle, rightAngle; + + // if the standing foot is the right -> the unicycle starts parallel to the right foot + if(m_swingLeft) + { + rightPosition(0) = 0.0; + rightPosition(1) = -leftToRightTransform.inverse().getPosition()(1)/2; + rightAngle = 0; + right->addStep(rightPosition, rightAngle, 0.0); + + leftPosition(0) = leftToRightTransform.inverse().getPosition()(0); + leftPosition(1) = leftToRightTransform.inverse().getPosition()(1)/2; + leftAngle = leftToRightTransform.inverse().getRotation().asRPY()(2); + left->addStep(leftPosition, leftAngle, 0.0); + } else - m_measuredTransformRight = measured; + { + leftPosition(0) = 0.0; + leftPosition(1) = -leftToRightTransform.getPosition()(1)/2; + leftAngle = 0; + left->addStep(leftPosition, leftAngle, 0.0); + + rightPosition(0) = leftToRightTransform.getPosition()(0); + rightPosition(1) = leftToRightTransform.getPosition()(1)/2; + rightAngle = leftToRightTransform.getRotation().asRPY()(2); + right->addStep(rightPosition, rightAngle, 0.0); + } + + // generate the first trajectories + if(!m_trajectoryGenerator.generate(initTime, m_dT, endTime)) + { + yError() << "[generateFirstTrajectories] Error while computing the first trajectories."; + return false; + } + + m_generatorState = GeneratorState::Returned; + return true; +} + +bool TrajectoryGenerator::updateTrajectories(double initTime, const iDynTree::Vector2& DCMBoundaryConditionAtMergePointPosition, + const iDynTree::Vector2& DCMBoundaryConditionAtMergePointVelocity, bool correctLeft, + const iDynTree::Transform& measured, const iDynTree::Vector2& desiredPosition) +{ + { + std::lock_guard guard(m_mutex); + + if(m_generatorState == GeneratorState::Called) + { + yError() << "[updateTrajectories_one correction] Cannot launch the generator twice. " + << "Please wait until the trajectory has be evaluated ."; + return false; + } + + if(m_generatorState != GeneratorState::Returned) + { + yError() << "[updateTrajectories_one correction] The trajectory generator has not computed any trajectory yet. " + << "Please call 'generateFirstTrajectories()' method."; + return false; + } + } + // if correctLeft is true the stance foot is the true. + // The vector (expressed in the unicycle reference frame from the left foot to the center of the + // unicycle is [0, width/2]') + iDynTree::Vector2 unicyclePositionFromStanceFoot; + unicyclePositionFromStanceFoot(0) = 0.0; + unicyclePositionFromStanceFoot(1) = correctLeft ? -m_nominalWidth/2 : m_nominalWidth/2; + + iDynTree::Vector2 desredPositionFromStanceFoot; + iDynTree::toEigen(desredPositionFromStanceFoot) = iDynTree::toEigen(unicyclePositionFromStanceFoot) + + iDynTree::toEigen(m_referencePointDistance) + iDynTree::toEigen(desiredPosition); + + // prepare the rotation matrix w_R_{unicycle} + double theta = measured.getRotation().asRPY()(2); + double s_theta = std::sin(theta); + double c_theta = std::cos(theta); + + // save the data + { + std::lock_guard guard(m_mutex); - m_generatorState = GeneratorState::Called; - } + // apply the homogeneous transformation w_H_{unicycle} + m_desiredPoint(0) = c_theta * desredPositionFromStanceFoot(0) + - s_theta * desredPositionFromStanceFoot(1) + measured.getPosition()(0); + m_desiredPoint(1) = s_theta * desredPositionFromStanceFoot(0) + + c_theta * desredPositionFromStanceFoot(1) + measured.getPosition()(1); - m_conditionVariable.notify_one(); + m_initTime = initTime; - return true; + // Boundary condition + m_DCMBoundaryConditionAtMergePointPosition = DCMBoundaryConditionAtMergePointPosition; + m_DCMBoundaryConditionAtMergePointVelocity = DCMBoundaryConditionAtMergePointVelocity; + + m_correctLeft = correctLeft; + + if(correctLeft) + m_measuredTransformLeft = measured; + else + m_measuredTransformRight = measured; + + m_generatorState = GeneratorState::Called; + } + + m_conditionVariable.notify_one(); + + return true; } -bool TrajectoryGenerator::isTrajectoryComputed() { - std::lock_guard guard(m_mutex); - return m_generatorState == GeneratorState::Returned; +bool TrajectoryGenerator::isTrajectoryComputed() +{ + std::lock_guard guard(m_mutex); + return m_generatorState == GeneratorState::Returned; } -bool TrajectoryGenerator::isTrajectoryAsked() { - std::lock_guard guard(m_mutex); - return m_generatorState == GeneratorState::Called; +bool TrajectoryGenerator::isTrajectoryAsked() +{ + std::lock_guard guard(m_mutex); + return m_generatorState == GeneratorState::Called; } -bool TrajectoryGenerator::getDCMPositionTrajectory( - std::vector &DCMPositionTrajectory) { - if (!isTrajectoryComputed()) { - yError() << "[getDCMPositionTrajectory] No trajectories are available"; - return false; - } +bool TrajectoryGenerator::getDCMPositionTrajectory(std::vector& DCMPositionTrajectory) +{ + if(!isTrajectoryComputed()) + { + yError() << "[getDCMPositionTrajectory] No trajectories are available"; + return false; + } - DCMPositionTrajectory = m_dcmGenerator->getDCMPosition(); - return true; + DCMPositionTrajectory = m_dcmGenerator->getDCMPosition(); + return true; } -bool TrajectoryGenerator::getDCMVelocityTrajectory( - std::vector &DCMVelocityTrajectory) { - if (!isTrajectoryComputed()) { - yError() << "[getDCMVelocityTrajectory] No trajectories are available"; - return false; - } +bool TrajectoryGenerator::getDCMVelocityTrajectory(std::vector& DCMVelocityTrajectory) +{ + if(!isTrajectoryComputed()) + { + yError() << "[getDCMVelocityTrajectory] No trajectories are available"; + return false; + } - DCMVelocityTrajectory = m_dcmGenerator->getDCMVelocity(); - return true; + DCMVelocityTrajectory = m_dcmGenerator->getDCMVelocity(); + return true; } -bool TrajectoryGenerator::getFeetTrajectories( - std::vector &lFootTrajectory, - std::vector &rFootTrajectory) { - if (!isTrajectoryComputed()) { - yError() << "[getFeetTrajectories] No trajectories are available"; - return false; - } +bool TrajectoryGenerator::getFeetTrajectories(std::vector& lFootTrajectory, + std::vector& rFootTrajectory) +{ + if(!isTrajectoryComputed()) + { + yError() << "[getFeetTrajectories] No trajectories are available"; + return false; + } - m_feetGenerator->getFeetTrajectories(lFootTrajectory, rFootTrajectory); + m_feetGenerator->getFeetTrajectories(lFootTrajectory, rFootTrajectory); - return true; + return true; } -bool TrajectoryGenerator::getFeetTwist( - std::vector &lFootTwist, - std::vector &rFootTwist) { - if (!isTrajectoryComputed()) { - yError() << "[getFeetTwist] No trajectories are available"; - return false; - } +bool TrajectoryGenerator::getFeetTwist(std::vector& lFootTwist, + std::vector& rFootTwist) +{ + if(!isTrajectoryComputed()) + { + yError() << "[getFeetTwist] No trajectories are available"; + return false; + } - m_feetGenerator->getFeetTwistsInMixedRepresentation(lFootTwist, rFootTwist); + m_feetGenerator->getFeetTwistsInMixedRepresentation(lFootTwist, rFootTwist); - return true; + return true; } -bool TrajectoryGenerator::getWhenUseLeftAsFixed( - std::vector &isLeftFixedFrame) { - if (!isTrajectoryComputed()) { - yError() << "[getWhenUseLeftAsFixed] No trajectories are available"; - return false; - } +bool TrajectoryGenerator::getWhenUseLeftAsFixed(std::vector& isLeftFixedFrame) +{ + if(!isTrajectoryComputed()) + { + yError() << "[getWhenUseLeftAsFixed] No trajectories are available"; + return false; + } - m_trajectoryGenerator.getWhenUseLeftAsFixed(isLeftFixedFrame); - return true; + m_trajectoryGenerator.getWhenUseLeftAsFixed(isLeftFixedFrame); + return true; } -bool TrajectoryGenerator::getFeetStandingPeriods( - std::vector &lFootContacts, std::vector &rFootContacts) { - if (!isTrajectoryComputed()) { - yError() << "[getFeetStandingPeriods] No trajectories are available"; - return false; - } - m_trajectoryGenerator.getFeetStandingPeriods(lFootContacts, rFootContacts); - return true; +bool TrajectoryGenerator::getFeetStandingPeriods(std::vector& lFootContacts, + std::vector& rFootContacts) +{ + if(!isTrajectoryComputed()) + { + yError() << "[getFeetStandingPeriods] No trajectories are available"; + return false; + } + + m_trajectoryGenerator.getFeetStandingPeriods(lFootContacts, rFootContacts); + return true; } -bool TrajectoryGenerator::getCoMHeightTrajectory( - std::vector &CoMHeightTrajectory) { - if (!isTrajectoryComputed()) { - yError() << "[getCoMHeightTrajectory] No trajectories are available"; - return false; - } +bool TrajectoryGenerator::getCoMHeightTrajectory(std::vector& CoMHeightTrajectory) +{ + if(!isTrajectoryComputed()) + { + yError() << "[getCoMHeightTrajectory] No trajectories are available"; + return false; + } - m_heightGenerator->getCoMHeightTrajectory(CoMHeightTrajectory); - return true; + m_heightGenerator->getCoMHeightTrajectory(CoMHeightTrajectory); + return true; } -bool TrajectoryGenerator::getCoMHeightVelocity( - std::vector &CoMHeightVelocity) { - if (!isTrajectoryComputed()) { - yError() << "[getCoMHeightVelocity] No trajectories are available"; - return false; - } +bool TrajectoryGenerator::getCoMHeightVelocity(std::vector& CoMHeightVelocity) +{ + if(!isTrajectoryComputed()) + { + yError() << "[getCoMHeightVelocity] No trajectories are available"; + return false; + } - m_heightGenerator->getCoMHeightVelocity(CoMHeightVelocity); - return true; + m_heightGenerator->getCoMHeightVelocity(CoMHeightVelocity); + return true; } -bool TrajectoryGenerator::getMergePoints(std::vector &mergePoints) { - if (!isTrajectoryComputed()) { - yError() << "[getMergePoints] No trajectories are available"; - return false; - } +bool TrajectoryGenerator::getMergePoints(std::vector& mergePoints) +{ + if(!isTrajectoryComputed()) + { + yError() << "[getMergePoints] No trajectories are available"; + return false; + } - m_trajectoryGenerator.getMergePoints(mergePoints); - return true; + m_trajectoryGenerator.getMergePoints(mergePoints); + return true; } -bool TrajectoryGenerator::getWeightPercentage( - std::vector &weightInLeft, std::vector &weightInRight) { - if (!isTrajectoryComputed()) { - yError() << "[getWeightPercentage] No trajectories are available"; - return false; - } +bool TrajectoryGenerator::getWeightPercentage(std::vector &weightInLeft, + std::vector &weightInRight) +{ + if(!isTrajectoryComputed()) + { + yError() << "[getWeightPercentage] No trajectories are available"; + return false; + } - m_dcmGenerator->getWeightPercentage(weightInLeft, weightInRight); - return true; + m_dcmGenerator->getWeightPercentage(weightInLeft, weightInRight); + return true; } -void TrajectoryGenerator::reset() { - // the mutex is automatically released when lock_guard goes out of its scope - std::lock_guard guard(m_mutex); +void TrajectoryGenerator::reset() +{ + // the mutex is automatically released when lock_guard goes out of its scope + std::lock_guard guard(m_mutex); - // change the state of the generator - m_generatorState = GeneratorState::FirstStep; + // change the state of the generator + m_generatorState = GeneratorState::FirstStep; } From f3c1f494f0af97a663f068cdbf0926c84a498575 Mon Sep 17 00:00:00 2001 From: Emilio Benenati <34814729+bemilio@users.noreply.github.com> Date: Tue, 17 Mar 2020 10:45:16 +0100 Subject: [PATCH 3/5] Update CHANGELOG.md --- CHANGELOG.md | 1 + 1 file changed, 1 insertion(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index 2ef5d661..3aeaaa25 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -17,6 +17,7 @@ All notable changes to this project are documented in this file. - `TrajectoryPlanner` library related to trajectory planner - `KinDynWrapper` iDynTree `KinDynComputation` wrapper. - `RetargetingClient` client for the retargeting +- `TrajectoryGenerator` class of the `TrajectoryPlanner` library includes now the method `getWeightPercentage` to retrieve the amount of weight on each foot requested by the planner. ## [0.2.0] - 2019-10-24 ### Added From 9a60aac25d463b8c646d6a514fa01fd11b513f3d Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Thu, 26 Mar 2020 13:24:59 +0100 Subject: [PATCH 4/5] Dump version to 0.4.102 --- CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 8a2c54b4..87112036 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -7,7 +7,7 @@ set(CMAKE_CXX_STANDARD 14) ## MAIN project project(WalkingControllers - VERSION 0.4.101) + VERSION 0.4.102) # Defines the CMAKE_INSTALL_LIBDIR, CMAKE_INSTALL_BINDIR and many other useful macros. # See https://cmake.org/cmake/help/latest/module/GNUInstallDirs.html From 204a113b56908e83675679d6621c65e28451d2d3 Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Thu, 26 Mar 2020 13:25:19 +0100 Subject: [PATCH 5/5] Update CHANGELOG.md --- CHANGELOG.md | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 02b2f609..e060bd7d 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -4,11 +4,14 @@ All notable changes to this project are documented in this file. ## [Unreleased] ### Added - Adding the possibility to use Gazebo base data inside the walking controller +- `TrajectoryGenerator` class of the `TrajectoryPlanner` library includes now + the method `getWeightPercentage` to retrieve the amount of weight on each foot + requested by the planner. ### Changed - Adding the `use_external_robot_base` parameter inside the `dcm_walking_with_joypad.ini` - Adding the Gazebo base data port inside the `robotControl.ini` -- Tunning the `zmpControllerParams.ini` and `dcmReactiveControllerParams.ini` +- Tunning the `zmpControllerParams.ini` and `dcmReactiveControllerParams.ini` - Modifying the follwoing classes for geting and using Gazebo base data: - `/KinDynWrapper/Wrapper` - `RobotInterface/Helper` @@ -40,7 +43,6 @@ All notable changes to this project are documented in this file. - `TrajectoryPlanner` library related to trajectory planner - `KinDynWrapper` iDynTree `KinDynComputation` wrapper. - `RetargetingClient` client for the retargeting -- `TrajectoryGenerator` class of the `TrajectoryPlanner` library includes now the method `getWeightPercentage` to retrieve the amount of weight on each foot requested by the planner. ## [0.2.0] - 2019-10-24 ### Added