nodes_variables_phase_based.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 
32 
33 #include <iostream>
34 
35 namespace towr {
36 
37 
38 std::vector<NodesVariablesPhaseBased::PolyInfo>
39 BuildPolyInfos (int phase_count, bool first_phase_constant,
40  int n_polys_in_changing_phase)
41 {
42  using PolyInfo = NodesVariablesPhaseBased::PolyInfo;
43  std::vector<PolyInfo> polynomial_info;
44 
45  bool phase_constant = first_phase_constant;
46 
47  for (int i=0; i<phase_count; ++i) {
48  if (phase_constant)
49  polynomial_info.push_back(PolyInfo(i,0,1, true));
50  else
51  for (int j=0; j<n_polys_in_changing_phase; ++j)
52  polynomial_info.push_back(PolyInfo(i,j,n_polys_in_changing_phase, false));
53 
54  phase_constant = !phase_constant; // constant and non-constant phase alternate
55  }
56 
57  return polynomial_info;
58 }
59 
61  bool first_phase_constant,
62  const std::string& name,
63  int n_polys_in_changing_phase)
64  :NodesVariables(name)
65 {
66  polynomial_info_ = BuildPolyInfos(phase_count, first_phase_constant, n_polys_in_changing_phase);
67 
68  n_dim_ = k3D;
69  int n_nodes = polynomial_info_.size()+1;
70  nodes_ = std::vector<Node>(n_nodes, Node(n_dim_));
71 }
72 
75 {
76  VecDurations poly_durations;
77 
78  for (int i=0; i<GetPolynomialCount(); ++i) {
79  auto info = polynomial_info_.at(i);
80  poly_durations.push_back(phase_durations.at(info.phase_)/info.n_polys_in_phase_);
81  }
82 
83  return poly_durations;
84 }
85 
86 double
88 {
89  int n_polys_in_phase = polynomial_info_.at(poly_id).n_polys_in_phase_;
90  return 1./n_polys_in_phase;
91 }
92 
93 int
95 {
96  return polynomial_info_.at(poly_id).poly_in_phase_;
97 }
98 
99 bool
101 {
102  bool is_constant = false;
103 
104  // node is considered constant if either left or right polynomial
105  // belongs to a constant phase
106  for (int poly_id : GetAdjacentPolyIds(node_id))
107  if (IsInConstantPhase(poly_id))
108  is_constant = true;
109 
110  return is_constant;
111 }
112 
113 bool
115 {
116  return polynomial_info_.at(poly_id).is_constant_;
117 }
118 
121 {
122  NodeIds node_ids;
123 
124  for (int id=0; id<GetNodes().size(); ++id)
125  if (!IsConstantNode(id))
126  node_ids.push_back(id);
127 
128  return node_ids;
129 }
130 
131 int
133 {
134  assert(!IsConstantNode(node_id)); // because otherwise it has two phases
135 
136  int poly_id = GetAdjacentPolyIds(node_id).front();
137  return polynomial_info_.at(poly_id).phase_;
138 }
139 
140 int
142 {
143  int poly_id=0;
144  for (int i=0; i<polynomial_info_.size(); ++i)
145  if (polynomial_info_.at(i).phase_ == phase)
146  return i;
147 }
148 
149 Eigen::Vector3d
151 {
152  int node_id = GetNodeIDAtStartOfPhase(phase);
153  return GetNodes().at(node_id).p();
154 }
155 
156 int
158 {
159  int poly_id=GetPolyIDAtStartOfPhase(phase);
160  return GetNodeId(poly_id, Side::Start);
161 }
162 
163 std::vector<int>
165 {
166  std::vector<int> poly_ids;
167  int last_node_id = GetNodes().size()-1;
168 
169  if (node_id==0)
170  poly_ids.push_back(0);
171  else if (node_id==last_node_id)
172  poly_ids.push_back(last_node_id-1);
173  else {
174  poly_ids.push_back(node_id-1);
175  poly_ids.push_back(node_id);
176  }
177 
178  return poly_ids;
179 }
180 
181 NodesVariablesPhaseBased::PolyInfo::PolyInfo(int phase, int poly_id_in_phase,
182  int num_polys_in_phase, bool is_constant)
183  :phase_(phase),
184  poly_in_phase_(poly_id_in_phase),
185  n_polys_in_phase_(num_polys_in_phase),
186  is_constant_(is_constant)
187 {
188 }
189 
190 void
192 {
193  bounds_ = VecBound(n_variables, ifopt::NoBound);
194  SetRows(n_variables);
195 }
196 
198  bool is_in_contact_at_start,
199  const std::string& name,
200  int n_polys_in_changing_phase)
201  :NodesVariablesPhaseBased(phase_count,
202  is_in_contact_at_start, // contact phase for motion is constant
203  name,
204  n_polys_in_changing_phase)
205 {
208 }
209 
212 {
213  OptIndexMap index_map;
214 
215  int idx = 0; // index in variables set
216  for (int node_id=0; node_id<nodes_.size(); ++node_id) {
217  // swing node:
218  if (!IsConstantNode(node_id)) {
219  for (int dim=0; dim<GetDim(); ++dim) {
220  // intermediate way-point position of swing motion are optimized
221  index_map[idx++].push_back(NodeValueInfo(node_id, kPos, dim));
222 
223  // velocity in vertical direction fixed to zero and not optimized.
224  // Since we often choose two polynomials per swing-phase, this restricts
225  // the swing to have reached it's extreme at half-time and creates
226  // smoother stepping motions.
227  if (dim == Z)
228  nodes_.at(node_id).at(kVel).z() = 0.0;
229  else
230  // velocity in x,y dimension during swing fully optimized.
231  index_map[idx++].push_back(NodeValueInfo(node_id, kVel, dim));
232  }
233  }
234  // stance node (next one will also be stance, so handle that one too):
235  else {
236  // ensure that foot doesn't move by not even optimizing over velocities
237  nodes_.at(node_id).at(kVel).setZero();
238  nodes_.at(node_id+1).at(kVel).setZero();
239 
240  // position of foot is still an optimization variable used for
241  // both start and end node of that polynomial
242  for (int dim=0; dim<GetDim(); ++dim) {
243  index_map[idx].push_back(NodeValueInfo(node_id, kPos, dim));
244  index_map[idx].push_back(NodeValueInfo(node_id+1, kPos, dim));
245  idx++;
246  }
247 
248  node_id += 1; // already added next constant node, so skip
249  }
250  }
251 
252  return index_map;
253 }
254 
256  bool is_in_contact_at_start,
257  const std::string& name,
258  int n_polys_in_changing_phase)
259  :NodesVariablesPhaseBased(phase_count,
260  !is_in_contact_at_start, // contact phase for force is non-constant
261  name,
262  n_polys_in_changing_phase)
263 {
266 }
267 
270 {
271  OptIndexMap index_map;
272 
273  int idx = 0; // index in variables set
274  for (int id=0; id<nodes_.size(); ++id) {
275  // stance node:
276  // forces can be created during stance, so these nodes are optimized over.
277  if (!IsConstantNode(id)) {
278  for (int dim=0; dim<GetDim(); ++dim) {
279  index_map[idx++].push_back(NodeValueInfo(id, kPos, dim));
280  index_map[idx++].push_back(NodeValueInfo(id, kVel, dim));
281  }
282  }
283  // swing node (next one will also be swing, so handle that one too)
284  else {
285  // forces can't exist during swing phase, so no need to be optimized
286  // -> all node values simply set to zero.
287  nodes_.at(id).at(kPos).setZero();
288  nodes_.at(id+1).at(kPos).setZero();
289 
290  nodes_.at(id).at(kVel).setZero();
291  nodes_.at(id+1).at(kVel).setZero();
292 
293  id += 1; // already added next constant node, so skip
294  }
295  }
296 
297  return index_map;
298 }
299 
300 } /* namespace towr */
int GetPolynomialCount() const
NodesVariablesEEMotion(int phase_count, bool is_in_contact_at_start, const std::string &name, int n_polys_in_changing_phase)
const std::vector< Node > GetNodes() const
std::map< int, std::vector< NodeValueInfo > > OptIndexMap
std::vector< int > GetAdjacentPolyIds(int node_id) const
IDs of the polynomial to the left and right of node_id.
Holds semantic information each polynomial in spline.
Eigen::Vector3d GetValueAtStartOfPhase(int phase) const
virtual bool IsInConstantPhase(int polynomial_id) const
Is the polynomial constant, so not changing the value.
PolyInfo(int phase, int poly_in_phase, int n_polys_in_phase, bool is_const)
Position and velocity of nodes used to generate a Hermite spline.
std::vector< Node > nodes_
static const Bounds NoBound
static int GetNodeId(int poly_id, Side side)
The node ID that belongs to a specific side of a specific polynomial.
std::vector< PolyInfo > polynomial_info_
semantic information associated with each polynomial
Semantic information associated with a scalar node value.
OptIndexMap index_to_node_value_info_
Assign optimization variables to the correct node values.
void SetRows(int num_rows)
NodeIds GetIndicesOfNonConstantNodes() const
The indices of those nodes that don&#39;t belong to a constant phase.
std::vector< NodesVariablesPhaseBased::PolyInfo > BuildPolyInfos(int phase_count, bool first_phase_constant, int n_polys_in_changing_phase)
virtual double GetDerivativeOfPolyDurationWrtPhaseDuration(int polynomial_id) const
How a change in the phase duration affects the polynomial duration.
static constexpr int k3D
std::vector< double > VecDurations
NodesVariablesPhaseBased(int phase_count, bool first_phase_constant, const std::string &var_name, int n_polys_in_changing_phase)
Constructs a variable set of node variables.
int GetPolyIDAtStartOfPhase(int phase) const
ID of the first polynomial of a phase.
Nodes that are associated to either swing or stance phases.
virtual VecDurations ConvertPhaseToPolyDurations(const VecDurations &phase_durations) const
Converts durations of swing and stance phases to polynomial durations.
VecBound bounds_
the bounds on the node values.
std::vector< Bounds > VecBound
virtual bool IsConstantNode(int node_id) const
node is constant if either left or right polynomial belongs to a constant phase.
A node represents the state of a trajectory at a specific time.
Definition: state.h:107
virtual int GetNumberOfPrevPolynomialsInPhase(int polynomial_id) const
How many polynomials in the current phase come before.
NodesVariablesEEForce(int phase_count, bool is_in_contact_at_start, const std::string &name, int n_polys_in_changing_phase)


towr
Author(s): Alexander W. Winkler
autogenerated on Mon Feb 28 2022 23:54:22