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-" + 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 {
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  int phase = ee_force_->GetPhase(f_node_id);
71  Vector3d p = ee_motion_->GetValueAtStartOfPhase(phase); // doesn't change during stance phase
72  Vector3d n = terrain_->GetNormalizedBasis(HeightMap::Normal, p.x(), p.y());
73  Vector3d f = force_nodes.at(f_node_id).p();
74 
75  // unilateral force
76  g(row++) = f.transpose() * n; // >0 (unilateral forces)
77 
78  // frictional pyramid
79  Vector3d t1 = terrain_->GetNormalizedBasis(HeightMap::Tangent1, p.x(), p.y());
80  g(row++) = f.transpose() * (t1 - mu_*n); // t1 < mu*n
81  g(row++) = f.transpose() * (t1 + mu_*n); // t1 > -mu*n
82 
83  Vector3d t2 = terrain_->GetNormalizedBasis(HeightMap::Tangent2, p.x(), p.y());
84  g(row++) = f.transpose() * (t2 - mu_*n); // t2 < mu*n
85  g(row++) = f.transpose() * (t2 + mu_*n); // t2 > -mu*n
86  }
87 
88  return g;
89 }
90 
93 {
94  VecBound bounds;
95 
96  for (int f_node_id : pure_stance_force_node_ids_) {
97  bounds.push_back(ifopt::Bounds(0.0, fn_max_)); // unilateral forces
98  bounds.push_back(ifopt::BoundSmallerZero); // f_t1 < mu*n
99  bounds.push_back(ifopt::BoundGreaterZero); // f_t1 > -mu*n
100  bounds.push_back(ifopt::BoundSmallerZero); // f_t2 < mu*n
101  bounds.push_back(ifopt::BoundGreaterZero); // f_t2 > -mu*n
102  }
103 
104  return bounds;
105 }
106 
107 void
109  Jacobian& jac) const
110 {
111  if (var_set == ee_force_->GetName()) {
112  int row = 0;
113  for (int f_node_id : pure_stance_force_node_ids_) {
114  // unilateral force
115  int phase = ee_force_->GetPhase(f_node_id);
116  Vector3d p = ee_motion_->GetValueAtStartOfPhase(phase); // doesn't change during phase
117  Vector3d n = terrain_->GetNormalizedBasis(HeightMap::Normal, p.x(), p.y());
118  Vector3d t1 = terrain_->GetNormalizedBasis(HeightMap::Tangent1, p.x(), p.y());
119  Vector3d t2 = terrain_->GetNormalizedBasis(HeightMap::Tangent2, p.x(), p.y());
120 
121  for (auto dim : {X,Y,Z}) {
122  int idx = ee_force_->GetOptIndex(NodesVariables::NodeValueInfo(f_node_id, kPos, dim));
123 
124  int row_reset=row;
125 
126  jac.coeffRef(row_reset++, idx) = n(dim); // unilateral force
127  jac.coeffRef(row_reset++, idx) = t1(dim)-mu_*n(dim); // f_t1 < mu*n
128  jac.coeffRef(row_reset++, idx) = t1(dim)+mu_*n(dim); // f_t1 > -mu*n
129  jac.coeffRef(row_reset++, idx) = t2(dim)-mu_*n(dim); // f_t2 < mu*n
130  jac.coeffRef(row_reset++, idx) = t2(dim)+mu_*n(dim); // f_t2 > -mu*n
131  }
132 
134  }
135  }
136 
137 
138  if (var_set == ee_motion_->GetName()) {
139  int row = 0;
140  auto force_nodes = ee_force_->GetNodes();
141  for (int f_node_id : pure_stance_force_node_ids_) {
142  int phase = ee_force_->GetPhase(f_node_id);
143  int ee_node_id = ee_motion_->GetNodeIDAtStartOfPhase(phase);
144 
145  Vector3d p = ee_motion_->GetValueAtStartOfPhase(phase); // doesn't change during pahse
146  Vector3d f = force_nodes.at(f_node_id).p();
147 
148  for (auto dim : {X_,Y_}) {
149  Vector3d dn = terrain_->GetDerivativeOfNormalizedBasisWrt(HeightMap::Normal, dim, p.x(), p.y());
150  Vector3d dt1 = terrain_->GetDerivativeOfNormalizedBasisWrt(HeightMap::Tangent1, dim, p.x(), p.y());
151  Vector3d dt2 = terrain_->GetDerivativeOfNormalizedBasisWrt(HeightMap::Tangent2, dim, p.x(), p.y());
152 
153  int idx = ee_motion_->GetOptIndex(NodesVariables::NodeValueInfo(ee_node_id, kPos, dim));
154  int row_reset=row;
155 
156  // unilateral force
157  jac.coeffRef(row_reset++, idx) = f.transpose()*dn;
158 
159  // friction force tangent 1 derivative
160  jac.coeffRef(row_reset++, idx) = f.transpose()*(dt1-mu_*dn);
161  jac.coeffRef(row_reset++, idx) = f.transpose()*(dt1+mu_*dn);
162 
163  // friction force tangent 2 derivative
164  jac.coeffRef(row_reset++, idx) = f.transpose()*(dt2-mu_*dn);
165  jac.coeffRef(row_reset++, idx) = f.transpose()*(dt2+mu_*dn);
166  }
167 
169  }
170  }
171 }
172 
173 } /* namespace towr */
std::vector< int > pure_stance_force_node_ids_
static const Bounds BoundGreaterZero
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.
Composite::Ptr VariablesPtr
Eigen::Vector3d Vector3d
static const Bounds BoundSmallerZero
void InitVariableDependedQuantities(const VariablesPtr &x) override
Eigen::VectorXd VectorXd
int GetRows() const
double fn_max_
force limit in normal direction.
Semantic information associated with a scalar node value.
VecBound GetBounds() const override
static constexpr int k2D
EE ee_
The endeffector force to be constrained.
void SetRows(int num_rows)
int n_constraints_per_node_
number of constraint for each node.
static std::string EEMotionNodes(uint ee)
NodesVariablesPhaseBased::Ptr ee_force_
the current xyz foot forces.
Eigen::SparseMatrix< double, Eigen::RowMajor > Jacobian
VectorXd GetValues() const override
static std::string EEForceNodes(uint ee)
Nodes that are associated to either swing or stance phases.
HeightMap::Ptr terrain_
gradient information at every position (x,y).
NodesVariablesPhaseBased::Ptr ee_motion_
the current xyz foot positions.
std::vector< Bounds > VecBound
std::shared_ptr< HeightMap > Ptr
Definition: height_map.h:73
Eigen::VectorXd VectorXd


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