tasks::qp::DistanceConstr Class Reference

#include <Tasks/QPConstr.h>

Inheritance diagram for tasks::qp::DistanceConstr:

Collaboration diagram for tasks::qp::DistanceConstr:

Classes
struct	DistLimData

Public Member Functions
	DistanceConstr (const std::vector< rbd::MultiBody > &mbs, double step)
void	addDistanceLimit (const std::vector< rbd::MultiBody > &mbs, int dlId, int r1Index, const std::string &r1BodyName, sch::S_Object *body1, const sva::PTransformd &X_op1_o1, int r2Index, const std::string &r2BodyName, sch::S_Object *body2, const sva::PTransformd &X_op2_o2, double di, double ds, double damping, double dampingOff=0., const Eigen::VectorXd &r1Selector=Eigen::VectorXd::Zero(0), const Eigen::VectorXd &r2Selector=Eigen::VectorXd::Zero(0))
bool	rmDistanceLimit (int dlId)
std::size_t	nrDistanceLimits () const
void	reset ()
	Remove all distance constraints. More...
void	updateNrDistanceLimits ()
	Reallocate A and b matrix. More...
virtual void	updateNrVars (const std::vector< rbd::MultiBody > &mbs, const SolverData &data) override
virtual void	update (const std::vector< rbd::MultiBody > &mbs, const std::vector< rbd::MultiBodyConfig > &mbcs, const SolverData &data) override
virtual std::string	nameInEq () const override
virtual std::string	descInEq (const std::vector< rbd::MultiBody > &mbs, int line) override
virtual int	nrInEq () const override
virtual int	maxInEq () const override
virtual const Eigen::MatrixXd &	AInEq () const override
virtual const Eigen::VectorXd &	bInEq () const override
const DistLimData &	getDistanceData (int dlId) const
Public Member Functions inherited from tasks::qp::ConstraintFunction< Inequality >
virtual	~ConstraintFunction () override
void	addToSolver (QPSolver &sol)
void	addToSolver (const std::vector< rbd::MultiBody > &mbs, QPSolver &sol)
void	removeFromSolver (QPSolver &sol)
Public Member Functions inherited from tasks::qp::Constraint
virtual	~Constraint ()

Detailed Description

Set a limit for a distance between two robot links based on a velocity damper. For each distance limit pair if d \geq d_s :

\[ \dot{d} + \ddot{d}\Delta_{dt} \geq -\xi \frac{d - d_s}{d_i - d_s} \]

and for d \leq d_s (inverted into -d \geq \d_s (note that d_i also gets negated)):

\[ -\dot{d} - \ddot{d}\Delta_{dt} \geq -\xi \frac{d - d_s}{d_i - d_s} \]

with \( d \) the distance between the two links, \( d_i \) the interactive distance, \( d_s \) the security distance and \( \xi \) the damper.

The damper \( \xi \) can be calculated automatically each time the distance \( d \) go below the interactive distance \( d_i \) with the following formula:

\[ \xi = -\frac{d_i - d_s}{d - d_s}\alpha + \xi_{\text{off}} \]

Constructor & Destructor Documentation

DistanceConstr()

tasks::qp::DistanceConstr::DistanceConstr	(	const std::vector< rbd::MultiBody > &	mbs,
		double	step
	)

Parameters

mbs	Multi-robot system.
step	Time step in second.

Member Function Documentation

addDistanceLimit()

void tasks::qp::DistanceConstr::addDistanceLimit	(	const std::vector< rbd::MultiBody > &	mbs,
		int	dlId,
		int	r1Index,
		const std::string &	r1BodyName,
		sch::S_Object *	body1,
		const sva::PTransformd &	X_op1_o1,
		int	r2Index,
		const std::string &	r2BodyName,
		sch::S_Object *	body2,
		const sva::PTransformd &	X_op2_o2,
		double	di,
		double	ds,
		double	damping,
		double	dampingOff = 0.,
		const Eigen::VectorXd &	r1Selector = Eigen::VectorXd::Zero(0),
		const Eigen::VectorXd &	r2Selector = Eigen::VectorXd::Zero(0)
	)

Add a distance constraint. Don't forget to call updateNrDistanceLimits and QPSolver::updateConstrSize. You can also only call QPSolver::nrVars or QPSolver::updateConstrsNrVars or QPSolver::updateNrVars.

Parameters

mbs	Multi-robot system (must be the same given in the constructor.
dlId	Id of this distance limit, must be unique.
r1Index	First constrained robot Index in mbs.
r1BodyId	Constrained body id in mbs[r1Index].
body1	sch-core hull associated to the r1BodyId link.
X_op1_o1	body1 position will be set at each iteration to \( {}^{o1}X_{op1} {}^{r1BodyId}X_O \).
r2Index	Second constrained robot Index in mbs (can be equal to r1Index).
r2BodyId	Constrained body id in mbs[r2Index].
body2	sch-core hull associated to the r2BodyId link.
X_op2_o2	body2 position will be set at each iteration to \( {}^{o2}X_{op2} {}^{r2BodyId}X_O \).
di	\( d_i \).
ds	\( d_s \).
damping	\( \xi \), if set to 0 the damping is computed automatically.
dampingOff	\( \xi_{\text{off}} \).
r1Selector	A joint selection vector for r1Index the default selects all joints
r2Selector	A joint selection vector for r2Index the default selects all joints, ignored if r1Index == r2Index

AInEq()

virtual const Eigen::MatrixXd& tasks::qp::DistanceConstr::AInEq ( ) const

overridevirtual

bInEq()

virtual const Eigen::VectorXd& tasks::qp::DistanceConstr::bInEq ( ) const

overridevirtual

descInEq()

virtual std::string tasks::qp::DistanceConstr::descInEq ( const std::vector< rbd::MultiBody > &  mbs, int  line  )

overridevirtual

getDistanceData()

const DistLimData& tasks::qp::DistanceConstr::getDistanceData

(

int

dlId

)

const

Access the distance data computed by the constraint

maxInEq()

virtual int tasks::qp::DistanceConstr::maxInEq ( ) const

overridevirtual

nameInEq()

virtual std::string tasks::qp::DistanceConstr::nameInEq ( ) const

overridevirtual

nrDistanceLimits()

std::size_t tasks::qp::DistanceConstr::nrDistanceLimits

(

)

const

Returns

Number of distance constraint.

nrInEq()

virtual int tasks::qp::DistanceConstr::nrInEq ( ) const

overridevirtual

reset()

void tasks::qp::DistanceConstr::reset

(

)

Remove all distance constraints.

rmDistanceLimit()

bool tasks::qp::DistanceConstr::rmDistanceLimit

(

int

dlId

)

Remove a distance constraint.

Parameters

dlId	DistanceLimit id to remove.

Returns

true if the distance limit as been removed false if the distance limit id was associated with no distance limit.

update()

virtual void tasks::qp::DistanceConstr::update ( const std::vector< rbd::MultiBody > &  mbs, const std::vector< rbd::MultiBodyConfig > &  mbcs, const SolverData &  data  )

overridevirtual

Implements tasks::qp::Constraint.

updateNrDistanceLimits()

void tasks::qp::DistanceConstr::updateNrDistanceLimits

(

)

Reallocate A and b matrix.

updateNrVars()

virtual void tasks::qp::DistanceConstr::updateNrVars ( const std::vector< rbd::MultiBody > &  mbs, const SolverData &  data  )

overridevirtual

Implements tasks::qp::Constraint.

---

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

• src/Tasks/QPConstr.h