add way to use towr in own project.

README.md

@@ -93,6 +93,7 @@ We provide a [ROS]-wrapper for the pure cmake towr library, which adds a keyboar
 
 * Use: Include in your catkin project by adding to your *CMakeLists.txt* 
  ```cmake
+ add_compile_options(-std=c++11)
  find_package(catkin COMPONENTS towr) 
  include_directories(${catkin_INCLUDE_DIRS})
  target_link_libraries(foo ${catkin_LIBRARIES})

towr_ros/CMakeLists.txt

@@ -19,8 +19,17 @@ find_package(catkin REQUIRED COMPONENTS
 add_message_files( FILES TowrCommand.msg )
 generate_messages( DEPENDENCIES std_msgs xpp_msgs )
 
-catkin_package()
 
+###################################
+## catkin specific configuration ##
+###################################
+## The catkin_package macro generates cmake config files for your package
+catkin_package(
+ INCLUDE_DIRS include
+ LIBRARIES ${PROJECT_NAME}
+ CATKIN_DEPENDS xpp_states roscpp
+ DEPENDS towr
+)
 
 ###########
 ## Build ##
@@ -30,17 +39,28 @@ include_directories(
  ${catkin_INCLUDE_DIRS}
 )
 
+# The interface to derive from for ros integration
+add_library(${PROJECT_NAME} SHARED
+ src/towr_ros_interface.cc
+)
+add_dependencies(${PROJECT_NAME} 
+ ${PROJECT_NAME}_gencpp
+)
+target_link_libraries(${PROJECT_NAME}
+ ${catkin_LIBRARIES}
+)
+
 
-## The main executable that runs TOWR
-add_executable(towr_ros 
- src/towr_app.cc
- src/towr_ros.cc
+## An example executable that runs TOWR
+add_executable(towr_ros_app 
+ src/towr_ros_app.cc
 )
-add_dependencies(towr_ros 
+add_dependencies(towr_ros_app 
  ${PROJECT_NAME}_gencpp
 )
-target_link_libraries(towr_ros
+target_link_libraries(towr_ros_app
  ${catkin_LIBRARIES}
+ ${PROJECT_NAME}
 )
 
 
@@ -104,17 +124,25 @@ target_link_libraries(rosbag_geom_msg_extractor
 #############
 # Mark library for installation
 install(
- TARGETS towr_ros 
+ TARGETS towr_ros_app
  towr_user_interface
  rviz_terrain_publisher
  goal_pose_publisher
  rosbag_traj_combiner
  rosbag_geom_msg_extractor
+ ${PROJECT_NAME}
  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
 )
 
+# Mark header files for installation
+install(
+ DIRECTORY include/${PROJECT_NAME}/
+ DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+ FILES_MATCHING PATTERN "*.h"
+)
+
 # Mark other files for installation
 install(
  DIRECTORY launch rviz bash

towr_ros/include/towr_ros/towr_ros.h → towr_ros/include/towr_ros/towr_ros_interface.h

@@ -47,21 +47,46 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 namespace towr {
 
 
-
-class TowrRos {
+/**
+ * @brief Base class to interface TOWR with a ROS GUI and RVIZ.
+ *
+ * This is very convenient to change goal states or terrains on the fly and
+ * test how your formulation holds up. A sample application implementing this
+ * interface is TowrRosApp.
+ */
+class TowrRosInterface {
 public:
  using XppVec = std::vector<xpp::RobotStateCartesian>;
  using TowrCommandMsg = towr_ros::TowrCommand;
  using Vector3d = Eigen::Vector3d;
 
 protected:
- TowrRos ();
- virtual ~TowrRos () = default;
-
- virtual void SetSolverParameters(const TowrCommandMsg& msg) = 0;
+ TowrRosInterface ();
+ virtual ~TowrRosInterface () = default;
+
+ /**
+ * @brief Sets the base state and end-effector position.
+ *
+ * As a default the endeffectors can be set to the nominal postions and
+ * the base at nominal height.
+ */
  virtual void SetTowrInitialState(const std::vector<Eigen::Vector3d>& nominal_stance) = 0;
+
+ /**
+ * @brief Formulates the actual TOWR problem to be solved
+ * @param msg User message to adjust the parameters dynamically.
+ *
+ * When formulating your own application, here you can set your specific
+ * set of constraints and variables.
+ */
  virtual Parameters GetTowrParameters(int n_ee, const TowrCommandMsg& msg) const = 0;
 
+ /**
+ * @brief Sets the parameters of the nonlinear programming solver IPOPT.
+ * @param msg User message that can be used to change the parameters.
+ */
+ virtual void SetIpoptParameters(const TowrCommandMsg& msg) = 0;
+
  HeightMap::Ptr terrain_;
  TOWR towr_;
  ifopt::IpoptSolver::Ptr solver_; // could also use SNOPT

towr_ros/launch/towr_nodes.launch

@@ -33,9 +33,9 @@
 
  <!-- start the actual NLP solver using TOWR -->
  <node 
- name="towr_ros" 
+ name="towr_ros_app" 
  pkg="towr_ros" 
- type="towr_ros" 
+ type="towr_ros_app" 
  output="screen"
  launch-prefix="$(arg gdb)"
  >

towr_ros/src/towr_app.cc → towr_ros/src/towr_ros_app.cc

@@ -27,31 +27,25 @@ OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
 OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 ******************************************************************************/
 
-#include <towr_ros/towr_ros.h>
 #include <towr/initialization/gait_generator.h>
+#include <towr_ros/towr_ros_interface.h>
 
 
 namespace towr {
 
-
-class TowrApp : public TowrRos {
+/**
+ * @brief An example application of using TOWR together with ROS.
+ *
+ * Build your own application with your own formulation using the building
+ * blocks provided in TOWR and following the example below.
+ */
+class TowrRosApp : public TowrRosInterface {
 public:
- void SetSolverParameters(const TowrCommandMsg& msg) override {
- solver_->SetOption("linear_solver", "mumps");
- solver_->SetOption("jacobian_approximation", "exact");
- solver_->SetOption("max_cpu_time", 40.0);
- solver_->SetOption("print_level", 5);
- // solver_->SetOption("derivative_test", "first-order");
- // solver_->SetOption("max_iter", 0);
-
- // modify solver parameters
- if (msg.play_initialization)
- solver_->SetOption("max_iter", 0);
- else
- solver_->SetOption("max_iter", 3000);
- }
-
- void SetTowrInitialState(const std::vector<Eigen::Vector3d>& nominal_stance_B) override {
+ /**
+ * @brief Sets the feet to nominal position on flat ground and base above.
+ */
+ void SetTowrInitialState(const std::vector<Eigen::Vector3d>& nominal_stance_B) override
+ {
  double z_ground = 0.0;
  std::vector<Eigen::Vector3d> initial_ee_pos = nominal_stance_B;
  std::for_each(initial_ee_pos.begin(), initial_ee_pos.end(),
@@ -64,8 +58,16 @@ class TowrApp : public TowrRos {
  towr_.SetInitialState(initial_base, initial_ee_pos);
  }
 
- Parameters GetTowrParameters(int n_ee, const TowrCommandMsg& msg) const override {
+ /**
+ * @brief Sets the parameters required to formulate the TOWR problem.
+ */
+ Parameters GetTowrParameters(int n_ee, const TowrCommandMsg& msg) const override
+ {
  Parameters params;
+
+ // Instead of manually defining the initial durations for each foot and
+ // step, for convenience we use a GaitGenerator with some predefined gaits
+ // for a variety of robots (walk, trot, pace, ...).
  auto gait_gen_ = GaitGenerator::MakeGaitGenerator(n_ee);
  auto id_gait = static_cast<GaitGenerator::Combos>(msg.gait);
  gait_gen_->SetCombo(id_gait);
@@ -74,21 +76,56 @@ class TowrApp : public TowrRos {
  params.ee_in_contact_at_start_.push_back(gait_gen_->IsInContactAtStart(ee));
  }
 
+ // Here you can also add other constraints or values.
+ // creates smoother swing motions, not absolutely required.
  params.SetSwingConstraint();
 
+ // increases optimization time, but sometimes helps find a solution for
+ // more difficult terrain.
  if (msg.optimize_phase_durations)
  params.OptimizePhaseDurations();
 
  return params;
  }
+
+ /**
+ * @brief Sets the paramters for IPOPT.
+ */
+ void SetIpoptParameters(const TowrCommandMsg& msg) override
+ {
+ // the HA-L solvers are alot faster, so consider installing and using
+ solver_->SetOption("linear_solver", "mumps"); // ma27, ma57
+
+ // Analytically defining the derivatives in IFOPT as we do it, makes the
+ // problem a lot faster. However, if this becomes too difficult, we can also
+ // tell IPOPT to just approximate them using finite differences. However,
+ // this uses numerical derivatives for ALL constraints, there doesn't yet
+ // exist an option to turn on numerical derivatives for only some constraint
+ // sets.
+ solver_->SetOption("jacobian_approximation", "exact"); // finite difference-values
+
+ // This is a great to test if the analytical derivatives implemented in are
+ // correct. Some derivatives that are correct are still flagged, showing a
+ // deviation of 10e-4, which is fine. What to watch out for is deviations > 10e-2.
+ // solver_->SetOption("derivative_test", "first-order");
+
+ solver_->SetOption("max_cpu_time", 40.0);
+ solver_->SetOption("print_level", 5);
+
+ if (msg.play_initialization)
+ solver_->SetOption("max_iter", 0);
+ else
+ solver_->SetOption("max_iter", 3000);
+ }
 };
 
 } // namespace towr
 
+
 int main(int argc, char *argv[])
 {
- ros::init(argc, argv, "towr_app");
- towr::TowrApp towr_app;
+ ros::init(argc, argv, "my_towr_ros_app");
+ towr::TowrRosApp towr_app;
  ros::spin();
 
  return 1;