phase_spline.cc
Go to the documentation of this file.
00001 /******************************************************************************
00002 Copyright (c) 2018, Alexander W. Winkler. All rights reserved.
00003 
00004 Redistribution and use in source and binary forms, with or without
00005 modification, are permitted provided that the following conditions are met:
00006 
00007 * Redistributions of source code must retain the above copyright notice, this
00008   list of conditions and the following disclaimer.
00009 
00010 * Redistributions in binary form must reproduce the above copyright notice,
00011   this list of conditions and the following disclaimer in the documentation
00012   and/or other materials provided with the distribution.
00013 
00014 * Neither the name of the copyright holder nor the names of its
00015   contributors may be used to endorse or promote products derived from
00016   this software without specific prior written permission.
00017 
00018 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00019 AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00020 IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
00021 DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
00022 FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
00023 DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
00024 SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00025 CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
00026 OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
00027 OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00028 ******************************************************************************/
00029 
00030 #include <towr/variables/phase_spline.h>
00031 #include <towr/variables/phase_durations.h>
00032 
00033 namespace towr {
00034 
00035 PhaseSpline::PhaseSpline(
00036     NodesVariablesPhaseBased::Ptr const nodes,
00037     PhaseDurations* const phase_durations)
00038     : NodeSpline(nodes.get(), nodes->ConvertPhaseToPolyDurations(phase_durations->GetPhaseDurations())),
00039       PhaseDurationsObserver(phase_durations)
00040 {
00041   phase_nodes_ = nodes;
00042 
00043   UpdatePolynomialDurations();
00044 
00045   // if durations change, the polynomial active at a specified global time
00046   // changes. Therefore, all elements of the Jacobian could be non-zero
00047   // and must make sure that Jacobian structure never changes during
00048   // the iterations.
00049   // assume every global time time can fall into every polynomial
00050   for (int i=0; i<nodes->GetPolynomialCount(); ++i)
00051     FillJacobianWrtNodes(i, 0.0, kPos, jac_wrt_nodes_structure_, true);
00052 }
00053 
00054 void
00055 PhaseSpline::UpdatePolynomialDurations()
00056 {
00057   auto phase_duration = phase_durations_->GetPhaseDurations();
00058   auto poly_durations = phase_nodes_->ConvertPhaseToPolyDurations(phase_duration);
00059 
00060   for (int i=0; i<cubic_polys_.size(); ++i) {
00061     cubic_polys_.at(i).SetDuration(poly_durations.at(i));
00062   }
00063 
00064   UpdatePolynomialCoeff();
00065 }
00066 
00067 PhaseSpline::Jacobian
00068 PhaseSpline::GetJacobianOfPosWrtDurations (double t_global) const
00069 {
00070   VectorXd dx_dT  = GetDerivativeOfPosWrtPhaseDuration(t_global);
00071   VectorXd xd     = GetPoint(t_global).v();
00072   int current_phase = GetSegmentID(t_global, phase_durations_->GetPhaseDurations());
00073 
00074   return phase_durations_->GetJacobianOfPos(current_phase, dx_dT, xd);
00075 }
00076 
00077 Eigen::VectorXd
00078 PhaseSpline::GetDerivativeOfPosWrtPhaseDuration (double t_global) const
00079 {
00080   int poly_id; double t_local;
00081   std::tie(poly_id, t_local) = GetLocalTime(t_global, GetPolyDurations());
00082 
00083   VectorXd vel  = GetPoint(t_global).v();
00084   VectorXd dxdT = cubic_polys_.at(poly_id).GetDerivativeOfPosWrtDuration(t_local);
00085 
00086   double inner_derivative = phase_nodes_->GetDerivativeOfPolyDurationWrtPhaseDuration(poly_id);
00087   double prev_polys_in_phase = phase_nodes_->GetNumberOfPrevPolynomialsInPhase(poly_id);
00088 
00089   // where this minus term comes from:
00090   // from number of polynomials before current polynomial that
00091   // cause shifting of entire spline
00092   return inner_derivative*(dxdT - prev_polys_in_phase*vel);
00093 }
00094 
00095 
00096 } /* namespace towr */


towr
Author(s): Alexander W. Winkler
autogenerated on Mon Apr 15 2019 02:42:32