nlp_formulation.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/nlp_formulation.h>
00031 
00032 #include <towr/variables/variable_names.h>
00033 #include <towr/variables/phase_durations.h>
00034 
00035 #include <towr/constraints/base_motion_constraint.h>
00036 #include <towr/constraints/dynamic_constraint.h>
00037 #include <towr/constraints/force_constraint.h>
00038 #include <towr/constraints/range_of_motion_constraint.h>
00039 #include <towr/constraints/swing_constraint.h>
00040 #include <towr/constraints/terrain_constraint.h>
00041 #include <towr/constraints/total_duration_constraint.h>
00042 #include <towr/constraints/spline_acc_constraint.h>
00043 
00044 #include <towr/costs/node_cost.h>
00045 #include <towr/variables/nodes_variables_all.h>
00046 
00047 #include <iostream>
00048 
00049 namespace towr {
00050 
00051 NlpFormulation::NlpFormulation ()
00052 {
00053   using namespace std;
00054   cout << "\n";
00055   cout << "************************************************************\n";
00056   cout << " TOWR - Trajectory Optimization for Walking Robots (v1.4)\n";
00057   cout << "                \u00a9 Alexander W. Winkler\n";
00058   cout << "           https://github.com/ethz-adrl/towr\n";
00059   cout << "************************************************************";
00060   cout << "\n\n";
00061 }
00062 
00063 NlpFormulation::VariablePtrVec
00064 NlpFormulation::GetVariableSets (SplineHolder& spline_holder)
00065 {
00066   VariablePtrVec vars;
00067 
00068   auto base_motion = MakeBaseVariables();
00069   vars.insert(vars.end(), base_motion.begin(), base_motion.end());
00070 
00071   auto ee_motion = MakeEndeffectorVariables();
00072   vars.insert(vars.end(), ee_motion.begin(), ee_motion.end());
00073 
00074   auto ee_force = MakeForceVariables();
00075   vars.insert(vars.end(), ee_force.begin(), ee_force.end());
00076 
00077   auto contact_schedule = MakeContactScheduleVariables();
00078   // can also just be fixed timings that aren't optimized over, but still added
00079   // to spline_holder.
00080   if (params_.IsOptimizeTimings()) {
00081     vars.insert(vars.end(), contact_schedule.begin(), contact_schedule.end());
00082   }
00083 
00084   // stores these readily constructed spline
00085   spline_holder = SplineHolder(base_motion.at(0), // linear
00086                                base_motion.at(1), // angular
00087                                params_.GetBasePolyDurations(),
00088                                ee_motion,
00089                                ee_force,
00090                                contact_schedule,
00091                                params_.IsOptimizeTimings());
00092   return vars;
00093 }
00094 
00095 std::vector<NodesVariables::Ptr>
00096 NlpFormulation::MakeBaseVariables () const
00097 {
00098   std::vector<NodesVariables::Ptr> vars;
00099 
00100   int n_nodes = params_.GetBasePolyDurations().size() + 1;
00101 
00102   auto spline_lin = std::make_shared<NodesVariablesAll>(n_nodes, k3D, id::base_lin_nodes);
00103 
00104   double x = final_base_.lin.p().x();
00105   double y = final_base_.lin.p().y();
00106   double z = terrain_->GetHeight(x,y) - model_.kinematic_model_->GetNominalStanceInBase().front().z();
00107   Vector3d final_pos(x, y, z);
00108 
00109   spline_lin->SetByLinearInterpolation(initial_base_.lin.p(), final_pos, params_.GetTotalTime());
00110   spline_lin->AddStartBound(kPos, {X,Y,Z}, initial_base_.lin.p());
00111   spline_lin->AddStartBound(kVel, {X,Y,Z}, initial_base_.lin.v());
00112   spline_lin->AddFinalBound(kPos, params_.bounds_final_lin_pos_,   final_base_.lin.p());
00113   spline_lin->AddFinalBound(kVel, params_.bounds_final_lin_vel_, final_base_.lin.v());
00114   vars.push_back(spline_lin);
00115 
00116   auto spline_ang = std::make_shared<NodesVariablesAll>(n_nodes, k3D, id::base_ang_nodes);
00117   spline_ang->SetByLinearInterpolation(initial_base_.ang.p(), final_base_.ang.p(), params_.GetTotalTime());
00118   spline_ang->AddStartBound(kPos, {X,Y,Z}, initial_base_.ang.p());
00119   spline_ang->AddStartBound(kVel, {X,Y,Z}, initial_base_.ang.v());
00120   spline_ang->AddFinalBound(kPos, params_.bounds_final_ang_pos_, final_base_.ang.p());
00121   spline_ang->AddFinalBound(kVel, params_.bounds_final_ang_vel_, final_base_.ang.v());
00122   vars.push_back(spline_ang);
00123 
00124   return vars;
00125 }
00126 
00127 std::vector<NodesVariablesPhaseBased::Ptr>
00128 NlpFormulation::MakeEndeffectorVariables () const
00129 {
00130   std::vector<NodesVariablesPhaseBased::Ptr> vars;
00131 
00132   // Endeffector Motions
00133   double T = params_.GetTotalTime();
00134   for (int ee=0; ee<params_.GetEECount(); ee++) {
00135     auto nodes = std::make_shared<NodesVariablesEEMotion>(
00136                                               params_.GetPhaseCount(ee),
00137                                               params_.ee_in_contact_at_start_.at(ee),
00138                                               id::EEMotionNodes(ee),
00139                                               params_.ee_polynomials_per_swing_phase_);
00140 
00141     // initialize towards final footholds
00142     double yaw = final_base_.ang.p().z();
00143     Eigen::Vector3d euler(0.0, 0.0, yaw);
00144     Eigen::Matrix3d w_R_b = EulerConverter::GetRotationMatrixBaseToWorld(euler);
00145     Vector3d final_ee_pos_W = final_base_.lin.p() + w_R_b*model_.kinematic_model_->GetNominalStanceInBase().at(ee);
00146     double x = final_ee_pos_W.x();
00147     double y = final_ee_pos_W.y();
00148     double z = terrain_->GetHeight(x,y);
00149     nodes->SetByLinearInterpolation(initial_ee_W_.at(ee), Vector3d(x,y,z), T);
00150 
00151     nodes->AddStartBound(kPos, {X,Y,Z}, initial_ee_W_.at(ee));
00152     vars.push_back(nodes);
00153   }
00154 
00155   return vars;
00156 }
00157 
00158 std::vector<NodesVariablesPhaseBased::Ptr>
00159 NlpFormulation::MakeForceVariables () const
00160 {
00161   std::vector<NodesVariablesPhaseBased::Ptr> vars;
00162 
00163   double T = params_.GetTotalTime();
00164   for (int ee=0; ee<params_.GetEECount(); ee++) {
00165     auto nodes = std::make_shared<NodesVariablesEEForce>(
00166                                               params_.GetPhaseCount(ee),
00167                                               params_.ee_in_contact_at_start_.at(ee),
00168                                               id::EEForceNodes(ee),
00169                                               params_.force_polynomials_per_stance_phase_);
00170 
00171     // initialize with mass of robot distributed equally on all legs
00172     double m = model_.dynamic_model_->m();
00173     double g = model_.dynamic_model_->g();
00174 
00175     Vector3d f_stance(0.0, 0.0, m*g/params_.GetEECount());
00176     nodes->SetByLinearInterpolation(f_stance, f_stance, T); // stay constant
00177     vars.push_back(nodes);
00178   }
00179 
00180   return vars;
00181 }
00182 
00183 std::vector<PhaseDurations::Ptr>
00184 NlpFormulation::MakeContactScheduleVariables () const
00185 {
00186   std::vector<PhaseDurations::Ptr> vars;
00187 
00188   for (int ee=0; ee<params_.GetEECount(); ee++) {
00189     auto var = std::make_shared<PhaseDurations>(ee,
00190                                                 params_.ee_phase_durations_.at(ee),
00191                                                 params_.ee_in_contact_at_start_.at(ee),
00192                                                 params_.bound_phase_duration_.first,
00193                                                 params_.bound_phase_duration_.second);
00194     vars.push_back(var);
00195   }
00196 
00197   return vars;
00198 }
00199 
00200 NlpFormulation::ContraintPtrVec
00201 NlpFormulation::GetConstraints(const SplineHolder& spline_holder) const
00202 {
00203   ContraintPtrVec constraints;
00204   for (auto name : params_.constraints_)
00205     for (auto c : GetConstraint(name, spline_holder))
00206       constraints.push_back(c);
00207 
00208   return constraints;
00209 }
00210 
00211 NlpFormulation::ContraintPtrVec
00212 NlpFormulation::GetConstraint (Parameters::ConstraintName name,
00213                            const SplineHolder& s) const
00214 {
00215   switch (name) {
00216     case Parameters::Dynamic:        return MakeDynamicConstraint(s);
00217     case Parameters::EndeffectorRom: return MakeRangeOfMotionBoxConstraint(s);
00218     case Parameters::BaseRom:        return MakeBaseRangeOfMotionConstraint(s);
00219     case Parameters::TotalTime:      return MakeTotalTimeConstraint();
00220     case Parameters::Terrain:        return MakeTerrainConstraint();
00221     case Parameters::Force:          return MakeForceConstraint();
00222     case Parameters::Swing:          return MakeSwingConstraint();
00223     case Parameters::BaseAcc:        return MakeBaseAccConstraint(s);
00224     default: throw std::runtime_error("constraint not defined!");
00225   }
00226 }
00227 
00228 
00229 NlpFormulation::ContraintPtrVec
00230 NlpFormulation::MakeBaseRangeOfMotionConstraint (const SplineHolder& s) const
00231 {
00232   return {std::make_shared<BaseMotionConstraint>(params_.GetTotalTime(),
00233                                                  params_.dt_constraint_base_motion_,
00234                                                  s)};
00235 }
00236 
00237 NlpFormulation::ContraintPtrVec
00238 NlpFormulation::MakeDynamicConstraint(const SplineHolder& s) const
00239 {
00240   auto constraint = std::make_shared<DynamicConstraint>(model_.dynamic_model_,
00241                                                         params_.GetTotalTime(),
00242                                                         params_.dt_constraint_dynamic_,
00243                                                         s);
00244   return {constraint};
00245 }
00246 
00247 NlpFormulation::ContraintPtrVec
00248 NlpFormulation::MakeRangeOfMotionBoxConstraint (const SplineHolder& s) const
00249 {
00250   ContraintPtrVec c;
00251 
00252   for (int ee=0; ee<params_.GetEECount(); ee++) {
00253     auto rom = std::make_shared<RangeOfMotionConstraint>(model_.kinematic_model_,
00254                                                          params_.GetTotalTime(),
00255                                                          params_.dt_constraint_range_of_motion_,
00256                                                          ee,
00257                                                          s);
00258     c.push_back(rom);
00259   }
00260 
00261   return c;
00262 }
00263 
00264 NlpFormulation::ContraintPtrVec
00265 NlpFormulation::MakeTotalTimeConstraint () const
00266 {
00267   ContraintPtrVec c;
00268   double T = params_.GetTotalTime();
00269 
00270   for (int ee=0; ee<params_.GetEECount(); ee++) {
00271     auto duration_constraint = std::make_shared<TotalDurationConstraint>(T, ee);
00272     c.push_back(duration_constraint);
00273   }
00274 
00275   return c;
00276 }
00277 
00278 NlpFormulation::ContraintPtrVec
00279 NlpFormulation::MakeTerrainConstraint () const
00280 {
00281   ContraintPtrVec constraints;
00282 
00283   for (int ee=0; ee<params_.GetEECount(); ee++) {
00284     auto c = std::make_shared<TerrainConstraint>(terrain_, id::EEMotionNodes(ee));
00285     constraints.push_back(c);
00286   }
00287 
00288   return constraints;
00289 }
00290 
00291 NlpFormulation::ContraintPtrVec
00292 NlpFormulation::MakeForceConstraint () const
00293 {
00294   ContraintPtrVec constraints;
00295 
00296   for (int ee=0; ee<params_.GetEECount(); ee++) {
00297     auto c = std::make_shared<ForceConstraint>(terrain_,
00298                                                params_.force_limit_in_normal_direction_,
00299                                                ee);
00300     constraints.push_back(c);
00301   }
00302 
00303   return constraints;
00304 }
00305 
00306 NlpFormulation::ContraintPtrVec
00307 NlpFormulation::MakeSwingConstraint () const
00308 {
00309   ContraintPtrVec constraints;
00310 
00311   for (int ee=0; ee<params_.GetEECount(); ee++) {
00312     auto swing = std::make_shared<SwingConstraint>(id::EEMotionNodes(ee));
00313     constraints.push_back(swing);
00314   }
00315 
00316   return constraints;
00317 }
00318 
00319 NlpFormulation::ContraintPtrVec
00320 NlpFormulation::MakeBaseAccConstraint (const SplineHolder& s) const
00321 {
00322   ContraintPtrVec constraints;
00323 
00324   constraints.push_back(std::make_shared<SplineAccConstraint>
00325                         (s.base_linear_, id::base_lin_nodes));
00326 
00327   constraints.push_back(std::make_shared<SplineAccConstraint>
00328                         (s.base_angular_, id::base_ang_nodes));
00329 
00330   return constraints;
00331 }
00332 
00333 NlpFormulation::ContraintPtrVec
00334 NlpFormulation::GetCosts() const
00335 {
00336   ContraintPtrVec costs;
00337   for (const auto& pair : params_.costs_)
00338     for (auto c : GetCost(pair.first, pair.second))
00339       costs.push_back(c);
00340 
00341   return costs;
00342 }
00343 
00344 NlpFormulation::CostPtrVec
00345 NlpFormulation::GetCost(const Parameters::CostName& name, double weight) const
00346 {
00347   switch (name) {
00348     case Parameters::ForcesCostID:   return MakeForcesCost(weight);
00349     case Parameters::EEMotionCostID: return MakeEEMotionCost(weight);
00350     default: throw std::runtime_error("cost not defined!");
00351   }
00352 }
00353 
00354 NlpFormulation::CostPtrVec
00355 NlpFormulation::MakeForcesCost(double weight) const
00356 {
00357   CostPtrVec cost;
00358 
00359   for (int ee=0; ee<params_.GetEECount(); ee++)
00360     cost.push_back(std::make_shared<NodeCost>(id::EEForceNodes(ee), kPos, Z, weight));
00361 
00362   return cost;
00363 }
00364 
00365 NlpFormulation::CostPtrVec
00366 NlpFormulation::MakeEEMotionCost(double weight) const
00367 {
00368   CostPtrVec cost;
00369 
00370   for (int ee=0; ee<params_.GetEECount(); ee++) {
00371     cost.push_back(std::make_shared<NodeCost>(id::EEMotionNodes(ee), kVel, X, weight));
00372     cost.push_back(std::make_shared<NodeCost>(id::EEMotionNodes(ee), kVel, Y, weight));
00373   }
00374 
00375   return cost;
00376 }
00377 
00378 } /* namespace towr */


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