stores constraints and solves them. More...

#include <constraints.hpp>

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.

ConstraintMatrix::~ConstraintMatrix ( )

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]	nrofalljoints	the size of the output vector.
[in]	list	indicates 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.