DirectTrajectoryOptimization  v0.3
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Pages
InterPhaseBaseConstraintFreeControl.hpp
1 /***********************************************************************************
2 Copyright (c) 2017, Diego Pardo. All rights reserved.
3 
4 Redistribution and use in source and binary forms, with or without modification,
5 are permitted provided that the following conditions are met:
6  * Redistributions of source code must retain the above copyright notice,
7  this list of conditions and the following disclaimer.
8  * Redistributions in binary form must reproduce the above copyright notice,
9  this list of conditions and the following disclaimer in the documentation
10  and/or other materials provided with the distribution.
11  * Neither the name of ETH ZURICH nor the names of its contributors may be used
12  to endorse or promote products derived from this software without specific
13  prior written permission.
14 
15 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY
16 EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
17 OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT
18 SHALL ETH ZURICH BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY,
19 OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE
20 GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
21 CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
22 (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
23 EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
24 ***************************************************************************************/
25 
26 /*
27  * InterPhaseBaseConstraintFreecontrol.hpp
28  *
29  * Created on: Dec 20, 2016
30  * Author: depardo
31  */
32 
33 #ifndef _INTERPHASEBASE_FREECONTROL_HPP_
34 #define _INTERPHASEBASE_FREECONTROL_HPP_
35 
36 #include <memory>
37 #include <optimization/constraints/GenericConstraintsBase.hpp>
38 
39 namespace DirectTrajectoryOptimization{
40 namespace BaseClass{
41 
42 template <class DIMENSIONS>
43 class InterPhaseBaseFreeControl : public NumDiffDerivativesBase,
44  public std::enable_shared_from_this<InterPhaseBaseFreeControl<DIMENSIONS> > {
45 
46 public:
47 
48  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
49 
50  typedef typename DIMENSIONS::state_vector_t state_vector_t;
51  typedef typename DIMENSIONS::control_vector_t control_vector_t;
52 
53  int s_size = static_cast<int>(DIMENSIONS::DimensionsSize::STATE_SIZE);
54  int c_size = static_cast<int>(DIMENSIONS::DimensionsSize::CONTROL_SIZE);
55 
56  Eigen::VectorXd ipc_lb;
57  Eigen::VectorXd ipc_ub;
58  int complete_vector_size = 0;
59  int size_of_jacobian = 0;
60  int total_inputs_jacobian=0;
61 
62  std::shared_ptr<InterPhaseBaseFreeControl<DIMENSIONS> > my_pointer;
63 
64 
65  InterPhaseBaseFreeControl(const int & n_constraints , const Eigen::VectorXd & lb , const Eigen::VectorXd & ub):complete_vector_size(n_constraints),ipc_lb(lb),ipc_ub(ub){
66 
67  if(ipc_ub.size()!=ipc_lb.size() || ipc_ub.size() != n_constraints) {
68  std::cout << "Wrong size of InterPhase constraints! " << std::endl;
69  std::exit(EXIT_FAILURE);
70  }
71  complete_vector_size = n_constraints;
72  }
73 
74  InterPhaseBaseFreeControl() {
75 
76  //Initialization for the default case
77  complete_vector_size = s_size;
78  ipc_lb = Eigen::VectorXd::Zero(complete_vector_size);
79  ipc_ub = Eigen::VectorXd::Zero(complete_vector_size);
80  }
81 
82  int GetSize() { return complete_vector_size;}
83  int GetNnzJacobian() { return size_of_jacobian;}
84  Eigen::VectorXd getLowerBound(){return ipc_lb;}
85  Eigen::VectorXd getUpperBound(){return ipc_ub;}
86 
87  virtual ~InterPhaseBaseFreeControl(){ };
88 
89  void initialize();
90  void initialize_num_diff();
91  void fx(const Eigen::VectorXd & in, Eigen::VectorXd & out);
92 
93  // IMPORTANT: u1 and u2 are NOW used in the structure of the jacobian,
94 
95  // Implement a default constraint where the positions and controls must be the same
96  virtual Eigen::VectorXd evalConstraintsFct(const state_vector_t& x1, const state_vector_t & x2,
97  const control_vector_t & u1, const control_vector_t & u2);
98 
99  Eigen::VectorXd evalConstraintsFctDerivatives(const state_vector_t & x1, const state_vector_t & x2,
100  const control_vector_t & u1, const control_vector_t & u2);
101 
102  //TEMP??
103  Eigen::VectorXd temp_jacobian_vector;
104 };
105 
106 template <class DIMENSIONS>
107 void InterPhaseBaseFreeControl<DIMENSIONS>::initialize() {
108 
109  // this is always like this!
110  total_inputs_jacobian = 2*s_size + 2*c_size;
111  size_of_jacobian = complete_vector_size*total_inputs_jacobian;
112 
113  temp_jacobian_vector.resize(size_of_jacobian);
114 
115  initialize_num_diff();
116 }
117 
118 template <class DIMENSIONS>
119 Eigen::VectorXd InterPhaseBaseFreeControl<DIMENSIONS>::evalConstraintsFct(const state_vector_t& x1, const state_vector_t & x2,
120  const control_vector_t & u1, const control_vector_t & u2) {
121 
122  // Poistions should be the same
123  return (x1-x2);
124 
125 }
126 
127 template <class DIMENSIONS>
128 Eigen::VectorXd InterPhaseBaseFreeControl<DIMENSIONS>::evalConstraintsFctDerivatives(
129  const state_vector_t & x1, const state_vector_t & x2 ,
130  const control_vector_t &u1,const control_vector_t &u2) {
131 
132  Eigen::VectorXd in_vector(total_inputs_jacobian);
133 
134  in_vector.segment(0,s_size) = x1;
135  in_vector.segment(s_size,c_size) = u1;
136  in_vector.segment(s_size+c_size,s_size) = x2;
137  in_vector.segment(2*s_size+c_size,c_size) = u2;
138 
139  this->numDiff->df(in_vector,this->mJacobian);
140 
141  Eigen::MatrixXd temp = this->mJacobian.transpose();
142 
143  //returning ROW-wise the constraint as a vector
144  return (Eigen::Map<Eigen::VectorXd>(temp.data(),size_of_jacobian));
145 }
146 
147 template <class DIMENSIONS>
148 void InterPhaseBaseFreeControl<DIMENSIONS>::fx(const Eigen::VectorXd & inputs, Eigen::VectorXd & outputs) {
149 
150  state_vector_t x1 = inputs.segment(0,s_size);
151  control_vector_t u1 = inputs.segment(s_size,c_size);
152  state_vector_t x2 = inputs.segment(s_size+c_size,s_size);
153  control_vector_t u2 = inputs.segment(2*s_size+c_size,c_size);
154 
155  outputs = evalConstraintsFct(x1,x2,u1,u2);
156 }
157 
158 template <class DIMENSIONS>
159 void InterPhaseBaseFreeControl<DIMENSIONS>::initialize_num_diff() {
160 
161  this->my_pointer = std::enable_shared_from_this<InterPhaseBaseFreeControl<DIMENSIONS>>::shared_from_this();
162 
163  mJacobian.resize(complete_vector_size,total_inputs_jacobian);
164 
165  this->numdifoperator = std::shared_ptr<FunctionOperator>(new FunctionOperator(total_inputs_jacobian, complete_vector_size , this->my_pointer));
166 
167  this->numDiff = std::shared_ptr<Eigen::NumericalDiff<FunctionOperator> >( new Eigen::NumericalDiff<FunctionOperator>(*this->numdifoperator,std::sqrt(std::numeric_limits<double>::epsilon())));
168 
169 }
170 
171 } // BaseClass
172 } // DirectTrajectoryOptimization
173 #endif /* _INTERPHASEBASE_FREECONTROL_HPP_ */
GenericConstraintsBase.hpp
Base class for vector constraints and numdiff derivatives.