Klamp't  0.9.0
RobotCSpace.h
1 #ifndef ROBOT_CSPACE_H
2 #define ROBOT_CSPACE_H
3 
4 #include <Klampt/Modeling/World.h>
5 #include <Klampt/Modeling/GeneralizedRobot.h>
6 #include "PlannerSettings.h"
7 #include <KrisLibrary/planning/CSpaceHelpers.h>
8 #include <KrisLibrary/planning/RigidBodyCSpace.h>
9 #include <KrisLibrary/utils/ArrayMapping.h>
10 
11 namespace Klampt {
12 
32 class RobotCSpace : public GeodesicCSpace
33 {
34 public:
35  RobotCSpace(RobotModel& robot);
36  RobotCSpace(const RobotCSpace& space);
37  virtual int NumDimensions() override;
38  virtual string VariableName(int i) override;
39  virtual void Sample(Config& x) override;
40  virtual void SampleNeighborhood(const Config& c,Real r,Config& out) override;
41  virtual void Interpolate(const Config& x,const Config& y,Real u,Config& out) override;
42  virtual Real Distance(const Config& x,const Config& y) override;
43  virtual void Properties(PropertyMap&) override;
44 
45  virtual void InterpolateDeriv(const Config& a,const Config& b,Real u,Vector& dx) override;
46  virtual void InterpolateDerivA(const Config& a,const Config& b,Real u,const Vector& da,Vector& dx) override;
47  virtual void InterpolateDerivB(const Config& a,const Config& b,Real u,const Vector& db,Vector& dx) override;
48  virtual void InterpolateDeriv2(const Config& a,const Config& b,Real u,Vector& ddx) override;
49  virtual void Integrate(const Config& a,const Vector& da,Config& b) override;
50 
51  RobotModel& robot;
52  //optional: can edit weights for distance metric and neighborhood sampling
53  Real norm;
54  vector<Real> jointWeights;
55  Real floatingRotationWeight;
56  vector<Real> jointRadiusScale;
57  Real floatingRotationRadiusScale;
58  Real unboundedStdDeviation;
59 };
60 
61 
66 class ActiveRobotCSpace : public GeodesicCSpace
67 {
68 public:
69  ActiveRobotCSpace(RobotModel& robot,const ArrayMapping& dofs);
70  virtual int NumDimensions() override;
71  virtual string VariableName(int i) override;
72  virtual void Sample(Config& x) override;
73  virtual void Interpolate(const Config& x,const Config& y,Real u,Config& out) override;
74  virtual Real Distance(const Config& x,const Config& y) override;
75  virtual void Properties(PropertyMap&) override;
76 
77  virtual void InterpolateDeriv(const Config& a,const Config& b,Real u,Vector& dx) override;
78  virtual void InterpolateDerivA(const Config& a,const Config& b,Real u,const Vector& da,Vector& dx) override;
79  virtual void InterpolateDerivB(const Config& a,const Config& b,Real u,const Vector& db,Vector& dx) override;
80  virtual void InterpolateDeriv2(const Config& a,const Config& b,Real u,Vector& ddx) override;
81  virtual void Integrate(const Config& a,const Vector& da,Config& b) override;
82 
83  RobotModel& robot;
84  ArrayMapping dofs;
85  Config xq,yq,tempq;
86  vector<int> invMap;
87  vector<int> joints;
88 };
89 
90 
91 
103 class SingleRobotCSpace : public RobotCSpace
104 {
105  public:
106  SingleRobotCSpace(WorldModel& world,int index,
107  WorldPlannerSettings* settings);
108  SingleRobotCSpace(const SingleRobotCSpace& space);
109 
111  void FixDof(int dof,Real value);
113  void IgnoreCollisions(int a,int b);
114  void Init();
115 
116  virtual ~SingleRobotCSpace() {}
117  virtual void Sample(Config& x) override;
118  virtual void SampleNeighborhood(const Config& c,Real r,Config& x) override;
119  virtual bool IsFeasible(const Config& x) override;
120  virtual EdgePlannerPtr PathChecker(const Config& a,const Config& b,int obstacle) override;
121  virtual EdgePlannerPtr PathChecker(const Config& a,const Config& b) override;
122  virtual void Properties(PropertyMap& map) override;
123 
124  virtual void GetJointLimits(Vector& bmin,Vector& bmax);
125 
126  bool UpdateGeometry(const Config& x);
127  bool CheckJointLimits(const Config& x);
128  bool CheckCollisionFree(const Config& x);
129 
130  WorldModel& world;
131  int index;
132  WorldPlannerSettings* settings;
133 
134  vector<pair<int,int> > collisionPairs;
135  vector<Geometry::AnyCollisionQuery> collisionQueries;
136 
137  vector<int> fixedDofs;
138  vector<Real> fixedValues;
139  vector<pair<int,int> > ignoreCollisions;
140  bool constraintsDirty;
141 };
142 
148 class SingleRigidObjectCSpace: public SE3CSpace
149 {
150  public:
151  SingleRigidObjectCSpace(WorldModel& world,int index,WorldPlannerSettings* settings);
152  RigidObjectModel* GetObject() const;
153  virtual EdgePlannerPtr PathChecker(const Config& a,const Config& b) override;
154 
156  void IgnoreCollisions(int id);
157  void Init();
158  bool UpdateGeometry(const Config& x);
159 
160  WorldModel& world;
161  int index;
162  WorldPlannerSettings* settings;
163 
164  vector<pair<int,int> > collisionPairs;
165  vector<Geometry::AnyCollisionQuery> collisionQueries;
166 
167  bool constraintsDirty;
168 };
169 
170 } // namespace Klampt
171 
172 #endif
Klampt::SingleRobotCSpace
A cspace consisting of a single robot configuration in a WorldModel. Feasibility constraints are join...
Definition: RobotCSpace.h:103
Klampt::WorldPlannerSettings
A structure containing settings that should be used for collision detection, contact solving...
Definition: PlannerSettings.h:43
Klampt::RigidObjectModel
A (static) rigid object that may be manipulated.
Definition: RigidObject.h:15
World.h
Defines the WorldModel class.
Klampt::Distance
Real Distance(const RobotModel &robot, const Config &a, const Config &b, Real norm, Real floatingRotationWeight=1.0)
Returns the geodesic distance between a and b. Combines individual joint distances together via the L...
Klampt::Integrate
void Integrate(RobotModel &robot, const Config &q, const Vector &dq, Config &b)
Integrates a velocity vector dq from q to obtain the configuration b.
Klampt::RobotModel
The main robot type used in RobotSim.
Definition: Robot.h:83
Klampt::ActiveRobotCSpace
A CSpace for just a few dofs of a robot. Slightly faster than using a regular RobotCSpace then pickin...
Definition: RobotCSpace.h:66
Klampt::RobotCSpace
Implements a basic robot configuration space with only joint limit constraint testing.
Definition: RobotCSpace.h:32
Klampt
Definition: ContactDistance.h:6
Klampt::Interpolate
void Interpolate(RobotModel &robot, const Config &x, const Config &y, Real u, Config &out)
Interpolates between the two configurations in geodesic fashion on the robot&#39;s underlying configurati...
Klampt::SingleRigidObjectCSpace
A configuration space for a rigid object, treated like a robot.
Definition: RobotCSpace.h:148
Klampt::WorldModel
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
Generated by   doxygen 1.8.11