From 33a6067fcf84c10e9ba27fe6b7f692a99ca47b8c Mon Sep 17 00:00:00 2001 From: Levi Armstrong Date: Sat, 27 Apr 2024 18:30:20 -0500 Subject: [PATCH] Add node variabel set --- trajopt_ifopt/trajopt/CMakeLists.txt | 5 +- .../trajopt/variable_sets/node.h | 34 ++++ .../trajopt/variable_sets/nodes_observer.h | 70 ++++++++ .../trajopt/variable_sets/nodes_variables.h | 143 ++++++++++++++++ .../trajopt_ifopt/trajopt/variable_sets/var.h | 159 ++++++++++++++++++ .../trajopt/src/variable_sets/node.cpp | 35 ++++ .../src/variable_sets/nodes_observer.cpp | 42 +++++ .../src/variable_sets/nodes_variables.cpp | 79 +++++++++ .../trajopt/src/variable_sets/var.cpp | 27 +++ 9 files changed, 593 insertions(+), 1 deletion(-) create mode 100644 trajopt_ifopt/trajopt/include/trajopt_ifopt/trajopt/variable_sets/node.h create mode 100644 trajopt_ifopt/trajopt/include/trajopt_ifopt/trajopt/variable_sets/nodes_observer.h create mode 100644 trajopt_ifopt/trajopt/include/trajopt_ifopt/trajopt/variable_sets/nodes_variables.h create mode 100644 trajopt_ifopt/trajopt/include/trajopt_ifopt/trajopt/variable_sets/var.h create mode 100644 trajopt_ifopt/trajopt/src/variable_sets/node.cpp create mode 100644 trajopt_ifopt/trajopt/src/variable_sets/nodes_observer.cpp create mode 100644 trajopt_ifopt/trajopt/src/variable_sets/nodes_variables.cpp create mode 100644 trajopt_ifopt/trajopt/src/variable_sets/var.cpp diff --git a/trajopt_ifopt/trajopt/CMakeLists.txt b/trajopt_ifopt/trajopt/CMakeLists.txt index 01e16562..8f182fa3 100644 --- a/trajopt_ifopt/trajopt/CMakeLists.txt +++ b/trajopt_ifopt/trajopt/CMakeLists.txt @@ -42,7 +42,10 @@ 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_variables.cpp + src/variable_sets/var.cpp) add_library(${PROJECT_NAME}_trajopt ${TRAJOPT_IFOPT_SOURCE_FILES}) target_link_libraries( diff --git a/trajopt_ifopt/trajopt/include/trajopt_ifopt/trajopt/variable_sets/node.h b/trajopt_ifopt/trajopt/include/trajopt_ifopt/trajopt/variable_sets/node.h new file mode 100644 index 00000000..0a4ebec1 --- /dev/null +++ b/trajopt_ifopt/trajopt/include/trajopt_ifopt/trajopt/variable_sets/node.h @@ -0,0 +1,34 @@ +#ifndef TRAJOPT_IFOPT_TRAJOPT_NODE_H +#define TRAJOPT_IFOPT_TRAJOPT_NODE_H + +#include +#include + +namespace trajopt_ifopt +{ +class Node +{ +public: + Node(std::string node_name = "Node"); + + Var addVar(const std::string& name); + + Var addVar(const std::string& name, const std::vector& child_names); + + Var getVar(const std::string& name) const; + + bool hasVar(const std::string& name) const; + + Eigen::Index size() const; + +protected: + friend class NodesVariables; + std::string name_; + std::unordered_map vars_; + Eigen::Index length_{ 0 }; + + void setVariables(const Eigen::Ref& x); +}; + +} // namespace trajopt_ifopt +#endif // TRAJOPT_IFOPT_TRAJOPT_NODE_H diff --git a/trajopt_ifopt/trajopt/include/trajopt_ifopt/trajopt/variable_sets/nodes_observer.h b/trajopt_ifopt/trajopt/include/trajopt_ifopt/trajopt/variable_sets/nodes_observer.h new file mode 100644 index 00000000..3a1ba2b3 --- /dev/null +++ b/trajopt_ifopt/trajopt/include/trajopt_ifopt/trajopt/variable_sets/nodes_observer.h @@ -0,0 +1,70 @@ +/****************************************************************************** +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_TRAJOPT_NODES_OBSERVER_H +#define TRAJOPT_IFOPT_TRAJOPT_NODES_OBSERVER_H + +#include + +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 the positions or + * velocities of a node change, 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: + /** + * @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 node_values); + virtual ~NodesObserver() = default; + + /** + * @brief Callback method called every time the subject changes. + */ + virtual void UpdateNodes() = 0; + +protected: + std::weak_ptr node_values_; +}; +} // namespace trajopt_ifopt +#endif // TRAJOPT_IFOPT_TRAJOPT_NODES_OBSERVER_H diff --git a/trajopt_ifopt/trajopt/include/trajopt_ifopt/trajopt/variable_sets/nodes_variables.h b/trajopt_ifopt/trajopt/include/trajopt_ifopt/trajopt/variable_sets/nodes_variables.h new file mode 100644 index 00000000..cca5ca10 --- /dev/null +++ b/trajopt_ifopt/trajopt/include/trajopt_ifopt/trajopt/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_TRAJOPT_NODES_VARIABLES_H +#define TRAJOPT_IFOPT_TRAJOPT_NODES_VARIABLES_H + +#include +TRAJOPT_IGNORE_WARNINGS_PUSH +#include +#include +#include +TRAJOPT_IGNORE_WARNINGS_POP + +#include + +namespace trajopt_ifopt +{ +class NodesObserver; + +class NodesVariables : public ifopt::VariableSet +{ +public: + using Ptr = std::shared_ptr; + + /** + * @brief Add node to the variable set + * @param node The node to append + */ + void AddNode(Node node); + + /** + * @brief Get node based on index + * @param opt_idx The node index + * @return The node + */ + 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. + */ + const std::vector 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 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> nodes_; + Eigen::Index n_dim_{ -1 }; + std::vector> 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& 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_TRAJOPT_NODES_VARIABLES_H diff --git a/trajopt_ifopt/trajopt/include/trajopt_ifopt/trajopt/variable_sets/var.h b/trajopt_ifopt/trajopt/include/trajopt_ifopt/trajopt/variable_sets/var.h new file mode 100644 index 00000000..f5eacd26 --- /dev/null +++ b/trajopt_ifopt/trajopt/include/trajopt_ifopt/trajopt/variable_sets/var.h @@ -0,0 +1,159 @@ +#ifndef TRAJOPT_IFOPT_TRAJOPT_VAR_H +#define TRAJOPT_IFOPT_TRAJOPT_VAR_H + +#include +#include +#include + +namespace trajopt_ifopt +{ +// struct VarRep +// { +// VarRep(Eigen::Index _index, std::string _name); +// VarRep(Eigen::Index _index, Eigen::Index _length, std::string _name, std::vector _child_names); + +// Eigen::Index index{-1}; +// Eigen::Index length{-1}; +// std::string name; +// std::vector child_names; +// }; + +// class Var +// { +// public: +// Var() = default; +// ~Var() = default; +// Var(std::shared_ptr var_rep); +// Var(const Var& other) = default; +// Var& operator=(const Var&) = default; +// Var(Var&&) = default; +// Var& operator=(Var&&) = default; + +// template +// T value(const Eigen::VectorXd& x) const +// { +// throw std::runtime_error("This should never be used"); +// } + +// template +// T name() const +// { +// throw std::runtime_error("This should never be used"); +// } + +// private: +// friend class Node; +// std::shared_ptr var_rep_{ nullptr }; +// }; + +// template<> +// double Var::value(const Eigen::VectorXd& x) const +// { +// assert(var_rep_->index > -1 && var_rep_->index < x.size()); +// assert(var_rep_->child_names.empty()); +// return x[var_rep_->index]; +// } + +// template<> +// Eigen::VectorXd Var::value(const Eigen::VectorXd& x) const +// { +// assert(!var_rep_->child_names.empty()); +// assert(var_rep_->index > -1 && var_rep_->index < x.size()); +// assert(var_rep_->length > -1 && (var_rep_->index + var_rep_->length) < x.size()); +// return x.segment(var_rep_->index, var_rep_->length); +// } + +// template<> +// std::string Var::name() const +// { +// assert(var_rep_->child_names.empty()); +// return var_rep_->name; +// } + +// template<> +// std::vector Var::name() const +// { +// assert(!var_rep_->child_names.empty()); +// return var_rep_->child_names; +// } + +struct VarRep +{ + VarRep(Eigen::Index _index, std::string _name); + VarRep(Eigen::Index _index, Eigen::Index _length, std::string _identifier, std::vector _names); + + Eigen::Index index{ -1 }; + Eigen::Index length{ -1 }; + std::string identifier; + std::vector names; + Eigen::VectorXd values; +}; + +class Var +{ +public: + Var() = default; + ~Var() = default; + Var(std::unique_ptr var_rep); + Var(const Var& other) = default; + Var& operator=(const Var&) = default; + Var(Var&&) = default; + Var& operator=(Var&&) = default; + + template + T value() const + { + throw std::runtime_error("This should never be used"); + } + + template + T name() const + { + throw std::runtime_error("This should never be used"); + } + + void setVariables(const Eigen::Ref& x) + { + assert(var_rep_->index > -1 && var_rep_->index < x.size()); + assert(var_rep_->length > 0 && (var_rep_->index + var_rep_->length) < x.size()); + var_rep_->values = x.segment(var_rep_->index, var_rep_->length); + } + +private: + std::shared_ptr var_rep_{ nullptr }; +}; + +template <> +inline double Var::value() const +{ + assert(var_rep_->values.size() == 1); + assert(var_rep_->index > -1); + assert(var_rep_->length == 1); + return var_rep_->values[0]; +} + +template <> +inline Eigen::VectorXd Var::value() const +{ + assert(var_rep_->index > -1); + assert(var_rep_->length > 1); + assert(var_rep_->names.size() > 1); + return var_rep_->values; +} + +template <> +inline std::string Var::name() const +{ + assert(var_rep_->names.size() == 1); + return var_rep_->names[0]; +} + +template <> +inline std::vector Var::name() const +{ + assert(var_rep_->names.size() > 1); + return var_rep_->names; +} +} // namespace trajopt_ifopt + +#endif // TRAJOPT_IFOPT_TRAJOPT_VAR_H diff --git a/trajopt_ifopt/trajopt/src/variable_sets/node.cpp b/trajopt_ifopt/trajopt/src/variable_sets/node.cpp new file mode 100644 index 00000000..404e0d50 --- /dev/null +++ b/trajopt_ifopt/trajopt/src/variable_sets/node.cpp @@ -0,0 +1,35 @@ +#include +#include + +namespace trajopt_ifopt +{ +Node::Node(std::string node_name) : name_(std::move(node_name)) {} + +Var Node::addVar(const std::string& name, const std::vector& child_names) +{ + Var var(std::make_unique(length_, child_names.size(), name, child_names)); + vars_[name] = var; + length_ += static_cast(child_names.size()); + return var; +} + +Var Node::addVar(const std::string& name) +{ + Var var(std::make_unique(length_, name)); + vars_[name] = var; + length_ += 1; + return var; +} + +Var Node::getVar(const std::string& name) const { return vars_.at(name); } + +bool Node::hasVar(const std::string& name) const { return (vars_.find(name) != vars_.end()); } + +Eigen::Index Node::size() const { return length_; } + +void Node::setVariables(const Eigen::Ref& x) +{ + for (auto& pair : vars_) + pair.second.setVariables(x); +} +} // namespace trajopt_ifopt diff --git a/trajopt_ifopt/trajopt/src/variable_sets/nodes_observer.cpp b/trajopt_ifopt/trajopt/src/variable_sets/nodes_observer.cpp new file mode 100644 index 00000000..d4238305 --- /dev/null +++ b/trajopt_ifopt/trajopt/src/variable_sets/nodes_observer.cpp @@ -0,0 +1,42 @@ +/****************************************************************************** +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. +******************************************************************************/ + +#include +#include + +namespace trajopt_ifopt +{ +NodesObserver::NodesObserver(std::weak_ptr subject) +{ + node_values_ = subject; + + // register observer to subject so this class always up-to-date + subject->AddObserver(this); +} +} // namespace trajopt_ifopt diff --git a/trajopt_ifopt/trajopt/src/variable_sets/nodes_variables.cpp b/trajopt_ifopt/trajopt/src/variable_sets/nodes_variables.cpp new file mode 100644 index 00000000..94bd4391 --- /dev/null +++ b/trajopt_ifopt/trajopt/src/variable_sets/nodes_variables.cpp @@ -0,0 +1,79 @@ +/****************************************************************************** +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. +******************************************************************************/ + +#include +#include + +namespace trajopt_ifopt +{ +NodesVariables::NodesVariables(const std::string& name) : VariableSet(kSpecifyLater, name) {} + +void NodesVariables::AddNode(Node node) +{ + Eigen::Index length = node.size(); + nodes_.emplace_back(n_dim_, std::move(node)); + n_dim_ += length; +} + +const Node& NodesVariables::GetNode(std::size_t opt_idx) const { return nodes_.at(opt_idx).second; } + +void NodesVariables::SetVariables(const VectorXd& x) +{ + for (auto& pair : nodes_) + pair.second.setVariables(x.segment(pair.first, pair.second.size())); + + values_ = x; + + UpdateObservers(); +} + +Eigen::VectorXd NodesVariables::GetValues() const { return values_; } + +void NodesVariables::UpdateObservers() +{ + for (auto& o : observers_) + o->UpdateNodes(); +} + +void NodesVariables::AddObserver(std::shared_ptr observer) { observers_.push_back(observer); } + +Eigen::Index NodesVariables::GetDim() const { return n_dim_; } + +NodesVariables::VecBound NodesVariables::GetBounds() const { return bounds_; } + +const std::vector NodesVariables::GetNodes() const +{ + std::vector nodes; + nodes.reserve(nodes_.size()); + for (const auto& pair : nodes_) + nodes.push_back(pair.second); + return nodes; +} + +} // namespace trajopt_ifopt diff --git a/trajopt_ifopt/trajopt/src/variable_sets/var.cpp b/trajopt_ifopt/trajopt/src/variable_sets/var.cpp new file mode 100644 index 00000000..28ab35f9 --- /dev/null +++ b/trajopt_ifopt/trajopt/src/variable_sets/var.cpp @@ -0,0 +1,27 @@ +#include + +namespace trajopt_ifopt +{ +// VarRep::VarRep(Eigen::Index _index, std::string _name) +// : index(_index), name(std::move(_name)) {} +// VarRep::VarRep(Eigen::Index _index, Eigen::Index _length, std::string _name, std::vector _child_names) +// : index(_index), length(_length), name(std::move(_name)), child_names(std::move(_child_names)) {} + +// Var::Var(std::shared_ptr var_rep) : var_rep_(std::move(var_rep)) {} + +VarRep::VarRep(Eigen::Index _index, std::string _name) + : index(_index), identifier(std::move(_name)), names({ identifier }), values(Eigen::VectorXd::Zero(1)) +{ +} + +VarRep::VarRep(Eigen::Index _index, Eigen::Index _length, std::string _identifier, std::vector _names) + : index(_index) + , length(_length) + , identifier(std::move(_identifier)) + , names(std::move(_names)) + , values(Eigen::VectorXd::Zero(static_cast(names.size()))) +{ +} + +Var::Var(std::unique_ptr var_rep) : var_rep_(std::move(var_rep)) {} +} // namespace trajopt_ifopt