fixup

trajopt_ifopt/trajopt/include/trajopt_ifopt/trajopt/variable_sets/node.h

@@ -3,14 +3,22 @@
 
 #include <trajopt_ifopt/trajopt/variable_sets/var.h>
 #include <unordered_map>
+#include <boost/uuid/uuid.hpp>
 
 namespace trajopt_ifopt
 {
+class NodesVariables;
 class Node
 {
 public:
   Node(std::string node_name = "Node");
 
+  boost::uuids::uuid getUUID() const;
+
+  const std::string& getName() const;
+
+  NodesVariables* const getParent() const;
+
   Var addVar(const std::string& name);
 
   Var addVar(const std::string& name, const std::vector<std::string>& child_names);
@@ -23,11 +31,14 @@ class Node
 
 protected:
   friend class NodesVariables;
+  boost::uuids::uuid uuid_{};
   std::string name_;
   std::unordered_map<std::string, Var> vars_;
   Eigen::Index length_{ 0 };
+  NodesVariables* parent_{ nullptr };
 
   void setVariables(const Eigen::Ref<const Eigen::VectorXd>& x);
+  void incrementIndex(Eigen::Index value);
 };
 
 }  // namespace trajopt_ifopt

trajopt_ifopt/trajopt/include/trajopt_ifopt/trajopt/variable_sets/nodes_variables.h

@@ -91,7 +91,7 @@ class NodesVariables : public ifopt::VariableSet
   /**
    * @returns All the nodes that can be used to reconstruct the spline.
    */
-  const std::vector<Node> GetNodes() const;
+  std::vector<Node> GetNodes() const;
 
   /**
    * @brief Adds a dependent observer that gets notified when the nodes change.
@@ -114,8 +114,8 @@ class NodesVariables : public ifopt::VariableSet
 
   Eigen::VectorXd values_;
   VecBound bounds_;  ///< the bounds on the node values.
-  std::vector<std::pair<Eigen::Index, Node>> nodes_;
-  Eigen::Index n_dim_{ -1 };
+  std::vector<Node> nodes_;
+  Eigen::Index n_dim_{ 0 };
   std::vector<std::shared_ptr<NodesObserver>> observers_;
 
   /** @brief Notifies the subscribed observers that the node values changes. */

trajopt_ifopt/trajopt/include/trajopt_ifopt/trajopt/variable_sets/var.h

@@ -89,6 +89,10 @@ struct VarRep
   Eigen::VectorXd values;
 };
 
+/**
+ * @brief This is the class which the constraints should be storing
+ * @details This class contains all informtion necessary for filling the jacobian
+ */
 class Var
 {
 public:
@@ -100,6 +104,9 @@ class Var
   Var(Var&&) = default;
   Var& operator=(Var&&) = default;
 
+  Eigen::Index getIndex() const { return var_rep_->index; }
+  Eigen::Index getLength() const { return var_rep_->length; }
+
   template <typename T>
   T value() const
   {
@@ -120,6 +127,7 @@ class Var
   }
 
 private:
+  friend class Node;
   std::shared_ptr<VarRep> var_rep_{ nullptr };
 };
 

trajopt_ifopt/trajopt/src/variable_sets/node.cpp

@@ -1,10 +1,15 @@
 #include <trajopt_ifopt/trajopt/variable_sets/node.h>
+#include <trajopt_ifopt/trajopt/variable_sets/nodes_variables.h>
 #include <memory>
+#include <boost/uuid/uuid_generators.hpp>
 
 namespace trajopt_ifopt
 {
-Node::Node(std::string node_name) : name_(std::move(node_name)) {}
+Node::Node(std::string node_name) : uuid_(boost::uuids::random_generator()()), name_(std::move(node_name)) {}
 
+boost::uuids::uuid Node::getUUID() const { return uuid_; }
+const std::string& Node::getName() const { return name_; }
+NodesVariables* const Node::getParent() const { return parent_; }
 Var Node::addVar(const std::string& name, const std::vector<std::string>& child_names)
 {
   Var var(std::make_unique<VarRep>(length_, child_names.size(), name, child_names));
@@ -32,4 +37,9 @@ void Node::setVariables(const Eigen::Ref<const Eigen::VectorXd>& x)
   for (auto& pair : vars_)
     pair.second.setVariables(x);
 }
+void Node::incrementIndex(Eigen::Index value)
+{
+  for (auto& pair : vars_)
+    pair.second.var_rep_->index += value;
+}
 }  // namespace trajopt_ifopt

trajopt_ifopt/trajopt/src/variable_sets/nodes_variables.cpp

@@ -36,17 +36,24 @@ NodesVariables::NodesVariables(const std::string& name) : VariableSet(kSpecifyLa
 
 void NodesVariables::AddNode(Node node)
 {
+  auto it =
+      std::find_if(nodes_.begin(), nodes_.begin(), [&node](const Node& n) { return n.getUUID() == node.getUUID(); });
+  if (it != nodes_.end())
+    throw std::runtime_error("NodesVariables, Node must be unique!");
+
+  node.incrementIndex(n_dim_);
+  node.parent_ = this;
   Eigen::Index length = node.size();
-  nodes_.emplace_back(n_dim_, std::move(node));
+  nodes_.emplace_back(std::move(node));
   n_dim_ += length;
 }
 
-const Node& NodesVariables::GetNode(std::size_t opt_idx) const { return nodes_.at(opt_idx).second; }
+const Node& NodesVariables::GetNode(std::size_t opt_idx) const { return nodes_.at(opt_idx); }
 
 void NodesVariables::SetVariables(const VectorXd& x)
 {
-  for (auto& pair : nodes_)
-    pair.second.setVariables(x.segment(pair.first, pair.second.size()));
+  for (auto& node : nodes_)
+    node.setVariables(x);
 
   values_ = x;
 
@@ -61,19 +68,12 @@ void NodesVariables::UpdateObservers()
     o->UpdateNodes();
 }
 
-void NodesVariables::AddObserver(std::shared_ptr<NodesObserver> observer) { observers_.push_back(observer); }
+void NodesVariables::AddObserver(std::shared_ptr<NodesObserver> observer) { observers_.push_back(std::move(observer)); }
 
 Eigen::Index NodesVariables::GetDim() const { return n_dim_; }
 
 NodesVariables::VecBound NodesVariables::GetBounds() const { return bounds_; }
 
-const std::vector<Node> NodesVariables::GetNodes() const
-{
-  std::vector<Node> nodes;
-  nodes.reserve(nodes_.size());
-  for (const auto& pair : nodes_)
-    nodes.push_back(pair.second);
-  return nodes;
-}
+std::vector<Node> NodesVariables::GetNodes() const { return nodes_; }
 
 }  // namespace trajopt_ifopt