gait_generator.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 
31 
32 #include <cassert>
33 #include <numeric> // std::accumulate
34 #include <algorithm> // std::transform
35 
39 
40 namespace towr {
41 
42 
45 {
46  switch (leg_count) {
47  case 1: return std::make_shared<MonopedGaitGenerator>(); break;
48  case 2: return std::make_shared<BipedGaitGenerator>(); break;
49  case 4: return std::make_shared<QuadrupedGaitGenerator>(); break;
50  default: assert(false); break; // Error: Not implemented
51  }
52 }
53 
55 GaitGenerator::GetPhaseDurations (double t_total, EE ee) const
56 {
57  // scale total time tu t_total
58  std::vector<double> durations;
59  for (auto d : GetNormalizedPhaseDurations(ee))
60  durations.push_back(d*t_total);
61 
62  return durations;
63 }
64 
67 {
68  auto v = GetPhaseDurations().at(ee); // shorthand
69  double total_time = std::accumulate(v.begin(), v.end(), 0.0);
70  std::transform(v.begin(), v.end(), v.begin(),
71  [total_time](double t_phase){ return t_phase/total_time;});
72 
73  return v;
74 }
75 
78 {
79  int n_ee = contacts_.front().size();
80  VecTimes d_accumulated(n_ee, 0.0);
81 
82  FootDurations foot_durations(n_ee);
83  for (int phase=0; phase<contacts_.size()-1; ++phase) {
84  ContactState curr = contacts_.at(phase);
85  ContactState next = contacts_.at(phase+1);
86 
87  for (int ee=0; ee<curr.size(); ++ee) {
88  d_accumulated.at(ee) += times_.at(phase);
89 
90  // if contact will change in next phase, so this phase duration complete
91  bool contacts_will_change = curr.at(ee) != next.at(ee);
92  if (contacts_will_change) {
93  foot_durations.at(ee).push_back(d_accumulated.at(ee));
94  d_accumulated.at(ee) = 0.0;
95  }
96  }
97  }
98 
99  // push back last phase
100  for (int ee=0; ee<contacts_.back().size(); ++ee)
101  foot_durations.at(ee).push_back(d_accumulated.at(ee) + times_.back());
102 
103 
104  return foot_durations;
105 }
106 
107 bool
109 {
110  return contacts_.front().at(ee);
111 }
112 
113 void
114 GaitGenerator::SetGaits (const std::vector<Gaits>& gaits)
115 {
116  contacts_.clear();
117  times_.clear();
118 
119  for (Gaits g : gaits) {
120  auto info = GetGait(g);
121 
122  std::vector<double> t = info.first;
123  std::vector<ContactState> c = info.second;
124  assert(t.size() == c.size()); // make sure every phase has a time
125 
126  times_.insert (times_.end(), t.begin(), t.end());
127  contacts_.insert(contacts_.end(), c.begin(), c.end());
128  }
129 }
130 
133 {
134  GaitInfo new_gait = g;
135 
136  // remove the final transition between strides
137  // but ensure that last step duration is not cut off
138  new_gait.first.pop_back();
139  new_gait.first.back() += g.first.back();
140 
141  new_gait.second.pop_back();
142 
143  return new_gait;
144 }
145 
146 } /* namespace towr */
147 
148 
std::vector< double > times_
Phase times for the complete robot during which no contact state changes.
VecTimes GetNormalizedPhaseDurations(EE ee) const
Gaits
Predefined strides, each with a different gait diagram.
GaitInfo RemoveTransition(const GaitInfo &g) const
FootDurations GetPhaseDurations() const
std::vector< ContactState > contacts_
void SetGaits(const std::vector< Gaits > &gaits)
Sets the times_ and contacts_ variables according to the gaits.
std::vector< double > VecTimes
bool IsInContactAtStart(EE ee) const
std::pair< VecTimes, std::vector< ContactState >> GaitInfo
std::shared_ptr< GaitGenerator > Ptr
std::vector< bool > ContactState
static Ptr MakeGaitGenerator(int leg_count)
std::vector< VecTimes > FootDurations
virtual GaitInfo GetGait(Gaits gait) const =0


towr
Author(s): Alexander W. Winkler
autogenerated on Fri Apr 2 2021 02:14:16