KBM Class Reference

The Kinematic B\'ezier Maps. More...

#include <MemoryX/libraries/helpers/KinematicBezierMaps/kbm.h>

Classes

struct  ErrorValuesType
 Return type of the evaluation (KBM::getErrors()) function providing insight in the statistics of the prediction error. More...
 

Public Types

enum  BBCheckType {
  INCLUDED = 1, COVERED = 2, DISJOINT = 4, OVERLAPPING = 8,
  ALL = 15
}
 Cases for the check of bounding boxes (i.e., an interval $ [c_{l,1},c_{u,1}]\times\ldots\times [c_{l,n},c_{u,n}] $) and an interval $i$ ( $ [i_{l,1},i_{u,1}]\times\ldots\times [i_{l,n},i_{u,n}] $)both in Cartesian coordinates. More...
 
enum  Optimization { STANDARD, PLS }
 Enum for the preferred optimization method during batch learning. More...
 

Public Member Functions

void batch (const Matrix &proprioception, const Matrix &shape, Optimization method=KBM::STANDARD, Real threshold=0.0)
 Implements the batch learning algorithm. More...
 
BBCheckType checkBBox (const Vector &lower, const Vector &upper, Real tolerance=0.0f) const
 Check whether an interval overlaps the the bounding box of the control points. More...
 
void differentiate ()
 Prepares the calculus of the derivatives. Has to be called alwayes after learning. More...
 
void getBBox (Vector &lower, Vector &upper)
 
Vector getCenter () const
 Return the center. More...
 
Matrix getControlNet () const
 
ErrorValuesType getErrors (const Matrix &proprioception, const Matrix &shape)
 After learning this function gives various error measures over a given test set.online Proprioception must have the same number of columns (i.e., number of samples) while the rows must match the number of input and output dimensions respectively. More...
 
Matrix getJacobian (const Vector &proprioception) const
 Computes the partial derivative with respect to a configuration. More...
 
int getNDim () const
 
int getNDoF () const
 Get the number of degrees of freedom (DoF). More...
 
Real getOverlappingVolume (const Vector &lower, const Vector &upper)
 
Real getOverlappingVolumeRatio (const Vector &lower, const Vector &upper, Real targetVolume)
 
Vector getPartialDerivative (const Vector &proprioception, int iDoF) const
 Computes a partial derivative with respect to a configuration and the specified degree of freedom. More...
 
Vector getSpreadAngles () const
 Return the spread angles. More...
 
void getSubdivisedNet (const Vector newCenter, Vector newSpreadAngles, Matrix &resultingControlNet) const
 Work in progress This method computes the control net of a subdivised KBM instance. More...
 
void getSubdivisedNet (unsigned int dof, Real center, Real newSpreadAngle, Matrix &resultingControlNet) const
 Makes the subdivision only for one degree of freedom. More...
 
Real getVolume ()
 
 KBM (const KBM &other)
 Copy constructor. More...
 
 KBM (int nDoF, int dim, Real spreadAngle)
 Constructor. More...
 
void online (const Matrix &proprioception, const Matrix &shape, Real learnRate=1.0)
 Implements the online learning algorithm. More...
 
Matrix predict (const Matrix &proprioception, int dim=0) const
 Predicts a value of the FK given a set of joint angles. More...
 
void reset ()
 Forgets all previously learned samples. More...
 
bool restore (std::string fileName="")
 loads a KBM with same input/output dimensions from disk (comma-separated values). More...
 
void store (std::string fileName="")
 stores the current KBM as a comma-separated matrix to disk. More...
 
void subdivide (const Vector &center, const Vector &newSpreadAngles)
 Work in progress This method subdivides the KBM representation. More...
 

Static Public Member Functions

static BBCheckType checkBBoxes (const Vector &lower, const Vector &upper, const Matrix &controlNet, Real tolerance=0.0f)
 Check whether an interval overlaps the the bounding box of a given control net. More...
 
static KBM_ptr createFromVirtualRobot (VirtualRobot::KinematicChainPtr chain, VirtualRobot::SceneObjectPtr FoR, const Vector &spreadAngles, bool useOrientation=false)
 
static KBM_ptr createKBM (int nDoF, int dim, const Vector &center, const Vector &spreadAngles, const Matrix &controlNet)
 Factory methods. More...
 
static void getBBox (Vector &lower, Vector &upper, const Matrix &controlNet)
 Helper function that extracts the bounding box of a control net. More...
 

Detailed Description

The Kinematic B\'ezier Maps.

This class represents the Kinematic B\'ezier Map algorithm. See the unit test on hints on how to use it.

Todo:

In the predict function, the center vector should be substracted from the proprioception!

Introduce more the terms of intervals instead of spread angle and center!?

offer a linearized version (for small angles).

Definition at line 78 of file kbm.h.

Member Enumeration Documentation

◆ BBCheckType

Cases for the check of bounding boxes (i.e., an interval $ [c_{l,1},c_{u,1}]\times\ldots\times [c_{l,n},c_{u,n}] $) and an interval $i$ ( $ [i_{l,1},i_{u,1}]\times\ldots\times [i_{l,n},i_{u,n}] $)both in Cartesian coordinates.

Enumerator
INCLUDED 

The interval is included in the bounding box $ \forall j: i_{l,j} > c_{l,j} \wedge c_{u,j} > i_{u,j} $.

COVERED 

The bounding box is covered by the interval $ \forall j: i_{l,j} < c_{l,j} \wedge c_{u,j} < i_{u,j} $ (most interesting case for inverse kinematics).

DISJOINT 

The bounding box and interval are disjoint $ \exists_j: i_{u,j} < c_{l,j} \vee c_{u,j} < i_{l,j} $.

OVERLAPPING 

The interval and bounding box are partially overlapping (if none of the above applies).

ALL 

Definition at line 92 of file kbm.h.

◆ Optimization

Enum for the preferred optimization method during batch learning.

Enumerator
STANDARD 
PLS 

Definition at line 83 of file kbm.h.

Constructor & Destructor Documentation

◆ KBM() [1/2]

KBM ( const KBM other)

Copy constructor.

Definition at line 30 of file kbm.cpp.

+ Here is the caller graph for this function:

◆ KBM() [2/2]

KBM ( int  nDoF,
int  dim,
Real  spreadAngle 
)

Constructor.

Parameters
nDoFThe number of the kinematic chain's degrees of freedom (number of input dimensions)
dimThe number of output dimensions (usually three)
spreadAngleThe input values should be centered around 0 and this angle should represent their distribution in input space.

Definition at line 42 of file kbm.cpp.

Member Function Documentation

◆ batch()

void batch ( const Matrix proprioception,
const Matrix shape,
Optimization  method = KBM::STANDARD,
Real  threshold = 0.0 
)

Implements the batch learning algorithm.

Parameters
proprioceptionThe input values (joint angles)
shapeThe output values (e.g. 3D TCP positions)
methodSelect optimization with standard linear least mean squares (KBM::STANDARD) or partial least squares (KMB::PLS)
thresholdModels the noise and should equal the expected mean positioning error of the system. Only used with partial least squares (KBM::PLS) Proprioception must have the same number of columns (i.e., number of samples) while the rows must match the number of input and output dimensions respectively.

Definition at line 200 of file kbm.cpp.

+ Here is the call graph for this function:

◆ checkBBox()

KBM::BBCheckType checkBBox ( const Vector lower,
const Vector upper,
Real  tolerance = 0.0f 
) const

Check whether an interval overlaps the the bounding box of the control points.

Parameters
lowerThe lower bounds of the interval.
upperThe upper bounds of the interval.
toleranceWidens each bounding box by twice this skalar. You should use lower and upper to implement this behavior regularly.
Todo:
Add parameter to chose between overlapping and containment.

Definition at line 231 of file inverse.cpp.

+ Here is the call graph for this function:

◆ checkBBoxes()

KBM::BBCheckType checkBBoxes ( const Vector lower,
const Vector upper,
const Matrix controlNet,
Real  tolerance = 0.0f 
)
static

Check whether an interval overlaps the the bounding box of a given control net.

Parameters
lowerThe lower bounds of the interval.
upperThe upper bounds of the interval.
controlNetCheck againt the bounding box of the points defined by the columns of this matrix.
toleranceWidens each bounding box by twice this skalar. You should use lower and upper to implement this behavior regularly.
See also
KBM::Models::KBM::BBCheckType for details.
Todo:
Add parameter to chose between overlapping and containment. Containment seems better but more expensive (more recursions)

Definition at line 236 of file inverse.cpp.

+ Here is the call graph for this function:
+ Here is the caller graph for this function:

◆ createFromVirtualRobot()

KBM_ptr createFromVirtualRobot ( VirtualRobot::KinematicChainPtr  chain,
VirtualRobot::SceneObjectPtr  FoR,
const Vector spreadAngles,
bool  useOrientation = false 
)
static

Definition at line 440 of file kbm.cpp.

+ Here is the call graph for this function:
+ Here is the caller graph for this function:

◆ createKBM()

KBM_ptr createKBM ( int  nDoF,
int  dim,
const Vector center,
const Vector spreadAngles,
const Matrix controlNet 
)
static

Factory methods.

Definition at line 407 of file kbm.cpp.

+ Here is the call graph for this function:
+ Here is the caller graph for this function:

◆ differentiate()

void differentiate ( )

Prepares the calculus of the derivatives. Has to be called alwayes after learning.

Definition at line 351 of file kbm.cpp.

◆ getBBox() [1/2]

void getBBox ( Vector lower,
Vector upper 
)

Definition at line 289 of file inverse.cpp.

+ Here is the call graph for this function:

◆ getBBox() [2/2]

void getBBox ( Vector lower,
Vector upper,
const Matrix controlNet 
)
static

Helper function that extracts the bounding box of a control net.

Definition at line 283 of file inverse.cpp.

+ Here is the caller graph for this function:

◆ getCenter()

Vector getCenter ( ) const

Return the center.

Definition at line 387 of file kbm.cpp.

◆ getControlNet()

Matrix getControlNet ( ) const

Definition at line 402 of file kbm.cpp.

◆ getErrors()

KBM::ErrorValuesType getErrors ( const Matrix proprioception,
const Matrix shape 
)

After learning this function gives various error measures over a given test set.online Proprioception must have the same number of columns (i.e., number of samples) while the rows must match the number of input and output dimensions respectively.

Definition at line 57 of file kbm.cpp.

+ Here is the call graph for this function:

◆ getJacobian()

Matrix getJacobian ( const Vector proprioception) const

Computes the partial derivative with respect to a configuration.

Definition at line 370 of file kbm.cpp.

+ Here is the call graph for this function:
+ Here is the caller graph for this function:

◆ getNDim()

int getNDim ( ) const

Definition at line 397 of file kbm.cpp.

+ Here is the caller graph for this function:

◆ getNDoF()

int getNDoF ( ) const

Get the number of degrees of freedom (DoF).

Definition at line 392 of file kbm.cpp.

+ Here is the caller graph for this function:

◆ getOverlappingVolume()

Real getOverlappingVolume ( const Vector lower,
const Vector upper 
)

Definition at line 308 of file inverse.cpp.

+ Here is the call graph for this function:
+ Here is the caller graph for this function:

◆ getOverlappingVolumeRatio()

Real getOverlappingVolumeRatio ( const Vector lower,
const Vector upper,
Real  targetVolume 
)

Definition at line 302 of file inverse.cpp.

+ Here is the call graph for this function:

◆ getPartialDerivative()

Vector getPartialDerivative ( const Vector proprioception,
int  iDoF 
) const

Computes a partial derivative with respect to a configuration and the specified degree of freedom.

Definition at line 359 of file kbm.cpp.

+ Here is the caller graph for this function:

◆ getSpreadAngles()

Vector getSpreadAngles ( ) const

Return the spread angles.

Definition at line 382 of file kbm.cpp.

◆ getSubdivisedNet() [1/2]

void getSubdivisedNet ( const Vector  newCenter,
Vector  newSpreadAngles,
Matrix resultingControlNet 
) const

Work in progress This method computes the control net of a subdivised KBM instance.

Parameters
centerThe new zero of the input values.
newSpreadAnglesThe new spread angle vector after subdivision.
resultingControlNetThe resulting control net of the subdivised KBM.

This method computes only the control net (i.e., does not create a new KBM instance). It can be used, for instance, in the global inverse kinematics algorithms.

Definition at line 138 of file inverse.cpp.

+ Here is the caller graph for this function:

◆ getSubdivisedNet() [2/2]

void getSubdivisedNet ( unsigned int  dof,
Real  center,
Real  newSpreadAngle,
Matrix resultingControlNet 
) const

Makes the subdivision only for one degree of freedom.

Definition at line 35 of file inverse.cpp.

◆ getVolume()

Real getVolume ( )

Definition at line 294 of file inverse.cpp.

+ Here is the call graph for this function:

◆ online()

void online ( const Matrix proprioception,
const Matrix shape,
Real  learnRate = 1.0 
)

Implements the online learning algorithm.

Parameters
proprioceptionThe input values (joint angles)
shapeThe output values (e.g. 3D TCP positions)
learnRateLearning rate between (0,1) Can be called (and still makes sense) after KBM::restore() and KBM::batch() and prior calls to KBM::online(). Proprioception must have the same number of columns (i.e., number of samples) while the rows must match the number of input and output dimensions respectively.

Definition at line 221 of file kbm.cpp.

+ Here is the call graph for this function:

◆ predict()

Matrix predict ( const Matrix proprioception,
int  dim = 0 
) const

Predicts a value of the FK given a set of joint angles.

Parameters
proprioceptionThe input values (joint angles in the columns).
dimIf > 0, the prediction assumes it is the last joint in the kinematic chain and creates a coordinate frame according to the partial derivative.

Definition at line 317 of file kbm.cpp.

+ Here is the caller graph for this function:

◆ reset()

void reset ( )

Forgets all previously learned samples.

Definition at line 304 of file kbm.cpp.

◆ restore()

bool restore ( std::string  fileName = "")

loads a KBM with same input/output dimensions from disk (comma-separated values).

Definition at line 248 of file kbm.cpp.

+ Here is the call graph for this function:

◆ store()

void store ( std::string  fileName = "")

stores the current KBM as a comma-separated matrix to disk.

Definition at line 418 of file kbm.cpp.

◆ subdivide()

void subdivide ( const Vector center,
const Vector newSpreadAngles 
)

Work in progress This method subdivides the KBM representation.

Parameters
centerThe new zero of the input values.
newSpreadAnglesThe new spread angle vector after subdivision. It can also be used to increase the original angles. Important: The new spread angles must be smaller than $90^\circ$ degrees and a smaller value is more suitable in general.
Todo:
This method needs to be further developed to enable the global inverse kinematics search.

Definition at line 28 of file inverse.cpp.

+ Here is the call graph for this function:

The documentation for this class was generated from the following files: