phase_durations.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 <numeric> // std::accumulate
33 
35 #include <towr/variables/spline.h> // for Spline::GetSegmentID()
36 
37 
38 namespace towr {
39 
40 
42  const VecDurations& timings,
43  bool is_first_phase_in_contact,
44  double min_duration,
45  double max_duration)
46  // -1 since last phase-duration is not optimized over, but comes from total time
47  :VariableSet(timings.size()-1, id::EESchedule(ee))
48 {
49  durations_ = timings;
50  t_total_ = std::accumulate(timings.begin(), timings.end(), 0.0);
51  phase_duration_bounds_ = ifopt::Bounds(min_duration, max_duration);
52  initial_contact_state_ = is_first_phase_in_contact;
53 }
54 
55 void
57 {
58  observers_.push_back(o);
59 }
60 
61 void
63 {
64  for (auto& spline : observers_)
65  spline->UpdatePolynomialDurations();
66 }
67 
70 {
71  VectorXd x(GetRows());
72 
73  for (int i=0; i<x.rows(); ++i)
74  x(i) = durations_.at(i);
75 
76  return x;
77 }
78 
79 void
81 {
82  // the sum of all phase durations should never be larger than the total time of
83  // the trajectory. This would e.g. query constraints after duration of the trajectory and
84  // causes undefined behavior. However, when IPOPT optimizes the phase durations,
85  // it's not possible to enforce that in every iteration this condition is fulfilled. Sure,
86  // we can set this as a constraint, but during the solution process constraints might still
87  // be violated.
88  // Fortunately, violation of this doesn't seem to mess up IPOPT too much and a solution is
89  // often found. So if you get this error you can ignore it by compiling in Release mode,
90  // but I'm leaving this in here to show that this is undefined behavior and a clean
91  // implementation is still required. PR desired ;)
92  assert(t_total_>x.sum());
93 
94  for (int i=0; i<GetRows(); ++i)
95  durations_.at(i) = x(i);
96 
97  // last phase duration not optimized, used to fill up to total time.
98  durations_.back() = t_total_ - x.sum();
100 }
101 
104 {
105  VecBound bounds;
106 
107  for (int i=0; i<GetRows(); ++i)
108  bounds.push_back(phase_duration_bounds_);
109 
110  return bounds;
111 }
112 
115 {
116  return durations_;
117 }
118 
119 bool
121 {
122  int phase_id = Spline::GetSegmentID(t, durations_);
123  return phase_id%2 == 0? initial_contact_state_ : !initial_contact_state_;
124 }
125 
128  const VectorXd& dx_dT,
129  const VectorXd& xd) const
130 {
131  int n_dim = xd.rows();
132  Eigen::MatrixXd jac = Eigen::MatrixXd::Zero(n_dim, GetRows());
133 
134  bool in_last_phase = (current_phase == durations_.size()-1);
135 
136  // duration of current phase expands and compressed spline
137  if (!in_last_phase)
138  jac.col(current_phase) = dx_dT;
139 
140  for (int phase=0; phase<current_phase; ++phase) {
141  // each previous durations shifts spline along time axis
142  jac.col(phase) = -1*xd;
143 
144  // in last phase previous duration cause expansion/compression of spline
145  // as final time is fixed.
146  if (in_last_phase)
147  jac.col(phase) -= dx_dT;
148  }
149 
150  // convert to sparse, but also regard 0.0 as non-zero element, because
151  // could turn nonzero during the course of the program
152  // as durations change and t_global falls into different spline
153  return jac.sparseView(1.0, -1.0);
154 }
155 
156 } /* namespace towr */
157 
158 
ifopt::Bounds phase_duration_bounds_
Base class to receive up-to-date values of the ContactSchedule.
Eigen::VectorXd VectorXd
static std::string EESchedule(uint ee)
VectorXd GetValues() const override
bool IsContactPhase(double t) const
Whether the endeffector is in contact with the environment.
VecBound GetBounds() const override
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.
VecDurations GetPhaseDurations() const
static int GetSegmentID(double t_global, const VecTimes &durations)
Definition: spline.cc:49
void UpdateObservers() const
PhaseDurations(EndeffectorID ee, const VecDurations &initial_durations, bool is_first_phase_in_contact, double min_phase_duration, double max_phase_duration)
Constructs a variable set for a specific endeffector.
Eigen::SparseMatrix< double, Eigen::RowMajor > Jacobian
std::vector< double > VecDurations
void SetVariables(const VectorXd &x) override
Sets the phase durations from pure Eigen optimization variables.
bool initial_contact_state_
true if first phase in contact
void AddObserver(PhaseDurationsObserver *const spline)
Adds observer that is updated every time new variables are set.
std::vector< Bounds > VecBound
std::vector< PhaseDurationsObserver * > observers_
int GetRows() const
Eigen::VectorXd VectorXd


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