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

#include <ContactAvoidanceConstraint.h>

Inheritance diagram for ocra::ContactAvoidanceFunction:

Collaboration diagram for ocra::ContactAvoidanceFunction:

Public Types
typedef LinearFunction	functionType_t

Public Types inherited from ocra::LinearFunction
typedef Function	functionType_t

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

	~ContactAvoidanceFunction ()

double	getHorizonOfPrediction () const

void	setHorizonOfPrediction (double newHpos)

double	getMargin () const

void	setMargin (double newMargin)

void	updateContactInformation (const Eigen::MatrixXd &_JObst, const Eigen::VectorXd &_dJdqOst, const Eigen::VectorXd &_distObst, const Eigen::VectorXd &_velObst)

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	updateJacobian () const

void	updateb () const

virtual void	doUpdateDimensionBegin (int newDimension)

virtual void	doUpdateDimensionEnd (int oldDimension)

void	computeFullJacobian (const Eigen::MatrixXd &_JObst, Eigen::MatrixXd &fullJacobian) const

void	computeFullb (const Eigen::VectorXd &_dJdqObst, const Eigen::VectorXd &_distObst, const Eigen::VectorXd &_velObst, Eigen::VectorXd &fullb) const

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

virtual void	doUpdateInputSizeEnd ()

virtual void	updateValue () 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 ()

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
double	hpos

double	margin

Eigen::MatrixXd	JObst

Eigen::VectorXd	dJdqObst

Eigen::VectorXd	distObst

Eigen::VectorXd	velObst

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 contact avoidance constraint is written at first as: $\vec{0} < \vec{d}_{oa}$ where $\vec{d}_{oa}$ is the contatenation of the minimal distances between the couples of shapes that should not collide.

To relate this constraint with the dynamic variables, we constrain the estimated future distances to remain positive, as explained in wJointLimitFunction .

In this case, the constraint is expressed as follows:

$\begin{align*} \vec{d}_{oa} - \vec{m} &> \vec{d}_{oa}(h) \quad \text{at instant h} \\ \vec{d}_{oa} - \vec{m} &> \dot{\vec{d}}_{oa} h + \ddot{\vec{d}}_{oa} \frac{h^2}{2} \\ \vec{d}_{oa} - \vec{m} &> \dot{\vec{d}}_{oa} h + \left( \J_{oa} \ddq + \dJ_{oa} \dq \right) \frac{h^2}{2} \end{align*}$

where $\vec{m}$ is a margin vector. So it becomes:

$\begin{align*} \A \x + \b &> \vec{0} & &\Leftrightarrow & \begin{bmatrix} - \J_{oa} \end{bmatrix} \ddq + \begin{bmatrix} - \dJ_{oa} \dq + 2 \left( \vec{d}_{oa} - \vec{m} - \dot{\vec{d}}_{oa} h \right) / h^2 \end{bmatrix} & > \vec{0} \end{align*}$

There is some similarity with the wJointLimitFunction because we constrain an estimated future state. The same issues arise (it only constrain the final point, no middle points), so it is interesting to look at the inflexion point. In the same manner, the time of inflexion for a constant acceleration such as the inflexion point is on 0 is computed as follows:

$\begin{align*} t_{max} &= 2*(\vec{d}_{oa} - \vec{m}) / \dot{\vec{d}}_{oa} & & \text{(element-wise operations)} \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 contact avoidance:

$\begin{align*} \ddot{\vec{d}}_{max}[i] &= \frac{ \dot{\vec{d}}_{oa}[i]^2 }{ 2( \vec{d}_{oa}[i] - \vec{m}[i] ) } & & \Rightarrow & \begin{bmatrix} - \J_{oa} \end{bmatrix} [i] \ddq + \ddot{\vec{d}}_{max}[i] > 0 \end{align*}$

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

Definition at line 67 of file ContactAvoidanceConstraint.h.

Member Typedef Documentation

typedef LinearFunction ocra::ContactAvoidanceFunction::functionType_t

Definition at line 70 of file ContactAvoidanceConstraint.h.

Constructor & Destructor Documentation

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

Initialize the contact avoidance 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 contact avoidance 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 ]$ :

 - wFullContactAvoidanceFunction
 - wReducedContactAvoidanceFunction

Definition at line 30 of file ContactAvoidanceConstraint.cpp.

ContactAvoidanceFunction::~ContactAvoidanceFunction ( )

Destructor

Definition at line 46 of file ContactAvoidanceConstraint.cpp.

Member Function Documentation

void ContactAvoidanceFunction::computeFullb	(	const Eigen::VectorXd &	_dJdqObst,
		const Eigen::VectorXd &	_distObst,
		const Eigen::VectorXd &	_velObst,
		Eigen::VectorXd &	fullb
	)		const

protected

Compute the vector $\b$ of the linear function for the collision avoidance constraint expressed in the full formalism.

Parameters

_dJdqObst	The derivative of the Jacobian of obstacle avoidance multiplied by generalized velocity $= \dJ_{Obst} \dq$
_distObst	The relative distance of obstacle avoidance
_velObst	The relative velocity of obstacle avoidance
fullb	The vector instance where to write the collision avoidance data in the full formalism

Definition at line 121 of file ContactAvoidanceConstraint.cpp.

void ContactAvoidanceFunction::computeFullJacobian	(	const Eigen::MatrixXd &	_JObst,
		Eigen::MatrixXd &	fullJacobian
	)		const

protected

Compute the Jacobian matrix $\A$ of the linear function for the collision avoidance constraint expressed in the full formalism.

Parameters

_JObst	The Jacobian of obstacle avoidance
fullJacobian	The matrix instance where to write the collision avoidance data in the full formalism

Definition at line 109 of file ContactAvoidanceConstraint.cpp.

void ContactAvoidanceFunction::doUpdateDimensionBegin ( int newDimension )

protectedvirtual

Do when linear function dimension changes, before.

By overloading this function, it allows linear function modification when function size changes. It does nothing actually.

Reimplemented from ocra::Function.

Definition at line 167 of file ContactAvoidanceConstraint.cpp.

void ContactAvoidanceFunction::doUpdateDimensionEnd ( int oldDimension )

protectedvirtual

Do when linear function dimension changes, after.

By overloading this function, it allows linear function modification when function size changes. It does nothing actually.

Reimplemented from ocra::Function.

Definition at line 177 of file ContactAvoidanceConstraint.cpp.

double ContactAvoidanceFunction::getHorizonOfPrediction ( ) const

Get the time horizon of prediction $ h $ for the contact avoidance function.

Returns: The time horizon (s)

Definition at line 55 of file ContactAvoidanceConstraint.cpp.

double ContactAvoidanceFunction::getMargin ( ) const

Get the obstacle avoidance margin $\vec{m}$ .

Returns: The margin vector

Definition at line 73 of file ContactAvoidanceConstraint.cpp.

void ContactAvoidanceFunction::setHorizonOfPrediction ( double newHpos )

Set the time horizon of prediction $ h $ for the contact avoidance function.

Parameters

newHpos The new time horizon (s)

Definition at line 64 of file ContactAvoidanceConstraint.cpp.

void ContactAvoidanceFunction::setMargin ( double newMargin )

Set the obstacle avoidance margin $\vec{m}$ .

Parameters

newMargin The margin vector

Definition at line 82 of file ContactAvoidanceConstraint.cpp.

void ContactAvoidanceFunction::updateb ( ) const

protectedvirtual

update the vector $\b$ .

Does nothing.

Todo:: It should be conform to the ocra framework, the computation of the vector should be here.

Reimplemented from ocra::LinearFunction.

Reimplemented in ocra::ReducedContactAvoidanceFunction, and ocra::FullContactAvoidanceFunction.

Definition at line 157 of file ContactAvoidanceConstraint.cpp.

void ContactAvoidanceFunction::updateContactInformation	(	const Eigen::MatrixXd &	_JObst,
		const Eigen::VectorXd &	_dJdqObst,
		const Eigen::VectorXd &	_distObst,
		const Eigen::VectorXd &	_velObst
	)

Update contact information to compute obstacle avoidance function.

Parameters

_JObst	The Jacobian of obstacle avoidance
_dJdqObst	The derivative of the Jacobian of obstacle avoidance multiplied by generalized velocity $= \dJ_{Obst} \dq$
_distObst	The relative distance of obstacle avoidance
_velObst	The relative velocity of obstacle avoidance

Definition at line 94 of file ContactAvoidanceConstraint.cpp.

void ContactAvoidanceFunction::updateJacobian ( ) const

protectedvirtual

update the Jacobian matrix $\A$ .

Does nothing.