ofkt_state_solver.h

Go to the documentation of this file.

 
#ifndef TESSERACT_STATE_SOLVER_OFKT_STATE_SOLVER_H
#define TESSERACT_STATE_SOLVER_OFKT_STATE_SOLVER_H
 
#include <tesseract_common/macros.h>
TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
#include <Eigen/Geometry>
#include <memory>
#include <string>
#include <shared_mutex>
TESSERACT_COMMON_IGNORE_WARNINGS_POP
 
#include <tesseract_state_solver/mutable_state_solver.h>
#include <tesseract_state_solver/ofkt/ofkt_node.h>
#include <tesseract_scene_graph/scene_state.h>
#include <tesseract_common/kinematic_limits.h>
 
namespace tesseract_scene_graph
{
class OFKTStateSolver : public MutableStateSolver
{
public:
  // LCOV_EXCL_START
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  // LCOV_EXCL_STOP
 
  using Ptr = std::shared_ptr<OFKTStateSolver>;
  using ConstPtr = std::shared_ptr<const OFKTStateSolver>;
  using UPtr = std::unique_ptr<OFKTStateSolver>;
  using ConstUPtr = std::unique_ptr<const OFKTStateSolver>;
 
  OFKTStateSolver(const tesseract_scene_graph::SceneGraph& scene_graph, const std::string& prefix = "");
  OFKTStateSolver(const std::string& root_name);
  ~OFKTStateSolver() override = default;
  OFKTStateSolver(const OFKTStateSolver& other);
  OFKTStateSolver& operator=(const OFKTStateSolver& other);
  OFKTStateSolver(OFKTStateSolver&&) = delete;
  OFKTStateSolver& operator=(OFKTStateSolver&&) = delete;
 
  void setRevision(int revision) override final;
 
  int getRevision() const override final;
 
  void setState(const Eigen::Ref<const Eigen::VectorXd>& joint_values,
                const tesseract_common::TransformMap& floating_joint_values = {}) override final;
  void setState(const std::unordered_map<std::string, double>& joint_values,
                const tesseract_common::TransformMap& floating_joint_values = {}) override final;
  void setState(const std::vector<std::string>& joint_names,
                const Eigen::Ref<const Eigen::VectorXd>& joint_values,
                const tesseract_common::TransformMap& floating_joint_values = {}) override final;
  void setState(const tesseract_common::TransformMap& floating_joint_values) override final;
 
  SceneState getState(const Eigen::Ref<const Eigen::VectorXd>& joint_values,
                      const tesseract_common::TransformMap& floating_joint_values = {}) const override final;
  SceneState getState(const std::unordered_map<std::string, double>& joint_values,
                      const tesseract_common::TransformMap& floating_joint_values = {}) const override final;
  SceneState getState(const std::vector<std::string>& joint_names,
                      const Eigen::Ref<const Eigen::VectorXd>& joint_values,
                      const tesseract_common::TransformMap& floating_joint_values = {}) const override final;
  SceneState getState(const tesseract_common::TransformMap& floating_joint_values) const override final;
 
  SceneState getState() const override final;
 
  SceneState getRandomState() const override final;
 
  Eigen::MatrixXd getJacobian(const Eigen::Ref<const Eigen::VectorXd>& joint_values,
                              const std::string& link_name,
                              const tesseract_common::TransformMap& floating_joint_values = {}) const override final;
 
  Eigen::MatrixXd getJacobian(const std::unordered_map<std::string, double>& joints_values,
                              const std::string& link_name,
                              const tesseract_common::TransformMap& floating_joint_values = {}) const override final;
  Eigen::MatrixXd getJacobian(const std::vector<std::string>& joint_names,
                              const Eigen::Ref<const Eigen::VectorXd>& joint_values,
                              const std::string& link_name,
                              const tesseract_common::TransformMap& floating_joint_values = {}) const override final;
 
  std::vector<std::string> getJointNames() const override final;
 
  std::vector<std::string> getFloatingJointNames() const override final;
 
  std::vector<std::string> getActiveJointNames() const override final;
 
  std::string getBaseLinkName() const override final;
 
  std::vector<std::string> getLinkNames() const override final;
 
  std::vector<std::string> getActiveLinkNames() const override final;
 
  std::vector<std::string> getStaticLinkNames() const override final;
 
  bool isActiveLinkName(const std::string& link_name) const override final;
 
  bool hasLinkName(const std::string& link_name) const override final;
 
  tesseract_common::VectorIsometry3d getLinkTransforms() const override final;
 
  Eigen::Isometry3d getLinkTransform(const std::string& link_name) const override final;
 
  Eigen::Isometry3d getRelativeLinkTransform(const std::string& from_link_name,
                                             const std::string& to_link_name) const override final;
 
  tesseract_common::KinematicLimits getLimits() const override final;
 
  bool addLink(const Link& link, const Joint& joint) override final;
 
  bool moveLink(const Joint& joint) override final;
 
  bool removeLink(const std::string& name) override final;
 
  bool replaceJoint(const Joint& joint) override final;
 
  bool removeJoint(const std::string& name) override final;
 
  bool moveJoint(const std::string& name, const std::string& parent_link) override final;
 
  bool changeJointOrigin(const std::string& name, const Eigen::Isometry3d& new_origin) override final;
 
  bool changeJointPositionLimits(const std::string& name, double lower, double upper) override final;
 
  bool changeJointVelocityLimits(const std::string& name, double limit) override final;
 
  bool changeJointAccelerationLimits(const std::string& name, double limit) override final;
 
  bool changeJointJerkLimits(const std::string& name, double limit) override final;
 
  bool insertSceneGraph(const SceneGraph& scene_graph,
                        const Joint& joint,
                        const std::string& prefix = "") override final;
 
  StateSolver::UPtr clone() const override final;
 
private:
  SceneState current_state_;                                         
  std::vector<std::string> joint_names_;                             
  std::vector<std::string> active_joint_names_;                      
  std::vector<std::string> floating_joint_names_;                    
  std::vector<std::string> link_names_;                              
  std::unordered_map<std::string, std::unique_ptr<OFKTNode>> nodes_; 
  std::unordered_map<std::string, OFKTNode*> link_map_;              
  tesseract_common::KinematicLimits limits_;                         
  std::unique_ptr<OFKTNode> root_;                                   
  int revision_{ 0 };                                                
  mutable std::shared_mutex mutex_;
 
  bool initHelper(const tesseract_scene_graph::SceneGraph& scene_graph, const std::string& prefix);
 
  void clear();
 
  void loadActiveLinkNamesRecursive(std::vector<std::string>& active_link_names,
                                    const OFKTNode* node,
                                    bool active) const;
 
  void loadStaticLinkNamesRecursive(std::vector<std::string>& static_link_names, const OFKTNode* node) const;
 
  void update(OFKTNode* node, bool update_required);
 
  void
  update(SceneState& state, const OFKTNode* node, const Eigen::Isometry3d& parent_world_tf, bool update_required) const;
 
  Eigen::MatrixXd calcJacobianHelper(const std::unordered_map<std::string, double>& joints,
                                     const std::string& link_name,
                                     const tesseract_common::TransformMap& floating_joint_values = {}) const;
 
  void cloneHelper(OFKTStateSolver& cloned, const OFKTNode* node) const;
 
  void addNode(const Joint& joint,
               const std::string& joint_name,
               const std::string& parent_link_name,
               const std::string& child_link_name,
               std::vector<std::shared_ptr<const JointLimits>>& new_joint_limits);
 
  void removeNode(OFKTNode* node,
                  std::vector<std::string>& removed_links,
                  std::vector<std::string>& removed_joints,
                  std::vector<std::string>& removed_active_joints,
                  std::vector<long>& removed_active_joints_indices);
 
  void moveLinkHelper(std::vector<std::shared_ptr<const JointLimits>>& new_joint_limits, const Joint& joint);
 
  void replaceJointHelper(std::vector<std::shared_ptr<const JointLimits>>& new_joint_limits, const Joint& joint);
 
  void removeJointHelper(const std::vector<std::string>& removed_links,
                         const std::vector<std::string>& removed_joints,
                         const std::vector<std::string>& removed_active_joints,
                         const std::vector<long>& removed_active_joints_indices);
 
  void addNewJointLimits(const std::vector<std::shared_ptr<const JointLimits>>& new_joint_limits);
  friend struct ofkt_builder;
};
 
}  // namespace tesseract_scene_graph
 
#endif  // TESSERACT_STATE_SOLVER_OFKT_STATE_SOLVER_H