Create a linear function that represents the joint limit function. More...

#include <JointLimitConstraint.h>

Inheritance diagram for ocra::JointLimitFunction:

Collaboration diagram for ocra::JointLimitFunction:

Public Types
typedef LinearFunction	functionType_t

Public Types inherited from ocra::LinearFunction
typedef Function	functionType_t

Public Member Functions
	JointLimitFunction (const Model &m, Variable &var)

	~JointLimitFunction ()

double	getHorizonOfPrediction () const

void	setHorizonOfPrediction (double newHpos)

void	setJointLimits (const Eigen::VectorXd &lowerLimits, const Eigen::VectorXd &upperLimits)

void	setJointLowerLimits (const Eigen::VectorXd &newLowerLimits)

void	setJointUpperLimits (const Eigen::VectorXd &newUpperLimits)

const Eigen::VectorXd &	getJointLowerLimits () const

const Eigen::VectorXd &	getJointUpperLimits () const

void	setJointLimit (int i, double newLowerLimit, double newUpperLimit)

void	setJointLowerLimit (int i, double newLowerLimit)

void	setJointUpperLimit (int i, double newUpperLimit)

double	getJointLowerLimit (int i) const

double	getJointUpperLimit (int i) const

Public Member Functions inherited from ocra::LinearFunction
template<class Derived , class VectorBase >
	LinearFunction (Variable &x, const MatrixBase< Derived > &A, const VectorBase &b)

	~LinearFunction ()

void	invalidateb (int timestamp)

const MatrixXd &	getA () const

const VectorXd &	getb () const

void	changeA (const MatrixXd &A)

void	changeb (const VectorXd &b)

Public Member Functions inherited from ocra::Function
virtual	~Function ()

int	getDimension () const

virtual void	updateFdot () const

virtual void	updateFddot () const

virtual void	updateJdotXdot () const

const Variable &	getVariable () const

Variable &	getVariable ()

template<eFunctionAbility Ability>
void	invalidate ()

void	invalidateAll (int timestamp)

void	invalidateAll ()

template<eFunctionAbility Ability>
bool	isValid () const

template<eFunctionAbility Ability>
bool	canCompute () const

template<eFunctionAbility Ability>
const IFunction< Ability >::return_type &	get () const

template<eFunctionAbility Ability>
IFunction< Ability >::return_sub_type	get (int index) const

const VectorXd &	getValue () const

double	getValue (int index) const

const MatrixXd &	getJacobian () const

MatrixXdRow	getJacobian (int index) const

Public Member Functions inherited from ocra::ObserverSubject
	ObserverSubject ()

virtual	~ObserverSubject ()

Public Member Functions inherited from ocra::Observer
virtual	~Observer ()

template<int EVT>
void	bind (SubjectBase< EVT > &subject)
	Call this method to automatically propagate observed events to observers connected to the subject given in argument. More...

template<int EVT>
void	stopPropagation ()

Public Member Functions inherited from ocra::ObserverBase< EVT_RESIZE >
void	bind (subject_type &subject)
	Call this method to automatically propagate observed events to observers connected to the subject given in argument. More...

Public Member Functions inherited from ocra::ObserverBase< EVT_CHANGE_DEPENDENCIES >
void	bind (subject_type &subject)
	Call this method to automatically propagate observed events to observers connected to the subject given in argument. More...

Public Member Functions inherited from ocra::ObserverBase< EVT_CHANGE_VALUE >
void	bind (subject_type &subject)
	Call this method to automatically propagate observed events to observers connected to the subject given in argument. More...

Public Member Functions inherited from ocra::Subject
virtual	~Subject ()

template<int EVT, class Derived , class Base >
void	connect (Derived &object, void(Base::*newCallback)(int)) const
	Call this method to register a non-static method as a callback. More...

template<int EVT>
void	connect (void(*newCallback)(int)) const
	Call this method to register a free function as a callback. More...

template<int EVT, class Derived , class Base >
void	disconnect (Derived &object, void(Base::*callbackToErase)(int)) const
	Disconnect non-static method. More...

template<int EVT>
void	disconnect (void(*callbackToErase)(int)) const
	Disconnect free function. More...

template<int EVT>
void	propagate () const

template<int EVT>
void	propagate (int timestamp) const

Public Member Functions inherited from ocra::SubjectBase< EVT_RESIZE >
void	connect (T &object, typename SubjectBaseTraits< EVT, T >::callback_type newCallback) const
	Call this method to register a non-static method as a callback. More...

void	connect (typename SubjectBaseTraits< EVT, void >::callback_type newCallback) const
	Call this method to register a free function as a callback. More...

void	disconnect (T &object, typename SubjectBaseTraits< EVT, T >::callback_type callback) const
	Disconnect non-static method. More...

void	disconnect (typename SubjectBaseTraits< EVT, void >::callback_type callbackToErase) const
	Disconnect free function. More...

void	propagate (int timestamp) const

void	propagate () const

Public Member Functions inherited from ocra::SubjectBase< EVT_CHANGE_DEPENDENCIES >
void	connect (T &object, typename SubjectBaseTraits< EVT, T >::callback_type newCallback) const
	Call this method to register a non-static method as a callback. More...

void	connect (typename SubjectBaseTraits< EVT, void >::callback_type newCallback) const
	Call this method to register a free function as a callback. More...

void	disconnect (T &object, typename SubjectBaseTraits< EVT, T >::callback_type callback) const
	Disconnect non-static method. More...

void	disconnect (typename SubjectBaseTraits< EVT, void >::callback_type callbackToErase) const
	Disconnect free function. More...

void	propagate (int timestamp) const

void	propagate () const

Public Member Functions inherited from ocra::SubjectBase< EVT_CHANGE_VALUE >
void	connect (T &object, typename SubjectBaseTraits< EVT, T >::callback_type newCallback) const
	Call this method to register a non-static method as a callback. More...

void	connect (typename SubjectBaseTraits< EVT, void >::callback_type newCallback) const
	Call this method to register a free function as a callback. More...

void	disconnect (T &object, typename SubjectBaseTraits< EVT, T >::callback_type callback) const
	Disconnect non-static method. More...

void	disconnect (typename SubjectBaseTraits< EVT, void >::callback_type callbackToErase) const
	Disconnect free function. More...

void	propagate (int timestamp) const

void	propagate () const

Public Member Functions inherited from ocra::IFunctionProperties
bool	hasSeparableTimeDependancy (void) const

eFunctionLinearity	getType (void) const

eFunctionConvexity	getConvexityProperty (void) const

int	getContinuityProperty (void) const

const std::string &	getProperty (int i) const

int	getNumberOfProperties (void) const

bool	hasProperty (const std::string &functionProperty) const

bool	isExplicitlyTimeDependant (void) const

Public Member Functions inherited from ocra::NamedInstance
	NamedInstance (const std::string &name)

const std::string &	getName () const

virtual	~NamedInstance ()

Public Member Functions inherited from ocra::AbilitySet
AbilitySet &	add (eFunctionAbility prop)

AbilitySet &	remove (eFunctionAbility prop)

Protected Member Functions
void	computeFullJacobian (int varDofs, int nIDofs, Eigen::MatrixXd &fullJacobian) const

void	computeFullb (const Eigen::VectorXd &gpos, const Eigen::VectorXd &gvel, Eigen::VectorXd &fullb) const

Protected Member Functions inherited from ocra::LinearFunction
	LinearFunction (Variable &x, int dimension)

virtual void	updateb () const

virtual void	doUpdateInputSizeEnd ()

virtual void	updateValue () const

virtual void	updateJacobian () const

virtual void	doChangeA (const MatrixXd &A)

virtual void	doChangeb (const VectorXd &b)

void	inhibitPropagationFromb () const

void	desinhibitPropagationFromb () const

Protected Member Functions inherited from ocra::Function
	Function (Variable &x, int dimension, eFunctionLinearity linearity=LINEARITY_UNDEFINED, eFunctionConvexity convexity=CONVEXITY_UNDEFINED, int continuity=CONTINUITY_UNKNOWN, bool explicitlyTimeDependant=false, bool separableTimeDependancy=true)
	Function Constructor. More...

void	disconnectVariable ()

void	changeFunctionDimension (int newDimension)

void	updateInputSize (int timestamp)

void	resize ()

virtual int	computeDimensionFromInputSize () const

virtual void	doUpdateInputSizeBegin ()

virtual void	doUpdateDimensionBegin (int newDimension)

virtual void	doUpdateDimensionEnd (int oldDimension)

Protected Member Functions inherited from ocra::ObserverBase< EVT_RESIZE >
void	stopPropagation ()
	Call this method from your callbacks to avoid propagation to the bound subject (if any). More...

	ObserverBase ()

	~ObserverBase ()

Protected Member Functions inherited from ocra::ObserverBase< EVT_CHANGE_DEPENDENCIES >
void	stopPropagation ()
	Call this method from your callbacks to avoid propagation to the bound subject (if any). More...

	ObserverBase ()

	~ObserverBase ()

Protected Member Functions inherited from ocra::ObserverBase< EVT_CHANGE_VALUE >
void	stopPropagation ()
	Call this method from your callbacks to avoid propagation to the bound subject (if any). More...

	ObserverBase ()

	~ObserverBase ()

Protected Member Functions inherited from ocra::SubjectBase< EVT_RESIZE >
	SubjectBase ()

	~SubjectBase ()

Protected Member Functions inherited from ocra::SubjectBase< EVT_CHANGE_DEPENDENCIES >
	SubjectBase ()

	~SubjectBase ()

Protected Member Functions inherited from ocra::SubjectBase< EVT_CHANGE_VALUE >
	SubjectBase ()

	~SubjectBase ()

Protected Member Functions inherited from ocra::IFunctionProperties
	IFunctionProperties (eFunctionLinearity linearity=LINEARITY_UNDEFINED, eFunctionConvexity convexity=CONVEXITY_UNDEFINED, int continuity=CONTINUITY_UNKNOWN, bool explicitlyTimeDependant=false, bool separableTimeDependancy=true)
	IFunctionProperties Constructor. More...

void	changeType (eFunctionLinearity newType)

void	changeConvexityProperty (eFunctionConvexity newProperty)

void	changeContinuityProperty (int newProperty)

void	addProperty (const std::string &functionProperty)

void	removeProperty (const std::string &functionProperty)

void	changeExplicitTimeDependancy (bool b)

void	changeSeparableTimeDependancy (bool b)

Protected Member Functions inherited from ocra::AbilitySet
	AbilitySet (const std::vector< bool > &usageSet)

const std::vector< bool > &	getUsageSet () const

	AbilitySet (eFunctionAbility prop0=FUN_VALUE, eFunctionAbility prop1=FUN_VALUE)

	AbilitySet (eFunctionAbility prop0, eFunctionAbility prop1, eFunctionAbility prop2, eFunctionAbility prop3=FUN_VALUE, eFunctionAbility prop4=FUN_VALUE)

	AbilitySet (eFunctionAbility prop0, eFunctionAbility prop1, eFunctionAbility prop2, eFunctionAbility prop3, eFunctionAbility prop4, eFunctionAbility prop5, eFunctionAbility prop6=FUN_VALUE, eFunctionAbility prop7=FUN_VALUE, eFunctionAbility prop8=FUN_VALUE, eFunctionAbility prop9=FUN_VALUE)

Protected Member Functions inherited from ocra::CoupledInputOutputSize
	CoupledInputOutputSize (bool coupledInputOutputSize)

bool	inputAndOutputSizesAreCoupled () const

Protected Attributes
Eigen::VectorXd	jointLowerLimits

Eigen::VectorXd	jointUpperLimits

double	hpos

Protected Attributes inherited from ocra::LinearFunction
VectorXd	_b

bool	_bIsUpToDate

Protected Attributes inherited from ocra::Function
Variable &	x

VectorXd &	_value

MatrixXd &	_jacobian

const int &	_dim

Additional Inherited Members
Protected Types inherited from ocra::ObserverBase< EVT_RESIZE >
typedef SubjectBase< EVT >	subject_type

typedef InvokerBase< EVT >	invoker_type

Protected Types inherited from ocra::ObserverBase< EVT_CHANGE_DEPENDENCIES >
typedef SubjectBase< EVT >	subject_type

typedef InvokerBase< EVT >	invoker_type

Protected Types inherited from ocra::ObserverBase< EVT_CHANGE_VALUE >
typedef SubjectBase< EVT >	subject_type

typedef InvokerBase< EVT >	invoker_type

Detailed Description

Create a linear function that represents the joint limit function.

The joint limit function $\q_{min} < \q < \q{max}$ defined in this controller is composed of a couple of inequalities:

 - the first one is computed on a horizon h. The system brakes far from the limit, but can pass inside the constraint when close to it (because it only constrains the final point, not the middle ones).
 - the second one is computed with the time of inflexion. The system brakes when close to the limits, but some singularities appears when it is on the limits.

The two computation have their drawbacks, but together they allow limits avoidance properly.

Joint limit constraint if inflexion point is not taken into account

The first one which constrains the last point over the horizon h can be expressed as follows (depending on $\ddq$ ):

$\begin{align*} \q_{min} < \q + \dq h + \ddq \frac{h^2}{2} < \q_{max} \end{align*}$

$\begin{align*} A \x + \b &> \vec{0} & &\Leftrightarrow & \begin{bmatrix} - \Id{} \\ \Id{} \end{bmatrix} \ddq + 2 \begin{bmatrix} \q_{max} - \q + \dq h \\ \q_{min} - \q + \dq h \end{bmatrix} / h^2 &> \vec{0} \end{align*}$

The second one is more "tricky". First it computes the times of inflexion for a constant acceleration such as the inflexion point is on 0:

$\begin{align*} t_{max} &= - 2 ( \q - \q_{max} ) / \dq & &\text{(element-wise operations)} \\ t_{min} &= - 2 ( \q - \q_{min} ) / \dq \end{align*}$

For each dof (each line $ i $ ), we test where is the time of inflexion. If $0 < t_{max}[i] < h$ , then the inflexion point is in the horizon of time, we should consider to constrain the motion of this dof:

$\begin{align*} \ddq_{max}[i] &= \frac{ \dq[i]^2 }{ 2(\q[i] - \q_{max}[i] ) } & & \Rightarrow & \ddq [i] + \ddq_{max}[i] > 0 \end{align*}$

Again, if $0 < t_{min}[i] < h$ :

$\begin{align*} \ddq_{min}[i] &= \frac{ \dq[i]^2 }{ 2(\q[i] - \q_{min}[i] ) } & & \Rightarrow & \ddq [i] + \ddq_{min}[i] > 0 \end{align*}$

When all these constraints have been defined, we select the tightest ones for each dof.

Explaination of the inflexion point in the joint limit constraint

Definition at line 72 of file JointLimitConstraint.h.

Member Typedef Documentation

typedef LinearFunction ocra::JointLimitFunction::functionType_t

Definition at line 75 of file JointLimitConstraint.h.

Constructor & Destructor Documentation

JointLimitFunction::JointLimitFunction	(	const Model &	model,
		Variable &	var
	)

Initialize the joint limit constraint function.

Parameters

model	The Model on which we will get the dynamic parameters
var	The problem variable that will be used to write this constraint

This class is a generic class to compute matrices for the joint limit function, but it should not be used. You would rather choose one of the following derivative classes, depending on the choosen formalism, either full $\x = [ \ddq \; \torque \; \force_c ]$ or reduced $\x = [ \torque \; \force_c ]$ :

 - wFullJointLimitFunction
 - wReducedJointLimitFunction

The function is on the form: | O -I | | ddqmax | | O I | ddq + | ddqmin |

where O (nx6) is defined if the model has a free-floating base (we cannot limit the range of this "joint"), else O is a void matrix (nx0)

The associated constraint is: A ddq + b >= 0

Definition at line 38 of file JointLimitConstraint.cpp.

JointLimitFunction::~JointLimitFunction ( )

Destructor

Definition at line 52 of file JointLimitConstraint.cpp.

Member Function Documentation

void JointLimitFunction::computeFullb	(	const Eigen::VectorXd &	gpos,
		const Eigen::VectorXd &	gvel,
		Eigen::VectorXd &	fullb
	)		const

protected

Compute the vector $\b$ for the joint limits constraint.

Parameters

gpos	The genralized position
gvel	The generalized velocity
fullb	The vector instance where to write the limits vector in the full formalism

This function compute vector $\b$ for the full formalism. When using the reduced one, this vector is first computed, and then transformed (see sec_transform_formalism) to fit the new variable.

The computation of $\b$ is explained in wJointLimitFunction

Definition at line 205 of file JointLimitConstraint.cpp.

void JointLimitFunction::computeFullJacobian	(	int	varDofs,
		int	nIDofs,
		Eigen::MatrixXd &	fullJacobian
	)		const

protected

Compute the Jacobian matrix for the joint limits constraint.

Parameters

varDofs	The variable dimension
nIDofs	Number of internal dof of the robot
fullJacobian	The jacobian matrix instance where to write the jacobian in the full formalism

This function compute matrix $\A$ for the full formalism. When using the reduced one, this matrix is first computed, and then transformed (see sec_transform_formalism) to fit the new variable.

The computation of $\A$ is explained in wJointLimitFunction

Definition at line 185 of file JointLimitConstraint.cpp.

double JointLimitFunction::getHorizonOfPrediction ( ) const

Get the time horizon of prediction $ h $ for the joint limit function.

Returns: The time horizon (s)

Definition at line 61 of file JointLimitConstraint.cpp.

double JointLimitFunction::getJointLowerLimit ( int i ) const

Get the joint limit for one dof $\q_{min}[i]$ .

Parameters

i	The dof index

Returns: The lower bound

Definition at line 159 of file JointLimitConstraint.cpp.

const Eigen::VectorXd & JointLimitFunction::getJointLowerLimits ( ) const

Get the joint limit $\q_{min}$ .

Returns: The lower bound

Definition at line 140 of file JointLimitConstraint.cpp.

double JointLimitFunction::getJointUpperLimit ( int i ) const

Get the joint limit for one dof $\q_{max}[i]$ .

Parameters

i	The dof index

Returns: The upper bound

Definition at line 169 of file JointLimitConstraint.cpp.

const Eigen::VectorXd & JointLimitFunction::getJointUpperLimits ( ) const

Get the joint limit $\q_{max}$ .

Returns: The upper bound

Definition at line 149 of file JointLimitConstraint.cpp.

void JointLimitFunction::setHorizonOfPrediction ( double newHpos )

Set the time horizon of prediction $ h $ for the joint limit function.

Parameters

newHpos The new time horizon (s)

Definition at line 70 of file JointLimitConstraint.cpp.

void JointLimitFunction::setJointLimit	(	int	i,
		double	newLowerLimit,
		double	newUpperLimit
	)

Set the joint limits for one dof $\q_{min}[i], \q_{max}[i]$ .

Parameters

i	The dof index whose bounds are modified
newLowerLimit	The lower bound
newUpperLimit	The upper bound

Definition at line 110 of file JointLimitConstraint.cpp.

void JointLimitFunction::setJointLimits	(	const Eigen::VectorXd &	lowerLimits,
		const Eigen::VectorXd &	upperLimits
	)

Set the joint limits $\q_{min}, \q_{max}$ .

Parameters

lowerLimits	The lower bound
upperLimits	The upper bound

Definition at line 80 of file JointLimitConstraint.cpp.

void JointLimitFunction::setJointLowerLimit	(	int	i,
		double	newLowerLimit
	)

Set the joint limit for one dof $\q_{min}[i]$ .

Parameters

i	The dof index whose bound is modified
newLowerLimit	The lower bound

Definition at line 121 of file JointLimitConstraint.cpp.

void JointLimitFunction::setJointLowerLimits ( const Eigen::VectorXd & newLowerLimits )

Set the joint limit $\q_{min}$ .

Parameters

newLowerLimits The lower bound

Definition at line 90 of file JointLimitConstraint.cpp.

void JointLimitFunction::setJointUpperLimit	(	int	i,
		double	newUpperLimit
	)

Set the joint limit for one dof $\q_{max}[i]$ .

Parameters

i	The dof index whose bound is modified
newUpperLimit	The upper bound

Definition at line 131 of file JointLimitConstraint.cpp.

void JointLimitFunction::setJointUpperLimits ( const Eigen::VectorXd & newUpperLimits )

Set the joint limits $\q_{max}$ .

Parameters

newUpperLimits The upper bound

Definition at line 99 of file JointLimitConstraint.cpp.

Member Data Documentation

double ocra::JointLimitFunction::hpos

protected

Definition at line 105 of file JointLimitConstraint.h.

Eigen::VectorXd ocra::JointLimitFunction::jointLowerLimits

protected

Definition at line 103 of file JointLimitConstraint.h.

Eigen::VectorXd ocra::JointLimitFunction::jointUpperLimits

protected

Definition at line 104 of file JointLimitConstraint.h.

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

Public Types

Public Member Functions

Protected Member Functions

Protected Attributes

Additional Inherited Members

Detailed Description

Member Typedef Documentation

Constructor & Destructor Documentation

Member Function Documentation

Member Data Documentation