Merge pull request #136 from mayataka/devel

Devel

bindings/python/robotoc/line_search/line_search_settings.cpp

@@ -23,6 +23,8 @@ PYBIND11_MODULE(line_search_settings, m) {
     .def_readwrite("line_search_method", &LineSearchSettings::line_search_method)
     .def_readwrite("step_size_reduction_rate", &LineSearchSettings::step_size_reduction_rate)
     .def_readwrite("min_step_size", &LineSearchSettings::min_step_size)
+    .def_readwrite("filter_cost_reduction_rate", &LineSearchSettings::filter_cost_reduction_rate)
+    .def_readwrite("filter_constraint_violation_reduction_rate", &LineSearchSettings::filter_constraint_violation_reduction_rate)
     .def_readwrite("armijo_control_rate", &LineSearchSettings::armijo_control_rate)
     .def_readwrite("margin_rate", &LineSearchSettings::margin_rate)
     .def_readwrite("eps", &LineSearchSettings::eps)

bindings/python/robotoc/mpc/mpc_jump.cpp

@@ -48,6 +48,7 @@ PYBIND11_MODULE(mpc_jump, m) {
     .def("get_config_cost_handle", &MPCJump::getConfigCostHandle)
     .def("get_constraints_handle", &MPCJump::getConstraintsHandle)
     .def("get_friction_cone_handle", &MPCJump::getFrictionConeHandle)
+    .def("get_contact_wrench_cone_handle", &MPCJump::getContactWrenchConeHandle)
     .def("get_solver", &MPCJump::getSolver)
     .def("get_contact_sequence", &MPCJump::getContactSequence)
     .def("set_robot_properties", &MPCJump::setRobotProperties)

include/robotoc/line_search/line_search.hpp

@@ -64,7 +64,7 @@ class LineSearch {
 
   ///
   /// @brief Compute primal step size by fliter line search method. 
-  /// @param[in, out] dms Direct multiple shooting structure.
+  /// @param[in] dms Direct multiple shooting structure.
   /// @param[in, out] robots aligned_vector of Robot for parallel computing.
   /// @param[in] time_discretization Time discretization. 
   /// @param[in] q Initial configuration.
@@ -74,7 +74,7 @@ class LineSearch {
   /// @param[in] max_primal_step_size Maximum primal step size. 
   ///
   double computeStepSize(
-      DirectMultipleShooting& dms, aligned_vector<Robot>& robots,
+      const DirectMultipleShooting& dms, aligned_vector<Robot>& robots,
       const TimeDiscretization& time_discretization,
       const Eigen::VectorXd& q, const Eigen::VectorXd& v, const Solution& s, 
       const Direction& d, const double max_primal_step_size);
@@ -84,12 +84,6 @@ class LineSearch {
   ///
   void clearHistory();
 
-  ///
-  /// @brief Checks wheather the line search filter is empty or not. 
-  /// @return true if the filter is empty. false if not.
-  ///
-  bool isFilterEmpty() const;
-
   ///
   /// @brief Set line search settings.
   /// @param[in] settings Line search settings. 
@@ -105,40 +99,25 @@ class LineSearch {
 private:
   LineSearchFilter filter_;
   LineSearchSettings settings_;
+  DirectMultipleShooting dms_trial_;
   Solution s_trial_;
   KKTResidual kkt_residual_;
 
-  void computeCostAndViolation(
-      OCP& ocp, aligned_vector<Robot>& robots, 
-      const std::shared_ptr<ContactSequence>& contact_sequence, 
-      const Eigen::VectorXd& q, const Eigen::VectorXd& v, const Solution& s);
-
-  void computeSolutionTrial(const aligned_vector<Robot>& robots, 
-                            const TimeDiscretization& time_discretization,
-                            const Solution& s, const Direction& d, 
-                            const double step_size);
-
-  static void computeSolutionTrial(const Robot& robot, const SplitSolution& s, 
-                                   const SplitDirection& d, 
-                                   const double step_size, 
-                                   SplitSolution& s_trial,
-                                   const bool impact=false);
-
   double lineSearchFilterMethod(
-      DirectMultipleShooting& dms, aligned_vector<Robot>& robots,
+      const DirectMultipleShooting& dms, aligned_vector<Robot>& robots,
       const TimeDiscretization& time_discretization,
       const Eigen::VectorXd& q, const Eigen::VectorXd& v, const Solution& s, 
       const Direction& d, const double max_primal_step_size);
 
   double meritBacktrackingLineSearch(
-      DirectMultipleShooting& dms, aligned_vector<Robot>& robots,
+      const DirectMultipleShooting& dms, aligned_vector<Robot>& robots,
       const TimeDiscretization& time_discretization,
       const Eigen::VectorXd& q, const Eigen::VectorXd& v, const Solution& s, 
       const Direction& d, const double max_primal_step_size);
 
-  bool armijoCond(const double merit_now, const double merit_next, 
-                  const double dd, const double step_size, 
-                  const double armijo_control_rate) const;
+  bool armijoCondition(const double merit, const double merit_trial, 
+                       const double merit_directional_derivative, 
+                       const double step_size) const;
 
   double penaltyParam(const TimeDiscretization time_discretization, 
                       const Solution& s) const;

include/robotoc/line_search/line_search_filter.hpp

@@ -1,7 +1,7 @@
 #ifndef ROBOTOC_LINE_SEARCH_FILTER_HPP_
 #define ROBOTOC_LINE_SEARCH_FILTER_HPP_
 
-#include <set>
+#include <vector>
 #include <utility>
 
 
@@ -15,11 +15,13 @@ class LineSearchFilter {
 public:
   ///
   /// @brief Construct a line search filter.
-  /// @param[in] cost_reduction_rate Reduction rate of the cost. 
-  /// @param[in] constraints_reduction_rate Reduction rate of the constraints. 
+  /// @param[in] cost_reduction_rate Reduction rate of the cost. Default is 
+  /// 0.005.
+  /// @param[in] constraint_violation_reduction_rate Reduction rate of the 
+  /// constraint violation. Default is 0.005
   ///
   LineSearchFilter(const double cost_reduction_rate=0.005, 
-                   const double constraints_reduction_rate=0.005);
+                   const double constraint_violation_reduction_rate=0.005);
 
   ///
   /// @brief Default destructor. 
@@ -54,7 +56,7 @@ class LineSearchFilter {
   /// @return true if the pair of cost and constraint violation is accepted.
   /// false if not. 
   ///
-  bool isAccepted(const double cost, const double constraint_violation);
+  bool isAccepted(const double cost, const double constraint_violation) const;
 
   ///
   /// @brief Augment the pair of cost and constraint violation to the filter. 
@@ -75,8 +77,8 @@ class LineSearchFilter {
   bool isEmpty() const;
 
 private:
-  std::set<std::pair<double, double>> filter_;
-  double cost_reduction_rate_, constraints_reduction_rate_;
+  std::vector<std::pair<double, double>> filter_;
+  double cost_reduction_rate_, constraint_violation_reduction_rate_;
 
 };
 

include/robotoc/line_search/line_search_settings.hpp

@@ -38,6 +38,17 @@ struct LineSearchSettings {
   ///
   double min_step_size = 0.05;
 
+  ///
+  /// @brief The reduction rate of the cost in the filter line search method. 
+  ///
+  double filter_cost_reduction_rate = 0.005;
+
+  ///
+  /// @brief The reduction rate of the constraint violation in the filter line 
+  /// search method. 
+  ///
+  double filter_constraint_violation_reduction_rate = 0.005;
+
   ///
   /// @brief Control rate in Armijo condition. This value 
   /// sets the slope of the linear approximation in the Armijo condition.

include/robotoc/line_search/unconstr_line_search.hpp

@@ -16,6 +16,7 @@
 #include "robotoc/unconstr/unconstr_direct_multiple_shooting.hpp"
 #include "robotoc/parnmpc/unconstr_backward_correction.hpp"
 #include "robotoc/line_search/line_search_filter.hpp"
+#include "robotoc/line_search/line_search_settings.hpp"
 
 
 namespace robotoc {
@@ -30,12 +31,10 @@ class UnconstrLineSearch {
   ///
   /// @brief Construct a line search.
   /// @param[in] ocp Optimal control problem. 
-  /// @param[in] step_size_reduction_rate Reduction rate of the step size. 
-  /// Defalt is 0.75.
-  /// @param[in] min_step_size Minimum step size. Default is 0.05.
+  /// @param[in] settings Line search settings.
   ///
-  UnconstrLineSearch(const OCP& ocp, const double step_size_reduction_rate=0.75, 
-                     const double min_step_size=0.05);
+  UnconstrLineSearch(const OCP& ocp, 
+                     const LineSearchSettings& settings=LineSearchSettings());
 
   ///
   /// @brief Default constructor. 
@@ -69,16 +68,16 @@ class UnconstrLineSearch {
 
   ///
   /// @brief Compute primal step size by fliter line search method. 
-  /// @param[in, out] dms Direct multiple shooting method.
-  /// @param[in] robots aligned_vector of Robot.
+  /// @param[in] dms Direct multiple shooting method.
+  /// @param[in, out] robots aligned_vector of Robot.
   /// @param[in] time_discretization Time discretization. 
   /// @param[in] q Initial configuration.
   /// @param[in] v Initial generalized velocity.
   /// @param[in] s Solution. 
   /// @param[in] d Direction. 
   /// @param[in] max_primal_step_size Maximum primal step size. 
   ///
-  double computeStepSize(UnconstrDirectMultipleShooting& dms, 
+  double computeStepSize(const UnconstrDirectMultipleShooting& dms, 
                          aligned_vector<Robot>& robots, 
                          const std::vector<GridInfo>& time_discretization, 
                          const Eigen::VectorXd& q, const Eigen::VectorXd& v, 
@@ -87,16 +86,16 @@ class UnconstrLineSearch {
 
   ///
   /// @brief Compute primal step size by fliter line search method. 
-  /// @param[in, out] backward_correction Backward correction method.
-  /// @param[in] robots aligned_vector of Robot.
+  /// @param[in] bc Backward correction method.
+  /// @param[in, out] robots aligned_vector of Robot.
   /// @param[in] time_discretization Time discretization. 
   /// @param[in] q Initial configuration.
   /// @param[in] v Initial generalized velocity.
   /// @param[in] s Solution. 
   /// @param[in] d Direction. 
   /// @param[in] max_primal_step_size Maximum primal step size. 
   ///
-  double computeStepSize(UnconstrBackwardCorrection& backward_correction, 
+  double computeStepSize(const UnconstrBackwardCorrection& bc, 
                          aligned_vector<Robot>& robots, 
                          const std::vector<GridInfo>& time_discretization, 
                          const Eigen::VectorXd& q, const Eigen::VectorXd& v, 
@@ -108,25 +107,14 @@ class UnconstrLineSearch {
   ///
   void clearHistory();
 
-  ///
-  /// @brief Clear the line search filter. 
-  ///
-  bool isFilterEmpty() const;
-
 private:
   LineSearchFilter filter_;
-  double step_size_reduction_rate_, min_step_size_;
+  LineSearchSettings settings_;
+  UnconstrDirectMultipleShooting dms_trial_;
+  UnconstrBackwardCorrection bc_trial_;
   Solution s_trial_;
   KKTResidual kkt_residual_;
 
-  void computeSolutionTrial(const Solution& s, const Direction& d, 
-                            const double step_size);
-
-  static void computeSolutionTrial(const SplitSolution& s, 
-                                  const SplitDirection& d, 
-                                  const double step_size, 
-                                  SplitSolution& s_trial);
-
 };
 
 } // namespace robotoc 

include/robotoc/mpc/mpc_jump.hpp

@@ -25,6 +25,7 @@
 #include "robotoc/constraints/joint_torques_lower_limit.hpp"
 #include "robotoc/constraints/joint_torques_upper_limit.hpp"
 #include "robotoc/constraints/friction_cone.hpp"
+#include "robotoc/constraints/contact_wrench_cone.hpp"
 #include "robotoc/mpc/control_policy.hpp"
 
 
@@ -213,6 +214,12 @@ class MPCJump {
   ///
   std::shared_ptr<FrictionCone> getFrictionConeHandle();
 
+  ///
+  /// @brief Gets the contact wrench cone constraints handle.  
+  /// @return Shared ptr to the wrench cone constraints.
+  ///
+  std::shared_ptr<ContactWrenchCone> getContactWrenchConeHandle();
+
   ///
   /// @brief Gets the const handle of the MPC solver.  
   /// @return Const reference to the MPC solver.
@@ -253,6 +260,7 @@ class MPCJump {
 
   std::shared_ptr<ConfigurationSpaceCost> config_cost_;
   std::shared_ptr<FrictionCone> friction_cone_;
+  std::shared_ptr<ContactWrenchCone> contact_wrench_cone_;
 
   void resetMinimumDwellTimes(const double t, const double min_dt);
 

include/robotoc/ocp/direct_multiple_shooting.hpp

@@ -167,21 +167,31 @@ class DirectMultipleShooting {
   double maxDualStepSize() const;
 
   ///
-  /// @brief Computes the initial state direction. 
+  /// @brief Integrates the solution. 
   /// @param[in, out] robots aligned_vector of Robot for paralle computing.
   /// @param[in] time_discretization Time discretization. 
   /// @param[in] primal_step_size Primal step size.
   /// @param[in] dual_step_size Dual step size.
-  /// @param[in] kkt_matrix KKT matrix. 
   /// @param[in, out] d Direction. 
   /// @param[in, out] s Solution. 
   ///
   void integrateSolution(const aligned_vector<Robot>& robots,
                          const TimeDiscretization& time_discretization, 
                          const double primal_step_size,
-                         const double dual_step_size,
-                         const KKTMatrix& kkt_matrix,
-                         Direction& d, Solution& s);
+                         const double dual_step_size, Direction& d, Solution& s);
+
+  ///
+  /// @brief Integrates the primal solution. 
+  /// @param[in, out] robots aligned_vector of Robot for paralle computing.
+  /// @param[in] time_discretization Time discretization. 
+  /// @param[in] primal_step_size Primal step size.
+  /// @param[in] d Direction. 
+  /// @param[in, out] s Solution. 
+  ///
+  void integratePrimalSolution(const aligned_vector<Robot>& robots,
+                               const TimeDiscretization& time_discretization, 
+                               const double primal_step_size,
+                               const Direction& d, Solution& s);
 
   ///
   /// @brief Resizes the internal data. 

include/robotoc/parnmpc/unconstr_backward_correction.hpp

@@ -195,6 +195,19 @@ class UnconstrBackwardCorrection {
                          const double primal_step_size, 
                          const double dual_step_size, Direction& d, Solution& s);
 
+  ///
+  /// @brief Integrates the solution. 
+  /// @param[in, out] robots aligned_vector of Robot for paralle computing.
+  /// @param[in] time_discretization Time discretization. 
+  /// @param[in] primal_step_size Primal step size.
+  /// @param[in] d Direction. 
+  /// @param[in, out] s Solution. 
+  ///
+  void integratePrimalSolution(const aligned_vector<Robot>& robots,
+                               const std::vector<GridInfo>& time_discretization, 
+                               const double primal_step_size, 
+                               const Direction& d, Solution& s);
+
 private:
   int nthreads_;
   OCP ocp_;

include/robotoc/unconstr/unconstr_direct_multiple_shooting.hpp

@@ -182,8 +182,20 @@ class UnconstrDirectMultipleShooting {
   void integrateSolution(const aligned_vector<Robot>& robots,
                          const std::vector<GridInfo>& time_discretization, 
                          const double primal_step_size,
-                         const double dual_step_size,
-                         Direction& d, Solution& s);
+                         const double dual_step_size, Direction& d, Solution& s);
+
+  ///
+  /// @brief Integrates the primal solution. 
+  /// @param[in, out] robots aligned_vector of Robot for paralle computing.
+  /// @param[in] time_discretization Time discretization. 
+  /// @param[in] primal_step_size Primal step size.
+  /// @param[in] d Direction. 
+  /// @param[in, out] s Solution. 
+  ///
+  void integratePrimalSolution(const aligned_vector<Robot>& robots,
+                               const std::vector<GridInfo>& time_discretization, 
+                               const double primal_step_size,
+                               const Direction& d, Solution& s);
 
 private:
   int nthreads_;