nodes.cc
Go to the documentation of this file.
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 #include <towr/variables/nodes.h>
31 
32 
33 namespace towr {
34 
35 
36 Nodes::Nodes (int n_dim, const std::string& name)
37  : VariableSet(kSpecifyLater, name)
38 {
39  n_dim_ = n_dim;
40 }
41 
42 void
43 Nodes::InitMembers(int n_nodes, int n_variables)
44 {
45  nodes_ = std::vector<Node>(n_nodes, Node(n_dim_));
46  bounds_ = VecBound(n_variables, ifopt::NoBound);
47  SetRows(n_variables);
48 }
49 
50 void
52  const VectorXd& final_pos,
53  double t_total)
54 {
55  VectorXd dp = final_pos-initial_pos;
56  VectorXd average_velocity = dp/t_total;
57  int num_nodes = nodes_.size();
58  for (int i=0; i<nodes_.size(); ++i) {
59  Node n(n_dim_);
60  n.at(kPos) = initial_pos + i/static_cast<double>(num_nodes-1)*dp;
61  n.at(kVel) = average_velocity;
62  nodes_.at(i) = n;
63  }
64 }
65 
66 Nodes::IndexInfo::IndexInfo(int node_id, Dx deriv, int node_dim)
67 {
68  node_id_ = node_id;
69  node_deriv_ = deriv;
70  node_dim_ = node_dim;
71 }
72 
73 int
74 Nodes::Index(int node_id, Dx deriv, int node_dim) const
75 {
76  return Index(IndexInfo(node_id, deriv, node_dim));
77 }
78 
79 int
80 Nodes::Index(const IndexInfo& n) const
81 {
82  // could also cache this as map for more efficiency, but adding complexity
83  for (int idx=0; idx<GetRows(); ++idx)
84  for ( IndexInfo node_info : GetNodeInfoAtOptIndex(idx))
85  if ( node_info == n )
86  return idx;
87 
88  assert(false); // index representing these quantities doesn't exist
89 }
90 
91 // reverse of the above
92 std::vector<Nodes::IndexInfo>
94 {
95  std::vector<IndexInfo> nodes;
96 
97  // always two consecutive node pairs are equal
98  int n_opt_values_per_node_ = 2*n_dim_;
99  int internal_id = idx%n_opt_values_per_node_; // 0...6 (p.x, p.y, p.z, v.x, v.y. v.z)
100 
101  IndexInfo node;
102  node.node_deriv_ = internal_id<n_dim_? kPos : kVel;
103  node.node_dim_ = internal_id%n_dim_;
104  node.node_id_ = std::floor(idx/n_opt_values_per_node_);
105 
106  nodes.push_back(node);
107 
108  return nodes;
109 }
110 
113 {
114  VectorXd x(GetRows());
115 
116  for (int idx=0; idx<x.rows(); ++idx)
117  for (auto info : GetNodeInfoAtOptIndex(idx))
118  x(idx) = nodes_.at(info.node_id_).at(info.node_deriv_)(info.node_dim_);
119 
120  return x;
121 }
122 
123 void
125 {
126  for (int idx=0; idx<x.rows(); ++idx)
127  for (auto info : GetNodeInfoAtOptIndex(idx))
128  nodes_.at(info.node_id_).at(info.node_deriv_)(info.node_dim_) = x(idx);
129 
130  UpdateObservers();
131 }
132 
133 void
135 {
136  for (auto& o : observers_)
137  o->UpdateNodes();
138 }
139 
140 void
142 {
143  observers_.push_back(o);
144 }
145 
146 int
147 Nodes::GetNodeId (int poly_id, Side side)
148 {
149  return poly_id + side;
150 }
151 
152 const std::vector<Node>
153 Nodes::GetBoundaryNodes(int poly_id) const
154 {
155  std::vector<Node> nodes;
156  nodes.push_back(nodes_.at(GetNodeId(poly_id, Side::Start)));
157  nodes.push_back(nodes_.at(GetNodeId(poly_id, Side::End)));
158  return nodes;
159 }
160 
161 int
163 {
164  return n_dim_;
165 }
166 
167 int
169 {
170  return nodes_.size() - 1;
171 }
172 
173 Nodes::VecBound
175 {
176  return bounds_;
177 }
178 
179 const std::vector<Node>
181 {
182  return nodes_;
183 }
184 
185 void
186 Nodes::AddBounds(int node_id, Dx deriv,
187  const std::vector<int>& dimensions,
188  const VectorXd& val)
189 {
190  for (auto dim : dimensions)
191  AddBound(IndexInfo(node_id, deriv, dim), val(dim));
192 }
193 
194 void
195 Nodes::AddBound (const IndexInfo& node_info, double val)
196 {
197  for (int idx=0; idx<GetRows(); ++idx)
198  for (auto info : GetNodeInfoAtOptIndex(idx))
199  if (info == node_info)
200  bounds_.at(idx) = ifopt::Bounds(val, val);
201 }
202 
203 void
205  const std::vector<int>& dimensions,
206  const VectorXd& val)
207 {
208  AddBounds(0, d, dimensions, val);
209 }
210 
211 void
213  const std::vector<int>& dimensions,
214  const VectorXd& val)
215 {
216  AddBounds(nodes_.size()-1, deriv, dimensions, val);
217 }
218 
219 int
221 {
222  return (node_id_ == right.node_id_)
223  && (node_deriv_ == right.node_deriv_)
224  && (node_dim_ == right.node_dim_);
225 };
226 
227 } /* namespace towr */
void AddBound(const IndexInfo &node_info, double value)
Restricts a specific optimization variables.
Definition: nodes.cc:195
Base class to receive up-to-date values of the NodeVariables.
const VectorXd at(Dx deriv) const
Read the state value or it&#39;s derivatives by index.
Definition: state.cc:41
std::vector< Node > nodes_
Definition: nodes.h:189
VectorXd GetValues() const override
Definition: nodes.cc:112
Eigen::VectorXd VectorXd
void UpdateObservers() const
Notifies the subscribed observers that the node values changes.
Definition: nodes.cc:134
void AddObserver(ObserverPtr const spline)
Adds a dependent observer that gets notified when the nodes change.
Definition: nodes.cc:141
virtual std::vector< IndexInfo > GetNodeInfoAtOptIndex(int idx) const =0
The node information that the optimization index represents.
Definition: nodes.cc:93
void InitializeNodesTowardsGoal(const VectorXd &initial_pos, const VectorXd &final_pos, double t_total)
Sets nodes pos/vel equally spaced from initial to final position.
Definition: nodes.cc:51
int node_dim_
the dimension (x,y,z) of that optimization index.
Definition: nodes.h:64
void AddFinalBound(Dx deriv, const std::vector< int > &dimensions, const VectorXd &val)
Restricts the last node in the spline.
Definition: nodes.cc:212
int n_dim_
Definition: nodes.h:190
int Index(const IndexInfo &node_info) const
The index at which a specific node variable is stored.
Definition: nodes.cc:80
void AddBounds(int node_id, Dx deriv, const std::vector< int > &dim, const VectorXd &values)
Bounds a specific node variables.
Definition: nodes.cc:186
int GetDim() const
Definition: nodes.cc:162
void InitMembers(int n_nodes, int n_variables)
initializes the member variables.
Definition: nodes.cc:43
VecBound bounds_
the bounds on the node values.
Definition: nodes.h:176
static int GetNodeId(int poly_id, Side side)
The node ID that belongs to a specific side of a specific polynomial.
Definition: nodes.cc:147
Nodes(int n_dim, const std::string &variable_name)
Definition: nodes.cc:36
int GetPolynomialCount() const
Definition: nodes.cc:168
std::vector< ObserverPtr > observers_
Definition: nodes.h:196
Dx node_deriv_
The derivative (pos,vel) of that optimziation index.
Definition: nodes.h:63
virtual VecBound GetBounds() const override
Definition: nodes.cc:174
Holds information about the node the optimization index represents.
Definition: nodes.h:61
int node_id_
The ID of the node of the optimization index.
Definition: nodes.h:62
void SetVariables(const VectorXd &x) override
Sets the node position and velocity optimization variables.
Definition: nodes.cc:124
int operator==(const IndexInfo &right) const
Definition: nodes.cc:220
const std::vector< Node > GetBoundaryNodes(int poly_id) const
Definition: nodes.cc:153
void AddStartBound(Dx deriv, const std::vector< int > &dimensions, const VectorXd &val)
Restricts the first node in the spline.
Definition: nodes.cc:204
A node represents the state of a trajectory at a specific time.
Definition: state.h:107
Dx
< the values or derivative. For motions e.g. position, velocity, ...
Definition: state.h:41
const std::vector< Node > GetNodes() const
Definition: nodes.cc:180


towr_core
Author(s): Alexander W. Winkler
autogenerated on Sun Apr 8 2018 02:18:53