towr.h
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 
30 #ifndef TOWR_TOWR_H_
31 #define TOWR_TOWR_H_
32 
33 #include <ifopt/problem.h>
34 
35 #include <iostream>
36 
37 #include <ifopt_ipopt/ipopt_adapter.h>
38 
40 
41 #include "robot_model.h"
42 #include "height_map.h"
43 #include "nlp_factory.h"
44 #include "parameters.h"
45 #include "nlp_factory.h"
46 
47 
48 namespace towr {
49 
63 class TOWR {
64 public:
65  using FeetPos = std::vector<Eigen::Vector3d>;
66 
67  TOWR ()
68  {
69  using namespace std;
70  cout << "\n";
71  cout << "************************************************************\n";
72  cout << " TOWR - Trajectory Optimizer for Walking Robots (v1.1.0)\n";
73  cout << " \u00a9 Alexander W. Winkler\n";
74  cout << " https://github.com/ethz-adrl/towr\n";
75  cout << "************************************************************";
76  cout << "\n\n";
77  }
78  virtual ~TOWR () = default;
79 
86  void SetInitialState(const BaseState& base, const FeetPos& feet)
87  {
88  factory_.initial_base_ = base;
89  factory_.initial_ee_W_ = feet;
90  }
91 
100  void SetParameters(const BaseState& final_base,
101  const Parameters& params,
102  const RobotModel& model,
103  HeightMap::Ptr terrain)
104  {
105  factory_.final_base_ = final_base;
106  factory_.params_ = params;
107  factory_.model_ = model;
108  factory_.terrain_ = terrain;
109  }
110 
117  void SolveNLP()
118  {
119  nlp_ = BuildNLP();
120 
121  ifopt::IpoptAdapter::Solve(nlp_);
122  // ifopt::SnoptAdapter::Solve(nlp_);
123 
124  nlp_.PrintCurrent();
125  }
126 
134  {
135  return factory_.spline_holder_;
136  }
137 
146  void SetSolution(int solver_iteration)
147  {
148  nlp_.SetOptVariables(solver_iteration);
149  }
150 
154  int GetIterationCount() const
155  {
156  return nlp_.GetIterationCount();
157  }
158 
159 private:
166  ifopt::Problem nlp_;
167 
169 
173  ifopt::Problem BuildNLP()
174  {
175  ifopt::Problem nlp;
176 
177  for (auto c : factory_.GetVariableSets())
178  nlp.AddVariableSet(c);
179 
180  for (auto c : factory_.GetConstraints())
181  nlp.AddConstraintSet(c);
182 
183  for (auto c : factory_.GetCosts())
184  nlp.AddCostSet(c);
185 
186  return nlp;
187  }
188 };
189 
190 } /* namespace towr */
191 
192 #endif /* TOWR_TOWR_H_ */
BaseState initial_base_
Definition: nlp_factory.h:78
ContraintPtrVec GetConstraints() const
Definition: nlp_factory.cc:195
Holds pointers to the robot specific kinematics and dynamics.
Definition: robot_model.h:41
VariablePtrVec GetVariableSets()
Definition: nlp_factory.cc:51
std::vector< Eigen::Vector3d > FeetPos
Definition: towr.h:65
void SetSolution(int solver_iteration)
Sets the solution to a previous iteration of solver.
Definition: towr.h:146
void SetInitialState(const BaseState &base, const FeetPos &feet)
The current state of the robot where the optimization starts from.
Definition: towr.h:86
void SetParameters(const BaseState &final_base, const Parameters &params, const RobotModel &model, HeightMap::Ptr terrain)
The parameters that determine the type of motion produced.
Definition: towr.h:100
virtual ~TOWR()=default
ContraintPtrVec GetCosts() const
Definition: nlp_factory.cc:323
Holds pointers to fully constructed splines, that are linked to the optimization variables.
Definition: spline_holder.h:46
Can represent the 6Degree-of-Freedom floating base of a robot.
Definition: state.h:122
Parameters params_
Definition: nlp_factory.h:83
ifopt::Problem BuildNLP()
Definition: towr.h:173
NlpFactory factory_
Definition: towr.h:168
TOWR - Trajectory Optimizer for Walking Robots.
Definition: towr.h:63
Holds the parameters to tune the optimization problem.
Definition: parameters.h:45
void SolveNLP()
Constructs the problem and solves it with IPOPT.
Definition: towr.h:117
int GetIterationCount() const
Definition: towr.h:154
SplineHolder GetSolution() const
Definition: towr.h:133
BaseState final_base_
Definition: nlp_factory.h:79
ifopt::Problem nlp_
The solver independent optimization problem formulation.
Definition: towr.h:166
RobotModel model_
Definition: nlp_factory.h:81
SplineHolder spline_holder_
Definition: nlp_factory.h:85
TOWR()
Definition: towr.h:67
std::shared_ptr< HeightMap > Ptr
Definition: height_map.h:55
HeightMap::Ptr terrain_
Definition: nlp_factory.h:82


towr_core
Author(s): Alexander W. Winkler
autogenerated on Sun Apr 8 2018 02:18:53