diff --git a/CONTRIBUTING.md b/CONTRIBUTING.md index edff5675f..c8e6b9268 100644 --- a/CONTRIBUTING.md +++ b/CONTRIBUTING.md @@ -1,5 +1,5 @@ -# Contributing to <img src="https://i.imgur.com/zm2nwF7.png" width="15%"/> +# Contributing to towr :+1::tada: First off, thanks for taking the time to contribute! :tada::+1: diff --git a/README.md b/README.md index 18499bea9..655630f00 100644 --- a/README.md +++ b/README.md @@ -98,13 +98,18 @@ We provide a [ROS]-wrapper for the pure cmake towr library, which adds a keyboar ```bash roslaunch towr_ros towr_ros.launch ``` - Click in the xterm terminal and hit 'o'. + Click in the xterm terminal and hit 'o'. + + [![Documentation](https://img.shields.io/badge/docs-generated-brightgreen.svg)](http://docs.ros.org/kinetic/api/towr/html/) + For more detailed information on the parameters and how to tune them according to your problem, please see the + doxygen [documentation](http://docs.ros.org/kinetic/api/towr/html/). [<img src="https://i.imgur.com/LNuyEIn.gif" />](https://www.youtube.com/embed/7dP_QTPOyQ8?rel=0&autoplay=1) ## Publications The theory behind this code can be found in this paper: + A. W. Winkler, D. Bellicoso, M. Hutter, J. Buchli, [Gait and Trajectory Optimization for Legged Systems through Phase-based End-Effector Parameterization](https://awinkler.github.io/publications), IEEE Robotics and Automation Letters (RA-L), 2018: @article{winkler18, @@ -124,13 +129,14 @@ A. W. Winkler, D. Bellicoso, M. Hutter, J. Buchli, [Gait and Trajectory Optimiza ## Authors [Alexander W. Winkler](http://awinkler.me) - Initial Work/Maintainer +The work was carried out at the following institutions: + [<img src="https://i.imgur.com/aGOnNTZ.png" height="45" />](https://www.ethz.ch/en.html "ETH Zurich") [<img src="https://i.imgur.com/uCvLs2j.png" height="45" />](http://www.adrl.ethz.ch/doku.php "Agile and Dexterous Robotics Lab") [<img src="https://i.imgur.com/gYxWH9p.png" height="45" />](http://www.rsl.ethz.ch/ "Robotic Systems Lab") ## Contributing We love pull request, whether its new constraint formulations, additional robot models, bug fixes, unit tests or updating the documentation. Please have a look at [CONTRIBUTING.md](CONTRIBUTING.md) for more information. - See here the list of [contributors](https://github.com/ethz-adrl/towr/graphs/contributors) who participated in this project. @@ -159,4 +165,3 @@ To report bugs, request features or ask questions, please have a look at [CONTRI [catkin]: http://wiki.ros.org/catkin [catkin tools]: http://catkin-tools.readthedocs.org/ [Eigen]: http://eigen.tuxfamily.org -[Fa2png]: http://fa2png.io/r/font-awesome/link/ diff --git a/towr/.gitignore b/towr/.gitignore index 378eac25d..8429f259f 100644 --- a/towr/.gitignore +++ b/towr/.gitignore @@ -1 +1,3 @@ build +doc/html/* +doc/manifest.yaml diff --git a/towr/doc/figures/towr_motion_snaps.png b/towr/doc/figures/towr_motion_snaps.png new file mode 100644 index 000000000..39e9a6faf Binary files /dev/null and b/towr/doc/figures/towr_motion_snaps.png differ diff --git a/towr/doc/figures/towr_problem.png b/towr/doc/figures/towr_problem.png new file mode 100644 index 000000000..ea534f56a Binary files /dev/null and b/towr/doc/figures/towr_problem.png differ diff --git a/towr/doc/figures/towr_problem_with_code.png b/towr/doc/figures/towr_problem_with_code.png new file mode 100644 index 000000000..e7862e7aa Binary files /dev/null and b/towr/doc/figures/towr_problem_with_code.png differ diff --git a/towr/doc/mainpage.md b/towr/doc/mainpage.md new file mode 100644 index 000000000..7dd4aeac2 --- /dev/null +++ b/towr/doc/mainpage.md @@ -0,0 +1,83 @@ +TOWR - Trajectory Optimizer for Walking Robots {#mainpage} +--------------------- + +| *TOWR - A light-weight and extensible C++ library for trajectory optimization for legged robots.* | | +| -------|------ | +| <img src="../figures/towr_motion_snaps.png" width="700px"/> | | + +This [Eigen]-based library implements variables, costs and constraints that can be +used to formulate and solve a legged locomotion optimization problem. +It has been used to generate a variety of motions such as monoped hopping, +biped walking, or a complete quadruped trotting cycle, while optimizing over +the gait and step durations in less than 100ms ([paper](https://ieeexplore.ieee.org/document/8283570/)). + +Features: + +* Inuitive formulations of variables, cost and constraints using [Eigen]. +* [ifopt] allows to choose between [Ipopt] or [Snopt] to solve the NLP. +* fast performance due to Eigen sparse matrix exploitation. +* light-weight ([~6k lines](https://i.imgur.com/gP3gv34.png) of code) makes it easy to use and extend with own formulations. +* elegant rviz visualization of motion plans using [xpp]. +* [catkin] integration (optional). + + +[TOC] + +Install {#install} +======================== +In case the binaries for you ROS distro don't exist, for more detailed +instructions as well as all dependencies, please checkout the +github +<a href="https://github.com/ethz-adrl/towr/blob/master/README.md"> +README.md +</a>. Shortcut: +\code{.sh} +sudo apt-get install ros-<ros-distro>-towr_ros +\endcode + + + +Run {#run} +========================= +To run the code, type +\code{.sh} +roslaunch towr_ros towr_ros.launch +\endcode + + +Tune {#tune} +========================== +See [parameters.h](structtowr_1_1Parameters.html#details) for +an explanation in how to tune the optimization problem. +The number of parameters to tune is relatively small (~10), however, they +do have a large impact on speed and convergence of the optimizer. + + +Paper {#paper} +======================= +It is helpful to read up on the theory underlying this code, which is explained +in this paper +[DOI: 10.1109/LRA.2018.2798285](https://doi.org/10.1109/LRA.2018.2798285). + + +Contribute {#contribute} +========================== +We love pull requests. Please see +<a href="https://github.com/ethz-adrl/towr/blob/master/CONTRIBUTING.md"> +CONTRIBUTING.md +</a> if you want to contribute. +See here the list of +[contributors](https://github.com/ethz-adrl/towr/graphs/contributors) +who participated in this project. + + +Authors {#authors} +======================= +[Alexander W. Winkler](http://awinkler.me) - Initial Developer & Maintainence + + +[ROS]: http://www.ros.org +[xpp]: http://wiki.ros.org/xpp +[catkin]: http://wiki.ros.org/catkin +[Eigen]: http://eigen.tuxfamily.org + diff --git a/towr/doc/rosdoc.yaml b/towr/doc/rosdoc.yaml new file mode 100644 index 000000000..e3fdad644 --- /dev/null +++ b/towr/doc/rosdoc.yaml @@ -0,0 +1,2 @@ +- builder: doxygen + file_patterns: '*.c *.cpp *.h *.cc *.hh *.dox *.md' \ No newline at end of file diff --git a/towr/include/towr/constraints/base_motion_constraint.h b/towr/include/towr/constraints/base_motion_constraint.h index 9f3feed8b..eb9969c8e 100644 --- a/towr/include/towr/constraints/base_motion_constraint.h +++ b/towr/include/towr/constraints/base_motion_constraint.h @@ -41,7 +41,9 @@ namespace towr { * @brief Keeps the 6D base motion in a specified range. * * In general this constraint should be avoided, since a similar affect can be - * achieved with @ref RangeOfMotion. + * achieved with RangeOfMotionConstraint. + * + * @ingroup Constraints */ class BaseMotionConstraint : public TimeDiscretizationConstraint { public: diff --git a/towr/include/towr/constraints/dynamic_constraint.h b/towr/include/towr/constraints/dynamic_constraint.h index f7316f425..b75aea27a 100644 --- a/towr/include/towr/constraints/dynamic_constraint.h +++ b/towr/include/towr/constraints/dynamic_constraint.h @@ -49,12 +49,16 @@ namespace towr { * defined by the system dynamics. * * The physics-based acceleration is influenced by the robot state as - * xdd(t) = f(x(t), xd(t), f(t)) + * + * xdd(t) = f(x(t), xd(t), f(t)) * * The constraint of the optimization variables w is then: - * g(t) = acc_spline(t) - xdd(t) - * = acc_spline(t) - xdd(x(t), xd(t), f(t)) - * = acc_spline(w) - xdd(w) + * + * g(t) = acc_spline(t) - xdd(t) + * = acc_spline(t) - xdd(x(t), xd(t), f(t)) + * = acc_spline(w) - xdd(w) + * + * @ingroup Constraints */ class DynamicConstraint : public TimeDiscretizationConstraint { public: diff --git a/towr/include/towr/constraints/force_constraint.h b/towr/include/towr/constraints/force_constraint.h index 6b42f790b..7bfd30070 100644 --- a/towr/include/towr/constraints/force_constraint.h +++ b/towr/include/towr/constraints/force_constraint.h @@ -49,6 +49,8 @@ namespace towr { * * Attention: Constraint is enforced only at the spline nodes. In between * violations of this constraint can occur. + * + * @ingroup Constraints */ class ForceConstraint : public ifopt::ConstraintSet { public: diff --git a/towr/include/towr/constraints/linear_constraint.h b/towr/include/towr/constraints/linear_constraint.h index 0a59f6b45..0b980616e 100644 --- a/towr/include/towr/constraints/linear_constraint.h +++ b/towr/include/towr/constraints/linear_constraint.h @@ -36,6 +36,8 @@ namespace towr { /** * @brief Calculates the constraint violations for linear constraints. + * + * @ingroup Constraints */ class LinearEqualityConstraint : public ifopt::ConstraintSet { public: diff --git a/towr/include/towr/constraints/range_of_motion_constraint.h b/towr/include/towr/constraints/range_of_motion_constraint.h index e64f5933d..52fab8c53 100644 --- a/towr/include/towr/constraints/range_of_motion_constraint.h +++ b/towr/include/towr/constraints/range_of_motion_constraint.h @@ -49,6 +49,8 @@ namespace towr { * This constraint calculates the position of of the contact expressed in the * current CoM frame and constrains it to lie in a box around the nominal/ * natural contact position for that leg. + * + * @ingroup Constraints */ class RangeOfMotionConstraint : public TimeDiscretizationConstraint { public: diff --git a/towr/include/towr/constraints/spline_acc_constraint.h b/towr/include/towr/constraints/spline_acc_constraint.h index 745da9761..812176890 100644 --- a/towr/include/towr/constraints/spline_acc_constraint.h +++ b/towr/include/towr/constraints/spline_acc_constraint.h @@ -42,6 +42,8 @@ namespace towr { * This is used to restrict jumps in linear and angular base accelerations, * since this would require jumps in foot positions or endeffector forces, * which aren't allowed in our formulation. + * + * @ingroup Constraints */ class SplineAccConstraint : public ifopt::ConstraintSet { public: diff --git a/towr/include/towr/constraints/swing_constraint.h b/towr/include/towr/constraints/swing_constraint.h index 91e7f5306..a88b17dbb 100644 --- a/towr/include/towr/constraints/swing_constraint.h +++ b/towr/include/towr/constraints/swing_constraint.h @@ -43,6 +43,8 @@ namespace towr { * leaves the e.g. range-of-motion in between nodes. This constraint can also * be used to force a leg lift. However, it is cleanest if the optimization * can be performed without this heuristic constraint. + * + * @ingroup Constraints */ class SwingConstraint : public ifopt::ConstraintSet { public: diff --git a/towr/include/towr/constraints/terrain_constraint.h b/towr/include/towr/constraints/terrain_constraint.h index 8743ab6ca..2aa8b98e6 100644 --- a/towr/include/towr/constraints/terrain_constraint.h +++ b/towr/include/towr/constraints/terrain_constraint.h @@ -45,6 +45,8 @@ namespace towr { * lifting during swing-phase. This is convenient. * * Attention: This is enforced only at the spline nodes. + * + * @ingroup Constraints */ class TerrainConstraint : public ifopt::ConstraintSet { public: diff --git a/towr/include/towr/constraints/time_discretization_constraint.h b/towr/include/towr/constraints/time_discretization_constraint.h index 8958801e3..94cce356e 100644 --- a/towr/include/towr/constraints/time_discretization_constraint.h +++ b/towr/include/towr/constraints/time_discretization_constraint.h @@ -44,6 +44,8 @@ namespace towr { * @ref RangeOfMotion, or @ref DynamicConstraint at specific times t along * the trajectory. This class is responsible for building the overall * Jacobian from the individual Jacobians at each time instance. + * + * @ingroup Constraints */ class TimeDiscretizationConstraint : public ifopt::ConstraintSet { public: diff --git a/towr/include/towr/constraints/total_duration_constraint.h b/towr/include/towr/constraints/total_duration_constraint.h index 6278023de..31c716302 100644 --- a/towr/include/towr/constraints/total_duration_constraint.h +++ b/towr/include/towr/constraints/total_duration_constraint.h @@ -46,6 +46,8 @@ namespace towr { * Attention: At this point last phase duration is not an optimization variable * and a way should be found to optimize over all phases while setting the * total duration by constraint and not through hard parameterization. + * + * @ingroup Constraints */ class TotalDurationConstraint : public ifopt::ConstraintSet { public: diff --git a/towr/include/towr/costs/node_cost.h b/towr/include/towr/costs/node_cost.h index 22f578b59..75a873157 100644 --- a/towr/include/towr/costs/node_cost.h +++ b/towr/include/towr/costs/node_cost.h @@ -42,6 +42,8 @@ namespace towr { /** * @brief Assigns a cost to node values. + * + * @ingroup Costs */ class NodeCost : public ifopt::CostTerm { public: diff --git a/towr/include/towr/costs/soft_constraint.h b/towr/include/towr/costs/soft_constraint.h index 82946058f..06fa26d13 100644 --- a/towr/include/towr/costs/soft_constraint.h +++ b/towr/include/towr/costs/soft_constraint.h @@ -38,11 +38,23 @@ namespace towr { * @brief Converts a constraint to a cost by weighing the quadratic violations. * * Let constraint g(x) \in R^m with upper bound b_u and lower bound b_l. - * Let g'(x) = g(x) - 0.5(b_u+b_l) = g(x) - b - * And it's derivative dg'(x)/dx = J(x). - * Define a cost as c(x) = 0.5 * g'^T * W * g', where W = diag(w1,...,wm). + * Let + * + * g'(x) = g(x) - 0.5(b_u+b_l) = g(x) - b + * + * And it's derivative + * + * dg'(x)/dx = J(x). + * + * Define a cost as + * + * c(x) = 0.5 * g'^T * W * g', where W = diag(w1,...,wm). + * * Then the gradient of the cost is defined as: - * dc(x)/dx = (g'(x)^T * W * J)^T = J^T * W * (g(x)-b). + * + * dc(x)/dx = (g'(x)^T * W * J)^T = J^T * W * (g(x)-b). + * + * @ingroup Costs */ class SoftConstraint : public ifopt::Component { public: diff --git a/towr/include/towr/models/centroidal_model.h b/towr/include/towr/models/centroidal_model.h index c7600a8ef..0cc6015d7 100644 --- a/towr/include/towr/models/centroidal_model.h +++ b/towr/include/towr/models/centroidal_model.h @@ -49,6 +49,8 @@ namespace towr { * all quantities in Cartesian space. * * \sa https://en.wikipedia.org/wiki/Newton%E2%80%93Euler_equations + * + * @ingroup Robots */ class CentroidalModel : public DynamicModel { public: diff --git a/towr/include/towr/models/dynamic_model.h b/towr/include/towr/models/dynamic_model.h index 882cef836..3f9f585d9 100644 --- a/towr/include/towr/models/dynamic_model.h +++ b/towr/include/towr/models/dynamic_model.h @@ -54,6 +54,8 @@ namespace towr { * Currently, only @ref CentroidalModel is implemented, but this can be * extended in the future to also incorporate full rigid body dynamics. This * interface to the solver should remain the same. + * + * @ingroup Robots */ class DynamicModel { public: diff --git a/towr/include/towr/models/examples/anymal_model.h b/towr/include/towr/models/examples/anymal_model.h index f0c27cd2e..7e6860315 100644 --- a/towr/include/towr/models/examples/anymal_model.h +++ b/towr/include/towr/models/examples/anymal_model.h @@ -36,6 +36,10 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. namespace towr { +/** + * \addtogroup Robots + * @{ + */ class AnymalKinematicModel : public KinematicModel { public: AnymalKinematicModel () : KinematicModel(4) @@ -61,8 +65,8 @@ class AnymalDynamicModel : public CentroidalModel { 0.946438, 1.94478, 2.01835, 0.000938112, -0.00595386, -0.00146328, 4) {} }; +/** @}*/ } // namespace towr - #endif /* TOWR_TOWR_ROS_INCLUDE_TOWR_ROS_ANYMAL_MODEL_H_ */ diff --git a/towr/include/towr/models/examples/biped_model.h b/towr/include/towr/models/examples/biped_model.h index a4d48e547..6da557239 100644 --- a/towr/include/towr/models/examples/biped_model.h +++ b/towr/include/towr/models/examples/biped_model.h @@ -36,6 +36,10 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. namespace towr { +/** + * \addtogroup Robots + * @{ + */ class BipedKinematicModel : public KinematicModel { public: BipedKinematicModel () : KinematicModel(2) @@ -58,6 +62,7 @@ class BipedDynamicModel : public CentroidalModel { 1.209,5.583,6.056,0.005,-0.190,-0.012, 2) {} }; +/** @}*/ } /* namespace towr */ diff --git a/towr/include/towr/models/examples/hyq_model.h b/towr/include/towr/models/examples/hyq_model.h index b7dc4c1e3..1cef1e28d 100644 --- a/towr/include/towr/models/examples/hyq_model.h +++ b/towr/include/towr/models/examples/hyq_model.h @@ -36,6 +36,10 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. namespace towr { +/** + * \addtogroup Robots + * @{ + */ class HyqKinematicModel : public KinematicModel { public: HyqKinematicModel () : KinematicModel(4) @@ -53,13 +57,13 @@ class HyqKinematicModel : public KinematicModel { } }; - class HyqDynamicModel : public CentroidalModel { public: HyqDynamicModel() : CentroidalModel(83, 4.26, 8.97, 9.88, -0.0063, 0.193, 0.0126, 4) {} }; +/** @}*/ } /* namespace towr */ diff --git a/towr/include/towr/models/examples/monoped_model.h b/towr/include/towr/models/examples/monoped_model.h index 2edbb6f20..c7ffd251d 100644 --- a/towr/include/towr/models/examples/monoped_model.h +++ b/towr/include/towr/models/examples/monoped_model.h @@ -35,6 +35,10 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. namespace towr { +/** + * \addtogroup Robots + * @{ + */ class MonopedKinematicModel : public KinematicModel { public: MonopedKinematicModel () : KinematicModel(1) @@ -52,8 +56,7 @@ class MonopedDynamicModel : public CentroidalModel { 1.2, 5.5, 6.0, 0.0, -0.2, -0.01, // base inertia 1) {} // number of endeffectors }; - - +/** @}*/ } /* namespace towr */ diff --git a/towr/include/towr/models/kinematic_model.h b/towr/include/towr/models/kinematic_model.h index 3f8005602..b75e6d715 100644 --- a/towr/include/towr/models/kinematic_model.h +++ b/towr/include/towr/models/kinematic_model.h @@ -42,6 +42,8 @@ namespace towr { * * This class is mainly used to formulate the @ref RangeOfMotionConstraint, * restricting each endeffector to stay inside it's kinematic range. + * + * @ingroup Robots */ class KinematicModel { public: diff --git a/towr/include/towr/models/robot_model.h b/towr/include/towr/models/robot_model.h index ec614ff67..da69f3131 100644 --- a/towr/include/towr/models/robot_model.h +++ b/towr/include/towr/models/robot_model.h @@ -38,12 +38,24 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. namespace towr { +/** + * @defgroup Robots + * @brief The kinematic and dynamic model of the robot. + * (\ref include/towr/models). + * + * These models contain all the robot specific quantities in this problem. + */ + /** * @brief Holds pointers to the robot specific kinematics and dynamics. + * + * @ingroup RobotModels */ struct RobotModel { /** * @brief Examples robots for which kinematic and dynamic models exist. + * + * @ingroup Robots */ enum Robot { Monoped, Biped, Hyq, Anymal, ROBOT_COUNT }; diff --git a/towr/include/towr/nlp_factory.h b/towr/include/towr/nlp_factory.h index 6bf2427c6..c0f8362b5 100644 --- a/towr/include/towr/nlp_factory.h +++ b/towr/include/towr/nlp_factory.h @@ -42,6 +42,32 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. namespace towr { +/** + * @defgroup Variables + * @brief Variables of the trajectory optimization problem + * (@ref include/towr/variables). + * + * These are the quantities through which the optimization problem is + * parameterized. + */ + +/** + * @defgroup Constraints + * @brief Constraints of the trajectory optimization problem. + * (@ref include/towr/constraints) + * + * These are the constraint sets that characterize legged locomotion. + */ + +/** + * @defgroup Costs + * @brief Costs of the trajectory optimization problem. + * (@ref include/towr/costs) + * + * These the the cost terms that prioritize certain solutions to the + * legged locomotion problem. + */ + /** Builds variables, cost and constraints for the legged locomotion problem. * * Abstracts the entire problem of Trajectory Optimization for walking @@ -92,7 +118,7 @@ class NlpFactory { std::vector<PhaseDurations::Ptr> MakeContactScheduleVariables() const; // constraints - ContraintPtrVec GetConstraint(ConstraintName name) const; + ContraintPtrVec GetConstraint(Parameters::ConstraintName name) const; ContraintPtrVec MakeDynamicConstraint() const; ContraintPtrVec MakeRangeOfMotionBoxConstraint() const; ContraintPtrVec MakeTotalTimeConstraint() const; @@ -103,7 +129,7 @@ class NlpFactory { ContraintPtrVec MakeBaseAccConstraint() const; // costs - CostPtrVec GetCost(const CostName& id, double weight) const; + CostPtrVec GetCost(const Parameters::CostName& id, double weight) const; CostPtrVec MakeForcesCost(double weight) const; }; diff --git a/towr/include/towr/parameters.h b/towr/include/towr/parameters.h index 53fef1689..33899402b 100644 --- a/towr/include/towr/parameters.h +++ b/towr/include/towr/parameters.h @@ -33,17 +33,118 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include <vector> #include <array> - namespace towr { -enum ConstraintName { Dynamic, EndeffectorRom, TotalTime, Terrain, - Force, Swing, BaseRom, BaseAcc }; -enum CostName { ForcesCostID }; - /** - * @brief Holds the parameters to tune the optimization problem. + * @brief The parameters to tune the optimization problem. + * + * The number of parameters to tune is relatively small (~10), however, they + * do have a large impact on speed and convergence of the optimizer. Below + * we give some insights into how different values affect the solution and + * what should be taken into consideration when tuning these. For more + * background knowledge, refer to the corresponding + * [paper](https://doi.org/10.1109/LRA.2018.2798285). + * + * | Towr problem formulation | | + * | -------|------ | + * | <img src="../figures/towr_problem_with_code.png" width="700px"/> | | + * + * ### Constraint discretization ### + * Another factor that strongly impacts the **solution time** is how + * often the DynamicConstraint and the RangeOfMotionConstraint are enforced + * along the trajectory (given by the values of @ref dt_constraint_dynamic_ + * and @ref dt_constraint_range_of_motion_). Increasing the discretization + * interval of e.g. @ref dt_constraint_range_of_motion_ to 1.5s greatly speeds + * up the process. However, if the discretization is too coarse, the motion + * of e.g. the endeffector can move way outside the bounding boxes, as long it + * comes back inside this box at the few times the constraint is actually + * enforced. This is a valid solution to the optimization problem, but of + * course physically infeasible. Therefore, when the solution shows the + * feet shooting quickly through 3D space, @ref dt_constraint_range_of_motion_ + * should probably be decreased. The same logic holds for too large + * @ref dt_constraint_dynamic_: This can produce motion that violate the + * dynamics at most times, except exactly where they are enforced. This leads + * to unphysical motions that will be impossible to track on a real system. The + * more finely the constraint discretization is set, the more sure one can be + * that the dynamic model is being respected. + * + * ### Number of optimization variables ### + * In order to **shorten the solution time**, one way is to use less polynomials, + * but each of longer duration. This can be achieved by increasing the + * @ref duration_base_polynomial_. However, the longer this duration becomes, the + * less parameters (freedom), the solver has to find a solution that + * fullfills all the constraints, so it becomes more likely that no solution + * is found. This variable is related to @ref dt_constraint_dynamic_ -- + * if the dynamic constraint should be enforced at very short intervals, + * it is often also required that the base motion has enough freedom (short + * durations) to enable this. + * + * ### Available steps ### + * One of the main reasons that the solver **fails to find a solution**, is when + * there are are not enough steps available to reach a goal position that is + * either too far away, or the terrain too complex. The number of steps per leg + * and their initial durations are set by @ref ee_phase_durations_. On the other + * hand, too many steps per leg are hardly an issue and the solver simply + * generates more short steps, but finds a solution nonetheless. Therefore: + * * push back more phases for each foot into this vector to give the + * optimizer more freedom in finding a solution. + * + * ### Jittering (and cost terms) ### + * Sometimes the solution to the optimization problem looks jerky, with the + * forces and base motion quickly jittering back and forth. This is because + * by default, we don't include any @ref costs_ in the formulation. The solver + * has too many optimization variables (degrees of freedom) to modify and only + * few constraints that restrict the motion, so these extra and unnecessary + * values can be set to extreme values. The cleanest way to counter this + * is to add a cost term that e.g. penalizes base and endeffector accelerations. + * See @ref Costs for some inspiration. We try to avoid cost terms if possible, + * as they require tuning different weighing parameters w.r.t. each other and + * make the problem slower. Other ways to remove the jittering are as follows: + * * Increase @ref duration_base_polynomial_ to remove some DoF. + * * Decrease the overall time-horizon of the optimization problem. This is + * determined by the sum of all elements in @ref ee_phase_durations_. A + * shorter time horizon means less base polynomials, therefore less + * extra degrees of freedom for the optimizer to set to extreme values. + * * Decrease @ref force_polynomials_per_stance_phase_ to have less jittering + * force profiles and therefore also less jittering base-motions. + * + * ### Optimizing the gait ### + * The solver is able to modify the gait to best achieve a given task. This + * can be turned on by calling OptimizePhaseDurations(). This can help the + * solver find a solution to terrains which cannot be crossed with the + * initialized gait. However, this optimization over the phase + * durations, which affect the entire motion of that endeffector, reduce the + * sparsity of the formulation. This increases the chances of getting stuck + * in local minima, as well as increases the computation time. Therefore, + * this option should be treated with caution. An alternative to turning on + * this option is initializing with different gaits and/or changing the + * parameters described above. */ struct Parameters { + /** + * @brief Identifiers to be used to add certain constraints to the + * optimization problem. + * + * @ingroup Constraints + */ + enum ConstraintName { Dynamic, ///< sets DynamicConstraint + EndeffectorRom, ///< sets RangeOfMotionConstraint + TotalTime, ///< sets TotalDurationConstraint + Terrain, ///< sets TerrainConstraint + Force, ///< sets ForceConstraint + Swing, ///< sets SwingConstraint + BaseRom, ///< sets BaseMotionConstraint + BaseAcc ///< sets SplineAccConstraint + }; + /** + * @brief Indentifiers to be used to add certain costs to the optimization + * problem. + * + * @ingroup Costs + */ + enum CostName { ForcesCostID ///< sets NodeCost on force nodes + }; + using CostWeights = std::vector<std::pair<CostName, double>>; using UsedConstraints = std::vector<ConstraintName>; using VecTimes = std::vector<double>; diff --git a/towr/include/towr/terrain/examples/height_map_examples.h b/towr/include/towr/terrain/examples/height_map_examples.h index 479e5532a..b752ba456 100644 --- a/towr/include/towr/terrain/examples/height_map_examples.h +++ b/towr/include/towr/terrain/examples/height_map_examples.h @@ -34,6 +34,10 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. namespace towr { +/** + * @addtogroup Terrains + * @{ + */ class FlatGround : public HeightMap { public: @@ -146,6 +150,7 @@ class ChimneyLR : public HeightMap { const double x_end2_ = x_start_+2*length_; }; +/** @}*/ } /* namespace towr */ diff --git a/towr/include/towr/terrain/height_map.h b/towr/include/towr/terrain/height_map.h index 6d377a1f3..e7b664544 100644 --- a/towr/include/towr/terrain/height_map.h +++ b/towr/include/towr/terrain/height_map.h @@ -41,6 +41,12 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. namespace towr { +/** + * @defgroup Terrains + * @brief The heightmap and slope of various terrains. + * (\ref include/towr/terrain). + */ + /** * @brief Holds the height and slope information of the terrain. * @@ -55,6 +61,8 @@ namespace towr { * The height map is used to formulate constraints such as * "foot must be touching terrain during stance phase". * @sa TerrainConstraint + * + * @ingroup Terrains */ class HeightMap { public: diff --git a/towr/include/towr/towr.h b/towr/include/towr/towr.h index 97d8ffad1..a586c9c97 100644 --- a/towr/include/towr/towr.h +++ b/towr/include/towr/towr.h @@ -43,6 +43,9 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "parameters.h" +/** + * @brief Entire code of this project is defined in this namespace. + */ namespace towr { /** diff --git a/towr/include/towr/variables/base_nodes.h b/towr/include/towr/variables/base_nodes.h index fd049f799..f79e9582a 100644 --- a/towr/include/towr/variables/base_nodes.h +++ b/towr/include/towr/variables/base_nodes.h @@ -39,6 +39,8 @@ namespace towr { * * Every node is optimized over, in contrast to PhaseNodes, where multiple * nodes in the spline are represented by the same optimization variables. + * + * @ingroup Variables */ class BaseNodes : public Nodes { public: diff --git a/towr/include/towr/variables/node_spline.h b/towr/include/towr/variables/node_spline.h index 4f51a5efb..5eca9f117 100644 --- a/towr/include/towr/variables/node_spline.h +++ b/towr/include/towr/variables/node_spline.h @@ -45,6 +45,8 @@ namespace towr { * fixed polynomial durations to construct a sequence of CubicHermitePolynomial. * For this it observers whether the node values change and then updates all the * polynomials accordingly. + * + * @ingroup Variables */ class NodeSpline : public Spline, public NodesObserver { public: diff --git a/towr/include/towr/variables/nodes.h b/towr/include/towr/variables/nodes.h index a25a9a802..b457269ac 100644 --- a/towr/include/towr/variables/nodes.h +++ b/towr/include/towr/variables/nodes.h @@ -47,6 +47,7 @@ namespace towr { * specifying a polynomial is called "Hermite". These are the node values * composed of position and velocity. * + * @ingroup Variables * @sa class CubicHermitePolynomial */ class Nodes : public ifopt::VariableSet { diff --git a/towr/include/towr/variables/phase_durations.h b/towr/include/towr/variables/phase_durations.h index f0db416f5..b5396b099 100644 --- a/towr/include/towr/variables/phase_durations.h +++ b/towr/include/towr/variables/phase_durations.h @@ -42,6 +42,8 @@ namespace towr { * This class holds the current variables determining the alternating swing- * and stance durations of one foot. These durations are then used in the * Spline together with NodeVariables to construct foot motions and forces. + * + * @ingroup Variables */ class PhaseDurations : public ifopt::VariableSet { public: diff --git a/towr/include/towr/variables/phase_nodes.h b/towr/include/towr/variables/phase_nodes.h index 6e15a8b62..4fc4187d0 100644 --- a/towr/include/towr/variables/phase_nodes.h +++ b/towr/include/towr/variables/phase_nodes.h @@ -42,6 +42,8 @@ namespace towr { * same optimization variable. This is because a foot in stance cannot * move (or a force in flight must be zero). This makes the number of * optimization variables less than the total node values. + * + * @ingroup Variables */ class PhaseNodes : public Nodes { public: diff --git a/towr/include/towr/variables/phase_spline.h b/towr/include/towr/variables/phase_spline.h index 1b8c3cde5..2297754ab 100644 --- a/towr/include/towr/variables/phase_spline.h +++ b/towr/include/towr/variables/phase_spline.h @@ -43,6 +43,8 @@ namespace towr { * the optimized phase durations to construct a sequence of * CubicHermitePolynomial. For this it observers whether one of the quantities * changed and then updates all the polynomials accordingly. + * + * @ingroup Variables */ class PhaseSpline : public NodeSpline, public PhaseDurationsObserver{ public: diff --git a/towr/include/towr/variables/polynomial.h b/towr/include/towr/variables/polynomial.h index 8260017b3..02dde22b6 100644 --- a/towr/include/towr/variables/polynomial.h +++ b/towr/include/towr/variables/polynomial.h @@ -49,6 +49,8 @@ namespace towr { * * This class is responsible for calculating the values f(t) and higher order * derivatives from the coefficient values. + * + * @ingroup Variables */ class Polynomial { public: diff --git a/towr/include/towr/variables/spline.h b/towr/include/towr/variables/spline.h index f90584f9c..358e409f3 100644 --- a/towr/include/towr/variables/spline.h +++ b/towr/include/towr/variables/spline.h @@ -41,6 +41,8 @@ namespace towr { * * This class is responsible for stitching together multiple individual * polynomials into one spline. + * + * @ingroup Variables */ class Spline { public: diff --git a/towr/include/towr/variables/spline_holder.h b/towr/include/towr/variables/spline_holder.h index 2ae4d63c7..c949e5f5f 100644 --- a/towr/include/towr/variables/spline_holder.h +++ b/towr/include/towr/variables/spline_holder.h @@ -42,6 +42,8 @@ namespace towr { * optimization variables. * * This is independent from whether they are added as optimization variables. + * + * @ingroup Variables */ struct SplineHolder { /** diff --git a/towr/include/towr/variables/state.h b/towr/include/towr/variables/state.h index 8339e7a18..c7561b473 100644 --- a/towr/include/towr/variables/state.h +++ b/towr/include/towr/variables/state.h @@ -103,6 +103,8 @@ class State { * * In this framework a node only has position and velocity values, no * acceleration. + * + * @ingroup Variables */ class Node : public State { public: diff --git a/towr/include/towr/variables/variable_names.h b/towr/include/towr/variables/variable_names.h index 32689f926..e5bfbbfe7 100644 --- a/towr/include/towr/variables/variable_names.h +++ b/towr/include/towr/variables/variable_names.h @@ -33,6 +33,9 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include <string> namespace towr { +/** + * @brief Identifiers (names) used for variables in the optimization problem. + */ namespace id { static const std::string base_lin_nodes = "base-lin"; diff --git a/towr/package.xml b/towr/package.xml index bdcde0712..56c87b101 100644 --- a/towr/package.xml +++ b/towr/package.xml @@ -26,6 +26,7 @@ <buildtool_depend>cmake</buildtool_depend> <export> <build_type>cmake</build_type> + <rosdoc config="${prefix}/doc/rosdoc.yaml"/> </export> - + </package> diff --git a/towr/src/nlp_factory.cc b/towr/src/nlp_factory.cc index 66746ac64..b2286206f 100644 --- a/towr/src/nlp_factory.cc +++ b/towr/src/nlp_factory.cc @@ -187,7 +187,7 @@ NlpFactory::ContraintPtrVec NlpFactory::GetConstraints() const { ContraintPtrVec constraints; - for (ConstraintName name : params_.constraints_) + for (auto name : params_.constraints_) for (auto c : GetConstraint(name)) constraints.push_back(c); @@ -195,17 +195,17 @@ NlpFactory::GetConstraints() const } NlpFactory::ContraintPtrVec -NlpFactory::GetConstraint (ConstraintName name) const +NlpFactory::GetConstraint (Parameters::ConstraintName name) const { switch (name) { - case Dynamic: return MakeDynamicConstraint(); - case EndeffectorRom: return MakeRangeOfMotionBoxConstraint(); - case BaseRom: return MakeBaseRangeOfMotionConstraint(); - case TotalTime: return MakeTotalTimeConstraint(); - case Terrain: return MakeTerrainConstraint(); - case Force: return MakeForceConstraint(); - case Swing: return MakeSwingConstraint(); - case BaseAcc: return MakeBaseAccConstraint(); + case Parameters::Dynamic: return MakeDynamicConstraint(); + case Parameters::EndeffectorRom: return MakeRangeOfMotionBoxConstraint(); + case Parameters::BaseRom: return MakeBaseRangeOfMotionConstraint(); + case Parameters::TotalTime: return MakeTotalTimeConstraint(); + case Parameters::Terrain: return MakeTerrainConstraint(); + case Parameters::Force: return MakeForceConstraint(); + case Parameters::Swing: return MakeSwingConstraint(); + case Parameters::BaseAcc: return MakeBaseAccConstraint(); default: throw std::runtime_error("constraint not defined!"); } } @@ -327,10 +327,10 @@ NlpFactory::GetCosts() const } NlpFactory::CostPtrVec -NlpFactory::GetCost(const CostName& name, double weight) const +NlpFactory::GetCost(const Parameters::CostName& name, double weight) const { switch (name) { - case ForcesCostID: return MakeForcesCost(weight); + case Parameters::ForcesCostID: return MakeForcesCost(weight); default: throw std::runtime_error("cost not defined!"); } } diff --git a/towr/src/towr.cc b/towr/src/towr.cc index fb33f84ee..00cfdbdd1 100644 --- a/towr/src/towr.cc +++ b/towr/src/towr.cc @@ -70,7 +70,6 @@ TOWR::SolveNLP(const ifopt::Solver::Ptr& solver) nlp_.PrintCurrent(); } - SplineHolder TOWR::GetSolution() const {