Klamp't  0.9.0
ContactCSpace.h
1 #ifndef ROBOT_CONTACT_CSPACE_H
2 #define ROBOT_CONTACT_CSPACE_H
3 
4 #include "RobotCSpace.h"
5 #include <Klampt/Modeling/GeneralizedRobot.h>
6 #include <Klampt/Contact/Stance.h>
7 #include <KrisLibrary/robotics/IK.h>
8 
9 namespace Klampt {
10 
20 {
21  public:
22  ContactCSpace(WorldModel& world,int index,
23  WorldPlannerSettings* settings);
24  ContactCSpace(const SingleRobotCSpace& space);
25  ContactCSpace(const ContactCSpace& space);
26 
27  virtual void Sample(Config& q) override;
28  virtual void SampleNeighborhood(const Config& c,Real r,Config& q) override;
29  virtual void Interpolate(const Config& x,const Config& y,Real u,Config& out) override;
30  virtual EdgePlannerPtr PathChecker(const Config& a,const Config& b) override;
31  virtual void Properties(PropertyMap&) override;
32 
33  void AddContact(const IKGoal& goal);
34  void AddContact(int link,const Vector3& localPos,const Vector3& worldPos);
35  void AddContact(int link,const vector<Vector3>& localPos,const vector<Vector3>& worldPos);
36  void RemoveContact(int link);
37  bool SolveContact(int numIters=0,Real tol=0);
40  Real ContactDistance();
42  Real ContactDistance(const Config& q);
44  //current configuration. If dist is nonzero, this overrides the defaults
46  bool CheckContact(Real dist=0);
48  bool CheckContact(const Config& q,Real dist=0);
49 
50  vector<IKGoal> contactIK;
51  int numSolveContact;
52  double solveContactTime;
53 };
54 
55 /*
56 class MultiContactCSpace : public MultiRobotCSpace
57 {
58  public:
59  struct ContactPair
60  {
61  int id1,id2; //world id of the given object
62  vector<Vector3> c1,c2; //cx = contacts in local frame of object x
63  vector<Vector3> n1,n2; //nx = normals in local frame of object x
64  vector<Real> kFriction;
65  };
66 
67  MultiContactCSpace(WorldModel& world,
68  WorldPlannerSettings* settings);
69  MultiContactCSpace(const MultiRobotCSpace&);
70  MultiContactCSpace(const MultiContactCSpace&);
71  virtual ~MultiContactCSpace() {}
73  void InitContactPairs(const vector<ContactPair>& pairs);
75  void InitContactPairs(const ContactFormation& formation);
76 
77  virtual int NumDimensions() const override;
78  virtual void Sample(Config& x) override;
79  virtual void SampleNeighborhood(const Config& c,Real r,Config& x) override;
80  virtual bool IsFeasible(const Config&) override;
81  virtual void Interpolate(const Config& x,const Config& y,Real u,Config& out) override;
82  virtual void Midpoint(const Config& x,const Config& y,Config& out) override;
83  virtual void Properties(PropertyMap&) override;
84 
85  bool SolveContact(Config& x,int numIters=0,Real tol=0);
86  Real ContactDistance();
87  Real ContactDistance(const Config& x);
88  bool CheckContact(Real dist=0);
89  bool CheckContact(const Config& x,Real dist=0);
90 
91  vector<ContactPair> contactPairs;
92  Robot aggregateRobot;
93  vector<IKGoal> closedChainConstraints;
94  Stance aggregateStance;
95 
96  //stats
97  int numSolveContact,numIsFeasible;
98  double solveContactTime,isFeasibleTime;
99 };
100 */
101 
102 } // namespace Klampt
103 
104 #endif
A cspace consisting of a single robot configuration in a WorldModel. Feasibility constraints are join...
Definition: RobotCSpace.h:103
A structure containing settings that should be used for collision detection, contact solving...
Definition: PlannerSettings.h:43
bool CheckContact(Real dist=0)
Checks whether the contact constraints are satisfied at the robot&#39;s.
A SingleRobotCSpace for a robot maintaining contact.
Definition: ContactCSpace.h:19
Definition: ContactDistance.h:6
The main world class containing multiple robots, objects, and static geometries (terrains). Lights and other viewport information may also be stored here.
Definition: World.h:24