Add floating joint support

Author: Levi-Armstrong

tesseract_environment/include/tesseract_environment/environment.h

@@ -296,8 +296,11 @@ class Environment
    * will update the contact managers transforms
    *
    */
-  void setState(const std::unordered_map<std::string, double>& joints);
-  void setState(const std::vector<std::string>& joint_names, const Eigen::Ref<const Eigen::VectorXd>& joint_values);
+  void setState(const std::unordered_map<std::string, double>& joints,
+                const tesseract_common::TransformMap& floating_joints = {});
+  void setState(const std::vector<std::string>& joint_names,
+                const Eigen::Ref<const Eigen::VectorXd>& joint_values,
+                const tesseract_common::TransformMap& floating_joints = {});
 
   /**
    * @brief Get the state of the environment for a given set or subset of joint values.
@@ -307,9 +310,11 @@ class Environment
    * @param joints A map of joint names to joint values to change.
    * @return A the state of the environment
    */
-  tesseract_scene_graph::SceneState getState(const std::unordered_map<std::string, double>& joints) const;
+  tesseract_scene_graph::SceneState getState(const std::unordered_map<std::string, double>& joints,
+                                             const tesseract_common::TransformMap& floating_joints = {}) const;
   tesseract_scene_graph::SceneState getState(const std::vector<std::string>& joint_names,
-                                             const Eigen::Ref<const Eigen::VectorXd>& joint_values) const;
+                                             const Eigen::Ref<const Eigen::VectorXd>& joint_values,
+                                             const tesseract_common::TransformMap& floating_joints = {}) const;
 
   /** @brief Get the current state of the environment */
   tesseract_scene_graph::SceneState getState() const;
@@ -389,6 +394,18 @@ class Environment
    */
   Eigen::VectorXd getCurrentJointValues(const std::vector<std::string>& joint_names) const;
 
+  /**
+   * @brief Get the current floating joint values
+   * @return The joint origin transform for the floating joint
+   */
+  tesseract_common::TransformMap getCurrentFloatingJointValues() const;
+
+  /**
+   * @brief Get the current floating joint values
+   * @return The joint origin transform for the floating joint
+   */
+  tesseract_common::TransformMap getCurrentFloatingJointValues(const std::vector<std::string>& joint_names) const;
+
   /**
    * @brief Get the root link name
    * @return String

tesseract_environment/src/environment.cpp

@@ -289,14 +289,21 @@ struct Environment::Implementation
 
   bool initHelper(const std::vector<std::shared_ptr<const Command>>& commands);
 
-  void setState(const std::unordered_map<std::string, double>& joints);
+  void setState(const std::unordered_map<std::string, double>& joints,
+                const tesseract_common::TransformMap& floating_joints = {});
 
-  void setState(const std::vector<std::string>& joint_names, const Eigen::Ref<const Eigen::VectorXd>& joint_values);
+  void setState(const std::vector<std::string>& joint_names,
+                const Eigen::Ref<const Eigen::VectorXd>& joint_values,
+                const tesseract_common::TransformMap& floating_joints = {});
 
   Eigen::VectorXd getCurrentJointValues() const;
 
   Eigen::VectorXd getCurrentJointValues(const std::vector<std::string>& joint_names) const;
 
+  tesseract_common::TransformMap getCurrentFloatingJointValues() const;
+
+  tesseract_common::TransformMap getCurrentFloatingJointValues(const std::vector<std::string>& joint_names) const;
+
   std::vector<std::string> getStaticLinkNames(const std::vector<std::string>& joint_names) const;
 
   void clear();
@@ -425,7 +432,7 @@ struct Environment::Implementation
 
     tesseract_scene_graph::SceneState current_state;
     ar& boost::serialization::make_nvp("current_state", current_state);
-    setState(current_state.joints);
+    setState(current_state.joints, current_state.floating_joints);
 
     // No need to serialize the contact allowed validator because it cannot be modified and is constructed internally
     // from the scene graph
@@ -567,16 +574,18 @@ bool Environment::Implementation::initHelper(const std::vector<std::shared_ptr<c
   return initialized;
 }
 
-void Environment::Implementation::setState(const std::unordered_map<std::string, double>& joints)
+void Environment::Implementation::setState(const std::unordered_map<std::string, double>& joints,
+                                           const tesseract_common::TransformMap& floating_joints)
 {
-  state_solver->setState(joints);
+  state_solver->setState(joints, floating_joints);
   currentStateChanged();
 }
 
 void Environment::Implementation::setState(const std::vector<std::string>& joint_names,
-                                           const Eigen::Ref<const Eigen::VectorXd>& joint_values)
+                                           const Eigen::Ref<const Eigen::VectorXd>& joint_values,
+                                           const tesseract_common::TransformMap& floating_joints)
 {
-  state_solver->setState(joint_names, joint_values);
+  state_solver->setState(joint_names, joint_values, floating_joints);
   currentStateChanged();
 }
 
@@ -601,6 +610,21 @@ Eigen::VectorXd Environment::Implementation::getCurrentJointValues(const std::ve
   return jv;
 }
 
+tesseract_common::TransformMap Environment::Implementation::getCurrentFloatingJointValues() const
+{
+  return current_state.floating_joints;
+}
+
+tesseract_common::TransformMap
+Environment::Implementation::getCurrentFloatingJointValues(const std::vector<std::string>& joint_names) const
+{
+  tesseract_common::TransformMap fjv;
+  for (const auto& joint_name : joint_names)
+    fjv[joint_name] = current_state.floating_joints.at(joint_name);
+
+  return fjv;
+}
+
 std::vector<std::string>
 Environment::Implementation::getStaticLinkNames(const std::vector<std::string>& joint_names) const
 {
@@ -2427,40 +2451,44 @@ const std::string& Environment::getName() const
   return std::as_const<Implementation>(*impl_).scene_graph->getName();
 }
 
-void Environment::setState(const std::unordered_map<std::string, double>& joints)
+void Environment::setState(const std::unordered_map<std::string, double>& joints,
+                           const tesseract_common::TransformMap& floating_joints)
 {
   {
     std::unique_lock<std::shared_mutex> lock(mutex_);
-    impl_->setState(joints);
+    impl_->setState(joints, floating_joints);
   }
 
   std::shared_lock<std::shared_mutex> lock(mutex_);
   impl_->triggerCurrentStateChangedCallbacks();
 }
 
 void Environment::setState(const std::vector<std::string>& joint_names,
-                           const Eigen::Ref<const Eigen::VectorXd>& joint_values)
+                           const Eigen::Ref<const Eigen::VectorXd>& joint_values,
+                           const tesseract_common::TransformMap& floating_joints)
 {
   {
     std::unique_lock<std::shared_mutex> lock(mutex_);
-    impl_->setState(joint_names, joint_values);
+    impl_->setState(joint_names, joint_values, floating_joints);
   }
 
   std::shared_lock<std::shared_mutex> lock(mutex_);
   impl_->triggerCurrentStateChangedCallbacks();
 }
 
-tesseract_scene_graph::SceneState Environment::getState(const std::unordered_map<std::string, double>& joints) const
+tesseract_scene_graph::SceneState Environment::getState(const std::unordered_map<std::string, double>& joints,
+                                                        const tesseract_common::TransformMap& floating_joints) const
 {
   std::shared_lock<std::shared_mutex> lock(mutex_);
-  return std::as_const<Implementation>(*impl_).state_solver->getState(joints);
+  return std::as_const<Implementation>(*impl_).state_solver->getState(joints, floating_joints);
 }
 
 tesseract_scene_graph::SceneState Environment::getState(const std::vector<std::string>& joint_names,
-                                                        const Eigen::Ref<const Eigen::VectorXd>& joint_values) const
+                                                        const Eigen::Ref<const Eigen::VectorXd>& joint_values,
+                                                        const tesseract_common::TransformMap& floating_joints) const
 {
   std::shared_lock<std::shared_mutex> lock(mutex_);
-  return std::as_const<Implementation>(*impl_).state_solver->getState(joint_names, joint_values);
+  return std::as_const<Implementation>(*impl_).state_solver->getState(joint_names, joint_values, floating_joints);
 }
 
 tesseract_scene_graph::SceneState Environment::getState() const
@@ -2543,6 +2571,19 @@ Eigen::VectorXd Environment::getCurrentJointValues(const std::vector<std::string
   return std::as_const<Implementation>(*impl_).getCurrentJointValues(joint_names);
 }
 
+tesseract_common::TransformMap Environment::getCurrentFloatingJointValues() const
+{
+  std::shared_lock<std::shared_mutex> lock(mutex_);
+  return std::as_const<Implementation>(*impl_).getCurrentFloatingJointValues();
+}
+
+tesseract_common::TransformMap
+Environment::getCurrentFloatingJointValues(const std::vector<std::string>& joint_names) const
+{
+  std::shared_lock<std::shared_mutex> lock(mutex_);
+  return std::as_const<Implementation>(*impl_).getCurrentFloatingJointValues(joint_names);
+}
+
 std::string Environment::getRootLinkName() const
 {
   std::shared_lock<std::shared_mutex> lock(mutex_);

tesseract_kinematics/core/src/joint_group.cpp

@@ -52,8 +52,8 @@ JointGroup::JointGroup(std::string name,
       throw std::runtime_error("Joint name '" + joint_name + "' does not exist in the provided scene graph!");
   }
 
-  tesseract_scene_graph::KDLTreeData data =
-      tesseract_scene_graph::parseSceneGraph(scene_graph, joint_names_, scene_state.joints);
+  tesseract_scene_graph::KDLTreeData data = tesseract_scene_graph::parseSceneGraph(
+      scene_graph, joint_names_, scene_state.joints, scene_state.floating_joints);
   state_solver_ = std::make_unique<tesseract_scene_graph::KDLStateSolver>(scene_graph, data);
   jacobian_map_.reserve(joint_names_.size());
   std::vector<std::string> solver_jn = state_solver_->getActiveJointNames();

tesseract_scene_graph/include/tesseract_scene_graph/kdl_parser.h

@@ -49,6 +49,8 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
 #include <kdl/jacobian.hpp>
 TESSERACT_COMMON_IGNORE_WARNINGS_POP
 
+#include <tesseract_common/eigen_types.h>
+
 namespace tesseract_scene_graph
 {
 class SceneGraph;
@@ -122,13 +124,20 @@ KDL::RigidBodyInertia convert(const std::shared_ptr<const Inertial>& inertial);
 /** @brief The KDLTreeData populated when parsing scene graph */
 struct KDLTreeData
 {
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
   KDL::Tree tree;
   std::string base_link_name;
   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::vector<std::string> active_link_names;
   std::vector<std::string> static_link_names;
+  tesseract_common::TransformMap floating_joint_values;
+
+  bool operator==(const KDLTreeData& rhs) const;
+  bool operator!=(const KDLTreeData& rhs) const;
 };
 
 /**
@@ -153,7 +162,8 @@ KDLTreeData parseSceneGraph(const SceneGraph& scene_graph);
  */
 KDLTreeData parseSceneGraph(const SceneGraph& scene_graph,
                             const std::vector<std::string>& joint_names,
-                            const std::unordered_map<std::string, double>& joint_values);
+                            const std::unordered_map<std::string, double>& joint_values,
+                            const tesseract_common::TransformMap& floating_joint_values);
 
 }  // namespace tesseract_scene_graph
 

tesseract_scene_graph/include/tesseract_scene_graph/scene_state.h

@@ -69,6 +69,9 @@ struct SceneState
   /**  @brief The joint values used for calculating the joint and link transforms */
   std::unordered_map<std::string, double> joints;
 
+  /** @brief The floating joint values used for calculating the joint and link transforms */
+  tesseract_common::TransformMap floating_joints;
+
   /** @brief The link transforms in world coordinate system */
   tesseract_common::TransformMap link_transforms;
 
@@ -77,6 +80,8 @@ struct SceneState
 
   Eigen::VectorXd getJointValues(const std::vector<std::string>& joint_names) const;
 
+  tesseract_common::TransformMap getFloatingJointValues(const std::vector<std::string>& joint_names) const;
+
   bool operator==(const SceneState& rhs) const;
   bool operator!=(const SceneState& rhs) const;