├── .gitignore ├── CMakeLists.txt ├── README.md ├── TODO.txt ├── mpc-walkgen ├── api.h ├── constant.h ├── convexpolygon.h ├── function │ ├── humanoid_cop_centering_objective.h │ ├── humanoid_cop_constraint.h │ ├── humanoid_foot_constraint.h │ ├── humanoid_lip_com_jerk_minimization_objective.h │ ├── humanoid_lip_com_velocity_tracking_objective.h │ ├── trajectory_jerk_minimization_objective.h │ ├── trajectory_motion_constraint.h │ ├── trajectory_position_tracking_objective.h │ ├── trajectory_velocity_tracking_objective.h │ ├── zebulon_base_motion_constraint.h │ ├── zebulon_base_position_tracking_objective.h │ ├── zebulon_base_velocity_tracking_objective.h │ ├── zebulon_com_centering_objective.h │ ├── zebulon_com_constraint.h │ ├── zebulon_cop_centering_objective.h │ ├── zebulon_cop_constraint.h │ ├── zebulon_jerk_minimization_objective.h │ ├── zebulon_tilt_minimization_objective.h │ ├── zebulon_tilt_motion_constraint.h │ └── zebulon_tilt_velocity_minimization_objective.h ├── humanoid_feet_supervisor.h ├── humanoid_walkgen.h ├── humanoid_walkgen_type.h ├── interpolator.h ├── lineardynamic.h ├── model │ ├── humanoid_foot_model.h │ ├── lip_model.h │ ├── no_dynamic_model.h │ └── zebulon_base_model.h ├── qpsolverfactory.h ├── tools.h ├── trajectory_walkgen.h ├── trajectory_walkgen_type.h ├── type.h ├── zebulon_walkgen.h └── zebulon_walkgen_type.h ├── qiproject.xml ├── qpsolver ├── CMakeLists.txt ├── mpc-walkgen │ ├── qpsolver.h │ ├── qpsolver_qpoases_double.h │ └── qpsolver_qpoases_float.h └── src │ ├── qpsolver_qpoases.hxx │ ├── qpsolver_qpoases_double.cpp │ └── qpsolver_qpoases_float.cpp ├── src ├── convexpolygon.cpp ├── function │ ├── humanoid_cop_centering_objective.cpp │ ├── humanoid_cop_constraint.cpp │ ├── humanoid_foot_constraint.cpp │ ├── humanoid_lip_com_jerk_minimization_objective.cpp │ ├── humanoid_lip_com_velocity_tracking_objective.cpp │ ├── trajectory_jerk_minimization_objective.cpp │ ├── trajectory_motion_constraint.cpp │ ├── trajectory_position_tracking_objective.cpp │ ├── trajectory_velocity_tracking_objective.cpp │ ├── zebulon_base_motion_constraint.cpp │ ├── zebulon_base_position_tracking_objective.cpp │ ├── zebulon_base_velocity_tracking_objective.cpp │ ├── zebulon_com_centering_objective.cpp │ ├── zebulon_com_constraint.cpp │ ├── zebulon_cop_centering_objective.cpp │ ├── zebulon_cop_constraint.cpp │ ├── zebulon_jerk_minimization_objective.cpp │ ├── zebulon_tilt_minimization_objective.cpp │ ├── zebulon_tilt_motion_constraint.cpp │ └── zebulon_tilt_velocity_minimization_objective.cpp ├── humanoid_feet_supervisor.cpp ├── humanoid_walkgen.cpp ├── interpolator.cpp ├── lineardynamic.cpp ├── macro.h ├── model │ ├── humanoid_foot_model.cpp │ ├── lip_model.cpp │ ├── no_dynamic_model.cpp │ └── zebulon_base_model.cpp ├── qpsolverfactory.cpp ├── tools.cpp ├── trajectory_walkgen.cpp └── zebulon_walkgen.cpp └── test ├── CMakeLists.txt ├── humanoid-walkgen-bin.cpp ├── mpc_walkgen_gtest.h ├── test-convex-polygon-function.cpp ├── test-humanoid-cop-centering-objective.cpp ├── test-humanoid-cop-constraint.cpp ├── test-humanoid-feet-supervisor.cpp ├── test-humanoid-foot-constraint.cpp ├── test-humanoid-foot-model.cpp ├── test-humanoid-lip-com-jerk-minimization-objective.cpp ├── test-humanoid-lip-com-velocity-tracking-objective.cpp ├── test-interpolation-function.cpp ├── test-lip-model.cpp ├── test-qpoases-solver.cpp ├── test-zebulon-base-model.cpp ├── test-zebulon-base-motion-constraint.cpp ├── test-zebulon-base-position-tracking-objective.cpp ├── test-zebulon-base-velocity-tracking-objective.cpp ├── test-zebulon-com-constraint.cpp ├── test-zebulon-cop-centering-objective.cpp ├── test-zebulon-cop-constraint.cpp ├── test-zebulon-jerk-minimization-objective.cpp ├── test-zebulon-tilt-motion-constraint.cpp ├── trajectory-walkgen-bin.cpp └── zebulon-walkgen-bin.cpp /.gitignore: -------------------------------------------------------------------------------- 1 | Makefile.in 2 | aclocal.m4 3 | autom4te.cache 4 | config.guess 5 | config.sub 6 | configure 7 | doc/Makefile.in 8 | include/Makefile.in 9 | src/Makefile.in 10 | unitTesting/Makefile.in 11 | ltmain.sh 12 | plugin/CollisionDetector.h 13 | plugin/CollisionDetectorSK.cc 14 | plugin/DynamicsSimulator.h 15 | plugin/DynamicsSimulatorSK.cc 16 | plugin/HRPcontroller.h 17 | plugin/HRPcontrollerSK.cc 18 | plugin/ModelLoader.h 19 | plugin/ModelLoaderSK.cc 20 | plugin/OpenHRPCommon.h 21 | plugin/OpenHRPCommonSK.cc 22 | plugin/SequencePlayer.h 23 | plugin/SequencePlayerSK.cc 24 | plugin/ViewSimulator.h 25 | plugin/ViewSimulatorSK.cc 26 | plugin/walkpluginJRL.h 27 | plugin/walkpluginJRLSK.cc 28 | build/ 29 | ,svn-log 30 | +committed/ 31 | *~ 32 | *build* 33 | CMakeLists.txt.user 34 | *.autosave 35 | cmake/ 36 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # Copyright 2012, Olivier Stasse, JRL, CNRS/AIST 2 | # Requires at least CMake 2.6 to configure the package. 3 | cmake_minimum_required(VERSION 2.8) 4 | project(mpc-walkgen) 5 | enable_testing() 6 | find_package(qibuild) 7 | 8 | qi_sanitize_compile_flags(HIDDEN_SYMBOLS) 9 | include_directories("${CMAKE_CURRENT_SOURCE_DIR}") 10 | 11 | if("${CMAKE_BUILD_TYPE}" STREQUAL "Debug") 12 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DEIGEN_INITIALIZE_MATRICES_BY_NAN=1") 13 | endif() 14 | 15 | add_subdirectory(qpsolver) 16 | 17 | SET(mpc-walkgen_common_PUBLIC_HEADERS 18 | mpc-walkgen/api.h 19 | mpc-walkgen/constant.h 20 | mpc-walkgen/convexpolygon.h 21 | mpc-walkgen/interpolator.h 22 | mpc-walkgen/lineardynamic.h 23 | mpc-walkgen/model/lip_model.h 24 | mpc-walkgen/qpsolverfactory.h 25 | mpc-walkgen/tools.h 26 | mpc-walkgen/type.h 27 | ) 28 | SET(mpc-walkgen_humanoid_PUBLIC_HEADERS 29 | mpc-walkgen/function/humanoid_cop_centering_objective.h 30 | mpc-walkgen/function/humanoid_cop_constraint.h 31 | mpc-walkgen/function/humanoid_foot_constraint.h 32 | mpc-walkgen/function/humanoid_lip_com_jerk_minimization_objective.h 33 | mpc-walkgen/function/humanoid_lip_com_velocity_tracking_objective.h 34 | mpc-walkgen/humanoid_feet_supervisor.h 35 | mpc-walkgen/humanoid_walkgen.h 36 | mpc-walkgen/humanoid_walkgen_type.h 37 | mpc-walkgen/model/humanoid_foot_model.h 38 | ) 39 | SET(mpc-walkgen_zebulon_PUBLIC_HEADERS 40 | mpc-walkgen/function/zebulon_base_velocity_tracking_objective.h 41 | mpc-walkgen/function/zebulon_base_position_tracking_objective.h 42 | mpc-walkgen/function/zebulon_jerk_minimization_objective.h 43 | mpc-walkgen/function/zebulon_tilt_minimization_objective.h 44 | mpc-walkgen/function/zebulon_tilt_velocity_minimization_objective.h 45 | mpc-walkgen/function/zebulon_cop_centering_objective.h 46 | mpc-walkgen/function/zebulon_com_centering_objective.h 47 | mpc-walkgen/function/zebulon_cop_constraint.h 48 | mpc-walkgen/function/zebulon_com_constraint.h 49 | mpc-walkgen/function/zebulon_base_motion_constraint.h 50 | mpc-walkgen/function/zebulon_tilt_motion_constraint.h 51 | mpc-walkgen/model/zebulon_base_model.h 52 | mpc-walkgen/zebulon_walkgen.h 53 | mpc-walkgen/zebulon_walkgen_type.h 54 | ) 55 | SET(mpc-walkgen_trajectory_PUBLIC_HEADERS 56 | mpc-walkgen/function/trajectory_motion_constraint.h 57 | mpc-walkgen/function/trajectory_velocity_tracking_objective.h 58 | mpc-walkgen/function/trajectory_position_tracking_objective.h 59 | mpc-walkgen/function/trajectory_jerk_minimization_objective.h 60 | mpc-walkgen/model/no_dynamic_model.h 61 | mpc-walkgen/trajectory_walkgen.h 62 | mpc-walkgen/trajectory_walkgen_type.h 63 | ) 64 | SET(mpc-walkgen_PUBLIC_HEADERS 65 | ${mpc-walkgen_common_PUBLIC_HEADERS} 66 | ${mpc-walkgen_humanoid_PUBLIC_HEADERS} 67 | ${mpc-walkgen_zebulon_PUBLIC_HEADERS} 68 | ${mpc-walkgen_trajectory_PUBLIC_HEADERS}) 69 | 70 | 71 | SET(mpc-walkgen_SRC 72 | src/convexpolygon.cpp 73 | src/interpolator.cpp 74 | src/lineardynamic.cpp 75 | src/macro.h 76 | src/model/lip_model.cpp 77 | src/qpsolverfactory.cpp 78 | src/tools.cpp 79 | 80 | src/function/humanoid_lip_com_velocity_tracking_objective.cpp 81 | src/function/humanoid_lip_com_jerk_minimization_objective.cpp 82 | src/function/humanoid_cop_centering_objective.cpp 83 | src/function/humanoid_cop_constraint.cpp 84 | src/function/humanoid_foot_constraint.cpp 85 | src/humanoid_walkgen.cpp 86 | src/humanoid_feet_supervisor.cpp 87 | src/model/humanoid_foot_model.cpp 88 | 89 | src/function/zebulon_base_motion_constraint.cpp 90 | src/function/zebulon_base_position_tracking_objective.cpp 91 | src/function/zebulon_base_velocity_tracking_objective.cpp 92 | src/function/zebulon_com_centering_objective.cpp 93 | src/function/zebulon_com_constraint.cpp 94 | src/function/zebulon_cop_centering_objective.cpp 95 | src/function/zebulon_cop_constraint.cpp 96 | src/function/zebulon_jerk_minimization_objective.cpp 97 | src/function/zebulon_tilt_minimization_objective.cpp 98 | src/function/zebulon_tilt_motion_constraint.cpp 99 | src/function/zebulon_tilt_velocity_minimization_objective.cpp 100 | src/model/zebulon_base_model.cpp 101 | src/zebulon_walkgen.cpp 102 | 103 | src/function/trajectory_position_tracking_objective.cpp 104 | src/function/trajectory_velocity_tracking_objective.cpp 105 | src/function/trajectory_motion_constraint.cpp 106 | src/function/trajectory_jerk_minimization_objective.cpp 107 | src/model/no_dynamic_model.cpp 108 | src/trajectory_walkgen.cpp 109 | ) 110 | 111 | qi_create_lib(mpc-walkgen SHARED 112 | ${mpc-walkgen_PUBLIC_HEADERS} 113 | ${mpc-walkgen_SRC}) 114 | qi_use_lib(mpc-walkgen 115 | eigen3 QI boost 116 | mpc-walkgen_qpsolver 117 | mpc-walkgen_qpsolver_qpoases_double 118 | mpc-walkgen_qpsolver_qpoases_float) 119 | qi_stage_lib(mpc-walkgen) 120 | 121 | qi_install_header(${mpc-walkgen_PUBLIC_HEADERS} KEEP_RELATIVE_PATHS) 122 | 123 | add_subdirectory(test) 124 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | mpc-walkgen 2 | =========== 3 | 4 | This software provides... 5 | 6 | Setup 7 | ----- 8 | 9 | ### Dependencies 10 | 11 | The package mpc-walkgen depends on several packages which 12 | have to be available on your machine. 13 | 14 | - System tools: 15 | - CMake (>=2.8) 16 | - pkg-config 17 | - usual compilation tools (GCC/G++, make, etc.) 18 | 19 | - Libraries 20 | - boost (>=1.40) 21 | - eigen (>=3.0.0) 22 | - qpoases (version 2.0, compiled twice for both float and double, with 23 | special care taken to avoid symbol collisions) 24 | 25 | ### Compile 26 | 27 | The project is installed and built thanks to qibuild 28 | (https://github.com/aldebaran/qibuild). 29 | 30 | Once you have qibuild installed, simply run 31 | 32 | qibuild configure 33 | qibuild make 34 | 35 | Design choices 36 | -------------- 37 | 38 | ### API 39 | 40 | At some stage, the lib might be split in two in order to separate the humanoid 41 | and zebulon parts (the public and currently secret parts). 42 | The common pieces used by these two might go in a third "common" library. 43 | 44 | In this spirit it was decided that most of the header could be kept public. 45 | 46 | 47 | ### Template instantiations 48 | 49 | Just like Eigen, mpc-walkgen is templated by a scalar type. However unlike 50 | Eigen, mpc-walkgen is not a head-only library. Its algorithms are not 51 | implemented in headers the user includes, but in a shared library the 52 | user links with. 53 | 54 | This means: 55 | 56 | * the algorithms are not implemented in (public) headers but in (private) 57 | cpp files. 58 | 59 | * the templates are not instantiated in the user code but in the mpc-walkgen 60 | library (there are explicit instantiation for float and double in each cpp 61 | files) 62 | 63 | * the user code will be compiled faster (no need to instantiate mpc-walkgen 64 | templates in it) 65 | 66 | * the user code can be compile in debug while using mpc-walkgen compiled in 67 | release 68 | 69 | * the user cannot extend mpc-walkgen to support scalar types besides float 70 | and double. (Note that we currently have no QP solver for other scalar types 71 | anyway.) 72 | 73 | -------------------------------------------------------------------------------- /TODO.txt: -------------------------------------------------------------------------------- 1 | This is the Todo list precising the required things before a clean release. 2 | 3 | Complete the documentation 4 | 5 | Make a real unittest out of test-walkgen 6 | (Use Romeo data rather than hrp2 data) 7 | 8 | Add a switch for the displays (amelif and debug) 9 | 10 | Clean the public interface: some attributes appear twice, and the method 11 | CallMethod does nothing at all sometimes ! 12 | 13 | It is important to know what parameters of generalData_ can be changed from the outside and when 14 | I think two categories can be noticed: 15 | 1\ Those mutable "at will" by the user, ie during run time, and smartly (ie like the reference in velocity) 16 | 2\ Those not mutable OR only at the cost of the recomputation of the precomputed matrices. 17 | -- Whereas in a first time they can be fixed, it leaves room for reflexion concerning the initialization of 18 | -- those matrices (typically, they should be initialiazed after the construction). 19 | 20 | Remove the remaining TODO in the code. 21 | 22 | ///// 23 | // The following are important remarks (maybe not all are necessary for a(n) (urgent) release) 24 | 25 | Reintroduce class RigidBody - absolutely necessary for multiBody 26 | 27 | Move ::display(�) out of QPGenerator - generator is generating not displaying 28 | 29 | Distinguish enums from variables by changing enum naming convention - confusing 30 | 31 | MPCData and especially RobotData, shouldn't they rather point to parameters instead of containing them? - Data has often a better place (robot_ for RobotData) 32 | 33 | (Re)Introduce dump() functions in structures and classes - convenient debugging, had been (partly) implemented before 34 | 35 | Reintroduce 3d convex hulls and correspondent constraints on the com and feet - had been implemented before 36 | 37 | Initialization still hard coded 38 | 39 | Can interpolation be relative to a RigidBody? - CoM.interpolate() 40 | -------------------------------------------------------------------------------- /mpc-walkgen/api.h: -------------------------------------------------------------------------------- 1 | //////////////////////////////////////////////////////////////////////////////// 2 | /// 3 | ///\file zebulon_type.h 4 | ///\brief Some structures and typedefs 5 | ///\author Lafaye Jory 6 | ///\author Barthelemy Sebastien 7 | /// 8 | //////////////////////////////////////////////////////////////////////////////// 9 | 10 | #pragma once 11 | #ifndef MPC_WALKGEN_API_H 12 | #define MPC_WALKGEN_API_H 13 | 14 | #include 15 | #define MPC_WALKGEN_API QI_LIB_API(mpc_walkgen) 16 | 17 | #endif 18 | -------------------------------------------------------------------------------- /mpc-walkgen/constant.h: -------------------------------------------------------------------------------- 1 | //////////////////////////////////////////////////////////////////////////////// 2 | /// 3 | ///\file zebulon_type.h 4 | ///\brief Some structures and typedefs 5 | ///\author Lafaye Jory 6 | /// 7 | //////////////////////////////////////////////////////////////////////////////// 8 | #pragma once 9 | #ifndef MPC_WALKGEN_CONSTANT_H 10 | #define MPC_WALKGEN_CONSTANT_H 11 | 12 | #include 13 | 14 | namespace MPCWalkgen 15 | { 16 | template 17 | struct Constant 18 | { 19 | TEMPLATE_TYPEDEF(Scalar) 20 | 21 | static const Scalar EPSILON; 22 | static const Scalar GRAVITY_NORM; 23 | static const Vector3 GRAVITY_VECTOR; 24 | static const Scalar MAXIMUM_BOUND_VALUE; 25 | }; 26 | 27 | template 28 | const Scalar Constant::EPSILON = static_cast(0.0001); 29 | 30 | template 31 | const Scalar Constant::GRAVITY_NORM = static_cast(9.81); 32 | 33 | template 34 | const typename Type::Vector3 Constant::GRAVITY_VECTOR 35 | = typename Type::Vector3(0, 0, Constant::GRAVITY_NORM); 36 | 37 | template 38 | const Scalar Constant::MAXIMUM_BOUND_VALUE = static_cast(10e10); 39 | } 40 | 41 | #endif 42 | -------------------------------------------------------------------------------- /mpc-walkgen/convexpolygon.h: -------------------------------------------------------------------------------- 1 | //////////////////////////////////////////////////////////////////////////////// 2 | /// 3 | ///\file type.h 4 | ///\brief Some structures and typedefs 5 | ///\author Lafaye Jory 6 | ///\author de Gourcuff Martin 7 | ///\author Barthelemy Sebastien 8 | /// 9 | //////////////////////////////////////////////////////////////////////////////// 10 | 11 | #pragma once 12 | #ifndef MPC_WALKGEN_CONVEXPOLYGON_H 13 | #define MPC_WALKGEN_CONVEXPOLYGON_H 14 | 15 | #include 16 | #include 17 | 18 | #ifdef _MSC_VER 19 | # pragma warning( push ) 20 | // C4251: class needs to have DLL interface 21 | # pragma warning( disable: 4251) 22 | #endif 23 | 24 | namespace MPCWalkgen 25 | { 26 | /// \brief Define a 2D convex polygon bounded by a set p of vertices 27 | template 28 | class MPC_WALKGEN_API ConvexPolygon 29 | { 30 | TEMPLATE_TYPEDEF(Scalar) 31 | public: 32 | 33 | ConvexPolygon(); 34 | 35 | ConvexPolygon(const vectorOfVector2 &p); 36 | 37 | ~ConvexPolygon(); 38 | 39 | inline const vectorOfVector2& getVertices() const 40 | {return p_;} 41 | 42 | /// \brief Get the number of vertices of the convex polygon 43 | inline const int getNbVertices() const 44 | {return p_.size();} 45 | 46 | inline Scalar getXSupBound() const 47 | {return xSupBound_;} 48 | inline Scalar getXInfBound() const 49 | {return xInfBound_;} 50 | inline Scalar getYSupBound() const 51 | {return ySupBound_;} 52 | inline Scalar getYInfBound() const 53 | {return yInfBound_;} 54 | 55 | inline const VectorX& getGeneralConstraintsMatrixCoefsForX() const 56 | {return generalConstraintsMatrixCoefsForX_;} 57 | inline const VectorX& getGeneralConstraintsMatrixCoefsForY() const 58 | {return generalConstraintsMatrixCoefsForY_;} 59 | inline const VectorX& getGeneralConstraintsConstantPart() const 60 | {return generalConstraintsConstantPart_;} 61 | 62 | inline int getNbGeneralConstraints() const 63 | {return generalConstraintsConstantPart_.rows();} 64 | 65 | //TODO: think about changing type and tools into something else. E.g. angleBetweenVecs 66 | //should be in tools, but tools depends of type... Also these static attributes are an 67 | //ugly solution but comfortable for now. 68 | 69 | 70 | /// \brief Return the set of vertices of the convex polygon of all vectors from Vec 71 | /// The algorithm is similar to the gift-wrapping or Jarvis 72 | /// march algorithm. The convex polygon vertices are counter-clockwise ordered 73 | static vectorOfVector2 extractVertices(const vectorOfVector2 &points); 74 | /// \brief Measure the angle between vec1 and vec2. 75 | /// Returns -pi if vec1 = -vec2 and 0 if vec1=0 or vec2=0 76 | static Scalar angleBetweenVecs(const Vector2& vec1, const Vector2 &vec2); 77 | /// \brief Return the vector index with the lowest value of Y coordinate. 78 | /// If several vectors match this value, it returns the leftmost one 79 | /// among these vectors, i.e. the vector with the lowest value of X coordinate. 80 | /// All vectors are assumed to have the same Z coordinate. 81 | static int getIndexOfLowestAndLeftmostVertice(const vectorOfVector2 &p); 82 | /// \brief Return the next counter-clockwise element of the ConvexPolygon, 83 | /// ptot[currentVerticeIndex] being the current convex polygon vertice and 84 | /// lastVertice the one before. 85 | static int getIndexOfSmallestAngleVertice(int currentVerticeIndex, 86 | const Vector2 &lastVertice, 87 | const vectorOfVector2 &ptot); 88 | 89 | private: 90 | /// \brief Compute Vectors generalConstraintsMatrixCoefsForX_, 91 | /// generalConstraintsMatrixCoefsForY_, generalConstraintsConstantPart_, 92 | /// and values of xInfBound_, yInfBound_, xSupBound_ and ySupBound_. 93 | void computeBoundsAndGeneralConstraintValues(); 94 | 95 | private: 96 | /// \brief p_ is the set of vertices of the convex polygon 97 | vectorOfVector2 p_; 98 | 99 | /// \brief Let X be a point of coordinates (x, y). X is located inside the convex polygon 100 | /// if it is a solution of the general constraints matrix inequation: 101 | /// AX + b <=0 102 | /// 103 | /// Let (p(1),...,p(n)) be the set of vertices of the convex polygon. X is located 104 | /// inside the convex polygon if for all i in [1, n], 105 | /// (py(i+1) - py(i))(x - px(i)) - (px(i+1) - px(i))(y - py(i)) <= 0 106 | /// This is how matrices A and b are built. However, in order to save computation 107 | /// time, it is easier to detect bounds contraints of the form Xinf <= X <= Xmax. 108 | /// We do have bounds constraints if one or several edges of the convex polygon 109 | /// are horizontal or vertical. 110 | /// 111 | /// For a given convex polygon, we store these matrices as below: 112 | /// A = (generalConstraintsMatrixCoefsForX_, generalConstraintsMatrixCoefsForY_) 113 | /// b = generalConstraintsConstantPart_ 114 | /// Xinf = (xInfBound_, yInfBound_) 115 | /// XSup = (xSupBound_, ySupBound_) 116 | /// 117 | 118 | Scalar xSupBound_; 119 | Scalar xInfBound_; 120 | Scalar ySupBound_; 121 | Scalar yInfBound_; 122 | 123 | VectorX generalConstraintsMatrixCoefsForX_; 124 | VectorX generalConstraintsMatrixCoefsForY_; 125 | VectorX generalConstraintsConstantPart_; 126 | }; 127 | } 128 | 129 | #ifdef _MSC_VER 130 | # pragma warning( pop ) 131 | #endif 132 | 133 | #endif 134 | -------------------------------------------------------------------------------- /mpc-walkgen/function/humanoid_cop_centering_objective.h: -------------------------------------------------------------------------------- 1 | //////////////////////////////////////////////////////////////////////////////// 2 | /// 3 | ///\file humanoid_cop_centering_objective.h 4 | ///\brief Implement the CoP centering objective 5 | ///\author de Gourcuff Martin 6 | ///\author Barthelemy Sebastien 7 | /// 8 | //////////////////////////////////////////////////////////////////////////////// 9 | 10 | #pragma once 11 | #ifndef MPC_WALKGEN_FUNCTION_HUMANOID_COP_CENTERING_OBJECTIVE_H 12 | #define MPC_WALKGEN_FUNCTION_HUMANOID_COP_CENTERING_OBJECTIVE_H 13 | 14 | #include 15 | #include 16 | #include 17 | #include 18 | 19 | #ifdef _MSC_VER 20 | # pragma warning( push ) 21 | // C4251: class needs to have DLL interface 22 | # pragma warning( disable: 4251 ) 23 | #endif 24 | 25 | namespace MPCWalkgen 26 | { 27 | template 28 | class MPC_WALKGEN_API HumanoidCopCenteringObjective 29 | { 30 | TEMPLATE_TYPEDEF(Scalar) 31 | 32 | public: 33 | HumanoidCopCenteringObjective(const LIPModel& lipModel, 34 | const HumanoidFeetSupervisor& feetSupervisor); 35 | ~HumanoidCopCenteringObjective(); 36 | 37 | const VectorX& getGradient(const VectorX& x0); 38 | const MatrixX& getHessian(); 39 | 40 | private: 41 | const LIPModel& lipModel_; 42 | const HumanoidFeetSupervisor& feetSupervisor_; 43 | 44 | VectorX gradient_; 45 | MatrixX hessian_; 46 | }; 47 | } 48 | 49 | #ifdef _MSC_VER 50 | # pragma warning( pop ) 51 | #endif 52 | 53 | #endif 54 | -------------------------------------------------------------------------------- /mpc-walkgen/function/humanoid_cop_constraint.h: -------------------------------------------------------------------------------- 1 | //////////////////////////////////////////////////////////////////////////////// 2 | /// 3 | ///\file humanoid_cop_constraint.h 4 | ///\brief Implement the CoP constraint 5 | ///\author de Gourcuff Martin 6 | ///\author Barthelemy Sebastien 7 | /// 8 | //////////////////////////////////////////////////////////////////////////////// 9 | 10 | #pragma once 11 | #ifndef MPC_WALKGEN_FUNCTION_HUMANOID_COP_CONSTRAINT_H 12 | #define MPC_WALKGEN_FUNCTION_HUMANOID_COP_CONSTRAINT_H 13 | 14 | #include 15 | #include 16 | #include 17 | #include 18 | 19 | #ifdef _MSC_VER 20 | # pragma warning( push ) 21 | // C4251: class needs to have DLL interface 22 | # pragma warning( disable: 4251 ) 23 | #endif 24 | 25 | namespace MPCWalkgen 26 | { 27 | template 28 | class MPC_WALKGEN_API HumanoidCopConstraint 29 | { 30 | TEMPLATE_TYPEDEF(Scalar) 31 | 32 | public: 33 | HumanoidCopConstraint(const LIPModel& lipModel, 34 | const HumanoidFeetSupervisor& feetSupervisor); 35 | ~HumanoidCopConstraint(); 36 | 37 | int getNbConstraints(); 38 | const VectorX& getFunction(const VectorX &x0); 39 | const MatrixX& getGradient(int sizeVec); 40 | const VectorX& getSupBounds(const VectorX& x0); 41 | const VectorX& getInfBounds(const VectorX& x0); 42 | 43 | void computeConstantPart(); 44 | 45 | private: 46 | /// \brief Compute the number of general constraints over the whole 47 | /// QP preview window 48 | void computeNbGeneralConstraints(); 49 | /// \brief Compute general constraints Matrices A and b 50 | /// for constraint of the form A*X +b <= 0 51 | void computeGeneralConstraintsMatrices(int sizeVec); 52 | /// \brief Compute bounds vectors infBound_ and supBound_ 53 | /// for constraint of the form infBound_ <= X <= supBound_ 54 | void computeBoundsVectors(const VectorX &x0); 55 | 56 | private: 57 | const LIPModel& lipModel_; 58 | const HumanoidFeetSupervisor& feetSupervisor_; 59 | 60 | int nbGeneralConstraints_; 61 | 62 | VectorX function_; 63 | MatrixX gradient_; 64 | 65 | VectorX supBound_; 66 | VectorX infBound_; 67 | 68 | MatrixX A_; 69 | VectorX b_; 70 | }; 71 | } 72 | 73 | #ifdef _MSC_VER 74 | # pragma warning( pop ) 75 | #endif 76 | 77 | #endif 78 | -------------------------------------------------------------------------------- /mpc-walkgen/function/humanoid_foot_constraint.h: -------------------------------------------------------------------------------- 1 | //////////////////////////////////////////////////////////////////////////////// 2 | /// 3 | ///\file humanoid_foot_constraint.h 4 | ///\brief Implement the foot constraints 5 | ///\author de Gourcuff Martin 6 | ///\author Barthelemy Sebastien 7 | /// 8 | //////////////////////////////////////////////////////////////////////////////// 9 | 10 | #pragma once 11 | #ifndef MPC_WALKGEN_FUNCTION_HUMANOID_FOOT_CONSTRAINT_H 12 | #define MPC_WALKGEN_FUNCTION_HUMANOID_FOOT_CONSTRAINT_H 13 | 14 | #include 15 | #include 16 | #include 17 | #include 18 | 19 | #ifdef _MSC_VER 20 | # pragma warning( push ) 21 | // C4251: class needs to have DLL interface 22 | # pragma warning( disable: 4251 ) 23 | #endif 24 | 25 | namespace MPCWalkgen 26 | { 27 | template 28 | class MPC_WALKGEN_API HumanoidFootConstraint 29 | { 30 | TEMPLATE_TYPEDEF(Scalar) 31 | 32 | public: 33 | HumanoidFootConstraint(const LIPModel& lipModel, 34 | const HumanoidFeetSupervisor& feetSupervisor); 35 | ~HumanoidFootConstraint(); 36 | 37 | int getNbConstraints(); 38 | const VectorX& getFunction(const VectorX &x0); 39 | const MatrixX& getGradient(int sizeVec); 40 | const VectorX& getSupBounds(const VectorX &x0); 41 | const VectorX& getInfBounds(const VectorX &x0); 42 | 43 | private: 44 | /// \brief Compute the number of general constraints over the whole 45 | /// QP preview window 46 | void computeNbGeneralConstraints(); 47 | /// \brief Compute general constraints Matrices A and b 48 | /// such that A*X + b <= 0 49 | void computeGeneralConstraintsMatrices(int sizeVec); 50 | /// \brief Compute bounds vectors infBound_ and supBound_ 51 | /// such that infBound_ <= X <= supBound_ 52 | void computeBoundsVectors(const VectorX &x0); 53 | 54 | const LIPModel& lipModel_; 55 | const HumanoidFeetSupervisor& feetSupervisor_; 56 | 57 | int nbGeneralConstraints_; 58 | 59 | VectorX function_; 60 | MatrixX gradient_; 61 | 62 | VectorX supBound_; 63 | VectorX infBound_; 64 | 65 | MatrixX A_; 66 | VectorX b_; 67 | }; 68 | } 69 | 70 | #ifdef _MSC_VER 71 | # pragma warning( pop ) 72 | #endif 73 | 74 | #endif 75 | -------------------------------------------------------------------------------- /mpc-walkgen/function/humanoid_lip_com_jerk_minimization_objective.h: -------------------------------------------------------------------------------- 1 | //////////////////////////////////////////////////////////////////////////////// 2 | /// 3 | ///\file humanoid_lip_com_jerk_minimization_objective.h 4 | ///\brief Implement the LIP CoM jerk minimization objective 5 | ///\author de Gourcuff Martin 6 | ///\author Barthelemy Sebastien 7 | /// 8 | //////////////////////////////////////////////////////////////////////////////// 9 | 10 | #pragma once 11 | #ifndef MPC_WALKGEN_FUNCTION_HUMANOID_JERK_MINIMIZATION_OBJECTIVE_H 12 | #define MPC_WALKGEN_FUNCTION_HUMANOID_JERK_MINIMIZATION_OBJECTIVE_H 13 | 14 | #include 15 | #include 16 | #include 17 | #include 18 | 19 | #ifdef _MSC_VER 20 | # pragma warning( push ) 21 | // C4251: class needs to have DLL interface 22 | # pragma warning( disable: 4251 ) 23 | #endif 24 | 25 | namespace MPCWalkgen{ 26 | template 27 | class MPC_WALKGEN_API HumanoidLipComJerkMinimizationObjective // That's a 28 | // little bit 29 | // too long... 30 | { 31 | TEMPLATE_TYPEDEF(Scalar) 32 | 33 | public: 34 | HumanoidLipComJerkMinimizationObjective( 35 | const LIPModel& lipModel, 36 | const HumanoidFeetSupervisor& feetSupervisor); 37 | ~HumanoidLipComJerkMinimizationObjective(); 38 | 39 | const VectorX& getGradient(const VectorX& x0); 40 | const MatrixX& getHessian(); 41 | 42 | private: 43 | const LIPModel& lipModel_; 44 | const HumanoidFeetSupervisor& feetSupervisor_; 45 | 46 | VectorX gradient_; 47 | MatrixX hessian_; 48 | }; 49 | } 50 | 51 | #ifdef _MSC_VER 52 | # pragma warning( pop ) 53 | #endif 54 | 55 | #endif 56 | -------------------------------------------------------------------------------- /mpc-walkgen/function/humanoid_lip_com_velocity_tracking_objective.h: -------------------------------------------------------------------------------- 1 | //////////////////////////////////////////////////////////////////////////////// 2 | /// 3 | ///\file humanoid_lip_com_velocity_tracking_objective.h 4 | ///\brief Implement the LIP CoM velocity tracking objective 5 | ///\author de Gourcuff Martin 6 | ///\author Barthelemy Sebastien 7 | /// 8 | //////////////////////////////////////////////////////////////////////////////// 9 | 10 | #ifndef MPC_WALKGEN_FUNCTION_HUMANOID_VELOCITY_TRACKING_OBJECTIVE_H 11 | #define MPC_WALKGEN_FUNCTION_HUMANOID_VELOCITY_TRACKING_OBJECTIVE_H 12 | 13 | #include 14 | #include 15 | #include 16 | #include 17 | 18 | #ifdef _MSC_VER 19 | # pragma warning( push ) 20 | // C4251: class needs to have DLL interface 21 | # pragma warning( disable: 4251 ) 22 | #endif 23 | 24 | namespace MPCWalkgen{ 25 | template 26 | class MPC_WALKGEN_API HumanoidLipComVelocityTrackingObjective 27 | { 28 | TEMPLATE_TYPEDEF(Scalar) 29 | 30 | public: 31 | HumanoidLipComVelocityTrackingObjective( 32 | const LIPModel& lipModel, 33 | const HumanoidFeetSupervisor& feetSupervisor); 34 | ~HumanoidLipComVelocityTrackingObjective(); 35 | 36 | const VectorX& getGradient(const VectorX& x0); 37 | const MatrixX& getHessian(); 38 | 39 | /// \brief Set the torso velocity reference in the world frame 40 | /// It is a vector of size 2*N, with N the number of samples 41 | /// (refX, refY) 42 | void setVelRefInWorldFrame(const VectorX& velRefInWorldFrame); 43 | 44 | private: 45 | const LIPModel& lipModel_; 46 | const HumanoidFeetSupervisor& feetSupervisor_; 47 | 48 | VectorX velRefInWorldFrame_; 49 | 50 | VectorX gradient_; 51 | MatrixX hessian_; 52 | }; 53 | } 54 | 55 | #ifdef _MSC_VER 56 | # pragma warning( pop ) 57 | #endif 58 | 59 | #endif 60 | -------------------------------------------------------------------------------- /mpc-walkgen/function/trajectory_jerk_minimization_objective.h: -------------------------------------------------------------------------------- 1 | //////////////////////////////////////////////////////////////////////////////// 2 | /// 3 | ///\author Lafaye Jory 4 | /// 5 | //////////////////////////////////////////////////////////////////////////////// 6 | 7 | #pragma once 8 | #ifndef MPC_WALKGEN_FUNCTION_TRAJECTORY_JERK_MINIMIZATION_OBJECTIVE_H 9 | #define MPC_WALKGEN_FUNCTION_TRAJECTORY_JERK_MINIMIZATION_OBJECTIVE_H 10 | 11 | #include 12 | #include 13 | 14 | #ifdef _MSC_VER 15 | # pragma warning( push ) 16 | // C4251: class needs to have DLL interface 17 | # pragma warning( disable: 4251) 18 | #endif 19 | 20 | namespace MPCWalkgen 21 | { 22 | template 23 | class TrajectoryJerkMinimizationObjective 24 | { 25 | TEMPLATE_TYPEDEF(Scalar) 26 | 27 | public: 28 | TrajectoryJerkMinimizationObjective(const NoDynamicModel& model); 29 | ~TrajectoryJerkMinimizationObjective(); 30 | 31 | const VectorX& getGradient(const VectorX& x0); 32 | const MatrixX& getHessian(); 33 | 34 | void computeConstantPart(); 35 | 36 | private: 37 | const NoDynamicModel& model_; 38 | 39 | VectorX function_; 40 | MatrixX gradient_; 41 | MatrixX hessian_; 42 | }; 43 | 44 | } 45 | 46 | #ifdef _MSC_VER 47 | # pragma warning( pop ) 48 | #endif 49 | 50 | #endif 51 | -------------------------------------------------------------------------------- /mpc-walkgen/function/trajectory_motion_constraint.h: -------------------------------------------------------------------------------- 1 | //////////////////////////////////////////////////////////////////////////////// 2 | /// 3 | ///\author Lafaye Jory 4 | /// 5 | //////////////////////////////////////////////////////////////////////////////// 6 | 7 | #pragma once 8 | #ifndef MPC_WALKGEN_FUNCTION_TRAJECTORY_MOTION_CONSTRAINT_H 9 | #define MPC_WALKGEN_FUNCTION_TRAJECTORY_MOTION_CONSTRAINT_H 10 | 11 | #include 12 | #include 13 | 14 | #ifdef _MSC_VER 15 | # pragma warning( push ) 16 | // C4251: class needs to have DLL interface 17 | # pragma warning( disable: 4251) 18 | #endif 19 | 20 | namespace MPCWalkgen 21 | { 22 | template 23 | class MotionConstraint 24 | { 25 | TEMPLATE_TYPEDEF(Scalar) 26 | 27 | public: 28 | MotionConstraint(const NoDynamicModel& model); 29 | ~MotionConstraint(); 30 | 31 | const VectorX& getFunctionInf(const VectorX& x0); 32 | const VectorX& getFunctionSup(const VectorX& x0); 33 | const MatrixX& getGradient(); 34 | 35 | int getNbConstraints(); 36 | 37 | void computeConstantPart(); 38 | 39 | private: 40 | void computeFunction(const VectorX& x0, VectorX& func, 41 | Scalar velLimit, Scalar accLimit); 42 | 43 | 44 | private: 45 | const NoDynamicModel& model_; 46 | 47 | VectorX functionInf_; 48 | VectorX functionSup_; 49 | MatrixX gradient_; 50 | MatrixX hessian_; 51 | 52 | VectorX tmp_; 53 | VectorX tmp2_; 54 | }; 55 | } 56 | 57 | #ifdef _MSC_VER 58 | # pragma warning( pop ) 59 | #endif 60 | 61 | #endif 62 | -------------------------------------------------------------------------------- /mpc-walkgen/function/trajectory_position_tracking_objective.h: -------------------------------------------------------------------------------- 1 | //////////////////////////////////////////////////////////////////////////////// 2 | /// 3 | ///\author Lafaye Jory 4 | /// 5 | //////////////////////////////////////////////////////////////////////////////// 6 | 7 | #pragma once 8 | #ifndef MPC_WALKGEN_FUNCTION_TRAJECTORY_POSITION_TRACKING_OBJECTIVE_H 9 | #define MPC_WALKGEN_FUNCTION_TRAJECTORY_POSITION_TRACKING_OBJECTIVE_H 10 | 11 | #include 12 | #include 13 | 14 | #ifdef _MSC_VER 15 | # pragma warning( push ) 16 | // C4251: class needs to have DLL interface 17 | # pragma warning( disable: 4251) 18 | #endif 19 | 20 | namespace MPCWalkgen 21 | { 22 | template 23 | class PositionTrackingObjective 24 | { 25 | TEMPLATE_TYPEDEF(Scalar) 26 | 27 | public: 28 | PositionTrackingObjective(const NoDynamicModel& model); 29 | ~PositionTrackingObjective(); 30 | 31 | const MatrixX& getGradient(const VectorX& x0); 32 | const MatrixX& getHessian(); 33 | 34 | /// \brief Set the base position reference in the world frame 35 | void setPosRefInWorldFrame(const VectorX& posRefInWorldFrame); 36 | 37 | void computeConstantPart(); 38 | 39 | protected: 40 | const NoDynamicModel& model_; 41 | 42 | VectorX posRefInWorldFrame_; 43 | 44 | VectorX function_; 45 | MatrixX gradient_; 46 | MatrixX hessian_; 47 | 48 | VectorX tmp_; 49 | }; 50 | } 51 | 52 | #ifdef _MSC_VER 53 | # pragma warning( pop ) 54 | #endif 55 | 56 | #endif 57 | -------------------------------------------------------------------------------- /mpc-walkgen/function/trajectory_velocity_tracking_objective.h: -------------------------------------------------------------------------------- 1 | //////////////////////////////////////////////////////////////////////////////// 2 | /// 3 | ///\author Lafaye Jory 4 | /// 5 | //////////////////////////////////////////////////////////////////////////////// 6 | 7 | #pragma once 8 | #ifndef MPC_WALKGEN_FUNCTION_TRAJECTORY_VELOCITY_TRACKING_OBJECTIVE_H 9 | #define MPC_WALKGEN_FUNCTION_TRAJECTORY_VELOCITY_TRACKING_OBJECTIVE_H 10 | 11 | #include 12 | #include 13 | 14 | #ifdef _MSC_VER 15 | # pragma warning( push ) 16 | // C4251: class needs to have DLL interface 17 | # pragma warning( disable: 4251) 18 | #endif 19 | 20 | namespace MPCWalkgen 21 | { 22 | template 23 | class VelocityTrackingObjective 24 | { 25 | TEMPLATE_TYPEDEF(Scalar) 26 | 27 | public: 28 | VelocityTrackingObjective(const NoDynamicModel& model); 29 | ~VelocityTrackingObjective(); 30 | 31 | const MatrixX& getGradient(const VectorX& x0); 32 | const MatrixX& getHessian(); 33 | 34 | /// \brief Set the base velocity reference in the world frame 35 | void setVelRefInWorldFrame(const VectorX& velRefInWorldFrame); 36 | 37 | void computeConstantPart(); 38 | 39 | protected: 40 | const NoDynamicModel& model_; 41 | 42 | VectorX velRefInWorldFrame_; 43 | 44 | VectorX function_; 45 | MatrixX gradient_; 46 | MatrixX hessian_; 47 | 48 | VectorX tmp_; 49 | }; 50 | } 51 | 52 | #ifdef _MSC_VER 53 | # pragma warning( pop ) 54 | #endif 55 | 56 | #endif 57 | -------------------------------------------------------------------------------- /mpc-walkgen/function/zebulon_base_motion_constraint.h: -------------------------------------------------------------------------------- 1 | //////////////////////////////////////////////////////////////////////////////// 2 | /// 3 | ///\file base_motion_constraint.h 4 | ///\brief Implement the base motion constraints 5 | ///\author Lafaye Jory 6 | ///\author Barthelemy Sebastien 7 | /// 8 | //////////////////////////////////////////////////////////////////////////////// 9 | 10 | #pragma once 11 | #ifndef MPC_WALKGEN_FUNCTION_ZEBULON_BASE_MOTION_CONSTRAINT_H 12 | #define MPC_WALKGEN_FUNCTION_ZEBULON_BASE_MOTION_CONSTRAINT_H 13 | 14 | #include 15 | #include 16 | 17 | #ifdef _MSC_VER 18 | # pragma warning( push ) 19 | // C4251: class needs to have DLL interface 20 | # pragma warning( disable: 4251) 21 | #endif 22 | 23 | namespace MPCWalkgen 24 | { 25 | template 26 | class BaseMotionConstraint 27 | { 28 | TEMPLATE_TYPEDEF(Scalar) 29 | 30 | public: 31 | BaseMotionConstraint(const BaseModel& baseModel); 32 | ~BaseMotionConstraint(); 33 | 34 | const VectorX& getFunctionInf(const VectorX& x0); 35 | const VectorX& getFunctionSup(const VectorX& x0); 36 | const MatrixX& getGradient(); 37 | 38 | int getNbConstraints(); 39 | 40 | void computeConstantPart(); 41 | 42 | private: 43 | void computeFunction(const VectorX& x0, VectorX& func, 44 | Scalar velLimit, Scalar accLimit); 45 | 46 | 47 | private: 48 | const BaseModel& baseModel_; 49 | 50 | VectorX functionInf_; 51 | VectorX functionSup_; 52 | MatrixX gradient_; 53 | MatrixX hessian_; 54 | 55 | VectorX tmp_; 56 | VectorX tmp2_; 57 | }; 58 | } 59 | 60 | #ifdef _MSC_VER 61 | # pragma warning( pop ) 62 | #endif 63 | 64 | #endif 65 | -------------------------------------------------------------------------------- /mpc-walkgen/function/zebulon_base_position_tracking_objective.h: -------------------------------------------------------------------------------- 1 | //////////////////////////////////////////////////////////////////////////////// 2 | /// 3 | ///\file base_position_tracking_objective.h 4 | ///\brief Implement the base position tracking objective 5 | ///\author Lafaye Jory 6 | ///\author Barthelemy Sebastien 7 | /// 8 | //////////////////////////////////////////////////////////////////////////////// 9 | 10 | #pragma once 11 | #ifndef MPC_WALKGEN_FUNCTION_ZEBULON_BASE_POSITION_TRACKING_OBJECTIVE_H 12 | #define MPC_WALKGEN_FUNCTION_ZEBULON_BASE_POSITION_TRACKING_OBJECTIVE_H 13 | 14 | #include 15 | #include 16 | 17 | #ifdef _MSC_VER 18 | # pragma warning( push ) 19 | // C4251: class needs to have DLL interface 20 | # pragma warning( disable: 4251) 21 | #endif 22 | 23 | namespace MPCWalkgen 24 | { 25 | template 26 | class BasePositionTrackingObjective 27 | { 28 | TEMPLATE_TYPEDEF(Scalar) 29 | 30 | public: 31 | BasePositionTrackingObjective(const BaseModel& baseModel); 32 | ~BasePositionTrackingObjective(); 33 | 34 | const MatrixX& getGradient(const VectorX& x0); 35 | const MatrixX& getHessian(); 36 | 37 | /// \brief Set the base position reference in the world frame 38 | /// It's a vector of size 2*N, where N is the number of samples 39 | /// of the base model: 40 | /// (refX, refY) 41 | void setPosRefInWorldFrame(const VectorX& posRefInWorldFrame); 42 | 43 | void computeConstantPart(); 44 | 45 | protected: 46 | const BaseModel& baseModel_; 47 | 48 | VectorX posRefInWorldFrame_; 49 | 50 | VectorX function_; 51 | MatrixX gradient_; 52 | MatrixX hessian_; 53 | 54 | VectorX tmp_; 55 | }; 56 | } 57 | 58 | #ifdef _MSC_VER 59 | # pragma warning( pop ) 60 | #endif 61 | 62 | #endif 63 | -------------------------------------------------------------------------------- /mpc-walkgen/function/zebulon_base_velocity_tracking_objective.h: -------------------------------------------------------------------------------- 1 | //////////////////////////////////////////////////////////////////////////////// 2 | /// 3 | ///\file base_velocity_tracking_objective.h 4 | ///\brief Implement the base velocity tracking objective 5 | ///\author Lafaye Jory 6 | ///\author Barthelemy Sebastien 7 | /// 8 | //////////////////////////////////////////////////////////////////////////////// 9 | 10 | #pragma once 11 | #ifndef MPC_WALKGEN_FUNCTION_ZEBULON_BASE_VELOCITY_TRACKING_OBJECTIVE_H 12 | #define MPC_WALKGEN_FUNCTION_ZEBULON_BASE_VELOCITY_TRACKING_OBJECTIVE_H 13 | 14 | #include 15 | #include 16 | 17 | #ifdef _MSC_VER 18 | # pragma warning( push ) 19 | // C4251: class needs to have DLL interface 20 | # pragma warning( disable: 4251) 21 | #endif 22 | 23 | namespace MPCWalkgen 24 | { 25 | template 26 | class BaseVelocityTrackingObjective 27 | { 28 | TEMPLATE_TYPEDEF(Scalar) 29 | 30 | public: 31 | BaseVelocityTrackingObjective(const BaseModel& baseModel); 32 | ~BaseVelocityTrackingObjective(); 33 | 34 | const MatrixX& getGradient(const VectorX& x0); 35 | const MatrixX& getHessian(); 36 | 37 | /// \brief Set the base velocity reference in the world frame 38 | /// It's a vector of size 2*N, where N is the number of samples 39 | /// of the base model: 40 | /// (refX, refY) 41 | void setVelRefInWorldFrame(const VectorX& velRefInWorldFrame); 42 | 43 | void computeConstantPart(); 44 | 45 | protected: 46 | const BaseModel& baseModel_; 47 | 48 | VectorX velRefInWorldFrame_; 49 | 50 | VectorX function_; 51 | MatrixX gradient_; 52 | MatrixX hessian_; 53 | 54 | VectorX tmp_; 55 | }; 56 | } 57 | 58 | #ifdef _MSC_VER 59 | # pragma warning( pop ) 60 | #endif 61 | 62 | #endif 63 | -------------------------------------------------------------------------------- /mpc-walkgen/function/zebulon_com_centering_objective.h: -------------------------------------------------------------------------------- 1 | //////////////////////////////////////////////////////////////////////////////// 2 | /// 3 | ///\file com_centering_objective.h 4 | ///\brief Implement the com centering objective 5 | ///\author Lafaye Jory 6 | ///\author Barthelemy Sebastien 7 | /// 8 | //////////////////////////////////////////////////////////////////////////////// 9 | 10 | #pragma once 11 | #ifndef MPC_WALKGEN_FUNCTION_ZEBULON_COM_CENTERING_OBJECTIVE_H 12 | #define MPC_WALKGEN_FUNCTION_ZEBULON_COM_CENTERING_OBJECTIVE_H 13 | 14 | #include 15 | #include 16 | #include 17 | 18 | #ifdef _MSC_VER 19 | # pragma warning( push ) 20 | // C4251: class needs to have DLL interface 21 | # pragma warning( disable: 4251) 22 | #endif 23 | 24 | namespace MPCWalkgen 25 | { 26 | template 27 | class ComCenteringObjective 28 | { 29 | TEMPLATE_TYPEDEF(Scalar) 30 | 31 | public: 32 | ComCenteringObjective(const LIPModel& lipModel, const BaseModel& baseModel); 33 | ~ComCenteringObjective(); 34 | 35 | const MatrixX& getGradient(const VectorX& x0); 36 | const MatrixX& getHessian(); 37 | 38 | /// \brief Set the CoP reference in the world frame 39 | /// It's a vector of size 2*N, where N is the number of samples 40 | /// of the lip/base model: 41 | /// (refX, refY) 42 | void setComRefInLocalFrame(const VectorX& copRefInWorldFrame); 43 | 44 | void computeConstantPart(); 45 | void updateGravityShift(); 46 | void setNbSamples(int nbSamples); 47 | 48 | private: 49 | const LIPModel& lipModel_; 50 | const BaseModel& baseModel_; 51 | 52 | VectorX comRefInLocalFrame_; 53 | VectorX comShiftInLocalFrame_; 54 | VectorX gravityShift_; 55 | 56 | VectorX function_; 57 | MatrixX gradient_; 58 | MatrixX hessian_; 59 | 60 | VectorX tmp_; 61 | }; 62 | 63 | } 64 | 65 | #ifdef _MSC_VER 66 | # pragma warning( pop ) 67 | #endif 68 | 69 | #endif 70 | -------------------------------------------------------------------------------- /mpc-walkgen/function/zebulon_com_constraint.h: -------------------------------------------------------------------------------- 1 | //////////////////////////////////////////////////////////////////////////////// 2 | /// 3 | ///\file cop_constraint.h 4 | ///\brief Implement the com constraints 5 | ///\author Lafaye Jory 6 | ///\author Barthelemy Sebastien 7 | /// 8 | //////////////////////////////////////////////////////////////////////////////// 9 | 10 | #pragma once 11 | #ifndef MPC_WALKGEN_FUNCTION_ZEBULON_COM_CONSTRAINT_H 12 | #define MPC_WALKGEN_FUNCTION_ZEBULON_COM_CONSTRAINT_H 13 | 14 | #include 15 | #include 16 | #include 17 | 18 | #ifdef _MSC_VER 19 | # pragma warning( push ) 20 | // C4251: class needs to have DLL interface 21 | # pragma warning( disable: 4251) 22 | #endif 23 | 24 | namespace MPCWalkgen 25 | { 26 | template 27 | class ComConstraint 28 | { 29 | TEMPLATE_TYPEDEF(Scalar) 30 | 31 | public: 32 | ComConstraint(const LIPModel& lipModel, const BaseModel& baseModel); 33 | ~ComConstraint(); 34 | 35 | const VectorX& getFunction(const VectorX& x0); 36 | const MatrixX& getGradient(); 37 | 38 | int getNbConstraints(); 39 | 40 | void computeConstantPart(); 41 | 42 | private: 43 | /// \brief Compute the constraints inequalities : A X + b <= 0 44 | void computeconstraintMatrices(); 45 | 46 | 47 | private: 48 | const LIPModel& lipModel_; 49 | const BaseModel& baseModel_; 50 | 51 | VectorX function_; 52 | MatrixX gradient_; 53 | MatrixX hessian_; 54 | 55 | MatrixX A_; 56 | VectorX b_; 57 | 58 | VectorX tmp_; 59 | 60 | }; 61 | 62 | } 63 | 64 | #ifdef _MSC_VER 65 | # pragma warning( pop ) 66 | #endif 67 | 68 | #endif 69 | -------------------------------------------------------------------------------- /mpc-walkgen/function/zebulon_cop_centering_objective.h: -------------------------------------------------------------------------------- 1 | //////////////////////////////////////////////////////////////////////////////// 2 | /// 3 | ///\file cop_centering_objective.h 4 | ///\brief Implement the cop centering objective 5 | ///\author Lafaye Jory 6 | ///\author Barthelemy Sebastien 7 | /// 8 | //////////////////////////////////////////////////////////////////////////////// 9 | 10 | #pragma once 11 | #ifndef MPC_WALKGEN_FUNCTION_ZEBULON_COP_CENTERING_OBJECTIVE_H 12 | #define MPC_WALKGEN_FUNCTION_ZEBULON_COP_CENTERING_OBJECTIVE_H 13 | 14 | #include 15 | #include 16 | #include 17 | 18 | #ifdef _MSC_VER 19 | # pragma warning( push ) 20 | // C4251: class needs to have DLL interface 21 | # pragma warning( disable: 4251) 22 | #endif 23 | 24 | namespace MPCWalkgen 25 | { 26 | template 27 | class CopCenteringObjective 28 | { 29 | TEMPLATE_TYPEDEF(Scalar) 30 | 31 | public: 32 | CopCenteringObjective(const LIPModel& lipModel, const BaseModel& baseModel); 33 | ~CopCenteringObjective(); 34 | 35 | const MatrixX& getGradient(const VectorX& x0); 36 | const MatrixX& getHessian(); 37 | 38 | /// \brief Set the CoP reference in the world frame 39 | /// It's a vector of size 2*N, where N is the number of samples 40 | /// of the lip/base model: 41 | /// (refX, refY) 42 | void setCopRefInLocalFrame(const VectorX& copRefInLocalFrame); 43 | void setCopRefInWorldFrame(const VectorX& comPosRefInWorldFrame, 44 | const VectorX& comAccRefInWorldFrame, 45 | const VectorX& basePosRefInWorldFrame, 46 | const VectorX& baseAccRefInWorldFrame); 47 | 48 | void computeConstantPart(); 49 | 50 | private: 51 | const LIPModel& lipModel_; 52 | const BaseModel& baseModel_; 53 | 54 | VectorX copRefInLocalFrame_; 55 | 56 | VectorX function_; 57 | MatrixX gradient_; 58 | MatrixX hessian_; 59 | 60 | // Temporary computation variables 61 | VectorX copRef_; 62 | VectorX constantGravity_; 63 | }; 64 | 65 | } 66 | 67 | #ifdef _MSC_VER 68 | # pragma warning( pop ) 69 | #endif 70 | 71 | #endif 72 | -------------------------------------------------------------------------------- /mpc-walkgen/function/zebulon_cop_constraint.h: -------------------------------------------------------------------------------- 1 | //////////////////////////////////////////////////////////////////////////////// 2 | /// 3 | ///\file cop_constraint.h 4 | ///\brief Implement the cop constraints 5 | ///\author Lafaye Jory 6 | ///\author Barthelemy Sebastien 7 | /// 8 | //////////////////////////////////////////////////////////////////////////////// 9 | 10 | #pragma once 11 | #ifndef MPC_WALKGEN_FUNCTION_ZEBULON_COP_CONSTRAINT_H 12 | #define MPC_WALKGEN_FUNCTION_ZEBULON_COP_CONSTRAINT_H 13 | 14 | #include 15 | #include 16 | #include 17 | 18 | #ifdef _MSC_VER 19 | # pragma warning( push ) 20 | // C4251: class needs to have DLL interface 21 | # pragma warning( disable: 4251) 22 | #endif 23 | 24 | namespace MPCWalkgen 25 | { 26 | template 27 | class CopConstraint 28 | { 29 | TEMPLATE_TYPEDEF(Scalar) 30 | 31 | public: 32 | CopConstraint(const LIPModel& lipModel, const BaseModel& baseModel); 33 | ~CopConstraint(); 34 | 35 | const VectorX& getFunction(const VectorX& x0); 36 | const MatrixX& getGradient(); 37 | 38 | int getNbConstraints(); 39 | 40 | void computeConstantPart(); 41 | 42 | private: 43 | /// \brief Compute the constraints inequalities : A X + b <= 0 44 | void computeconstraintMatrices(); 45 | 46 | private: 47 | const LIPModel& lipModel_; 48 | const BaseModel& baseModel_; 49 | 50 | VectorX function_; 51 | MatrixX gradient_; 52 | MatrixX hessian_; 53 | 54 | MatrixX A_; 55 | VectorX b_; 56 | 57 | VectorX tmp_; 58 | 59 | }; 60 | 61 | } 62 | 63 | #ifdef _MSC_VER 64 | # pragma warning( pop ) 65 | #endif 66 | 67 | #endif 68 | -------------------------------------------------------------------------------- /mpc-walkgen/function/zebulon_jerk_minimization_objective.h: -------------------------------------------------------------------------------- 1 | //////////////////////////////////////////////////////////////////////////////// 2 | /// 3 | ///\file zebulon_jerk_minimization_objective.h 4 | ///\brief Implement the jerk minimization objective 5 | ///\author Lafaye Jory 6 | ///\author Barthelemy Sebastien 7 | /// 8 | //////////////////////////////////////////////////////////////////////////////// 9 | 10 | #pragma once 11 | #ifndef MPC_WALKGEN_FUNCTION_ZEBULON_JERK_MINIMIZATION_OBJECTIVE_H 12 | #define MPC_WALKGEN_FUNCTION_ZEBULON_JERK_MINIMIZATION_OBJECTIVE_H 13 | 14 | #include 15 | #include 16 | #include 17 | 18 | #ifdef _MSC_VER 19 | # pragma warning( push ) 20 | // C4251: class needs to have DLL interface 21 | # pragma warning( disable: 4251) 22 | #endif 23 | 24 | namespace MPCWalkgen 25 | { 26 | template 27 | class JerkMinimizationObjective 28 | { 29 | TEMPLATE_TYPEDEF(Scalar) 30 | 31 | public: 32 | JerkMinimizationObjective(const LIPModel& lipModel, const BaseModel& baseModel); 33 | ~JerkMinimizationObjective(); 34 | 35 | const VectorX& getGradient(const VectorX& x0); 36 | const MatrixX& getHessian(); 37 | 38 | void computeConstantPart(); 39 | 40 | private: 41 | const LIPModel& lipModel_; 42 | const BaseModel& baseModel_; 43 | 44 | VectorX function_; 45 | MatrixX gradient_; 46 | MatrixX hessian_; 47 | }; 48 | 49 | } 50 | 51 | #ifdef _MSC_VER 52 | # pragma warning( pop ) 53 | #endif 54 | 55 | #endif 56 | -------------------------------------------------------------------------------- /mpc-walkgen/function/zebulon_tilt_minimization_objective.h: -------------------------------------------------------------------------------- 1 | //////////////////////////////////////////////////////////////////////////////// 2 | /// 3 | ///\file zebulon_tilt_minimization_objective.h 4 | ///\brief Implement the tilt minimization objective 5 | ///\author Lafaye Jory 6 | ///\author Barthelemy Sebastien 7 | /// 8 | //////////////////////////////////////////////////////////////////////////////// 9 | 10 | #pragma once 11 | #ifndef MPC_WALKGEN_FUNCTION_ZEBULON_TILT_MINIMIZATION_OBJECTIVE_H 12 | #define MPC_WALKGEN_FUNCTION_ZEBULON_TILT_MINIMIZATION_OBJECTIVE_H 13 | 14 | #include 15 | #include 16 | #include 17 | 18 | #ifdef _MSC_VER 19 | # pragma warning( push ) 20 | // C4251: class needs to have DLL interface 21 | # pragma warning( disable: 4251) 22 | #endif 23 | 24 | namespace MPCWalkgen 25 | { 26 | template 27 | class TiltMinimizationObjective 28 | { 29 | TEMPLATE_TYPEDEF(Scalar) 30 | 31 | public: 32 | TiltMinimizationObjective(const LIPModel& lipModel, const BaseModel& baseModel); 33 | ~TiltMinimizationObjective(); 34 | 35 | const MatrixX& getGradient(const VectorX& x0); 36 | const MatrixX& getHessian(); 37 | 38 | void updateTiltContactPoint(); 39 | void computeConstantPart(); 40 | 41 | private: 42 | const LIPModel& lipModel_; 43 | const BaseModel& baseModel_; 44 | 45 | VectorX function_; 46 | MatrixX gradient_; 47 | MatrixX hessian_; 48 | 49 | MatrixX uInv_; 50 | LinearDynamic dynB_; 51 | LinearDynamic dynC_; 52 | LinearDynamic dynPsiX_; 53 | LinearDynamic dynPsiY_; 54 | 55 | }; 56 | 57 | } 58 | 59 | #ifdef _MSC_VER 60 | # pragma warning( pop ) 61 | #endif 62 | 63 | #endif 64 | -------------------------------------------------------------------------------- /mpc-walkgen/function/zebulon_tilt_motion_constraint.h: -------------------------------------------------------------------------------- 1 | //////////////////////////////////////////////////////////////////////////////// 2 | /// 3 | ///\file zebulon_tilt_motion_constraint.h 4 | ///\brief Implement the tilt motion constraint 5 | ///\author Lafaye Jory 6 | ///\author Barthelemy Sebastien 7 | /// 8 | //////////////////////////////////////////////////////////////////////////////// 9 | 10 | #pragma once 11 | #ifndef MPC_WALKGEN_FUNCTION_ZEBULON_TILT_MOTION_CONSTRAINT_H 12 | #define MPC_WALKGEN_FUNCTION_ZEBULON_TILT_MOTION_CONSTRAINT_H 13 | 14 | #include 15 | #include 16 | #include 17 | 18 | #ifdef _MSC_VER 19 | # pragma warning( push ) 20 | // C4251: class needs to have DLL interface 21 | # pragma warning( disable: 4251) 22 | #endif 23 | 24 | namespace MPCWalkgen 25 | { 26 | template 27 | class TiltMotionConstraint 28 | { 29 | TEMPLATE_TYPEDEF(Scalar) 30 | 31 | public: 32 | TiltMotionConstraint(const LIPModel& lipModel, const BaseModel& baseModel); 33 | ~TiltMotionConstraint(); 34 | 35 | const VectorX& getFunction(const VectorX& x0); 36 | const MatrixX& getGradient(); 37 | 38 | int getNbConstraints(); 39 | 40 | void computeConstantPart(); 41 | 42 | 43 | private: 44 | const LIPModel& lipModel_; 45 | const BaseModel& baseModel_; 46 | 47 | VectorX function_; 48 | MatrixX gradient_; 49 | MatrixX hessian_; 50 | }; 51 | 52 | } 53 | 54 | #ifdef _MSC_VER 55 | # pragma warning( pop ) 56 | #endif 57 | 58 | #endif 59 | -------------------------------------------------------------------------------- /mpc-walkgen/function/zebulon_tilt_velocity_minimization_objective.h: -------------------------------------------------------------------------------- 1 | //////////////////////////////////////////////////////////////////////////////// 2 | /// 3 | ///\file zebulon_tilt_velocity_minimization_objective.h 4 | ///\brief Implement the tilt velocity minimization objective 5 | ///\author Lafaye Jory 6 | ///\author Barthelemy Sebastien 7 | ///\version 1.0 8 | /// 9 | //////////////////////////////////////////////////////////////////////////////// 10 | 11 | #pragma once 12 | #ifndef MPC_WALKGEN_FUNCTION_ZEBULON_TILT_VELOCITY_MINIMIZATION_OBJECTIVE_H 13 | #define MPC_WALKGEN_FUNCTION_ZEBULON_TILT_VELOCITY_MINIMIZATION_OBJECTIVE_H 14 | 15 | #include 16 | #include 17 | 18 | #ifdef _MSC_VER 19 | # pragma warning( push ) 20 | // C4251: class needs to have DLL interface 21 | # pragma warning( disable: 4251) 22 | #endif 23 | 24 | namespace MPCWalkgen 25 | { 26 | template 27 | class TiltVelMinimizationObjective 28 | { 29 | TEMPLATE_TYPEDEF(Scalar) 30 | 31 | public: 32 | TiltVelMinimizationObjective(const LIPModel& lipModel, 33 | const BaseModel& baseModel); 34 | ~TiltVelMinimizationObjective(); 35 | 36 | const MatrixX& getGradient(const VectorX& x0); 37 | const MatrixX& getHessian(); 38 | 39 | void updateTiltContactPoint(); 40 | void computeConstantPart(); 41 | 42 | private: 43 | const LIPModel& lipModel_; 44 | const BaseModel& baseModel_; 45 | 46 | VectorX function_; 47 | MatrixX gradient_; 48 | MatrixX hessian_; 49 | 50 | MatrixX uInv_; 51 | LinearDynamic dynB_; 52 | LinearDynamic dynC_; 53 | LinearDynamic dynPsiX_; 54 | LinearDynamic dynPsiY_; 55 | 56 | }; 57 | 58 | } 59 | 60 | #ifdef _MSC_VER 61 | # pragma warning( pop ) 62 | #endif 63 | 64 | #endif 65 | -------------------------------------------------------------------------------- /mpc-walkgen/humanoid_walkgen_type.h: -------------------------------------------------------------------------------- 1 | //////////////////////////////////////////////////////////////////////////////// 2 | /// 3 | ///\file humanoid_walkgen_abstract.h 4 | ///\brief Main program for Humanoid 5 | ///\author de Gourcuff Martin 6 | ///\author Barthelemy Sebastien 7 | /// 8 | //////////////////////////////////////////////////////////////////////////////// 9 | 10 | #pragma once 11 | #ifndef MPC_WALKGEN_HUMANOID_WALKGEN_TYPE_H 12 | #define MPC_WALKGEN_HUMANOID_WALKGEN_TYPE_H 13 | 14 | #include 15 | #include 16 | 17 | namespace MPCWalkgen 18 | { 19 | template 20 | class MPC_WALKGEN_API HumanoidWalkgenWeighting 21 | { 22 | public: 23 | HumanoidWalkgenWeighting() 24 | :velocityTracking(0.) 25 | ,copCentering(0.) 26 | ,jerkMinimization(0.) 27 | {} 28 | 29 | Scalar velocityTracking; 30 | Scalar copCentering; 31 | Scalar jerkMinimization; 32 | }; 33 | 34 | template 35 | class MPC_WALKGEN_API HumanoidWalkgenConfig 36 | { 37 | public: 38 | HumanoidWalkgenConfig() 39 | :withCopConstraints(false) 40 | ,withFeetConstraints(false) 41 | {} 42 | 43 | bool withCopConstraints; 44 | bool withFeetConstraints; 45 | }; 46 | } 47 | 48 | #endif 49 | -------------------------------------------------------------------------------- /mpc-walkgen/interpolator.h: -------------------------------------------------------------------------------- 1 | //////////////////////////////////////////////////////////////////////////////// 2 | /// 3 | ///\file type.h 4 | ///\brief Some structures and typedefs 5 | ///\author Lafaye Jory 6 | ///\author de Gourcuff Martin 7 | ///\author Barthelemy Sebastien 8 | /// 9 | //////////////////////////////////////////////////////////////////////////////// 10 | 11 | #pragma once 12 | #ifndef MPC_WALKGEN_INTERPOLATOR_H 13 | #define MPC_WALKGEN_INTERPOLATOR_H 14 | 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | 21 | #ifdef _MSC_VER 22 | # pragma warning( push ) 23 | // C4251: class needs to have DLL interface 24 | # pragma warning( disable: 4251 ) 25 | #endif 26 | 27 | namespace MPCWalkgen 28 | { 29 | template 30 | class MPC_WALKGEN_API Interpolator 31 | { 32 | TEMPLATE_TYPEDEF(Scalar) 33 | public: 34 | Interpolator(); 35 | ~Interpolator(); 36 | 37 | /// \brief Computes normalised spline factors for each of its three polynoms 38 | /// (of degree three). Constraints are at both ends of the spline, 39 | /// and at the two junction between the three polynoms 40 | void computePolynomialNormalisedFactors(VectorX &factor, 41 | const Vector3 & initialstate, 42 | const Vector3 & finalState, 43 | Scalar T ) const; 44 | 45 | /// \brief select the proper 4 factors corresponding to one of the three polynoms in a 46 | /// normalised spline of degree three 47 | void selectFactors(Vector4 &subfactor, 48 | const VectorX &factor, 49 | Scalar t, 50 | Scalar T) const; 51 | private: 52 | /// \brief We have a cubic spline of three polynoms with 3 constraints 53 | /// (position, velocity and acceleration) on both edges of the spline, 54 | /// and 3 constraints (position velocity and acceleration) for each junctions 55 | /// between the three polynoms. The spline is normalised, and the junction are 56 | /// set at x=1/3 and x=2/3. We have then 2*3 + 2*3 = 12 equations 57 | /// (one per constraint), and 12 factor (4 for each polynom) to compute. 58 | /// As we have 3 constraints at x=0 for the first polynom, one can easily compute 59 | /// the three first factors. The 9 left factors can be obtained from the 9 60 | /// equations corresponding to the 9 left constraints. This set of 9 linear 61 | /// equations can be written as a matricial equation AX = b_. AinvNorm_ is 62 | /// then the inverse of matrix A. 63 | MatrixX aInvNorm_; 64 | mutable VectorX b_; 65 | mutable VectorX abc_; 66 | }; 67 | } 68 | 69 | #ifdef _MSC_VER 70 | # pragma warning( pop ) 71 | #endif 72 | 73 | #endif 74 | -------------------------------------------------------------------------------- /mpc-walkgen/lineardynamic.h: -------------------------------------------------------------------------------- 1 | //////////////////////////////////////////////////////////////////////////////// 2 | /// 3 | ///\file type.h 4 | ///\brief Some structures and typedefs 5 | ///\author Lafaye Jory 6 | ///\author de Gourcuff Martin 7 | ///\author Barthelemy Sebastien 8 | /// 9 | //////////////////////////////////////////////////////////////////////////////// 10 | 11 | #pragma once 12 | #ifndef MPC_WALKGEN_LINEARDYNAMIC_H 13 | #define MPC_WALKGEN_LINEARDYNAMIC_H 14 | 15 | #include 16 | #include 17 | 18 | #ifdef _MSC_VER 19 | # pragma warning( push ) 20 | // C4251: class needs to have DLL interface 21 | # pragma warning( disable: 4251 ) 22 | #endif 23 | 24 | namespace MPCWalkgen 25 | { 26 | /// \brief Matrices relative to a linear dynamic of type : Y = U X + S x + K 27 | /// Where X is the control variables and x the initial state 28 | /// UT is U transpose. 29 | /// K is a constant which does not depend on the initial state. For now it is 30 | /// always a zero vector, except for dynamics related to the CoP. 31 | /// If U is square, Uinv and UTinv are its inverse and transpose inverse. 32 | /// inverse, respectively. If U is not square, these matrices are filled 33 | /// with NaN values. 34 | /// -nbSamples stands for Y's number of rows 35 | /// -stateVectorSize stands for x's number of rows 36 | /// -variableVectorSize stands for X's number of rows 37 | template 38 | class MPC_WALKGEN_API LinearDynamic 39 | { 40 | TEMPLATE_TYPEDEF(Scalar) 41 | 42 | public: 43 | void reset(int nbSamples, 44 | int stateVectorSize, 45 | int variableVectorSize); 46 | 47 | public: 48 | MatrixX U; 49 | MatrixX UT; 50 | MatrixX Uinv; 51 | MatrixX UTinv; 52 | MatrixX S; 53 | VectorX K; 54 | }; 55 | } 56 | 57 | #ifdef _MSC_VER 58 | # pragma warning( pop ) 59 | #endif 60 | 61 | #endif 62 | -------------------------------------------------------------------------------- /mpc-walkgen/model/no_dynamic_model.h: -------------------------------------------------------------------------------- 1 | //////////////////////////////////////////////////////////////////////////////// 2 | /// 3 | ///\author Lafaye Jory 4 | /// 5 | //////////////////////////////////////////////////////////////////////////////// 6 | 7 | #pragma once 8 | #ifndef MPC_WALKGEN_NO_DYNAMIC_MODEL_H 9 | #define MPC_WALKGEN_NO_DYNAMIC_MODEL_H 10 | 11 | #include 12 | 13 | #ifdef _MSC_VER 14 | # pragma warning( push ) 15 | // C4251: class needs to have DLL interface 16 | # pragma warning( disable: 4251 ) 17 | #endif 18 | 19 | namespace MPCWalkgen 20 | { 21 | template 22 | class MPC_WALKGEN_API NoDynamicModel 23 | { 24 | TEMPLATE_TYPEDEF(Scalar) 25 | 26 | public: 27 | NoDynamicModel(int nbSamples, 28 | Scalar samplingPeriod, 29 | bool autoCompute); 30 | NoDynamicModel(); 31 | ~NoDynamicModel(); 32 | 33 | /// \brief compute all of the dynamics 34 | void computeDynamics(); 35 | void computePosDynamic(); 36 | void computeVelDynamic(); 37 | void computeAccDynamic(); 38 | void computeJerkDynamic(); 39 | 40 | /// \brief Get the linear dynamic correspond to the base position 41 | inline const LinearDynamic& getPosLinearDynamic() const 42 | {return posDynamic_;} 43 | 44 | /// \brief Get the linear dynamic correspond to the base velocity 45 | inline const LinearDynamic& getVelLinearDynamic() const 46 | {return velDynamic_;} 47 | 48 | /// \brief Get the linear dynamic correspond to the base acceleration 49 | inline const LinearDynamic& getAccLinearDynamic() const 50 | {return accDynamic_;} 51 | 52 | /// \brief Get the linear dynamic correspond to the base jerk 53 | inline const LinearDynamic& getJerkLinearDynamic() const 54 | {return jerkDynamic_;} 55 | 56 | /// \brief Get the state of the base 57 | inline const VectorX& getState() const 58 | {return state_;} 59 | 60 | /// \brief Get the state 61 | /// It's a vector of size 3: 62 | /// (Position, Velocity, Acceleration) 63 | inline void setState(const VectorX& state) 64 | { 65 | assert(state==state); 66 | assert(state.size()==3); 67 | state_ = state; 68 | } 69 | 70 | /// \brief Set the number of samples for this dynamic 71 | void setNbSamples(int nbSamples); 72 | 73 | /// \brief Get the number of samples for this dynamic 74 | inline int getNbSamples() const 75 | {return nbSamples_;} 76 | 77 | /// \brief Update the state 78 | void updateState(Scalar jerk, Scalar feedBackPeriod); 79 | 80 | /// \brief Set the sampling period for each sample 81 | void setSamplingPeriod(Scalar samplingPeriod); 82 | 83 | /// \brief Get the sampling period for each sample 84 | inline Scalar getSamplingPeriod(void) const 85 | {return samplingPeriod_;} 86 | 87 | /// \brief Get the velocity maximum value 88 | inline Scalar getVelocityLimit(void) const 89 | {return velocityLimit_;} 90 | 91 | /// \brief Set the velocity maximum value 92 | inline void setVelocityLimit(Scalar velocityLimit) 93 | { 94 | assert(velocityLimit>=0); 95 | velocityLimit_ = velocityLimit; 96 | } 97 | 98 | /// \brief Get the acceleration maximum value 99 | inline Scalar getAccelerationLimit(void) const 100 | {return accelerationLimit_;} 101 | 102 | /// \brief Set the acceleration maximum value 103 | inline void setAccelerationLimit(Scalar accelerationLimit) 104 | { 105 | assert(accelerationLimit>=0); 106 | accelerationLimit_ = accelerationLimit; 107 | } 108 | 109 | /// \brief Get the jerk maximum value 110 | inline Scalar getJerkLimit(void) const 111 | {return jerkLimit_;} 112 | 113 | /// \brief Set the jerk maximum value 114 | inline void setJerkLimit(Scalar jerkLimit) 115 | { 116 | assert(jerkLimit>=0); 117 | jerkLimit_ = jerkLimit; 118 | } 119 | 120 | private: 121 | bool autoCompute_; 122 | 123 | int nbSamples_; 124 | Scalar samplingPeriod_; 125 | 126 | VectorX state_; 127 | 128 | Scalar velocityLimit_; 129 | Scalar accelerationLimit_; 130 | Scalar jerkLimit_; 131 | 132 | LinearDynamic posDynamic_; 133 | LinearDynamic velDynamic_; 134 | LinearDynamic accDynamic_; 135 | LinearDynamic jerkDynamic_; 136 | }; 137 | } 138 | 139 | #ifdef _MSC_VER 140 | # pragma warning( pop ) 141 | #endif 142 | 143 | #endif 144 | -------------------------------------------------------------------------------- /mpc-walkgen/qpsolverfactory.h: -------------------------------------------------------------------------------- 1 | //////////////////////////////////////////////////////////////////////////////// 2 | /// 3 | ///\file qpsolverfactory.h 4 | ///\brief Templated factory functions for QP solvers 5 | ///\author Barthelemy Sebastien 6 | /// 7 | //////////////////////////////////////////////////////////////////////////////// 8 | 9 | #pragma once 10 | #ifndef MPC_WALKGEN_QPSOLVERFACTORY_H 11 | #define MPC_WALKGEN_QPSOLVERFACTORY_H 12 | 13 | #include 14 | #include 15 | 16 | namespace MPCWalkgen 17 | { 18 | 19 | template 20 | QPSolver *makeQPSolver(int nbVar, int nbCtr) 21 | { 22 | // useless default implementation 23 | std::abort(); 24 | return NULL; 25 | } 26 | 27 | // useful specializations 28 | template <> MPC_WALKGEN_API 29 | QPSolver *makeQPSolver(int nbVar, int nbCtr); 30 | 31 | template <> MPC_WALKGEN_API 32 | QPSolver *makeQPSolver(int nbVar, int nbCtr); 33 | } 34 | #endif 35 | -------------------------------------------------------------------------------- /mpc-walkgen/tools.h: -------------------------------------------------------------------------------- 1 | //////////////////////////////////////////////////////////////////////////////// 2 | /// 3 | ///\file tools.h 4 | ///\brief Some tools 5 | ///\author Lafaye Jory 6 | ///\author Barthelemy Sebastien 7 | /// 8 | //////////////////////////////////////////////////////////////////////////////// 9 | 10 | #pragma once 11 | #ifndef MPC_WALKGEN_TOOLS_H 12 | #define MPC_WALKGEN_TOOLS_H 13 | 14 | #include 15 | #include 16 | #include 17 | 18 | namespace MPCWalkgen 19 | { 20 | namespace Tools 21 | { 22 | template 23 | class MPC_WALKGEN_API ConstantJerkDynamic 24 | { 25 | TEMPLATE_TYPEDEF(Scalar) 26 | public: 27 | /// \brief These functions compute linear dynamics dyn relative to the 28 | /// CoP position or the LIP CoM position, velocity, 29 | /// acceleration or jerk. 30 | /// S is the remaining time before the next sample. 31 | /// T is the sampling period. 32 | /// N is the number of samples. 33 | 34 | static void computeCopDynamic(Scalar S, Scalar T, 35 | int N, LinearDynamic& dyn, 36 | Scalar comHeight, Scalar gravityX, 37 | Scalar gravityZ, Scalar mass, 38 | Scalar totalMass); 39 | 40 | static void computePosDynamic(Scalar S, Scalar T, 41 | int N, LinearDynamic& dyn); 42 | 43 | static void computeVelDynamic(Scalar S, Scalar T, 44 | int N, LinearDynamic& dyn); 45 | 46 | static void computeOrder2PosDynamic(Scalar S, Scalar T, 47 | int N, LinearDynamic& dyn); 48 | 49 | static void computeAccDynamic(Scalar S, Scalar T, 50 | int N, LinearDynamic& dyn); 51 | 52 | static void computeOrder2VelDynamic(Scalar S, Scalar T, 53 | int N, LinearDynamic& dyn); 54 | 55 | static void computeJerkDynamic(int N, LinearDynamic& dyn); 56 | 57 | static void updateState(Scalar jerk, Scalar T, VectorX& state); 58 | }; 59 | 60 | /// \brief Compute inverse of matrix A using LU decomposition, 61 | /// and store it in matrix Ap. All values < than eps are set to zero 62 | template 63 | void inverseLU(const typename Type::MatrixX& A, 64 | typename Type::MatrixX& Ap, Scalar eps) { 65 | Eigen::FullPivLU::MatrixX> lu(A); 66 | Ap = lu.inverse(); 67 | for(int i=0; i(0); 71 | } 72 | } 73 | } 74 | } 75 | 76 | /// \brief Compute inverse of matrix A using SVD decomposition, 77 | /// and store it in matrix Ap. All values < than eps are set to zero 78 | template 79 | void inverseSVD(const typename Type::MatrixX& A, 80 | typename Type::MatrixX& Ap, Scalar eps=1e-15f) { 81 | Eigen::JacobiSVD::MatrixX> svd = 82 | A.jacobiSvd(Eigen::ComputeFullU | Eigen::ComputeFullV); 83 | 84 | Scalar tolerance = eps * std::max(A.cols(), A.rows()) 85 | * svd.singularValues().array().abs().maxCoeff(); 86 | 87 | Ap = svd.matrixV() 88 | * ( (svd.singularValues().array().abs() > tolerance).select( 89 | svd.singularValues().array().inverse(), 0) 90 | ).matrix().asDiagonal() * svd.matrixU().adjoint(); 91 | } 92 | 93 | /// \brief Methods relative to the computation of polynomials 94 | template 95 | inline Scalar polynomValue(const typename Type::Vector4& factor, 96 | Scalar x) { 97 | return factor(0)*std::pow(x, 3) + 98 | factor(1)*std::pow(x, 2) + 99 | factor(2)*x + factor(3); 100 | } 101 | 102 | template 103 | inline Scalar dPolynomValue(const typename Type::Vector4& factor, 104 | Scalar x) { 105 | return 3.0f*factor(0)*std::pow(x, 2) + 2.0f*factor(1)*x + factor(2); 106 | } 107 | 108 | template 109 | inline Scalar ddPolynomValue(const typename Type::Vector4& factor, 110 | Scalar x) { 111 | return 6.0f*factor(0)*x + 2.0f*factor(1); 112 | } 113 | } 114 | } 115 | 116 | #endif 117 | -------------------------------------------------------------------------------- /mpc-walkgen/trajectory_walkgen.h: -------------------------------------------------------------------------------- 1 | //////////////////////////////////////////////////////////////////////////////// 2 | /// 3 | ///\file trajectory_walkgen.h 4 | ///\brief Main program for controlling over a trajectory 5 | ///\author Lafaye Jory 6 | ///\author Barthelemy Sebastien 7 | /// 8 | //////////////////////////////////////////////////////////////////////////////// 9 | 10 | #pragma once 11 | #ifndef MPC_WALKGEN_TRAJECTORY_WALKGEN_H 12 | #define MPC_WALKGEN_TRAJECTORY_WALKGEN_H 13 | 14 | #include 15 | #include 16 | 17 | 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | 24 | 25 | #include 26 | #include 27 | 28 | #include 29 | #include 30 | 31 | #ifdef _MSC_VER 32 | # pragma warning( push ) 33 | // C4251: class needs to have DLL interface 34 | // C4275: non dll-interface class used as base for dll-interface class 35 | # pragma warning( disable: 4251 4275) 36 | #endif 37 | 38 | namespace MPCWalkgen 39 | { 40 | template 41 | class MPC_WALKGEN_API TrajectoryWalkgen : boost::noncopyable 42 | { 43 | TEMPLATE_TYPEDEF(Scalar) 44 | public: 45 | TrajectoryWalkgen(); 46 | ~TrajectoryWalkgen(); 47 | 48 | void setNbSamples(int nbSamples); 49 | void setSamplingPeriod(Scalar samplingPeriod); 50 | 51 | void setVelRefInWorldFrame(const VectorX& velRef); 52 | void setPosRefInWorldFrame(const VectorX& posRef); 53 | 54 | void setVelLimit(Scalar limit); 55 | void setAccLimit(Scalar limit); 56 | void setJerkLimit(Scalar limit); 57 | 58 | void setState(const VectorX& state); 59 | 60 | void setWeightings(const TrajectoryWalkgenWeighting& weighting); 61 | void setConfig(const TrajectoryWalkgenConfig& config); 62 | 63 | bool solve(Scalar feedBackPeriod); 64 | 65 | const VectorX& getState() const; 66 | const Scalar getJerk() const; 67 | 68 | private: 69 | void computeConstantPart(); 70 | 71 | private: 72 | boost::scoped_ptr< QPSolver > qpoasesSolver_; 73 | 74 | NoDynamicModel noDynModel_; 75 | 76 | TrajectoryJerkMinimizationObjective jerkMinObj_; 77 | VelocityTrackingObjective velTrackingObj_; 78 | PositionTrackingObjective posTrackingObj_; 79 | MotionConstraint motionConstraint_; 80 | 81 | TrajectoryWalkgenWeighting weighting_; 82 | TrajectoryWalkgenConfig config_; 83 | 84 | VectorX dX_; 85 | VectorX X_; 86 | 87 | QPMatrices qpMatrix_; 88 | }; 89 | 90 | } 91 | #ifdef _MSC_VER 92 | # pragma warning( pop ) 93 | #endif 94 | 95 | #endif 96 | -------------------------------------------------------------------------------- /mpc-walkgen/trajectory_walkgen_type.h: -------------------------------------------------------------------------------- 1 | //////////////////////////////////////////////////////////////////////////////// 2 | /// 3 | ///\author Lafaye Jory 4 | /// 5 | //////////////////////////////////////////////////////////////////////////////// 6 | 7 | #pragma once 8 | #ifndef MPC_WALKGEN_TRAJECTORY_WALKGEN_TYPE_H 9 | #define MPC_WALKGEN_TRAJECTORY_WALKGEN_TYPE_H 10 | 11 | #include 12 | 13 | namespace MPCWalkgen 14 | { 15 | template 16 | class MPC_WALKGEN_API TrajectoryWalkgenWeighting 17 | { 18 | public: 19 | TrajectoryWalkgenWeighting() 20 | :velocityTracking(0.) 21 | ,positionTracking(0.) 22 | ,jerkMinimization(0.) 23 | {} 24 | 25 | Scalar velocityTracking; 26 | Scalar positionTracking; 27 | Scalar jerkMinimization; 28 | }; 29 | 30 | template 31 | class MPC_WALKGEN_API TrajectoryWalkgenConfig 32 | { 33 | public: 34 | TrajectoryWalkgenConfig() 35 | :withMotionConstraints(false) 36 | {} 37 | 38 | bool withMotionConstraints; 39 | }; 40 | } 41 | 42 | #endif 43 | -------------------------------------------------------------------------------- /mpc-walkgen/type.h: -------------------------------------------------------------------------------- 1 | //////////////////////////////////////////////////////////////////////////////// 2 | /// 3 | ///\file zebulon_type.h 4 | ///\brief Some structures and typedefs 5 | ///\author Lafaye Jory 6 | ///\author Barthelemy Sebastien 7 | /// 8 | //////////////////////////////////////////////////////////////////////////////// 9 | #pragma once 10 | #ifndef MPC_WALKGEN_TYPE_H 11 | #define MPC_WALKGEN_TYPE_H 12 | 13 | #include 14 | #include 15 | 16 | #define TEMPLATE_TYPEDEF(S) \ 17 | typedef typename Type::MatrixX MatrixX; \ 18 | typedef typename Type::MatrixXrm MatrixXrm; \ 19 | typedef typename Type::Matrix3 Matrix3; \ 20 | typedef typename Type::VectorX VectorX; \ 21 | typedef typename Type::Vector3 Vector3; \ 22 | typedef typename Type::Vector4 Vector4; \ 23 | typedef typename Type::Vector2 Vector2; \ 24 | typedef typename Type::vectorOfVector2 vectorOfVector2; \ 25 | typedef typename Type::vectorOfVector3 vectorOfVector3; \ 26 | typedef typename Type::vectorOfVector4 vectorOfVector4; 27 | 28 | 29 | namespace MPCWalkgen 30 | { 31 | /// \brief Typedefs to abstract the choice between float and double. 32 | /// The following "typedefed" matrices must contain Scalar type 33 | template 34 | struct Type 35 | { 36 | typedef Eigen::Matrix MatrixX; 37 | typedef Eigen::Matrix MatrixXrm; 38 | typedef Eigen::Matrix Matrix3; 39 | typedef Eigen::Matrix VectorX; 40 | typedef Eigen::Matrix Vector3; 41 | typedef Eigen::Matrix Vector2; 42 | typedef Eigen::Matrix Vector4; 43 | typedef std::vector > vectorOfVector2; 45 | typedef std::vector vectorOfVector3; 46 | typedef std::vector > vectorOfVector4; 48 | 49 | }; 50 | } 51 | 52 | #endif 53 | -------------------------------------------------------------------------------- /mpc-walkgen/zebulon_walkgen.h: -------------------------------------------------------------------------------- 1 | //////////////////////////////////////////////////////////////////////////////// 2 | /// 3 | ///\file zebulon_walkgen.h 4 | ///\brief Main program for Zebulon 5 | ///\author Lafaye Jory 6 | ///\author Barthelemy Sebastien 7 | /// 8 | //////////////////////////////////////////////////////////////////////////////// 9 | 10 | #pragma once 11 | #ifndef MPC_WALKGEN_ZEBULON_WALKGEN_H 12 | #define MPC_WALKGEN_ZEBULON_WALKGEN_H 13 | 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | 29 | #include 30 | #include 31 | 32 | #include 33 | #include 34 | #include 35 | 36 | #ifdef _MSC_VER 37 | # pragma warning( push ) 38 | // C4251: class needs to have DLL interface 39 | // C4275: non dll-interface class used as base for dll-interface class 40 | # pragma warning( disable: 4251 4275) 41 | #endif 42 | 43 | namespace MPCWalkgen 44 | { 45 | template 46 | class MPC_WALKGEN_API ZebulonWalkgen : boost::noncopyable 47 | { 48 | TEMPLATE_TYPEDEF(Scalar) 49 | public: 50 | ZebulonWalkgen(); 51 | ~ZebulonWalkgen(); 52 | 53 | void setNbSamples(int nbSamples); 54 | void setSamplingPeriod(Scalar samplingPeriod); 55 | 56 | void setGravity(const Vector3& gravity); 57 | void setBaseCopConvexPolygon(const ConvexPolygon& convexPolygon); 58 | void setBaseComConvexPolygon(const ConvexPolygon& convexPolygon); 59 | // legacy 60 | void setBaseCopHull(const vectorOfVector3& p); 61 | void setBaseComHull(const vectorOfVector3& p); 62 | 63 | void setComBodyHeight(Scalar comHeight); 64 | void setComBaseHeight(Scalar comHeight); 65 | void setBodyMass(Scalar mass); 66 | void setBaseMass(Scalar mass); 67 | 68 | void setVelRefInWorldFrame(const VectorX& velRef); 69 | void setPosRefInWorldFrame(const VectorX& posRef); 70 | void setCopRefInLocalFrame(const VectorX& copRef); 71 | void setComRefInLocalFrame(const VectorX& comRef); 72 | void setComAndCopRefInWorldFrame(const VectorX& comPosRefInWorldFrame, 73 | const VectorX& comAccRefInWorldFrame, 74 | const VectorX& basePosRefInWorldFrame, 75 | const VectorX& baseAccRefInWorldFrame); 76 | 77 | void setBaseVelLimit(Scalar limit); 78 | void setBaseAccLimit(Scalar limit); 79 | void setBaseJerkLimit(Scalar limit); 80 | 81 | void setBaseStateX(const VectorX& state); 82 | void setBaseStateY(const VectorX& state); 83 | void setBaseStateRoll(const VectorX& state); 84 | void setBaseStatePitch(const VectorX& state); 85 | void setBaseStateYaw(const VectorX& state); 86 | void setComStateX(const VectorX& state); 87 | void setComStateY(const VectorX& state); 88 | 89 | void setTiltContactPointOnTheGroundInLocalFrameX(Scalar pos); 90 | void setTiltContactPointOnTheGroundInLocalFrameY(Scalar pos); 91 | 92 | void setWeightings(const ZebulonWalkgenWeighting& weighting); 93 | void setConfig(const ZebulonWalkgenConfig& config); 94 | 95 | bool solve(Scalar feedBackPeriod); 96 | 97 | const VectorX& getBaseStateX() const; 98 | const VectorX& getBaseStateY() const; 99 | const VectorX& getComStateX() const; 100 | const VectorX& getComStateY() const; 101 | 102 | private: 103 | void computeConstantPart(); 104 | 105 | void computeNormalizationFactor(MatrixXrm& Q, MatrixXrm& A); 106 | 107 | private: 108 | LIPModel lipModel_; 109 | BaseModel baseModel_; 110 | 111 | BaseVelocityTrackingObjective velTrackingObj_; 112 | BasePositionTrackingObjective posTrackingObj_; 113 | JerkMinimizationObjective jerkMinObj_; 114 | TiltMinimizationObjective tiltMinObj_; 115 | TiltVelMinimizationObjective tiltVelMinObj_; 116 | CopCenteringObjective copCenteringObj_; 117 | ComCenteringObjective comCenteringObj_; 118 | CopConstraint copConstraint_; 119 | ComConstraint comConstraint_; 120 | BaseMotionConstraint baseMotionConstraint_; 121 | TiltMotionConstraint tiltMotionConstraint_; 122 | 123 | boost::scoped_ptr< QPSolver > qpoasesSolver_; 124 | 125 | ZebulonWalkgenWeighting weighting_; 126 | ZebulonWalkgenConfig config_; 127 | 128 | VectorX dX_; 129 | VectorX X_; 130 | VectorX B_; 131 | 132 | QPMatrices qpMatrix_; 133 | 134 | Scalar invObjNormFactor_; 135 | Scalar invCtrNormFactor_; 136 | }; 137 | 138 | } 139 | #ifdef _MSC_VER 140 | # pragma warning( pop ) 141 | #endif 142 | 143 | #endif 144 | -------------------------------------------------------------------------------- /mpc-walkgen/zebulon_walkgen_type.h: -------------------------------------------------------------------------------- 1 | //////////////////////////////////////////////////////////////////////////////// 2 | /// 3 | ///\file zebulon_walkgen_abstract.h 4 | ///\brief Main program for Zebulon 5 | ///\author Lafaye Jory 6 | ///\author Barthelemy Sebastien 7 | /// 8 | //////////////////////////////////////////////////////////////////////////////// 9 | 10 | #pragma once 11 | #ifndef MPC_WALKGEN_ZEBULON_WALKGEN_TYPE_H 12 | #define MPC_WALKGEN_ZEBULON_WALKGEN_TYPE_H 13 | 14 | #include 15 | 16 | namespace MPCWalkgen 17 | { 18 | template 19 | class MPC_WALKGEN_API ZebulonWalkgenWeighting 20 | { 21 | public: 22 | ZebulonWalkgenWeighting() 23 | :velocityTracking(0.) 24 | ,positionTracking(0.) 25 | ,copCentering(0.) 26 | ,comCentering(0.) 27 | ,jerkMinimization(0.) 28 | ,tiltMinimization(0.) 29 | ,tiltVelMinimization(0.) 30 | {} 31 | 32 | Scalar velocityTracking; 33 | Scalar positionTracking; 34 | Scalar copCentering; 35 | Scalar comCentering; 36 | Scalar jerkMinimization; 37 | Scalar tiltMinimization; 38 | Scalar tiltVelMinimization; 39 | }; 40 | 41 | template 42 | class MPC_WALKGEN_API ZebulonWalkgenConfig 43 | { 44 | public: 45 | ZebulonWalkgenConfig() 46 | :withCopConstraints(false) 47 | ,withComConstraints(false) 48 | ,withBaseMotionConstraints(false) 49 | ,withTiltMotionConstraints(false) 50 | {} 51 | 52 | bool withCopConstraints; 53 | bool withComConstraints; 54 | bool withBaseMotionConstraints; 55 | bool withTiltMotionConstraints; 56 | }; 57 | } 58 | 59 | #endif 60 | -------------------------------------------------------------------------------- /qiproject.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | -------------------------------------------------------------------------------- /qpsolver/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # Here we provide 3 libraries 2 | # 3 | # * mpc-walkgen_qpsolver is a header-only library which defines an 4 | # interface template (ie. pure virtual class template) 5 | # QPSolver for QP solvers. 6 | # And also factory template for such solvers. 7 | # 8 | # * mpc-walkgen_qpsolver_qpoases_{float,double} are two libraries which provide 9 | # (non-templated) factories returning QPSolver* based on 10 | # qpOASES compiled for float and double types, respectively. 11 | 12 | # We *need* to hide symbols in order to avoid qpOASES symbols clashes 13 | qi_sanitize_compile_flags(HIDDEN_SYMBOLS) 14 | 15 | include_directories(${CMAKE_CURRENT_SOURCE_DIR}) 16 | 17 | qi_stage_header_only_lib(mpc-walkgen_qpsolver 18 | INCLUDE_DIRS "${CMAKE_CURRENT_SOURCE_DIR}" 19 | DEPENDS EIGEN3) 20 | 21 | set(_mpc-walkgen_qpsolver_headers 22 | mpc-walkgen/qpsolver.h) 23 | qi_install_header(${_mpc-walkgen_qpsolver_headers} KEEP_RELATIVE_PATHS) 24 | 25 | 26 | set(_qpoases_name_double "qpOASES") 27 | set(_qpoases_name_float "qpOASESfloat") 28 | 29 | set(_qpoases_flags_double "") 30 | set(_qpoases_flags_float "-DQPOASES_REAL_IS_FLOAT") 31 | 32 | set(_qpoases_is_shared_double FALSE) 33 | set(_qpoases_is_shared_float TRUE) 34 | 35 | foreach(_scalar "double" "float") 36 | set(_mpc-walkgen_qpsolver_qpoases_${_scalar}_headers 37 | mpc-walkgen/qpsolver_qpoases_${_scalar}.h) 38 | 39 | qi_install_header(${_mpc-walkgen_qpsolver_qpoases_${_scalar}_headers} 40 | KEEP_RELATIVE_PATHS) 41 | 42 | qi_create_lib(mpc-walkgen_qpsolver_qpoases_${_scalar} SHARED 43 | ${_mpc-walkgen_qpsolver_qpoases_${_scalar}_headers} 44 | src/qpsolver_qpoases_${_scalar}.cpp 45 | src/qpsolver_qpoases.hxx 46 | ${_mpc-walkgen_qpsolver_headers}) 47 | 48 | if(_qpoases_flags_${_scalar}) 49 | set_target_properties(mpc-walkgen_qpsolver_qpoases_${_scalar} 50 | PROPERTIES COMPILE_FLAGS "${_qpoases_flags_${_scalar}}") 51 | endif() 52 | # mpc-walkgen_qpsolver_qpoases_${_scalar} is a shared lib which depends on 53 | # the lib ${_qpoases_name_${_scalar}} which is sometimes shared, sometimes 54 | # static. 55 | # People using mpc-walkgen_qpsolver_qpoases_${_scalar} should not link 56 | # with ${_qpoases_name_${_scalar}} when it is static. 57 | # Since qi_stage_lib exports all the dependencies by default we need to 58 | # explicitly list the "public" dependencies. 59 | set(_public_deps qi mpc-walkgen_qpsolver) 60 | set(_private_deps) 61 | if(_qpoases_is_shared_${_scalar}) 62 | list(APPEND _public_deps ${_qpoases_name_${_scalar}}) 63 | else() 64 | list(APPEND _private_deps ${_qpoases_name_${_scalar}}) 65 | endif() 66 | qi_use_lib(mpc-walkgen_qpsolver_qpoases_${_scalar} 67 | ${_public_deps} 68 | ${_private_deps}) 69 | # qi_stage_lib(... DEPENDS ...) expects a flat the list of all the public 70 | # dependencies, so let find the dependencies of our dependencies... 71 | set(_full_public_deps) 72 | foreach(_public_dep ${_public_deps}) 73 | string(TOUPPER ${_public_dep} _public_dep_upper) 74 | list(APPEND _full_public_deps ${_public_dep_upper}) 75 | list(APPEND _full_public_deps ${${_public_dep_upper}_DEPENDS}) 76 | endforeach() 77 | qi_stage_lib(mpc-walkgen_qpsolver_qpoases_${_scalar} 78 | DEPENDS ${_full_public_deps}) 79 | endforeach() 80 | -------------------------------------------------------------------------------- /qpsolver/mpc-walkgen/qpsolver.h: -------------------------------------------------------------------------------- 1 | //////////////////////////////////////////////////////////////////////////////// 2 | /// 3 | ///\file qpsolver.h 4 | ///\brief Interface template for a QP solver 5 | ///\author Lafaye Jory 6 | ///\author Barthelemy Sebastien 7 | /// 8 | //////////////////////////////////////////////////////////////////////////////// 9 | 10 | #pragma once 11 | #ifndef MPC_WALKGEN_QPSOLVER_H 12 | #define MPC_WALKGEN_QPSOLVER_H 13 | 14 | #include 15 | #include 16 | 17 | namespace MPCWalkgen 18 | { 19 | 20 | // matrices which describe a QP 21 | // x = argmin 0.5 * x' * Q * x + p'*x 22 | // 23 | // such that 24 | // xl <= x <= xu 25 | // bl <= A*x <= bu 26 | // Q is called the hessian 27 | // p is called the gradient 28 | // 29 | // The QP solver we use expects matrices in row-major order. 30 | // Note that this does not matter for the Q matrix, which is symmetric. 31 | template 32 | class QPMatrices 33 | { 34 | public: 35 | typedef Eigen::Matrix MatrixXrm; 36 | typedef Eigen::Matrix VectorX; 37 | 38 | bool dimensionsAreConsistent(int nbVar, int nbCtr) const; 39 | 40 | /// \brief Normalize all QP matrices except xu and xl using two normalization 41 | /// factors computed from Q and A 42 | void normalizeMatrices(Scalar epsilon); 43 | /// \brief If the smallest element m of mat is smaller than 1, 44 | /// this function returns 1/m. Otherwise it returns 1 45 | static Scalar getNormalizationFactor(const MatrixXrm& mat, Scalar epsilon); 46 | 47 | public: 48 | MatrixXrm Q; 49 | VectorX p; 50 | 51 | VectorX xl; 52 | VectorX xu; 53 | 54 | MatrixXrm A; 55 | VectorX bu; 56 | VectorX bl; 57 | }; 58 | 59 | template 60 | class QPSolver 61 | { 62 | 63 | public: 64 | virtual ~QPSolver() {} 65 | // Return true if a solution is found. In such a case, the solution is 66 | // written to sol. If no solution is found, sol may still be written to. 67 | // 68 | // if useWarmStart==true, and the solver was already initialized, 69 | // then m.Q and m.A won't be used. 70 | // 71 | // If no solution is found and os != nullptr, then debug information may 72 | // be written to *os. 73 | virtual bool solve(const QPMatrices& m, 74 | int maxNbIterations, 75 | typename QPMatrices::VectorX& sol, 76 | bool useWarmStart = false, 77 | std::ostream *os = nullptr) = 0; 78 | virtual int getNbVar() const = 0; 79 | virtual int getNbCtr() const = 0; 80 | }; 81 | 82 | ///QPMatrices 83 | template 84 | bool QPMatrices::dimensionsAreConsistent(int nbVar, int nbCtr) const { 85 | return (Q.rows() == nbVar) && (Q.cols() == nbVar) && (p.size() == nbVar) && 86 | (xl.size() == nbVar) && (xu.size() == nbVar) && 87 | (A.cols() == nbVar) && (A.rows() == nbCtr) && 88 | (bl.size() == nbCtr) && (bu.size() == nbCtr); 89 | } 90 | 91 | template 92 | void QPMatrices::normalizeMatrices(Scalar epsilon) 93 | { 94 | Scalar objFactor = getNormalizationFactor(Q, epsilon); 95 | Scalar ctrFactor = getNormalizationFactor(A, epsilon); 96 | 97 | Q *= objFactor; 98 | p *= objFactor; 99 | 100 | A *= ctrFactor; 101 | bu *= ctrFactor; 102 | bl *= ctrFactor; 103 | } 104 | 105 | template 106 | Scalar QPMatrices::getNormalizationFactor(const MatrixXrm& mat, 107 | Scalar epsilon) 108 | { 109 | Scalar normalizationFactor = 1.0; 110 | for(int i=0; iepsilon && v 13 | #define MPC_WALKGEN_QPSOLVER_QPOASES_DOUBLE_API QI_LIB_API(mpc_walkgen_qpsolver_qpoases_double) 14 | #include 15 | 16 | namespace MPCWalkgen 17 | { 18 | // factory: return a qpOASES solver 19 | MPC_WALKGEN_QPSOLVER_QPOASES_DOUBLE_API 20 | QPSolver *makeQPSolverDouble(int nbVar, int nbCtr); 21 | } 22 | 23 | #endif 24 | -------------------------------------------------------------------------------- /qpsolver/mpc-walkgen/qpsolver_qpoases_float.h: -------------------------------------------------------------------------------- 1 | //////////////////////////////////////////////////////////////////////////////// 2 | /// 3 | ///\file qpsolver_qpoases_double.h 4 | ///\brief Factory for qpoases solver, compiled with double 5 | ///\author Barthelemy Sebastien 6 | /// 7 | //////////////////////////////////////////////////////////////////////////////// 8 | 9 | #pragma once 10 | #ifndef MPC_WALKGEN_QPSOLVER_QPOASES_FLOAT_H 11 | #define MPC_WALKGEN_QPSOLVER_QPOASES_FLOAT_H 12 | #include 13 | #define MPC_WALKGEN_QPSOLVER_QPOASES_FLOAT_API QI_LIB_API(mpc_walkgen_qpsolver_qpoases_float) 14 | #include 15 | 16 | namespace MPCWalkgen 17 | { 18 | // factory: return a qpOASES solver 19 | MPC_WALKGEN_QPSOLVER_QPOASES_FLOAT_API 20 | QPSolver *makeQPSolverFloat(int nbVar, int nbCtr); 21 | } 22 | 23 | #endif 24 | -------------------------------------------------------------------------------- /qpsolver/src/qpsolver_qpoases.hxx: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #ifndef MPC_WALKGEN_QPSOLVER_SRC_QPOASES_HXX 3 | #define MPC_WALKGEN_QPSOLVER_SRC_QPOASES_HXX 4 | 5 | #include 6 | #include 7 | #include 8 | 9 | using namespace MPCWalkgen; 10 | 11 | template 12 | class QPOasesSolver : public QPSolver 13 | { 14 | public: 15 | QPOasesSolver(int nbVar, int nbCtr); 16 | 17 | bool solve(const QPMatrices& m, 18 | int maxNbIterations, 19 | typename QPMatrices::VectorX& sol, 20 | bool useWarmStart = false, 21 | std::ostream *os = nullptr); 22 | 23 | inline int getNbVar() const 24 | {return nbVar_;} 25 | 26 | inline int getNbCtr() const 27 | {return nbCtr_;} 28 | 29 | private: 30 | Eigen::VectorXi constraints_; 31 | int nbVar_; 32 | int nbCtr_; 33 | ::qpOASES::QProblem qp_; 34 | 35 | bool qpIsInitialized_; 36 | }; 37 | 38 | template 39 | QPOasesSolver::QPOasesSolver(int nbVar, int nbCtr) 40 | :constraints_(nbVar+nbCtr) 41 | ,nbVar_(nbVar) 42 | ,nbCtr_(nbCtr) 43 | ,qp_(nbVar, nbCtr) 44 | ,qpIsInitialized_(false) 45 | { 46 | constraints_.fill(0); 47 | qp_.setPrintLevel(qpOASES::PL_NONE); 48 | } 49 | 50 | template 51 | bool QPOasesSolver::solve(const QPMatrices& m, 52 | int maxNbIterations, 53 | typename QPMatrices::VectorX& sol, 54 | bool useWarmStart, 55 | std::ostream *os) 56 | { 57 | assert(m.dimensionsAreConsistent(nbVar_, nbCtr_)); 58 | 59 | qp_.setPrintLevel(qpOASES::PL_NONE); 60 | 61 | ::qpOASES::returnValue ret; 62 | const bool doWarmStart = qpIsInitialized_ && useWarmStart; 63 | if (doWarmStart) 64 | { 65 | ret = qp_.hotstart(m.p.data(), m.xl.data(), m.xu.data(), m.bl.data(), 66 | m.bu.data(), maxNbIterations, 0); 67 | } 68 | else 69 | { 70 | ret = qp_.init(m.Q.data(), m.p.data(), m.A.data(), 71 | m.xl.data(), m.xu.data(), m.bl.data(), m.bu.data(), 72 | maxNbIterations, 0); 73 | qpIsInitialized_ = true; 74 | } 75 | // we write the result even if qpOASES did report an error: 76 | // the "non-solution" might be useful: it is optimal but unfeasible with 77 | // respect to some constraints. 78 | qp_.getPrimalSolution(sol.data()); 79 | if ((ret != ::qpOASES::SUCCESSFUL_RETURN) && (os != nullptr)) 80 | { 81 | *os << "QP unfeasible. QPOases error code: " << ret 82 | << ". Number of working set recalculations: " << maxNbIterations 83 | << ". Was warm start: " << doWarmStart << ".\n"; 84 | return false; 85 | } 86 | 87 | return true; 88 | } 89 | 90 | #endif //MPC_WALKGEN_QPSOLVER_SRC_QPOASES_HXX 91 | -------------------------------------------------------------------------------- /qpsolver/src/qpsolver_qpoases_double.cpp: -------------------------------------------------------------------------------- 1 | #include "qpsolver_qpoases.hxx" 2 | #include 3 | namespace MPCWalkgen 4 | { 5 | QPSolver *makeQPSolverDouble(int nbVar, int nbCtr) 6 | {return new QPOasesSolver(nbVar, nbCtr);} 7 | } 8 | -------------------------------------------------------------------------------- /qpsolver/src/qpsolver_qpoases_float.cpp: -------------------------------------------------------------------------------- 1 | #include "qpsolver_qpoases.hxx" 2 | #include 3 | namespace MPCWalkgen 4 | { 5 | QPSolver *makeQPSolverFloat(int nbVar, int nbCtr) 6 | {return new QPOasesSolver(nbVar, nbCtr);} 7 | } 8 | -------------------------------------------------------------------------------- /src/function/humanoid_cop_centering_objective.cpp: -------------------------------------------------------------------------------- 1 | //////////////////////////////////////////////////////////////////////////////// 2 | /// 3 | ///\file humanoid_cop_centering_objective.cpp 4 | ///\brief Implement the CoP centering objective 5 | ///\author de Gourcuff Martin 6 | ///\author Barthelemy Sebastien 7 | /// 8 | //////////////////////////////////////////////////////////////////////////////// 9 | 10 | #include 11 | #include "../macro.h" 12 | 13 | namespace MPCWalkgen 14 | { 15 | template 16 | HumanoidCopCenteringObjective::HumanoidCopCenteringObjective 17 | (const LIPModel& lipModel, 18 | const HumanoidFeetSupervisor& feetSupervisor) 19 | :lipModel_(lipModel) 20 | ,feetSupervisor_(feetSupervisor) 21 | { 22 | gradient_.setZero(1); 23 | hessian_.setZero(1, 1); 24 | } 25 | 26 | template 27 | HumanoidCopCenteringObjective::~HumanoidCopCenteringObjective(){} 28 | 29 | template 30 | const typename 31 | Type::VectorX& HumanoidCopCenteringObjective::getGradient(const VectorX& x0) 32 | { 33 | assert(feetSupervisor_.getNbSamples() == lipModel_.getNbSamples()); 34 | assert(x0.rows() == 2*lipModel_.getNbSamples() + 2*feetSupervisor_.getNbPreviewedSteps()); 35 | 36 | gradient_.noalias() = getHessian()*x0; 37 | 38 | return gradient_; 39 | } 40 | 41 | template 42 | const typename Type::MatrixX& HumanoidCopCenteringObjective::getHessian() 43 | { 44 | int N = lipModel_.getNbSamples(); 45 | int M = feetSupervisor_.getNbPreviewedSteps(); 46 | 47 | hessian_ = MatrixX::Identity(2*N + 2*M, 2*N + 2*M); 48 | hessian_.block(2*N, 2*N, 2*M, 2*M) = MatrixX::Zero(2*M, 2*M); 49 | 50 | return hessian_; 51 | } 52 | 53 | MPC_WALKGEN_INSTANTIATE_CLASS_TEMPLATE(HumanoidCopCenteringObjective); 54 | } 55 | -------------------------------------------------------------------------------- /src/function/humanoid_lip_com_jerk_minimization_objective.cpp: -------------------------------------------------------------------------------- 1 | //////////////////////////////////////////////////////////////////////////////// 2 | /// 3 | ///\file humanoid_lip_com_jerk_minimization_objective.cpp 4 | ///\brief Implement the LIP CoM jerk minimization objective 5 | ///\author de Gourcuff Martin 6 | ///\author Barthelemy Sebastien 7 | /// 8 | //////////////////////////////////////////////////////////////////////////////// 9 | 10 | #include 11 | #include "../macro.h" 12 | 13 | namespace MPCWalkgen 14 | { 15 | template 16 | HumanoidLipComJerkMinimizationObjective::HumanoidLipComJerkMinimizationObjective( 17 | const LIPModel& lipModel, 18 | const HumanoidFeetSupervisor& feetSupervisor) 19 | :lipModel_(lipModel) 20 | ,feetSupervisor_(feetSupervisor) 21 | { 22 | assert(feetSupervisor_.getNbSamples() == lipModel_.getNbSamples()); 23 | 24 | gradient_.setZero(1); 25 | hessian_.setZero(1, 1); 26 | } 27 | 28 | template 29 | HumanoidLipComJerkMinimizationObjective::~HumanoidLipComJerkMinimizationObjective(){} 30 | 31 | template 32 | const typename Type::VectorX& 33 | HumanoidLipComJerkMinimizationObjective::getGradient(const VectorX& x0) 34 | { 35 | assert(x0.rows() == 2*lipModel_.getNbSamples() + 2*feetSupervisor_.getNbPreviewedSteps()); 36 | 37 | int N = lipModel_.getNbSamples(); 38 | int M = feetSupervisor_.getNbPreviewedSteps(); 39 | 40 | int nb = feetSupervisor_.getNbOfCallsBeforeNextSample() - 1; 41 | 42 | const LinearDynamic& dynCopX = lipModel_.getCopXLinearDynamic(nb); 43 | const LinearDynamic& dynCopY = lipModel_.getCopYLinearDynamic(nb); 44 | const LinearDynamic& dynFeetPos = feetSupervisor_.getFeetPosLinearDynamic(); 45 | 46 | const MatrixX& weight = feetSupervisor_.getSampleWeightMatrix(); 47 | 48 | //TODO: sparse matrices? 49 | // Ill change these tmp matrices after calculus optimization 50 | // I may change some function to avoid copies 51 | MatrixX tmp = MatrixX::Zero(2*N + 2*M, 2*N); 52 | tmp.block(0, 0, 2*N, 2*N) = feetSupervisor_.getRotationMatrix(); 53 | tmp.block(2*N, 0, M, N) = feetSupervisor_.getFeetPosLinearDynamic().UT; 54 | tmp.block(2*N + M, N, M, N) = feetSupervisor_.getFeetPosLinearDynamic().UT; 55 | 56 | VectorX tmp2 = VectorX::Zero(2*N); 57 | tmp2.segment(0, N) = dynCopX.UTinv*weight*dynCopX.Uinv*dynFeetPos.S 58 | *feetSupervisor_.getSupportFootStateX()(0) 59 | - dynCopX.UTinv*weight*dynCopX.Uinv 60 | *(dynCopX.S*lipModel_.getStateX() + dynCopX.K); 61 | 62 | tmp2.segment(N, N) = dynCopY.UTinv*weight*dynCopY.Uinv*dynFeetPos.S 63 | *feetSupervisor_.getSupportFootStateY()(0) 64 | - dynCopY.UTinv*weight*dynCopY.Uinv 65 | *(dynCopY.S*lipModel_.getStateY() + dynCopY.K); 66 | 67 | gradient_ = tmp*tmp2; 68 | gradient_ += getHessian()*x0; 69 | return gradient_; 70 | } 71 | 72 | template 73 | const typename 74 | Type::MatrixX& HumanoidLipComJerkMinimizationObjective::getHessian() 75 | { 76 | assert(feetSupervisor_.getNbSamples() == lipModel_.getNbSamples());; 77 | 78 | int N = lipModel_.getNbSamples(); 79 | int M = feetSupervisor_.getNbPreviewedSteps(); 80 | 81 | int nb = feetSupervisor_.getNbOfCallsBeforeNextSample() - 1; 82 | 83 | const LinearDynamic& dynCopX = lipModel_.getCopXLinearDynamic(nb); 84 | const LinearDynamic& dynCopY = lipModel_.getCopYLinearDynamic(nb); 85 | 86 | MatrixX tmp = MatrixX::Zero(2*N, 2*N + 2*M); 87 | 88 | tmp.block(0, 0, 2*N, 2*N) = feetSupervisor_.getRotationMatrixT(); 89 | tmp.block(0, 2*N, N, M) = feetSupervisor_.getFeetPosLinearDynamic().U; 90 | tmp.block(N, 2*N + M, N, M) = feetSupervisor_.getFeetPosLinearDynamic().U; 91 | 92 | MatrixX tmp2 = MatrixX::Zero(2*N, 2*N); 93 | 94 | const MatrixX& weight = feetSupervisor_.getSampleWeightMatrix(); 95 | 96 | tmp2.block(0, 0, N, N) = dynCopX.UTinv*weight*dynCopX.Uinv; 97 | tmp2.block(N, N, N, N) = dynCopY.UTinv*weight*dynCopY.Uinv; 98 | hessian_.noalias() = tmp.transpose()*tmp2*tmp; 99 | 100 | return hessian_; 101 | } 102 | 103 | MPC_WALKGEN_INSTANTIATE_CLASS_TEMPLATE(HumanoidLipComJerkMinimizationObjective); 104 | } 105 | -------------------------------------------------------------------------------- /src/function/humanoid_lip_com_velocity_tracking_objective.cpp: -------------------------------------------------------------------------------- 1 | //////////////////////////////////////////////////////////////////////////////// 2 | /// 3 | ///\file humanoid_lip_com_velocity_tracking_objective.cpp 4 | ///\brief Implement the LIP CoM velocity tracking objective 5 | ///\author de Gourcuff Martin 6 | ///\author Barthelemy Sebastien 7 | /// 8 | //////////////////////////////////////////////////////////////////////////////// 9 | 10 | #include 11 | #include "../macro.h" 12 | 13 | namespace MPCWalkgen 14 | { 15 | template 16 | HumanoidLipComVelocityTrackingObjective::HumanoidLipComVelocityTrackingObjective( 17 | const LIPModel& lipModel, 18 | const HumanoidFeetSupervisor &feetSupervisor) 19 | :lipModel_(lipModel) 20 | ,feetSupervisor_(feetSupervisor) 21 | { 22 | assert(feetSupervisor_.getNbSamples() == lipModel_.getNbSamples()); 23 | 24 | velRefInWorldFrame_.setZero(2*lipModel_.getNbSamples()); 25 | gradient_.setZero(1); 26 | hessian_.setZero(1, 1); 27 | } 28 | 29 | template 30 | HumanoidLipComVelocityTrackingObjective::~HumanoidLipComVelocityTrackingObjective(){} 31 | 32 | template 33 | const typename Type::VectorX& 34 | HumanoidLipComVelocityTrackingObjective::getGradient(const VectorX& x0) 35 | { 36 | assert(velRefInWorldFrame_.size()==2*lipModel_.getNbSamples()); 37 | assert(x0.rows() == 38 | 2*feetSupervisor_.getNbSamples() + 2*feetSupervisor_.getNbPreviewedSteps()); 39 | 40 | int N = lipModel_.getNbSamples(); 41 | int M = feetSupervisor_.getNbPreviewedSteps(); 42 | 43 | int nb = feetSupervisor_.getNbOfCallsBeforeNextSample() - 1; 44 | 45 | const LinearDynamic& dynCopX = lipModel_.getCopXLinearDynamic(nb); 46 | const LinearDynamic& dynCopY = lipModel_.getCopYLinearDynamic(nb); 47 | const LinearDynamic& dynComVel = lipModel_.getComVelLinearDynamic(nb); 48 | const LinearDynamic& dynFeetPos = feetSupervisor_.getFeetPosLinearDynamic(); 49 | 50 | const MatrixX& weight = feetSupervisor_.getSampleWeightMatrix(); 51 | 52 | MatrixX tmp = MatrixX::Zero(2*N, 1); //I need a matrix here 53 | VectorX tmp2 = VectorX::Zero(2*N); 54 | 55 | tmp2.segment(0, N) = 56 | dynComVel.S*lipModel_.getStateX() 57 | - dynComVel.U*dynCopX.Uinv*(dynCopX.S*lipModel_.getStateX() + dynCopX.K) 58 | + dynComVel.U*dynCopX.Uinv*dynFeetPos.S*feetSupervisor_.getSupportFootStateX()(0) 59 | - velRefInWorldFrame_.segment(0, N); 60 | tmp.block(0, 0, N, 1) = weight*tmp2.block(0, 0, N, 1); 61 | 62 | tmp2.segment(N, N) = 63 | dynComVel.S*lipModel_.getStateY() 64 | - dynComVel.U*dynCopY.Uinv*(dynCopY.S*lipModel_.getStateY() + dynCopY.K) 65 | + dynComVel.U*dynCopY.Uinv*dynFeetPos.S*feetSupervisor_.getSupportFootStateY()(0) 66 | - velRefInWorldFrame_.segment(N, N); 67 | tmp.block(N, 0, N, 1) = weight*tmp2.block(N, 0, N, 1); 68 | 69 | tmp2.segment(0, N) = dynCopX.UTinv*dynComVel.UT*tmp.block(0, 0, N, 1); 70 | tmp2.segment(N, N) = dynCopY.UTinv*dynComVel.UT*tmp.block(N, 0, N, 1); 71 | 72 | tmp.setZero(2*N + 2*M, 2*N); 73 | 74 | tmp.block(0, 0, 2*N, 2*N) = feetSupervisor_.getRotationMatrix(); 75 | tmp.block(2*N, 0, M, N) = dynFeetPos.UT; 76 | tmp.block(2*N + M, N, M, N) = dynFeetPos.UT; 77 | 78 | gradient_ = tmp*tmp2; 79 | gradient_ += getHessian()*x0; 80 | 81 | return gradient_; 82 | } 83 | 84 | template 85 | const typename 86 | Type::MatrixX& HumanoidLipComVelocityTrackingObjective::getHessian() 87 | { 88 | assert(feetSupervisor_.getNbSamples() == lipModel_.getNbSamples()); 89 | 90 | int N = lipModel_.getNbSamples(); 91 | int M = feetSupervisor_.getNbPreviewedSteps(); 92 | 93 | int nb = feetSupervisor_.getNbOfCallsBeforeNextSample() - 1; 94 | 95 | const LinearDynamic& dynCopX = lipModel_.getCopXLinearDynamic(nb); 96 | const LinearDynamic& dynCopY = lipModel_.getCopYLinearDynamic(nb); 97 | const LinearDynamic& dynComVel = lipModel_.getComVelLinearDynamic(nb); 98 | const LinearDynamic& dynFeetPos = feetSupervisor_.getFeetPosLinearDynamic(); 99 | 100 | MatrixX tmp = MatrixX::Zero(2*N, 2*N + 2*M); 101 | 102 | tmp.block(0, 0, 2*N, 2*N) = feetSupervisor_.getRotationMatrixT(); 103 | tmp.block(0, 2*N, N, M) = dynFeetPos.U; 104 | tmp.block(N, 2*N + M, N, M) = dynFeetPos.U; 105 | 106 | MatrixX tmp2 = MatrixX::Zero(2*N, 2*N); 107 | 108 | const MatrixX& weight = feetSupervisor_.getSampleWeightMatrix(); 109 | 110 | tmp2.block(0, 0, N, N) = dynCopX.UTinv*dynComVel.UT*weight*dynComVel.U*dynCopX.Uinv; 111 | tmp2.block(N, N, N, N) = dynCopY.UTinv*dynComVel.UT*weight*dynComVel.U*dynCopY.Uinv; 112 | 113 | hessian_ = tmp.transpose()*tmp2*tmp; 114 | 115 | return hessian_; 116 | } 117 | 118 | template 119 | void HumanoidLipComVelocityTrackingObjective::setVelRefInWorldFrame( 120 | const VectorX& velRefInWorldFrame) 121 | { 122 | assert(velRefInWorldFrame.size()==2*lipModel_.getNbSamples()); 123 | assert(velRefInWorldFrame==velRefInWorldFrame); 124 | 125 | velRefInWorldFrame_ = velRefInWorldFrame; 126 | } 127 | 128 | MPC_WALKGEN_INSTANTIATE_CLASS_TEMPLATE(HumanoidLipComVelocityTrackingObjective); 129 | } 130 | -------------------------------------------------------------------------------- /src/function/trajectory_jerk_minimization_objective.cpp: -------------------------------------------------------------------------------- 1 | //////////////////////////////////////////////////////////////////////////////// 2 | /// 3 | ///\author Lafaye Jory 4 | /// 5 | //////////////////////////////////////////////////////////////////////////////// 6 | 7 | #include 8 | #include "../macro.h" 9 | 10 | using namespace MPCWalkgen; 11 | 12 | template 13 | TrajectoryJerkMinimizationObjective::TrajectoryJerkMinimizationObjective( 14 | const NoDynamicModel& model) 15 | :model_(model) 16 | ,function_(1) 17 | { 18 | function_.fill(0); 19 | gradient_.setZero(1, 1); 20 | hessian_.setZero(1, 1); 21 | 22 | computeConstantPart(); 23 | } 24 | 25 | 26 | template 27 | TrajectoryJerkMinimizationObjective::~TrajectoryJerkMinimizationObjective(){} 28 | 29 | template 30 | const typename 31 | Type::VectorX& TrajectoryJerkMinimizationObjective::getGradient(const VectorX& x0) 32 | { 33 | assert(model_.getNbSamples()==x0.size()); 34 | 35 | return x0; 36 | } 37 | 38 | template 39 | const typename Type::MatrixX& TrajectoryJerkMinimizationObjective::getHessian() 40 | { 41 | return hessian_; 42 | } 43 | 44 | template 45 | void TrajectoryJerkMinimizationObjective::computeConstantPart() 46 | { 47 | int N = model_.getNbSamples(); 48 | hessian_ = MatrixX::Identity(N, N); 49 | } 50 | 51 | namespace MPCWalkgen 52 | { 53 | MPC_WALKGEN_INSTANTIATE_CLASS_TEMPLATE(TrajectoryJerkMinimizationObjective); 54 | } 55 | -------------------------------------------------------------------------------- /src/function/trajectory_motion_constraint.cpp: -------------------------------------------------------------------------------- 1 | //////////////////////////////////////////////////////////////////////////////// 2 | /// 3 | ///\author Lafaye Jory 4 | /// 5 | //////////////////////////////////////////////////////////////////////////////// 6 | 7 | #include 8 | #include "../macro.h" 9 | 10 | using namespace MPCWalkgen; 11 | 12 | template 13 | MotionConstraint::MotionConstraint(const NoDynamicModel& model) 14 | :model_(model) 15 | ,functionInf_(1) 16 | ,functionSup_(1) 17 | ,tmp_(1) 18 | ,tmp2_(1) 19 | { 20 | functionInf_.fill(0); 21 | functionSup_.fill(0); 22 | gradient_.setZero(1, 1); 23 | hessian_.setZero(1, 1); 24 | 25 | computeConstantPart(); 26 | } 27 | 28 | 29 | template 30 | MotionConstraint::~MotionConstraint(){} 31 | 32 | template 33 | const typename 34 | Type::VectorX& MotionConstraint::getFunctionInf(const VectorX& x0) 35 | { 36 | assert(model_.getNbSamples()==x0.size()); 37 | 38 | computeFunction(x0, functionInf_, 39 | -model_.getVelocityLimit(), 40 | -model_.getAccelerationLimit()); 41 | 42 | return functionInf_; 43 | } 44 | 45 | template 46 | const typename 47 | Type::VectorX& MotionConstraint::getFunctionSup(const VectorX& x0) 48 | { 49 | assert(model_.getNbSamples()==x0.size()); 50 | 51 | computeFunction(x0, functionSup_, 52 | model_.getVelocityLimit(), 53 | model_.getAccelerationLimit()); 54 | 55 | return functionSup_; 56 | } 57 | 58 | template 59 | void MotionConstraint::computeFunction(const VectorX& x0, VectorX& func, 60 | Scalar velLimit, Scalar accLimit) 61 | { 62 | int N = model_.getNbSamples(); 63 | 64 | const LinearDynamic& dynBaseVel = model_.getVelLinearDynamic(); 65 | const LinearDynamic& dynBaseAcc = model_.getAccLinearDynamic(); 66 | 67 | tmp_.fill(velLimit); 68 | tmp2_.fill(accLimit); 69 | 70 | func.noalias() = -getGradient()*x0; 71 | func.segment(0, N).noalias() += tmp_; 72 | func.segment(0, N).noalias() -= dynBaseVel.S * model_.getState(); 73 | func.segment(N, N).noalias() += tmp2_; 74 | func.segment(N, N).noalias() -= dynBaseAcc.S * model_.getState(); 75 | 76 | } 77 | 78 | template 79 | const typename Type::MatrixX& MotionConstraint::getGradient() 80 | { 81 | return gradient_; 82 | } 83 | 84 | template 85 | int MotionConstraint::getNbConstraints() 86 | { 87 | return model_.getNbSamples()*2; 88 | } 89 | 90 | template 91 | void MotionConstraint::computeConstantPart() 92 | { 93 | const LinearDynamic& dynBaseVel = model_.getVelLinearDynamic(); 94 | const LinearDynamic& dynBaseAcc = model_.getAccLinearDynamic(); 95 | 96 | int N = model_.getNbSamples(); 97 | int M = getNbConstraints(); 98 | 99 | gradient_.setZero(M, N); 100 | gradient_.block(0, 0, N, N) = dynBaseVel.U; 101 | gradient_.block(N, 0, N, N) = dynBaseAcc.U; 102 | 103 | tmp_.resize(N); 104 | tmp2_.resize(N); 105 | } 106 | 107 | namespace MPCWalkgen 108 | { 109 | MPC_WALKGEN_INSTANTIATE_CLASS_TEMPLATE(MotionConstraint); 110 | } 111 | -------------------------------------------------------------------------------- /src/function/trajectory_position_tracking_objective.cpp: -------------------------------------------------------------------------------- 1 | //////////////////////////////////////////////////////////////////////////////// 2 | /// 3 | ///\author Lafaye Jory 4 | /// 5 | //////////////////////////////////////////////////////////////////////////////// 6 | 7 | #include 8 | #include "../macro.h" 9 | 10 | using namespace MPCWalkgen; 11 | 12 | template 13 | PositionTrackingObjective 14 | ::PositionTrackingObjective(const NoDynamicModel& model) 15 | :model_(model) 16 | ,function_(1) 17 | ,tmp_(1) 18 | { 19 | posRefInWorldFrame_.setZero(model.getNbSamples()); 20 | 21 | function_.fill(0); 22 | gradient_.setZero(1, 1); 23 | hessian_.setZero(1, 1); 24 | 25 | computeConstantPart(); 26 | } 27 | 28 | 29 | template 30 | PositionTrackingObjective::~PositionTrackingObjective(){} 31 | 32 | template 33 | const typename 34 | Type::MatrixX& PositionTrackingObjective::getGradient(const VectorX& x0) 35 | { 36 | assert(posRefInWorldFrame_.size()==x0.size()); 37 | assert(posRefInWorldFrame_.size()==model_.getNbSamples()); 38 | 39 | const LinearDynamic& dyn = model_.getPosLinearDynamic(); 40 | 41 | gradient_.noalias() = getHessian()*x0; 42 | 43 | 44 | tmp_.noalias() = dyn.S * model_.getState(); 45 | tmp_.noalias() -= posRefInWorldFrame_; 46 | gradient_.noalias() += dyn.UT*tmp_; 47 | return gradient_; 48 | } 49 | 50 | template 51 | const typename Type::MatrixX& PositionTrackingObjective::getHessian() 52 | { 53 | return hessian_; 54 | } 55 | 56 | template 57 | void PositionTrackingObjective::setPosRefInWorldFrame(const VectorX& posRefInWorldFrame) 58 | { 59 | assert(posRefInWorldFrame.size()==model_.getNbSamples()); 60 | assert(posRefInWorldFrame==posRefInWorldFrame); 61 | 62 | posRefInWorldFrame_ = posRefInWorldFrame; 63 | } 64 | 65 | 66 | template 67 | void PositionTrackingObjective::computeConstantPart() 68 | { 69 | const LinearDynamic& dyn = model_.getPosLinearDynamic(); 70 | 71 | int N = model_.getNbSamples(); 72 | 73 | hessian_ = dyn.UT*dyn.U; 74 | 75 | tmp_.resize(N); 76 | } 77 | 78 | namespace MPCWalkgen 79 | { 80 | MPC_WALKGEN_INSTANTIATE_CLASS_TEMPLATE(PositionTrackingObjective); 81 | } 82 | -------------------------------------------------------------------------------- /src/function/trajectory_velocity_tracking_objective.cpp: -------------------------------------------------------------------------------- 1 | //////////////////////////////////////////////////////////////////////////////// 2 | /// 3 | ///\author Lafaye Jory 4 | /// 5 | //////////////////////////////////////////////////////////////////////////////// 6 | 7 | #include 8 | #include "../macro.h" 9 | 10 | using namespace MPCWalkgen; 11 | 12 | template 13 | VelocityTrackingObjective 14 | ::VelocityTrackingObjective(const NoDynamicModel& model) 15 | :model_(model) 16 | ,function_(1) 17 | ,tmp_(1) 18 | { 19 | velRefInWorldFrame_.setZero(model.getNbSamples()); 20 | 21 | function_.fill(0); 22 | gradient_.setZero(1, 1); 23 | hessian_.setZero(1, 1); 24 | 25 | computeConstantPart(); 26 | } 27 | 28 | 29 | template 30 | VelocityTrackingObjective::~VelocityTrackingObjective(){} 31 | 32 | template 33 | const typename 34 | Type::MatrixX& VelocityTrackingObjective::getGradient(const VectorX& x0) 35 | { 36 | assert(velRefInWorldFrame_.size()==x0.size()); 37 | assert(velRefInWorldFrame_.size()==model_.getNbSamples()); 38 | 39 | const LinearDynamic& dyn = model_.getVelLinearDynamic(); 40 | 41 | gradient_.noalias() = getHessian()*x0; 42 | 43 | 44 | tmp_.noalias() = dyn.S * model_.getState(); 45 | tmp_.noalias() -= velRefInWorldFrame_; 46 | gradient_.noalias() += dyn.UT*tmp_; 47 | 48 | 49 | return gradient_; 50 | } 51 | 52 | template 53 | const typename Type::MatrixX& VelocityTrackingObjective::getHessian() 54 | { 55 | return hessian_; 56 | } 57 | 58 | template 59 | void VelocityTrackingObjective::setVelRefInWorldFrame(const VectorX& velRefInWorldFrame) 60 | { 61 | assert(velRefInWorldFrame.size()==model_.getNbSamples()); 62 | assert(velRefInWorldFrame==velRefInWorldFrame); 63 | 64 | velRefInWorldFrame_ = velRefInWorldFrame; 65 | } 66 | 67 | template 68 | void VelocityTrackingObjective::computeConstantPart() 69 | { 70 | const LinearDynamic& dyn = model_.getVelLinearDynamic(); 71 | 72 | int N = model_.getNbSamples(); 73 | 74 | hessian_ = dyn.UT*dyn.U; 75 | 76 | tmp_.resize(N); 77 | } 78 | 79 | namespace MPCWalkgen 80 | { 81 | MPC_WALKGEN_INSTANTIATE_CLASS_TEMPLATE(VelocityTrackingObjective); 82 | } 83 | -------------------------------------------------------------------------------- /src/function/zebulon_base_motion_constraint.cpp: -------------------------------------------------------------------------------- 1 | //////////////////////////////////////////////////////////////////////////////// 2 | /// 3 | ///\author Lafaye Jory 4 | ///\author Barthelemy Sebastien 5 | /// 6 | //////////////////////////////////////////////////////////////////////////////// 7 | 8 | #include 9 | #include "../macro.h" 10 | 11 | using namespace MPCWalkgen; 12 | 13 | template 14 | BaseMotionConstraint::BaseMotionConstraint(const BaseModel& baseModel) 15 | :baseModel_(baseModel) 16 | ,functionInf_(1) 17 | ,functionSup_(1) 18 | ,tmp_(1) 19 | ,tmp2_(1) 20 | { 21 | functionInf_.fill(0); 22 | functionSup_.fill(0); 23 | gradient_.setZero(1, 1); 24 | hessian_.setZero(1, 1); 25 | 26 | computeConstantPart(); 27 | } 28 | 29 | 30 | template 31 | BaseMotionConstraint::~BaseMotionConstraint(){} 32 | 33 | template 34 | const typename 35 | Type::VectorX& BaseMotionConstraint::getFunctionInf(const VectorX& x0) 36 | { 37 | assert(baseModel_.getNbSamples()*2==x0.size()); 38 | 39 | computeFunction(x0, functionInf_, 40 | -baseModel_.getVelocityLimit(), 41 | -baseModel_.getAccelerationLimit()); 42 | 43 | return functionInf_; 44 | } 45 | 46 | template 47 | const typename 48 | Type::VectorX& BaseMotionConstraint::getFunctionSup(const VectorX& x0) 49 | { 50 | assert(baseModel_.getNbSamples()*2==x0.size()); 51 | 52 | computeFunction(x0, functionSup_, 53 | baseModel_.getVelocityLimit(), 54 | baseModel_.getAccelerationLimit()); 55 | 56 | return functionSup_; 57 | } 58 | 59 | template 60 | void BaseMotionConstraint::computeFunction(const VectorX& x0, VectorX& func, 61 | Scalar velLimit, Scalar accLimit) 62 | { 63 | int N = baseModel_.getNbSamples(); 64 | 65 | const LinearDynamic& dynBaseVel = baseModel_.getBaseVelLinearDynamic(); 66 | const LinearDynamic& dynBaseAcc = baseModel_.getBaseAccLinearDynamic(); 67 | 68 | tmp_.fill(velLimit); 69 | tmp2_.fill(accLimit); 70 | 71 | func.noalias() = -getGradient()*x0; 72 | func.segment(0, N).noalias() += tmp_; 73 | func.segment(0, N).noalias() -= dynBaseVel.S * baseModel_.getStateX(); 74 | 75 | func.segment(N, N).noalias() += tmp_; 76 | func.segment(N, N).noalias() -= dynBaseVel.S * baseModel_.getStateY(); 77 | 78 | func.segment(2*N, N).noalias() += tmp2_; 79 | func.segment(2*N, N).noalias() -= dynBaseAcc.S * baseModel_.getStateX(); 80 | 81 | func.segment(3*N, N).noalias() += tmp2_; 82 | func.segment(3*N, N).noalias() -= dynBaseAcc.S * baseModel_.getStateY(); 83 | 84 | } 85 | 86 | template 87 | const typename Type::MatrixX& BaseMotionConstraint::getGradient() 88 | { 89 | return gradient_; 90 | } 91 | 92 | template 93 | int BaseMotionConstraint::getNbConstraints() 94 | { 95 | return baseModel_.getNbSamples()*4; 96 | } 97 | 98 | template 99 | void BaseMotionConstraint::computeConstantPart() 100 | { 101 | const LinearDynamic& dynBaseVel = baseModel_.getBaseVelLinearDynamic(); 102 | const LinearDynamic& dynBaseAcc = baseModel_.getBaseAccLinearDynamic(); 103 | 104 | int N = baseModel_.getNbSamples(); 105 | int M = getNbConstraints(); 106 | 107 | gradient_.setZero(M, 2*N); 108 | gradient_.block(0, 0, N, N) = dynBaseVel.U; 109 | gradient_.block(N, N, N, N) = dynBaseVel.U; 110 | gradient_.block(2*N, 0, N, N) = dynBaseAcc.U; 111 | gradient_.block(3*N, N, N, N) = dynBaseAcc.U; 112 | 113 | tmp_.resize(N); 114 | tmp2_.resize(N); 115 | } 116 | 117 | namespace MPCWalkgen 118 | { 119 | MPC_WALKGEN_INSTANTIATE_CLASS_TEMPLATE(BaseMotionConstraint); 120 | } 121 | -------------------------------------------------------------------------------- /src/function/zebulon_base_position_tracking_objective.cpp: -------------------------------------------------------------------------------- 1 | //////////////////////////////////////////////////////////////////////////////// 2 | /// 3 | ///\author Lafaye Jory 4 | ///\author Barthelemy Sebastien 5 | /// 6 | //////////////////////////////////////////////////////////////////////////////// 7 | 8 | #include 9 | #include "../macro.h" 10 | 11 | using namespace MPCWalkgen; 12 | 13 | template 14 | BasePositionTrackingObjective 15 | ::BasePositionTrackingObjective(const BaseModel& baseModel) 16 | :baseModel_(baseModel) 17 | ,function_(1) 18 | ,tmp_(1) 19 | { 20 | posRefInWorldFrame_.setZero(2*baseModel_.getNbSamples()); 21 | 22 | function_.fill(0); 23 | gradient_.setZero(1, 1); 24 | hessian_.setZero(1, 1); 25 | 26 | computeConstantPart(); 27 | } 28 | 29 | 30 | template 31 | BasePositionTrackingObjective::~BasePositionTrackingObjective(){} 32 | 33 | template 34 | const typename 35 | Type::MatrixX& BasePositionTrackingObjective::getGradient(const VectorX& x0) 36 | { 37 | assert(posRefInWorldFrame_.size()==x0.size()); 38 | assert(posRefInWorldFrame_.size()==baseModel_.getNbSamples()*2); 39 | 40 | const LinearDynamic& dyn = baseModel_.getBasePosLinearDynamic(); 41 | 42 | int N = baseModel_.getNbSamples(); 43 | 44 | gradient_.noalias() = getHessian()*x0; 45 | 46 | 47 | tmp_.noalias() = dyn.S * baseModel_.getStateX(); 48 | tmp_.noalias() -= posRefInWorldFrame_.segment(0, N); 49 | gradient_.block(0, 0, N, 1).noalias() += dyn.UT*tmp_; 50 | 51 | tmp_.noalias() = dyn.S * baseModel_.getStateY(); 52 | tmp_.noalias()-= posRefInWorldFrame_.segment(N, N); 53 | gradient_.block(N, 0, N, 1).noalias() += dyn.UT*tmp_; 54 | return gradient_; 55 | } 56 | 57 | template 58 | const typename Type::MatrixX& BasePositionTrackingObjective::getHessian() 59 | { 60 | return hessian_; 61 | } 62 | 63 | template 64 | void BasePositionTrackingObjective::setPosRefInWorldFrame(const VectorX& posRefInWorldFrame) 65 | { 66 | assert(posRefInWorldFrame.size()==baseModel_.getNbSamples()*2); 67 | assert(posRefInWorldFrame==posRefInWorldFrame); 68 | 69 | posRefInWorldFrame_ = posRefInWorldFrame; 70 | } 71 | 72 | 73 | template 74 | void BasePositionTrackingObjective::computeConstantPart() 75 | { 76 | const LinearDynamic& dyn = baseModel_.getBasePosLinearDynamic(); 77 | 78 | int N = baseModel_.getNbSamples(); 79 | 80 | hessian_.resize(2*N, 2*N); 81 | hessian_.block(0, 0, N, N) = dyn.UT*dyn.U; 82 | hessian_.block(N, N, N, N) = dyn.UT*dyn.U; 83 | hessian_.block(0, N, N, N).fill(0.0); 84 | hessian_.block(N, 0, N, N).fill(0.0); 85 | 86 | tmp_.resize(N); 87 | } 88 | 89 | namespace MPCWalkgen 90 | { 91 | MPC_WALKGEN_INSTANTIATE_CLASS_TEMPLATE(BasePositionTrackingObjective); 92 | } 93 | -------------------------------------------------------------------------------- /src/function/zebulon_base_velocity_tracking_objective.cpp: -------------------------------------------------------------------------------- 1 | //////////////////////////////////////////////////////////////////////////////// 2 | /// 3 | ///\author Lafaye Jory 4 | ///\author Barthelemy Sebastien 5 | /// 6 | //////////////////////////////////////////////////////////////////////////////// 7 | 8 | #include 9 | #include "../macro.h" 10 | 11 | using namespace MPCWalkgen; 12 | 13 | template 14 | BaseVelocityTrackingObjective 15 | ::BaseVelocityTrackingObjective(const BaseModel& baseModel) 16 | :baseModel_(baseModel) 17 | ,function_(1) 18 | ,tmp_(1) 19 | { 20 | velRefInWorldFrame_.setZero(2*baseModel_.getNbSamples()); 21 | 22 | function_.fill(0); 23 | gradient_.setZero(1, 1); 24 | hessian_.setZero(1, 1); 25 | 26 | computeConstantPart(); 27 | } 28 | 29 | 30 | template 31 | BaseVelocityTrackingObjective::~BaseVelocityTrackingObjective(){} 32 | 33 | template 34 | const typename 35 | Type::MatrixX& BaseVelocityTrackingObjective::getGradient(const VectorX& x0) 36 | { 37 | assert(velRefInWorldFrame_.size()==x0.size()); 38 | assert(velRefInWorldFrame_.size()==baseModel_.getNbSamples()*2); 39 | 40 | const LinearDynamic& dyn = baseModel_.getBaseVelLinearDynamic(); 41 | 42 | int N = baseModel_.getNbSamples(); 43 | 44 | gradient_.noalias() = getHessian()*x0; 45 | 46 | 47 | tmp_.noalias() = dyn.S * baseModel_.getStateX(); 48 | tmp_.noalias() -= velRefInWorldFrame_.segment(0, N); 49 | gradient_.block(0, 0, N, 1).noalias() += dyn.UT*tmp_; 50 | 51 | tmp_.noalias() = dyn.S * baseModel_.getStateY(); 52 | tmp_.noalias()-= velRefInWorldFrame_.segment(N, N); 53 | gradient_.block(N, 0, N, 1).noalias() += dyn.UT*tmp_; 54 | 55 | 56 | return gradient_; 57 | } 58 | 59 | template 60 | const typename Type::MatrixX& BaseVelocityTrackingObjective::getHessian() 61 | { 62 | return hessian_; 63 | } 64 | 65 | template 66 | void BaseVelocityTrackingObjective::setVelRefInWorldFrame(const VectorX& velRefInWorldFrame) 67 | { 68 | assert(velRefInWorldFrame.size()==baseModel_.getNbSamples()*2); 69 | assert(velRefInWorldFrame==velRefInWorldFrame); 70 | 71 | velRefInWorldFrame_ = velRefInWorldFrame; 72 | } 73 | 74 | template 75 | void BaseVelocityTrackingObjective::computeConstantPart() 76 | { 77 | const LinearDynamic& dyn = baseModel_.getBaseVelLinearDynamic(); 78 | 79 | int N = baseModel_.getNbSamples(); 80 | 81 | hessian_.resize(2*N, 2*N); 82 | hessian_.block(0, 0, N, N) = dyn.UT*dyn.U; 83 | hessian_.block(N, N, N, N) = dyn.UT*dyn.U; 84 | hessian_.block(0, N, N, N).fill(0.0); 85 | hessian_.block(N, 0, N, N).fill(0.0); 86 | tmp_.resize(N); 87 | } 88 | 89 | namespace MPCWalkgen 90 | { 91 | MPC_WALKGEN_INSTANTIATE_CLASS_TEMPLATE(BaseVelocityTrackingObjective); 92 | } 93 | -------------------------------------------------------------------------------- /src/function/zebulon_com_constraint.cpp: -------------------------------------------------------------------------------- 1 | //////////////////////////////////////////////////////////////////////////////// 2 | /// 3 | ///\author Lafaye Jory 4 | ///\author Barthelemy Sebastien 5 | /// 6 | //////////////////////////////////////////////////////////////////////////////// 7 | 8 | #include 9 | #include "../macro.h" 10 | 11 | using namespace MPCWalkgen; 12 | 13 | template 14 | ComConstraint::ComConstraint(const LIPModel& lipModel, 15 | const BaseModel& baseModel) 16 | :lipModel_(lipModel) 17 | ,baseModel_(baseModel) 18 | ,function_(1) 19 | ,b_(1) 20 | ,tmp_(1) 21 | { 22 | assert(baseModel_.getNbSamples() == lipModel_.getNbSamples()); 23 | assert(baseModel_.getSamplingPeriod() == lipModel_.getSamplingPeriod()); 24 | 25 | function_.fill(0); 26 | gradient_.setZero(1, 1); 27 | hessian_.setZero(1, 1); 28 | A_.setZero(1, 1); 29 | b_.fill(0); 30 | 31 | computeConstantPart(); 32 | } 33 | 34 | 35 | template 36 | ComConstraint::~ComConstraint(){} 37 | 38 | template 39 | const typename Type::VectorX& ComConstraint::getFunction(const VectorX& x0) 40 | { 41 | assert(baseModel_.getNbSamples()*4==x0.size()); 42 | assert(baseModel_.getNbSamples() == lipModel_.getNbSamples()); 43 | assert(baseModel_.getSamplingPeriod() == lipModel_.getSamplingPeriod()); 44 | 45 | const LinearDynamic& dynBasePos = baseModel_.getBasePosLinearDynamic(); 46 | const LinearDynamic& dynCom = lipModel_.getComPosLinearDynamic(); 47 | 48 | int N = lipModel_.getNbSamples(); 49 | 50 | tmp_.segment(0, N).noalias() = dynCom.S * lipModel_.getStateX(); 51 | tmp_.segment(0, N).noalias() -= dynBasePos.S * baseModel_.getStateX(); 52 | tmp_.segment(N, N).noalias() = dynCom.S * lipModel_.getStateY() ; 53 | tmp_.segment(N, N).noalias() -= dynBasePos.S * baseModel_.getStateY(); 54 | 55 | function_.noalias() = -b_; 56 | function_.noalias() -= getGradient()*x0; 57 | function_.noalias() -= A_*tmp_; 58 | 59 | return function_; 60 | } 61 | 62 | template 63 | const typename Type::MatrixX& ComConstraint::getGradient() 64 | { 65 | assert(baseModel_.getNbSamples() == lipModel_.getNbSamples()); 66 | assert(baseModel_.getSamplingPeriod() == lipModel_.getSamplingPeriod()); 67 | 68 | return gradient_; 69 | } 70 | 71 | template 72 | int ComConstraint::getNbConstraints() 73 | { 74 | assert(baseModel_.getNbSamples() == lipModel_.getNbSamples()); 75 | 76 | return baseModel_.getNbSamples()*baseModel_.getComSupportConvexPolygon().getNbVertices(); 77 | } 78 | 79 | template 80 | void ComConstraint::computeConstantPart() 81 | { 82 | assert(baseModel_.getNbSamples() == lipModel_.getNbSamples()); 83 | assert(baseModel_.getSamplingPeriod() == lipModel_.getSamplingPeriod()); 84 | 85 | computeconstraintMatrices(); 86 | 87 | const LinearDynamic& dynBasePos = baseModel_.getBasePosLinearDynamic(); 88 | const LinearDynamic& dynCom = lipModel_.getComPosLinearDynamic(); 89 | 90 | int N = lipModel_.getNbSamples(); 91 | 92 | 93 | MatrixX tmp(2*N, 4*N); 94 | tmp.fill(0.0); 95 | tmp.block(0, 0, N, N) = dynCom.U; 96 | tmp.block(N, N, N, N) = dynCom.U; 97 | tmp.block(0, 2*N, N, N) = -dynBasePos.U; 98 | tmp.block(N, 3*N, N, N) = -dynBasePos.U; 99 | 100 | gradient_ = A_ * tmp; 101 | 102 | tmp_.resize(2*N); 103 | 104 | } 105 | 106 | template 107 | void ComConstraint::computeconstraintMatrices() 108 | { 109 | int N = lipModel_.getNbSamples(); 110 | const ConvexPolygon& supportConvexPolygon = baseModel_.getComSupportConvexPolygon(); 111 | int M = supportConvexPolygon.getNbVertices(); 112 | 113 | A_.setZero(M*N, 2*N); 114 | b_.resize(M*N); 115 | 116 | for(int i=0; i 9 | #include "../macro.h" 10 | 11 | using namespace MPCWalkgen; 12 | 13 | template 14 | JerkMinimizationObjective::JerkMinimizationObjective(const LIPModel& lipModel, 15 | const BaseModel& baseModel) 16 | :lipModel_(lipModel) 17 | ,baseModel_(baseModel) 18 | ,function_(1) 19 | { 20 | assert(baseModel_.getNbSamples() == lipModel_.getNbSamples()); 21 | assert(baseModel_.getSamplingPeriod() == lipModel_.getSamplingPeriod()); 22 | 23 | function_.fill(0); 24 | gradient_.setZero(1, 1); 25 | hessian_.setZero(1, 1); 26 | 27 | computeConstantPart(); 28 | } 29 | 30 | 31 | template 32 | JerkMinimizationObjective::~JerkMinimizationObjective(){} 33 | 34 | template 35 | const typename 36 | Type::VectorX& JerkMinimizationObjective::getGradient(const VectorX& x0) 37 | { 38 | assert(baseModel_.getNbSamples()*4==x0.size()); 39 | assert(baseModel_.getNbSamples() == lipModel_.getNbSamples()); 40 | 41 | return x0; 42 | } 43 | 44 | template 45 | const typename Type::MatrixX& JerkMinimizationObjective::getHessian() 46 | { 47 | assert(baseModel_.getNbSamples() == lipModel_.getNbSamples()); 48 | assert(baseModel_.getSamplingPeriod() == lipModel_.getSamplingPeriod()); 49 | 50 | return hessian_; 51 | } 52 | 53 | template 54 | void JerkMinimizationObjective::computeConstantPart() 55 | { 56 | assert(baseModel_.getNbSamples() == lipModel_.getNbSamples()); 57 | assert(baseModel_.getSamplingPeriod() == lipModel_.getSamplingPeriod()); 58 | 59 | int N = baseModel_.getNbSamples(); 60 | hessian_ = MatrixX::Identity(N*4, N*4); 61 | } 62 | 63 | namespace MPCWalkgen 64 | { 65 | MPC_WALKGEN_INSTANTIATE_CLASS_TEMPLATE(JerkMinimizationObjective); 66 | } 67 | -------------------------------------------------------------------------------- /src/function/zebulon_tilt_motion_constraint.cpp: -------------------------------------------------------------------------------- 1 | //////////////////////////////////////////////////////////////////////////////// 2 | /// 3 | ///\author Lafaye Jory 4 | ///\author Barthelemy Sebastien 5 | /// 6 | //////////////////////////////////////////////////////////////////////////////// 7 | 8 | #include 9 | #include "../macro.h" 10 | 11 | using namespace MPCWalkgen; 12 | 13 | template 14 | TiltMotionConstraint::TiltMotionConstraint(const LIPModel& lipModel, 15 | const BaseModel& baseModel) 16 | :lipModel_(lipModel) 17 | ,baseModel_(baseModel) 18 | ,function_(1) 19 | { 20 | assert(baseModel_.getNbSamples() == lipModel_.getNbSamples()); 21 | assert(baseModel_.getSamplingPeriod() == lipModel_.getSamplingPeriod()); 22 | 23 | function_.fill(0); 24 | gradient_.setZero(1, 1); 25 | hessian_.setZero(1, 1); 26 | 27 | computeConstantPart(); 28 | } 29 | 30 | 31 | template 32 | TiltMotionConstraint::~TiltMotionConstraint(){} 33 | 34 | template 35 | const typename Type::VectorX& TiltMotionConstraint::getFunction(const VectorX& x0) 36 | { 37 | assert(baseModel_.getNbSamples()*4==x0.size()); 38 | assert(baseModel_.getNbSamples() == lipModel_.getNbSamples()); 39 | assert(baseModel_.getSamplingPeriod() == lipModel_.getSamplingPeriod()); 40 | 41 | 42 | int N = baseModel_.getNbSamples(); 43 | Scalar theta = baseModel_.getStateYaw()(0); 44 | 45 | const LinearDynamic& dynBaseVel = baseModel_.getBaseVelLinearDynamic(); 46 | const LinearDynamic& dynComVel = lipModel_.getComVelLinearDynamic(); 47 | 48 | 49 | function_.noalias() = -getGradient()*x0; 50 | function_.segment(0, N).noalias() -= dynComVel.S * lipModel_.getStateX()*std::sin(theta); 51 | function_.segment(0, N).noalias() += dynComVel.S * lipModel_.getStateY()*std::cos(theta); 52 | function_.segment(N, N).noalias() -= dynBaseVel.S * baseModel_.getStateX()*std::sin(theta); 53 | function_.segment(N, N).noalias() += dynBaseVel.S * baseModel_.getStateY()*std::cos(theta); 54 | 55 | 56 | return function_; 57 | } 58 | 59 | 60 | template 61 | const typename Type::MatrixX& TiltMotionConstraint::getGradient() 62 | { 63 | assert(baseModel_.getNbSamples() == lipModel_.getNbSamples()); 64 | assert(baseModel_.getSamplingPeriod() == lipModel_.getSamplingPeriod()); 65 | 66 | return gradient_; 67 | } 68 | 69 | template 70 | int TiltMotionConstraint::getNbConstraints() 71 | { 72 | return baseModel_.getNbSamples()*2; 73 | } 74 | 75 | template 76 | void TiltMotionConstraint::computeConstantPart() 77 | { 78 | assert(baseModel_.getNbSamples() == lipModel_.getNbSamples()); 79 | assert(baseModel_.getSamplingPeriod() == lipModel_.getSamplingPeriod()); 80 | 81 | const LinearDynamic& dynBaseVel = baseModel_.getBaseVelLinearDynamic(); 82 | const LinearDynamic& dynComVel = lipModel_.getComVelLinearDynamic(); 83 | 84 | int N = baseModel_.getNbSamples(); 85 | int M = getNbConstraints(); 86 | Scalar theta = baseModel_.getStateYaw()(0); 87 | 88 | gradient_.setZero(M, 4*N); 89 | gradient_.block(0, 0, N, N) = dynComVel.U*std::sin(theta); 90 | gradient_.block(0, N, N, N) = -dynComVel.U*std::cos(theta); 91 | gradient_.block(N, 2*N, N, N) = dynBaseVel.U*std::sin(theta); 92 | gradient_.block(N, 3*N, N, N) = -dynBaseVel.U*std::cos(theta); 93 | } 94 | 95 | namespace MPCWalkgen 96 | { 97 | MPC_WALKGEN_INSTANTIATE_CLASS_TEMPLATE(TiltMotionConstraint); 98 | } 99 | -------------------------------------------------------------------------------- /src/interpolator.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include "macro.h" 3 | 4 | namespace MPCWalkgen 5 | { 6 | ///Interpolator 7 | template 8 | Interpolator::Interpolator() 9 | :aInvNorm_(9,9) 10 | ,b_(9) 11 | ,abc_(9) 12 | { 13 | // temporary to avoid warning about double to float lossy conversion. 14 | Eigen::MatrixAinvNorm; 15 | AinvNorm << 9./2. , 3./2., 1./6. , 9./2. , 0. , -1./12., 9./2. , -3./2., 1./6. , 16 | -9. ,-3./2., 5./12., -9. , 3./2., 5./12., -9. , 9./2., -7./12. , 17 | 27./2., 3. , -3./4. , 27./2., -3./2., -1./2. , 27./2., -6. , 3./4. , 18 | -9./2. ,-2. , 5./12., -9./2. , 1./2., 1./6. , -9./2. , 2. , -1./4. , 19 | -1./2. , 4./9., -7./108., 1./2. , -1./18.,-1./54., 1./2. , -2./9., 1./36. , 20 | 9./2. , 0. , -1./12., 9./2. , -3./2., 1./6. , 9./2. , -3. , 11./12., 21 | -27./2., 0. , 1./4. , -27./2., 9./2., -1./2. , -27./2., 9. , -9./4. , 22 | 27./2., 0. , -1./4. , 27./2., -9./2., 1./2. , 27./2., -8. , 7./4. , 23 | -9./2. , 0. , 1./12., -9./2. , 3./2., -1./6. , -7./2. , 2. , -5./12. ; 24 | aInvNorm_ = AinvNorm.cast(); 25 | } 26 | 27 | template 28 | Interpolator::~Interpolator(){} 29 | 30 | template 31 | void Interpolator::computePolynomialNormalisedFactors( 32 | VectorX &factor, 33 | const Vector3 &initialstate, 34 | const Vector3 &finalState, 35 | Scalar T ) const 36 | { 37 | factor(3) = initialstate(0); 38 | factor(2) = T*initialstate(1); 39 | factor(1) = T*T*initialstate(2)/2; 40 | 41 | b_(0) = - T*T*initialstate(2)/18 - T*initialstate(1)/3 - initialstate(0); 42 | b_(1) = - T*T*initialstate(2)/3 - T*initialstate(1); 43 | b_(2) = - T*T*initialstate(2); 44 | b_(3) = 0; 45 | b_(4) = 0; 46 | b_(5) = 0; 47 | b_(6) = finalState(0); 48 | b_(7) = T*finalState(1); 49 | b_(8) = T*T*finalState(2); 50 | 51 | abc_=aInvNorm_*b_; 52 | 53 | factor(0) = abc_(0); 54 | factor.template segment<8>(4) = abc_.template segment<8>(1); 55 | } 56 | 57 | template 58 | void Interpolator::selectFactors(Vector4 &subfactor, 59 | const VectorX &factor, 60 | Scalar t, 61 | Scalar T) const 62 | { 63 | if (t<=T/3.0) 64 | { 65 | subfactor = factor.template segment<4>(0); 66 | } 67 | else if(t<=2.0*T/3.0) 68 | { 69 | subfactor = factor.template segment<4>(4); 70 | } 71 | else 72 | { 73 | subfactor = factor.template segment<4>(8); 74 | } 75 | } 76 | 77 | MPC_WALKGEN_INSTANTIATE_CLASS_TEMPLATE(Interpolator); 78 | } 79 | -------------------------------------------------------------------------------- /src/lineardynamic.cpp: -------------------------------------------------------------------------------- 1 | //////////////////////////////////////////////////////////////////////////////// 2 | /// 3 | ///\author Lafaye Jory 4 | ///\author Barthelemy Sebastien 5 | /// 6 | //////////////////////////////////////////////////////////////////////////////// 7 | 8 | #include 9 | #include "macro.h" 10 | 11 | namespace MPCWalkgen 12 | { 13 | template 14 | void LinearDynamic::reset(int nbSamples, 15 | int stateVectorSize, 16 | int variableVectorSize) 17 | { 18 | U.setZero(nbSamples, variableVectorSize); 19 | UT.setZero(variableVectorSize, nbSamples); 20 | 21 | if(nbSamples==variableVectorSize) 22 | { 23 | Uinv.setZero(nbSamples, variableVectorSize); 24 | UTinv.setZero(variableVectorSize, nbSamples); 25 | } 26 | else 27 | { 28 | Uinv.setConstant(nbSamples, 29 | variableVectorSize, 30 | std::numeric_limits::quiet_NaN()); 31 | UTinv.setConstant(variableVectorSize, 32 | nbSamples, 33 | std::numeric_limits::quiet_NaN()); 34 | } 35 | 36 | S.setZero(nbSamples, stateVectorSize); 37 | 38 | K.setZero(nbSamples); 39 | } 40 | 41 | MPC_WALKGEN_INSTANTIATE_CLASS_TEMPLATE(LinearDynamic); 42 | } 43 | -------------------------------------------------------------------------------- /src/macro.h: -------------------------------------------------------------------------------- 1 | //////////////////////////////////////////////////////////////////////////////// 2 | /// 3 | ///\file macro.h 4 | ///\brief macro to explicitly instantiate template 5 | ///\author Barthelemy Sebastien 6 | /// 7 | //////////////////////////////////////////////////////////////////////////////// 8 | 9 | #ifndef MPC_WALKGEN_MACRO_H 10 | #define MPC_WALKGEN_MACRO_H 11 | 12 | #define MPC_WALKGEN_INSTANTIATE_CLASS_TEMPLATE(className) \ 13 | template class MPC_WALKGEN_API className;\ 14 | template class MPC_WALKGEN_API className 15 | 16 | #endif 17 | -------------------------------------------------------------------------------- /src/model/humanoid_foot_model.cpp: -------------------------------------------------------------------------------- 1 | //////////////////////////////////////////////////////////////////////////////// 2 | /// 3 | ///\file humanoid_foot_model.cpp 4 | ///\brief Implement of a foot model 5 | ///\author de Gourcuff Martin 6 | ///\author Barthelemy Sebastien 7 | /// 8 | //////////////////////////////////////////////////////////////////////////////// 9 | #include 10 | #include "../macro.h" 11 | 12 | namespace MPCWalkgen 13 | { 14 | template 15 | HumanoidFootModel::KinematicLimits::KinematicLimits() 16 | :hipYawUpperBound_(0) 17 | ,hipYawSpeedUpperBound_(0) 18 | ,hipYawAccelerationUpperBound_(0) 19 | ,hipYawLowerBound_(0) 20 | ,maxHeight_(0) 21 | ,kinematicConvexPolygon_() 22 | ,kinematicConvexPolygonInWorldFrame_() 23 | {} 24 | 25 | template 26 | HumanoidFootModel::HumanoidFootModel(int nbSamples, 27 | Scalar samplingPeriod) 28 | :nbSamples_(nbSamples) 29 | ,samplingPeriod_(samplingPeriod) 30 | ,kinematicLimits_() 31 | ,CopConvexPolygon_() 32 | ,CopConvexPolygonInWorldFrame_() 33 | ,interpolator_() 34 | { 35 | assert(samplingPeriod>0); 36 | assert(nbSamples>0); 37 | 38 | init(); 39 | } 40 | 41 | template 42 | HumanoidFootModel::HumanoidFootModel() 43 | :nbSamples_(1) 44 | ,samplingPeriod_(1.0) 45 | ,kinematicLimits_() 46 | ,CopConvexPolygon_() 47 | ,CopConvexPolygonInWorldFrame_() 48 | { 49 | init(); 50 | } 51 | 52 | template 53 | HumanoidFootModel::~HumanoidFootModel(){} 54 | 55 | template 56 | void HumanoidFootModel::setNbSamples(int nbSamples) 57 | { 58 | assert(nbSamples>0); 59 | 60 | nbSamples_ = nbSamples; 61 | } 62 | 63 | template 64 | void HumanoidFootModel::setSamplingPeriod(Scalar samplingPeriod) 65 | { 66 | assert(samplingPeriod>0); 67 | 68 | samplingPeriod_ = samplingPeriod; 69 | } 70 | 71 | template 72 | void HumanoidFootModel::updateStateX(const Vector3& obj, 73 | Scalar T, 74 | Scalar t) 75 | { 76 | interpolateTrajectory(stateX_, obj, T, t); 77 | } 78 | 79 | template 80 | void HumanoidFootModel::updateStateY(const Vector3& obj, 81 | Scalar T, 82 | Scalar t) 83 | { 84 | interpolateTrajectory(stateY_, obj, T, t); 85 | } 86 | 87 | template 88 | void HumanoidFootModel::updateStateZ(const Vector3& obj, 89 | Scalar T, 90 | Scalar t) 91 | { 92 | interpolateTrajectory(stateZ_, obj, T, t); 93 | } 94 | 95 | template 96 | void HumanoidFootModel::setHipYawUpperBound( 97 | Scalar hipYawUpperBound) 98 | {kinematicLimits_.hipYawUpperBound_ = hipYawUpperBound;} 99 | 100 | template 101 | void HumanoidFootModel::setHipYawSpeedUpperBound( 102 | Scalar hipYawSpeedUpperBound) 103 | {kinematicLimits_.hipYawSpeedUpperBound_ = hipYawSpeedUpperBound;} 104 | 105 | template 106 | void HumanoidFootModel::setHipYawAccelerationUpperBound( 107 | Scalar hipYawAccelerationUpperBound) 108 | {kinematicLimits_.hipYawAccelerationUpperBound_ = hipYawAccelerationUpperBound;} 109 | 110 | template 111 | void HumanoidFootModel::setHipYawLowerBound( 112 | Scalar hipYawLowerBound) 113 | {kinematicLimits_.hipYawLowerBound_ = hipYawLowerBound;} 114 | 115 | template 116 | void HumanoidFootModel::setMaxHeight( 117 | Scalar maxHeight) 118 | {kinematicLimits_.maxHeight_ = maxHeight;} 119 | 120 | template 121 | void HumanoidFootModel::setKinematicConvexPolygon( 122 | const ConvexPolygon& kinematicConvexPolygon) 123 | { 124 | kinematicLimits_.kinematicConvexPolygon_ = kinematicConvexPolygon; 125 | } 126 | 127 | template 128 | void HumanoidFootModel::init() 129 | { 130 | stateX_.setZero(3); 131 | stateY_.setZero(3); 132 | stateZ_.setZero(3); 133 | stateYaw_.setZero(3); 134 | 135 | isInContact_.resize(nbSamples_, true); 136 | 137 | int nbOfPolynomCoefficient = 4; 138 | factor_.setZero(3*nbOfPolynomCoefficient); 139 | subFactor_.setZero(nbOfPolynomCoefficient); 140 | } 141 | 142 | template 143 | void HumanoidFootModel::interpolateTrajectory(VectorX& currentState, 144 | const Vector3& objState, 145 | Scalar T, 146 | Scalar t) 147 | { 148 | interpolator_.computePolynomialNormalisedFactors(factor_, 149 | currentState, 150 | objState, 151 | T); 152 | 153 | interpolator_.selectFactors(subFactor_, factor_, t, T); 154 | 155 | // Interpolated values of the state, its derivative and second derivative 156 | // are computed here. 157 | // Division by T is a consequence of the polynoms normalization 158 | currentState(0) = Tools::polynomValue(subFactor_, t/T); 159 | currentState(1) = Tools::dPolynomValue(subFactor_, t/T)/T; 160 | currentState(2) = Tools::ddPolynomValue(subFactor_, t/T)/(T*T); 161 | } 162 | 163 | MPC_WALKGEN_INSTANTIATE_CLASS_TEMPLATE(HumanoidFootModel); 164 | } 165 | -------------------------------------------------------------------------------- /src/model/no_dynamic_model.cpp: -------------------------------------------------------------------------------- 1 | //////////////////////////////////////////////////////////////////////////////// 2 | /// 3 | ///\author Lafaye Jory 4 | /// 5 | //////////////////////////////////////////////////////////////////////////////// 6 | 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include "../macro.h" 12 | 13 | using namespace MPCWalkgen; 14 | 15 | template 16 | NoDynamicModel::NoDynamicModel(int nbSamples, 17 | Scalar samplingPeriod, 18 | bool autoCompute) 19 | :autoCompute_(autoCompute) 20 | ,nbSamples_(nbSamples) 21 | ,samplingPeriod_(samplingPeriod) 22 | ,velocityLimit_(1.0) 23 | ,accelerationLimit_(1.0) 24 | ,jerkLimit_(1.0) 25 | { 26 | assert(samplingPeriod>0); 27 | assert(nbSamples>0); 28 | 29 | state_.setZero(3); 30 | if (autoCompute_) 31 | { 32 | computeDynamics(); 33 | } 34 | } 35 | 36 | template 37 | NoDynamicModel::NoDynamicModel() 38 | :autoCompute_(true) 39 | ,nbSamples_(1) 40 | ,samplingPeriod_(1.0) 41 | ,velocityLimit_(1.0) 42 | ,accelerationLimit_(1.0) 43 | ,jerkLimit_(1.0) 44 | { 45 | state_.setZero(3); 46 | if (autoCompute_) 47 | { 48 | computeDynamics(); 49 | } 50 | } 51 | 52 | template 53 | NoDynamicModel::~NoDynamicModel(){} 54 | 55 | template 56 | void NoDynamicModel::computeDynamics() 57 | { 58 | computePosDynamic(); 59 | computeVelDynamic(); 60 | computeAccDynamic(); 61 | computeJerkDynamic(); 62 | } 63 | 64 | template 65 | void NoDynamicModel::computePosDynamic() 66 | { 67 | Tools::ConstantJerkDynamic::computePosDynamic(samplingPeriod_, samplingPeriod_, 68 | nbSamples_, posDynamic_); 69 | 70 | } 71 | 72 | template 73 | void NoDynamicModel::computeVelDynamic() 74 | { 75 | Tools::ConstantJerkDynamic::computeVelDynamic(samplingPeriod_, samplingPeriod_, 76 | nbSamples_, velDynamic_); 77 | } 78 | 79 | template 80 | void NoDynamicModel::computeAccDynamic() 81 | { 82 | Tools::ConstantJerkDynamic::computeAccDynamic(samplingPeriod_, samplingPeriod_, 83 | nbSamples_, accDynamic_); 84 | } 85 | 86 | template 87 | void NoDynamicModel::computeJerkDynamic() 88 | { 89 | Tools::ConstantJerkDynamic::computeJerkDynamic(nbSamples_, jerkDynamic_); 90 | } 91 | 92 | template 93 | void NoDynamicModel::setNbSamples(int nbSamples) 94 | { 95 | assert(nbSamples>0); 96 | 97 | nbSamples_ = nbSamples; 98 | 99 | if (autoCompute_) 100 | { 101 | computeDynamics(); 102 | } 103 | } 104 | 105 | template 106 | void NoDynamicModel::updateState(Scalar jerk, Scalar feedBackPeriod) 107 | { 108 | Tools::ConstantJerkDynamic::updateState(jerk, feedBackPeriod, state_); 109 | } 110 | 111 | template 112 | void NoDynamicModel::setSamplingPeriod(Scalar samplingPeriod) 113 | { 114 | assert(samplingPeriod>0); 115 | 116 | samplingPeriod_ = samplingPeriod; 117 | 118 | if (autoCompute_) 119 | { 120 | computeDynamics(); 121 | } 122 | } 123 | 124 | namespace MPCWalkgen 125 | { 126 | MPC_WALKGEN_INSTANTIATE_CLASS_TEMPLATE(NoDynamicModel); 127 | } 128 | -------------------------------------------------------------------------------- /src/qpsolverfactory.cpp: -------------------------------------------------------------------------------- 1 | //////////////////////////////////////////////////////////////////////////////// 2 | /// 3 | ///\author Lafaye Jory 4 | ///\author Barthelemy Sebastien 5 | /// 6 | //////////////////////////////////////////////////////////////////////////////// 7 | 8 | #include 9 | #include 10 | #include 11 | 12 | namespace MPCWalkgen 13 | { 14 | template <> 15 | QPSolver *makeQPSolver(int nbVar, int nbCtr) 16 | {return makeQPSolverDouble(nbVar, nbCtr);} 17 | 18 | template <> 19 | QPSolver *makeQPSolver(int nbVar, int nbCtr) 20 | {return makeQPSolverFloat(nbVar, nbCtr);} 21 | } 22 | -------------------------------------------------------------------------------- /test/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # common stuff 2 | qi_create_gtest(test-qpoases-solver 3 | SRC ./test-qpoases-solver.cpp 4 | DEPENDS mpc-walkgen 5 | qpOASESfloat 6 | boost 7 | TIMEOUT 1 8 | ) 9 | 10 | qi_create_gtest(test-convex-polygon-function 11 | SRC ./test-convex-polygon-function.cpp 12 | DEPENDS mpc-walkgen 13 | TIMEOUT 1 14 | ) 15 | 16 | qi_create_gtest(test-interpolation-function 17 | SRC ./test-interpolation-function.cpp 18 | DEPENDS mpc-walkgen 19 | TIMEOUT 1 20 | ) 21 | 22 | qi_create_gtest(test-lip-model 23 | SRC ./test-lip-model.cpp 24 | DEPENDS mpc-walkgen 25 | TIMEOUT 1 26 | ) 27 | 28 | # zebulon stuff 29 | qi_create_gtest(test-zebulon-base-model 30 | SRC ./test-zebulon-base-model.cpp 31 | DEPENDS mpc-walkgen 32 | TIMEOUT 1 33 | ) 34 | 35 | qi_create_gtest(test-zebulon-base-velocity-tracking-objective 36 | SRC ./test-zebulon-base-velocity-tracking-objective.cpp 37 | DEPENDS mpc-walkgen 38 | TIMEOUT 1 39 | ) 40 | 41 | qi_create_gtest(test-zebulon-base-position-tracking-objective 42 | SRC ./test-zebulon-base-position-tracking-objective.cpp 43 | DEPENDS mpc-walkgen 44 | TIMEOUT 1 45 | ) 46 | 47 | qi_create_gtest(test-zebulon-jerk-minimization-objective 48 | SRC ./test-zebulon-jerk-minimization-objective.cpp 49 | DEPENDS mpc-walkgen 50 | TIMEOUT 1 51 | ) 52 | 53 | qi_create_gtest(test-zebulon-cop-centering-objective 54 | SRC ./test-zebulon-cop-centering-objective.cpp 55 | DEPENDS mpc-walkgen 56 | TIMEOUT 1 57 | ) 58 | 59 | qi_create_gtest(test-zebulon-cop-constraint 60 | SRC ./test-zebulon-cop-constraint.cpp 61 | DEPENDS mpc-walkgen 62 | TIMEOUT 1 63 | ) 64 | 65 | qi_create_gtest(test-zebulon-com-constraint 66 | SRC ./test-zebulon-com-constraint.cpp 67 | DEPENDS mpc-walkgen 68 | TIMEOUT 1 69 | ) 70 | 71 | qi_create_gtest(test-zebulon-base-motion-constraint 72 | SRC ./test-zebulon-base-motion-constraint.cpp 73 | DEPENDS mpc-walkgen 74 | TIMEOUT 1 75 | ) 76 | 77 | qi_create_gtest(test-zebulon-tilt-motion-constraint 78 | SRC ./test-zebulon-tilt-motion-constraint.cpp 79 | DEPENDS mpc-walkgen 80 | TIMEOUT 1 81 | ) 82 | 83 | qi_create_bin(zebulon-walkgen-bin 84 | SRC ./zebulon-walkgen-bin.cpp 85 | DEPENDS mpc-walkgen 86 | NO_INSTALL 87 | ) 88 | 89 | qi_create_bin(trajectory-walkgen-bin 90 | SRC ./trajectory-walkgen-bin.cpp 91 | DEPENDS mpc-walkgen 92 | NO_INSTALL 93 | ) 94 | 95 | qi_create_bin(humanoid-walkgen-bin 96 | SRC ./humanoid-walkgen-bin.cpp 97 | DEPENDS mpc-walkgen 98 | NO_INSTALL 99 | ) 100 | # humanoid stuff 101 | # mostly smoke tests that only help ensure the templates keep building 102 | qi_create_gtest(test-humanoid-foot-model 103 | SRC ./test-humanoid-foot-model.cpp 104 | DEPENDS mpc-walkgen 105 | TIMEOUT 1 106 | ) 107 | 108 | qi_create_gtest(test-humanoid-feet-supervisor 109 | SRC ./test-humanoid-feet-supervisor.cpp 110 | DEPENDS mpc-walkgen 111 | TIMEOUT 1 112 | ) 113 | 114 | qi_create_gtest(test-humanoid-lip-com-velocity-tracking-objective 115 | SRC ./test-humanoid-lip-com-velocity-tracking-objective.cpp 116 | DEPENDS mpc-walkgen 117 | TIMEOUT 1 118 | ) 119 | 120 | qi_create_gtest(test-humanoid-lip-com-jerk-minimization-objective 121 | SRC ./test-humanoid-lip-com-jerk-minimization-objective.cpp 122 | DEPENDS mpc-walkgen 123 | TIMEOUT 1 124 | ) 125 | 126 | qi_create_gtest(test-humanoid-cop-centering-objective 127 | SRC ./test-humanoid-cop-centering-objective.cpp 128 | DEPENDS mpc-walkgen 129 | TIMEOUT 1 130 | ) 131 | 132 | qi_create_gtest(test-humanoid-foot-constraint 133 | SRC ./test-humanoid-foot-constraint.cpp 134 | DEPENDS mpc-walkgen 135 | TIMEOUT 1 136 | ) 137 | 138 | qi_create_gtest(test-humanoid-cop-constraint 139 | SRC ./test-humanoid-cop-constraint.cpp 140 | DEPENDS mpc-walkgen 141 | TIMEOUT 1 142 | ) 143 | -------------------------------------------------------------------------------- /test/humanoid-walkgen-bin.cpp: -------------------------------------------------------------------------------- 1 | //////////////////////////////////////////////////////////////////////////////// 2 | /// 3 | ///\file humanoid_walkgen.h 4 | ///\brief Main program for Humanoid 5 | ///\author Lafaye Jory 6 | /// 7 | //////////////////////////////////////////////////////////////////////////////// 8 | 9 | #include 10 | 11 | using namespace MPCWalkgen; 12 | typedef double Real; 13 | 14 | 15 | int main(void) 16 | { 17 | HumanoidWalkgen walkgen; 18 | 19 | walkgen.solve(0.02); 20 | 21 | return 0; 22 | } 23 | -------------------------------------------------------------------------------- /test/mpc_walkgen_gtest.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #ifndef MPC_WALKGEN_GTEST_H 3 | #define MPC_WALKGEN_GTEST_H 4 | 5 | #include 6 | #include 7 | #include 8 | #include // often used in tests 9 | 10 | template 11 | class MpcWalkgenTest: public ::testing::Test 12 | { 13 | }; 14 | typedef ::testing::Types MyTypes; 15 | TYPED_TEST_CASE(MpcWalkgenTest, MyTypes); 16 | 17 | template 18 | void checkLinearDynamicSize(const MPCWalkgen::LinearDynamic& dyn, 19 | int nbSamples) 20 | { 21 | ASSERT_EQ(dyn.U.rows(), nbSamples); 22 | ASSERT_EQ(dyn.U.cols(), nbSamples); 23 | 24 | ASSERT_EQ(dyn.UT.rows(), nbSamples); 25 | ASSERT_EQ(dyn.UT.cols(), nbSamples); 26 | 27 | ASSERT_EQ(dyn.S.rows(), nbSamples); 28 | ASSERT_EQ(dyn.S.cols(), 3); 29 | } 30 | 31 | #endif 32 | -------------------------------------------------------------------------------- /test/test-convex-polygon-function.cpp: -------------------------------------------------------------------------------- 1 | //////////////////////////////////////////////////////////////////////////////// 2 | /// 3 | ///\file test-convex-ConvexPolygon-function.cpp 4 | ///\brief Test of the convex polygons static functions defined in ConvexPolygon 5 | /// class 6 | ///\author de Gourcuff Martin 7 | ///\author Barthelemy Sebastien 8 | /// 9 | //////////////////////////////////////////////////////////////////////////////// 10 | 11 | #include "mpc_walkgen_gtest.h" 12 | #include 13 | #include 14 | 15 | using namespace MPCWalkgen; 16 | 17 | template 18 | void createStandardConvexSet(typename Type::vectorOfVector2& p) 19 | { 20 | using namespace MPCWalkgen; 21 | typedef typename Type::Vector2 Vector2; 22 | p.resize(5); 23 | 24 | p[0] = Vector2(1.0, 0.0); 25 | p[1] = Vector2(-1.0, -1.0); 26 | p[2] = Vector2(1.0, -1.0); 27 | p[3] = Vector2(1.0, 1.0); 28 | p[4] = Vector2(-1.0, 1.0); 29 | } 30 | 31 | TYPED_TEST(MpcWalkgenTest, angleBetweenVecs) 32 | { 33 | using namespace MPCWalkgen; 34 | TEMPLATE_TYPEDEF(TypeParam); 35 | 36 | vectorOfVector2 p(5); 37 | createStandardConvexSet(p); 38 | 39 | ASSERT_NEAR(ConvexPolygon::angleBetweenVecs(p[0], p[1]), 40 | -2.3561944961547852, Constant::EPSILON); 41 | ASSERT_NEAR(ConvexPolygon::angleBetweenVecs(p[0], p[2]), 42 | -0.78539818525314331, Constant::EPSILON); 43 | ASSERT_NEAR(ConvexPolygon::angleBetweenVecs(p[0], p[3]), 44 | 0.78539818525314331, Constant::EPSILON); 45 | ASSERT_NEAR(ConvexPolygon::angleBetweenVecs(p[0], p[4]), 46 | 2.3561944961547852, Constant::EPSILON); 47 | ASSERT_NEAR(ConvexPolygon::angleBetweenVecs(p[2], 48 | Vector2(0.0, 0.0)), 49 | 0.0, Constant::EPSILON); 50 | ASSERT_NEAR(ConvexPolygon::angleBetweenVecs(p[0], p[0]), 51 | 0.0, Constant::EPSILON); 52 | ASSERT_NEAR(ConvexPolygon::angleBetweenVecs(p[0], 53 | Vector2(2.0, 0.0)), 54 | 0.0, Constant::EPSILON); 55 | } 56 | 57 | TYPED_TEST(MpcWalkgenTest, geLowestAndLeftmostPointsIndex) 58 | { 59 | using namespace MPCWalkgen; 60 | TEMPLATE_TYPEDEF(TypeParam); 61 | 62 | vectorOfVector2 p(5); 63 | createStandardConvexSet(p); 64 | 65 | ASSERT_EQ(1, ConvexPolygon::getIndexOfLowestAndLeftmostVertice(p)); 66 | 67 | p[2] = p[1]; 68 | 69 | ASSERT_EQ(1, ConvexPolygon::getIndexOfLowestAndLeftmostVertice(p)); 70 | } 71 | 72 | // disabled because it fails on 32bit archs when Scalar == double 73 | TYPED_TEST(MpcWalkgenTest, DISABLED_getIndexOfSmallestAngleVertice) 74 | { 75 | using namespace MPCWalkgen; 76 | TEMPLATE_TYPEDEF(TypeParam); 77 | vectorOfVector2 p(5); 78 | createStandardConvexSet(p); 79 | 80 | ASSERT_EQ(3, ConvexPolygon::getIndexOfSmallestAngleVertice(2, p[1], p)); 81 | 82 | p[4] = p[3]; 83 | 84 | ASSERT_EQ(3, ConvexPolygon::getIndexOfSmallestAngleVertice(2, p[1], p)); 85 | } 86 | 87 | // disabled because it fails on 32bit archs when Scalar == double 88 | TYPED_TEST(MpcWalkgenTest, DISABLED_getConvexPolygon) 89 | { 90 | using namespace MPCWalkgen; 91 | TEMPLATE_TYPEDEF(TypeParam); 92 | 93 | vectorOfVector2 p1(9); 94 | p1[0] = Vector2(-4.0, 0.0); 95 | p1[1] = Vector2(-2.0, 2.0); 96 | p1[2] = Vector2(-2.0, -1.5); 97 | p1[3] = Vector2(0.0, 2.0); 98 | p1[4] = Vector2(3.0, 0.0); 99 | p1[5] = Vector2(2.0, 2.0); 100 | p1[6] = Vector2(2.0, -1.0); 101 | p1[7] = Vector2(1.0, 1.0); 102 | p1[8] = Vector2(1.0, 0.0); 103 | 104 | vectorOfVector2 convexSet 105 | = ConvexPolygon::extractVertices(p1); 106 | ASSERT_EQ(6u, convexSet.size()); 107 | ASSERT_TRUE(convexSet[0].isApprox(p1[2])); 108 | ASSERT_TRUE(convexSet[1].isApprox(p1[6])); 109 | ASSERT_TRUE(convexSet[2].isApprox(p1[4])); 110 | ASSERT_TRUE(convexSet[3].isApprox(p1[5])); 111 | ASSERT_TRUE(convexSet[4].isApprox(p1[1])); 112 | ASSERT_TRUE(convexSet[5].isApprox(p1[0])); 113 | 114 | p1[0] = Vector2(1.0, 0.0); 115 | p1[1] = Vector2(-1.0, 1.0); 116 | p1[2] = Vector2(-1.0, 1.0); 117 | p1[3] = Vector2(0.0, 0.0); 118 | p1[4] = Vector2(1.0, 1.0); 119 | p1[5] = Vector2(1.0, -1.0); 120 | p1[6] = Vector2(0.0, 1.0); 121 | p1[7] = Vector2(1.0, -1.0); 122 | p1[8] = Vector2(1.0, 1.0); 123 | 124 | convexSet = ConvexPolygon::extractVertices(p1); 125 | ASSERT_EQ(convexSet.size(), static_cast(3)); 126 | ASSERT_TRUE(convexSet[0].isApprox(p1[5])); 127 | ASSERT_TRUE(convexSet[1].isApprox(p1[4])); 128 | ASSERT_TRUE(convexSet[2].isApprox(p1[1])); 129 | } 130 | -------------------------------------------------------------------------------- /test/test-humanoid-cop-centering-objective.cpp: -------------------------------------------------------------------------------- 1 | //////////////////////////////////////////////////////////////////////////////// 2 | /// 3 | ///\file test-humanoid-cop-centering-objective.cpp 4 | ///\brief Test of the CoP centering objective function 5 | ///\author de Gourcuff Martin 6 | ///\author Barthelemy Sebastien 7 | /// 8 | //////////////////////////////////////////////////////////////////////////////// 9 | 10 | #include "mpc_walkgen_gtest.h" 11 | #include 12 | #include 13 | 14 | TYPED_TEST(MpcWalkgenTest, HessianAndGradientValue) 15 | { 16 | using namespace MPCWalkgen; 17 | TEMPLATE_TYPEDEF(TypeParam) 18 | 19 | int nbSamples = 3; 20 | TypeParam samplingPeriod = 1.0; 21 | bool autoCompute = true; 22 | 23 | HumanoidFeetSupervisor feetSupervisor(nbSamples, 24 | samplingPeriod); 25 | LIPModel lip(nbSamples, samplingPeriod, autoCompute); 26 | HumanoidCopCenteringObjective obj(lip, feetSupervisor); 27 | 28 | ASSERT_EQ(MatrixX::Identity(6, 6), obj.getHessian().block(0, 0, 6, 6)); 29 | 30 | VectorX jerkInit(2*nbSamples + 2*feetSupervisor.getNbPreviewedSteps()); 31 | jerkInit.fill(1.0); 32 | 33 | for (int i=0; i<2*nbSamples; i++) 34 | { 35 | ASSERT_NEAR(1.0, 36 | obj.getGradient(jerkInit)(i), 37 | Constant::EPSILON); 38 | } 39 | for (int i=0; i<2*feetSupervisor.getNbPreviewedSteps(); ++i) 40 | { 41 | ASSERT_NEAR(0.0, 42 | obj.getGradient(jerkInit)(i + 2*nbSamples), 43 | Constant::EPSILON); 44 | } 45 | 46 | } 47 | 48 | 49 | TYPED_TEST(MpcWalkgenTest, HessianAndGradientSize) 50 | { 51 | using namespace MPCWalkgen; 52 | TEMPLATE_TYPEDEF(TypeParam) 53 | 54 | int nbSamples = 3; 55 | TypeParam samplingPeriod = 1.0; 56 | bool autoCompute = true; 57 | 58 | HumanoidFeetSupervisor feetSupervisor(nbSamples, 59 | samplingPeriod); 60 | LIPModel lip(nbSamples, samplingPeriod, autoCompute); 61 | HumanoidCopCenteringObjective obj(lip, feetSupervisor); 62 | 63 | VectorX jerkInit(2*nbSamples + 2*feetSupervisor.getNbPreviewedSteps()); 64 | jerkInit.fill(0.0); 65 | 66 | ASSERT_EQ(2*nbSamples + 2*feetSupervisor.getNbPreviewedSteps(), 67 | obj.getHessian().rows()); 68 | ASSERT_EQ(2*nbSamples + 2*feetSupervisor.getNbPreviewedSteps(), 69 | obj.getHessian().cols()); 70 | ASSERT_EQ(2*nbSamples + 2*feetSupervisor.getNbPreviewedSteps(), 71 | obj.getGradient(jerkInit).rows()); 72 | } 73 | 74 | -------------------------------------------------------------------------------- /test/test-humanoid-cop-constraint.cpp: -------------------------------------------------------------------------------- 1 | //////////////////////////////////////////////////////////////////////////////// 2 | /// 3 | ///\file test-humanoid-cop-constraint.cpp 4 | ///\brief Test of the CoP constraint function 5 | ///\author de Gourcuff Martin 6 | /// 7 | //////////////////////////////////////////////////////////////////////////////// 8 | 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | 15 | using namespace MPCWalkgen; 16 | typedef float Real; 17 | 18 | class HumanoidCopConstraintTest: public ::testing::Test 19 | { 20 | protected: 21 | typedef Type::MatrixX MatrixX; 22 | typedef Type::VectorX VectorX; 23 | typedef Type::Vector2 Vector2; 24 | typedef Type::vectorOfVector2 vectorOfVector2; 25 | 26 | virtual void SetUp() 27 | { 28 | using namespace MPCWalkgen; 29 | 30 | int nbSamples = 3; 31 | Real samplingPeriod = 1.0; 32 | bool autoCompute = true; 33 | VectorX variable; 34 | variable.setZero(2*nbSamples); 35 | Real feedbackPeriod = 0.5; 36 | 37 | vectorOfVector2 p(3); 38 | p[0] = Vector2(1.0, 1.0); 39 | p[1] = Vector2(-1.0, 1.0); 40 | p[2] = Vector2(1.0, -1.0); 41 | 42 | HumanoidFeetSupervisor feetSupervisor(nbSamples, 43 | samplingPeriod); 44 | feetSupervisor.setLeftFootCopConvexPolygon(ConvexPolygon(p)); 45 | feetSupervisor.setRightFootCopConvexPolygon(ConvexPolygon(p)); 46 | 47 | 48 | feetSupervisor.updateTimeline(variable, feedbackPeriod); 49 | 50 | LIPModel lip(nbSamples, samplingPeriod, autoCompute); 51 | lip.setFeedbackPeriod(feedbackPeriod); 52 | 53 | HumanoidCopConstraint copCtr(lip, feetSupervisor); 54 | 55 | VectorX x0 = VectorX::Zero(6); 56 | 57 | function_ = copCtr.getFunction(x0); 58 | gradient_ = copCtr.getGradient(x0.rows()); 59 | supBounds_ = copCtr.getSupBounds(x0); 60 | infBounds_ = copCtr.getInfBounds(x0); 61 | } 62 | 63 | VectorX function_; 64 | MatrixX gradient_; 65 | VectorX supBounds_; 66 | VectorX infBounds_; 67 | }; 68 | 69 | TEST_F(HumanoidCopConstraintTest, functionValue) 70 | { 71 | using namespace MPCWalkgen; 72 | 73 | ASSERT_TRUE(function_.isZero(Constant::EPSILON)); 74 | 75 | for (int i=0; i<3; i++) 76 | { 77 | ASSERT_NEAR(gradient_(i, i), -2, Constant::EPSILON); 78 | ASSERT_NEAR(gradient_(i, i + 3), -2, Constant::EPSILON); 79 | } 80 | 81 | ASSERT_TRUE(supBounds_.isConstant(1, Constant::EPSILON)); 82 | 83 | ASSERT_TRUE(infBounds_.isConstant(-Constant::MAXIMUM_BOUND_VALUE, Constant::EPSILON)); 84 | } 85 | 86 | 87 | TEST_F(HumanoidCopConstraintTest, sizeOfValues) 88 | { 89 | using namespace MPCWalkgen; 90 | 91 | ASSERT_EQ(function_.rows(), 3); 92 | ASSERT_EQ(gradient_.rows(), 3); 93 | ASSERT_EQ(gradient_.cols(), 6); 94 | 95 | ASSERT_EQ(supBounds_.rows(), 6); 96 | ASSERT_EQ(infBounds_.rows(), 6); 97 | 98 | } 99 | 100 | -------------------------------------------------------------------------------- /test/test-humanoid-feet-supervisor.cpp: -------------------------------------------------------------------------------- 1 | //////////////////////////////////////////////////////////////////////////////// 2 | /// 3 | ///\file test-humanoid-feet-supervisor.cpp 4 | ///\brief Test of the humanoid foot function 5 | ///\author de Gourcuff Martin 6 | ///\author Barthelemy Sebastien 7 | /// 8 | //////////////////////////////////////////////////////////////////////////////// 9 | 10 | #include "mpc_walkgen_gtest.h" 11 | #include 12 | 13 | using namespace MPCWalkgen; 14 | 15 | TYPED_TEST(MpcWalkgenTest, smoke) 16 | { 17 | HumanoidFeetSupervisor foo(10, 1.f); 18 | } 19 | 20 | 21 | 22 | -------------------------------------------------------------------------------- /test/test-humanoid-foot-constraint.cpp: -------------------------------------------------------------------------------- 1 | //////////////////////////////////////////////////////////////////////////////// 2 | /// 3 | ///\file test-humanoid-foot-constraint.cpp 4 | ///\brief Test of the foot constraint function 5 | ///\author de Gourcuff Martin 6 | ///\author Barthelemy Sebastien 7 | /// 8 | //////////////////////////////////////////////////////////////////////////////// 9 | 10 | #include "mpc_walkgen_gtest.h" 11 | #include 12 | #include 13 | #include 14 | 15 | class HumanoidFootConstraintTest: public ::testing::Test{}; 16 | 17 | 18 | TYPED_TEST(MpcWalkgenTest, functionValue) 19 | { 20 | 21 | //TODO: Complete with steps 22 | /* 23 | using namespace MPCWalkgen; 24 | TEMPLATE_TYPEDEF(TypeParam) 25 | 26 | int nbSamples = 3; 27 | TypeParam samplingPeriod = 1.0; 28 | bool autoCompute = true; 29 | HumanoidFootModel leftFoot(nbSamples, samplingPeriod), 30 | rightFoot(nbSamples, samplingPeriod); 31 | HumanoidFeetSupervisor feetSupervisor(leftFoot, 32 | rightFoot, 33 | nbSamples, 34 | samplingPeriod); 35 | LIPModel lip(nbSamples, samplingPeriod, autoCompute); 36 | 37 | 38 | HumanoidFootConstraint copCtr(lip, feetSupervisor); 39 | VectorX x0; 40 | x0.setZero(6); 41 | 42 | ASSERT_TRUE(copCtr.getFunction(x0).isZero(EPSILON)); 43 | 44 | for (int i=0; i<3; i++) 45 | { 46 | ASSERT_NEAR(copCtr.getGradient(x0.rows())(i, i), -2, EPSILON); 47 | ASSERT_NEAR(copCtr.getGradient(x0.rows())(i, i + 3), -2, EPSILON); 48 | } 49 | 50 | ASSERT_TRUE(copCtr.getSupBounds().isConstant(1, EPSILON)); 51 | ASSERT_TRUE(copCtr.getInfBounds().isZero(EPSILON)); 52 | */ 53 | } 54 | 55 | 56 | TYPED_TEST(MpcWalkgenTest, sizeOfValues) 57 | { 58 | using namespace MPCWalkgen; 59 | TEMPLATE_TYPEDEF(TypeParam) 60 | 61 | int nbSamples = 3; 62 | TypeParam samplingPeriod = 1.0; 63 | bool autoCompute = true; 64 | 65 | HumanoidFeetSupervisor feetSupervisor(nbSamples, 66 | samplingPeriod); 67 | LIPModel lip(nbSamples, samplingPeriod, autoCompute); 68 | 69 | 70 | HumanoidFootConstraint copCtr(lip, feetSupervisor); 71 | VectorX x0 = VectorX::Zero(6); 72 | 73 | ASSERT_EQ(copCtr.getFunction(x0).rows(), 0); 74 | ASSERT_EQ(copCtr.getGradient(x0.rows()).rows(), 0); 75 | ASSERT_EQ(copCtr.getGradient(x0.rows()).cols(), 6); 76 | 77 | ASSERT_EQ(copCtr.getSupBounds(x0).rows(), 0); 78 | ASSERT_EQ(copCtr.getInfBounds(x0).rows(), 0); 79 | 80 | } 81 | 82 | 83 | -------------------------------------------------------------------------------- /test/test-humanoid-foot-model.cpp: -------------------------------------------------------------------------------- 1 | //////////////////////////////////////////////////////////////////////////////// 2 | /// 3 | ///\file test-humanoid-foot-model.cpp 4 | ///\brief Test of the humanoid foot function 5 | ///\author de Gourcuff Martin 6 | ///\author Barthelemy Sebastien 7 | /// 8 | //////////////////////////////////////////////////////////////////////////////// 9 | 10 | #include "mpc_walkgen_gtest.h" 11 | #include 12 | 13 | using namespace MPCWalkgen; 14 | 15 | TYPED_TEST(MpcWalkgenTest, smoke) 16 | { 17 | HumanoidFootModel foo(10, 1.f); 18 | } 19 | 20 | 21 | 22 | -------------------------------------------------------------------------------- /test/test-humanoid-lip-com-jerk-minimization-objective.cpp: -------------------------------------------------------------------------------- 1 | //////////////////////////////////////////////////////////////////////////////// 2 | /// 3 | ///\file test-humanoid-lip-com-jerk-minimization-objective.cpp 4 | ///\brief Test of the LIP CoM jerk minimization objective function 5 | ///\author de Gourcuff Martin 6 | ///\author Barthelemy Sebastien 7 | /// 8 | //////////////////////////////////////////////////////////////////////////////// 9 | 10 | #include "mpc_walkgen_gtest.h" 11 | #include 12 | #include 13 | 14 | //TYPED_TEST(MpcWalkgenTest, HessianAndGradientValue) 15 | //{ 16 | // 17 | //} 18 | 19 | TYPED_TEST(MpcWalkgenTest, HessianAndGradientSize) 20 | { 21 | using namespace MPCWalkgen; 22 | TEMPLATE_TYPEDEF(TypeParam) 23 | //TODO: Add footSteps 24 | 25 | int nbSamples = 3; 26 | TypeParam samplingPeriod = 1.0; 27 | bool autoCompute = true; 28 | VectorX variable; 29 | variable.setZero(2*nbSamples); 30 | TypeParam feedbackPeriod = 0.5; 31 | 32 | HumanoidFeetSupervisor feetSupervisor(nbSamples, 33 | samplingPeriod); 34 | feetSupervisor.updateTimeline(variable, feedbackPeriod); 35 | 36 | LIPModel lip(nbSamples, samplingPeriod, autoCompute); 37 | lip.setFeedbackPeriod(feedbackPeriod); 38 | 39 | HumanoidLipComJerkMinimizationObjective obj(lip, feetSupervisor); 40 | 41 | VectorX jerkInit(2*nbSamples + 2*feetSupervisor.getNbPreviewedSteps()); 42 | jerkInit.fill(0.0); 43 | 44 | ASSERT_EQ(obj.getHessian().rows(), 6); 45 | ASSERT_EQ(obj.getHessian().cols(), 6); 46 | ASSERT_EQ(obj.getGradient(jerkInit).rows(), 6); 47 | } 48 | 49 | -------------------------------------------------------------------------------- /test/test-humanoid-lip-com-velocity-tracking-objective.cpp: -------------------------------------------------------------------------------- 1 | //////////////////////////////////////////////////////////////////////////////// 2 | /// 3 | ///\file test-humanoid-lip-com-velocity-tracking-objective.cpp 4 | ///\brief Test of the LIP CoM velocity tracking objective function 5 | ///\author de Gourcuff Martin 6 | ///\author Barthelemy Sebastien 7 | /// 8 | //////////////////////////////////////////////////////////////////////////////// 9 | 10 | #include "mpc_walkgen_gtest.h" 11 | #include 12 | #include 13 | 14 | using namespace MPCWalkgen; 15 | 16 | TYPED_TEST(MpcWalkgenTest, HessianAndGradientValue) 17 | { 18 | using namespace MPCWalkgen; 19 | 20 | 21 | int nbSamples = 2; 22 | TypeParam samplingPeriod = 0.2f; 23 | TypeParam stepPeriod = 0.4f; 24 | 25 | TypeParam initDSLength = 1.0f; 26 | 27 | HumanoidFeetSupervisor feetSupervisor(nbSamples, 28 | samplingPeriod); 29 | feetSupervisor.setStepPeriod(stepPeriod); 30 | feetSupervisor.setInitialDoubleSupportLength(initDSLength); 31 | LIPModel lip; 32 | //HumanoidLipComVelocityTrackingObjective obj(lip, leftFoot, rightFoot); 33 | 34 | } 35 | 36 | 37 | TYPED_TEST(MpcWalkgenTest, HessianAndGradientSize) 38 | { 39 | 40 | using namespace MPCWalkgen; 41 | TEMPLATE_TYPEDEF(TypeParam); 42 | //TODO: Add footSteps 43 | 44 | int nbSamples = 3; 45 | TypeParam samplingPeriod = 1.0f; 46 | bool autoCompute = true; 47 | VectorX variable; 48 | variable.setZero(2*nbSamples); 49 | TypeParam feedbackPeriod = 0.5f; 50 | 51 | HumanoidFeetSupervisor feetSupervisor(nbSamples, 52 | samplingPeriod); 53 | feetSupervisor.updateTimeline(variable, feedbackPeriod); 54 | 55 | LIPModel lip(nbSamples, samplingPeriod, autoCompute); 56 | lip.setFeedbackPeriod(feedbackPeriod); 57 | 58 | HumanoidLipComVelocityTrackingObjective obj(lip, feetSupervisor); 59 | 60 | VectorX jerkInit(2*nbSamples + 2*feetSupervisor.getNbPreviewedSteps()); 61 | jerkInit.fill(0.0); 62 | 63 | ASSERT_EQ(obj.getHessian().rows(), 6); 64 | ASSERT_EQ(obj.getHessian().cols(), 6); 65 | ASSERT_EQ(obj.getGradient(jerkInit).rows(), 6); 66 | } 67 | -------------------------------------------------------------------------------- /test/test-interpolation-function.cpp: -------------------------------------------------------------------------------- 1 | //////////////////////////////////////////////////////////////////////////////// 2 | /// 3 | ///\file test-interpolation-function.cpp 4 | ///\brief Test the cubic spline interpolator methods from type.cpp 5 | ///\author de Gourcuff Martin 6 | ///\author Barthelemy Sebastien 7 | /// 8 | //////////////////////////////////////////////////////////////////////////////// 9 | 10 | #include 11 | #include 12 | #include 13 | #include 14 | 15 | using namespace Eigen; 16 | using namespace MPCWalkgen; 17 | 18 | typedef double Real; 19 | 20 | class InterpolationTest: public ::testing::Test 21 | { 22 | public: 23 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 24 | protected: 25 | 26 | virtual void SetUp() 27 | { 28 | 29 | T_ = 10; 30 | initialstate_ << 1, 2, 3; 31 | finalState_ << 4, 5, 6; 32 | factor_.resize(12); 33 | interpolator_.computePolynomialNormalisedFactors(factor_, 34 | initialstate_, 35 | finalState_, 36 | T_); 37 | } 38 | 39 | MPCWalkgen::Interpolator interpolator_; 40 | Real T_; 41 | Real t_; 42 | Type::Vector3 initialstate_; 43 | Type::Vector3 finalState_; 44 | Type::VectorX factor_; 45 | Type::Vector4 subfactor_; 46 | Type::Vector4 subfactor1_; 47 | Type::Vector4 subfactor2_; 48 | }; 49 | 50 | 51 | TEST_F(InterpolationTest, initialConstraints) 52 | { 53 | t_ = 0.0; 54 | 55 | interpolator_.selectFactors(subfactor_, factor_, T_/6, T_); 56 | ASSERT_NEAR (Tools::polynomValue(subfactor_, t_/T_), 57 | initialstate_[0], Constant::EPSILON); 58 | ASSERT_NEAR (Tools::dPolynomValue(subfactor_, t_/T_)/T_, 59 | initialstate_[1], Constant::EPSILON); 60 | ASSERT_NEAR (Tools::ddPolynomValue(subfactor_, t_/T_)/(T_*T_), 61 | initialstate_[2], Constant::EPSILON); 62 | } 63 | 64 | 65 | TEST_F(InterpolationTest, finalConstraints) 66 | { 67 | t_ = T_; 68 | 69 | interpolator_.selectFactors(subfactor_, factor_, 5*T_/6, T_); 70 | 71 | ASSERT_NEAR (Tools::polynomValue(subfactor_, t_/T_), 72 | finalState_[0], Constant::EPSILON); 73 | ASSERT_NEAR (Tools::dPolynomValue(subfactor_, t_/T_)/T_, 74 | finalState_[1], Constant::EPSILON); 75 | ASSERT_NEAR (Tools::ddPolynomValue(subfactor_, t_/T_)/(T_*T_), 76 | finalState_[2], Constant::EPSILON); 77 | } 78 | 79 | 80 | TEST_F(InterpolationTest, firstJunction) 81 | { 82 | t_ = T_/3; 83 | 84 | interpolator_.selectFactors(subfactor1_, factor_, T_/6, T_); 85 | interpolator_.selectFactors(subfactor2_, factor_, T_/2, T_); 86 | 87 | ASSERT_NEAR (Tools::polynomValue(subfactor1_, t_/T_), 88 | Tools::polynomValue(subfactor2_, t_/T_), Constant::EPSILON); 89 | ASSERT_NEAR (Tools::dPolynomValue(subfactor1_, t_/T_)/T_, 90 | Tools::dPolynomValue(subfactor2_, t_/T_)/T_, Constant::EPSILON); 91 | ASSERT_NEAR (Tools::ddPolynomValue(subfactor1_, t_/T_)/(T_*T_), 92 | Tools::ddPolynomValue(subfactor2_, t_/T_)/(T_*T_), Constant::EPSILON); 93 | } 94 | 95 | 96 | TEST_F(InterpolationTest, secondJunction) 97 | { 98 | t_ = 2*T_/3; 99 | 100 | interpolator_.selectFactors(subfactor1_, factor_, T_/2, T_); 101 | interpolator_.selectFactors(subfactor2_, factor_, 5*T_/6, T_); 102 | 103 | ASSERT_NEAR (Tools::polynomValue(subfactor1_, t_/T_), 104 | Tools::polynomValue(subfactor2_, t_/T_), Constant::EPSILON); 105 | ASSERT_NEAR (Tools::dPolynomValue(subfactor1_, t_/T_)/T_, 106 | Tools::dPolynomValue(subfactor2_, t_/T_)/T_, Constant::EPSILON); 107 | ASSERT_NEAR (Tools::ddPolynomValue(subfactor1_, t_/T_)/(T_*T_), 108 | Tools::ddPolynomValue(subfactor2_, t_/T_)/(T_*T_), Constant::EPSILON); 109 | } 110 | 111 | 112 | TEST_F(InterpolationTest, firstPolynomValue) 113 | { 114 | t_ = T_/6; 115 | 116 | interpolator_.selectFactors(subfactor_, factor_, T_/6, T_); 117 | 118 | ASSERT_NEAR (Tools::polynomValue(subfactor_, t_/T_), 7.12731, Constant::EPSILON); 119 | ASSERT_NEAR (Tools::dPolynomValue(subfactor_, t_/T_), 45.2917, Constant::EPSILON); 120 | ASSERT_NEAR (Tools::ddPolynomValue(subfactor_, t_/T_), 3.5, Constant::EPSILON); 121 | } 122 | 123 | 124 | TEST_F(InterpolationTest, secondPolynomValue) 125 | { 126 | t_ = T_/2; 127 | 128 | interpolator_.selectFactors(subfactor_, factor_, T_/2, T_); 129 | 130 | ASSERT_NEAR (Tools::polynomValue(subfactor_, t_/T_), 13.3333, Constant::EPSILON); 131 | ASSERT_NEAR (Tools::dPolynomValue(subfactor_, t_/T_), -18.25, Constant::EPSILON); 132 | ASSERT_NEAR (Tools::ddPolynomValue(subfactor_, t_/T_), -180, Constant::EPSILON); 133 | } 134 | 135 | 136 | TEST_F(InterpolationTest, thirdPolynomValue) 137 | { 138 | t_ = 5*T_/6; 139 | 140 | interpolator_.selectFactors(subfactor_, factor_, 5*T_/6, T_); 141 | 142 | ASSERT_NEAR (Tools::polynomValue(subfactor_, t_/T_), 2.45602, Constant::EPSILON); 143 | ASSERT_NEAR (Tools::dPolynomValue(subfactor_, t_/T_), -22.2083, Constant::EPSILON); 144 | ASSERT_NEAR (Tools::ddPolynomValue(subfactor_, t_/T_), 266.5, Constant::EPSILON); 145 | } 146 | 147 | -------------------------------------------------------------------------------- /test/test-lip-model.cpp: -------------------------------------------------------------------------------- 1 | //////////////////////////////////////////////////////////////////////////////// 2 | /// 3 | ///\file test-lip-model.cpp 4 | ///\brief Test the LIP model 5 | ///\author Lafaye Jory 6 | ///\author Barthelemy Sebastien 7 | /// 8 | //////////////////////////////////////////////////////////////////////////////// 9 | 10 | #include "mpc_walkgen_gtest.h" 11 | #include 12 | 13 | TYPED_TEST(MpcWalkgenTest, sizeOfMatrices) 14 | { 15 | using namespace MPCWalkgen; 16 | TEMPLATE_TYPEDEF(TypeParam); 17 | 18 | int nbSamples = 10; 19 | TypeParam samplingPeriod = 1.0; 20 | bool autoCompute = true; 21 | TypeParam feebackPeriod = 0.25; 22 | LIPModel m(nbSamples, samplingPeriod, autoCompute); 23 | m.setFeedbackPeriod(feebackPeriod); 24 | 25 | checkLinearDynamicSize(m.getComPosLinearDynamic(), nbSamples); 26 | checkLinearDynamicSize(m.getComVelLinearDynamic(), nbSamples); 27 | checkLinearDynamicSize(m.getComAccLinearDynamic(), nbSamples); 28 | checkLinearDynamicSize(m.getComJerkLinearDynamic(), nbSamples); 29 | checkLinearDynamicSize(m.getCopXLinearDynamic(), nbSamples); 30 | checkLinearDynamicSize(m.getCopYLinearDynamic(), nbSamples); 31 | 32 | 33 | for(int i=0; i((samplingPeriod + Constant::EPSILON)/feebackPeriod); ++i) 34 | { 35 | checkLinearDynamicSize(m.getComPosLinearDynamic(i), nbSamples); 36 | checkLinearDynamicSize(m.getComVelLinearDynamic(i), nbSamples); 37 | checkLinearDynamicSize(m.getComAccLinearDynamic(i), nbSamples); 38 | checkLinearDynamicSize(m.getComJerkLinearDynamic(i), nbSamples); 39 | checkLinearDynamicSize(m.getCopXLinearDynamic(i), nbSamples); 40 | checkLinearDynamicSize(m.getCopYLinearDynamic(i), nbSamples); 41 | } 42 | } 43 | 44 | TYPED_TEST(MpcWalkgenTest, valuesOfMatrices) 45 | { 46 | using namespace MPCWalkgen; 47 | TEMPLATE_TYPEDEF(TypeParam); 48 | 49 | int nbSamples = 1; 50 | TypeParam samplingPeriod = 2.0f; 51 | bool autoCompute = true; 52 | TypeParam feebackPeriod = 0.25f; 53 | TypeParam comHeight = 0.45f; 54 | Vector3 gravity(1.0, -1.0, 9.0); 55 | LIPModel m(nbSamples, samplingPeriod, autoCompute); 56 | 57 | m.setSamplingPeriod(samplingPeriod); 58 | m.setComHeight(comHeight); 59 | m.setGravity(gravity); 60 | m.setFeedbackPeriod(feebackPeriod); 61 | 62 | VectorX jerk(nbSamples); 63 | jerk(0) = 1.0; 64 | 65 | VectorX init(3); 66 | init(0)=2.0; 67 | init(1)=1.5; 68 | init(2)=-3.0; 69 | 70 | const LinearDynamic& dynPos = m.getComPosLinearDynamic(); 71 | TypeParam pos = (dynPos.S * init + dynPos.K + dynPos.U * jerk)(0); 72 | ASSERT_NEAR(pos, 0.333333333, Constant::EPSILON); 73 | 74 | const LinearDynamic& dynVel = m.getComVelLinearDynamic(); 75 | TypeParam vel = (dynVel.S * init + dynVel.K + dynVel.U * jerk)(0); 76 | ASSERT_NEAR(vel, -2.5, Constant::EPSILON); 77 | 78 | const LinearDynamic& dynAcc = m.getComAccLinearDynamic(); 79 | TypeParam acc = (dynAcc.S * init + dynAcc.K + dynAcc.U * jerk)(0); 80 | ASSERT_NEAR(acc, -1.0, Constant::EPSILON); 81 | 82 | const LinearDynamic& dynJerk = m.getComJerkLinearDynamic(); 83 | TypeParam j = (dynJerk.S * init + dynJerk.K + dynJerk.U * jerk)(0); 84 | ASSERT_NEAR(j, 1.0, Constant::EPSILON); 85 | 86 | const LinearDynamic& dynCopX = m.getCopXLinearDynamic(); 87 | TypeParam copX = (dynCopX.S * init + dynCopX.K + dynCopX.U * jerk)(0); 88 | ASSERT_NEAR(copX, 0.333333333, Constant::EPSILON); 89 | 90 | const LinearDynamic& dynCopY = m.getCopYLinearDynamic(); 91 | TypeParam copY = (dynCopY.S * init + dynCopY.K + dynCopY.U * jerk)(0); 92 | ASSERT_NEAR(copY, 0.433333333, Constant::EPSILON); 93 | } 94 | 95 | -------------------------------------------------------------------------------- /test/test-qpoases-solver.cpp: -------------------------------------------------------------------------------- 1 | //////////////////////////////////////////////////////////////////////////////// 2 | /// 3 | ///\file test-qpoases-solver.cpp 4 | ///\brief Test the QPOases solver 5 | ///\author Lafaye Jory 6 | ///\author Barthelemy Sebastien 7 | /// 8 | /// The purpose of this test is to 9 | /// * check the QPSolvers return the expected results 10 | /// * check that using all the QPSolver variants from the same binary works 11 | /// (no symbol collision...) so we use makeQPSolver, 12 | /// makeQPSolver and also the raw qpAOSESfloat library 13 | //////////////////////////////////////////////////////////////////////////////// 14 | 15 | #include 16 | #include 17 | #include 18 | #include 19 | 20 | #define QPOASES_REAL_IS_FLOAT 21 | #include 22 | 23 | template 24 | class QPSolverTest: public ::testing::Test {}; 25 | 26 | typedef ::testing::Types MyTypes; 27 | 28 | TYPED_TEST_CASE(QPSolverTest, MyTypes); 29 | 30 | using namespace MPCWalkgen; 31 | 32 | const int maxNbIterations = 10000; 33 | 34 | TYPED_TEST(QPSolverTest, testSolver) 35 | { 36 | using namespace MPCWalkgen; 37 | 38 | boost::scoped_ptr< QPSolver > qp(makeQPSolver(2, 0)); 39 | QPMatrices m; 40 | 41 | typename QPMatrices::MatrixXrm Q(2, 2); 42 | typename QPMatrices::MatrixXrm A(0, 2); 43 | typename QPMatrices::VectorX p(2); 44 | typename QPMatrices::VectorX b(0, 1); 45 | typename QPMatrices::VectorX bl(0, 1); 46 | typename QPMatrices::VectorX xu(2); 47 | xu.fill(100); 48 | typename QPMatrices::VectorX xl(2); 49 | xl.fill(-100); 50 | 51 | Q(0,0)=5.f; Q(0,1)=4.f; 52 | Q(1,0)=4.f; Q(1,1)=5.f; 53 | 54 | p[0]=1.f; p[1]=-1.f; 55 | 56 | typename QPMatrices::VectorX x(2); 57 | x[0]=-10.f; 58 | x[1]=-10.f; 59 | 60 | m.Q = Q; 61 | m.p = p; 62 | m.A = A; 63 | m.bl = bl; 64 | m.bu = b; 65 | m.xl = xl; 66 | m.xu = xu; 67 | 68 | qp->solve(m, maxNbIterations, x); 69 | 70 | ASSERT_NEAR(x(0), -1, Constant::EPSILON); 71 | ASSERT_NEAR(x(1), 1, Constant::EPSILON); 72 | } 73 | 74 | 75 | TYPED_TEST(QPSolverTest, testSolverWithConstraint) 76 | { 77 | using namespace MPCWalkgen; 78 | 79 | boost::scoped_ptr< QPSolver > qp(makeQPSolver(2, 1)); 80 | QPMatrices m; 81 | 82 | typename QPMatrices::MatrixXrm Q(2, 2); 83 | typename QPMatrices::MatrixXrm A(1, 2); 84 | typename QPMatrices::VectorX p(2); 85 | typename QPMatrices::VectorX b(1); 86 | typename QPMatrices::VectorX bl(1); 87 | bl.fill(-100); 88 | typename QPMatrices::VectorX xu(2); 89 | xu.fill(100); 90 | typename QPMatrices::VectorX xl(2); 91 | xl.fill(-100); 92 | 93 | Q(0,0)=5.f; Q(0,1)=4.f; 94 | Q(1,0)=4.f; Q(1,1)=5.f; 95 | 96 | p[0]=1.f; p[1]=-1.f; 97 | 98 | A(0,0)=1.f; A(0,1)=0.f; 99 | b(0)=-2.f; 100 | 101 | typename QPMatrices::VectorX x(2); 102 | x[0]=-10.f; 103 | x[1]=-10.f; 104 | 105 | m.Q = Q; 106 | m.p = p; 107 | m.A = A; 108 | m.bl = bl; 109 | m.bu = b; 110 | m.xl = xl; 111 | m.xu = xu; 112 | 113 | qp->solve(m, maxNbIterations, x); 114 | 115 | ASSERT_NEAR(x(0), -2.0f, Constant::EPSILON); 116 | ASSERT_NEAR(x(1), 1.8f, Constant::EPSILON); 117 | } 118 | 119 | 120 | TEST(QPOasesTest, testSolverWithConstraint) 121 | { 122 | using namespace MPCWalkgen; 123 | 124 | QPMatrices m; 125 | 126 | QPMatrices::MatrixXrm Q(2, 2); 127 | QPMatrices::MatrixXrm A(1, 2); 128 | QPMatrices::VectorX p(2); 129 | QPMatrices::VectorX b(1); 130 | QPMatrices::VectorX bl(1); 131 | bl.fill(-100); 132 | QPMatrices::VectorX xu(2); 133 | xu.fill(100); 134 | QPMatrices::VectorX xl(2); 135 | xl.fill(-100); 136 | 137 | Q(0,0)=5.f; Q(0,1)=4.f; 138 | Q(1,0)=4.f; Q(1,1)=5.f; 139 | 140 | p[0]=1.f; p[1]=-1.f; 141 | 142 | A(0,0)=1.f; A(0,1)=0.f; 143 | b(0)=-2.f; 144 | 145 | QPMatrices::VectorX x(2); 146 | x[0]=-10.f; 147 | x[1]=-10.f; 148 | 149 | m.Q = Q; 150 | m.p = p; 151 | m.A = A; 152 | m.bl = bl; 153 | m.bu = b; 154 | m.xl = xl; 155 | m.xu = xu; 156 | 157 | boost::scoped_ptr qpRaw(new qpOASES::QProblem(2, 1)); 158 | qpRaw->setPrintLevel(qpOASES::PL_NONE); 159 | int nbIterations = maxNbIterations; 160 | qpRaw->init(Q.data(), 161 | p.data(), 162 | m.A.data(), 163 | m.xl.data(), 164 | m.xu.data(), 165 | m.bl.data(), 166 | m.bu.data(), 167 | nbIterations, 168 | NULL); 169 | qpRaw->getPrimalSolution(x.data()); 170 | ASSERT_NEAR(x(0), -2.0f, Constant::EPSILON); 171 | ASSERT_NEAR(x(1), 1.8f, Constant::EPSILON); 172 | } 173 | -------------------------------------------------------------------------------- /test/test-zebulon-base-model.cpp: -------------------------------------------------------------------------------- 1 | //////////////////////////////////////////////////////////////////////////////// 2 | /// 3 | ///\file test-zebulon-base-model.cpp 4 | ///\brief Test the Zebulon base model 5 | ///\author Lafaye Jory 6 | ///\author Barthelemy Sebastien 7 | /// 8 | //////////////////////////////////////////////////////////////////////////////// 9 | 10 | #include "mpc_walkgen_gtest.h" 11 | #include 12 | 13 | using namespace MPCWalkgen; 14 | 15 | TYPED_TEST(MpcWalkgenTest, sizeOfMatrices) 16 | { 17 | using namespace MPCWalkgen; 18 | 19 | int nbSamples = 10; 20 | TypeParam samplingPeriod = 1.0; 21 | bool autoCompute = true; 22 | BaseModel m(nbSamples, samplingPeriod, autoCompute); 23 | 24 | checkLinearDynamicSize(m.getBasePosLinearDynamic(), nbSamples); 25 | checkLinearDynamicSize(m.getBaseVelLinearDynamic(), nbSamples); 26 | checkLinearDynamicSize(m.getBaseAccLinearDynamic(), nbSamples); 27 | checkLinearDynamicSize(m.getBaseJerkLinearDynamic(), nbSamples); 28 | } 29 | 30 | TYPED_TEST(MpcWalkgenTest, valuesOfMatrices) 31 | { 32 | using namespace MPCWalkgen; 33 | TEMPLATE_TYPEDEF(TypeParam); 34 | 35 | int nbSamples = 1; 36 | TypeParam samplingPeriod = 2.0; 37 | bool autoCompute = true; 38 | BaseModel m(nbSamples, samplingPeriod, autoCompute); 39 | 40 | VectorX jerk(nbSamples); 41 | jerk(0) = 1.0; 42 | 43 | VectorX init(3); 44 | init(0)=2.0; 45 | init(1)=1.5; 46 | init(2)=-3.0; 47 | 48 | const LinearDynamic& dynPos = m.getBasePosLinearDynamic(); 49 | TypeParam pos = (dynPos.S * init + dynPos.U * jerk)(0); 50 | ASSERT_NEAR(pos, 0.333333333, Constant::EPSILON); 51 | 52 | const LinearDynamic& dynVel = m.getBaseVelLinearDynamic(); 53 | TypeParam vel = (dynVel.S * init + dynVel.U * jerk)(0); 54 | ASSERT_NEAR(vel, -2.5, Constant::EPSILON); 55 | 56 | const LinearDynamic& dynAcc = m.getBaseAccLinearDynamic(); 57 | TypeParam acc = (dynAcc.S * init + dynAcc.U * jerk)(0); 58 | ASSERT_NEAR(acc, -1.0, Constant::EPSILON); 59 | 60 | const LinearDynamic& dynJerk = m.getBaseJerkLinearDynamic(); 61 | TypeParam j = (dynJerk.S * init + dynJerk.U * jerk)(0); 62 | ASSERT_NEAR(j, 1.0, Constant::EPSILON); 63 | } 64 | 65 | -------------------------------------------------------------------------------- /test/test-zebulon-base-motion-constraint.cpp: -------------------------------------------------------------------------------- 1 | //////////////////////////////////////////////////////////////////////////////// 2 | /// 3 | ///\file test-zebulon-base-motion-constraint.cpp 4 | ///\brief Test the Zebulon base motion constraint function 5 | ///\author Lafaye Jory 6 | ///\author Barthelemy Sebastien 7 | /// 8 | //////////////////////////////////////////////////////////////////////////////// 9 | 10 | #include "mpc_walkgen_gtest.h" 11 | #include 12 | #include 13 | 14 | class ZebulonBaseMotionConstraintTest: public ::testing::Test{}; 15 | 16 | using namespace MPCWalkgen; 17 | 18 | typedef float TypeParam; 19 | 20 | TYPED_TEST(MpcWalkgenTest, functionValue) 21 | { 22 | using namespace MPCWalkgen; 23 | TEMPLATE_TYPEDEF(TypeParam) 24 | 25 | BaseModel m; 26 | BaseMotionConstraint ctr(m); 27 | 28 | VectorX jerkInit(2); 29 | jerkInit(0) = 0.0f; 30 | jerkInit(1) = 0.0f; 31 | 32 | ASSERT_TRUE(ctr.getFunctionSup(jerkInit)(0)>0.0); 33 | ASSERT_TRUE(ctr.getFunctionSup(jerkInit)(1)>0.0); 34 | ASSERT_TRUE(ctr.getFunctionInf(jerkInit)(0)<0.0); 35 | ASSERT_TRUE(ctr.getFunctionInf(jerkInit)(1)<0.0); 36 | 37 | jerkInit(0) = 0.0f; 38 | jerkInit(1) = 2.0f; 39 | ASSERT_TRUE(ctr.getFunctionSup(jerkInit)(0)>0.0); 40 | ASSERT_NEAR(ctr.getFunctionSup(jerkInit)(1), 0.0, Constant::EPSILON); 41 | 42 | jerkInit(0) = 0.0f; 43 | jerkInit(1) = 2.1f; 44 | ASSERT_TRUE(ctr.getFunctionSup(jerkInit)(0)>0.0); 45 | ASSERT_TRUE(ctr.getFunctionSup(jerkInit)(1)<0.0); 46 | 47 | jerkInit(0) = 0.0f; 48 | jerkInit(1) = -2.0f; 49 | ASSERT_TRUE(ctr.getFunctionInf(jerkInit)(0)<0.0); 50 | ASSERT_NEAR(ctr.getFunctionInf(jerkInit)(1), 0.0, Constant::EPSILON); 51 | 52 | 53 | jerkInit(0) = 0.0f; 54 | jerkInit(1) = -2.1f; 55 | ASSERT_TRUE(ctr.getFunctionInf(jerkInit)(0)<0.0); 56 | ASSERT_TRUE(ctr.getFunctionInf(jerkInit)(1)>0.0); 57 | 58 | jerkInit(0) = 2.0f; 59 | jerkInit(1) = 0.0f; 60 | ASSERT_NEAR(ctr.getFunctionSup(jerkInit)(0), 0.0, Constant::EPSILON); 61 | ASSERT_TRUE(ctr.getFunctionSup(jerkInit)(1)>0.0); 62 | 63 | jerkInit(0) = 2.1f; 64 | jerkInit(1) = 0.0f; 65 | ASSERT_TRUE(ctr.getFunctionSup(jerkInit)(0)<0.0); 66 | ASSERT_TRUE(ctr.getFunctionSup(jerkInit)(1)>0.0); 67 | 68 | jerkInit(0) = -2.0f; 69 | jerkInit(1) = 0.0f; 70 | ASSERT_NEAR(ctr.getFunctionInf(jerkInit)(0), 0.0, Constant::EPSILON); 71 | ASSERT_TRUE(ctr.getFunctionInf(jerkInit)(1)<0.0); 72 | 73 | 74 | jerkInit(0) = -2.1f; 75 | jerkInit(1) = 0.0f; 76 | ASSERT_TRUE(ctr.getFunctionInf(jerkInit)(0)>0.0); 77 | ASSERT_TRUE(ctr.getFunctionInf(jerkInit)(1)<0.0); 78 | } 79 | 80 | 81 | TYPED_TEST(MpcWalkgenTest, sizeOfvalues) 82 | { 83 | using namespace MPCWalkgen; 84 | TEMPLATE_TYPEDEF(TypeParam); 85 | 86 | int nbSamples = 3; 87 | TypeParam samplingPeriod = 1.0f; 88 | bool autoCompute = true; 89 | BaseModel m(nbSamples, samplingPeriod, autoCompute); 90 | 91 | BaseMotionConstraint ctr(m); 92 | 93 | int M = ctr.getNbConstraints(); 94 | 95 | VectorX jerkInit(2*nbSamples); 96 | jerkInit.fill(0.0f); 97 | 98 | ASSERT_EQ(ctr.getGradient().rows(), M); 99 | ASSERT_EQ(ctr.getGradient().cols(), 2*nbSamples); 100 | ASSERT_EQ(ctr.getFunctionInf(jerkInit).rows(), M); 101 | ASSERT_EQ(ctr.getFunctionInf(jerkInit).cols(), 1); 102 | ASSERT_EQ(ctr.getFunctionSup(jerkInit).rows(), M); 103 | ASSERT_EQ(ctr.getFunctionSup(jerkInit).cols(), 1); 104 | } 105 | -------------------------------------------------------------------------------- /test/test-zebulon-base-position-tracking-objective.cpp: -------------------------------------------------------------------------------- 1 | //////////////////////////////////////////////////////////////////////////////// 2 | /// 3 | ///\file test-zebulon-base-position-tracking-objective.cpp 4 | ///\brief Test the Zebulon base position tracking objective function 5 | ///\author Lafaye Jory 6 | ///\author Barthelemy Sebastien 7 | /// 8 | //////////////////////////////////////////////////////////////////////////////// 9 | 10 | #include "mpc_walkgen_gtest.h" 11 | #include 12 | #include 13 | 14 | TYPED_TEST(MpcWalkgenTest, functionValue) 15 | { 16 | using namespace MPCWalkgen; 17 | TEMPLATE_TYPEDEF(TypeParam) 18 | 19 | BaseModel m; 20 | BasePositionTrackingObjective obj(m); 21 | 22 | VectorX jerkInit(2); 23 | jerkInit(0) = 1.0; 24 | jerkInit(1) = 1.0; 25 | 26 | VectorX velRef(2); 27 | velRef(0) = 2.0; 28 | velRef(1) = 2.0; 29 | obj.setPosRefInWorldFrame(velRef); 30 | 31 | 32 | VectorX baseState(3); 33 | baseState.fill(0); 34 | baseState(1) = -1.5; 35 | m.setStateX(baseState); 36 | baseState(1) = 0.5; 37 | m.setStateY(baseState); 38 | 39 | ASSERT_NEAR(obj.getGradient(jerkInit)(0,0), -0.555555, Constant::EPSILON); 40 | ASSERT_NEAR(obj.getGradient(jerkInit)(1,0), -0.222222, Constant::EPSILON); 41 | 42 | 43 | ASSERT_NEAR(obj.getHessian()(0,0), 0.0277777, Constant::EPSILON); 44 | ASSERT_NEAR(obj.getHessian()(1,1), 0.0277777, Constant::EPSILON); 45 | ASSERT_NEAR(obj.getHessian()(0,1), 0.0, Constant::EPSILON); 46 | ASSERT_NEAR(obj.getHessian()(1,0), 0.0, Constant::EPSILON); 47 | 48 | } 49 | 50 | 51 | TYPED_TEST(MpcWalkgenTest, sizeOfvalues) 52 | { 53 | using namespace MPCWalkgen; 54 | TEMPLATE_TYPEDEF(TypeParam) 55 | 56 | int nbSamples = 3; 57 | TypeParam samplingPeriod = 1.0; 58 | bool autoCompute = true; 59 | BaseModel m(nbSamples, samplingPeriod, autoCompute); 60 | BasePositionTrackingObjective obj(m); 61 | 62 | VectorX jerkInit(2*nbSamples); 63 | jerkInit.fill(0.0); 64 | 65 | ASSERT_EQ(obj.getHessian().rows(), 2*nbSamples); 66 | ASSERT_EQ(obj.getHessian().cols(), 2*nbSamples); 67 | ASSERT_EQ(obj.getGradient(jerkInit).rows(), 2*nbSamples); 68 | ASSERT_EQ(obj.getGradient(jerkInit).cols(), 1); 69 | } 70 | -------------------------------------------------------------------------------- /test/test-zebulon-base-velocity-tracking-objective.cpp: -------------------------------------------------------------------------------- 1 | //////////////////////////////////////////////////////////////////////////////// 2 | /// 3 | ///\file test-zebulon-base-velocity-tracking-objective.cpp 4 | ///\brief Test the Zebulon base velocity tracking objective function 5 | ///\author Lafaye Jory 6 | ///\author Barthelemy Sebastien 7 | /// 8 | //////////////////////////////////////////////////////////////////////////////// 9 | 10 | #include "mpc_walkgen_gtest.h" 11 | #include 12 | #include 13 | 14 | class ZebulonBaseVelocityTrackingTest: public ::testing::Test{}; 15 | 16 | using namespace MPCWalkgen; 17 | 18 | typedef float TypeParam; 19 | 20 | TYPED_TEST(MpcWalkgenTest, functionValue) 21 | { 22 | using namespace MPCWalkgen; 23 | TEMPLATE_TYPEDEF(TypeParam); 24 | 25 | BaseModel m; 26 | BaseVelocityTrackingObjective obj(m); 27 | 28 | VectorX jerkInit(2); 29 | jerkInit(0) = 1.0; 30 | jerkInit(1) = 1.0; 31 | 32 | VectorX velRef(2); 33 | velRef(0) = 2.0; 34 | velRef(1) = 2.0; 35 | obj.setVelRefInWorldFrame(velRef); 36 | 37 | VectorX baseState(3); 38 | baseState.fill(0); 39 | baseState(1) = -1.5f; 40 | m.setStateX(baseState); 41 | baseState(1) = 0.5f; 42 | m.setStateY(baseState); 43 | 44 | ASSERT_NEAR(obj.getGradient(jerkInit)(0,0), -1.5f, Constant::EPSILON); 45 | ASSERT_NEAR(obj.getGradient(jerkInit)(1,0), -0.5f, Constant::EPSILON); 46 | 47 | ASSERT_NEAR(obj.getHessian()(0,0), 0.25f, Constant::EPSILON); 48 | ASSERT_NEAR(obj.getHessian()(1,1), 0.25f, Constant::EPSILON); 49 | ASSERT_NEAR(obj.getHessian()(0,1), 0.0f, Constant::EPSILON); 50 | ASSERT_NEAR(obj.getHessian()(1,0), 0.0f, Constant::EPSILON); 51 | 52 | } 53 | 54 | 55 | TYPED_TEST(MpcWalkgenTest, sizeOfvalues) 56 | { 57 | using namespace MPCWalkgen; 58 | TEMPLATE_TYPEDEF(TypeParam); 59 | 60 | int nbSamples = 3; 61 | TypeParam samplingPeriod = 1.0; 62 | bool autoCompute = true; 63 | BaseModel m(nbSamples, samplingPeriod, autoCompute); 64 | BaseVelocityTrackingObjective obj(m); 65 | 66 | VectorX jerkInit(2*nbSamples); 67 | jerkInit.fill(0.0); 68 | 69 | ASSERT_EQ(obj.getHessian().rows(), 2*nbSamples); 70 | ASSERT_EQ(obj.getHessian().cols(), 2*nbSamples); 71 | ASSERT_EQ(obj.getGradient(jerkInit).rows(), 2*nbSamples); 72 | ASSERT_EQ(obj.getGradient(jerkInit).cols(), 1); 73 | 74 | } 75 | -------------------------------------------------------------------------------- /test/test-zebulon-com-constraint.cpp: -------------------------------------------------------------------------------- 1 | //////////////////////////////////////////////////////////////////////////////// 2 | /// 3 | ///\file test-zebulon-com-constraint.cpp 4 | ///\brief Test the Zebulon CoM constraint function 5 | ///\author Lafaye Jory 6 | ///\author Barthelemy Sebastien 7 | /// 8 | //////////////////////////////////////////////////////////////////////////////// 9 | 10 | #include "mpc_walkgen_gtest.h" 11 | #include 12 | #include 13 | #include 14 | 15 | TYPED_TEST(MpcWalkgenTest, functionValue) 16 | { 17 | using namespace MPCWalkgen; 18 | TEMPLATE_TYPEDEF(TypeParam); 19 | 20 | LIPModel m1; 21 | BaseModel m2; 22 | 23 | TypeParam copLimitMin = 0.1f; 24 | TypeParam copLimitMax = 0.15f; 25 | 26 | vectorOfVector2 p(8); 27 | p[7] = Vector2(copLimitMax, 0.0); 28 | p[6] = Vector2(copLimitMin, -copLimitMin); 29 | p[5] = Vector2(0.0, -copLimitMax); 30 | p[4] = Vector2(-copLimitMin, -copLimitMin); 31 | p[3] = Vector2(-copLimitMax, 0.0); 32 | p[2] = Vector2(-copLimitMin, copLimitMin); 33 | p[1] = Vector2(0.0, copLimitMax); 34 | p[0] = Vector2(copLimitMin, copLimitMin); 35 | ConvexPolygon h(p); 36 | m2.setComSupportConvexPolygon(h); 37 | 38 | ComConstraint ctr(m1, m2); 39 | VectorX jerkInit(4); 40 | 41 | jerkInit(0) = 0.0; 42 | jerkInit(1) = 0.0; 43 | jerkInit(2) = 0.0; 44 | jerkInit(3) = 0.0; 45 | ASSERT_TRUE(ctr.getFunction(jerkInit)(0)<0.0); 46 | ASSERT_TRUE(ctr.getFunction(jerkInit)(1)<0.0); 47 | ASSERT_TRUE(ctr.getFunction(jerkInit)(2)<0.0); 48 | ASSERT_TRUE(ctr.getFunction(jerkInit)(3)<0.0); 49 | ASSERT_TRUE(ctr.getFunction(jerkInit)(4)<0.0); 50 | ASSERT_TRUE(ctr.getFunction(jerkInit)(5)<0.0); 51 | ASSERT_TRUE(ctr.getFunction(jerkInit)(6)<0.0); 52 | ASSERT_TRUE(ctr.getFunction(jerkInit)(7)<0.0); 53 | 54 | jerkInit(0) = 0.0f; 55 | jerkInit(1) = 0.0f; 56 | jerkInit(2) = 6.0f*copLimitMax; 57 | jerkInit(3) = 0.0f; 58 | ASSERT_TRUE(ctr.getFunction(jerkInit)(3)<0.0); 59 | ASSERT_TRUE(ctr.getFunction(jerkInit)(4)<0.0); 60 | ASSERT_TRUE(ctr.getFunction(jerkInit)(5)<0.0); 61 | ASSERT_TRUE(ctr.getFunction(jerkInit)(6)<0.0); 62 | ASSERT_TRUE(ctr.getFunction(jerkInit)(7)<0.0); 63 | ASSERT_TRUE(ctr.getFunction(jerkInit)(0)<0.0); 64 | ASSERT_NEAR(ctr.getFunction(jerkInit)(1), 0.0, Constant::EPSILON); 65 | ASSERT_NEAR(ctr.getFunction(jerkInit)(2), 0.0, Constant::EPSILON); 66 | 67 | jerkInit(0) = 0.0; 68 | jerkInit(1) = 0.0; 69 | jerkInit(2) = -6.0f*copLimitMax; 70 | jerkInit(3) = 0.0; 71 | ASSERT_TRUE(ctr.getFunction(jerkInit)(3)<0.0); 72 | ASSERT_TRUE(ctr.getFunction(jerkInit)(4)<0.0); 73 | ASSERT_NEAR(ctr.getFunction(jerkInit)(5), 0.0, Constant::EPSILON); 74 | ASSERT_NEAR(ctr.getFunction(jerkInit)(6), 0.0, Constant::EPSILON); 75 | ASSERT_TRUE(ctr.getFunction(jerkInit)(7)<0.0); 76 | ASSERT_TRUE(ctr.getFunction(jerkInit)(0)<0.0); 77 | ASSERT_TRUE(ctr.getFunction(jerkInit)(1)<0.0); 78 | ASSERT_TRUE(ctr.getFunction(jerkInit)(2)<0.0); 79 | 80 | jerkInit(0) = 0.0; 81 | jerkInit(1) = 0.0; 82 | jerkInit(2) = 0.0; 83 | jerkInit(3) = 6.0f*copLimitMax; 84 | ASSERT_NEAR(ctr.getFunction(jerkInit)(3), 0.0, Constant::EPSILON); 85 | ASSERT_NEAR(ctr.getFunction(jerkInit)(4), 0.0, Constant::EPSILON); 86 | ASSERT_TRUE(ctr.getFunction(jerkInit)(5)<0.0); 87 | ASSERT_TRUE(ctr.getFunction(jerkInit)(6)<0.0); 88 | ASSERT_TRUE(ctr.getFunction(jerkInit)(7)<0.0); 89 | ASSERT_TRUE(ctr.getFunction(jerkInit)(0)<0.0); 90 | ASSERT_TRUE(ctr.getFunction(jerkInit)(1)<0.0); 91 | ASSERT_TRUE(ctr.getFunction(jerkInit)(2)<0.0); 92 | 93 | jerkInit(0) = 0.0; 94 | jerkInit(1) = 0.0; 95 | jerkInit(2) = 0.0; 96 | jerkInit(3) = -6.0f*copLimitMax; 97 | ASSERT_TRUE(ctr.getFunction(jerkInit)(3)<0.0); 98 | ASSERT_TRUE(ctr.getFunction(jerkInit)(4)<0.0); 99 | ASSERT_TRUE(ctr.getFunction(jerkInit)(5)<0.0); 100 | ASSERT_TRUE(ctr.getFunction(jerkInit)(6)<0.0); 101 | ASSERT_NEAR(ctr.getFunction(jerkInit)(7), 0.0, Constant::EPSILON); 102 | ASSERT_NEAR(ctr.getFunction(jerkInit)(0), 0.0, Constant::EPSILON); 103 | ASSERT_TRUE(ctr.getFunction(jerkInit)(1)<0.0); 104 | ASSERT_TRUE(ctr.getFunction(jerkInit)(2)<0.0); 105 | 106 | } 107 | 108 | 109 | TYPED_TEST(MpcWalkgenTest, sizeOfvalues) 110 | { 111 | using namespace MPCWalkgen; 112 | TEMPLATE_TYPEDEF(TypeParam); 113 | 114 | int nbSamples = 3; 115 | TypeParam samplingPeriod = 1.0f; 116 | bool autoCompute = true; 117 | LIPModel m1(nbSamples, samplingPeriod, autoCompute); 118 | BaseModel m2(nbSamples, samplingPeriod, autoCompute); 119 | 120 | ComConstraint ctr(m1, m2); 121 | 122 | int M = m2.getComSupportConvexPolygon().getNbVertices(); 123 | 124 | VectorX jerkInit(4*nbSamples); 125 | jerkInit.fill(0.0); 126 | 127 | ASSERT_EQ(ctr.getGradient().rows(), nbSamples*M); 128 | ASSERT_EQ(ctr.getGradient().cols(), 4*nbSamples); 129 | ASSERT_EQ(ctr.getFunction(jerkInit).rows(), nbSamples*M); 130 | ASSERT_EQ(ctr.getFunction(jerkInit).cols(), 1); 131 | } 132 | -------------------------------------------------------------------------------- /test/test-zebulon-cop-constraint.cpp: -------------------------------------------------------------------------------- 1 | //////////////////////////////////////////////////////////////////////////////// 2 | /// 3 | ///\file test-zebulon-cop-constraint.cpp 4 | ///\brief Test the Zebulon CoP constraints function 5 | ///\author Lafaye Jory 6 | ///\author Barthelemy Sebastien 7 | /// 8 | //////////////////////////////////////////////////////////////////////////////// 9 | 10 | #include "mpc_walkgen_gtest.h" 11 | #include 12 | #include 13 | #include 14 | 15 | TYPED_TEST(MpcWalkgenTest, functionValue) 16 | { 17 | using namespace MPCWalkgen; 18 | TEMPLATE_TYPEDEF(TypeParam); 19 | 20 | LIPModel m1; 21 | BaseModel m2; 22 | 23 | TypeParam copLimitMin = 0.1f; 24 | TypeParam copLimitMax = 0.15f; 25 | 26 | vectorOfVector2 p(8); 27 | p[7] = Vector2(copLimitMax, 0.0); 28 | p[6] = Vector2(copLimitMin, -copLimitMin); 29 | p[5] = Vector2(0.0, -copLimitMax); 30 | p[4] = Vector2(-copLimitMin, -copLimitMin); 31 | p[3] = Vector2(-copLimitMax, 0.0); 32 | p[2] = Vector2(-copLimitMin, copLimitMin); 33 | p[1] = Vector2(0.0, copLimitMax); 34 | p[0] = Vector2(copLimitMin, copLimitMin); 35 | ConvexPolygon h(p); 36 | m2.setCopSupportConvexPolygon(h); 37 | 38 | CopConstraint ctr(m1, m2); 39 | VectorX jerkInit(4); 40 | 41 | jerkInit(0) = 0.0; 42 | jerkInit(1) = 0.0; 43 | jerkInit(2) = 0.0; 44 | jerkInit(3) = 0.0; 45 | ASSERT_TRUE(ctr.getFunction(jerkInit)(0)<0.0); 46 | ASSERT_TRUE(ctr.getFunction(jerkInit)(1)<0.0); 47 | ASSERT_TRUE(ctr.getFunction(jerkInit)(2)<0.0); 48 | ASSERT_TRUE(ctr.getFunction(jerkInit)(3)<0.0); 49 | ASSERT_TRUE(ctr.getFunction(jerkInit)(4)<0.0); 50 | ASSERT_TRUE(ctr.getFunction(jerkInit)(5)<0.0); 51 | ASSERT_TRUE(ctr.getFunction(jerkInit)(6)<0.0); 52 | ASSERT_TRUE(ctr.getFunction(jerkInit)(7)<0.0); 53 | 54 | jerkInit(0) = 0.0; 55 | jerkInit(1) = 0.0; 56 | jerkInit(2) = 6.0f*copLimitMax; 57 | jerkInit(3) = 0.0; 58 | ASSERT_TRUE(ctr.getFunction(jerkInit)(3)<0.0); 59 | ASSERT_TRUE(ctr.getFunction(jerkInit)(4)<0.0); 60 | ASSERT_TRUE(ctr.getFunction(jerkInit)(5)<0.0); 61 | ASSERT_TRUE(ctr.getFunction(jerkInit)(6)<0.0); 62 | ASSERT_TRUE(ctr.getFunction(jerkInit)(7)<0.0); 63 | ASSERT_TRUE(ctr.getFunction(jerkInit)(0)<0.0); 64 | ASSERT_NEAR(ctr.getFunction(jerkInit)(1), 0.0, Constant::EPSILON); 65 | ASSERT_NEAR(ctr.getFunction(jerkInit)(2), 0.0, Constant::EPSILON); 66 | 67 | jerkInit(0) = 0.0; 68 | jerkInit(1) = 0.0; 69 | jerkInit(2) = -6.0f*copLimitMax; 70 | jerkInit(3) = 0.0; 71 | ASSERT_TRUE(ctr.getFunction(jerkInit)(3)<0.0); 72 | ASSERT_TRUE(ctr.getFunction(jerkInit)(4)<0.0); 73 | ASSERT_NEAR(ctr.getFunction(jerkInit)(5), 0.0, Constant::EPSILON); 74 | ASSERT_NEAR(ctr.getFunction(jerkInit)(6), 0.0, Constant::EPSILON); 75 | ASSERT_TRUE(ctr.getFunction(jerkInit)(7)<0.0); 76 | ASSERT_TRUE(ctr.getFunction(jerkInit)(0)<0.0); 77 | ASSERT_TRUE(ctr.getFunction(jerkInit)(1)<0.0); 78 | ASSERT_TRUE(ctr.getFunction(jerkInit)(2)<0.0); 79 | 80 | jerkInit(0) = 0.0; 81 | jerkInit(1) = 0.0; 82 | jerkInit(2) = 0.0; 83 | jerkInit(3) = 6.0f*copLimitMax; 84 | ASSERT_NEAR(ctr.getFunction(jerkInit)(3), 0.0, Constant::EPSILON); 85 | ASSERT_NEAR(ctr.getFunction(jerkInit)(4), 0.0, Constant::EPSILON); 86 | ASSERT_TRUE(ctr.getFunction(jerkInit)(5)<0.0); 87 | ASSERT_TRUE(ctr.getFunction(jerkInit)(6)<0.0); 88 | ASSERT_TRUE(ctr.getFunction(jerkInit)(7)<0.0); 89 | ASSERT_TRUE(ctr.getFunction(jerkInit)(0)<0.0); 90 | ASSERT_TRUE(ctr.getFunction(jerkInit)(1)<0.0); 91 | ASSERT_TRUE(ctr.getFunction(jerkInit)(2)<0.0); 92 | 93 | jerkInit(0) = 0.0; 94 | jerkInit(1) = 0.0; 95 | jerkInit(2) = 0.0; 96 | jerkInit(3) = -6.0f*copLimitMax; 97 | ASSERT_TRUE(ctr.getFunction(jerkInit)(3)<0.0); 98 | ASSERT_TRUE(ctr.getFunction(jerkInit)(4)<0.0); 99 | ASSERT_TRUE(ctr.getFunction(jerkInit)(5)<0.0); 100 | ASSERT_TRUE(ctr.getFunction(jerkInit)(6)<0.0); 101 | ASSERT_NEAR(ctr.getFunction(jerkInit)(7), 0.0, Constant::EPSILON); 102 | ASSERT_NEAR(ctr.getFunction(jerkInit)(0), 0.0, Constant::EPSILON); 103 | ASSERT_TRUE(ctr.getFunction(jerkInit)(1)<0.0); 104 | ASSERT_TRUE(ctr.getFunction(jerkInit)(2)<0.0); 105 | 106 | } 107 | 108 | 109 | TYPED_TEST(MpcWalkgenTest, sizeOfvalues) 110 | { 111 | using namespace MPCWalkgen; 112 | TEMPLATE_TYPEDEF(TypeParam); 113 | 114 | int nbSamples = 3; 115 | TypeParam samplingPeriod = 1.0f; 116 | bool autoCompute = true; 117 | LIPModel m1(nbSamples, samplingPeriod, autoCompute); 118 | BaseModel m2(nbSamples, samplingPeriod, autoCompute); 119 | 120 | 121 | CopConstraint ctr(m1, m2); 122 | 123 | int M = m2.getCopSupportConvexPolygon().getNbVertices(); 124 | 125 | VectorX jerkInit(4*nbSamples); 126 | jerkInit.fill(0.0); 127 | 128 | ASSERT_EQ(ctr.getGradient().rows(), nbSamples*M); 129 | ASSERT_EQ(ctr.getGradient().cols(), 4*nbSamples); 130 | ASSERT_EQ(ctr.getFunction(jerkInit).rows(), nbSamples*M); 131 | ASSERT_EQ(ctr.getFunction(jerkInit).cols(), 1); 132 | } 133 | -------------------------------------------------------------------------------- /test/test-zebulon-jerk-minimization-objective.cpp: -------------------------------------------------------------------------------- 1 | //////////////////////////////////////////////////////////////////////////////// 2 | /// 3 | ///\file test-zebulon-jerk-minimization-objective.cpp 4 | ///\brief Test the Zebulon jerk minimization objective function 5 | ///\author Lafaye Jory 6 | ///\author Barthelemy Sebastien 7 | /// 8 | //////////////////////////////////////////////////////////////////////////////// 9 | 10 | #include "mpc_walkgen_gtest.h" 11 | #include 12 | #include 13 | #include 14 | 15 | TYPED_TEST(MpcWalkgenTest, functionValue) 16 | { 17 | using namespace MPCWalkgen; 18 | TEMPLATE_TYPEDEF(TypeParam) 19 | 20 | BaseModel m; 21 | LIPModel l; 22 | JerkMinimizationObjective obj(l, m); 23 | 24 | VectorX jerkInit(4); 25 | jerkInit.fill(1.0); 26 | 27 | ASSERT_EQ(obj.getGradient(jerkInit), jerkInit); 28 | ASSERT_EQ(obj.getHessian(), MatrixX::Identity(4, 4)); 29 | } 30 | 31 | 32 | TYPED_TEST(MpcWalkgenTest, sizeOfvalues) 33 | { 34 | using namespace MPCWalkgen; 35 | TEMPLATE_TYPEDEF(TypeParam) 36 | 37 | int nbSamples = 3; 38 | TypeParam samplingPeriod = 1.0; 39 | bool autoCompute = true; 40 | BaseModel m(nbSamples, samplingPeriod, autoCompute); 41 | LIPModel l(nbSamples, samplingPeriod, autoCompute); 42 | JerkMinimizationObjective obj(l, m); 43 | 44 | VectorX jerkInit(4*nbSamples); 45 | jerkInit.fill(0.0); 46 | 47 | ASSERT_EQ(obj.getHessian().rows(), 4*nbSamples); 48 | ASSERT_EQ(obj.getHessian().cols(), 4*nbSamples); 49 | ASSERT_EQ(obj.getGradient(jerkInit).rows(), 4*nbSamples); 50 | ASSERT_EQ(obj.getGradient(jerkInit).cols(), 1); 51 | } 52 | -------------------------------------------------------------------------------- /test/test-zebulon-tilt-motion-constraint.cpp: -------------------------------------------------------------------------------- 1 | //////////////////////////////////////////////////////////////////////////////// 2 | /// 3 | ///\file test-zebulon-tilt-motion-constraint.cpp 4 | ///\brief Test the Zebulon tilt motion constraint function 5 | ///\author Justine Lança 6 | /// 7 | //////////////////////////////////////////////////////////////////////////////// 8 | 9 | #include "mpc_walkgen_gtest.h" 10 | #include 11 | #include 12 | #include 13 | 14 | typedef float TypeParam; 15 | using namespace MPCWalkgen; 16 | 17 | TYPED_TEST(MpcWalkgenTest, initialization) 18 | { 19 | using namespace MPCWalkgen; 20 | TEMPLATE_TYPEDEF(TypeParam); 21 | 22 | LIPModel lipModel; 23 | BaseModel baseModel; 24 | TiltMotionConstraint ctr(lipModel, baseModel); 25 | 26 | const unsigned int N = 10u; 27 | lipModel.setNbSamples(N); 28 | baseModel.setNbSamples(N); 29 | ctr.computeConstantPart(); 30 | 31 | const unsigned int M = ctr.getNbConstraints(); 32 | const MatrixX gradient = ctr.getGradient(); // (M, 4*N) 33 | ASSERT_EQ(4*N, gradient.cols()); 34 | ASSERT_EQ(M, gradient.rows()); 35 | 36 | VectorX X; 37 | X.setZero(4*N); 38 | VectorX function = ctr.getFunction(X); // (M) 39 | ASSERT_EQ(M, function.rows()); 40 | 41 | // Check if gradient and function contain NaN values 42 | for (unsigned int r=0u; r < gradient.rows(); ++r) 43 | { 44 | for (unsigned int c=0u; c < gradient.cols(); ++c) 45 | { 46 | ASSERT_FALSE(gradient(r,c) != gradient(r,c)); 47 | } 48 | } 49 | 50 | for (unsigned int r=0u; r < function.rows(); ++r) 51 | { 52 | ASSERT_FALSE(function(r) != function(r)); 53 | } 54 | } 55 | -------------------------------------------------------------------------------- /test/trajectory-walkgen-bin.cpp: -------------------------------------------------------------------------------- 1 | //////////////////////////////////////////////////////////////////////////////// 2 | /// 3 | ///\author Lafaye Jory 4 | /// 5 | //////////////////////////////////////////////////////////////////////////////// 6 | 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | 14 | using namespace MPCWalkgen; 15 | typedef double Real; 16 | 17 | void initLog(std::ofstream& out) 18 | { 19 | out << "# Time pos vel acc jerk refpos refvel #" << std::endl; 20 | } 21 | 22 | void log(std::ofstream& out, const TrajectoryWalkgen& mpc, Real t) 23 | { 24 | out << t*1000. << " " 25 | << mpc.getState()(0) << " " 26 | << mpc.getState()(1) << " " 27 | << mpc.getState()(2) << " " 28 | << mpc.getJerk() << " " 29 | << 0 << " " 30 | << 0 << std::endl; 31 | } 32 | 33 | int main(int argc, char *argv[]) 34 | { 35 | if (argc!=2) 36 | { 37 | std::cout << "usage : ./trajectory-walkgen-bin " 38 | << "/home/jlafaye/devel/src/tools/motion-data/Python/experiences/motion/log" 39 | << std::endl; 40 | } 41 | 42 | 43 | 44 | std::ofstream out; 45 | out.open((std::string(argv[1])+"/Exp1000.txt").c_str()); 46 | initLog(out); 47 | 48 | int nbSamples = 20; 49 | Real mpcSamplingPeriod = 0.1; 50 | Real mpcFeedbackPeriod = 0.02; 51 | 52 | TrajectoryWalkgen walkgen; 53 | walkgen.setNbSamples(nbSamples); 54 | walkgen.setSamplingPeriod(mpcSamplingPeriod); 55 | 56 | TrajectoryWalkgenWeighting weighting; 57 | weighting.velocityTracking = 1.0; 58 | weighting.positionTracking = 0.0; 59 | weighting.jerkMinimization = 0.000001; 60 | walkgen.setWeightings(weighting); 61 | 62 | TrajectoryWalkgenConfig config; 63 | config.withMotionConstraints = true; 64 | walkgen.setConfig(config); 65 | 66 | Type::VectorX velRef(nbSamples); 67 | velRef.fill(0.2); 68 | walkgen.setVelRefInWorldFrame(velRef); 69 | 70 | Type::VectorX posRef(nbSamples); 71 | posRef.fill(0.0); 72 | walkgen.setPosRefInWorldFrame(posRef); 73 | 74 | Type::VectorX state(3); 75 | state(0) = 0.0; 76 | state(1) = 0.0; 77 | state(2) = 0.0; 78 | walkgen.setState(state); 79 | 80 | walkgen.setVelLimit(1.0); 81 | walkgen.setAccLimit(1.0); 82 | walkgen.setJerkLimit(10.0); 83 | 84 | //Real jerk = 0.; 85 | for(Real t=0.0; t<5.0; t+=mpcFeedbackPeriod) 86 | { 87 | walkgen.solve(mpcFeedbackPeriod); 88 | 89 | log(out, walkgen, t); 90 | } 91 | 92 | out.close(); 93 | 94 | return 0; 95 | } 96 | --------------------------------------------------------------------------------