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 
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 void
47 {
48  for (int i=0; i<cubic_polys_.size(); ++i) {
49  auto nodes = node_values_->GetBoundaryNodes(i);
50  cubic_polys_.at(i).SetNodes(nodes.front(), nodes.back());
51  }
52 
54 }
55 
56 int
58 {
59  return node_values_->GetRows();
60 }
61 
63 NodeSpline::GetJacobianWrtNodes (double t_global, Dx dxdt) const
64 {
65  int id; double t_local;
66  std::tie(id, t_local) = GetLocalTime(t_global, GetPolyDurations());
67 
68  return GetJacobianWrtNodes(id, t_local, dxdt);
69 }
70 
72 NodeSpline::GetJacobianWrtNodes (int id, double t_local, Dx dxdt) const
73 {
75  FillJacobianWrtNodes(id, t_local, dxdt, jac, false);
76 
77  // needed to avoid Eigen::assert failure "wrong storage order" triggered
78  // in dynamic_constraint.cc
79  jac.makeCompressed();
80 
81  return jac;
82 }
83 
84 void
85 NodeSpline::FillJacobianWrtNodes (int poly_id, double t_local, Dx dxdt,
86  Jacobian& jac, bool fill_with_zeros) const
87 {
88  for (int idx=0; idx<jac.cols(); ++idx) {
89  for (auto nvi : node_values_->GetNodeValuesInfo(idx)) {
90  for (auto side : {NodesVariables::Side::Start, NodesVariables::Side::End}) { // every jacobian is affected by two nodes
91  int node = node_values_->GetNodeId(poly_id, side);
92 
93  if (node == nvi.id_) {
94  double val = 0.0;
95 
96  if (side == NodesVariables::Side::Start)
97  val = cubic_polys_.at(poly_id).GetDerivativeWrtStartNode(dxdt, nvi.deriv_, t_local);
98  else if (side == NodesVariables::Side::End)
99  val = cubic_polys_.at(poly_id).GetDerivativeWrtEndNode(dxdt, nvi.deriv_, t_local);
100  else
101  assert(false); // this shouldn't happen
102 
103  // if only want structure
104  if (fill_with_zeros)
105  val = 0.0;
106 
107  jac.coeffRef(nvi.dim_, idx) += val;
108  }
109  }
110  }
111  }
112 }
113 
114 } /* namespace towr */
Base class to receive up-to-date values of the NodeVariables.
VecTimes GetPolyDurations() const
Definition: spline.cc:109
int GetNodeVariablesCount() const
Definition: node_spline.cc:57
Jacobian jac_wrt_nodes_structure_
Definition: node_spline.h:108
A spline built from a sequence of cubic polynomials.
Definition: spline.h:45
NodeSubjectPtr node_values_
std::vector< double > VecTimes
Definition: spline.h:47
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
void UpdateNodes()
Called by subject to update the polynomials with new node values.
Definition: node_spline.cc:46
VecPoly cubic_polys_
the sequence of polynomials making up the spline.
Definition: spline.h:89
Jacobian GetJacobianWrtNodes(double t, Dx dxdt) const
How the spline changes when the node values change.
Definition: node_spline.cc:63
Position and velocity of nodes used to generate a Hermite spline.
int GetRows() const
static int GetNodeId(int poly_id, Side side)
The node ID that belongs to a specific side of a specific polynomial.
void UpdatePolynomialCoeff()
Updates the cubic-Hermite polynomial coefficients using the currently set nodes values and durations...
Definition: spline.cc:96
Eigen::SparseMatrix< double, Eigen::RowMajor > Jacobian
Definition: node_spline.h:52
NodeSpline(NodeSubjectPtr const node_variables, const VecTimes &polynomial_durations)
Constructs a spline with constant durations.
Definition: node_spline.cc:36
const std::vector< Node > GetBoundaryNodes(int poly_id) const
virtual std::vector< NodeValueInfo > GetNodeValuesInfo(int opt_idx) const =0
Node values affected by one specific optimization variable.
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:85
Dx
< the values or derivative. For motions e.g. position, velocity, ...
Definition: state.h:41


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