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 {
35  /// Type definition of the underlying Realing point type.
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 }
50 /// Namespace where the <i>Partial Least Squares (PLS-1)</i> solver is defined in.
52 {
53  /** Solves a linear system of equations using the partial least squares algorithm.
54  * @param input Input values (i.e., the rational polynomial basis)
55  * @param output Output values (e.g., TCP positions/orientation)
56  * @param threshold Tells the algorithm when to stop. If zero, the result is the same as the standard least mean squares (but w/o matrix inversions)
57  */
58  Matrix KBM_IMPORT_EXPORT solve(const Matrix& input, const Matrix output, Real threshold);
59 }
60 
61 /** Where the model representation for a Body Schema (especially the Kinematic B&acute;ezier Maps) reside.
62  * @todo Dynamic B&acute;ezier Maps (KBM for the inverse dynamics) are still to be implemented.
63  */
64 namespace memoryx::KBM::Models
65 {
66 
67  class KBM;
68  using KBM_ptr = std::shared_ptr<KBM>;
69 
70  /** @brief The Kinematic B\'ezier Maps.
71  *
72  * @details This class represents the Kinematic B\'ezier Map algorithm.
73  * See the unit test on hints on how to use it.
74  * @todo In the predict function, the center vector should be substracted from the proprioception!
75  * @todo Introduce more the terms of intervals instead of spread angle and center!?
76  * @todo offer a linearized version (for small angles).
77  */
79  {
80  public:
81 
82  /// Enum for the preferred optimization method during batch learning
84  {
86  PLS
87  };
88 
89  /** Cases for the check of bounding boxes (i.e., an interval \f$ [c_{l,1},c_{u,1}]\times\ldots\times [c_{l,n},c_{u,n}] \f$)
90  * and an interval \f$i\f$ (\f$ [i_{l,1},i_{u,1}]\times\ldots\times [i_{l,n},i_{u,n}] \f$)both in Cartesian coordinates.
91  */
93  {
94  /// The interval is included in the bounding box \f$ \forall j: i_{l,j} > c_{l,j} \wedge c_{u,j} > i_{u,j} \f$
95  INCLUDED = 1,
96  /// The bounding box is covered by the interval \f$ \forall j: i_{l,j} < c_{l,j} \wedge c_{u,j} < i_{u,j} \f$ (most interesting case for inverse kinematics).
97  COVERED = 2,
98  /// The bounding box and interval are disjoint \f$ \exists_j: i_{u,j} < c_{l,j} \vee c_{u,j} < i_{l,j} \f$
99  DISJOINT = 4,
100  /// The interval and bounding box are partially overlapping (if none of the above applies).
101  OVERLAPPING = 8,
102  ALL = 15
103  };
104 
105  /// Return type of the evaluation (KBM::getErrors()) function providing insight in the statistics of the prediction error.
107  {
108  /// Mean error
110  /// Standard deviation
112  /// Median of the error
114  /// Inerquartile range (50% of all errors): \f$IQR= Q_{75\%} - Q_{25\%}\f$
116  /// Maximal error
118  /// For debugging output
119  friend KBM_IMPORT_EXPORT std::ostream& operator<<(std::ostream& os, const ErrorValuesType& et);
120  };
121 
122  /// Copy constructor
123  KBM(const KBM& other);
124 
125  /** @brief Constructor
126  * @param nDoF The number of the kinematic chain's degrees of freedom (number of input dimensions)
127  * @param dim The number of output dimensions (usually three)
128  * @param spreadAngle The input values should be centered around 0 and this angle should represent
129  * their distribution in input space.
130  */
131  KBM(int nDoF, int dim, Real spreadAngle);
132 
133  /// loads a KBM with same input/output dimensions from disk (comma-separated values).
134  bool restore(std::string fileName = "");
135  /// stores the current KBM as a comma-separated matrix to disk.
136  void store(std::string fileName = "");
137  /** @brief Implements the online learning algorithm.
138  * @param proprioception The input values (joint angles)
139  * @param shape The output values (e.g. 3D TCP positions)
140  * @param learnRate Learning rate between (0,1)
141  * Can be called (and still makes sense) after KBM::restore() and KBM::batch() and
142  * prior calls to KBM::online().
143  * Proprioception must have the same number of columns (i.e., number of samples)
144  * while the rows must match the number of input and output dimensions respectively.
145  */
146  void online(const Matrix& proprioception, const Matrix& shape, Real learnRate = 1.0);
147  // @param learnRate Learning rate between (0,1)
148  /** @brief Implements the batch learning algorithm.
149  * @param proprioception The input values (joint angles)
150  * @param shape The output values (e.g. 3D TCP positions)
151  * @param method Select optimization with standard linear least mean squares (KBM::STANDARD) or partial least squares (KMB::PLS)
152  * @param threshold Models the noise and should equal the expected mean positioning error of the system. Only used with partial least squares (KBM::PLS)
153  * Proprioception must have the same number of columns (i.e., number of samples)
154  * while the rows must match the number of input and output dimensions respectively.
155  */
156  void batch(const Matrix& proprioception, const Matrix& shape, Optimization method = KBM::STANDARD, Real threshold = 0.0);
157  /** After learning this function gives various error measures over a given test set.online
158  * Proprioception must have the same number of columns (i.e., number of samples)
159  * while the rows must match the number of input and output dimensions respectively.*/
160  ErrorValuesType getErrors(const Matrix& proprioception, const Matrix& shape);
161  /// Forgets all previously learned samples.
162  void reset();
163 
164  /** @brief Predicts a value of the FK given a set of joint angles.
165  * @param proprioception The input values (joint angles in the columns).
166  * @param dim If > 0, the prediction assumes it is the last joint in the kinematic chain and creates a coordinate frame
167  * according to the partial derivative.
168  */
169  Matrix predict(const Matrix& proprioception, int dim = 0) const;
170 
171  /// Prepares the calculus of the derivatives. Has to be called alwayes after learning.
172  void differentiate();
173 
174  /// Computes a partial derivative with respect to a configuration and the specified degree of freedom.
175  Vector getPartialDerivative(const Vector& proprioception, int iDoF) const;
176 
177  /// Computes the partial derivative with respect to a configuration.
178  Matrix getJacobian(const Vector& proprioception) const;
179 
180  /** @brief <b>Work in progress</b> This method subdivides the KBM representation.
181  * @param center The new zero of the input values.
182  * @param newSpreadAngles The new spread angle vector after subdivision. It can also be used to increase
183  * the original angles. <b>Important:</b> The new spread angles must be smaller than
184  * \f$90^\circ\f$ degrees and a smaller value is more suitable in general.
185  * @todo This method needs to be further developed to enable the global inverse kinematics search.
186  */
187  void /*std::shared_ptr<KBM>*/ subdivide(const Vector& center, const Vector& newSpreadAngles);
188 
189  /** @brief <b>Work in progress</b> This method computes the control net of a subdivised KBM instance.
190  * @param center The new zero of the input values.
191  * @param newSpreadAngles The new spread angle vector after subdivision.
192  * @param resultingControlNet The resulting control net of the subdivised KBM.
193  *
194  * This method computes only the control net (i.e., does not create a new KBM instance). It can be used, for instance,
195  * in the global inverse kinematics algorithms.
196  */
197  void getSubdivisedNet(const Vector newCenter, Vector newSpreadAngles, Matrix& resultingControlNet) const;
198 
199  /// Makes the subdivision only for one degree of freedom
200  void getSubdivisedNet(unsigned int dof, Real center, Real newSpreadAngle, Matrix& resultingControlNet) const;
201 
202  /** @brief Check whether an interval overlaps the the bounding box of the control points.
203  * @param lower The lower bounds of the interval.
204  * @param upper The upper bounds of the interval.
205  * @param tolerance Widens each bounding box by twice this skalar. You should use lower and upper to implement this behavior regularly.
206  * @todo Add parameter to chose between overlapping and containment.
207  */
208  BBCheckType checkBBox(const Vector& lower, const Vector& upper, Real tolerance = 0.0f) const;
209 
210  /** @brief Check whether an interval overlaps the the bounding box of a given control net.
211  * @param lower The lower bounds of the interval.
212  * @param upper The upper bounds of the interval.
213  * @param controlNet Check againt the bounding box of the points defined by the columns of this matrix.
214  * @param tolerance Widens each bounding box by twice this skalar. You should use lower and upper to implement this behavior regularly.
215  * @see KBM::Models::KBM::BBCheckType for details.
216  * @todo Add parameter to chose between overlapping and containment. Containment seems better but more expensive (more recursions)
217  *
218  */
219  static BBCheckType checkBBoxes(const Vector& lower, const Vector& upper, const Matrix& controlNet, Real tolerance = 0.0f);
220 
221 
222  /// Helper function that extracts the bounding box of a control net.
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 
230  /// Return the spread angles.
231  Vector getSpreadAngles() const;
232 
233  /// Return the center.
234  Vector getCenter() const;
235 
236  /// Get the number of degrees of freedom (DoF).
237  int getNDoF() const;
238  int getNDim() const;
239  Matrix getControlNet() const;
240 
241  /// Factory methods
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:
245  /** @brief Creates the system of linear equations formed by the Bernstein-polynomials and the projection.
246  * @param proprioception Matrix with joint angles in its columns / (suggested) ColumnVector for blossom
247  * @param dim If 0 < dim < #joint angles, then calculate the blossom for the dim-th joint.
248  * @param a1 If calculating the blossom, a1 and a2 are used instead of proprioception(dim,*).
249  *
250  * About the blossom: In short, it creates all intermediate points constructed by DeCasteljau's algorithm.
251  * It can be used to obtain the control points for the curves in the main direction or its the partial derivative.
252  */
253  Matrix createSLE(const Matrix& proprioception, int dim = -1, Real a1 = 0.0f, Real a2 = 0.0f, bool project = true) const;
254  /// Creates an index for addressing the control vertice
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 
271 /// Namespace for algorithms related to solving the inverse kinematics
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:
314  /// Subdivides the kbm and creates new KBM. Calls GlobalIKBase::subdivideAngles().
315  Models::KBM_ptr subdivideKBM(GlobalIKBase::Side side, unsigned int recursion, Models::KBM_ptr kbm);
316  /// Subdivides the joint angles only. Can be used to select in which direction to subdivise first.
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 
336  /** Finds <i>all</i> solutions to the Global IK.
337  *
338  * Note that this may be \f$ 2^\frac{\alpha_{spread}}{\alpha_{resolution}}\f$ (i.e., <i> a lot </i>) solutions.
339  */
341  {
342  protected:
343  unsigned int recurse(unsigned int level, Models::KBM_ptr kbm) override;
344  };
345 
346 
347  /** Expands all nets until a resolution has been reached, then search only for a single solution.
348  *
349  * This is the most promising approach for large kinematics.
350  */
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 
377  /// Return type of the global inverse kinematics solvers.
378  using SolutionSet = std::vector<Solution>;
379 
380  /** @brief Algorithm to find the <b>global</b> solution to the inverse kinematics based on the KBM structure. <b>Work in progress</b>.
381  * @param kbm The already learned KBM representation of a kinematics.
382  * @param lower The lower bound of the interval in Cartesian space.
383  * @param upper The upper bound of the interval in Cartesian space. Lower and upper bounds should be a target destination plus a desired tolerance, but can be chosen with equal values.
384  * @param resolution The granularity of the found solutions (defaults to \f$2^\circ\f$).
385  * @return The algorithm returns a list of joint angle intervals (the size is specified by the resolution parameters).
386  *
387  * This algorithm solves the inverse kinematics <b>globally</b> and <b>numerically</b> by using an interval based
388  * version of the forward kinemtatics
389  * \f[ f(\alpha_1, \beta_1,\ldots,\alpha_n,\beta_n) = ([a_x, \ldots, b_x],[a_y,\ldots,b_y],[a_z,\ldots,b_z]).\f]
390  * which can be subdivided easily because of the linearisation emerging from the KBM model.
391  * This means that if there exist more than a single solution,
392  * a list of intervals that <i>probably</i> contain a solution. Specifically, this means that the target position lies within
393  * the bounding box of the control net of the subdivised kbm. If a sufficient amount of steps has been performed,
394  * the solution is garantueed to lie at least close to the desired target. The number of steps can be regulated
395  * by the resolution parameter (i.e., \f$n_{steps}=\log_2\frac{\alpha_{spread}}{\alpha_{resolution}}\f$).
396  * An interval-based version of this algorithm exists to define this distance in Cartesian space.
397  *
398  * \image html subdivision.png "Subdivision in angular space"
399  * \image html subdivision2.png "Subdivision in angular space"
400  *
401  * @todo Allow for additional constraints and behaviors in the form of a functor parameter.
402  * @todo Transform into class (with solution set embedded). For easier parameter settings. Further, SolutionSet should be a class with more information.
403  * @todo Change the enum type in KBM::Models::KBM::BBCheckType such that they can be combined by binary operators.
404  * @todo Allow depth search, also conditional after a given accuracy to avoid multiple solutions.
405  */
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
index
uint8_t index
Definition: EtherCATFrame.h:59
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:82
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:19
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