node_spline.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 #include <towr/variables/nodes.h>
33 
34 namespace towr {
35 
37  const VecTimes& polynomial_durations)
38  : Spline(polynomial_durations, node_variables->GetDim()),
39  NodesObserver(node_variables)
40 {
41  UpdateNodes();
42  jac_wrt_nodes_structure_ = Jacobian(node_variables->GetDim(), node_variables->GetRows());
43 }
44 
45 
46 void
48 {
49  for (int i=0; i<cubic_polys_.size(); ++i) {
50  auto nodes = node_values_->GetBoundaryNodes(i);
51  cubic_polys_.at(i).SetNodes(nodes.front(), nodes.back());
52  }
53 
55 }
56 
57 int
59 {
60  return node_values_->GetRows();
61 }
62 
64 NodeSpline::GetJacobianWrtNodes (double t_global, Dx dxdt) const
65 {
66  int id; double t_local;
67  std::tie(id, t_local) = GetLocalTime(t_global, GetPolyDurations());
68 
69  return GetJacobianWrtNodes(id, t_local, dxdt);
70 }
71 
73 NodeSpline::GetJacobianWrtNodes (int id, double t_local, Dx dxdt) const
74 {
76  FillJacobianWrtNodes(id, t_local, dxdt, jac, false);
77 
78  // needed to avoid Eigen::assert failure "wrong storage order" triggered
79  // in dynamic_constraint.cc
80  jac.makeCompressed();
81 
82  return jac;
83 }
84 
85 void
86 NodeSpline::FillJacobianWrtNodes (int poly_id, double t_local, Dx dxdt,
87  Jacobian& jac, bool fill_with_zeros) const
88 {
89  for (int idx=0; idx<jac.cols(); ++idx) {
90  for (auto info : node_values_->GetNodeInfoAtOptIndex(idx)) {
91  for (auto side : {Nodes::Side::Start, Nodes::Side::End}) { // every jacobian is affected by two nodes
92 
93  int node = node_values_->GetNodeId(poly_id, side);
94 
95  if (node == info.node_id_) {
96  double val = 0.0;
97 
98  if (side == Nodes::Side::Start)
99  val = cubic_polys_.at(poly_id).GetDerivativeWrtStartNode(dxdt, info.node_deriv_, t_local);
100  else if (side == Nodes::Side::End)
101  val = cubic_polys_.at(poly_id).GetDerivativeWrtEndNode(dxdt, info.node_deriv_, t_local);
102  else
103  assert(false); // this shouldn't happen
104 
105  // if only want structure
106  if (fill_with_zeros)
107  val = 0.0;
108 
109  jac.coeffRef(info.node_dim_, idx) += val;
110  }
111  }
112  }
113  }
114 }
115 
116 } /* namespace towr */
Base class to receive up-to-date values of the NodeVariables.
std::pair< int, double > GetLocalTime(double t_global, const VecTimes &d) const
How much time of the current segment has passed at t_global.
Definition: spline.cc:69
Jacobian GetJacobianWrtNodes(double t, Dx dxdt) const
How the spline changes when the node values change.
Definition: node_spline.cc:64
Jacobian jac_wrt_nodes_structure_
Definition: node_spline.h:110
A spline built from a sequence of cubic polynomials.
Definition: spline.h:45
NodeSubjectPtr node_values_
std::vector< double > VecTimes
Definition: spline.h:47
void UpdateNodes()
Called by subject to update the polynomials with new node values.
Definition: node_spline.cc:47
VecPoly cubic_polys_
the sequence of polynomials making up the spline.
Definition: spline.h:89
virtual std::vector< IndexInfo > GetNodeInfoAtOptIndex(int idx) const =0
The node information that the optimization index represents.
Definition: nodes.cc:93
void UpdatePolynomialCoeff()
Updates the cubic-Hermite polynomial coefficients using the currently set nodes values and durations...
Definition: spline.cc:96
VecTimes GetPolyDurations() const
Definition: spline.cc:109
Position and velocity of nodes used to generate a Hermite spline.
Definition: nodes.h:52
Eigen::SparseMatrix< double, Eigen::RowMajor > Jacobian
Definition: node_spline.h:53
int GetDim() const
Definition: nodes.cc:162
NodeSpline(NodeSubjectPtr const node_variables, const VecTimes &polynomial_durations)
Constructs a spline with constant durations.
Definition: node_spline.cc:36
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
const std::vector< Node > GetBoundaryNodes(int poly_id) const
Definition: nodes.cc:153
void FillJacobianWrtNodes(int poly_id, double t_local, Dx dxdt, Jacobian &jac, bool fill_with_zeros) const
Fills specific elements of the Jacobian with respect to nodes.
Definition: node_spline.cc:86
int GetNodeVariablesCount() const
Definition: node_spline.cc:58
Dx
< the values or derivative. For motions e.g. position, velocity, ...
Definition: state.h:41


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