nlp_factory.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 
30 #include <towr/nlp_factory.h>
31 
35 
44 
45 #include <towr/costs/node_cost.h>
46 
47 namespace towr {
48 
49 
52 {
53  VariablePtrVec vars;
54 
55  auto base_motion = MakeBaseVariables();
56  vars.insert(vars.end(), base_motion.begin(), base_motion.end());
57 
58  auto ee_motion = MakeEndeffectorVariables();
59  vars.insert(vars.end(), ee_motion.begin(), ee_motion.end());
60 
61  auto ee_force = MakeForceVariables();
62  vars.insert(vars.end(), ee_force.begin(), ee_force.end());
63 
65  if (params_.OptimizeTimings()) {
66  vars.insert(vars.end(), contact_schedule.begin(), contact_schedule.end());
67  }
68 
69  // stores these readily constructed spline, independent of whether the
70  // nodes and durations these depend on are optimized over
71  spline_holder_ = SplineHolder(base_motion.at(0), // linear
72  base_motion.at(1), // angular
74  ee_motion,
75  ee_force,
78  return vars;
79 }
80 
81 std::vector<Nodes::Ptr>
83 {
84  std::vector<Nodes::Ptr> vars;
85 
86  int n_nodes = params_.GetBasePolyDurations().size() + 1;
87 
88  auto spline_lin = std::make_shared<BaseNodes>(n_nodes, id::base_lin_nodes);
89  spline_lin->InitializeNodesTowardsGoal(initial_base_.lin.p(), final_base_.lin.p(), params_.t_total_);
90  spline_lin->AddStartBound(kPos, {X,Y,Z}, initial_base_.lin.p());
91  spline_lin->AddStartBound(kVel, {X,Y,Z}, initial_base_.lin.v());
92  spline_lin->AddFinalBound(kPos, {X,Y} , final_base_.lin.p());
93  spline_lin->AddFinalBound(kVel, {X,Y,Z}, final_base_.lin.v());
94  vars.push_back(spline_lin);
95 
96  auto spline_ang = std::make_shared<BaseNodes>(n_nodes, id::base_ang_nodes);
97  spline_ang->InitializeNodesTowardsGoal(initial_base_.ang.p(), final_base_.ang.p(), params_.t_total_);
98  spline_ang->AddStartBound(kPos, {X,Y,Z}, initial_base_.ang.p());
99  spline_ang->AddStartBound(kVel, {X,Y,Z}, initial_base_.ang.v());
100  spline_ang->AddFinalBound(kPos, {Z}, final_base_.ang.p());
101  spline_ang->AddFinalBound(kVel, {X,Y,Z}, final_base_.ang.v());
102  vars.push_back(spline_ang);
103 
104  return vars;
105 }
106 
107 std::vector<PhaseNodes::Ptr>
109 {
110  std::vector<PhaseNodes::Ptr> vars;
111 
112  // Endeffector Motions
113  double T = params_.t_total_;
114  for (int ee=0; ee<params_.GetEECount(); ee++) {
115 
116  auto nodes = std::make_shared<PhaseNodes>(params_.GetPhaseCount(ee),
118  id::EEMotionNodes(ee),
121 
122  double yaw = final_base_.ang.p().z();
123 
124  // smell adapt to desired yaw state
125 // Eigen::Matrix3d w_R_b = GetQuaternionFromEulerZYX(yaw, 0.0, 0.0).toRotationMatrix();
126  Eigen::Matrix3d w_R_b; w_R_b.setIdentity();
127 
128  Vector3d final_ee_pos_W = final_base_.lin.p() + w_R_b*model_.kinematic_model_->GetNominalStanceInBase().at(ee);
129 
130 
131 
132  nodes->InitializeNodesTowardsGoal(initial_ee_W_.at(ee), final_ee_pos_W, T);
133 
134  // actually initial Z position should be constrained as well...-.-
135  nodes->AddStartBound(kPos, {X,Y}, initial_ee_W_.at(ee));
136 
137  // fix final endeffector position
138 // bool step_taken = nodes->GetNodes().size() > 2;
139 // if (step_taken) // otherwise overwrites start bound
140 // nodes->AddFinalBound(kPos, {X,Y}, final_ee_pos_W);
141 
142  vars.push_back(nodes);
143  }
144 
145 
146  return vars;
147 }
148 
149 std::vector<PhaseNodes::Ptr>
151 {
152  std::vector<PhaseNodes::Ptr> vars;
153 
154  double T = params_.t_total_;
155  for (int ee=0; ee<params_.GetEECount(); ee++) {
156 
157  auto nodes = std::make_shared<PhaseNodes>(params_.GetPhaseCount(ee),
159  id::EEForceNodes(ee),
162 
163  // initialize with mass of robot distributed equally on all legs
164  double m = model_.dynamic_model_->m();
165  double g = model_.dynamic_model_->g();
166 
167 
168  Vector3d f_stance(0.0, 0.0, m*g/params_.GetEECount());
169  nodes->InitializeNodesTowardsGoal(f_stance, f_stance, T);
170  vars.push_back(nodes);
171  }
172 
173  return vars;
174 }
175 
176 std::vector<PhaseDurations::Ptr>
178 {
179  std::vector<PhaseDurations::Ptr> vars;
180 
181  for (int ee=0; ee<params_.GetEECount(); ee++) {
182 
183  auto var = std::make_shared<PhaseDurations>(ee,
188  vars.push_back(var);
189  }
190 
191  return vars;
192 }
193 
196 {
197  ContraintPtrVec constraints;
198  for (ConstraintName name : params_.constraints_)
199  for (auto c : GetConstraint(name))
200  constraints.push_back(c);
201 
202  return constraints;
203 }
204 
207 {
208  switch (name) {
209  case Dynamic: return MakeDynamicConstraint();
212  case TotalTime: return MakeTotalTimeConstraint();
213  case Terrain: return MakeTerrainConstraint();
214  case Force: return MakeForceConstraint();
215  case Swing: return MakeSwingConstraint();
216  case BaseAcc: return MakeBaseAccConstraint();
217  default: throw std::runtime_error("constraint not defined!");
218  }
219 }
220 
221 
224 {
225  return {std::make_shared<BaseMotionConstraint>(params_, spline_holder_)};
226 }
227 
230 {
231  auto constraint = std::make_shared<DynamicConstraint>(model_.dynamic_model_,
232  params_,
234  return {constraint};
235 }
236 
239 {
240  ContraintPtrVec c;
241 
242  for (int ee=0; ee<params_.GetEECount(); ee++) {
243  auto rom_constraints = std::make_shared<RangeOfMotionConstraint>(model_.kinematic_model_,
244  params_,
245  ee,
247  c.push_back(rom_constraints);
248  }
249 
250  return c;
251 }
252 
255 {
256  ContraintPtrVec c;
257  double T = params_.t_total_;
258 
259  for (int ee=0; ee<params_.GetEECount(); ee++) {
260  auto duration_constraint = std::make_shared<TotalDurationConstraint>(T, ee);
261  c.push_back(duration_constraint);
262  }
263 
264  return c;
265 }
266 
269 {
270  ContraintPtrVec constraints;
271 
272  for (int ee=0; ee<params_.GetEECount(); ee++) {
273  auto c = std::make_shared<TerrainConstraint>(terrain_, id::EEMotionNodes(ee));
274  constraints.push_back(c);
275  }
276 
277  return constraints;
278 }
279 
282 {
283  ContraintPtrVec constraints;
284 
285  for (int ee=0; ee<params_.GetEECount(); ee++) {
286  auto c = std::make_shared<ForceConstraint>(terrain_,
288  ee);
289  constraints.push_back(c);
290  }
291 
292  return constraints;
293 }
294 
297 {
298  ContraintPtrVec constraints;
299 
300  for (int ee=0; ee<params_.GetEECount(); ee++) {
301  auto swing = std::make_shared<SwingConstraint>(id::EEMotionNodes(ee));
302  constraints.push_back(swing);
303  }
304 
305  return constraints;
306 }
307 
310 {
311  ContraintPtrVec constraints;
312 
313  constraints.push_back(std::make_shared<SplineAccConstraint>
315 
316  constraints.push_back(std::make_shared<SplineAccConstraint>
318 
319  return constraints;
320 }
321 
324 {
325  ContraintPtrVec costs;
326  for (const auto& pair : params_.costs_)
327  for (auto c : GetCost(pair.first, pair.second))
328  costs.push_back(c);
329 
330  return costs;
331 }
332 
334 NlpFactory::GetCost(const CostName& name, double weight) const
335 {
336  switch (name) {
337  case ForcesCostID: return MakeForcesCost(weight);
338  default: throw std::runtime_error("cost not defined!");
339  }
340 }
341 
343 NlpFactory::MakeForcesCost(double weight) const
344 {
345  CostPtrVec cost;
346 
347  for (int ee=0; ee<params_.GetEECount(); ee++)
348  cost.push_back(std::make_shared<NodeCost>(id::EEForceNodes(ee), kPos, Z));
349 
350  return cost;
351 }
352 
353 } /* namespace towr */
DynamicModel::Ptr dynamic_model_
Definition: robot_model.h:44
BaseState initial_base_
Definition: nlp_factory.h:78
ContraintPtrVec GetConstraints() const
Definition: nlp_factory.cc:195
ContraintPtrVec GetConstraint(ConstraintName name) const
Definition: nlp_factory.cc:206
CostPtrVec GetCost(const CostName &id, double weight) const
Definition: nlp_factory.cc:334
ContraintPtrVec MakeTotalTimeConstraint() const
Definition: nlp_factory.cc:254
std::vector< ifopt::ConstraintSet::Ptr > ContraintPtrVec
Definition: nlp_factory.h:54
ContraintPtrVec MakeForceConstraint() const
Definition: nlp_factory.cc:281
int ee_polynomials_per_swing_phase_
Number of polynomials to parameterize foot movement during swing phases.
Definition: parameters.h:92
std::vector< PhaseNodes::Ptr > MakeEndeffectorVariables() const
Definition: nlp_factory.cc:108
UsedConstraints constraints_
Which constraints should be used in the optimization problem.
Definition: parameters.h:59
VariablePtrVec GetVariableSets()
Definition: nlp_factory.cc:51
double min_phase_duration_
When optimizing over phase duration, this is the minimum allowed.
Definition: parameters.h:86
std::vector< bool > ee_in_contact_at_start_
True if the foot is initially in contact with the terrain.
Definition: parameters.h:83
std::vector< PhaseDurations::Ptr > MakeContactScheduleVariables() const
Definition: nlp_factory.cc:177
CostWeights costs_
Which costs should be used in the optimiation problem.
Definition: parameters.h:62
double max_phase_duration_
When optimizing over phase duration, this is is maximum allowed.
Definition: parameters.h:89
ContraintPtrVec MakeTerrainConstraint() const
Definition: nlp_factory.cc:268
int GetPhaseCount(EEID ee) const
The number of phases allowed for endeffector ee.
Definition: parameters.cc:105
int GetEECount() const
The number of endeffectors.
Definition: parameters.cc:111
ContraintPtrVec GetCosts() const
Definition: nlp_factory.cc:323
ContraintPtrVec MakeRangeOfMotionBoxConstraint() const
Definition: nlp_factory.cc:238
std::vector< PhaseNodes::Ptr > MakeForceVariables() const
Definition: nlp_factory.cc:150
std::vector< ifopt::VariableSet::Ptr > VariablePtrVec
Definition: nlp_factory.h:53
Holds pointers to fully constructed splines, that are linked to the optimization variables.
Definition: spline_holder.h:46
std::vector< ifopt::CostTerm::Ptr > CostPtrVec
Definition: nlp_factory.h:55
NodeSpline::Ptr base_linear_
Definition: spline_holder.h:68
static const std::string contact_schedule
int force_polynomials_per_stance_phase_
Number of polynomials to parameterize each contact force during stance phase.
Definition: parameters.h:95
ContraintPtrVec MakeSwingConstraint() const
Definition: nlp_factory.cc:296
const VectorXd p() const
Definition: state.cc:53
std::vector< VecTimes > ee_phase_durations_
Number and initial duration of each foot&#39;s swing and stance phases.
Definition: parameters.h:80
VecTimes GetBasePolyDurations() const
The durations of each base polynomial in the spline (lin+ang).
Definition: parameters.cc:87
ContraintPtrVec MakeBaseRangeOfMotionConstraint() const
Definition: nlp_factory.cc:223
Parameters params_
Definition: nlp_factory.h:83
static std::string EEMotionNodes(uint ee)
Eigen::Vector3d Vector3d
Definition: nlp_factory.h:57
ConstraintName
Definition: parameters.h:38
NodeSpline::Ptr base_angular_
Definition: spline_holder.h:69
Node lin
linear position x,y,z and velocities.
Definition: state.h:125
double force_limit_in_norm_
The maximum allowable force [N] in normal direction.
Definition: parameters.h:98
bool OptimizeTimings() const
True if the phase durations should be optimized over.
Definition: parameters.cc:79
ContraintPtrVec MakeBaseAccConstraint() const
Definition: nlp_factory.cc:309
static std::string EEForceNodes(uint ee)
CostPtrVec MakeForcesCost(double weight) const
Definition: nlp_factory.cc:343
ContraintPtrVec MakeDynamicConstraint() const
Definition: nlp_factory.cc:229
double t_total_
Total duration [s] of the walking motion.
Definition: parameters.h:65
static const std::string base_ang_nodes
const VectorXd v() const
Definition: state.cc:59
BaseState final_base_
Definition: nlp_factory.h:79
RobotModel model_
Definition: nlp_factory.h:81
CostName
Definition: parameters.h:40
SplineHolder spline_holder_
Definition: nlp_factory.h:85
KinematicModel::Ptr kinematic_model_
Definition: robot_model.h:43
std::vector< Nodes::Ptr > MakeBaseVariables() const
Definition: nlp_factory.cc:82
Node ang
angular euler roll, pitch, yaw and rates.
Definition: state.h:126
HeightMap::Ptr terrain_
Definition: nlp_factory.h:82
static const std::string base_lin_nodes


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