ofkt_nodes.h

Go to the documentation of this file.

 
#ifndef TESSERACT_STATE_SOLVER_OFKT_NODES_H
#define TESSERACT_STATE_SOLVER_OFKT_NODES_H
 
#include <tesseract_state_solver/ofkt/ofkt_node.h>
 
namespace tesseract_scene_graph
{
/*********************************************************************/
/*************************** BASE NODE *******************************/
/*********************************************************************/
class OFKTBaseNode : public OFKTNode
{
public:
  // LCOV_EXCL_START
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  // LCOV_EXCL_STOP
 
  OFKTBaseNode(JointType type, OFKTNode* parent, std::string link_name);
 
  OFKTBaseNode(JointType type,
               OFKTNode* parent,
               std::string link_name,
               std::string joint_name,
               const Eigen::Isometry3d& static_tf);
 
  JointType getType() const override;
 
  void setParent(OFKTNode* parent) override;
  OFKTNode* getParent() override;
  const OFKTNode* getParent() const override;
 
  const std::string& getLinkName() const override;
  const std::string& getJointName() const override;
 
  void storeJointValue(double joint_value) override;
 
  double getJointValue() const override;
 
  bool hasJointValueChanged() const override;
 
  void setStaticTransformation(const Eigen::Isometry3d& static_tf) override;
 
  const Eigen::Isometry3d& getStaticTransformation() const override;
  const Eigen::Isometry3d& getLocalTransformation() const override;
  Eigen::Isometry3d computeLocalTransformation(double joint_value) const override;
 
  void computeAndStoreWorldTransformation() override;
  const Eigen::Isometry3d& getWorldTransformation() const override;
  bool updateWorldTransformationRequired() const override;
 
  Eigen::Matrix<double, 6, 1> getLocalTwist() const override;
 
  void addChild(OFKTNode* node) override;
  void removeChild(const OFKTNode* node) override;
  std::vector<OFKTNode*>& getChildren() override;
  const std::vector<const OFKTNode*>& getChildren() const override;
 
protected:
  tesseract_scene_graph::JointType type_;
  OFKTNode* parent_{ nullptr };
  std::string link_name_;
  std::string joint_name_;
  Eigen::Isometry3d static_tf_{ Eigen::Isometry3d::Identity() };
  Eigen::Isometry3d joint_tf_{ Eigen::Isometry3d::Identity() };
  Eigen::Isometry3d local_tf_{ Eigen::Isometry3d::Identity() };
  Eigen::Isometry3d world_tf_{ Eigen::Isometry3d::Identity() };
  Eigen::Matrix<double, 6, 1> local_twist_{ Eigen::VectorXd::Zero(6) };
 
  double joint_value_{ 0 };
  bool joint_value_changed_{ false };
 
  std::vector<OFKTNode*> children_;
  std::vector<const OFKTNode*> children_const_;
 
  bool update_world_required_{ true };
 
  friend class OFKTStateSolver;
};
 
/*********************************************************************/
/*************************** ROOT NODE *******************************/
/*********************************************************************/
class OFKTRootNode : public OFKTBaseNode
{
public:
  // LCOV_EXCL_START
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  // LCOV_EXCL_STOP
 
  OFKTRootNode(std::string link_name);
 
  void setParent(OFKTNode* parent) override;
  void storeJointValue(double joint_value) override;
  void setStaticTransformation(const Eigen::Isometry3d& static_tf) override;
  void computeAndStoreLocalTransformation() override;
  void computeAndStoreWorldTransformation() override;
  bool updateWorldTransformationRequired() const override;
 
  Eigen::Isometry3d computeLocalTransformation(double joint_value) const override;
 
private:
  friend class OFKTStateSolver;
};
 
/**********************************************************************/
/*************************** FIXED NODE *******************************/
/**********************************************************************/
class OFKTFixedNode : public OFKTBaseNode
{
public:
  // LCOV_EXCL_START
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  // LCOV_EXCL_STOP
 
  OFKTFixedNode(OFKTNode* parent, std::string link_name, std::string joint_name, const Eigen::Isometry3d& static_tf);
 
  void storeJointValue(double joint_value) override;
  double getJointValue() const override;
  void setStaticTransformation(const Eigen::Isometry3d& static_tf) override;
  void computeAndStoreLocalTransformation() override;
  Eigen::Isometry3d computeLocalTransformation(double joint_value) const override;
 
private:
  friend class OFKTStateSolver;
};
 
/**********************************************************************/
/************************* FLOATING NODE ******************************/
/**********************************************************************/
class OFKTFloatingNode : public OFKTBaseNode
{
public:
  // LCOV_EXCL_START
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  // LCOV_EXCL_STOP
 
  OFKTFloatingNode(OFKTNode* parent, std::string link_name, std::string joint_name, const Eigen::Isometry3d& static_tf);
 
  void storeJointValue(double joint_value) override;
  double getJointValue() const override;
  void setStaticTransformation(const Eigen::Isometry3d& static_tf) override;
  void computeAndStoreLocalTransformation() override;
  Eigen::Isometry3d computeLocalTransformation(double joint_value) const override;
 
private:
  friend class OFKTStateSolver;
};
 
/*********************************************************************/
/************************* REVOLUTE NODE *****************************/
/*********************************************************************/
class OFKTRevoluteNode : public OFKTBaseNode
{
public:
  // LCOV_EXCL_START
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  // LCOV_EXCL_STOP
 
  OFKTRevoluteNode(OFKTNode* parent,
                   std::string link_name,
                   std::string joint_name,
                   const Eigen::Isometry3d& static_tf,
                   const Eigen::Vector3d& axis);
 
  void storeJointValue(double joint_value) override;
  void computeAndStoreLocalTransformation() override;
  Eigen::Isometry3d computeLocalTransformation(double joint_value) const override;
  const Eigen::Vector3d& getAxis() const;
 
private:
  Eigen::Vector3d axis_;
 
  void computeAndStoreLocalTransformationImpl();
 
  friend class OFKTStateSolver;
};
 
/*********************************************************************/
/************************ CONTINUOUS NODE ****************************/
/*********************************************************************/
class OFKTContinuousNode : public OFKTBaseNode
{
public:
  // LCOV_EXCL_START
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  // LCOV_EXCL_STOP
 
  OFKTContinuousNode(OFKTNode* parent,
                     std::string link_name,
                     std::string joint_name,
                     const Eigen::Isometry3d& static_tf,
                     const Eigen::Vector3d& axis);
 
  void computeAndStoreLocalTransformation() override;
  Eigen::Isometry3d computeLocalTransformation(double joint_value) const override;
  const Eigen::Vector3d& getAxis() const;
 
private:
  Eigen::Vector3d axis_;
 
  void computeAndStoreLocalTransformationImpl();
 
  friend class OFKTStateSolver;
};
 
/*********************************************************************/
/************************* PRISMATIC NODE ****************************/
/*********************************************************************/
class OFKTPrismaticNode : public OFKTBaseNode
{
public:
  // LCOV_EXCL_START
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  // LCOV_EXCL_STOP
 
  OFKTPrismaticNode(OFKTNode* parent,
                    std::string link_name,
                    std::string joint_name,
                    const Eigen::Isometry3d& static_tf,
                    const Eigen::Vector3d& axis);
 
  void computeAndStoreLocalTransformation() override;
  Eigen::Isometry3d computeLocalTransformation(double joint_value) const override;
  const Eigen::Vector3d& getAxis() const;
 
private:
  Eigen::Vector3d axis_;
 
  void computeAndStoreLocalTransformationImpl();
 
  friend class OFKTStateSolver;
};
}  // namespace tesseract_scene_graph
 
#endif  // TESSERACT_STATE_SOLVER_OFKT_NODES_H