Add nodes variable set

trajopt_ifopt/CMakeLists.txt

@@ -57,7 +57,12 @@ set(TRAJOPT_IFOPT_SOURCE_FILES
     src/utils/ifopt_utils.cpp
     src/utils/numeric_differentiation.cpp
     src/utils/trajopt_utils.cpp
-    src/variable_sets/joint_position_variable.cpp)
+    src/variable_sets/joint_position_variable.cpp
+    src/variable_sets/node.cpp
+    src/variable_sets/nodes_observer.cpp
+    src/variable_sets/nodes_variables.cpp
+    src/variable_sets/var.cpp
+  )
 
 add_library(${PROJECT_NAME} ${TRAJOPT_IFOPT_SOURCE_FILES})
 target_link_libraries(

trajopt_ifopt/include/trajopt_ifopt/variable_sets/node.h

@@ -0,0 +1,46 @@
+#ifndef TRAJOPT_IFOPT_NODE_H
+#define TRAJOPT_IFOPT_NODE_H
+
+#include <trajopt_ifopt/variable_sets/var.h>
+#include <unordered_map>
+
+namespace trajopt_ifopt
+{
+class NodesVariables;
+class Node
+{
+public:
+  Node(std::string node_name = "Node");
+  Node(const Node&) = delete;
+  Node& operator=(const Node&) = delete;
+  Node(Node&&) = default;
+  Node& operator=(Node&&) = default;
+
+  const std::string& getName() const;
+
+  NodesVariables* getParent() const;
+
+  std::shared_ptr<const Var> addVar(const std::string& name);
+
+  std::shared_ptr<const Var> addVar(const std::string& name, const std::vector<std::string>& child_names);
+
+  std::shared_ptr<const Var> getVar(const std::string& name) const;
+
+  bool hasVar(const std::string& name) const;
+
+  Eigen::Index size() const;
+
+  void setVariables(const Eigen::Ref<const Eigen::VectorXd>& x);
+
+protected:
+  friend class NodesVariables;
+  std::string name_;
+  std::unordered_map<std::string, std::shared_ptr<Var>> vars_;
+  Eigen::Index length_{ 0 };
+  NodesVariables* parent_{ nullptr };
+
+  void incrementIndex(Eigen::Index value);
+};
+
+}  // namespace trajopt_ifopt
+#endif  // TRAJOPT_IFOPT_NODE_H

trajopt_ifopt/include/trajopt_ifopt/variable_sets/nodes_observer.h

@@ -0,0 +1,69 @@
+/******************************************************************************
+Copyright (c) 2018, Alexander W. Winkler. All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+* Redistributions of source code must retain the above copyright notice, this
+  list of conditions and the following disclaimer.
+
+* Redistributions in binary form must reproduce the above copyright notice,
+  this list of conditions and the following disclaimer in the documentation
+  and/or other materials provided with the distribution.
+
+* Neither the name of the copyright holder nor the names of its
+  contributors may be used to endorse or promote products derived from
+  this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+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.
+******************************************************************************/
+
+#ifndef TRAJOPT_IFOPT_NODES_OBSERVER_H
+#define TRAJOPT_IFOPT_NODES_OBSERVER_H
+
+#include <memory>
+
+namespace trajopt_ifopt
+{
+class NodesVariables;
+
+/**
+ * @brief Base class to receive up-to-date values of the NodeVariables.
+ *
+ * This class registers with the node variables and everytime a node changes,
+ * the subject updates this class by calling the UpdatePolynomials() method.
+ *
+ * Used by spline.h
+ *
+ * This class implements the observer pattern:
+ * https://sourcemaking.com/design_patterns/observer
+ */
+class NodesObserver : public std::enable_shared_from_this<NodesObserver>
+{
+public:
+  /**
+   * @brief Registers this observer with the subject class to receive updates.
+   * @param node_values  The subject holding the Hermite node values.
+   */
+  NodesObserver(std::weak_ptr<NodesVariables> node_values);
+  virtual ~NodesObserver() = default;
+
+  /**
+   * @brief Callback method called every time the subject changes.
+   */
+  virtual void UpdateNodes() = 0;
+
+protected:
+  std::weak_ptr<NodesVariables> node_values_;
+};
+}  // namespace trajopt_ifopt
+#endif  // TRAJOPT_IFOPT_NODES_OBSERVER_H

trajopt_ifopt/include/trajopt_ifopt/variable_sets/nodes_variables.h

@@ -0,0 +1,143 @@
+/******************************************************************************
+Copyright (c) 2018, Alexander W. Winkler. All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+* Redistributions of source code must retain the above copyright notice, this
+  list of conditions and the following disclaimer.
+
+* Redistributions in binary form must reproduce the above copyright notice,
+  this list of conditions and the following disclaimer in the documentation
+  and/or other materials provided with the distribution.
+
+* Neither the name of the copyright holder nor the names of its
+  contributors may be used to endorse or promote products derived from
+  this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+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.
+******************************************************************************/
+
+#ifndef TRAJOPT_IFOPT_NODES_VARIABLES_H
+#define TRAJOPT_IFOPT_NODES_VARIABLES_H
+
+#include <trajopt_common/macros.h>
+TRAJOPT_IGNORE_WARNINGS_PUSH
+#include <ifopt/variable_set.h>
+#include <ifopt/bounds.h>
+#include <Eigen/Core>
+TRAJOPT_IGNORE_WARNINGS_POP
+
+#include <trajopt_ifopt/variable_sets/node.h>
+
+namespace trajopt_ifopt
+{
+class NodesObserver;
+
+class NodesVariables : public ifopt::VariableSet
+{
+public:
+  using Ptr = std::shared_ptr<NodesVariables>;
+
+  /**
+   * @brief Add node to the variable set
+   * @param node The node to append
+   */
+  void AddNode(std::unique_ptr<Node> node);
+
+  /**
+   * @brief Get node based on index
+   * @param opt_idx The node index
+   * @return The node
+   */
+  std::shared_ptr<const Node> GetNode(std::size_t opt_idx) const;
+
+  /**
+   * @brief Pure optimization variables that define the nodes.
+   *
+   * Not all node position and velocities are independent or optimized over, so
+   * usually the number of optimization variables is less than all nodes' pos/vel.
+   *
+   * @sa GetNodeInfoAtOptIndex()
+   */
+  VectorXd GetValues() const override;
+
+  /**
+   * @brief Sets some node positions and velocity from the optimization variables.
+   * @param x The optimization variables.
+   *
+   * Not all node position and velocities are independent or optimized over, so
+   * usually the number of optimization variables is less than
+   * all nodes pos/vel.
+   *
+   * @sa GetNodeValuesInfo()
+   */
+  void SetVariables(const VectorXd& x) override;
+
+  /**
+   * @returns the bounds on position and velocity of each node and dimension.
+   */
+  VecBound GetBounds() const override;
+
+  /**
+   * @returns All the nodes that can be used to reconstruct the spline.
+   */
+  std::vector<std::shared_ptr<const Node>> GetNodes() const;
+
+  /**
+   * @brief Adds a dependent observer that gets notified when the nodes change.
+   * @param spline Usually a pointer to a spline which uses the node values.
+   */
+  void AddObserver(std::shared_ptr<NodesObserver> observer);
+
+  /**
+   * @returns  The dimensions (x,y,z) of every node.
+   */
+  Eigen::Index GetDim() const;
+
+protected:
+  /**
+   * @param n_dim  The number of dimensions (x,y,..) each node has.
+   * @param variable_name  The name of the variables in the optimization problem.
+   */
+  NodesVariables(const std::string& variable_name);
+  virtual ~NodesVariables() = default;
+
+  Eigen::VectorXd values_;
+  VecBound bounds_;  ///< the bounds on the node values.
+  std::vector<std::shared_ptr<Node>> nodes_;
+  Eigen::Index n_dim_{ 0 };
+  std::vector<std::shared_ptr<NodesObserver>> observers_;
+
+  /** @brief Notifies the subscribed observers that the node values changes. */
+  void UpdateObservers();
+
+  // /**
+  //  * @brief Bounds a specific node variables.
+  //  * @param node_id  The ID of the node to bound.
+  //  * @param deriv    The derivative of the node to set.
+  //  * @param dim      The dimension of the node to bound.
+  //  * @param values   The values to set the bounds to.
+  //  */
+  // void AddBounds(int node_id, Dx deriv, const std::vector<int>& dim,
+  //                const VectorXd& values);
+  // /**
+  //  * @brief Restricts a specific optimization variables.
+  //  * @param node_info The specs of the optimization variables to restrict.
+  //  * @param value     The value to set the bounds to.
+  //  */
+  // void AddBound(const NodeValueInfo& node_info, double value);
+};
+
+}  // namespace trajopt_ifopt
+
+#endif  // TRAJOPT_IFOPT_NODES_VARIABLES_H

trajopt_ifopt/include/trajopt_ifopt/variable_sets/var.h

@@ -0,0 +1,106 @@
+#ifndef TRAJOPT_IFOPT_VAR_H
+#define TRAJOPT_IFOPT_VAR_H
+
+#include <Eigen/Core>
+#include <vector>
+#include <memory>
+
+namespace trajopt_ifopt
+{
+/**
+ * @brief This is the class which the constraints should be storing
+ * @details This class contains all information necessary for filling the jacobian
+ * This was copied from the trajopt implementation of Var and VarRep
+ */
+class Var
+{
+public:
+  Var() = default;
+  ~Var() = default;
+  Var(Eigen::Index index, std::string name);
+  Var(Eigen::Index index, Eigen::Index length, std::string identifier, std::vector<std::string> names);
+  Var(const Var& other) = default;
+  Var& operator=(const Var&) = default;
+  Var(Var&&) = default;
+  Var& operator=(Var&&) = default;
+
+  /**
+   * @brief Set the variables. This is the full vector of variables and it will extract its variables from the full
+   * list.
+   * @param x The full vector of variables
+   */
+  void setVariables(const Eigen::Ref<const Eigen::VectorXd>& x);
+
+  /**
+   * @brief Get the variable size
+   * @return The size
+   */
+  Eigen::Index size() const;
+
+  template <typename T>
+  const T& value() const
+  {
+    throw std::runtime_error("This should never be used");
+  }
+
+  template <typename T>
+  const T& name() const
+  {
+    throw std::runtime_error("This should never be used");
+  }
+
+private:
+  friend class Node;
+  Eigen::Index index_{ -1 };
+  Eigen::Index length_{ -1 };
+  std::string identifier_;
+  std::vector<std::string> names_;
+  Eigen::VectorXd values_;
+
+  /**
+   * @brief Get the index in the full set of variables that these are stored
+   * @return The start index
+   */
+  Eigen::Index getIndex() const;
+
+  /**
+   * @brief Increment the start index by the provided value
+   * @param value The value to increment the start index by
+   */
+  void incrementIndex(Eigen::Index value);
+};
+
+template <>
+inline const double& Var::value() const
+{
+  assert(values_.size() == 1);
+  assert(index_ > -1);
+  assert(length_ == 1);
+  return values_[0];
+}
+
+template <>
+inline const Eigen::VectorXd& Var::value() const
+{
+  assert(index_ > -1);
+  assert(length_ > 1);
+  assert(names_.size() > 1);
+  return values_;
+}
+
+template <>
+inline const std::string& Var::name() const
+{
+  assert(names_.size() == 1);
+  return names_[0];
+}
+
+template <>
+inline const std::vector<std::string>& Var::name() const
+{
+  assert(names_.size() > 1);
+  return names_;
+}
+}  // namespace trajopt_ifopt
+
+#endif  // TRAJOPT_IFOPT_VAR_H