time_discretization_constraint.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/constraints/time_discretization_constraint.h>
00031 
00032 #include <cmath>
00033 
00034 namespace towr {
00035 
00036 
00037 TimeDiscretizationConstraint::TimeDiscretizationConstraint (double T, double dt,
00038                                                             std::string name)
00039     :ConstraintSet(kSpecifyLater, name)
00040 {
00041   double t = 0.0;
00042   dts_ = {t};
00043 
00044   for (int i=0; i<floor(T/dt); ++i) {
00045     t += dt;
00046     dts_.push_back(t);
00047   }
00048 
00049   dts_.push_back(T); // also ensure constraints at very last node/time.
00050 }
00051 
00052 TimeDiscretizationConstraint::TimeDiscretizationConstraint (const VecTimes& times,
00053                                                             std::string name)
00054    :ConstraintSet(kSpecifyLater, name) // just placeholder
00055 {
00056   dts_ = times;
00057 }
00058 
00059 int
00060 TimeDiscretizationConstraint::GetNumberOfNodes () const
00061 {
00062   return dts_.size();
00063 }
00064 
00065 TimeDiscretizationConstraint::VectorXd
00066 TimeDiscretizationConstraint::GetValues () const
00067 {
00068   VectorXd g = VectorXd::Zero(GetRows());
00069 
00070   int k = 0;
00071   for (double t : dts_)
00072     UpdateConstraintAtInstance(t, k++, g);
00073 
00074   return g;
00075 }
00076 
00077 TimeDiscretizationConstraint::VecBound
00078 TimeDiscretizationConstraint::GetBounds () const
00079 {
00080   VecBound bounds(GetRows());
00081 
00082   int k = 0;
00083   for (double t : dts_)
00084     UpdateBoundsAtInstance(t, k++, bounds);
00085 
00086   return bounds;
00087 }
00088 
00089 void
00090 TimeDiscretizationConstraint::FillJacobianBlock (std::string var_set,
00091                                                   Jacobian& jac) const
00092 {
00093   int k = 0;
00094   for (double t : dts_)
00095     UpdateJacobianAtInstance(t, k++, var_set, jac);
00096 }
00097 
00098 } /* namespace towr */
00099 
00100 


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