30 #ifndef EXOTICA_CORE_TASK_MAPS_INTERACTION_MESH_H_ 31 #define EXOTICA_CORE_TASK_MAPS_INTERACTION_MESH_H_ 36 #include <exotica_core_task_maps/interaction_mesh_initializer.h> 38 #include <visualization_msgs/Marker.h> 48 void Instantiate(
const InteractionMeshInitializer& init)
override;
55 void SetWeight(
int i,
int j,
double weight);
56 void SetWeights(
const Eigen::MatrixXd& weights);
76 #endif // EXOTICA_CORE_TASK_MAPS_INTERACTION_MESH_H_ void Debug(Eigen::VectorXdRefConst phi)
void SetWeight(int i, int j, double weight)
Eigen::MatrixXd GetWeights()
Eigen::Ref< Eigen::VectorXd > VectorXdRef
visualization_msgs::Marker imesh_mark_
std::shared_ptr< Scene > ScenePtr
ros::Publisher imesh_mark_pub_
const Eigen::Ref< const Eigen::MatrixXd > & MatrixXdRefConst
int TaskSpaceDim() override
void Update(Eigen::VectorXdRefConst x, Eigen::VectorXdRef phi) override
void SetWeights(const Eigen::MatrixXd &weights)
const Eigen::Ref< const Eigen::VectorXd > & VectorXdRefConst
void AssignScene(ScenePtr scene) override
void Instantiate(const InteractionMeshInitializer &init) override
Eigen::Ref< Eigen::MatrixXd > MatrixXdRef
void ComputeGoalLaplace(const Eigen::VectorXd &x, Eigen::VectorXd &goal)
static Eigen::VectorXd ComputeLaplace(Eigen::VectorXdRefConst eff_Phi, Eigen::MatrixXdRefConst weights, Eigen::MatrixXd *dist=nullptr, Eigen::VectorXd *wsum=nullptr)
void InitializeDebug(std::string ref)
virtual ~InteractionMesh()