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") &nbsp; &nbsp; &nbsp; &nbsp; [<img src="https://i.imgur.com/uCvLs2j.png" height="45" />](http://www.adrl.ethz.ch/doku.php "Agile and Dexterous Robotics Lab")  &nbsp; &nbsp; &nbsp; &nbsp;[<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
 {