nodes_variables.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 
31 
32 namespace towr {
33 
34 
35 NodesVariables::NodesVariables (const std::string& name)
36  : VariableSet(kSpecifyLater, name)
37 {
38 }
39 
40 int
42 {
43  // could also cache this as map for more efficiency, but adding complexity
44  for (int idx=0; idx<GetRows(); ++idx)
45  for ( NodeValueInfo nvi : GetNodeValuesInfo(idx))
46  if ( nvi == nvi_des )
47  return idx;
48 
49  return NodeValueNotOptimized; // index representing these quantities doesn't exist
50 }
51 
54 {
55  VectorXd x(GetRows());
56 
57  for (int idx=0; idx<x.rows(); ++idx)
58  for (auto nvi : GetNodeValuesInfo(idx))
59  x(idx) = nodes_.at(nvi.id_).at(nvi.deriv_)(nvi.dim_);
60 
61  return x;
62 }
63 
64 void
66 {
67  for (int idx=0; idx<x.rows(); ++idx)
68  for (auto nvi : GetNodeValuesInfo(idx))
69  nodes_.at(nvi.id_).at(nvi.deriv_)(nvi.dim_) = x(idx);
70 
72 }
73 
74 void
76 {
77  for (auto& o : observers_)
78  o->UpdateNodes();
79 }
80 
81 void
83 {
84  observers_.push_back(o);
85 }
86 
87 int
88 NodesVariables::GetNodeId (int poly_id, Side side)
89 {
90  return poly_id + side;
91 }
92 
93 const std::vector<Node>
95 {
96  std::vector<Node> nodes;
97  nodes.push_back(nodes_.at(GetNodeId(poly_id, Side::Start)));
98  nodes.push_back(nodes_.at(GetNodeId(poly_id, Side::End)));
99  return nodes;
100 }
101 
102 int
104 {
105  return n_dim_;
106 }
107 
108 int
110 {
111  return nodes_.size() - 1;
112 }
113 
116 {
117  return bounds_;
118 }
119 
120 const std::vector<Node>
122 {
123  return nodes_;
124 }
125 
126 void
128  const VectorXd& final_val,
129  double t_total)
130 {
131  // only set those that are part of optimization variables,
132  // do not overwrite phase-based parameterization
133  VectorXd dp = final_val-initial_val;
134  VectorXd average_velocity = dp / t_total;
135  int num_nodes = nodes_.size();
136 
137  for (int idx=0; idx<GetRows(); ++idx) {
138  for (auto nvi : GetNodeValuesInfo(idx)) {
139 
140  if (nvi.deriv_ == kPos) {
141  VectorXd pos = initial_val + nvi.id_/static_cast<double>(num_nodes-1)*dp;
142  nodes_.at(nvi.id_).at(kPos)(nvi.dim_) = pos(nvi.dim_);
143  }
144 
145  if (nvi.deriv_ == kVel) {
146  nodes_.at(nvi.id_).at(kVel)(nvi.dim_) = average_velocity(nvi.dim_);
147  }
148  }
149  }
150 }
151 
152 void
153 NodesVariables::AddBounds(int node_id, Dx deriv,
154  const std::vector<int>& dimensions,
155  const VectorXd& val)
156 {
157  for (auto dim : dimensions)
158  AddBound(NodeValueInfo(node_id, deriv, dim), val(dim));
159 }
160 
161 void
162 NodesVariables::AddBound (const NodeValueInfo& nvi_des, double val)
163 {
164  for (int idx=0; idx<GetRows(); ++idx)
165  for (auto nvi : GetNodeValuesInfo(idx))
166  if (nvi == nvi_des)
167  bounds_.at(idx) = ifopt::Bounds(val, val);
168 }
169 
170 void
171 NodesVariables::AddStartBound (Dx d, const std::vector<int>& dimensions, const VectorXd& val)
172 {
173  AddBounds(0, d, dimensions, val);
174 }
175 
176 void
177 NodesVariables::AddFinalBound (Dx deriv, const std::vector<int>& dimensions,
178  const VectorXd& val)
179 {
180  AddBounds(nodes_.size()-1, deriv, dimensions, val);
181 }
182 
183 NodesVariables::NodeValueInfo::NodeValueInfo(int node_id, Dx deriv, int node_dim)
184 {
185  id_ = node_id;
186  deriv_ = deriv;
187  dim_ = node_dim;
188 }
189 
190 int
192 {
193  return (id_ == right.id_)
194  && (deriv_ == right.deriv_)
195  && (dim_ == right.dim_);
196 };
197 
198 } /* namespace towr */
Base class to receive up-to-date values of the NodeVariables.
int GetPolynomialCount() const
int operator==(const NodeValueInfo &right) const
const std::vector< Node > GetNodes() const
VectorXd GetValues() const override
Pure optimization variables that define the nodes.
int id_
ID of the associated node (0 =< id < number of nodes in spline).
void AddBound(const NodeValueInfo &node_info, double value)
Restricts a specific optimization variables.
void UpdateObservers() const
Notifies the subscribed observers that the node values changes.
Eigen::VectorXd VectorXd
NodesVariables(const std::string &variable_name)
void SetVariables(const VectorXd &x) override
Sets some node positions and velocity from the optimization variables.
int dim_
Dimension (x,y,z) of that derivative.
int GetRows() const
static const int NodeValueNotOptimized
std::vector< Node > nodes_
static int GetNodeId(int poly_id, Side side)
The node ID that belongs to a specific side of a specific polynomial.
Semantic information associated with a scalar node value.
int GetOptIndex(const NodeValueInfo &nvi) const
Index in the optimization vector for a specific nodes&#39; pos/vel.
VecBound GetBounds() const override
Dx deriv_
Derivative (pos,vel) of the node with that ID.
const std::vector< Node > GetBoundaryNodes(int poly_id) const
void AddStartBound(Dx deriv, const std::vector< int > &dimensions, const VectorXd &val)
Restricts the first node in the spline.
virtual std::vector< NodeValueInfo > GetNodeValuesInfo(int opt_idx) const =0
Node values affected by one specific optimization variable.
void AddBounds(int node_id, Dx deriv, const std::vector< int > &dim, const VectorXd &values)
Bounds a specific node variables.
void AddFinalBound(Dx deriv, const std::vector< int > &dimensions, const VectorXd &val)
Restricts the last node in the spline.
void SetByLinearInterpolation(const VectorXd &initial_val, const VectorXd &final_val, double t_total)
Sets nodes pos/vel equally spaced from initial to final position.
VecBound bounds_
the bounds on the node values.
std::vector< Bounds > VecBound
void AddObserver(ObserverPtr const spline)
Adds a dependent observer that gets notified when the nodes change.
std::vector< ObserverPtr > observers_
Dx
< the values or derivative. For motions e.g. position, velocity, ...
Definition: state.h:41
Eigen::VectorXd VectorXd


towr
Author(s): Alexander W. Winkler
autogenerated on Mon Feb 28 2022 23:54:22