diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 2c8f716b09572b4db7eef46a1b66564687e4b8db..0c8e90b5c2f1541cca4f3238fecbff9a7189704e 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -58,8 +58,6 @@ SET(SOURCES ZMPRefTrajectoryGeneration/AnalyticalMorisawaCompact.cpp ZMPRefTrajectoryGeneration/FilteringAnalyticalTrajectoryByPreviewControl.cpp ZMPRefTrajectoryGeneration/qp-problem.cpp - ZMPRefTrajectoryGeneration/generator-vel-ref.cpp - ZMPRefTrajectoryGeneration/mpc-trajectory-generation.cpp MotionGeneration/StepOverPlanner.cpp MotionGeneration/CollisionDetector.cpp MotionGeneration/WaistHeightVariation.cpp diff --git a/src/ZMPRefTrajectoryGeneration/generator-vel-ref.cpp b/src/ZMPRefTrajectoryGeneration/generator-vel-ref.cpp deleted file mode 100644 index 0b1ac7b5b6d69db7bbd62ad1fb7e5df62d51269f..0000000000000000000000000000000000000000 --- a/src/ZMPRefTrajectoryGeneration/generator-vel-ref.cpp +++ /dev/null @@ -1,183 +0,0 @@ -/* - * Copyright 2010, - * - * Andrei Herdt - * Olivier Stasse - * - * JRL, CNRS/AIST - * - * This file is part of walkGenJrl. - * walkGenJrl is free software: you can redistribute it and/or modify - * it under the terms of the GNU Lesser General Public License as published by - * the Free Software Foundation, either version 3 of the License, or - * (at your option) any later version. - * - * walkGenJrl is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Lesser Public License for more details. - * You should have received a copy of the GNU Lesser General Public License - * along with walkGenJrl. If not, see <http://www.gnu.org/licenses/>. - * - * Research carried out within the scope of the - * Joint Japanese-French Robotics Laboratory (JRL) - */ -/*! This object constructs a QP as proposed by Herdt IROS 2010. - */ - - -#include <ZMPRefTrajectoryGeneration/generator-vel-ref.hh> - -using namespace PatternGeneratorJRL; - - -GeneratorVelRef::GeneratorVelRef(SimplePluginManager *lSPM, std::string DataFile, - CjrlHumanoidDynamicRobot *aHS) : MPCTrajectoryGeneration(lSPM) -{ - //TODO: -} - - -GeneratorVelRef::~GeneratorVelRef() -{ - //TODO: -} - - -void -GeneratorVelRef::CallMethod(std::string &Method, std::istringstream &strm) -{ - GeneratorVelRef::CallMethod(Method,strm); -} - - -void -GeneratorVelRef::setPonderation(double alpha, double beta, double gamma, double delta) -{ - //TODO: -} - - -void -GeneratorVelRef::setReference(std::istringstream &strm) -{ - //TODO: -} - - -void -GeneratorVelRef::setReference(double dx, double dy, double dyaw) -{ - //TODO: -} - - -void -GeneratorVelRef::buildConstantPartOfObjective(QPProblem & Pb) -{ - //TODO: -} - - -void -GeneratorVelRef::addEqConstraint(std::deque<LinearConstraintInequalityFreeFeet_t> ConstraintsDeque, - MAL_MATRIX (&DU, double), MAL_MATRIX (&DS, double)) -{ - //TODO: -} - - -void -GeneratorVelRef::addIneqConstraint(std::deque<LinearConstraintInequalityFreeFeet_t> ConstraintsDeque, - MAL_MATRIX (&DU, double), MAL_MATRIX (&DS, double)) -{ - //TODO: -} - - -void -GeneratorVelRef::generateZMPConstraints (CjrlFoot & Foot, - std::deque < double > & PreviewedSupportAngles, - SupportFSM & FSM, - SupportState_t & CurrentSupportState, - QPProblem & Pb) -{ - //TODO: -} - - -void -GeneratorVelRef::generateFeetPosConstraints (CjrlFoot & Foot, - std::deque < double > & PreviewedSupportAngles, - SupportFSM & , - SupportState_t &, - QPProblem & Pb) -{ - //TODO: -} - - -void -GeneratorVelRef::computeObjective(QPProblem & Pb, IntermedQPMat & QPMat, std::deque<SupportState_t> PrwSupStates) -{ - //TODO: -} - - -int -GeneratorVelRef::initConstants(QPProblem & Pb) -{ - //TODO: -} - - -void -GeneratorVelRef::setCoMPerturbationForce(double Fx, double Fy, - LinearizedInvertedPendulum2D & CoM) -{ - //TODO: -} - - -void -GeneratorVelRef::setCoMPerturbationForce(std::istringstream &strm, - LinearizedInvertedPendulum2D & CoM) -{ - //TODO: -} - - -void -GeneratorVelRef::addLeastSquaresTerm(ObjectiveTerm_t & type, QPProblem & Pb, int NbSt) -{ - //TODO: -} - - -int -GeneratorVelRef::buildConstantPartOfConstraintMatrices() -{ - //TODO: -} - - -int -GeneratorVelRef::buildConstantPartOfTheObjectiveFunction() -{ - //TODO: -} - - -int -GeneratorVelRef::initializeMatrixPbConstants() -{ - //TODO: -} - - -int -GeneratorVelRef::dumpProblem(MAL_VECTOR(& xk,double), - double Time) -{ - //TODO: -} diff --git a/src/ZMPRefTrajectoryGeneration/generator-vel-ref.hh b/src/ZMPRefTrajectoryGeneration/generator-vel-ref.hh deleted file mode 100644 index 6c0ee86d9c2078b6baf566442c3cb3ed3f1a94f9..0000000000000000000000000000000000000000 --- a/src/ZMPRefTrajectoryGeneration/generator-vel-ref.hh +++ /dev/null @@ -1,264 +0,0 @@ -/* - * Copyright 2010, - * - * Andrei Herdt - * Olivier Stasse - * - * JRL, CNRS/AIST - * - * This file is part of walkGenJrl. - * walkGenJrl is free software: you can redistribute it and/or modify - * it under the terms of the GNU Lesser General Public License as published by - * the Free Software Foundation, either version 3 of the License, or - * (at your option) any later version. - * - * walkGenJrl is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Lesser Public License for more details. - * You should have received a copy of the GNU Lesser General Public License - * along with walkGenJrl. If not, see <http://www.gnu.org/licenses/>. - * - * Research carried out within the scope of the - * Joint Japanese-French Robotics Laboratory (JRL) - */ -/*! This object constructs a QP as proposed by Herdt IROS 2010. - */ - -#ifndef GENERATORVELREF_HH_ -#define GENERATORVELREF_HH_ - - -#include <ZMPRefTrajectoryGeneration/mpc-trajectory-generation.hh> - -#include <ZMPRefTrajectoryGeneration/qp-problem.hh> -#include <abstract-robot-dynamics/humanoid-dynamic-robot.hh> -#include <PreviewControl/SupportFSM.h> -#include <privatepgtypes.h> -#include <Mathematics/intermediate-qp-matrices.hh> -#include <PreviewControl/LinearizedInvertedPendulum2D.h> - -namespace PatternGeneratorJRL -{ - - class GeneratorVelRef : public MPCTrajectoryGeneration - { - - // - //Public methods - // - public: - /// \name Constructors and destructors. - /// \{ - GeneratorVelRef(SimplePluginManager *lSPM, std::string DataFile, - CjrlHumanoidDynamicRobot *aHS=0); - ~GeneratorVelRef(); - /// \} - - /// \brief Call method to handle the plugins (SimplePlugin interface). - void CallMethod(std::string &Method, std::istringstream &strm); - - /// \brief Set the weights on the objective terms - /// - /// \param alpha - /// \param beta - /// \param gamma - /// \param delta - void setPonderation(double alpha, double beta, double gamma, double delta); - - /// \brief Set the velocity reference from string - /// - /// \param strm velocity reference string - void setReference(std::istringstream &strm); - - /// \brief Set the velocity reference from external reference - /// - /// \param dx - /// \param dy - /// \param dyaw - void setReference(double dx, double dy, double dyaw); - - /// \brief Build constant parts of the objective - void buildConstantPartOfObjective(QPProblem & Pb); - - /// \brief Add one equality constraint to the queue of constraints - /// - /// \param DU - /// \param DS - void addEqConstraint(std::deque<LinearConstraintInequalityFreeFeet_t> ConstraintsDeque, - MAL_MATRIX (&DU, double), MAL_MATRIX (&DS, double)); - - /// \brief Add one inequality constraint to the queue of constraints - /// - /// \param DU - /// \param DS - void addIneqConstraint(std::deque<LinearConstraintInequalityFreeFeet_t> ConstraintsDeque, - MAL_MATRIX (&DU, double), MAL_MATRIX (&DS, double)); - - /// \brief Generate a queue of inequality constraints on the ZMP for the whole preview window - /// - /// \param CjrlFoot - /// \param PreviewedSupportAngles - /// \param SupportFSM - /// \param CurrentSupportState - void generateZMPConstraints (CjrlFoot & Foot, - std::deque < double > & PreviewedSupportAngles, - SupportFSM & FSM, - SupportState_t & CurrentSupportState, - QPProblem & Pb); - - /// \brief Generate a queue of inequality constraints on the feet positions for the whole preview window - /// - /// \param Foot - /// \param PreviewedSupportAngles - /// \param SupportFSM - /// \param CurrentSupportState - void generateFeetPosConstraints (CjrlFoot & Foot, - std::deque < double > & PreviewedSupportAngles, - SupportFSM & , - SupportState_t &, - QPProblem & Pb); - - /// \brief Compute the objective matrices of a quadratic optimization problem - /// - /// \param Pb - void computeObjective(QPProblem & Pb, IntermedQPMat & QPMat, std::deque<SupportState_t> PrwSupStates); - - /// \brief Infitialize constant parts of the objective - /// \return A negative value in case of a problem 0 otherwise. - int initConstants(QPProblem & Pb); - - /// \brief Set the perturbation force on the CoM from external reference - /// - /// \param Fx - /// \param Fy - /// \param 2DLIPM - void setCoMPerturbationForce(double Fx, double Fy, LinearizedInvertedPendulum2D & CoM); - - /// \brief Set the perturbation force on the CoM from string - /// - /// \param strm - /// \param 2DLIPM - void setCoMPerturbationForce(std::istringstream & strm, - LinearizedInvertedPendulum2D & CoM); - - - // - //Private methods - // - private: - - /// \brief Compute the Least Squares objective term and add it to the optimization problem - void addLeastSquaresTerm(ObjectiveTerm_t & type, QPProblem & Pb, int NbSt); - - /// \brief Build the constant part of the constraint matrices. - int buildConstantPartOfConstraintMatrices(); - - /// \brief This method does the same once the previous method has been called - /// to compute the static part of the optimization function. - /// Assuming that the optimization function is of the form - /// \f$ min_{u_k} \frac{1}{2} u^{\top}_k Q u_k + p^{\top}_k u_k \f$ - /// this method computes \f$Q\f$, the constant part of $p^{\top}_k$. - int buildConstantPartOfTheObjectiveFunction(); - - - /// \brief Compute constant matrices over all the instances of the problem, i.e. - /// \f$P_{pu}, P_{px}, P_{vs}, P_{vu}\f$. - /// The necessary parameters to build those matrices are extracted from the - /// PreviewControl link. - int initializeMatrixPbConstants(); - - - /// \name Debugging related methods - /// @{ - /// \brief Dump the instance of the quadratic problem for one iteration. */ - int dumpProblem(MAL_VECTOR(& xk,double), - double Time); - /// @} - - - // - //Private members - // - private: - - /// \brief Current and previewed support states. - SupportState_t m_CurrentSupport, m_PrwSupport; - - /// \brief Current state of the trunk and the trunk state after m_QP_T - COMState m_TrunkState, m_TrunkStateT; - - /// Velocity reference (constant) - ReferenceAbsoluteVelocity m_RefVel; - - std::deque<LinearConstraintInequalityFreeFeet_t> m_ConstraintInequalitiesDeque; - std::deque<LinearConstraintInequalityFreeFeet_t> m_FeetPosInequalitiesDeque; - - /// \brief History of support states - std::deque<SupportFeet_t> m_SupportFeetDeque; - - /// \brief Previewed trunk states - std::deque<COMState> m_TrunkStatesDeque; - - /// \brief Additional term on the acceleration of the CoM - MAL_VECTOR(m_PerturbationAcceleration,double); - - /// \brief Mass of the robot - double m_RobotMass; - - /// \brief Perturbation - bool m_PerturbationOccured; - - /// \brief Distance between the feet - double m_FeetDistanceDS; - - /// \brief - bool m_EndingPhase; - - /// \brief - double m_TimeToStopOnLineMode; - - /// \brief Buffer to handle computational delay - double m_TimeBuffer; - - - /// \name Variables related to the QP - /// @{ - - /// \name Weights on objective functions - /// @{ - /// \brief Weight on the instant-velocity term in the objective - double m_Beta; - - /// \brief Weight on the jerk term in the objective. - double m_Alpha; - - /// \brief Weight on the ZMP term in the objective. - double m_Gamma; - - /// \brief Weight on the average-velocity term in the objective - double m_Delta; - /// @} - - /// \brief Sampling of the QP. - double m_QP_T; - - /// \brief Number of previewed samples - int m_QP_N; - - - /// \brief Constant part of the constraint matrices. - double * m_Pu; - /// @} - - - /// \brief Debugging variable - int m_FullDebug; - - /// \brief Fast formulations mode. - unsigned int m_FastFormulationMode; - - }; -} - -#endif /* GENERATORVELREF_HH_ */ diff --git a/src/ZMPRefTrajectoryGeneration/mpc-trajectory-generation.cpp b/src/ZMPRefTrajectoryGeneration/mpc-trajectory-generation.cpp deleted file mode 100644 index 89943532f45fb45004c186929486a6dfa3ee80cf..0000000000000000000000000000000000000000 --- a/src/ZMPRefTrajectoryGeneration/mpc-trajectory-generation.cpp +++ /dev/null @@ -1,111 +0,0 @@ -/* - * Copyright 2010, - * - * Andrei Herdt - * Olivier Stasse - * - * JRL, CNRS/AIST - * - * This file is part of walkGenJrl. - * walkGenJrl is free software: you can redistribute it and/or modify - * it under the terms of the GNU Lesser General Public License as published by - * the Free Software Foundation, either version 3 of the License, or - * (at your option) any later version. - * - * walkGenJrl is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Lesser Public License for more details. - * You should have received a copy of the GNU Lesser General Public License - * along with walkGenJrl. If not, see <http://www.gnu.org/licenses/>. - * - * Research carried out within the scope of the - * Joint Japanese-French Robotics Laboratory (JRL) - */ -/* \file mpc-trajectory-generation.cpp - \brief Abstract object for trajectory generation via model predictive control.*/ - -#include <Debug.h> -#include <ZMPRefTrajectoryGeneration/mpc-trajectory-generation.hh> - - -using namespace PatternGeneratorJRL; - -MPCTrajectoryGeneration::MPCTrajectoryGeneration(SimplePluginManager *lSPM) - : SimplePlugin(lSPM) - , m_Tsingle(0.) - , m_Tdble(0.) - , m_SamplingPeriodControl(0.) - , m_SamplingPeriodPreview(0.) - , m_PreviewControlTime(0.) - , m_StepHeight(0.) - , m_CurrentTime(0.) - , m_OnLineMode(false) - , m_CoMHeight(0.) - , m_SecurityMargin(0.) - , m_ModulationSupportCoefficient(0.) - , m_Omega(0.) -{ - - std::string aMethodName[6] = - {":omega", - ":stepheight", - ":singlesupporttime", - ":doublesupporttime", - ":comheight", - ":samplingperiod"}; - - for(int i=0;i<6;i++) - { - if (!RegisterMethod(aMethodName[i])) - { - std::cerr << "Unable to register " << aMethodName << std::endl; - } - else - { - ODEBUG("Succeed in registering " << aMethodName[i]); - } - - } -} - - -void MPCTrajectoryGeneration::CallMethod(std::string & Method, std::istringstream &strm) -{ - ODEBUG("Calling me (" << this << ") with Method: " << Method); - if (Method==":omega") - { - strm >> m_Omega; - } - else if (Method==":stepheight") - { - strm >> m_StepHeight; - ODEBUG("Value of stepheight " << m_StepHeight << " this:" << this); - } - else if (Method==":singlesupporttime") - { - strm >> m_Tsingle; - ODEBUG(":singlesupporttime " << m_Tsingle << " ID: " << this); - } - else if (Method==":doublesupporttime") - { - strm >> m_Tdble; - ODEBUG(":doublesupporttime " << m_Tdble << " ID: " << this); - } - else if (Method==":comheight") - { - strm >> m_CoMHeight; - ODEBUG(":comheight" << m_ComHeight << " ID: " << this); - } - else if (Method==":samplingperiod") - { - strm >> m_SamplingPeriodPreview; - ODEBUG(":samplingperiod" << m_SamplingPeriodPreview << " ID: " << this); - } - -} - -bool MPCTrajectoryGeneration::GetOnLineMode() -{ - return m_OnLineMode; -} diff --git a/src/ZMPRefTrajectoryGeneration/mpc-trajectory-generation.hh b/src/ZMPRefTrajectoryGeneration/mpc-trajectory-generation.hh deleted file mode 100644 index 1047a6bb219e4b4fdd24bece6dcbd6556ffcd6b3..0000000000000000000000000000000000000000 --- a/src/ZMPRefTrajectoryGeneration/mpc-trajectory-generation.hh +++ /dev/null @@ -1,216 +0,0 @@ -/* - * Copyright 2010, - * - * Andrei Herdt - * Olivier Stasse - * - * JRL, CNRS/AIST - * - * This file is part of walkGenJrl. - * walkGenJrl is free software: you can redistribute it and/or modify - * it under the terms of the GNU Lesser General Public License as published by - * the Free Software Foundation, either version 3 of the License, or - * (at your option) any later version. - * - * walkGenJrl is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Lesser Public License for more details. - * You should have received a copy of the GNU Lesser General Public License - * along with walkGenJrl. If not, see <http://www.gnu.org/licenses/>. - * - * Research carried out within the scope of the - * Joint Japanese-French Robotics Laboratory (JRL) - */ -/* \file mpc-trajectory-generation.cpp - \brief Abstract object for trajectory generation via model predictive control.*/ - -#ifndef _TRAJ_GEN_H_ -#define _TRAJ_GEN_H_ - -#include <jrl/mal/matrixabstractlayer.hh> - - -#include <deque> -#include <string> - -#include <jrl/walkgen/pgtypes.hh> -#include <SimplePlugin.h> - -namespace PatternGeneratorJRL -{ - - /*! This class defines an abstract interface to generate low-dimensional walking trajectories (CoM/CdP/Feet). - */ - class MPCTrajectoryGeneration : public SimplePlugin - { - - // - // Protected members - // - protected: - - /* ! \brief Time for single support. */ - double m_Tsingle; - - /* ! \brief Time for double support. */ - double m_Tdble; - - /// \brief Sampling periods control and preview - double m_SamplingPeriodControl, m_SamplingPeriodPreview; - - /* ! \brief Preview control window in second. */ - double m_PreviewControlTime; - - /* ! \brief Step height for the walking pattern. */ - double m_StepHeight; - - /* ! \brief Current time in the control loop (in seconds). */ - double m_CurrentTime; - - /*! \brief Specifies if we are or not in on line mode. */ - bool m_OnLineMode; - - /*! \brief Specifies Com Height. */ - double m_CoMHeight; - - /// \brief Security margins for the ZMP - double m_SecurityMargin; - - /// \name Members related to the generation of feet trajectories. - /// @{ - /// \brief ModulationSupportCoefficient coeeficient to wait a little before foot is of the ground - double m_ModulationSupportCoefficient; - - /// \brief The foot orientation for the lift off and the landing - double m_Omega; - /// @} - - - // - // Public methods - // - public: - - /// \brief Default constructor - MPCTrajectoryGeneration(SimplePluginManager * lSPM); - - /// \brief Default destructor. - virtual ~MPCTrajectoryGeneration() {}; - - /*! Set the current time. */ - void setCurrentTime(const double & aTime) - { m_CurrentTime = aTime;} - - /*! Get the current time. */ - double getCurrentTime() - { return m_CurrentTime;} - - /// \brief Set the length of the preview window. - inline void setPreviewLength(const double & aPP) - { m_PreviewControlTime = aPP;}; - - /// \brief Get the preview control time window. - inline const double & getPreviewLength( ) const - { return m_PreviewControlTime; }; - - /// \brief Returns the Com Height. - inline const double & getComHeight() const - { return m_CoMHeight; }; - - /// \brief Returns the Com Height. - inline void setComHeight(const double & aComHeight) - { m_CoMHeight = aComHeight; }; - - /// \brief Returns the single support time. - inline const double & getTSingleSupport() const - { return m_Tsingle; }; - - /// \brief Set the single support time. - inline void setTSingleSupport(const double & aTSingleSupport) - { m_Tsingle = aTSingleSupport; }; - - /// \brief Returns the double support time. - inline const double & getTDoubleSupport() const - {return m_Tdble;}; - - /// \brief Set the double support time. */ - inline void setTDoubleSupport(const double & aTdble) - { m_Tdble = aTdble;}; - - /// \brief Get the sampling period for the control, set to 0.005 by default. */ - inline const double & getSamplingPeriodControl() const - { return m_SamplingPeriodControl; }; - - /// \brief Set the sampling period for the control. */ - inline void setSamplingPeriodControl(const double &aSamplingPeriod) - { m_SamplingPeriodControl = aSamplingPeriod;}; - - /// \brief Get the sampling period for the preview, set to 0.100 by default. - inline const double & getSamplingPeriodPreview() const - { return m_SamplingPeriodPreview; }; - - /// \brief Set the sampling period for the preview. */ - inline void setSamplingPeriodPreview(const double &aSamplingPeriod) - { m_SamplingPeriodPreview = aSamplingPeriod;}; - - /// \brief Set the security margin for the zmp - /// - /// \param Margin - inline void setSecurityMargin(const double & Margin) - {m_SecurityMargin = Margin; }; - - - /// \name Methods related to the generation of feet trajectories. - /// @{ - /// \brief Returns the step height. - inline const double & getStepHeight() const - { return m_StepHeight;}; - - /// \brief Specify the step height. - inline void setStepHeight(const double & aSSH) - { m_StepHeight = aSSH;}; - - /// \brief Returns the ModulationSupportCoefficient. - inline const double &getModulationSupportCoefficient() const - { - return m_ModulationSupportCoefficient; - } - - /// \brief Specify the modulation support coefficient. - inline void setModulationSupportCoefficient(const double &af) - { - m_ModulationSupportCoefficient = af; - } - - /// \brief Set the pitch angle of foot when landing and taking off. - inline void setOmega(const double & anOmega) - { m_Omega = anOmega;}; - - /// \brief Get the pitch angle of foot when landing and taking off. - inline const double & getOmega(void) const - { return m_Omega;}; - /// @} - - - - /// \brief Handling methods for the plugin mecanism. - virtual void CallMethod(std::string & Method, std::istringstream &strm); - - /*! \name Methods related to the on line status generation of the ZMP. - @{ - */ - /*! \brief Returns the current status of the ZMP trajectory generator. - The online mode is determines internally. - A ZMP-generator can be still in on-line mode even the step-generator - is not because the ZMP-generator is generating the ending phase. - */ - bool GetOnLineMode(); - /// @} */ - - - - }; -} - -#endif /* _TRAJ_GEN_H_ */