From 39a9da623bca31695931d5c7902b554fc25be250 Mon Sep 17 00:00:00 2001 From: Levi Armstrong Date: Tue, 7 Jan 2025 17:19:58 -0600 Subject: [PATCH] Add nodes variable set --- trajopt_ifopt/CMakeLists.txt | 6 +- .../trajopt_ifopt/variable_sets/node.h | 46 ++++++ .../variable_sets/nodes_observer.h | 69 +++++++++ .../variable_sets/nodes_variables.h | 143 ++++++++++++++++++ .../include/trajopt_ifopt/variable_sets/var.h | 106 +++++++++++++ trajopt_ifopt/src/variable_sets/node.cpp | 45 ++++++ .../src/variable_sets/nodes_observer.cpp | 45 ++++++ .../src/variable_sets/nodes_variables.cpp | 80 ++++++++++ trajopt_ifopt/src/variable_sets/var.cpp | 30 ++++ 9 files changed, 569 insertions(+), 1 deletion(-) create mode 100644 trajopt_ifopt/include/trajopt_ifopt/variable_sets/node.h create mode 100644 trajopt_ifopt/include/trajopt_ifopt/variable_sets/nodes_observer.h create mode 100644 trajopt_ifopt/include/trajopt_ifopt/variable_sets/nodes_variables.h create mode 100644 trajopt_ifopt/include/trajopt_ifopt/variable_sets/var.h create mode 100644 trajopt_ifopt/src/variable_sets/node.cpp create mode 100644 trajopt_ifopt/src/variable_sets/nodes_observer.cpp create mode 100644 trajopt_ifopt/src/variable_sets/nodes_variables.cpp create mode 100644 trajopt_ifopt/src/variable_sets/var.cpp diff --git a/trajopt_ifopt/CMakeLists.txt b/trajopt_ifopt/CMakeLists.txt index a541acd3..bce47339 100644 --- a/trajopt_ifopt/CMakeLists.txt +++ b/trajopt_ifopt/CMakeLists.txt @@ -57,7 +57,11 @@ 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( diff --git a/trajopt_ifopt/include/trajopt_ifopt/variable_sets/node.h b/trajopt_ifopt/include/trajopt_ifopt/variable_sets/node.h new file mode 100644 index 00000000..04a0cb32 --- /dev/null +++ b/trajopt_ifopt/include/trajopt_ifopt/variable_sets/node.h @@ -0,0 +1,46 @@ +#ifndef TRAJOPT_IFOPT_NODE_H +#define TRAJOPT_IFOPT_NODE_H + +#include +#include + +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 addVar(const std::string& name); + + std::shared_ptr addVar(const std::string& name, const std::vector& child_names); + + std::shared_ptr getVar(const std::string& name) const; + + bool hasVar(const std::string& name) const; + + Eigen::Index size() const; + + void setVariables(const Eigen::Ref& x); + +protected: + friend class NodesVariables; + std::string name_; + std::unordered_map> vars_; + Eigen::Index length_{ 0 }; + NodesVariables* parent_{ nullptr }; + + void incrementIndex(Eigen::Index value); +}; + +} // namespace trajopt_ifopt +#endif // TRAJOPT_IFOPT_NODE_H diff --git a/trajopt_ifopt/include/trajopt_ifopt/variable_sets/nodes_observer.h b/trajopt_ifopt/include/trajopt_ifopt/variable_sets/nodes_observer.h new file mode 100644 index 00000000..ad97f3bf --- /dev/null +++ b/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 + +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 +{ +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_NODES_OBSERVER_H diff --git a/trajopt_ifopt/include/trajopt_ifopt/variable_sets/nodes_variables.h b/trajopt_ifopt/include/trajopt_ifopt/variable_sets/nodes_variables.h new file mode 100644 index 00000000..9a363d4f --- /dev/null +++ b/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_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(std::unique_ptr node); + + /** + * @brief Get node based on index + * @param opt_idx The node index + * @return The node + */ + std::shared_ptr 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> 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_{ 0 }; + 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_NODES_VARIABLES_H diff --git a/trajopt_ifopt/include/trajopt_ifopt/variable_sets/var.h b/trajopt_ifopt/include/trajopt_ifopt/variable_sets/var.h new file mode 100644 index 00000000..1e466ce9 --- /dev/null +++ b/trajopt_ifopt/include/trajopt_ifopt/variable_sets/var.h @@ -0,0 +1,106 @@ +#ifndef TRAJOPT_IFOPT_VAR_H +#define TRAJOPT_IFOPT_VAR_H + +#include +#include +#include + +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 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& x); + + /** + * @brief Get the variable size + * @return The size + */ + Eigen::Index size() const; + + template + const T& value() const + { + throw std::runtime_error("This should never be used"); + } + + template + 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 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& Var::name() const +{ + assert(names_.size() > 1); + return names_; +} +} // namespace trajopt_ifopt + +#endif // TRAJOPT_IFOPT_VAR_H diff --git a/trajopt_ifopt/src/variable_sets/node.cpp b/trajopt_ifopt/src/variable_sets/node.cpp new file mode 100644 index 00000000..7e5828d1 --- /dev/null +++ b/trajopt_ifopt/src/variable_sets/node.cpp @@ -0,0 +1,45 @@ +#include +#include +#include + +namespace trajopt_ifopt +{ +Node::Node(std::string node_name) : name_(std::move(node_name)) {} + +const std::string& Node::getName() const { return name_; } +trajopt_ifopt::NodesVariables* Node::getParent() const { return parent_; } +std::shared_ptr Node::addVar(const std::string& name, + const std::vector& child_names) +{ + auto var = std::make_shared(length_, child_names.size(), name, child_names); + vars_[name] = var; + length_ += static_cast(child_names.size()); + return var; +} + +std::shared_ptr Node::addVar(const std::string& name) +{ + auto var = std::make_shared(length_, name); + vars_[name] = var; + length_ += 1; + return var; +} + +std::shared_ptr 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); +} + +void Node::incrementIndex(Eigen::Index value) +{ + for (auto& pair : vars_) + pair.second->incrementIndex(value); +} +} // namespace trajopt_ifopt diff --git a/trajopt_ifopt/src/variable_sets/nodes_observer.cpp b/trajopt_ifopt/src/variable_sets/nodes_observer.cpp new file mode 100644 index 00000000..e646d438 --- /dev/null +++ b/trajopt_ifopt/src/variable_sets/nodes_observer.cpp @@ -0,0 +1,45 @@ +/****************************************************************************** +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; + + auto subject_shared = subject.lock(); // Convert weak_ptr to shared_ptr + if (subject_shared) + subject_shared->AddObserver(shared_from_this()); // register observer to subject so this class always up-to-date + else // Handle the case where the weak_ptr is expired + throw std::runtime_error("Failed to lock weak_ptr; the object might have been destroyed."); +} +} // namespace trajopt_ifopt diff --git a/trajopt_ifopt/src/variable_sets/nodes_variables.cpp b/trajopt_ifopt/src/variable_sets/nodes_variables.cpp new file mode 100644 index 00000000..6e37ad1d --- /dev/null +++ b/trajopt_ifopt/src/variable_sets/nodes_variables.cpp @@ -0,0 +1,80 @@ +/****************************************************************************** +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(std::unique_ptr node) +{ + node->incrementIndex(n_dim_); + node->parent_ = this; + Eigen::Index length = node->size(); + nodes_.emplace_back(std::move(node)); + n_dim_ += length; +} + +std::shared_ptr NodesVariables::GetNode(std::size_t opt_idx) const { return nodes_.at(opt_idx); } + +void NodesVariables::SetVariables(const VectorXd& x) +{ + for (auto& node : nodes_) + node->setVariables(x); + + 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(std::move(observer)); } + +Eigen::Index NodesVariables::GetDim() const { return n_dim_; } + +NodesVariables::VecBound NodesVariables::GetBounds() const { return bounds_; } + +std::vector > NodesVariables::GetNodes() const +{ + std::vector > nodes; + nodes.reserve(nodes_.size()); + std::copy(nodes_.begin(), nodes_.end(), std::back_inserter(nodes)); + return nodes; +} + +} // namespace trajopt_ifopt diff --git a/trajopt_ifopt/src/variable_sets/var.cpp b/trajopt_ifopt/src/variable_sets/var.cpp new file mode 100644 index 00000000..692e21f2 --- /dev/null +++ b/trajopt_ifopt/src/variable_sets/var.cpp @@ -0,0 +1,30 @@ +#include + +namespace trajopt_ifopt +{ +Var::Var(Eigen::Index index, std::string name) + : index_(index), identifier_(std::move(name)), names_({ identifier_ }), values_(Eigen::VectorXd::Zero(1)) +{ +} + +Var::Var(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()))) +{ +} + +Eigen::Index Var::getIndex() const { return index_; } +Eigen::Index Var::size() const { return length_; } + +void Var::incrementIndex(Eigen::Index value) { index_ += value; } +void Var::setVariables(const Eigen::Ref& x) +{ + assert(index_ > -1 && index_ < x.size()); + assert(length_ > 0 && (index_ + length_) < x.size()); + values_ = x.segment(index_, length_); +} + +} // namespace trajopt_ifopt