kinematic_group.h

Go to the documentation of this file.

 
#ifndef TESSERACT_KINEMATICS_KINEMATIC_GROUP_H
#define TESSERACT_KINEMATICS_KINEMATIC_GROUP_H
 
#include <tesseract_common/macros.h>
TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
#include <memory>
#include <Eigen/Geometry>
TESSERACT_COMMON_IGNORE_WARNINGS_POP
 
#include <tesseract_kinematics/core/joint_group.h>
#include <tesseract_kinematics/core/inverse_kinematics.h>
 
namespace tesseract_kinematics
{
struct KinGroupIKInput
{
  // LCOV_EXCL_START
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  // LCOV_EXCL_STOP
 
  KinGroupIKInput(const Eigen::Isometry3d& p, std::string wf, std::string tl)
    : pose(p), working_frame(std::move(wf)), tip_link_name(std::move(tl))
  {
  }
 
  KinGroupIKInput() = default;
 
  Eigen::Isometry3d pose;
 
  std::string working_frame;
 
  std::string tip_link_name;  // This defines the internal kinematic group the information belongs to
};
using KinGroupIKInputs = tesseract_common::AlignedVector<KinGroupIKInput>;
 
class KinematicGroup : public JointGroup
{
public:
  // LCOV_EXCL_START
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  // LCOV_EXCL_STOP
 
  using Ptr = std::shared_ptr<KinematicGroup>;
  using ConstPtr = std::shared_ptr<const KinematicGroup>;
  using UPtr = std::unique_ptr<KinematicGroup>;
  using ConstUPtr = std::unique_ptr<const KinematicGroup>;
 
  ~KinematicGroup() override = default;
  KinematicGroup(const KinematicGroup& other);
  KinematicGroup& operator=(const KinematicGroup& other);
  KinematicGroup(KinematicGroup&&) = default;
  KinematicGroup& operator=(KinematicGroup&&) = default;
 
  KinematicGroup(std::string name,
                 std::vector<std::string> joint_names,
                 InverseKinematics::UPtr inv_kin,
                 const tesseract_scene_graph::SceneGraph& scene_graph,
                 const tesseract_scene_graph::SceneState& scene_state);
 
  IKSolutions calcInvKin(const KinGroupIKInputs& tip_link_poses, const Eigen::Ref<const Eigen::VectorXd>& seed) const;
 
  IKSolutions calcInvKin(const KinGroupIKInput& tip_link_pose, const Eigen::Ref<const Eigen::VectorXd>& seed) const;
 
  std::vector<std::string> getAllValidWorkingFrames() const;
 
  std::vector<std::string> getAllPossibleTipLinkNames() const;
 
private:
  std::vector<std::string> joint_names_;
  bool reorder_required_{ false };
  std::vector<Eigen::Index> inv_kin_joint_map_;
  InverseKinematics::UPtr inv_kin_;
  Eigen::Isometry3d inv_to_fwd_base_{ Eigen::Isometry3d::Identity() };
  std::vector<std::string> working_frames_;
  std::unordered_map<std::string, std::string> inv_tip_links_map_;
};
 
}  // namespace tesseract_kinematics
 
#endif  // TESSERACT_KINEMATICS_KINEMATIC_GROUP_H