Planning: added initial definition of piecewise linear constraint.

modules/planning/math/smoothing_spline/piecewise_linear_constraint.cc

+/******************************************************************************
+ * Copyright 2017 The Apollo Authors. All Rights Reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *****************************************************************************/
+
+/**
+ * @file
+ * @brief: Implementation of PiecewiseLinearConstraint class
+ **/
+
+#include "modules/planning/math/smoothing_spline/piecewise_linear_constraint.h"
+
+#include <limits>
+
+namespace apollo {
+namespace planning {
+
+PiecewiseLinearConstraint::PiecewiseLinearConstraint(const uint32_t dimension)
+    : dimension_(dimension) {}
+
+bool PiecewiseLinearConstraint::AddBoundary(
+    const std::vector<uint32_t>& index_list,
+    const std::vector<double>& lower_bound,
+    const std::vector<double>& upper_bound) {
+  // TODO(Liangliang): implement this function
+  return true;
+}
+
+bool PiecewiseLinearConstraint::AddDerivativeBoundary(
+    const std::vector<uint32_t>& index_list,
+    const std::vector<double>& lower_bound,
+    const std::vector<double>& upper_bound) {
+  // TODO(Liangliang): implement this function
+  return true;
+}
+
+bool PiecewiseLinearConstraint::AddSecondDerivativeBoundary(
+    const std::vector<uint32_t>& index_list,
+    const std::vector<double>& lower_bound,
+    const std::vector<double>& upper_bound) {
+  // TODO(Liangliang): implement this function
+  return true;
+}
+
+bool PiecewiseLinearConstraint::AddThirdDerivativeBoundary(
+    const std::vector<uint32_t>& index_list,
+    const std::vector<double>& lower_bound,
+    const std::vector<double>& upper_bound) {
+  // TODO(Liangliang): implement this function
+  return true;
+}
+
+bool PiecewiseLinearConstraint::AddPointConstraint(const double x,
+                                                   const double fx) {
+  // TODO(Liangliang): implement this function
+  return true;
+}
+
+bool PiecewiseLinearConstraint::AddPointDerivativeConstraint(const double x,
+                                                             const double dfx) {
+  // TODO(Liangliang): implement this function
+  return true;
+}
+
+bool PiecewiseLinearConstraint::AddPointSecondDerivativeConstraint(
+    const double x, const double ddfx) {
+  // TODO(Liangliang): implement this function
+  return true;
+}
+
+bool PiecewiseLinearConstraint::AddPointThirdDerivativeConstraint(
+    const double x, const double dddfx) {
+  // TODO(Liangliang): implement this function
+  return true;
+}
+
+bool PiecewiseLinearConstraint::AddMonotoneInequalityConstraint() {
+  // TODO(Liangliang): implement this function
+  return true;
+}
+
+}  // namespace planning
+}  // namespace apollo

modules/planning/math/smoothing_spline/piecewise_linear_constraint.h

@@ -22,107 +22,54 @@
 #ifndef MODULES_PLANNING_MATH_SMOOTHING_SPLINE_PIECEWISE_LINEAR_CONSTRAINT_H_
 #define MODULES_PLANNING_MATH_SMOOTHING_SPLINE_PIECEWISE_LINEAR_CONSTRAINT_H_
 
-#include <algorithm>
 #include <vector>
-#include "Eigen/Core"
 
-#include "modules/planning/math/smoothing_spline/affine_constraint.h"
-#include "modules/planning/math/smoothing_spline/spline_1d.h"
+#include "Eigen/Core"
 
 namespace apollo {
 namespace planning {
 
-class Spline1dConstraint {
+class PiecewiseLinearConstraint {
  public:
-  explicit Spline1dConstraint(const Spline1d& pss);
-  Spline1dConstraint(const std::vector<double>& x_knots,
-                     const std::uint32_t order);
+  PiecewiseLinearConstraint(const uint32_t dimension);
+  virtual ~PiecewiseLinearConstraint() = default;
 
-  // direct methods
-  bool AddInequalityConstraint(const Eigen::MatrixXd& constraint_matrix,
-                               const Eigen::MatrixXd& constraint_boundary);
-  bool AddEqualityConstraint(const Eigen::MatrixXd& constraint_matrix,
-                             const Eigen::MatrixXd& constraint_boundary);
-
-  // preset method
   /**
-  *   @brief: inequality boundary constraints
-  *   if no boundary, do specify either by std::infinity or let vector.size() =
-  *0
-  **/
-  bool AddBoundary(const std::vector<double>& x_coord,
+   * @brief: inequality boundary constraints
+   **/
+  bool AddBoundary(const std::vector<uint32_t>& index_list,
                    const std::vector<double>& lower_bound,
                    const std::vector<double>& upper_bound);
 
-  bool AddDerivativeBoundary(const std::vector<double>& x_coord,
+  bool AddDerivativeBoundary(const std::vector<uint32_t>& index_list,
                              const std::vector<double>& lower_bound,
                              const std::vector<double>& upper_bound);
 
-  bool AddSecondDerivativeBoundary(const std::vector<double>& x_coord,
+  bool AddSecondDerivativeBoundary(const std::vector<uint32_t>& index_list,
                                    const std::vector<double>& lower_bound,
                                    const std::vector<double>& upper_bound);
 
-  bool AddThirdDerivativeBoundary(const std::vector<double>& x_coord,
+  bool AddThirdDerivativeBoundary(const std::vector<uint32_t>& index_list,
                                   const std::vector<double>& lower_bound,
                                   const std::vector<double>& upper_bound);
 
   /**
-  *   @brief: equality constraint to guarantee joint smoothness
-  **/
-  // boundary equality constriant
-  // constraint on fx, dfx, ddfx ... in vector form; upto third order
+   * @brief: equality constraints
+   **/
   bool AddPointConstraint(const double x, const double fx);
   bool AddPointDerivativeConstraint(const double x, const double dfx);
   bool AddPointSecondDerivativeConstraint(const double x, const double ddfx);
   bool AddPointThirdDerivativeConstraint(const double x, const double dddfx);
 
-  // guarantee upto values are joint
-  bool AddSmoothConstraint();
-
-  // guarantee upto derivative are joint
-  bool AddDerivativeSmoothConstraint();
-
-  // guarantee upto second order derivative are joint
-  bool AddSecondDerivativeSmoothConstraint();
-
-  // guarantee upto third order derivative are joint
-  bool AddThirdDerivativeSmoothConstraint();
-
   /**
-  *   @brief: Add monotone constraint inequality, guarantee the monotone city at
-  *evaluated point
-  **/
-
-  // customized monotone inequality constraint at x_coord
-  bool AddMonotoneInequalityConstraint(const std::vector<double>& x_coord);
-
-  // default inequality constraint at knots
-  bool AddMonotoneInequalityConstraintAtKnots();
-
-  /**
-  *   @brief: output interface inequality constraint
-  **/
-  const AffineConstraint& inequality_constraint() const;
-  const AffineConstraint& equality_constraint() const;
-
- private:
-  std::uint32_t FindIndex(const double x) const;
-
-  bool FilterConstraints(const std::vector<double>& x_coord,
-                         const std::vector<double>& lower_bound,
-                         const std::vector<double>& upper_bound,
-                         std::vector<double>* const filtered_lower_bound_x,
-                         std::vector<double>* const filtered_lower_bound,
-                         std::vector<double>* const filtered_upper_bound_x,
-                         std::vector<double>* const filtered_upper_bound);
-  void GeneratePowerX(const double x, const std::uint32_t order,
-                      std::vector<double>* const power_x) const;
+   * @brief: Add monotone constraint inequality at all indices
+   **/
+  bool AddMonotoneInequalityConstraint();
 
  private:
-  AffineConstraint inequality_constraint_;
-  AffineConstraint equality_constraint_;
-  std::vector<double> x_knots_;
-  std::uint32_t spline_order_;
+  const uint32_t dimension_;
+  Eigen::MatrixXd inequality_constraint_;
+  Eigen::MatrixXd equality_constraint_;
 };
 
 }  // namespace planning