kdl_state_solver.h

Go to the documentation of this file.

 
#ifndef TESSERACT_STATE_SOLVER_KDL_STATE_SOLVER_H
#define TESSERACT_STATE_SOLVER_KDL_STATE_SOLVER_H
 
#include <tesseract_common/macros.h>
TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
#include <kdl/tree.hpp>
#include <kdl/jntarray.hpp>
#include <kdl/treejnttojacsolver.hpp>
#include <mutex>
TESSERACT_COMMON_IGNORE_WARNINGS_POP
 
#include <tesseract_state_solver/state_solver.h>
#include <tesseract_scene_graph/kdl_parser.h>
#include <tesseract_scene_graph/scene_state.h>
#include <tesseract_common/kinematic_limits.h>
 
namespace tesseract_scene_graph
{
class KDLStateSolver : public StateSolver
{
public:
  using Ptr = std::shared_ptr<KDLStateSolver>;
  using ConstPtr = std::shared_ptr<const KDLStateSolver>;
  using UPtr = std::unique_ptr<KDLStateSolver>;
  using ConstUPtr = std::unique_ptr<const KDLStateSolver>;
 
  KDLStateSolver(const tesseract_scene_graph::SceneGraph& scene_graph);
  KDLStateSolver(const tesseract_scene_graph::SceneGraph& scene_graph, KDLTreeData data);
  ~KDLStateSolver() override = default;
  KDLStateSolver(const KDLStateSolver& other);
  KDLStateSolver& operator=(const KDLStateSolver& other);
  KDLStateSolver(KDLStateSolver&&) = delete;
  KDLStateSolver& operator=(KDLStateSolver&&) = delete;
 
  StateSolver::UPtr clone() const override;
 
  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>& joint_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;
 
private:
  SceneState current_state_;                                   
  KDLTreeData data_;                                           
  std::unique_ptr<KDL::TreeJntToJacSolver> jac_solver_;        
  std::unordered_map<std::string, unsigned int> joint_to_qnr_; 
  std::vector<int> joint_qnr_;               
  KDL::JntArray kdl_jnt_array_;              
  tesseract_common::KinematicLimits limits_; 
  mutable std::mutex mutex_; 
  void calculateTransforms(SceneState& state,
                           const KDL::JntArray& q_in,
                           const KDL::SegmentMap::const_iterator& it,
                           const Eigen::Isometry3d& parent_frame) const;
 
  void calculateTransformsHelper(SceneState& state,
                                 const KDL::JntArray& q_in,
                                 const KDL::SegmentMap::const_iterator& it,
                                 const Eigen::Isometry3d& parent_frame) const;
 
  bool setJointValuesHelper(KDL::JntArray& q, const std::string& joint_name, const double& joint_value) const;
 
  bool calcJacobianHelper(KDL::Jacobian& jacobian, const KDL::JntArray& kdl_joints, const std::string& link_name) const;
 
  KDL::JntArray getKDLJntArray(const std::vector<std::string>& joint_names,
                               const Eigen::Ref<const Eigen::VectorXd>& joint_values) const;
  KDL::JntArray getKDLJntArray(const std::unordered_map<std::string, double>& joint_values) const;
 
  bool processKDLData(const tesseract_scene_graph::SceneGraph& scene_graph);
};
 
}  // namespace tesseract_scene_graph
#endif  // TESSERACT_STATE_SOLVER_KDL_STATE_SOLVER_H