#include <ConstraintSet.hpp>

Inherited by iTaSC::CopyPose, and iTaSC::Distance.

Public Member Functions
	ConstraintSet (unsigned int nc, double accuracy, unsigned int maximum_iterations)
	ConstraintSet ()
virtual	~ConstraintSet ()
virtual EIGEN_MAKE_ALIGNED_OPERATOR_NEW bool	registerCallback (ConstraintCallback _function, void *_param)
virtual const e_vector &	getControlOutput () const
virtual const ConstraintValues *	getControlParameters (unsigned int *_nvalues)=0
virtual bool	setControlParameters (ConstraintValues *_values, unsigned int _nvalues, double timestep=0.0)=0
bool	setControlParameter (int id, ConstraintAction action, double value, double timestep=0.0)
virtual const e_matrix6 &	getJf () const
virtual const KDL::Frame &	getPose () const
virtual const e_matrix &	getCf () const
virtual const e_vector &	getWy () const
virtual void	setWy (const e_vector &Wy_in)
virtual void	setJointVelocity (const e_vector chidot_in)
virtual unsigned int	getNrOfConstraints ()
void	substep (bool _substep)
bool	substep ()

Protected Member Functions
virtual void	modelUpdate (KDL::Frame &_external_pose, const Timestamp &timestamp)
virtual void	updateKinematics (const Timestamp &timestamp)=0
virtual void	pushCache (const Timestamp &timestamp)=0
virtual void	updateJacobian ()=0
virtual void	updateControlOutput (const Timestamp &timestamp)=0
virtual void	initCache (Cache *_cache)=0
virtual bool	initialise (KDL::Frame &init_pose)
virtual void	reset (unsigned int nc, double accuracy, unsigned int maximum_iterations)
virtual bool	closeLoop ()
virtual double	getMaxTimestep (double &timestep)

Protected Attributes
unsigned int	m_nc
e_scalar	m_maxDeltaChi
e_matrix	m_Cf
e_vector	m_Wy
e_vector	m_y
e_vector	m_ydot
e_vector6	m_chi
e_vector6	m_chidot
e_vector6	m_S
e_vector6	m_temp
e_vector6	m_tdelta
e_matrix6	m_Jf
e_matrix6	m_U
e_matrix6	m_V
e_matrix6	m_B
e_matrix6	m_Jf_inv
KDL::Frame	m_internalPose
KDL::Frame	m_externalPose
ConstraintCallback	m_constraintCallback
void *	m_constraintParam
void *	m_poseParam
bool	m_toggle
bool	m_substep
double	m_threshold
unsigned int	m_maxIter

Friends
class	Scene

Detailed Description

Definition at line 52 of file ConstraintSet.hpp.

Constructor & Destructor Documentation

◆ ConstraintSet() [1/2]

iTaSC::ConstraintSet::ConstraintSet	(	unsigned int	nc,
		double	accuracy,
		unsigned int	maximum_iterations )

Definition at line 14 of file ConstraintSet.cpp.

References e_identity_matrix, e_scalar, e_scalar_vector, e_zero_matrix, e_zero_vector, iTaSC::F_identity, false, m_B, m_Cf, m_chi, m_constraintCallback, m_constraintParam, m_externalPose, m_internalPose, m_Jf, m_Jf_inv, m_maxDeltaChi, m_maxIter, m_nc, m_S, m_substep, m_tdelta, m_temp, m_threshold, m_toggle, m_U, m_V, m_Wy, m_y, and m_ydot.

Referenced by iTaSC::CopyPose::CopyPose(), and iTaSC::Distance::Distance().

◆ ConstraintSet() [2/2]

iTaSC::ConstraintSet::ConstraintSet ( )

Definition at line 31 of file ConstraintSet.cpp.

References e_scalar, iTaSC::F_identity, false, m_constraintCallback, m_constraintParam, m_externalPose, m_internalPose, m_maxDeltaChi, m_maxIter, m_nc, m_substep, m_threshold, and m_toggle.

◆ ~ConstraintSet()

iTaSC::ConstraintSet::~ConstraintSet ( )

virtual

Definition at line 62 of file ConstraintSet.cpp.

Member Function Documentation

◆ closeLoop()

bool iTaSC::ConstraintSet::closeLoop ( )

protectedvirtual

Reimplemented in iTaSC::Distance.

Definition at line 140 of file ConstraintSet.cpp.

References KDL::diff(), KDL::Equal(), iTaSC::F_identity, i, m_B, m_chi, m_externalPose, m_internalPose, m_Jf, m_Jf_inv, m_S, m_tdelta, m_temp, m_threshold, m_U, m_V, KDL::Twist::RefPoint(), ret, KDL::svd_eigen_HH(), and updateJacobian().

Referenced by initialise(), and modelUpdate().

◆ getCf()

virtual const e_matrix & iTaSC::ConstraintSet::getCf ( ) const

inlinevirtual

Definition at line 103 of file ConstraintSet.hpp.

References e_matrix, and m_Cf.

Referenced by iTaSC::Scene::initialize().

◆ getControlOutput()

virtual const e_vector & iTaSC::ConstraintSet::getControlOutput ( ) const

inlinevirtual

Definition at line 96 of file ConstraintSet.hpp.

References e_vector, and m_ydot.

Referenced by iTaSC::Scene::update().

◆ getControlParameters()

virtual const ConstraintValues * iTaSC::ConstraintSet::getControlParameters ( unsigned int * _nvalues )

pure virtual

Implemented in iTaSC::CopyPose, and iTaSC::Distance.

Referenced by execute_scene().

◆ getJf()

virtual const e_matrix6 & iTaSC::ConstraintSet::getJf ( ) const

inlinevirtual

Definition at line 101 of file ConstraintSet.hpp.

References e_matrix6, and m_Jf.

Referenced by iTaSC::Scene::update().

◆ getMaxTimestep()

double iTaSC::ConstraintSet::getMaxTimestep ( double & timestep )

protectedvirtual

Reimplemented in iTaSC::CopyPose.

Definition at line 79 of file ConstraintSet.cpp.

References e_scalar, m_chidot, and m_maxDeltaChi.

Referenced by iTaSC::Scene::update().

◆ getNrOfConstraints()

virtual unsigned int iTaSC::ConstraintSet::getNrOfConstraints ( )

inlinevirtual

Definition at line 109 of file ConstraintSet.hpp.

References m_nc.

Referenced by iTaSC::Scene::addConstraintSet().

◆ getPose()

virtual const KDL::Frame & iTaSC::ConstraintSet::getPose ( ) const

inlinevirtual

Definition at line 102 of file ConstraintSet.hpp.

References m_internalPose.

◆ getWy()

virtual const e_vector & iTaSC::ConstraintSet::getWy ( ) const

inlinevirtual

Definition at line 105 of file ConstraintSet.hpp.

References e_vector, and m_Wy.

◆ initCache()

virtual void iTaSC::ConstraintSet::initCache ( Cache * _cache )

protectedpure virtual

Implemented in iTaSC::CopyPose, and iTaSC::Distance.

Referenced by iTaSC::Scene::initialize().

◆ initialise()

bool iTaSC::ConstraintSet::initialise ( KDL::Frame & init_pose )

protectedvirtual

Reimplemented in iTaSC::CopyPose, and iTaSC::Distance.

Definition at line 88 of file ConstraintSet.cpp.

References closeLoop(), m_externalPose, m_maxIter, and updateJacobian().

Referenced by iTaSC::Scene::initialize(), and iTaSC::Scene::update().

◆ modelUpdate()

void iTaSC::ConstraintSet::modelUpdate	(	KDL::Frame &	_external_pose,
		const Timestamp &	timestamp )

protectedvirtual

Reimplemented in iTaSC::CopyPose.

Definition at line 66 of file ConstraintSet.cpp.

References closeLoop(), m_chi, m_chidot, m_externalPose, iTaSC::Timestamp::realTimestep, and updateJacobian().

Referenced by iTaSC::Scene::update().

◆ pushCache()

virtual void iTaSC::ConstraintSet::pushCache ( const Timestamp & timestamp )

protectedpure virtual

Implemented in iTaSC::CopyPose, and iTaSC::Distance.

◆ registerCallback()

virtual EIGEN_MAKE_ALIGNED_OPERATOR_NEW bool iTaSC::ConstraintSet::registerCallback	(	ConstraintCallback	_function,
		void *	_param )

inlinevirtual

Definition at line 89 of file ConstraintSet.hpp.

References m_constraintCallback, and m_constraintParam.

Referenced by convert_tree().

◆ reset()

void iTaSC::ConstraintSet::reset	(	unsigned int	nc,
		double	accuracy,
		unsigned int	maximum_iterations )

protectedvirtual

Definition at line 41 of file ConstraintSet.cpp.

References e_identity_matrix, e_scalar_vector, e_zero_matrix, e_zero_vector, m_B, m_Cf, m_chi, m_chidot, m_Jf, m_Jf_inv, m_maxIter, m_nc, m_S, m_tdelta, m_temp, m_threshold, m_U, m_V, m_Wy, m_y, and m_ydot.

◆ setControlParameter()

bool iTaSC::ConstraintSet::setControlParameter	(	int	id,
		ConstraintAction	action,
		double	value,
		double	timestep = 0.0 )

◆ setControlParameters()

virtual bool iTaSC::ConstraintSet::setControlParameters	(	ConstraintValues *	_values,
		unsigned int	_nvalues,
		double	timestep = 0.0 )

pure virtual

Implemented in iTaSC::CopyPose, and iTaSC::Distance.

Referenced by setControlParameter().

◆ setJointVelocity()

virtual void iTaSC::ConstraintSet::setJointVelocity ( const e_vector chidot_in )

inlinevirtual

Definition at line 107 of file ConstraintSet.hpp.

References e_vector, and m_chidot.

Referenced by iTaSC::Scene::update().

◆ setWy()

virtual void iTaSC::ConstraintSet::setWy ( const e_vector & Wy_in )

inlinevirtual

Definition at line 106 of file ConstraintSet.hpp.

References e_vector, and m_Wy.

◆ substep() [1/2]

bool iTaSC::ConstraintSet::substep ( )

inline

Definition at line 111 of file ConstraintSet.hpp.

References m_substep.

◆ substep() [2/2]

void iTaSC::ConstraintSet::substep ( bool _substep )

inline

Definition at line 110 of file ConstraintSet.hpp.

References m_substep.

Referenced by convert_tree(), and iTaSC::Scene::update().

◆ updateControlOutput()

virtual void iTaSC::ConstraintSet::updateControlOutput ( const Timestamp & timestamp )

protectedpure virtual

Implemented in iTaSC::CopyPose, and iTaSC::Distance.

Referenced by iTaSC::Scene::update().

◆ updateJacobian()

virtual void iTaSC::ConstraintSet::updateJacobian ( )

protectedpure virtual

Implemented in iTaSC::CopyPose, and iTaSC::Distance.

Referenced by closeLoop(), initialise(), and modelUpdate().

◆ updateKinematics()

virtual void iTaSC::ConstraintSet::updateKinematics ( const Timestamp & timestamp )

protectedpure virtual

Implemented in iTaSC::CopyPose, and iTaSC::Distance.

Referenced by iTaSC::Scene::update().

◆ Scene

friend class Scene

friend

Definition at line 69 of file ConstraintSet.hpp.

References Scene.

Referenced by Scene.

Member Data Documentation

◆ m_B

e_matrix6 iTaSC::ConstraintSet::m_B

protected

Definition at line 59 of file ConstraintSet.hpp.

Referenced by closeLoop(), ConstraintSet(), and reset().

◆ m_Cf

e_matrix iTaSC::ConstraintSet::m_Cf

protected

Definition at line 56 of file ConstraintSet.hpp.

Referenced by ConstraintSet(), iTaSC::CopyPose::CopyPose(), iTaSC::Distance::Distance(), getCf(), and reset().

◆ m_chi

e_vector6 iTaSC::ConstraintSet::m_chi

protected

Definition at line 58 of file ConstraintSet.hpp.

Referenced by closeLoop(), ConstraintSet(), iTaSC::Distance::getControlParameters(), modelUpdate(), reset(), iTaSC::Distance::updateControlOutput(), and iTaSC::Distance::updateJacobian().

◆ m_chidot

e_vector6 iTaSC::ConstraintSet::m_chidot

protected

Definition at line 58 of file ConstraintSet.hpp.

Referenced by getMaxTimestep(), iTaSC::CopyPose::getMaxTimestep(), modelUpdate(), reset(), and setJointVelocity().

◆ m_constraintCallback

ConstraintCallback iTaSC::ConstraintSet::m_constraintCallback

protected

Definition at line 61 of file ConstraintSet.hpp.

Referenced by ConstraintSet(), ConstraintSet(), registerCallback(), iTaSC::CopyPose::updateControlOutput(), and iTaSC::Distance::updateControlOutput().

◆ m_constraintParam

void* iTaSC::ConstraintSet::m_constraintParam

protected

Definition at line 62 of file ConstraintSet.hpp.

Referenced by ConstraintSet(), ConstraintSet(), registerCallback(), iTaSC::CopyPose::updateControlOutput(), and iTaSC::Distance::updateControlOutput().

◆ m_externalPose

KDL::Frame iTaSC::ConstraintSet::m_externalPose

protected

Definition at line 60 of file ConstraintSet.hpp.

Referenced by closeLoop(), iTaSC::Distance::closeLoop(), ConstraintSet(), ConstraintSet(), initialise(), iTaSC::CopyPose::initialise(), iTaSC::Distance::initialise(), modelUpdate(), and iTaSC::CopyPose::modelUpdate().

◆ m_internalPose

KDL::Frame iTaSC::ConstraintSet::m_internalPose

protected

Definition at line 60 of file ConstraintSet.hpp.

Referenced by closeLoop(), iTaSC::Distance::closeLoop(), ConstraintSet(), ConstraintSet(), iTaSC::CopyPose::getControlParameters(), getPose(), iTaSC::CopyPose::initialise(), iTaSC::CopyPose::modelUpdate(), iTaSC::CopyPose::updateControlOutput(), and iTaSC::Distance::updateJacobian().

◆ m_Jf

e_matrix6 iTaSC::ConstraintSet::m_Jf

protected

Definition at line 59 of file ConstraintSet.hpp.

Referenced by closeLoop(), ConstraintSet(), iTaSC::CopyPose::CopyPose(), getJf(), reset(), and iTaSC::Distance::updateJacobian().

◆ m_Jf_inv

e_matrix6 iTaSC::ConstraintSet::m_Jf_inv

protected

Definition at line 59 of file ConstraintSet.hpp.

Referenced by closeLoop(), ConstraintSet(), and reset().

◆ m_maxDeltaChi

e_scalar iTaSC::ConstraintSet::m_maxDeltaChi

protected

Definition at line 55 of file ConstraintSet.hpp.

Referenced by ConstraintSet(), ConstraintSet(), getMaxTimestep(), and iTaSC::CopyPose::getMaxTimestep().

◆ m_maxIter

unsigned int iTaSC::ConstraintSet::m_maxIter

protected

Definition at line 67 of file ConstraintSet.hpp.

Referenced by ConstraintSet(), ConstraintSet(), initialise(), and reset().

◆ m_nc

unsigned int iTaSC::ConstraintSet::m_nc

protected

Definition at line 54 of file ConstraintSet.hpp.

Referenced by ConstraintSet(), ConstraintSet(), iTaSC::CopyPose::CopyPose(), getNrOfConstraints(), and reset().

◆ m_poseParam

void* iTaSC::ConstraintSet::m_poseParam

protected

Definition at line 63 of file ConstraintSet.hpp.

◆ m_S

e_vector6 iTaSC::ConstraintSet::m_S

protected

Definition at line 58 of file ConstraintSet.hpp.

Referenced by closeLoop(), ConstraintSet(), and reset().

◆ m_substep

bool iTaSC::ConstraintSet::m_substep

protected

Definition at line 65 of file ConstraintSet.hpp.

Referenced by ConstraintSet(), ConstraintSet(), substep(), substep(), iTaSC::CopyPose::updateControlOutput(), and iTaSC::Distance::updateControlOutput().

◆ m_tdelta

e_vector6 iTaSC::ConstraintSet::m_tdelta

protected

Definition at line 58 of file ConstraintSet.hpp.

Referenced by closeLoop(), ConstraintSet(), and reset().

◆ m_temp

e_vector6 iTaSC::ConstraintSet::m_temp

protected

Definition at line 58 of file ConstraintSet.hpp.

Referenced by closeLoop(), ConstraintSet(), and reset().

◆ m_threshold

double iTaSC::ConstraintSet::m_threshold

protected

Definition at line 66 of file ConstraintSet.hpp.

Referenced by closeLoop(), iTaSC::Distance::closeLoop(), ConstraintSet(), ConstraintSet(), and reset().

◆ m_toggle

bool iTaSC::ConstraintSet::m_toggle

protected

Definition at line 64 of file ConstraintSet.hpp.

Referenced by ConstraintSet(), and ConstraintSet().

◆ m_U

e_matrix6 iTaSC::ConstraintSet::m_U

protected

Definition at line 59 of file ConstraintSet.hpp.

Referenced by closeLoop(), ConstraintSet(), and reset().

◆ m_V

e_matrix6 iTaSC::ConstraintSet::m_V

protected

Definition at line 59 of file ConstraintSet.hpp.

Referenced by closeLoop(), ConstraintSet(), and reset().

◆ m_Wy

e_vector iTaSC::ConstraintSet::m_Wy

protected

Definition at line 57 of file ConstraintSet.hpp.

Referenced by ConstraintSet(), iTaSC::CopyPose::CopyPose(), iTaSC::Distance::Distance(), getWy(), reset(), iTaSC::Distance::setControlParameters(), and setWy().

◆ m_y

e_vector iTaSC::ConstraintSet::m_y

protected

Definition at line 57 of file ConstraintSet.hpp.

Referenced by ConstraintSet(), and reset().

◆ m_ydot

e_vector iTaSC::ConstraintSet::m_ydot

protected

Definition at line 57 of file ConstraintSet.hpp.

Referenced by ConstraintSet(), getControlOutput(), iTaSC::Distance::getControlParameters(), reset(), and iTaSC::Distance::updateControlOutput().

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

Public Member Functions

Protected Member Functions

Protected Attributes

Friends

Detailed Description

Constructor & Destructor Documentation

◆ ConstraintSet() [1/2]

◆ ConstraintSet() [2/2]

◆ ~ConstraintSet()

Member Function Documentation

◆ closeLoop()

◆ getCf()

◆ getControlOutput()

◆ getControlParameters()

◆ getJf()

◆ getMaxTimestep()

◆ getNrOfConstraints()

◆ getPose()

◆ getWy()

◆ initCache()

◆ initialise()

◆ modelUpdate()

◆ pushCache()

◆ registerCallback()

◆ reset()

◆ setControlParameter()

◆ setControlParameters()

◆ setJointVelocity()

◆ setWy()

◆ substep() [1/2]

◆ substep() [2/2]

◆ updateControlOutput()

◆ updateJacobian()

◆ updateKinematics()

Friends And Related Symbol Documentation

◆ Scene

Member Data Documentation

◆ m_B

◆ m_Cf

◆ m_chi

◆ m_chidot

◆ m_constraintCallback

◆ m_constraintParam

◆ m_externalPose

◆ m_internalPose

◆ m_Jf

◆ m_Jf_inv

◆ m_maxDeltaChi

◆ m_maxIter

◆ m_nc

◆ m_poseParam

◆ m_S

◆ m_substep

◆ m_tdelta

◆ m_temp

◆ m_threshold

◆ m_toggle

◆ m_U

◆ m_V

◆ m_Wy

◆ m_y

◆ m_ydot