kbm.h
Go to the documentation of this file.
1 /* *********************
2  * @file kbm.h
3  *
4  * @author Stefan Ulbrich
5  * @date 2013-2014
6  *
7  * @brief This file contains the definition the class representing the Kinematic Bezier Maps.
8  * *********************/
9 
10 
11 #pragma once
12 
13 #include <iostream>
14 #include <vector>
15 #include <ostream>
16 #include <Eigen/Eigen>
17 #include <Eigen/QR>
18 #include <memory>
19 #include <queue>
20 
21 #define USE_SIMOX
22 #ifdef USE_SIMOX
23 #include <VirtualRobot/Robot.h>
24 #include <VirtualRobot/VirtualRobotException.h>
25 #include <VirtualRobot/Nodes/RobotNode.h>
26 #include <VirtualRobot/KinematicChain.h>
27 #include <VirtualRobot/Nodes/RobotNodeRevolute.h>
28 #endif
29 
30 #define KBM_USE_DOUBLE_PRECISION
31 
32 #define KBM_IMPORT_EXPORT
33 namespace memoryx::KBM
34 {
36 #ifdef KBM_USE_DOUBLE_PRECISION
37  using Real = double;
38  using Matrix = Eigen::MatrixXd;
39  using Vector = Eigen::VectorXd;
40  using Matrix4 = Eigen::Matrix4d;
41  using Vector3 = Eigen::Vector3d;
42 #else
43  using Real = float;
44  using Matrix = Eigen::MatrixXf;
45  using Vector = Eigen::VectorXf;
46  using Matrix4 = Eigen::Matrix4f;
47  using Vector3 = Eigen::Vector3f;
48 #endif
49 }
52 {
58  Matrix KBM_IMPORT_EXPORT solve(const Matrix& input, const Matrix output, Real threshold);
59 }
60 
64 namespace memoryx::KBM::Models
65 {
66 
67  class KBM;
68  using KBM_ptr = std::shared_ptr<KBM>;
69 
79  {
80  public:
81 
84  {
86  PLS
87  };
88 
93  {
95  INCLUDED = 1,
97  COVERED = 2,
99  DISJOINT = 4,
101  OVERLAPPING = 8,
102  ALL = 15
103  };
104 
107  {
119  friend KBM_IMPORT_EXPORT std::ostream& operator<<(std::ostream& os, const ErrorValuesType& et);
120  };
121 
123  KBM(const KBM& other);
124 
131  KBM(int nDoF, int dim, Real spreadAngle);
132 
134  bool restore(std::string fileName = "");
136  void store(std::string fileName = "");
146  void online(const Matrix& proprioception, const Matrix& shape, Real learnRate = 1.0);
147  // @param learnRate Learning rate between (0,1)
156  void batch(const Matrix& proprioception, const Matrix& shape, Optimization method = KBM::STANDARD, Real threshold = 0.0);
160  ErrorValuesType getErrors(const Matrix& proprioception, const Matrix& shape);
162  void reset();
163 
169  Matrix predict(const Matrix& proprioception, int dim = 0) const;
170 
172  void differentiate();
173 
175  Vector getPartialDerivative(const Vector& proprioception, int iDoF) const;
176 
178  Matrix getJacobian(const Vector& proprioception) const;
179 
187  void /*std::shared_ptr<KBM>*/ subdivide(const Vector& center, const Vector& newSpreadAngles);
188 
197  void getSubdivisedNet(const Vector newCenter, Vector newSpreadAngles, Matrix& resultingControlNet) const;
198 
200  void getSubdivisedNet(unsigned int dof, Real center, Real newSpreadAngle, Matrix& resultingControlNet) const;
201 
208  BBCheckType checkBBox(const Vector& lower, const Vector& upper, Real tolerance = 0.0f) const;
209 
219  static BBCheckType checkBBoxes(const Vector& lower, const Vector& upper, const Matrix& controlNet, Real tolerance = 0.0f);
220 
221 
223  static void getBBox(Vector& lower, Vector& upper, const Matrix& controlNet);
224 
225  void getBBox(Vector& lower, Vector& upper);
226  Real getVolume();
227  Real getOverlappingVolume(const Vector& lower, const Vector& upper);
228  Real getOverlappingVolumeRatio(const Vector& lower, const Vector& upper, Real targetVolume);
229 
231  Vector getSpreadAngles() const;
232 
234  Vector getCenter() const;
235 
237  int getNDoF() const;
238  int getNDim() const;
239  Matrix getControlNet() const;
240 
242  static KBM_ptr createKBM(int nDoF, int dim, const Vector& center, const Vector& spreadAngles, const Matrix& controlNet);
243  static KBM_ptr createFromVirtualRobot(VirtualRobot::KinematicChainPtr chain, VirtualRobot::SceneObjectPtr FoR, const Vector& spreadAngles, bool useOrientation = false);
244  private:
253  Matrix createSLE(const Matrix& proprioception, int dim = -1, Real a1 = 0.0f, Real a2 = 0.0f, bool project = true) const;
255  void createIndices();
256  int nDoF;
257  int dim;
258  //Real spreadAngle;
259  //std::vector<Real> spreadAngles;
260  Vector spreadAngles;
261  Matrix index;
262  Matrix controlNet;
263  std::vector<Matrix> partialDerivatives;
264  int nControlPoints;
265  Vector center;
266  };
267 
268 
269 }
270 
272 namespace memoryx::KBM::Inverse
273 {
274 
276  {
277  public:
278  struct Solution
279  {
280  Solution(const Vector& _center, const Vector& _spreadAngles, const Vector& _upper,
281  const Vector& _lower, const Models::KBM::BBCheckType& _type) :
282  center(_center), spreadAngles(_spreadAngles), upper(_upper), lower(_lower), type(_type) {}
284  {
285  this->center = kbm->getCenter();
286  this->spreadAngles = kbm->getSpreadAngles();
287  Vector BBupper, BBlower;
288  kbm->getBBox(BBupper, BBlower);
289  this->lower = BBlower;
290  this->upper = BBupper;
291  }
292 
298  };
299  using SolutionSet = std::vector<Solution>;
300 
301  enum Side
302  {
306  };
307 
309  void solve(Models::KBM_ptr kbm, const Vector lower, const Vector upper, Real resolution = 2.0f * M_PI / 180.0f);
310 
312 
313  protected:
315  Models::KBM_ptr subdivideKBM(GlobalIKBase::Side side, unsigned int recursion, Models::KBM_ptr kbm);
317  void subdivideAngles(Side side, unsigned int recursion, Models::KBM_ptr kbm, Vector& center, Vector& spreadAngles);
318 
319  virtual unsigned int recurse(unsigned int level, Models::KBM_ptr kbm) = 0;
322  };
323 
324 
325  struct GraphNode
326  {
327  GraphNode(Models::KBM_ptr _model, Real _ratio, int _level, Real _volume) : model(_model), ratio(_ratio), level(_level), volume(_volume)
328  {
329  }
332  int level;
334  };
335 
341  {
342  protected:
343  unsigned int recurse(unsigned int level, Models::KBM_ptr kbm) override;
344  };
345 
346 
352  {
353  public:
354  GlobalIKSemiBreadth(Real _semiBreadthRecursion, int _solutionSelect = Models::KBM::COVERED);
355  int misses;
357  protected:
361  unsigned int recurse(unsigned int level, Models::KBM_ptr kbm) override;
362  };
363 
364 
365  struct Solution
366  {
367  Solution(const Vector& _center, const Vector& _spreadAngles, const Vector& _upper,
368  const Vector& _lower, const Models::KBM::BBCheckType& _type) :
369  center(_center), spreadAngles(_spreadAngles), upper(_upper), lower(_lower), type(_type) {}
375  };
376 
378  using SolutionSet = std::vector<Solution>;
379 
406  SolutionSet solveGlobalIK(KBM::Models::KBM_ptr kbm, const Vector& lower, const Vector& upper, Real resolution = M_PI / 90.0f);
407  //SolutionSet solveGlobalIK(KBM::Models::KBM_ptr kbm, const Vector &target, Real resolution=M_PI/90.0f);
408 
409 
410 }
411 
412 
memoryx::KBM::Inverse::GraphNode::model
Models::KBM_ptr model
Definition: kbm.h:330
memoryx::KBM::Inverse::GlobalIKBase::GlobalIKBase
GlobalIKBase()
Definition: kbm.h:308
memoryx::KBM::PLS
Namespace where the Partial Least Squares (PLS-1) solver is defined in.
Definition: kbm.h:51
memoryx::KBM::Inverse::GlobalIKBase::SolutionSet
std::vector< Solution > SolutionSet
Definition: kbm.h:299
memoryx::KBM::Models::KBM_ptr
std::shared_ptr< KBM > KBM_ptr
Definition: kbm.h:68
memoryx::KBM::Inverse::GraphNode::volume
Real volume
Definition: kbm.h:333
memoryx::KBM::Inverse::GlobalIKBase::Solution::Solution
Solution(const Vector &_center, const Vector &_spreadAngles, const Vector &_upper, const Vector &_lower, const Models::KBM::BBCheckType &_type)
Definition: kbm.h:280
memoryx::KBM::Matrix
Eigen::MatrixXd Matrix
Definition: kbm.h:38
memoryx::KBM::Inverse::GlobalIKSemiBreadth::GlobalIKSemiBreadth
GlobalIKSemiBreadth(Real _semiBreadthRecursion, int _solutionSelect=Models::KBM::COVERED)
Definition: inverse.cpp:708
memoryx::KBM::Inverse::GlobalIKBase::solutions
SolutionSet solutions
Definition: kbm.h:311
memoryx::KBM::Models::KBM::ErrorValuesType::IQR
Real IQR
Inerquartile range (50% of all errors): .
Definition: kbm.h:115
memoryx::KBM::Inverse::GlobalIKBase::Solution::lower
Vector lower
Definition: kbm.h:296
memoryx::KBM::Inverse::Solution::type
Models::KBM::BBCheckType type
Definition: kbm.h:374
memoryx::KBM::Inverse::GlobalIKBase::subdivideAngles
void subdivideAngles(Side side, unsigned int recursion, Models::KBM_ptr kbm, Vector &center, Vector &spreadAngles)
Subdivides the joint angles only. Can be used to select in which direction to subdivise first.
Definition: inverse.cpp:466
memoryx::KBM::Inverse::Solution
Definition: kbm.h:365
memoryx::KBM::Inverse::GlobalIKSemiBreadth::solutionSelect
int solutionSelect
Definition: kbm.h:360
memoryx::KBM::Inverse::GlobalIKBase::recurse
virtual unsigned int recurse(unsigned int level, Models::KBM_ptr kbm)=0
memoryx::KBM::Inverse::GlobalIKSemiBreadth::misses
int misses
Definition: kbm.h:355
memoryx::KBM::Models::KBM::ErrorValuesType::STD
Real STD
Standard deviation.
Definition: kbm.h:111
memoryx::KBM::Models::KBM::ErrorValuesType::Mean
Real Mean
Mean error.
Definition: kbm.h:109
memoryx::KBM::Inverse::GlobalIKBase::Solution
Definition: kbm.h:278
memoryx::KBM::Inverse::GlobalIKSemiBreadth::semiBreadthRecursion
Real semiBreadthRecursion
Definition: kbm.h:359
memoryx::KBM::Vector
Eigen::VectorXd Vector
Definition: kbm.h:39
memoryx::KBM::Models::KBM::ErrorValuesType
Return type of the evaluation (KBM::getErrors()) function providing insight in the statistics of the ...
Definition: kbm.h:106
memoryx::KBM::Models::operator<<
std::ostream & operator<<(std::ostream &os, const KBM::ErrorValuesType &et)
Definition: kbm.cpp:24
memoryx::KBM::Inverse::GlobalIKBase::Solution::center
Vector center
Definition: kbm.h:293
project
std::string project
Definition: VisualizationRobot.cpp:83
memoryx::KBM::Inverse::Solution::upper
Vector upper
Definition: kbm.h:372
memoryx::KBM::Models::KBM::ErrorValuesType::Median
Real Median
Median of the error.
Definition: kbm.h:113
memoryx::KBM::Models::KBM::COVERED
@ COVERED
The bounding box is covered by the interval (most interesting case for inverse kinematics).
Definition: kbm.h:97
memoryx::KBM::Inverse::GlobalIKBase::Side
Side
Definition: kbm.h:301
memoryx::KBM
Definition: inverse.cpp:19
memoryx::KBM::Inverse::GlobalIKBase::END
@ END
Definition: kbm.h:305
armarx::armem::server::ltm::mongodb::util::store
void store(const mongocxx::database &db, const armem::wm::Memory &m)
Definition: operations.cpp:237
scene3D::ALL
@ ALL
Definition: ManipulatorMode.h:35
M_PI
#define M_PI
Definition: MathTools.h:17
memoryx::KBM::Inverse::GlobalIKBase
Definition: kbm.h:275
memoryx::KBM::Inverse::GraphNode::ratio
Real ratio
Definition: kbm.h:331
memoryx::KBM::Models::KBM::STANDARD
@ STANDARD
Definition: kbm.h:85
memoryx::KBM::Inverse::Solution::center
Vector center
Definition: kbm.h:370
memoryx::KBM::Models
Where the model representation for a Body Schema (especially the Kinematic BĀ“ezier Maps) reside.
Definition: inverse.cpp:19
armarx::aron::input
ReaderT::InputType & input
Definition: rw.h:16
memoryx::KBM::Matrix4
Eigen::Matrix4d Matrix4
Definition: kbm.h:40
memoryx::KBM::Models::KBM::BBCheckType
BBCheckType
Cases for the check of bounding boxes (i.e., an interval ) and an interval ( )both in Cartesian coor...
Definition: kbm.h:92
memoryx::KBM::Inverse::GlobalIKSemiBreadth::recurse
unsigned int recurse(unsigned int level, Models::KBM_ptr kbm) override
Definition: inverse.cpp:526
memoryx::KBM::Inverse::GlobalIKBase::Solution::type
Models::KBM::BBCheckType type
Definition: kbm.h:297
memoryx::KBM::Inverse::GlobalIKBase::Solution::Solution
Solution(Models::KBM_ptr kbm, Models::KBM::BBCheckType type)
Definition: kbm.h:283
KBM_IMPORT_EXPORT
#define KBM_IMPORT_EXPORT
Definition: kbm.h:32
memoryx::KBM::Inverse::GlobalIKExtensive
Finds all solutions to the Global IK.
Definition: kbm.h:340
memoryx::KBM::Models::KBM::ErrorValuesType::Max
Real Max
Maximal error.
Definition: kbm.h:117
memoryx::KBM::Inverse
Namespace for algorithms related to solving the inverse kinematics.
Definition: inverse.cpp:341
memoryx::KBM::Models::KBM::Optimization
Optimization
Enum for the preferred optimization method during batch learning.
Definition: kbm.h:83
memoryx::KBM::Inverse::Solution::spreadAngles
Vector spreadAngles
Definition: kbm.h:371
memoryx::KBM::Inverse::solveGlobalIK
void solveGlobalIK(unsigned int recursion, int side, SolutionSet &solutionSet, Models::KBM_ptr kbm, const Vector &lower, const Vector &upper, Real resolution, Vector spreadAngles, Vector center)
Definition: inverse.cpp:344
memoryx::KBM::Inverse::Solution::lower
Vector lower
Definition: kbm.h:373
memoryx::KBM::Inverse::GlobalIKBase::resolution
Real resolution
Definition: kbm.h:321
memoryx::KBM::Inverse::GraphNode
Definition: kbm.h:325
memoryx::KBM::Inverse::GlobalIKSemiBreadth
Expands all nets until a resolution has been reached, then search only for a single solution.
Definition: kbm.h:351
GfxTL::Matrix4f
MatrixXX< 4, 4, float > Matrix4f
Definition: MatrixXX.h:601
memoryx::KBM::Inverse::GlobalIKBase::subdivideKBM
Models::KBM_ptr subdivideKBM(GlobalIKBase::Side side, unsigned int recursion, Models::KBM_ptr kbm)
Subdivides the kbm and creates new KBM. Calls GlobalIKBase::subdivideAngles().
Definition: inverse.cpp:447
memoryx::KBM::Inverse::GlobalIKSemiBreadth::targetVolume
Real targetVolume
Definition: kbm.h:358
float
#define float
Definition: 16_Level.h:22
memoryx::KBM::Inverse::GlobalIKSemiBreadth::runDijkstra
GlobalIKBase::SolutionSet runDijkstra(KBM::Inverse::GraphNode initial)
Definition: inverse.cpp:658
memoryx::KBM::Inverse::GraphNode::level
int level
Definition: kbm.h:332
memoryx::KBM::Vector3
Eigen::Vector3d Vector3
Definition: kbm.h:41
memoryx::KBM::Inverse::GlobalIKBase::Solution::upper
Vector upper
Definition: kbm.h:295
memoryx::KBM::Models::KBM
The Kinematic B\'ezier Maps.
Definition: kbm.h:78
scene3D::SceneObjectPtr
boost::intrusive_ptr< SceneObject > SceneObjectPtr
Definition: PointerDefinitions.h:40
memoryx::KBM::Inverse::GraphNode::GraphNode
GraphNode(Models::KBM_ptr _model, Real _ratio, int _level, Real _volume)
Definition: kbm.h:327
memoryx::KBM::Inverse::GlobalIKBase::solve
void solve(Models::KBM_ptr kbm, const Vector lower, const Vector upper, Real resolution=2.0f *M_PI/180.0f)
Definition: inverse.cpp:439
memoryx::KBM::Inverse::GlobalIKExtensive::recurse
unsigned int recurse(unsigned int level, Models::KBM_ptr kbm) override
Definition: inverse.cpp:484
memoryx::KBM::Inverse::GlobalIKBase::Solution::spreadAngles
Vector spreadAngles
Definition: kbm.h:294
memoryx::KBM::Inverse::Solution::Solution
Solution(const Vector &_center, const Vector &_spreadAngles, const Vector &_upper, const Vector &_lower, const Models::KBM::BBCheckType &_type)
Definition: kbm.h:367
memoryx::KBM::Inverse::GlobalIKBase::LOWER
@ LOWER
Definition: kbm.h:303
memoryx::KBM::Real
double Real
Type definition of the underlying Realing point type.
Definition: kbm.h:37
memoryx::KBM::Inverse::GlobalIKBase::targetLower
Vector targetLower
Definition: kbm.h:320
memoryx::KBM::Inverse::GlobalIKBase::UPPER
@ UPPER
Definition: kbm.h:304
memoryx::KBM::Inverse::SolutionSet
std::vector< Solution > SolutionSet
Return type of the global inverse kinematics solvers.
Definition: kbm.h:378
memoryx::KBM::PLS::solve
Matrix KBM_IMPORT_EXPORT solve(const Matrix &input, const Matrix output, Real threshold)
Solves a linear system of equations using the partial least squares algorithm.
Definition: pls.cpp:32
memoryx::KBM::Inverse::GlobalIKBase::targetUpper
Vector targetUpper
Definition: kbm.h:320