tasks::qp::CollisionConstr Class Reference

#include <Tasks/QPConstr.h>

Inheritance diagram for tasks::qp::CollisionConstr:

Collaboration diagram for tasks::qp::CollisionConstr:

Public Member Functions
	CollisionConstr (const std::vector< rbd::MultiBody > &mbs, double step)
void	addCollision (const std::vector< rbd::MultiBody > &mbs, int collId, 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	rmCollision (int collId)
std::size_t	nrCollisions () const
void	reset ()
	Remove all collision constraints. More...
void	updateNrCollisions ()
	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 CollData &	getCollisionData (int collId) 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

Avoid that two robot links enter into collision based on a velocity damper. For each collision pair:

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

with \( d \) the minimal 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

CollisionConstr()

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

Parameters

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

Member Function Documentation

addCollision()

void tasks::qp::CollisionConstr::addCollision	(	const std::vector< rbd::MultiBody > &	mbs,
		int	collId,
		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 collision avoidance constraint. Don't forget to call updateNrCollisions 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.
collId	Id of this collision, 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::CollisionConstr::AInEq ( ) const

overridevirtual

bInEq()

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

overridevirtual

descInEq()

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

overridevirtual

getCollisionData()

const CollData& tasks::qp::CollisionConstr::getCollisionData

(

int

collId

)

const

Access the collision data computed by the constraint

maxInEq()

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

overridevirtual

nameInEq()

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

overridevirtual

nrCollisions()

std::size_t tasks::qp::CollisionConstr::nrCollisions

(

)

const

Returns

Number of collision constraint.

nrInEq()

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

overridevirtual

reset()

void tasks::qp::CollisionConstr::reset

(

)

Remove all collision constraints.

rmCollision()

bool tasks::qp::CollisionConstr::rmCollision

(

int

collId

)

Remove a collision avoidance constraint.

Parameters

collId

Collision id to remove.

Returns

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

update()

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

overridevirtual

Implements tasks::qp::Constraint.

updateNrCollisions()

void tasks::qp::CollisionConstr::updateNrCollisions

(

)

Reallocate A and b matrix.

updateNrVars()

virtual void tasks::qp::CollisionConstr::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