30 #ifndef EXOTICA_CORE_TASK_MAPS_AVOID_LOOK_AT_SPHERE_H_ 31 #define EXOTICA_CORE_TASK_MAPS_AVOID_LOOK_AT_SPHERE_H_ 35 #include <exotica_core_task_maps/avoid_look_at_sphere_initializer.h> 65 void Instantiate(
const AvoidLookAtSphereInitializer& init)
override;
87 #endif // EXOTICA_CORE_TASK_MAPS_AVOID_LOOK_AT_SPHERE_H_ void(AvoidLookAtSphere::* UpdateTaskMapWithJacobian)(Eigen::VectorXdRefConst, Eigen::VectorXdRef, Eigen::MatrixXdRef)
int TaskSpaceDim() override
void UpdateAsInequalityConstraintWithJacobian(Eigen::VectorXdRefConst x, Eigen::VectorXdRef phi, Eigen::MatrixXdRef jacobian)
Eigen::VectorXd radii_squared_
The square of each object radii.
Eigen::Ref< Eigen::VectorXd > VectorXdRef
void Instantiate(const AvoidLookAtSphereInitializer &init) override
void UpdateAsCostWithJacobian(Eigen::VectorXdRefConst x, Eigen::VectorXdRef phi, Eigen::MatrixXdRef jacobian)
void PublishObjectsAsMarkerArray()
void UpdateAsCostWithoutJacobian(Eigen::VectorXdRefConst x, Eigen::VectorXdRef phi)
ros::Publisher pub_markers_
publish marker for RViz
const Eigen::Ref< const Eigen::VectorXd > & VectorXdRefConst
Eigen::Ref< Eigen::MatrixXd > MatrixXdRef
void UpdateAsInequalityConstraintWithoutJacobian(Eigen::VectorXdRefConst x, Eigen::VectorXdRef phi)
Avoids pointing end-effector at a given spherical object.
Eigen::VectorXd diameter_
The diameter of each object.
int n_objects_
Number of spherical objects.
void Update(Eigen::VectorXdRefConst x, Eigen::VectorXdRef phi) override
void(AvoidLookAtSphere::* UpdateTaskMapWithoutJacobian)(Eigen::VectorXdRefConst, Eigen::VectorXdRef)