KrisLibrary  1.0.0
Stability.h
1 #ifndef ROBOTICS_STABILITY_H
2 #define ROBOTICS_STABILITY_H
3 
4 #include "Contact.h"
5 #include <KrisLibrary/geometry/UnboundedPolytope2D.h>
6 #include <KrisLibrary/optimization/LinearProgram.h>
7 #include <KrisLibrary/optimization/LPRobust.h>
8 
10 bool TestForceClosure(const std::vector<ContactPoint>& contacts,int numFCEdges);
11 bool TestForceClosure(const std::vector<CustomContactPoint>& contacts);
12 
14 bool TestForceClosure(const std::vector<ContactPoint2D>& contacts);
15 bool TestForceClosure(const std::vector<CustomContactPoint2D>& contacts);
16 
19 bool TestCOMEquilibrium(const std::vector<ContactPoint>& contacts,const Vector3& fext,int numFCEdges,const Vector3& com,std::vector<Vector3>& f);
20 bool TestCOMEquilibrium(const std::vector<CustomContactPoint>& contacts,const Vector3& fext,const Vector3& com,std::vector<Vector3>& f);
21 
23 bool TestAnyCOMEquilibrium(const std::vector<ContactPoint>& contacts,const Vector3& fext,int numFCEdges);
24 bool TestAnyCOMEquilibrium(const std::vector<CustomContactPoint>& contacts,const Vector3& fext);
25 
28 bool TestCOMEquilibrium(const std::vector<ContactPoint2D>& contacts,const Vector2& fext,const Vector2& com,std::vector<Vector2>& f);
29 bool TestCOMEquilibrium(const std::vector<CustomContactPoint2D>& contacts,const Vector2& fext,const Vector2& com,std::vector<Vector2>& f);
30 
32 bool TestAnyCOMEquilibrium(const std::vector<ContactPoint2D>& contacts,const Vector2& fext);
33 bool TestAnyCOMEquilibrium(const std::vector<CustomContactPoint2D>& contacts,const Vector2& fext);
34 
50 {
51  public:
53  void Setup(const std::vector<ContactPoint>& contacts,const Vector3& fext,int numFCEdges,const Vector3& com);
54  void Setup(const std::vector<CustomContactPoint>& contacts,const Vector3& fext,const Vector3& com);
55  void Setup(const CustomContactFormation& contacts,const Vector3& fext,const Vector3& com);
56  void SetupAnyCOM(const std::vector<ContactPoint>& contacts,const Vector3& fext,int numFCEdges);
57  void SetupAnyCOM(const std::vector<CustomContactPoint>& contacts,const Vector3& fext);
58  void SetupAnyCOM(const CustomContactFormation& contacts,const Vector3& fext);
59  bool TestCOM(const std::vector<ContactPoint>& contacts,const Vector3& fext,int numFCEdges,const Vector3& com);
60  bool TestCOM(const std::vector<CustomContactPoint>& contacts,const Vector3& fext,const Vector3& com);
61  bool TestCOM(const CustomContactFormation& contacts,const Vector3& fext,const Vector3& com);
62  bool TestAnyCOM(const std::vector<ContactPoint>& contacts,const Vector3& fext,int numFCEdges);
63  bool TestAnyCOM(const std::vector<CustomContactPoint>& contacts,const Vector3& fext);
64  bool TestAnyCOM(const CustomContactFormation& contacts,const Vector3& fext);
65  void ChangeContacts(const std::vector<ContactPoint>& contacts);
66  void ChangeContact(int i,ContactPoint& contact);
67  void ChangeGravity(const Vector3& fext);
68  void ChangeCOM(const Vector3& com);
69  void SetRobustnessFactor(Real frobust);
70  void SetRobustnessFactor(int i,Real frobust);
71  void LimitContactForce(int i,Real maximum,const Vector3& dir);
72  void LimitContactForceSum(const std::vector<int>& indices,Real maximum,const Vector3& dir);
73  bool TestCurrent();
74  void GetValidCOM(Vector3& com) const;
75  void GetForceVector(Vector& f) const;
76  void GetForces(std::vector<Vector3>& f) const;
77  void Clear();
78  bool IsEmpty();
79  int NumContacts() const;
80  int NumFCEdges() const;
81 
82  private:
85  bool testingAnyCOM;
86  Vector3 testedCOM;
87  Vector3 conditioningShift;
88  int numFCEdges;
89 };
90 
91 
109 {
110 public:
111  bool Set(const std::vector<ContactPoint>& contacts,const Vector3& fext,int numFCEdges,int maxExpandDepth=6);
112  bool Set(const std::vector<CustomContactPoint>& contacts,const Vector3& fext,int maxExpandDepth=6);
113  bool Set(const CustomContactFormation& contacts,const Vector3& fext,int maxExpandDepth=6);
114  bool Empty() const;
115  bool TestCOM(const Vector3& com) const;
116  Real COMMargin(const Vector3& com) const;
117 
118  Vector3 fext;
119  int numFCEdges;
120  std::vector<ContactPoint> contacts;
121 };
122 
127 {
128 public:
129  bool Set(const std::vector<ContactPoint>& contacts,const Vector3& fext,int numFCEdges);
130  bool Set(const std::vector<CustomContactPoint>& contacts,const Vector3& fext);
131  bool Set(const CustomContactFormation& contacts,const Vector3& fext);
132  bool TestCOM(const Vector3& com) const;
133  Real COMMargin(const Vector3& com) const;
134  //poly is a slice through the support polytope at the given height
135  //in the xy plane
136  void GetXYSlice(Real height,Geometry::UnboundedPolytope2D& poly) const;
137  //poly is returned in the xy plane of the frame
138  void GetSlice(const RigidTransform& frame,Geometry::UnboundedPolytope2D& poly) const;
139 
140  RigidTransform T; //transform from original space to gravity-oriented space
141  SupportPolygon sp; //support polygon in the gravity-oriented space
142 };
143 
144 
145 #endif
void Setup(const std::vector< ContactPoint > &contacts, const Vector3 &fext, int numFCEdges, const Vector3 &com)
Definition: Stability.cpp:757
Linear program definition with sparse matrix A.
Definition: LinearProgram.h:158
A single contact point, i.e. a position, normal, and friction coefficient.
Definition: Contact.h:15
A 3D vector class.
Definition: math3d/primitives.h:136
void SetupAnyCOM(const std::vector< ContactPoint > &contacts, const Vector3 &fext, int numFCEdges)
Definition: Stability.cpp:907
A class that tries out as many available routines as possible to solve an LP.
Definition: LPRobust.h:12
A rigid-body transformation.
Definition: math3d/primitives.h:820
A representation of a possibly unbounded polytope in the 2d plane.
Definition: UnboundedPolytope2D.h:17
Testing COM equilibrium given some number of contacts.
Definition: Stability.h:49
Calculation of the support polygon given a number of contacts.
Definition: Stability.h:108
A more advanced ContactFormation that accepts custom contact points and custom constraints.
Definition: Contact.h:249
Calculation of a support polygon with a non-vertical gravity vector.
Definition: Stability.h:126
A 2D vector class.
Definition: math3d/primitives.h:41