47 ThrowPretty(
"This task map requires three frames to initialise: The first two define the line, the third is the query frame.");
59 for (std::size_t i = 0; i < 4; ++i)
84 if (phi.rows() != 1)
ThrowNamed(
"Wrong size of Phi!");
93 for (std::size_t i = 0; i < 3; ++i)
102 for (std::size_t i = 0; i < 2; ++i)
115 if (phi.rows() != 1)
ThrowNamed(
"Wrong size of Phi!");
116 if (jacobian.rows() != 1 || jacobian.cols() !=
kinematics[0].jacobian(0).data.cols())
ThrowNamed(
"Wrong size of jacobian! " <<
kinematics[0].jacobian(0).data.cols());
void publish(const boost::shared_ptr< M > &message) const
std::vector< KinematicFrameRequest > frames_
void PointToLineDistance(const Eigen::Vector2d &P1, const Eigen::Vector2d &P2, const Eigen::Vector2d &P3, double &d)
Computes the signed distance between a point and a line defined by two points in 2D.
Eigen::Ref< Eigen::VectorXd > VectorXdRef
std::shared_ptr< Scene > ScenePtr
void PointToLineDistanceDerivative(const Eigen::Vector2d &P1, const Eigen::Vector2d &P2, const Eigen::Vector2d &P3, const Eigen::MatrixXd &dP1_dq, const Eigen::MatrixXd &dP2_dq, const Eigen::MatrixXd &dP3_dq, Eigen::Ref< Eigen::MatrixXd > &derivative)
Derivative of signed distance between a point and a line defined by two points in 2D...
const Eigen::Ref< const Eigen::VectorXd > & VectorXdRefConst
void Update(Eigen::VectorXdRefConst q, Eigen::VectorXdRef phi) final
visualization_msgs::MarkerArray debug_marker_array_msg_
Eigen::Ref< Eigen::MatrixXd > MatrixXdRef
REGISTER_TASKMAP_TYPE("DistanceToLine2D", exotica::DistanceToLine2D)
ros::Publisher pub_debug_
void AssignScene(ScenePtr scene) final