Klamp't  0.8.1
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 
30 class RobotCSpace : public GeodesicCSpace
31 {
32 public:
33  RobotCSpace(Robot& robot);
34  RobotCSpace(const RobotCSpace& space);
35  virtual int NumDimensions();
36  virtual string VariableName(int i);
37  virtual void Sample(Config& x);
38  virtual void SampleNeighborhood(const Config& c,Real r,Config& out);
39  virtual void Interpolate(const Config& x,const Config& y,Real u,Config& out);
40  virtual Real Distance(const Config& x,const Config& y);
41  virtual void Properties(PropertyMap&);
42 
43  virtual void InterpolateDeriv(const Config& a,const Config& b,Real u,Vector& dx);
44  virtual void InterpolateDerivA(const Config& a,const Config& b,Real u,const Vector& da,Vector& dx);
45  virtual void InterpolateDerivB(const Config& a,const Config& b,Real u,const Vector& db,Vector& dx);
46  virtual void InterpolateDeriv2(const Config& a,const Config& b,Real u,Vector& ddx);
47  virtual void Integrate(const Config& a,const Vector& da,Config& b);
48 
49  Robot& robot;
50  //optional: can edit weights for distance metric and neighborhood sampling
51  Real norm;
52  vector<Real> jointWeights;
53  Real floatingRotationWeight;
54  vector<Real> jointRadiusScale;
55  Real floatingRotationRadiusScale;
56 };
57 
58 
63 class ActiveRobotCSpace : public GeodesicCSpace
64 {
65 public:
66  ActiveRobotCSpace(Robot& robot,const ArrayMapping& dofs);
67  virtual int NumDimensions();
68  virtual string VariableName(int i);
69  virtual void Sample(Config& x);
70  virtual void Interpolate(const Config& x,const Config& y,Real u,Config& out);
71  virtual Real Distance(const Config& x,const Config& y);
72  virtual void Properties(PropertyMap&) const;
73 
74  virtual void InterpolateDeriv(const Config& a,const Config& b,Real u,Vector& dx);
75  virtual void InterpolateDerivA(const Config& a,const Config& b,Real u,const Vector& da,Vector& dx);
76  virtual void InterpolateDerivB(const Config& a,const Config& b,Real u,const Vector& db,Vector& dx);
77  virtual void InterpolateDeriv2(const Config& a,const Config& b,Real u,Vector& ddx);
78  virtual void Integrate(const Config& a,const Vector& da,Config& b);
79 
80  Robot& robot;
81  ArrayMapping dofs;
82  Config xq,yq,tempq;
83  vector<int> invMap;
84  vector<int> joints;
85 };
86 
87 
88 
100 class SingleRobotCSpace : public RobotCSpace
101 {
102  public:
103  SingleRobotCSpace(RobotWorld& world,int index,
104  WorldPlannerSettings* settings);
105  SingleRobotCSpace(const SingleRobotCSpace& space);
106 
108  void FixDof(int dof,Real value);
110  void IgnoreCollisions(int a,int b);
111  void Init();
112 
113  virtual ~SingleRobotCSpace() {}
114  virtual void Sample(Config& x);
115  virtual void SampleNeighborhood(const Config& c,Real r,Config& x);
116  virtual bool IsFeasible(const Config& x);
117  virtual EdgePlannerPtr PathChecker(const Config& a,const Config& b,int obstacle);
118  virtual EdgePlannerPtr PathChecker(const Config& a,const Config& b);
119  virtual void Properties(PropertyMap& map);
120 
121  virtual void GetJointLimits(Vector& bmin,Vector& bmax);
122 
123  bool UpdateGeometry(const Config& x);
124  bool CheckJointLimits(const Config& x);
125  bool CheckCollisionFree(const Config& x);
126 
127  RobotWorld& world;
128  int index;
129  WorldPlannerSettings* settings;
130 
131  vector<pair<int,int> > collisionPairs;
132  vector<Geometry::AnyCollisionQuery> collisionQueries;
133 
134  vector<int> fixedDofs;
135  vector<Real> fixedValues;
136  vector<pair<int,int> > ignoreCollisions;
137  bool constraintsDirty;
138 };
139 
145 class SingleRigidObjectCSpace: public SE3CSpace
146 {
147  public:
148  SingleRigidObjectCSpace(RobotWorld& world,int index,WorldPlannerSettings* settings);
149  RigidObject* GetObject() const;
150  virtual EdgePlannerPtr PathChecker(const Config& a,const Config& b);
151 
153  void IgnoreCollisions(int id);
154  void Init();
155  bool UpdateGeometry(const Config& x);
156 
157  RobotWorld& world;
158  int index;
159  WorldPlannerSettings* settings;
160 
161  vector<pair<int,int> > collisionPairs;
162  vector<Geometry::AnyCollisionQuery> collisionQueries;
163 
164  bool constraintsDirty;
165 };
166 
167 
168 #endif
Distance
Real Distance(const Robot &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...
Robot
The main robot type used in RobotSim.
Definition: Robot.h:79
ActiveRobotCSpace
A CSpace for just a few dofs of a robot. Slightly faster than using a regular RobotCSpace then pickin...
Definition: RobotCSpace.h:63
WorldPlannerSettings
A structure containing settings that should be used for collision detection, contact solving...
Definition: PlannerSettings.h:41
RobotWorld
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:20
SingleRobotCSpace
A cspace consisting of a single robot configuration in a RobotWorld. Feasibility constraints are join...
Definition: RobotCSpace.h:100
SingleRigidObjectCSpace
A configuration space for a rigid object, treated like a robot.
Definition: RobotCSpace.h:145
Interpolate
void Interpolate(Robot &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...
RigidObject
A (static) rigid object that may be manipulated.
Definition: RigidObject.h:13
RobotCSpace
Implements a basic robot configuration space with only joint limit constraint testing.
Definition: RobotCSpace.h:30
Integrate
void Integrate(Robot &robot, const Config &q, const Vector &dq, Config &b)
Integrates a velocity vector dq from q to obtain the configuration b.
Generated by   doxygen 1.8.11