Klamp't  0.9.0
NumericalConstraint.h
Go to the documentation of this file.
1 #ifndef NUMERICAL_CSPACE_CONSTRAINT_H
2 #define NUMERICAL_CSPACE_CONSTRAINT_H
3 
4 #include <KrisLibrary/math/InequalityConstraint.h>
5 #include <KrisLibrary/robotics/Stability.h>
6 #include <KrisLibrary/robotics/TorqueSolver.h>
7 #include "DistanceQuery.h"
8 #include <Klampt/Modeling/Robot.h>
9 #include <KrisLibrary/utils/ArrayMapping.h>
10 
11 namespace Klampt {
12 
20 struct JointLimitConstraint : public LimitConstraint
21 {
22  JointLimitConstraint(const RobotModel& _robot)
23  :LimitConstraint(_robot.qMin,_robot.qMax),robot(_robot)
24  {}
25  virtual string Label() const override;
26  virtual string Label(int i) const override;
27 
28  const RobotModel& robot;
29 };
30 
32 struct SuppPolyConstraint : public InequalityConstraint
33 {
34  SuppPolyConstraint(RobotModel& robot, SupportPolygon& sp);
35  virtual string Label() const override;
36  virtual int NumDimensions() const override;
37  virtual void PreEval(const Vector& x) override;
38  virtual void Eval(const Vector& x,Vector& v) override;
39  virtual Real Eval_i(const Vector& x,int i) override;
40  virtual void Jacobian(const Vector& x,Matrix& J) override;
41  virtual void Jacobian_i(const Vector& x,int i,Vector& Ji) override;
42  virtual void Hessian_i(const Vector& x,int i,Matrix& Hi) override;
43 
44  //saves by considering constraints dependent
45  virtual Real Margin(const Vector& x,int& minConstraint) override;
46  virtual bool Satisfies_i(const Vector& x,int i,Real d=Zero) override;
47 
48  RobotModel& robot;
49  SupportPolygon& sp;
50  LinearConstraint cmInequality;
51  Matrix A;
52  Vector b;
53 
54  //temp
55  Vector vcom;
56  DirtyData<Matrix> Jcom;
57  DirtyData<Matrix> Hcomx,Hcomy,Hcomz;
58 };
59 
61 struct CollisionConstraint : public InequalityConstraint
62 {
63  CollisionConstraint(RobotModel& robot, Geometry::AnyCollisionGeometry3D& geom);
64  virtual string Label() const override;
65  virtual string Label(int i) const override;
66  virtual int NumDimensions() const override {
67 // return this->activeDofs.Size();
68 // cout << "calling NumDimensions" << endl;
69 // getchar();
70  return robot.links.size();
71  }
72  virtual void PreEval(const Vector& x) override;
73  virtual void Eval(const Vector& x, Vector& v) override;
74  virtual Real Eval_i(const Vector& x,int i) override;
75  virtual void Jacobian(const Vector& x,Matrix& J) override;
76  virtual void Jacobian_i(const Vector& x,int i,Vector& Ji) override;
77  //virtual void DirectionalDeriv(const Vector& x,const Vector& h,Vector& v) override;
78  virtual void Hessian_i(const Vector& x,int i,Matrix& Hi) override;
79 
80  virtual bool Satisfies_i(const Vector& x,int i,Real d=Zero) override;
81 
82  RobotModel& robot;
83  Geometry::AnyCollisionGeometry3D& geometry;
84  vector<DistanceQuery> query;
85  ArrayMapping activeDofs;
86 };
87 
89 struct SelfCollisionConstraint : public InequalityConstraint
90 {
92  virtual string Label() const override;
93  virtual string Label(int i) const override;
94  virtual int NumDimensions() const override { return (int)collisionPairs.size(); }
95  virtual void PreEval(const Vector& x) override;
96  virtual void Eval(const Vector& x, Vector& v) override;
97  virtual Real Eval_i(const Vector& x,int i) override;
98  virtual void Jacobian(const Vector& x,Matrix& J) override;
99  virtual void Jacobian_i(const Vector& x,int i,Vector& Ji) override;
100  //virtual void DirectionalDeriv(const Vector& x,const Vector& h,Vector& v) override;
101  virtual void Hessian_i(const Vector& x,int i,Matrix& Hi) override;
102 
103  virtual bool Satisfies_i(const Vector& x,int i,Real d=Zero) override;
104 
105  RobotModel& robot;
106  vector<pair<int,int> > collisionPairs;
107  vector<DistanceQuery> query;
108 };
109 
111 struct TorqueLimitConstraint : public InequalityConstraint
112 {
113  TorqueLimitConstraint(TorqueSolver& solver);
114  virtual string Label() const override;
115  virtual int NumDimensions() const override;
116  virtual void PreEval(const Vector& x) override;
117  virtual void Eval(const Vector& x,Vector& v) override;
118 
119  TorqueSolver& solver;
120 };
121 
122 } //namespace Klampt
123 
124 #endif
Definition: NumericalConstraint.h:89
Definition: NumericalConstraint.h:111
Definition: NumericalConstraint.h:61
Definition: NumericalConstraint.h:20
The main robot type used in RobotSim.
Definition: Robot.h:83
Definition: NumericalConstraint.h:32
Definition: ContactDistance.h:6