30 #ifndef EXOTICA_CORE_TASK_MAPS_EFF_AXIS_ALIGNMENT_H_ 31 #define EXOTICA_CORE_TASK_MAPS_EFF_AXIS_ALIGNMENT_H_ 35 #include <exotica_core_task_maps/eff_axis_alignment_initializer.h> 36 #include <exotica_core_task_maps/frame_with_axis_and_direction_initializer.h> 38 #include <visualization_msgs/MarkerArray.h> 54 void SetDirection(
const std::string& frame_name,
const Eigen::Vector3d& dir_in);
55 Eigen::Vector3d
GetDirection(
const std::string& frame_name)
const;
57 void SetAxis(
const std::string& frame_name,
const Eigen::Vector3d& axis_in);
58 Eigen::Vector3d
GetAxis(
const std::string& frame_name)
const;
75 #endif // EXOTICA_CORE_TASK_MAPS_EFF_AXIS_ALIGNMENT_H_ void SetDirection(const std::string &frame_name, const Eigen::Vector3d &dir_in)
int TaskSpaceDim() override
Eigen::Ref< Eigen::VectorXd > VectorXdRef
Eigen::Vector3d link_axis_position_in_base_
Eigen::Vector3d GetAxis(const std::string &frame_name) const
Eigen::Vector3d GetDirection(const std::string &frame_name) const
std::shared_ptr< Scene > ScenePtr
ros::Publisher pub_debug_
void SetAxis(const std::string &frame_name, const Eigen::Vector3d &axis_in)
void Update(Eigen::VectorXdRefConst q, Eigen::VectorXdRef phi) override
const Eigen::Ref< const Eigen::VectorXd > & VectorXdRefConst
visualization_msgs::MarkerArray msg_debug_
Eigen::Vector3d link_position_in_base_
Eigen::Ref< Eigen::MatrixXd > MatrixXdRef
EIGEN_MAKE_ALIGNED_OPERATOR_NEW void AssignScene(ScenePtr scene) override