phase_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 
32 
33 namespace towr {
34 
37  PhaseDurations* const phase_durations)
38  : NodeSpline(nodes.get(), nodes->ConvertPhaseToPolyDurations(phase_durations->GetPhaseDurations())),
39  PhaseDurationsObserver(phase_durations)
40 {
41  phase_nodes_ = nodes;
42 
44 
45  // if durations change, the polynomial active at a specified global time
46  // changes. Therefore, all elements of the Jacobian could be non-zero
47  // and must make sure that Jacobian structure never changes during
48  // the iterations.
49  // assume every global time time can fall into every polynomial
50  for (int i=0; i<nodes->GetPolynomialCount(); ++i)
52 }
53 
54 void
56 {
57  auto phase_duration = phase_durations_->GetPhaseDurations();
58  auto poly_durations = phase_nodes_->ConvertPhaseToPolyDurations(phase_duration);
59 
60  for (int i=0; i<cubic_polys_.size(); ++i) {
61  cubic_polys_.at(i).SetDuration(poly_durations.at(i));
62  }
63 
65 }
66 
69 {
71  VectorXd xd = GetPoint(t_global).v();
72  int current_phase = GetSegmentID(t_global, phase_durations_->GetPhaseDurations());
73 
74  return phase_durations_->GetJacobianOfPos(current_phase, dx_dT, xd);
75 }
76 
79 {
80  int poly_id; double t_local;
81  std::tie(poly_id, t_local) = GetLocalTime(t_global, GetPolyDurations());
82 
83  VectorXd vel = GetPoint(t_global).v();
84  VectorXd dxdT = cubic_polys_.at(poly_id).GetDerivativeOfPosWrtDuration(t_local);
85 
86  double inner_derivative = phase_nodes_->GetDerivativeOfPolyDurationWrtPhaseDuration(poly_id);
87  double prev_polys_in_phase = phase_nodes_->GetNumberOfPrevPolynomialsInPhase(poly_id);
88 
89  // where this minus term comes from:
90  // from number of polynomials before current polynomial that
91  // cause shifting of entire spline
92  return inner_derivative*(dxdT - prev_polys_in_phase*vel);
93 }
94 
95 
96 } /* namespace towr */
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
Base class to receive up-to-date values of the ContactSchedule.
A spline built from node values and fixed polynomial durations.
Definition: node_spline.h:49
Jacobian jac_wrt_nodes_structure_
Definition: node_spline.h:108
Eigen::VectorXd VectorXd
void UpdatePolynomialDurations() override
Called by subject to update polynomial durations when they changed.
Definition: phase_spline.cc:55
VecPoly cubic_polys_
the sequence of polynomials making up the spline.
Definition: spline.h:89
std::shared_ptr< NodesVariablesPhaseBased > Ptr
A variable set composed of the phase durations of an endeffector.
Jacobian GetJacobianOfPos(int phase, const VectorXd &dx_dT, const VectorXd &xd) const
How a change in the phase durations affect the position of a spline.
NodesVariablesPhaseBased::Ptr phase_nodes_
Definition: phase_spline.h:83
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
VecDurations GetPhaseDurations() const
static int GetSegmentID(double t_global, const VecTimes &durations)
Definition: spline.cc:49
Eigen::SparseMatrix< double, Eigen::RowMajor > Jacobian
Definition: node_spline.h:52
PhaseDurationsSubjectPtr phase_durations_
const State GetPoint(double t) const
Definition: spline.cc:81
Eigen::VectorXd VectorXd
Definition: phase_spline.h:50
Eigen::VectorXd GetDerivativeOfPosWrtPhaseDuration(double t) const
How the position at time t changes with current phase duration.
Definition: phase_spline.cc:78
Jacobian GetJacobianOfPosWrtDurations(double t) const override
How the spline position changes when the polynomial durations change.
Definition: phase_spline.cc:68
const VectorXd v() const
read access to the first-derivative of the state, e.g. velocity.
Definition: state.cc:59
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
PhaseSpline(NodesVariablesPhaseBased::Ptr const node_variables, PhaseDurations *const phase_durations)
Constructs a spline with varying/optimized phase durations.
Definition: phase_spline.cc:35


towr
Author(s): Alexander W. Winkler
autogenerated on Fri Apr 2 2021 02:14:16