1 #ifndef NUMERICAL_CSPACE_CONSTRAINT_H 2 #define NUMERICAL_CSPACE_CONSTRAINT_H 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> 23 :LimitConstraint(_robot.qMin,_robot.qMax),robot(_robot)
25 virtual string Label()
const override;
26 virtual string Label(
int i)
const override;
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;
45 virtual Real Margin(
const Vector& x,
int& minConstraint)
override;
46 virtual bool Satisfies_i(
const Vector& x,
int i,Real d=Zero)
override;
50 LinearConstraint cmInequality;
56 DirtyData<Matrix> Jcom;
57 DirtyData<Matrix> Hcomx,Hcomy,Hcomz;
64 virtual string Label()
const override;
65 virtual string Label(
int i)
const override;
66 virtual int NumDimensions()
const override {
70 return robot.links.size();
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;
78 virtual void Hessian_i(
const Vector& x,
int i,Matrix& Hi)
override;
80 virtual bool Satisfies_i(
const Vector& x,
int i,Real d=Zero)
override;
83 Geometry::AnyCollisionGeometry3D& geometry;
84 vector<DistanceQuery> query;
85 ArrayMapping activeDofs;
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;
101 virtual void Hessian_i(
const Vector& x,
int i,Matrix& Hi)
override;
103 virtual bool Satisfies_i(
const Vector& x,
int i,Real d=Zero)
override;
106 vector<pair<int,int> > collisionPairs;
107 vector<DistanceQuery> query;
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;
119 TorqueSolver& solver;
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