nlp_formulation.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_formulation.h>
31 
34 
43 
44 #include <towr/costs/node_cost.h>
46 
47 #include <iostream>
48 
49 namespace towr {
50 
52 {
53  using namespace std;
54  cout << "\n";
55  cout << "************************************************************\n";
56  cout << " TOWR - Trajectory Optimization for Walking Robots (v1.4)\n";
57  cout << " \u00a9 Alexander W. Winkler\n";
58  cout << " https://github.com/ethz-adrl/towr\n";
59  cout << "************************************************************";
60  cout << "\n\n";
61 }
62 
65 {
66  VariablePtrVec vars;
67 
68  auto base_motion = MakeBaseVariables();
69  vars.insert(vars.end(), base_motion.begin(), base_motion.end());
70 
71  auto ee_motion = MakeEndeffectorVariables();
72  vars.insert(vars.end(), ee_motion.begin(), ee_motion.end());
73 
74  auto ee_force = MakeForceVariables();
75  vars.insert(vars.end(), ee_force.begin(), ee_force.end());
76 
78  // can also just be fixed timings that aren't optimized over, but still added
79  // to spline_holder.
80  if (params_.IsOptimizeTimings()) {
81  vars.insert(vars.end(), contact_schedule.begin(), contact_schedule.end());
82  }
83 
84  // stores these readily constructed spline
85  spline_holder = SplineHolder(base_motion.at(0), // linear
86  base_motion.at(1), // angular
88  ee_motion,
89  ee_force,
92  return vars;
93 }
94 
95 std::vector<NodesVariables::Ptr>
97 {
98  std::vector<NodesVariables::Ptr> vars;
99 
100  int n_nodes = params_.GetBasePolyDurations().size() + 1;
101 
102  auto spline_lin = std::make_shared<NodesVariablesAll>(n_nodes, k3D, id::base_lin_nodes);
103 
104  double x = final_base_.lin.p().x();
105  double y = final_base_.lin.p().y();
106  double z = terrain_->GetHeight(x,y) - model_.kinematic_model_->GetNominalStanceInBase().front().z();
107  Vector3d final_pos(x, y, z);
108 
109  spline_lin->SetByLinearInterpolation(initial_base_.lin.p(), final_pos, params_.GetTotalTime());
110  spline_lin->AddStartBound(kPos, {X,Y,Z}, initial_base_.lin.p());
111  spline_lin->AddStartBound(kVel, {X,Y,Z}, initial_base_.lin.v());
112  spline_lin->AddFinalBound(kPos, params_.bounds_final_lin_pos_, final_base_.lin.p());
113  spline_lin->AddFinalBound(kVel, params_.bounds_final_lin_vel_, final_base_.lin.v());
114  vars.push_back(spline_lin);
115 
116  auto spline_ang = std::make_shared<NodesVariablesAll>(n_nodes, k3D, id::base_ang_nodes);
117  spline_ang->SetByLinearInterpolation(initial_base_.ang.p(), final_base_.ang.p(), params_.GetTotalTime());
118  spline_ang->AddStartBound(kPos, {X,Y,Z}, initial_base_.ang.p());
119  spline_ang->AddStartBound(kVel, {X,Y,Z}, initial_base_.ang.v());
120  spline_ang->AddFinalBound(kPos, params_.bounds_final_ang_pos_, final_base_.ang.p());
121  spline_ang->AddFinalBound(kVel, params_.bounds_final_ang_vel_, final_base_.ang.v());
122  vars.push_back(spline_ang);
123 
124  return vars;
125 }
126 
127 std::vector<NodesVariablesPhaseBased::Ptr>
129 {
130  std::vector<NodesVariablesPhaseBased::Ptr> vars;
131 
132  // Endeffector Motions
133  double T = params_.GetTotalTime();
134  for (int ee=0; ee<params_.GetEECount(); ee++) {
135  auto nodes = std::make_shared<NodesVariablesEEMotion>(
138  id::EEMotionNodes(ee),
140 
141  // initialize towards final footholds
142  double yaw = final_base_.ang.p().z();
143  Eigen::Vector3d euler(0.0, 0.0, yaw);
144  Eigen::Matrix3d w_R_b = EulerConverter::GetRotationMatrixBaseToWorld(euler);
145  Vector3d final_ee_pos_W = final_base_.lin.p() + w_R_b*model_.kinematic_model_->GetNominalStanceInBase().at(ee);
146  double x = final_ee_pos_W.x();
147  double y = final_ee_pos_W.y();
148  double z = terrain_->GetHeight(x,y);
149  nodes->SetByLinearInterpolation(initial_ee_W_.at(ee), Vector3d(x,y,z), T);
150 
151  nodes->AddStartBound(kPos, {X,Y,Z}, initial_ee_W_.at(ee));
152  vars.push_back(nodes);
153  }
154 
155  return vars;
156 }
157 
158 std::vector<NodesVariablesPhaseBased::Ptr>
160 {
161  std::vector<NodesVariablesPhaseBased::Ptr> vars;
162 
163  double T = params_.GetTotalTime();
164  for (int ee=0; ee<params_.GetEECount(); ee++) {
165  auto nodes = std::make_shared<NodesVariablesEEForce>(
168  id::EEForceNodes(ee),
170 
171  // initialize with mass of robot distributed equally on all legs
172  double m = model_.dynamic_model_->m();
173  double g = model_.dynamic_model_->g();
174 
175  Vector3d f_stance(0.0, 0.0, m*g/params_.GetEECount());
176  nodes->SetByLinearInterpolation(f_stance, f_stance, T); // stay constant
177  vars.push_back(nodes);
178  }
179 
180  return vars;
181 }
182 
183 std::vector<PhaseDurations::Ptr>
185 {
186  std::vector<PhaseDurations::Ptr> vars;
187 
188  for (int ee=0; ee<params_.GetEECount(); ee++) {
189  auto var = std::make_shared<PhaseDurations>(ee,
194  vars.push_back(var);
195  }
196 
197  return vars;
198 }
199 
201 NlpFormulation::GetConstraints(const SplineHolder& spline_holder) const
202 {
203  ContraintPtrVec constraints;
204  for (auto name : params_.constraints_)
205  for (auto c : GetConstraint(name, spline_holder))
206  constraints.push_back(c);
207 
208  return constraints;
209 }
210 
213  const SplineHolder& s) const
214 {
215  switch (name) {
221  case Parameters::Force: return MakeForceConstraint();
222  case Parameters::Swing: return MakeSwingConstraint();
224  default: throw std::runtime_error("constraint not defined!");
225  }
226 }
227 
228 
231 {
232  return {std::make_shared<BaseMotionConstraint>(params_.GetTotalTime(),
234  s)};
235 }
236 
239 {
240  auto constraint = std::make_shared<DynamicConstraint>(model_.dynamic_model_,
243  s);
244  return {constraint};
245 }
246 
249 {
250  ContraintPtrVec c;
251 
252  for (int ee=0; ee<params_.GetEECount(); ee++) {
253  auto rom = std::make_shared<RangeOfMotionConstraint>(model_.kinematic_model_,
256  ee,
257  s);
258  c.push_back(rom);
259  }
260 
261  return c;
262 }
263 
266 {
267  ContraintPtrVec c;
268  double T = params_.GetTotalTime();
269 
270  for (int ee=0; ee<params_.GetEECount(); ee++) {
271  auto duration_constraint = std::make_shared<TotalDurationConstraint>(T, ee);
272  c.push_back(duration_constraint);
273  }
274 
275  return c;
276 }
277 
280 {
281  ContraintPtrVec constraints;
282 
283  for (int ee=0; ee<params_.GetEECount(); ee++) {
284  auto c = std::make_shared<TerrainConstraint>(terrain_, id::EEMotionNodes(ee));
285  constraints.push_back(c);
286  }
287 
288  return constraints;
289 }
290 
293 {
294  ContraintPtrVec constraints;
295 
296  for (int ee=0; ee<params_.GetEECount(); ee++) {
297  auto c = std::make_shared<ForceConstraint>(terrain_,
299  ee);
300  constraints.push_back(c);
301  }
302 
303  return constraints;
304 }
305 
308 {
309  ContraintPtrVec constraints;
310 
311  for (int ee=0; ee<params_.GetEECount(); ee++) {
312  auto swing = std::make_shared<SwingConstraint>(id::EEMotionNodes(ee));
313  constraints.push_back(swing);
314  }
315 
316  return constraints;
317 }
318 
321 {
322  ContraintPtrVec constraints;
323 
324  constraints.push_back(std::make_shared<SplineAccConstraint>
326 
327  constraints.push_back(std::make_shared<SplineAccConstraint>
329 
330  return constraints;
331 }
332 
335 {
336  ContraintPtrVec costs;
337  for (const auto& pair : params_.costs_)
338  for (auto c : GetCost(pair.first, pair.second))
339  costs.push_back(c);
340 
341  return costs;
342 }
343 
345 NlpFormulation::GetCost(const Parameters::CostName& name, double weight) const
346 {
347  switch (name) {
348  case Parameters::ForcesCostID: return MakeForcesCost(weight);
349  case Parameters::EEMotionCostID: return MakeEEMotionCost(weight);
350  default: throw std::runtime_error("cost not defined!");
351  }
352 }
353 
355 NlpFormulation::MakeForcesCost(double weight) const
356 {
357  CostPtrVec cost;
358 
359  for (int ee=0; ee<params_.GetEECount(); ee++)
360  cost.push_back(std::make_shared<NodeCost>(id::EEForceNodes(ee), kPos, Z, weight));
361 
362  return cost;
363 }
364 
367 {
368  CostPtrVec cost;
369 
370  for (int ee=0; ee<params_.GetEECount(); ee++) {
371  cost.push_back(std::make_shared<NodeCost>(id::EEMotionNodes(ee), kVel, X, weight));
372  cost.push_back(std::make_shared<NodeCost>(id::EEMotionNodes(ee), kVel, Y, weight));
373  }
374 
375  return cost;
376 }
377 
378 } /* namespace towr */
std::pair< double, double > bound_phase_duration_
Definition: parameters.h:212
DynamicModel::Ptr dynamic_model_
Definition: robot_model.h:81
sets SplineAccConstraint
Definition: parameters.h:146
sets DynamicConstraint
Definition: parameters.h:139
int force_polynomials_per_stance_phase_
Number of polynomials to parameterize each contact force during stance phase.
Definition: parameters.h:195
sets RangeOfMotionConstraint
Definition: parameters.h:140
ContraintPtrVec MakeTerrainConstraint() const
std::vector< NodesVariablesPhaseBased::Ptr > MakeEndeffectorVariables() const
std::vector< ifopt::ConstraintSet::Ptr > ContraintPtrVec
ConstraintName
Identifiers to be used to add certain constraints to the optimization problem.
Definition: parameters.h:139
ContraintPtrVec GetConstraint(Parameters::ConstraintName name, const SplineHolder &splines) const
sets BaseMotionConstraint
Definition: parameters.h:145
double GetTotalTime() const
Total duration [s] of the motion.
Definition: parameters.cc:113
std::vector< int > bounds_final_ang_vel_
Definition: parameters.h:201
CostPtrVec GetCost(const Parameters::CostName &id, double weight) const
ContraintPtrVec GetConstraints(const SplineHolder &spline_holder) const
The ifopt constraints that enforce feasible motions.
ContraintPtrVec MakeBaseRangeOfMotionConstraint(const SplineHolder &s) const
sets NodeCost on endeffector velocity
Definition: parameters.h:153
sets SwingConstraint
Definition: parameters.h:144
sets ForceConstraint
Definition: parameters.h:143
std::vector< int > bounds_final_lin_pos_
which dimensions (x,y,z) of the final base state should be bounded
Definition: parameters.h:201
CostName
Indentifiers to be used to add certain costs to the optimization problem.
Definition: parameters.h:152
std::vector< ifopt::VariableSet::Ptr > VariablePtrVec
VecTimes GetBasePolyDurations() const
The durations of each base polynomial in the spline (lin+ang).
Definition: parameters.cc:83
int ee_polynomials_per_swing_phase_
Number of polynomials to parameterize foot movement during swing phases.
Definition: parameters.h:192
double dt_constraint_range_of_motion_
Interval at which the range of motion constraint is enforced.
Definition: parameters.h:183
std::vector< PhaseDurations::Ptr > MakeContactScheduleVariables() const
int GetEECount() const
The number of endeffectors.
Definition: parameters.cc:107
ContraintPtrVec MakeForceConstraint() const
std::vector< int > bounds_final_lin_vel_
Definition: parameters.h:201
double dt_constraint_dynamic_
Interval at which the dynamic constraint is enforced.
Definition: parameters.h:180
ContraintPtrVec GetCosts() const
The ifopt costs to tune the motion.
bool IsOptimizeTimings() const
True if the phase durations should be optimized over.
Definition: parameters.cc:129
sets NodeCost on force nodes
Definition: parameters.h:152
Builds splines from node values (pos/vel) and durations.
Definition: spline_holder.h:47
std::vector< NodesVariablesPhaseBased::Ptr > MakeForceVariables() const
NodeSpline::Ptr base_linear_
Definition: spline_holder.h:71
ContraintPtrVec MakeDynamicConstraint(const SplineHolder &s) const
std::vector< VecTimes > ee_phase_durations_
Number and initial duration of each foot&#39;s swing and stance phases.
Definition: parameters.h:168
static const std::string contact_schedule
CostPtrVec MakeForcesCost(double weight) const
const VectorXd p() const
read access to the zero-derivative of the state, e.g. position.
Definition: state.cc:53
double force_limit_in_normal_direction_
The maximum allowable force [N] in normal direction.
Definition: parameters.h:198
sets TotalDurationConstraint
Definition: parameters.h:141
double dt_constraint_base_motion_
Interval at which the base motion constraint is enforced.
Definition: parameters.h:186
std::vector< int > bounds_final_ang_pos_
Definition: parameters.h:201
static std::string EEMotionNodes(uint ee)
ContraintPtrVec MakeTotalTimeConstraint() const
HeightMap::Ptr terrain_
NodeSpline::Ptr base_angular_
Definition: spline_holder.h:72
Node lin
linear position x,y,z and velocities.
Definition: state.h:125
std::vector< NodesVariables::Ptr > MakeBaseVariables() const
ContraintPtrVec MakeBaseAccConstraint(const SplineHolder &s) const
static constexpr int k3D
VariablePtrVec GetVariableSets(SplineHolder &spline_holder)
The ifopt variable sets that will be optimized over.
std::vector< bool > ee_in_contact_at_start_
True if the foot is initially in contact with the terrain.
Definition: parameters.h:171
MatrixSXd GetRotationMatrixBaseToWorld(double t) const
Converts the Euler angles at time t to a rotation matrix.
static std::string EEForceNodes(uint ee)
UsedConstraints constraints_
Which constraints should be used in the optimization problem.
Definition: parameters.h:174
static const std::string base_ang_nodes
Eigen::Vector3d Vector3d
const VectorXd v() const
read access to the first-derivative of the state, e.g. velocity.
Definition: state.cc:59
std::vector< ifopt::CostTerm::Ptr > CostPtrVec
CostWeights costs_
Which costs should be used in the optimiation problem.
Definition: parameters.h:177
ContraintPtrVec MakeSwingConstraint() const
int GetPhaseCount(EEID ee) const
The number of phases allowed for endeffector ee.
Definition: parameters.cc:101
KinematicModel::Ptr kinematic_model_
Definition: robot_model.h:80
ContraintPtrVec MakeRangeOfMotionBoxConstraint(const SplineHolder &s) const
sets TerrainConstraint
Definition: parameters.h:142
CostPtrVec MakeEEMotionCost(double weight) const
Node ang
angular euler roll, pitch, yaw and rates.
Definition: state.h:126
static const std::string base_lin_nodes


towr
Author(s): Alexander W. Winkler
autogenerated on Sat Apr 13 2019 02:28:00