Ensures foot force that is unilateral and inside friction cone. More...
#include <force_constraint.h>
Public Member Functions | |
| 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. | |
| VecBound | GetBounds () const override |
| VectorXd | GetValues () const override |
| virtual void | InitVariableDependedQuantities (const VariablesPtr &x) override |
| virtual | ~ForceConstraint () |
Private Attributes | |
| EE | ee_ |
| The endeffector force to be constrained. | |
| PhaseNodes::Ptr | ee_force_ |
| the current xyz foot forces. | |
| PhaseNodes::Ptr | ee_motion_ |
| the current xyz foot positions. | |
| double | fn_max_ |
| force limit in normal direction. | |
| double | mu_ |
| friction coeff between robot feet and terrain. | |
| int | n_constraints_per_node_ |
| number of constraint for each node. | |
| std::vector< int > | pure_stance_force_node_ids_ |
| HeightMap::Ptr | terrain_ |
| gradient information at every position (x,y). | |
Ensures foot force that is unilateral and inside friction cone.
This class is responsible for constraining the endeffector xyz-forces to only push into the terrain and additionally stay inside the friction cone according to the current slope.
In order to keep the constraint linear and simple for the solver to solve, we approximate the friction cone by a 4-sided pyramid.
Attention: Constraint is enforced only at the spline nodes. In between violations of this contraint can occur.
Definition at line 53 of file force_constraint.h.
| towr::ForceConstraint::ForceConstraint | ( | const HeightMap::Ptr & | terrain, |
| double | force_limit_in_normal_direction, | ||
| EE | endeffector_id | ||
| ) |
Constructs a force contraint.
| terrain | The gradient information of the terrain for friction cone. |
| force_limit_in_normal_direction | Maximum pushing force [N]. |
| endeffector_id | Which endeffector force should be constrained. |
Definition at line 37 of file force_constraint.cc.
| virtual towr::ForceConstraint::~ForceConstraint | ( | ) | [virtual] |
| void towr::ForceConstraint::FillJacobianBlock | ( | std::string | var_set, |
| Jacobian & | jac | ||
| ) | const [override] |
Definition at line 109 of file force_constraint.cc.
| ForceConstraint::VecBound towr::ForceConstraint::GetBounds | ( | ) | const [override] |
Definition at line 93 of file force_constraint.cc.
| Eigen::VectorXd towr::ForceConstraint::GetValues | ( | ) | const [override] |
Definition at line 63 of file force_constraint.cc.
| void towr::ForceConstraint::InitVariableDependedQuantities | ( | const VariablesPtr & | x | ) | [override, virtual] |
Definition at line 51 of file force_constraint.cc.
EE towr::ForceConstraint::ee_ [private] |
The endeffector force to be constrained.
Definition at line 83 of file force_constraint.h.
PhaseNodes::Ptr towr::ForceConstraint::ee_force_ [private] |
the current xyz foot forces.
Definition at line 76 of file force_constraint.h.
PhaseNodes::Ptr towr::ForceConstraint::ee_motion_ [private] |
the current xyz foot positions.
Definition at line 77 of file force_constraint.h.
double towr::ForceConstraint::fn_max_ [private] |
force limit in normal direction.
Definition at line 80 of file force_constraint.h.
double towr::ForceConstraint::mu_ [private] |
friction coeff between robot feet and terrain.
Definition at line 81 of file force_constraint.h.
int towr::ForceConstraint::n_constraints_per_node_ [private] |
number of constraint for each node.
Definition at line 82 of file force_constraint.h.
std::vector<int> towr::ForceConstraint::pure_stance_force_node_ids_ [private] |
The are those Hermite-nodes that shape the polynomial during the stance phases, while all the others are already set to zero force (swing)
Definition at line 89 of file force_constraint.h.
HeightMap::Ptr towr::ForceConstraint::terrain_ [private] |
gradient information at every position (x,y).
Definition at line 79 of file force_constraint.h.