Exotica
Public Member Functions | Private Member Functions | Private Attributes | List of all members
exotica::AvoidLookAtSphere Class Reference

Avoids pointing end-effector at a given spherical object. More...

#include <avoid_look_at_sphere.h>

Inheritance diagram for exotica::AvoidLookAtSphere:
Inheritance graph
Collaboration diagram for exotica::AvoidLookAtSphere:
Collaboration graph

Public Member Functions

void Instantiate (const AvoidLookAtSphereInitializer &init) override
 
void Update (Eigen::VectorXdRefConst x, Eigen::VectorXdRef phi) override
 
void Update (Eigen::VectorXdRefConst x, Eigen::VectorXdRef phi, Eigen::MatrixXdRef jacobian) override
 
int TaskSpaceDim () override
 
- Public Member Functions inherited from exotica::TaskMap
virtual void InstantiateBase (const Initializer &init)
 
virtual void AssignScene (ScenePtr scene)
 
virtual void Update (Eigen::VectorXdRefConst q, Eigen::VectorXdRef phi, Eigen::MatrixXdRef jacobian, HessianRef hessian)
 
virtual void Update (Eigen::VectorXdRefConst x, Eigen::VectorXdRefConst u, Eigen::VectorXdRef phi)
 
virtual void Update (Eigen::VectorXdRefConst x, Eigen::VectorXdRefConst u, Eigen::VectorXdRef phi, Eigen::MatrixXdRef dphi_dx, Eigen::MatrixXdRef dphi_du)
 
virtual void Update (Eigen::VectorXdRefConst x, Eigen::VectorXdRefConst u, Eigen::VectorXdRef phi, Eigen::MatrixXdRef dphi_dx, Eigen::MatrixXdRef dphi_du, HessianRef ddphi_ddx, HessianRef ddphi_ddu, HessianRef ddphi_dxdu)
 
virtual int TaskSpaceJacobianDim ()
 
virtual void PreUpdate ()
 
virtual std::vector< TaskVectorEntryGetLieGroupIndices ()
 
std::vector< KinematicFrameRequestGetFrames () const
 
- Public Member Functions inherited from exotica::Object
 Object ()
 
virtual ~Object ()
 
virtual std::string type () const
 Type Information wrapper: must be virtual so that it is polymorphic... More...
 
std::string GetObjectName ()
 
void InstantiateObject (const Initializer &init)
 
virtual std::string Print (const std::string &prepend) const
 
- Public Member Functions inherited from exotica::InstantiableBase
 InstantiableBase ()=default
 
virtual ~InstantiableBase ()=default
 
virtual std::vector< InitializerGetAllTemplates () const =0
 
- Public Member Functions inherited from exotica::Instantiable< AvoidLookAtSphereInitializer >
void InstantiateInternal (const Initializer &init) override
 
Initializer GetInitializerTemplate () override
 
std::vector< InitializerGetAllTemplates () const override
 
const AvoidLookAtSphereInitializer & GetParameters () const
 

Private Member Functions

void UpdateAsCostWithoutJacobian (Eigen::VectorXdRefConst x, Eigen::VectorXdRef phi)
 
void UpdateAsInequalityConstraintWithoutJacobian (Eigen::VectorXdRefConst x, Eigen::VectorXdRef phi)
 
void UpdateAsCostWithJacobian (Eigen::VectorXdRefConst x, Eigen::VectorXdRef phi, Eigen::MatrixXdRef jacobian)
 
void UpdateAsInequalityConstraintWithJacobian (Eigen::VectorXdRefConst x, Eigen::VectorXdRef phi, Eigen::MatrixXdRef jacobian)
 
void PublishObjectsAsMarkerArray ()
 

Private Attributes

void(AvoidLookAtSphere::* UpdateTaskMapWithoutJacobian )(Eigen::VectorXdRefConst, Eigen::VectorXdRef)
 
void(AvoidLookAtSphere::* UpdateTaskMapWithJacobian )(Eigen::VectorXdRefConst, Eigen::VectorXdRef, Eigen::MatrixXdRef)
 
int n_objects_
 Number of spherical objects. More...
 
Eigen::VectorXd diameter_
 The diameter of each object. More...
 
Eigen::VectorXd radii_squared_
 The square of each object radii. More...
 
ros::Publisher pub_markers_
 publish marker for RViz More...
 

Additional Inherited Members

- Public Attributes inherited from exotica::TaskMap
std::vector< KinematicSolutionkinematics = std::vector<KinematicSolution>(1)
 
int id = -1
 
int start = -1
 
int length = -1
 
int start_jacobian = -1
 
int length_jacobian = -1
 
bool is_used = false
 
- Public Attributes inherited from exotica::Object
std::string ns_
 
std::string object_name_
 
bool debug_
 
- Protected Attributes inherited from exotica::TaskMap
std::vector< KinematicFrameRequestframes_
 
ScenePtr scene_ = nullptr
 
- Protected Attributes inherited from exotica::Instantiable< AvoidLookAtSphereInitializer >
AvoidLookAtSphereInitializer parameters_
 

Detailed Description

Avoids pointing end-effector at a given spherical object.

This task map avoids pointing the end-effector a number of given spherical objects. AvoidLookAtSphere can be used as either a cost term or an inequality constraint. Each spherical object in the scene is defined by a position $o\in\mathbb{R}^3$ and radius $0<r\in\mathbb{R}$. The position and radii of each object must be given.

When using AvoidLookAtSphere as a cost term, the forward mapping defining the task map is expressed by

\[ \Phi_i(x) := \begin{cases}(1 - \frac{d_i^2}{r_i^2})^2 & d_i<r_i\\0 & \text{otherwise}\end{cases} \]

for all $i=1{:}N$ where $N$ is the number of objects, $d$ is the orthogonal distance between the object center and the end-effector approach vector.

When using AvoidLookAtSphere as an inequality constraint, the forward mapping defining the task map is expressed by

\[ \Phi_i(x) = r_i^2 - d_i^2 \]

for all $i=1{:}N$.

AvoidLookAtSphere task map.

Member Function Documentation

◆ Instantiate()

void exotica::AvoidLookAtSphere::Instantiate ( const AvoidLookAtSphereInitializer &  init)
overridevirtual

◆ PublishObjectsAsMarkerArray()

void exotica::AvoidLookAtSphere::PublishObjectsAsMarkerArray ( )
private

◆ TaskSpaceDim()

int exotica::AvoidLookAtSphere::TaskSpaceDim ( )
overridevirtual

Implements exotica::TaskMap.

◆ Update() [1/2]

void exotica::AvoidLookAtSphere::Update ( Eigen::VectorXdRefConst  x,
Eigen::VectorXdRef  phi 
)
overridevirtual

Implements exotica::TaskMap.

◆ Update() [2/2]

void exotica::AvoidLookAtSphere::Update ( Eigen::VectorXdRefConst  x,
Eigen::VectorXdRef  phi,
Eigen::MatrixXdRef  jacobian 
)
overridevirtual

Reimplemented from exotica::TaskMap.

◆ UpdateAsCostWithJacobian()

void exotica::AvoidLookAtSphere::UpdateAsCostWithJacobian ( Eigen::VectorXdRefConst  x,
Eigen::VectorXdRef  phi,
Eigen::MatrixXdRef  jacobian 
)
private

◆ UpdateAsCostWithoutJacobian()

void exotica::AvoidLookAtSphere::UpdateAsCostWithoutJacobian ( Eigen::VectorXdRefConst  x,
Eigen::VectorXdRef  phi 
)
private

◆ UpdateAsInequalityConstraintWithJacobian()

void exotica::AvoidLookAtSphere::UpdateAsInequalityConstraintWithJacobian ( Eigen::VectorXdRefConst  x,
Eigen::VectorXdRef  phi,
Eigen::MatrixXdRef  jacobian 
)
private

◆ UpdateAsInequalityConstraintWithoutJacobian()

void exotica::AvoidLookAtSphere::UpdateAsInequalityConstraintWithoutJacobian ( Eigen::VectorXdRefConst  x,
Eigen::VectorXdRef  phi 
)
private

Member Data Documentation

◆ diameter_

Eigen::VectorXd exotica::AvoidLookAtSphere::diameter_
private

The diameter of each object.

◆ n_objects_

int exotica::AvoidLookAtSphere::n_objects_
private

Number of spherical objects.

◆ pub_markers_

ros::Publisher exotica::AvoidLookAtSphere::pub_markers_
private

publish marker for RViz

◆ radii_squared_

Eigen::VectorXd exotica::AvoidLookAtSphere::radii_squared_
private

The square of each object radii.

◆ UpdateTaskMapWithJacobian

void(AvoidLookAtSphere::* exotica::AvoidLookAtSphere::UpdateTaskMapWithJacobian) (Eigen::VectorXdRefConst, Eigen::VectorXdRef, Eigen::MatrixXdRef)
private

◆ UpdateTaskMapWithoutJacobian

void(AvoidLookAtSphere::* exotica::AvoidLookAtSphere::UpdateTaskMapWithoutJacobian) (Eigen::VectorXdRefConst, Eigen::VectorXdRef)
private

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