tasks::qp::CoMIncPlaneConstr Class Reference

#include <Tasks/QPConstr.h>

Inheritance diagram for tasks::qp::CoMIncPlaneConstr:

Collaboration diagram for tasks::qp::CoMIncPlaneConstr:

Public Member Functions
	CoMIncPlaneConstr (const std::vector< rbd::MultiBody > &mbs, int robotIndex, double step)
void	addPlane (int planeId, const Eigen::Vector3d &normal, double offset, double di, double ds, double damping, double dampingOff=0.)
void	addPlane (int planeId, const Eigen::Vector3d &normal, double offset, double di, double ds, double damping, const Eigen::Vector3d &speed, const Eigen::Vector3d &normalDot, double dampingOff=0.)
bool	rmPlane (int planeId)
std::size_t	nrPlanes () const
void	reset ()
	Remove all plane. More...
Eigen::VectorXd &	selector () noexcept
const Eigen::VectorXd &	selector () const noexcept
void	updateNrPlanes ()
	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
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

Prevent robot CoM to go out of a convex hull. For each plane that compose the convex hull:

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

with \( d \) the CoM and the plane, \( 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

CoMIncPlaneConstr()

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

Parameters

mbs	Multi-robot system.
robotIndex	Constrained robot Index in mbs.
step	Time step in second.

Member Function Documentation

addPlane() [1/2]

void tasks::qp::CoMIncPlaneConstr::addPlane	(	int	planeId,
		const Eigen::Vector3d &	normal,
		double	offset,
		double	di,
		double	ds,
		double	damping,
		const Eigen::Vector3d &	speed,
		const Eigen::Vector3d &	normalDot,
		double	dampingOff = 0.
	)

addPlane() [2/2]

void tasks::qp::CoMIncPlaneConstr::addPlane	(	int	planeId,
		const Eigen::Vector3d &	normal,
		double	offset,
		double	di,
		double	ds,
		double	damping,
		double	dampingOff = 0.
	)

Add a plane to the convex hull. Don't forget to call updateNrPlanes and QPSolver::updateConstrSize. You can also only call QPSolver::nrVars or QPSolver::updateConstrsNrVars or QPSolver::updateNrVars.

Parameters

planeId	Id of this plane, must be unique.
normal	Normal of the plane, must be unitary and in the allowed zone direction.
offset	Origin of the normal.
di	\( d_i \).
ds	\( d_s \).
damping	\( \xi \), if set to 0 the damping is computed automatically.
dampingOff	\( \xi_{\text{off}} \).

AInEq()

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

overridevirtual

bInEq()

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

overridevirtual

descInEq()

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

overridevirtual

maxInEq()

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

overridevirtual

nameInEq()

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

overridevirtual

nrInEq()

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

overridevirtual

nrPlanes()

std::size_t tasks::qp::CoMIncPlaneConstr::nrPlanes

(

)

const

Returns

Number of plane.

reset()

void tasks::qp::CoMIncPlaneConstr::reset

(

)

Remove all plane.

rmPlane()

bool tasks::qp::CoMIncPlaneConstr::rmPlane

(

int

planeId

)

Remove a plane.

Parameters

planeId

Plane id to remove.

Returns

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

selector() [1/2]

const Eigen::VectorXd& tasks::qp::CoMIncPlaneConstr::selector ( ) const

inlinenoexcept

Access the joint selector (const)

By default the vector selects all joints in the robot

selector() [2/2]

Eigen::VectorXd& tasks::qp::CoMIncPlaneConstr::selector ( )

inlinenoexcept

Access the joint selector

By default the vector selects all joints in the robot

update()

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

overridevirtual

Implements tasks::qp::Constraint.

updateNrPlanes()

void tasks::qp::CoMIncPlaneConstr::updateNrPlanes

(

)

Reallocate A and b matrix.

updateNrVars()

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