Skip to content

Commit 71b2da4

Browse files
Levi-ArmstrongLevi-Armstrong
committed
Add nodes variable set
1 parent 0840fc7 commit 71b2da4

File tree

9 files changed

+570
-1
lines changed

9 files changed

+570
-1
lines changed

‎trajopt_ifopt/CMakeLists.txt‎

Lines changed: 6 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -57,7 +57,12 @@ set(TRAJOPT_IFOPT_SOURCE_FILES
5757
src/utils/ifopt_utils.cpp
5858
src/utils/numeric_differentiation.cpp
5959
src/utils/trajopt_utils.cpp
60-
src/variable_sets/joint_position_variable.cpp)
60+
src/variable_sets/joint_position_variable.cpp
61+
src/variable_sets/node.cpp
62+
src/variable_sets/nodes_observer.cpp
63+
src/variable_sets/nodes_variables.cpp
64+
src/variable_sets/var.cpp
65+
)
6166

6267
add_library(${PROJECT_NAME} ${TRAJOPT_IFOPT_SOURCE_FILES})
6368
target_link_libraries(

‎trajopt_ifopt/include/trajopt_ifopt/variable_sets/node.h‎

Lines changed: 46 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,46 @@
1+
#ifndef TRAJOPT_IFOPT_NODE_H
2+
#define TRAJOPT_IFOPT_NODE_H
3+
4+
#include <trajopt_ifopt/variable_sets/var.h>
5+
#include <unordered_map>
6+
7+
namespace trajopt_ifopt
8+
{
9+
class NodesVariables;
10+
class Node
11+
{
12+
public:
13+
Node(std::string node_name = "Node");
14+
Node(const Node&) = delete;
15+
Node& operator=(const Node&) = delete;
16+
Node(Node&&) = default;
17+
Node& operator=(Node&&) = default;
18+
19+
const std::string& getName() const;
20+
21+
NodesVariables* getParent() const;
22+
23+
std::shared_ptr<const Var> addVar(const std::string& name);
24+
25+
std::shared_ptr<const Var> addVar(const std::string& name, const std::vector<std::string>& child_names);
26+
27+
std::shared_ptr<const Var> getVar(const std::string& name) const;
28+
29+
bool hasVar(const std::string& name) const;
30+
31+
Eigen::Index size() const;
32+
33+
void setVariables(const Eigen::Ref<const Eigen::VectorXd>& x);
34+
35+
protected:
36+
friend class NodesVariables;
37+
std::string name_;
38+
std::unordered_map<std::string, std::shared_ptr<Var>> vars_;
39+
Eigen::Index length_{ 0 };
40+
NodesVariables* parent_{ nullptr };
41+
42+
void incrementIndex(Eigen::Index value);
43+
};
44+
45+
} // namespace trajopt_ifopt
46+
#endif // TRAJOPT_IFOPT_NODE_H

‎trajopt_ifopt/include/trajopt_ifopt/variable_sets/nodes_observer.h‎

Lines changed: 69 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,69 @@
1+
/******************************************************************************
2+
Copyright (c) 2018, Alexander W. Winkler. All rights reserved.
3+
4+
Redistribution and use in source and binary forms, with or without
5+
modification, are permitted provided that the following conditions are met:
6+
7+
* Redistributions of source code must retain the above copyright notice, this
8+
list of conditions and the following disclaimer.
9+
10+
* Redistributions in binary form must reproduce the above copyright notice,
11+
this list of conditions and the following disclaimer in the documentation
12+
and/or other materials provided with the distribution.
13+
14+
* Neither the name of the copyright holder nor the names of its
15+
contributors may be used to endorse or promote products derived from
16+
this software without specific prior written permission.
17+
18+
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
19+
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
20+
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
21+
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
22+
FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
23+
DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
24+
SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
25+
CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
26+
OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
27+
OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
28+
******************************************************************************/
29+
30+
#ifndef TRAJOPT_IFOPT_NODES_OBSERVER_H
31+
#define TRAJOPT_IFOPT_NODES_OBSERVER_H
32+
33+
#include <memory>
34+
35+
namespace trajopt_ifopt
36+
{
37+
class NodesVariables;
38+
39+
/**
40+
* @brief Base class to receive up-to-date values of the NodeVariables.
41+
*
42+
* This class registers with the node variables and everytime a node changes,
43+
* the subject updates this class by calling the UpdatePolynomials() method.
44+
*
45+
* Used by spline.h
46+
*
47+
* This class implements the observer pattern:
48+
* https://sourcemaking.com/design_patterns/observer
49+
*/
50+
class NodesObserver : public std::enable_shared_from_this<NodesObserver>
51+
{
52+
public:
53+
/**
54+
* @brief Registers this observer with the subject class to receive updates.
55+
* @param node_values The subject holding the Hermite node values.
56+
*/
57+
NodesObserver(std::weak_ptr<NodesVariables> node_values);
58+
virtual ~NodesObserver() = default;
59+
60+
/**
61+
* @brief Callback method called every time the subject changes.
62+
*/
63+
virtual void UpdateNodes() = 0;
64+
65+
protected:
66+
std::weak_ptr<NodesVariables> node_values_;
67+
};
68+
} // namespace trajopt_ifopt
69+
#endif // TRAJOPT_IFOPT_NODES_OBSERVER_H

‎trajopt_ifopt/include/trajopt_ifopt/variable_sets/nodes_variables.h‎

Lines changed: 143 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,143 @@
1+
/******************************************************************************
2+
Copyright (c) 2018, Alexander W. Winkler. All rights reserved.
3+
4+
Redistribution and use in source and binary forms, with or without
5+
modification, are permitted provided that the following conditions are met:
6+
7+
* Redistributions of source code must retain the above copyright notice, this
8+
list of conditions and the following disclaimer.
9+
10+
* Redistributions in binary form must reproduce the above copyright notice,
11+
this list of conditions and the following disclaimer in the documentation
12+
and/or other materials provided with the distribution.
13+
14+
* Neither the name of the copyright holder nor the names of its
15+
contributors may be used to endorse or promote products derived from
16+
this software without specific prior written permission.
17+
18+
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
19+
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
20+
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
21+
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
22+
FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
23+
DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
24+
SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
25+
CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
26+
OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
27+
OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
28+
******************************************************************************/
29+
30+
#ifndef TRAJOPT_IFOPT_NODES_VARIABLES_H
31+
#define TRAJOPT_IFOPT_NODES_VARIABLES_H
32+
33+
#include <trajopt_common/macros.h>
34+
TRAJOPT_IGNORE_WARNINGS_PUSH
35+
#include <ifopt/variable_set.h>
36+
#include <ifopt/bounds.h>
37+
#include <Eigen/Core>
38+
TRAJOPT_IGNORE_WARNINGS_POP
39+
40+
#include <trajopt_ifopt/variable_sets/node.h>
41+
42+
namespace trajopt_ifopt
43+
{
44+
class NodesObserver;
45+
46+
class NodesVariables : public ifopt::VariableSet
47+
{
48+
public:
49+
using Ptr = std::shared_ptr<NodesVariables>;
50+
51+
/**
52+
* @brief Add node to the variable set
53+
* @param node The node to append
54+
*/
55+
void AddNode(std::unique_ptr<Node> node);
56+
57+
/**
58+
* @brief Get node based on index
59+
* @param opt_idx The node index
60+
* @return The node
61+
*/
62+
std::shared_ptr<const Node> GetNode(std::size_t opt_idx) const;
63+
64+
/**
65+
* @brief Pure optimization variables that define the nodes.
66+
*
67+
* Not all node position and velocities are independent or optimized over, so
68+
* usually the number of optimization variables is less than all nodes' pos/vel.
69+
*
70+
* @sa GetNodeInfoAtOptIndex()
71+
*/
72+
VectorXd GetValues() const override;
73+
74+
/**
75+
* @brief Sets some node positions and velocity from the optimization variables.
76+
* @param x The optimization variables.
77+
*
78+
* Not all node position and velocities are independent or optimized over, so
79+
* usually the number of optimization variables is less than
80+
* all nodes pos/vel.
81+
*
82+
* @sa GetNodeValuesInfo()
83+
*/
84+
void SetVariables(const VectorXd& x) override;
85+
86+
/**
87+
* @returns the bounds on position and velocity of each node and dimension.
88+
*/
89+
VecBound GetBounds() const override;
90+
91+
/**
92+
* @returns All the nodes that can be used to reconstruct the spline.
93+
*/
94+
std::vector<std::shared_ptr<const Node>> GetNodes() const;
95+
96+
/**
97+
* @brief Adds a dependent observer that gets notified when the nodes change.
98+
* @param spline Usually a pointer to a spline which uses the node values.
99+
*/
100+
void AddObserver(std::shared_ptr<NodesObserver> observer);
101+
102+
/**
103+
* @returns The dimensions (x,y,z) of every node.
104+
*/
105+
Eigen::Index GetDim() const;
106+
107+
protected:
108+
/**
109+
* @param n_dim The number of dimensions (x,y,..) each node has.
110+
* @param variable_name The name of the variables in the optimization problem.
111+
*/
112+
NodesVariables(const std::string& variable_name);
113+
virtual ~NodesVariables() = default;
114+
115+
Eigen::VectorXd values_;
116+
VecBound bounds_; ///< the bounds on the node values.
117+
std::vector<std::shared_ptr<Node>> nodes_;
118+
Eigen::Index n_dim_{ 0 };
119+
std::vector<std::shared_ptr<NodesObserver>> observers_;
120+
121+
/** @brief Notifies the subscribed observers that the node values changes. */
122+
void UpdateObservers();
123+
124+
// /**
125+
// * @brief Bounds a specific node variables.
126+
// * @param node_id The ID of the node to bound.
127+
// * @param deriv The derivative of the node to set.
128+
// * @param dim The dimension of the node to bound.
129+
// * @param values The values to set the bounds to.
130+
// */
131+
// void AddBounds(int node_id, Dx deriv, const std::vector<int>& dim,
132+
// const VectorXd& values);
133+
// /**
134+
// * @brief Restricts a specific optimization variables.
135+
// * @param node_info The specs of the optimization variables to restrict.
136+
// * @param value The value to set the bounds to.
137+
// */
138+
// void AddBound(const NodeValueInfo& node_info, double value);
139+
};
140+
141+
} // namespace trajopt_ifopt
142+
143+
#endif // TRAJOPT_IFOPT_NODES_VARIABLES_H

‎trajopt_ifopt/include/trajopt_ifopt/variable_sets/var.h‎

Lines changed: 106 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,106 @@
1+
#ifndef TRAJOPT_IFOPT_VAR_H
2+
#define TRAJOPT_IFOPT_VAR_H
3+
4+
#include <Eigen/Core>
5+
#include <vector>
6+
#include <memory>
7+
8+
namespace trajopt_ifopt
9+
{
10+
/**
11+
* @brief This is the class which the constraints should be storing
12+
* @details This class contains all information necessary for filling the jacobian
13+
* This was copied from the trajopt implementation of Var and VarRep
14+
*/
15+
class Var
16+
{
17+
public:
18+
Var() = default;
19+
~Var() = default;
20+
Var(Eigen::Index index, std::string name);
21+
Var(Eigen::Index index, Eigen::Index length, std::string identifier, std::vector<std::string> names);
22+
Var(const Var& other) = default;
23+
Var& operator=(const Var&) = default;
24+
Var(Var&&) = default;
25+
Var& operator=(Var&&) = default;
26+
27+
/**
28+
* @brief Set the variables. This is the full vector of variables and it will extract its variables from the full
29+
* list.
30+
* @param x The full vector of variables
31+
*/
32+
void setVariables(const Eigen::Ref<const Eigen::VectorXd>& x);
33+
34+
/**
35+
* @brief Get the variable size
36+
* @return The size
37+
*/
38+
Eigen::Index size() const;
39+
40+
template <typename T>
41+
const T& value() const
42+
{
43+
throw std::runtime_error("This should never be used");
44+
}
45+
46+
template <typename T>
47+
const T& name() const
48+
{
49+
throw std::runtime_error("This should never be used");
50+
}
51+
52+
private:
53+
friend class Node;
54+
Eigen::Index index_{ -1 };
55+
Eigen::Index length_{ -1 };
56+
std::string identifier_;
57+
std::vector<std::string> names_;
58+
Eigen::VectorXd values_;
59+
60+
/**
61+
* @brief Get the index in the full set of variables that these are stored
62+
* @return The start index
63+
*/
64+
Eigen::Index getIndex() const;
65+
66+
/**
67+
* @brief Increment the start index by the provided value
68+
* @param value The value to increment the start index by
69+
*/
70+
void incrementIndex(Eigen::Index value);
71+
};
72+
73+
template <>
74+
inline const double& Var::value() const
75+
{
76+
assert(values_.size() == 1);
77+
assert(index_ > -1);
78+
assert(length_ == 1);
79+
return values_[0];
80+
}
81+
82+
template <>
83+
inline const Eigen::VectorXd& Var::value() const
84+
{
85+
assert(index_ > -1);
86+
assert(length_ > 1);
87+
assert(names_.size() > 1);
88+
return values_;
89+
}
90+
91+
template <>
92+
inline const std::string& Var::name() const
93+
{
94+
assert(names_.size() == 1);
95+
return names_[0];
96+
}
97+
98+
template <>
99+
inline const std::vector<std::string>& Var::name() const
100+
{
101+
assert(names_.size() > 1);
102+
return names_;
103+
}
104+
} // namespace trajopt_ifopt
105+
106+
#endif // TRAJOPT_IFOPT_VAR_H

0 commit comments

Comments
 (0)