Public Member Functions | Public Attributes | Protected Member Functions | Private Attributes
KDL::ConstraintMatrix Class Reference

stores constraints and solves them. More...

#include <constraints.hpp>

List of all members.

Public Member Functions

void addConstraint (const Jacobian< double > &c, double K, double _desval, double _desvald, double _weight)
 adds a 1 d.o.f. constraint on a double value.
void addConstraint (const Jacobian< Vector > &c, double K, const Vector &_desval, const Vector &_desvald, double _weight)
 adds a 3 d.o.f. constraint on a Vector value.
void addConstraint (const Jacobian< Frame > &c, double Kpos, double Krot, const Frame &_desval, const Twist &_desvald, double _weightpos, double _weightrot)
 adds a 6 d.o.f. constraint on a Frame value.
void addConstraint (const Jacobian< Frame > &c, const std::vector< double > &K, const Frame &_desval, const Twist &_desvald, const std::vector< double > &_weight)
 adds a 6 d.o.f. constraint on a Frame value and allows for control constant and weight for each component.
void addConstraint (const Jacobian< Twist > &c, double Kpos, double Krot, const Twist &_desval, const Twist &_desvald, double _weightpos, double _weightrot)
 adds a 6 d.o.f. constraint on a displacement twist value.
void addConstraint (const Jacobian< Wrench > &c, double Kpos, double Krot, const Wrench &_desval, const Wrench &_desvald, double _weightpos, double _weightrot)
 adds a 6 d.o.f. constraint on a Wrench.
void addConstraintJoint (int jointnr, double value, double K, double _desval, double _desvald, double _weight)
 adds a constraint to one joint.
void addConstraintWithoutControl (const Jacobian< Frame > &c, const Twist &desired, const std::vector< double > &_weight)
 adds a 6 d.o.f. constraint specifying the derivative of a Frame. If there should be a control loop, the user should implement it externally to this method.
void calculateOutput (std::vector< double > &output)
 solves the constraints.
void calculateOutput (std::vector< double > &output, const std::vector< double > &nullspace)
 solves the constraints while trying to be as close as possible to a given vector.
void calculateOutput (const std::vector< bool > &list, const std::vector< double > &values, std::vector< double > &output)
 solves the constraints if a certain number of joints have a fixed value.
void calculateOutput (const std::vector< bool > &list, const std::vector< double > &values, std::vector< double > &output, std::vector< double > &nullspace)
 solves the constraints while trying to be as close as possible to a given vector if a certain number of joints have a fixed value.
 ConstraintMatrix (int _maxrows, int _maxcols)
 constructs a Constraint Matrix.
void dump (std::ostream &os)
 dump internal structures to a stream for debugging.
void dumpsvd (std::ostream &os)
 dump statistics about the svd to a stream.
void resetConstraints (int nrofjoints)
 resets to zero constraints.
void setJointSpaceWeights (const std::vector< double > &jsw)
 set the joint space weights for each of the joints.
 ~ConstraintMatrix ()

Public Attributes

double eps
int nrofits

Protected Member Functions

void expandjv (int nrofalljoints, const std::vector< bool > &list, const std::vector< double > &values, std::vector< double > &vec)
 expands a reduced joint vector.
void reduce (const std::vector< bool > &list, const std::vector< double > value)
 reduces the matrix when a number of joints have a fixed value.
void reducejv (const std::vector< bool > &list, std::vector< double > &vec)
 reduces a joint vector when a number of joints have a fixed value.

Private Attributes

double * cmat
double * cmati
int cmatistride
int cmatstride
double * desval
double * jsweight
int maxcols
int maxrows
int nrOfConstraints
int nrofjoints
double * output2
PseudoInverse pinvtmps
double * weight

Detailed Description

stores constraints and solves them.

Definition at line 16 of file constraints.hpp.


Constructor & Destructor Documentation

ConstraintMatrix::ConstraintMatrix ( int  _maxrows,
int  _maxcols 
)

constructs a Constraint Matrix.

Initializes all data-structures maxrows = the number of d.o.f. that the constraints impose. maxcols = the number of d.o.f. of the joint variables.

Todo:
maxrows == maxcols

Definition at line 12 of file constraints.cpp.

Definition at line 376 of file constraints.cpp.


Member Function Documentation

void ConstraintMatrix::addConstraint ( const Jacobian< double > &  c,
double  K,
double  _desval,
double  _desvald,
double  _weight 
)

adds a 1 d.o.f. constraint on a double value.

Adds a constraint that imposes c to be equal to desval with a given weight. adds the line : Jc q = K ( c-desval) + desvald and changes the diagonal element of the weight to the given variable (note : this variable is a square, it is always > 0)

Definition at line 71 of file constraints.cpp.

void ConstraintMatrix::addConstraint ( const Jacobian< Vector > &  c,
double  K,
const Vector _desval,
const Vector _desvald,
double  _weight 
)

adds a 3 d.o.f. constraint on a Vector value.

Definition at line 105 of file constraints.cpp.

void ConstraintMatrix::addConstraint ( const Jacobian< Frame > &  c,
double  Kpos,
double  Krot,
const Frame _desval,
const Twist _desvald,
double  _weightpos,
double  _weightrot 
)

adds a 6 d.o.f. constraint on a Frame value.

Definition at line 203 of file constraints.cpp.

void ConstraintMatrix::addConstraint ( const Jacobian< Frame > &  c,
const std::vector< double > &  K,
const Frame _desval,
const Twist _desvald,
const std::vector< double > &  _weight 
)

adds a 6 d.o.f. constraint on a Frame value and allows for control constant and weight for each component.

Definition at line 139 of file constraints.cpp.

void ConstraintMatrix::addConstraint ( const Jacobian< Twist > &  c,
double  Kpos,
double  Krot,
const Twist _desval,
const Twist _desvald,
double  _weightpos,
double  _weightrot 
)

adds a 6 d.o.f. constraint on a displacement twist value.

Definition at line 245 of file constraints.cpp.

void ConstraintMatrix::addConstraint ( const Jacobian< Wrench > &  c,
double  Kpos,
double  Krot,
const Wrench _desval,
const Wrench _desvald,
double  _weightpos,
double  _weightrot 
)

adds a 6 d.o.f. constraint on a Wrench.

Definition at line 288 of file constraints.cpp.

void ConstraintMatrix::addConstraintJoint ( int  jointnr,
double  value,
double  K,
double  _desval,
double  _desvald,
double  _weight 
)

adds a constraint to one joint.

Parameters:
jointnr: the joint whos value you want to apply a constraint to.
value: actual value of the joint.
K: control constant for this constraint.
_desval: desired value of the joint.
_desvald,:desired velocity of the joint (feedforward).
_weight: weight of this constraint.

Definition at line 93 of file constraints.cpp.

void ConstraintMatrix::addConstraintWithoutControl ( const Jacobian< Frame > &  c,
const Twist desired,
const std::vector< double > &  _weight 
)

adds a 6 d.o.f. constraint specifying the derivative of a Frame. If there should be a control loop, the user should implement it externally to this method.

adds a 6 d.o.f. constraint specifying the derivative of a Frame.

Definition at line 173 of file constraints.cpp.

void ConstraintMatrix::calculateOutput ( std::vector< double > &  output)

solves the constraints.

Parameters:
[out]output: the calculated joint values.
Todo:
: ERROR HANDLING !!!
Todo:

ERROR HANDLING !!!

conversion of std::vector<double> to double* in a safe and portable way.

Definition at line 334 of file constraints.cpp.

void ConstraintMatrix::calculateOutput ( std::vector< double > &  output,
const std::vector< double > &  nullspace 
)

solves the constraints while trying to be as close as possible to a given vector.

Parameters:
[in]nullspace: desired joint values for motion in nullspace
[out]output: the calculated joint values.
Todo:
portable conversion of vector<> to double*

Definition at line 394 of file constraints.cpp.

void ConstraintMatrix::calculateOutput ( const std::vector< bool > &  list,
const std::vector< double > &  values,
std::vector< double > &  output 
)

solves the constraints if a certain number of joints have a fixed value.

Parameters:
[in]list: list[i] is true if joint i is fixed
[in]values: if joint i is fixed, then its value is value[i]
[out]output: the calculated joint values.
Todo:
: ERROR HANDLING !!!

Definition at line 466 of file constraints.cpp.

void ConstraintMatrix::calculateOutput ( const std::vector< bool > &  list,
const std::vector< double > &  values,
std::vector< double > &  output,
std::vector< double > &  nullspace 
)

solves the constraints while trying to be as close as possible to a given vector if a certain number of joints have a fixed value.

Parameters:
[in]list: list[i] is true if joint i is fixed
[in]values: if joint i is fixed, then its value is value[i]
[out]output: the calculated joint values.
[in/out]nullspace : the desired nullspace motion. This vector will be changed by this routine with temporary values.
Todo:
: ERROR HANDLING !!!

Definition at line 474 of file constraints.cpp.

void ConstraintMatrix::dump ( std::ostream &  os)

dump internal structures to a stream for debugging.

Definition at line 33 of file constraints.cpp.

void ConstraintMatrix::dumpsvd ( std::ostream &  os)

dump statistics about the svd to a stream.

Definition at line 50 of file constraints.cpp.

void ConstraintMatrix::expandjv ( int  nrofalljoints,
const std::vector< bool > &  list,
const std::vector< double > &  values,
std::vector< double > &  vec 
) [protected]

expands a reduced joint vector.

expand a reduced joint vector

Parameters:
[in]nrofalljointsthe size of the output vector.
[in]listindicates which joints are fixed
[in]values: values of the fixed joints.
[in/out]vec : input vector

Definition at line 449 of file constraints.cpp.

void ConstraintMatrix::reduce ( const std::vector< bool > &  list,
const std::vector< double >  value 
) [protected]

reduces the matrix when a number of joints have a fixed value.

reduce the problem by filling in the predefined value for some joints

Parameters:
[in]fixed: a bool vector indicating which joints are fixed. true means the joint is fixed.
[in]values: a double vector, for the indices where fixed[] is true it indicates the value.

Definition at line 413 of file constraints.cpp.

void ConstraintMatrix::reducejv ( const std::vector< bool > &  list,
std::vector< double > &  vec 
) [protected]

reduces a joint vector when a number of joints have a fixed value.

adapt a joint vector to a reduced vector

Parameters:
[in]listindicates which joints are fixed
[in/out]vec : original joint vector

Definition at line 438 of file constraints.cpp.

void ConstraintMatrix::resetConstraints ( int  nrofjoints)

resets to zero constraints.

should be called before you call addConstraints. Also resets the joint space weights.

Parameters:
[in]nrofjointsgive the number of joints. subsequent calls to addConstraint should have PV<..> variables with the same number of derivatives.

Definition at line 63 of file constraints.cpp.

void ConstraintMatrix::setJointSpaceWeights ( const std::vector< double > &  jsw)

set the joint space weights for each of the joints.

Definition at line 384 of file constraints.cpp.


Member Data Documentation

double* KDL::ConstraintMatrix::cmat [private]

Definition at line 18 of file constraints.hpp.

double* KDL::ConstraintMatrix::cmati [private]

Definition at line 25 of file constraints.hpp.

Definition at line 26 of file constraints.hpp.

Definition at line 19 of file constraints.hpp.

double* KDL::ConstraintMatrix::desval [private]

Definition at line 20 of file constraints.hpp.

Definition at line 32 of file constraints.hpp.

Definition at line 22 of file constraints.hpp.

Definition at line 17 of file constraints.hpp.

Definition at line 17 of file constraints.hpp.

Definition at line 28 of file constraints.hpp.

Definition at line 33 of file constraints.hpp.

Definition at line 30 of file constraints.hpp.

double* KDL::ConstraintMatrix::output2 [private]

Definition at line 29 of file constraints.hpp.

Definition at line 23 of file constraints.hpp.

double* KDL::ConstraintMatrix::weight [private]

Definition at line 21 of file constraints.hpp.


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


orocos_kdl
Author(s): Ruben Smits, Erwin Aertbelien, Orocos Developers
autogenerated on Sat Dec 28 2013 17:17:26