force_constraint.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 
33 
34 namespace towr {
35 
36 
38  double force_limit,
39  EE ee)
40  :ifopt::ConstraintSet(kSpecifyLater, "Force-Constraint-" + id::EEForceNodes(ee))
41 {
42  terrain_ = terrain;
43  fn_max_ = force_limit;
44  mu_ = terrain->GetFrictionCoeff();
45  ee_ = ee;
46 
47  n_constraints_per_node_ = 1 + 2*k2D; // positive normal force + 4 friction pyramid constraints
48 }
49 
50 void
52 {
53  ee_force_ = x->GetComponent<PhaseNodes>(id::EEForceNodes(ee_));
54  ee_motion_ = x->GetComponent<PhaseNodes>(id::EEMotionNodes(ee_));
55 
56  pure_stance_force_node_ids_ = ee_force_->GetIndicesOfNonConstantNodes();
57 
58  int constraint_count = pure_stance_force_node_ids_.size()*n_constraints_per_node_;
59  SetRows(constraint_count);
60 }
61 
64 {
65  VectorXd g(GetRows());
66 
67  int row=0;
68  auto force_nodes = ee_force_->GetNodes();
69  for (int f_node_id : pure_stance_force_node_ids_) {
70 
71  int phase = ee_force_->GetPhase(f_node_id);
72  Vector3d p = ee_motion_->GetValueAtStartOfPhase(phase); // doesn't change during stance phase
73  Vector3d n = terrain_->GetNormalizedBasis(HeightMap::Normal, p.x(), p.y());
74  Vector3d f = force_nodes.at(f_node_id).p();
75 
76  // unilateral force
77  g(row++) = f.transpose() * n; // >0 (unilateral forces)
78 
79  // frictional pyramid
80  Vector3d t1 = terrain_->GetNormalizedBasis(HeightMap::Tangent1, p.x(), p.y());
81  g(row++) = f.transpose() * (t1 - mu_*n); // t1 < mu*n
82  g(row++) = f.transpose() * (t1 + mu_*n); // t1 > -mu*n
83 
84  Vector3d t2 = terrain_->GetNormalizedBasis(HeightMap::Tangent2, p.x(), p.y());
85  g(row++) = f.transpose() * (t2 - mu_*n); // t2 < mu*n
86  g(row++) = f.transpose() * (t2 + mu_*n); // t2 > -mu*n
87  }
88 
89  return g;
90 }
91 
92 ForceConstraint::VecBound
94 {
95  VecBound bounds;
96 
97  for (int f_node_id : pure_stance_force_node_ids_) {
98  bounds.push_back(ifopt::Bounds(0.0, fn_max_)); // unilateral forces
99  bounds.push_back(ifopt::BoundSmallerZero); // f_t1 < mu*n
100  bounds.push_back(ifopt::BoundGreaterZero); // f_t1 > -mu*n
101  bounds.push_back(ifopt::BoundSmallerZero); // f_t2 < mu*n
102  bounds.push_back(ifopt::BoundGreaterZero); // f_t2 > -mu*n
103  }
104 
105  return bounds;
106 }
107 
108 void
110  Jacobian& jac) const
111 {
112  if (var_set == ee_force_->GetName()) {
113 
114  int row = 0;
115  for (int f_node_id : pure_stance_force_node_ids_) {
116 
117  // unilateral force
118  int phase = ee_force_->GetPhase(f_node_id);
119  Vector3d p = ee_motion_->GetValueAtStartOfPhase(phase); // doesn't change during phase
120  Vector3d n = terrain_->GetNormalizedBasis(HeightMap::Normal, p.x(), p.y());
121  Vector3d t1 = terrain_->GetNormalizedBasis(HeightMap::Tangent1, p.x(), p.y());
122  Vector3d t2 = terrain_->GetNormalizedBasis(HeightMap::Tangent2, p.x(), p.y());
123 
124  for (auto dim : {X,Y,Z}) {
125  int idx = ee_force_->Index(f_node_id, kPos, dim);
126 
127  int row_reset=row;
128 
129  jac.coeffRef(row_reset++, idx) = n(dim); // unilateral force
130  jac.coeffRef(row_reset++, idx) = t1(dim)-mu_*n(dim); // f_t1 < mu*n
131  jac.coeffRef(row_reset++, idx) = t1(dim)+mu_*n(dim); // f_t1 > -mu*n
132  jac.coeffRef(row_reset++, idx) = t2(dim)-mu_*n(dim); // f_t2 < mu*n
133  jac.coeffRef(row_reset++, idx) = t2(dim)+mu_*n(dim); // f_t2 > -mu*n
134  }
135 
137  }
138  }
139 
140 
141  if (var_set == ee_motion_->GetName()) {
142 
143  int row = 0;
144  auto force_nodes = ee_force_->GetNodes();
145  for (int f_node_id : pure_stance_force_node_ids_) {
146 
147  int phase = ee_force_->GetPhase(f_node_id);
148  int ee_node_id = ee_motion_->GetNodeIDAtStartOfPhase(phase);
149 
150  Vector3d p = ee_motion_->GetValueAtStartOfPhase(phase); // doesn't change during pahse
151  Vector3d f = force_nodes.at(f_node_id).p();
152 
153  for (auto dim : {X_,Y_}) {
154 
155  Vector3d dn = terrain_->GetDerivativeOfNormalizedBasisWrt(HeightMap::Normal, dim, p.x(), p.y());
156  Vector3d dt1 = terrain_->GetDerivativeOfNormalizedBasisWrt(HeightMap::Tangent1, dim, p.x(), p.y());
157  Vector3d dt2 = terrain_->GetDerivativeOfNormalizedBasisWrt(HeightMap::Tangent2, dim, p.x(), p.y());
158 
159  int idx = ee_motion_->Index(ee_node_id, kPos, dim);
160  int row_reset=row;
161 
162  // unilateral force
163  jac.coeffRef(row_reset++, idx) = f.transpose()*dn;
164 
165  // friction force tangent 1 derivative
166  jac.coeffRef(row_reset++, idx) = f.transpose()*(dt1-mu_*dn);
167  jac.coeffRef(row_reset++, idx) = f.transpose()*(dt1+mu_*dn);
168 
169  // friction force tangent 2 derivative
170  jac.coeffRef(row_reset++, idx) = f.transpose()*(dt2-mu_*dn);
171  jac.coeffRef(row_reset++, idx) = f.transpose()*(dt2+mu_*dn);
172  }
173 
175  }
176  }
177 }
178 
179 } /* namespace towr */
std::vector< int > pure_stance_force_node_ids_
Nodes that are associated to either swing or stance phases.
Definition: phase_nodes.h:46
PhaseNodes::Ptr ee_force_
the current xyz foot forces.
double mu_
friction coeff between robot feet and terrain.
void FillJacobianBlock(std::string var_set, Jacobian &) const override
ForceConstraint(const HeightMap::Ptr &terrain, double force_limit_in_normal_direction, EE endeffector_id)
Constructs a force contraint.
Eigen::Vector3d Vector3d
virtual void InitVariableDependedQuantities(const VariablesPtr &x) override
Eigen::VectorXd VectorXd
double fn_max_
force limit in normal direction.
VecBound GetBounds() const override
PhaseNodes::Ptr ee_motion_
the current xyz foot positions.
static constexpr int k2D
EE ee_
The endeffector force to be constrained.
int n_constraints_per_node_
number of constraint for each node.
static std::string EEMotionNodes(uint ee)
VectorXd GetValues() const override
static std::string EEForceNodes(uint ee)
HeightMap::Ptr terrain_
gradient information at every position (x,y).
std::shared_ptr< HeightMap > Ptr
Definition: height_map.h:55


towr_core
Author(s): Alexander W. Winkler
autogenerated on Sat Apr 7 2018 02:15:57