From f99cdc8b98f4c58c5ba5d3e79cb434db6e301605 Mon Sep 17 00:00:00 2001 From: Levi Armstrong Date: Wed, 6 Nov 2024 21:39:26 -0600 Subject: [PATCH] Add floating joint support --- .../bullet/src/create_convex_hull.cpp | 81 +++++++++-- .../tesseract_environment/environment.h | 25 +++- tesseract_environment/src/environment.cpp | 71 ++++++++-- .../tesseract_scene_graph/scene_state.h | 5 + tesseract_scene_graph/src/scene_state.cpp | 11 ++ .../kdl/kdl_state_solver.h | 29 ++-- .../tesseract_state_solver/ofkt/ofkt_nodes.h | 22 +++ .../ofkt/ofkt_state_solver.h | 34 +++-- .../tesseract_state_solver/state_solver.h | 35 +++-- .../src/kdl_state_solver.cpp | 29 ++-- tesseract_state_solver/src/ofkt_nodes.cpp | 37 +++++ .../src/ofkt_state_solver.cpp | 134 +++++++++++++++--- 12 files changed, 426 insertions(+), 87 deletions(-) diff --git a/tesseract_collision/bullet/src/create_convex_hull.cpp b/tesseract_collision/bullet/src/create_convex_hull.cpp index 454aa0309ce..a1587df8794 100644 --- a/tesseract_collision/bullet/src/create_convex_hull.cpp +++ b/tesseract_collision/bullet/src/create_convex_hull.cpp @@ -51,17 +51,76 @@ int main(int argc, char** argv) namespace po = boost::program_options; po::options_description desc("Options"); - desc.add_options()("help,h", "Print help messages")( - "input,i", po::value(&input)->required(), "File path to mesh used to create a convex hull.")( - "output,o", po::value(&output)->required(), "File path to save the generated convex hull as a ply.")( - "shrink,s", - po::value(&shrink), - "If positive, the convex hull is shrunken by that amount (each face is moved by 'shrink' length units towards " - "the center along its normal).")("clamp,c", - po::value(&clamp), - "If positive, 'shrink' is clamped to not exceed 'clamp * innerRadius', where " - "'innerRadius' is the minimum distance of a face to the center of the convex " - "hull."); + desc.add_options()( + "help,h", "Print help messages")("input,i", + po::value(&input)->required(), + "File path to mesh used to create a convex hull.")("output,o", + po::value( + &output) + ->required(), + "File path to save the " + "generated convex hull as a " + "ply.")("shrink,s", + po::value( + &shrink), + "If positive, the " + "convex hull is " + "shrunken by that " + "amount (each face " + "is moved by " + "'shrink' length " + "units towards " + "the center along " + "its normal).")("clam" + "p,c", + po::value< + double>( + &clamp), + "If " + "posi" + "tive" + ", " + "'shr" + "ink'" + " is " + "clam" + "ped " + "to " + "not " + "exce" + "ed " + "'cla" + "mp " + "* " + "inne" + "rRad" + "ius'" + ", " + "wher" + "e " + "'inn" + "erRa" + "dius" + "' " + "is " + "the " + "mini" + "mum " + "dist" + "ance" + " of " + "a " + "face" + " to " + "the " + "cent" + "er " + "of " + "the " + "conv" + "ex " + "hull" + "."); po::variables_map vm; try diff --git a/tesseract_environment/include/tesseract_environment/environment.h b/tesseract_environment/include/tesseract_environment/environment.h index d77415506bc..71deb622815 100644 --- a/tesseract_environment/include/tesseract_environment/environment.h +++ b/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& joints); - void setState(const std::vector& joint_names, const Eigen::Ref& joint_values); + void setState(const std::unordered_map& joints, + const tesseract_common::TransformMap& floating_joints = {}); + void setState(const std::vector& joint_names, + const Eigen::Ref& 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& joints) const; + tesseract_scene_graph::SceneState getState(const std::unordered_map& joints, + const tesseract_common::TransformMap& floating_joints = {}) const; tesseract_scene_graph::SceneState getState(const std::vector& joint_names, - const Eigen::Ref& joint_values) const; + const Eigen::Ref& 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& 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& joint_names) const; + /** * @brief Get the root link name * @return String diff --git a/tesseract_environment/src/environment.cpp b/tesseract_environment/src/environment.cpp index c76a790f7b7..eddb54c4aa5 100644 --- a/tesseract_environment/src/environment.cpp +++ b/tesseract_environment/src/environment.cpp @@ -289,14 +289,21 @@ struct Environment::Implementation bool initHelper(const std::vector>& commands); - void setState(const std::unordered_map& joints); + void setState(const std::unordered_map& joints, + const tesseract_common::TransformMap& floating_joints = {}); - void setState(const std::vector& joint_names, const Eigen::Ref& joint_values); + void setState(const std::vector& joint_names, + const Eigen::Ref& joint_values, + const tesseract_common::TransformMap& floating_joints = {}); Eigen::VectorXd getCurrentJointValues() const; Eigen::VectorXd getCurrentJointValues(const std::vector& joint_names) const; + tesseract_common::TransformMap getCurrentFloatingJointValues() const; + + tesseract_common::TransformMap getCurrentFloatingJointValues(const std::vector& joint_names) const; + std::vector getStaticLinkNames(const std::vector& 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& joints) +void Environment::Implementation::setState(const std::unordered_map& 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& joint_names, - const Eigen::Ref& joint_values) + const Eigen::Ref& 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& 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 Environment::Implementation::getStaticLinkNames(const std::vector& joint_names) const { @@ -2427,11 +2451,12 @@ const std::string& Environment::getName() const return std::as_const(*impl_).scene_graph->getName(); } -void Environment::setState(const std::unordered_map& joints) +void Environment::setState(const std::unordered_map& joints, + const tesseract_common::TransformMap& floating_joints) { { std::unique_lock lock(mutex_); - impl_->setState(joints); + impl_->setState(joints, floating_joints); } std::shared_lock lock(mutex_); @@ -2439,28 +2464,31 @@ void Environment::setState(const std::unordered_map& joints } void Environment::setState(const std::vector& joint_names, - const Eigen::Ref& joint_values) + const Eigen::Ref& joint_values, + const tesseract_common::TransformMap& floating_joints) { { std::unique_lock lock(mutex_); - impl_->setState(joint_names, joint_values); + impl_->setState(joint_names, joint_values, floating_joints); } std::shared_lock lock(mutex_); impl_->triggerCurrentStateChangedCallbacks(); } -tesseract_scene_graph::SceneState Environment::getState(const std::unordered_map& joints) const +tesseract_scene_graph::SceneState Environment::getState(const std::unordered_map& joints, + const tesseract_common::TransformMap& floating_joints) const { std::shared_lock lock(mutex_); - return std::as_const(*impl_).state_solver->getState(joints); + return std::as_const(*impl_).state_solver->getState(joints, floating_joints); } tesseract_scene_graph::SceneState Environment::getState(const std::vector& joint_names, - const Eigen::Ref& joint_values) const + const Eigen::Ref& joint_values, + const tesseract_common::TransformMap& floating_joints) const { std::shared_lock lock(mutex_); - return std::as_const(*impl_).state_solver->getState(joint_names, joint_values); + return std::as_const(*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(*impl_).getCurrentJointValues(joint_names); } +tesseract_common::TransformMap Environment::getCurrentFloatingJointValues() const +{ + std::shared_lock lock(mutex_); + return std::as_const(*impl_).getCurrentFloatingJointValues(); +} + +tesseract_common::TransformMap +Environment::getCurrentFloatingJointValues(const std::vector& joint_names) const +{ + std::shared_lock lock(mutex_); + return std::as_const(*impl_).getCurrentFloatingJointValues(joint_names); +} + std::string Environment::getRootLinkName() const { std::shared_lock lock(mutex_); diff --git a/tesseract_scene_graph/include/tesseract_scene_graph/scene_state.h b/tesseract_scene_graph/include/tesseract_scene_graph/scene_state.h index 67e3f77b660..ff9d262908f 100644 --- a/tesseract_scene_graph/include/tesseract_scene_graph/scene_state.h +++ b/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 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& joint_names) const; + tesseract_common::TransformMap getFloatingJointValues(const std::vector& joint_names) const; + bool operator==(const SceneState& rhs) const; bool operator!=(const SceneState& rhs) const; diff --git a/tesseract_scene_graph/src/scene_state.cpp b/tesseract_scene_graph/src/scene_state.cpp index 6972234ef66..be3aa3d3df6 100644 --- a/tesseract_scene_graph/src/scene_state.cpp +++ b/tesseract_scene_graph/src/scene_state.cpp @@ -52,6 +52,15 @@ Eigen::VectorXd SceneState::getJointValues(const std::vector& joint return jv; } +tesseract_common::TransformMap SceneState::getFloatingJointValues(const std::vector& joint_names) const +{ + tesseract_common::TransformMap fjv; + for (const auto& joint_name : joint_names) + fjv[joint_name] = floating_joints.at(joint_name); + + return fjv; +} + bool SceneState::operator==(const SceneState& rhs) const { auto isometry_equal = [](const Eigen::Isometry3d& iso_1, const Eigen::Isometry3d& iso_2) { @@ -61,6 +70,7 @@ bool SceneState::operator==(const SceneState& rhs) const using namespace tesseract_common; bool equal = true; equal &= isIdenticalMap, double>(joints, rhs.joints); + equal &= isIdenticalMap(floating_joints, rhs.floating_joints, isometry_equal); equal &= isIdenticalMap(link_transforms, rhs.link_transforms, isometry_equal); equal &= isIdenticalMap(joint_transforms, rhs.joint_transforms, isometry_equal); @@ -72,6 +82,7 @@ template void SceneState::serialize(Archive& ar, const unsigned int /*version*/) { ar& BOOST_SERIALIZATION_NVP(joints); + ar& BOOST_SERIALIZATION_NVP(floating_joints); ar& BOOST_SERIALIZATION_NVP(link_transforms); ar& BOOST_SERIALIZATION_NVP(joint_transforms); } diff --git a/tesseract_state_solver/include/tesseract_state_solver/kdl/kdl_state_solver.h b/tesseract_state_solver/include/tesseract_state_solver/kdl/kdl_state_solver.h index 120f684cfa9..3b629459323 100644 --- a/tesseract_state_solver/include/tesseract_state_solver/kdl/kdl_state_solver.h +++ b/tesseract_state_solver/include/tesseract_state_solver/kdl/kdl_state_solver.h @@ -59,31 +59,42 @@ class KDLStateSolver : public StateSolver StateSolver::UPtr clone() const override; - void setState(const Eigen::Ref& joint_values) override final; - void setState(const std::unordered_map& joint_values) override final; + void setState(const Eigen::Ref& joint_values, + const tesseract_common::TransformMap& floating_joint_values = {}) override final; + void setState(const std::unordered_map& joint_values, + const tesseract_common::TransformMap& floating_joint_values = {}) override final; void setState(const std::vector& joint_names, - const Eigen::Ref& joint_values) override final; + const Eigen::Ref& joint_values, + const tesseract_common::TransformMap& floating_joint_values = {}) override final; - SceneState getState(const Eigen::Ref& joint_values) const override final; - SceneState getState(const std::unordered_map& joint_values) const override final; + SceneState getState(const Eigen::Ref& joint_values, + const tesseract_common::TransformMap& floating_joint_values = {}) const override final; + SceneState getState(const std::unordered_map& joint_values, + const tesseract_common::TransformMap& floating_joint_values = {}) const override final; SceneState getState(const std::vector& joint_names, - const Eigen::Ref& joint_values) const override final; + const Eigen::Ref& joint_values, + 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& joint_values, - const std::string& link_name) const override final; + const std::string& link_name, + const tesseract_common::TransformMap& floating_joint_values = {}) const override final; Eigen::MatrixXd getJacobian(const std::unordered_map& joint_values, - const std::string& link_name) const override final; + const std::string& link_name, + const tesseract_common::TransformMap& floating_joint_values = {}) const override final; Eigen::MatrixXd getJacobian(const std::vector& joint_names, const Eigen::Ref& joint_values, - const std::string& link_name) const override final; + const std::string& link_name, + const tesseract_common::TransformMap& floating_joint_values = {}) const override final; std::vector getJointNames() const override final; + std::vector getFloatingJointNames() const override final; + std::vector getActiveJointNames() const override final; std::string getBaseLinkName() const override final; diff --git a/tesseract_state_solver/include/tesseract_state_solver/ofkt/ofkt_nodes.h b/tesseract_state_solver/include/tesseract_state_solver/ofkt/ofkt_nodes.h index 1d807187f8c..cd892f35296 100644 --- a/tesseract_state_solver/include/tesseract_state_solver/ofkt/ofkt_nodes.h +++ b/tesseract_state_solver/include/tesseract_state_solver/ofkt/ofkt_nodes.h @@ -158,6 +158,28 @@ class OFKTFixedNode : public OFKTBaseNode 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 *****************************/ /*********************************************************************/ diff --git a/tesseract_state_solver/include/tesseract_state_solver/ofkt/ofkt_state_solver.h b/tesseract_state_solver/include/tesseract_state_solver/ofkt/ofkt_state_solver.h index f0602502b0d..388df5f3106 100644 --- a/tesseract_state_solver/include/tesseract_state_solver/ofkt/ofkt_state_solver.h +++ b/tesseract_state_solver/include/tesseract_state_solver/ofkt/ofkt_state_solver.h @@ -77,31 +77,42 @@ class OFKTStateSolver : public MutableStateSolver int getRevision() const override final; - void setState(const Eigen::Ref& joint_values) override final; - void setState(const std::unordered_map& joint_values) override final; + void setState(const Eigen::Ref& joint_values, + const tesseract_common::TransformMap& floating_joint_values = {}) override final; + void setState(const std::unordered_map& joint_values, + const tesseract_common::TransformMap& floating_joint_values = {}) override final; void setState(const std::vector& joint_names, - const Eigen::Ref& joint_values) override final; + const Eigen::Ref& joint_values, + const tesseract_common::TransformMap& floating_joint_values = {}) override final; - SceneState getState(const Eigen::Ref& joint_values) const override final; - SceneState getState(const std::unordered_map& joint_values) const override final; + SceneState getState(const Eigen::Ref& joint_values, + const tesseract_common::TransformMap& floating_joint_values = {}) const override final; + SceneState getState(const std::unordered_map& joint_values, + const tesseract_common::TransformMap& floating_joint_values = {}) const override final; SceneState getState(const std::vector& joint_names, - const Eigen::Ref& joint_values) const override final; + const Eigen::Ref& joint_values, + 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& joint_values, - const std::string& link_name) const override final; + const std::string& link_name, + const tesseract_common::TransformMap& floating_joint_values = {}) const override final; Eigen::MatrixXd getJacobian(const std::unordered_map& joints_values, - const std::string& link_name) const override final; + const std::string& link_name, + const tesseract_common::TransformMap& floating_joint_values = {}) const override final; Eigen::MatrixXd getJacobian(const std::vector& joint_names, const Eigen::Ref& joint_values, - const std::string& link_name) const override final; + const std::string& link_name, + const tesseract_common::TransformMap& floating_joint_values = {}) const override final; std::vector getJointNames() const override final; + std::vector getFloatingJointNames() const override final; + std::vector getActiveJointNames() const override final; std::string getBaseLinkName() const override final; @@ -157,6 +168,7 @@ class OFKTStateSolver : public MutableStateSolver SceneState current_state_; /**< Current state of the scene */ std::vector joint_names_; /**< The link names */ std::vector active_joint_names_; /**< The active joint names */ + std::vector floating_joint_names_; /**< The floating joint names */ std::vector link_names_; /**< The link names */ std::unordered_map> nodes_; /**< The joint name map to node */ std::unordered_map link_map_; /**< The link name map to node */ @@ -199,10 +211,12 @@ class OFKTStateSolver : public MutableStateSolver * @brief Given a set of joint values calculate the jacobian for the provided link_name * @param joints The joint values to calculate the jacobian for * @param link_name The link name to calculate the jacobian for + * @param floating_joint_values The floating joint values * @return The calculated geometric jacobian */ Eigen::MatrixXd calcJacobianHelper(const std::unordered_map& joints, - const std::string& link_name) const; + const std::string& link_name, + const tesseract_common::TransformMap& floating_joint_values = {}) const; /** * @brief A helper function used for cloning the OFKTStateSolver diff --git a/tesseract_state_solver/include/tesseract_state_solver/state_solver.h b/tesseract_state_solver/include/tesseract_state_solver/state_solver.h index fcce945bd42..f21e1e241e1 100644 --- a/tesseract_state_solver/include/tesseract_state_solver/state_solver.h +++ b/tesseract_state_solver/include/tesseract_state_solver/state_solver.h @@ -69,7 +69,8 @@ class StateSolver * @details This must be the same size and order as what is returned by getJointNames * @param joint_values The joint values */ - virtual void setState(const Eigen::Ref& joint_values) = 0; + virtual void setState(const Eigen::Ref& joint_values, + const tesseract_common::TransformMap& floating_joint_values = {}) = 0; /** * @brief Set the current state of the solver @@ -78,16 +79,20 @@ class StateSolver * will update the contact managers transforms * */ - virtual void setState(const std::unordered_map& joint_values) = 0; + virtual void setState(const std::unordered_map& joint_values, + const tesseract_common::TransformMap& floating_joint_values = {}) = 0; virtual void setState(const std::vector& joint_names, - const Eigen::Ref& joint_values) = 0; + const Eigen::Ref& joint_values, + const tesseract_common::TransformMap& floating_joint_values = {}) = 0; /** * @brief Get the state of the solver given the joint values * @details This must be the same size and order as what is returned by getJointNames * @param joint_values The joint values + * @param floating_joint_values The floating joint origin transform */ - virtual SceneState getState(const Eigen::Ref& joint_values) const = 0; + virtual SceneState getState(const Eigen::Ref& joint_values, + const tesseract_common::TransformMap& floating_joint_values = {}) const = 0; /** * @brief Get the state of the scene for a given set or subset of joint values. @@ -95,11 +100,14 @@ class StateSolver * This does not change the internal state of the solver. * * @param joints A map of joint names to joint values to change. + * @param floating_joint_values The floating joint origin transform * @return A the state of the environment */ - virtual SceneState getState(const std::unordered_map& joint_values) const = 0; + virtual SceneState getState(const std::unordered_map& joint_values, + const tesseract_common::TransformMap& floating_joint_values = {}) const = 0; virtual SceneState getState(const std::vector& joint_names, - const Eigen::Ref& joint_values) const = 0; + const Eigen::Ref& joint_values, + const tesseract_common::TransformMap& floating_joint_values = {}) const = 0; /** * @brief Get the current state of the scene @@ -114,7 +122,8 @@ class StateSolver * @param link_name The link name to calculate the jacobian */ virtual Eigen::MatrixXd getJacobian(const Eigen::Ref& joint_values, - const std::string& link_name) const = 0; + const std::string& link_name, + const tesseract_common::TransformMap& floating_joint_values = {}) const = 0; /** * @brief Get the jacobian of the scene for a given set or subset of joint values. @@ -129,10 +138,12 @@ class StateSolver * @return A the state of the environment */ virtual Eigen::MatrixXd getJacobian(const std::unordered_map& joint_values, - const std::string& link_name) const = 0; + const std::string& link_name, + const tesseract_common::TransformMap& floating_joint_values = {}) const = 0; virtual Eigen::MatrixXd getJacobian(const std::vector& joint_names, const Eigen::Ref& joint_values, - const std::string& link_name) const = 0; + const std::string& link_name, + const tesseract_common::TransformMap& floating_joint_values = {}) const = 0; /** * @brief Get the random state of the environment @@ -146,6 +157,12 @@ class StateSolver */ virtual std::vector getJointNames() const = 0; + /** + * @brief Get the vector of floating joint names + * @return A vector of joint names + */ + virtual std::vector getFloatingJointNames() const = 0; + /** * @brief Get the vector of joint names which align with the limits * @return A vector of joint names diff --git a/tesseract_state_solver/src/kdl_state_solver.cpp b/tesseract_state_solver/src/kdl_state_solver.cpp index 7b146c5ca63..d4a113f4f4c 100644 --- a/tesseract_state_solver/src/kdl_state_solver.cpp +++ b/tesseract_state_solver/src/kdl_state_solver.cpp @@ -73,7 +73,8 @@ KDLStateSolver& KDLStateSolver::operator=(const KDLStateSolver& other) return *this; } -void KDLStateSolver::setState(const Eigen::Ref& joint_values) +void KDLStateSolver::setState(const Eigen::Ref& joint_values, + const tesseract_common::TransformMap& /*floating_joint_values*/) { assert(static_cast(data_.active_joint_names.size()) == joint_values.size()); for (auto i = 0U; i < data_.active_joint_names.size(); ++i) @@ -85,7 +86,8 @@ void KDLStateSolver::setState(const Eigen::Ref& joint_val calculateTransforms(current_state_, kdl_jnt_array_, data_.tree.getRootSegment(), Eigen::Isometry3d::Identity()); } -void KDLStateSolver::setState(const std::unordered_map& joint_values) +void KDLStateSolver::setState(const std::unordered_map& joint_values, + const tesseract_common::TransformMap& /*floating_joint_values*/) { for (const auto& joint : joint_values) { @@ -97,7 +99,8 @@ void KDLStateSolver::setState(const std::unordered_map& joi } void KDLStateSolver::setState(const std::vector& joint_names, - const Eigen::Ref& joint_values) + const Eigen::Ref& joint_values, + const tesseract_common::TransformMap& /*floating_joint_values*/) { assert(static_cast(joint_names.size()) == joint_values.size()); for (auto i = 0U; i < joint_names.size(); ++i) @@ -109,7 +112,8 @@ void KDLStateSolver::setState(const std::vector& joint_names, calculateTransforms(current_state_, kdl_jnt_array_, data_.tree.getRootSegment(), Eigen::Isometry3d::Identity()); } -SceneState KDLStateSolver::getState(const Eigen::Ref& joint_values) const +SceneState KDLStateSolver::getState(const Eigen::Ref& joint_values, + const tesseract_common::TransformMap& /*floating_joint_values*/) const { assert(static_cast(data_.active_joint_names.size()) == joint_values.size()); SceneState state{ current_state_ }; @@ -126,7 +130,8 @@ SceneState KDLStateSolver::getState(const Eigen::Ref& joi return state; } -SceneState KDLStateSolver::getState(const std::unordered_map& joint_values) const +SceneState KDLStateSolver::getState(const std::unordered_map& joint_values, + const tesseract_common::TransformMap& /*floating_joint_values*/) const { SceneState state{ current_state_ }; KDL::JntArray jnt_array = kdl_jnt_array_; @@ -144,7 +149,8 @@ SceneState KDLStateSolver::getState(const std::unordered_map& joint_names, - const Eigen::Ref& joint_values) const + const Eigen::Ref& joint_values, + const tesseract_common::TransformMap& /*floating_joint_values*/) const { SceneState state{ current_state_ }; KDL::JntArray jnt_array = kdl_jnt_array_; @@ -170,7 +176,8 @@ SceneState KDLStateSolver::getRandomState() const } Eigen::MatrixXd KDLStateSolver::getJacobian(const Eigen::Ref& joint_values, - const std::string& link_name) const + const std::string& link_name, + const tesseract_common::TransformMap& /*floating_joint_values*/) const { assert(joint_values.size() == data_.tree.getNrOfJoints()); KDL::JntArray kdl_joint_vals = getKDLJntArray(data_.active_joint_names, joint_values); @@ -182,7 +189,8 @@ Eigen::MatrixXd KDLStateSolver::getJacobian(const Eigen::Ref& joint_values, - const std::string& link_name) const + const std::string& link_name, + const tesseract_common::TransformMap& /*floating_joint_values*/) const { KDL::JntArray kdl_joint_vals = getKDLJntArray(joint_values); KDL::Jacobian kdl_jacobian; @@ -194,7 +202,8 @@ Eigen::MatrixXd KDLStateSolver::getJacobian(const std::unordered_map& joint_names, const Eigen::Ref& joint_values, - const std::string& link_name) const + const std::string& link_name, + const tesseract_common::TransformMap& /*floating_joint_values*/) const { KDL::JntArray kdl_joint_vals = getKDLJntArray(joint_names, joint_values); KDL::Jacobian kdl_jacobian; @@ -206,6 +215,8 @@ Eigen::MatrixXd KDLStateSolver::getJacobian(const std::vector& join std::vector KDLStateSolver::getJointNames() const { return data_.joint_names; } +std::vector KDLStateSolver::getFloatingJointNames() const { return {}; } + std::vector KDLStateSolver::getActiveJointNames() const { return data_.active_joint_names; } std::string KDLStateSolver::getBaseLinkName() const { return data_.base_link_name; } diff --git a/tesseract_state_solver/src/ofkt_nodes.cpp b/tesseract_state_solver/src/ofkt_nodes.cpp index d59f5820345..2e8062e3784 100644 --- a/tesseract_state_solver/src/ofkt_nodes.cpp +++ b/tesseract_state_solver/src/ofkt_nodes.cpp @@ -189,6 +189,43 @@ void OFKTFixedNode::computeAndStoreLocalTransformation() {} Eigen::Isometry3d OFKTFixedNode::computeLocalTransformation(double /*joint_value*/) const { return local_tf_; } +/**********************************************************************/ +/************************* FLOATING NODE ******************************/ +/**********************************************************************/ +OFKTFloatingNode::OFKTFloatingNode(OFKTNode* parent, + std::string link_name, + std::string joint_name, + const Eigen::Isometry3d& static_tf) + : OFKTBaseNode(tesseract_scene_graph::JointType::FLOATING, + parent, + std::move(link_name), + std::move(joint_name), + static_tf) +{ + OFKTBaseNode::computeAndStoreWorldTransformation(); +} + +void OFKTFloatingNode::storeJointValue(double /*joint_value*/) +{ + throw std::runtime_error("OFKTFloatingNode: does not have a joint value!"); +} + +double OFKTFloatingNode::getJointValue() const +{ + throw std::runtime_error("OFKTFloatingNode: does not have a joint value!"); +} + +void OFKTFloatingNode::setStaticTransformation(const Eigen::Isometry3d& static_tf) +{ + static_tf_ = static_tf; + local_tf_ = static_tf; + update_world_required_ = true; +} + +void OFKTFloatingNode::computeAndStoreLocalTransformation() {} + +Eigen::Isometry3d OFKTFloatingNode::computeLocalTransformation(double /*joint_value*/) const { return local_tf_; } + /*********************************************************************/ /************************* REVOLUTE NODE *****************************/ /*********************************************************************/ diff --git a/tesseract_state_solver/src/ofkt_state_solver.cpp b/tesseract_state_solver/src/ofkt_state_solver.cpp index c9dc15f3470..072ac010b97 100644 --- a/tesseract_state_solver/src/ofkt_state_solver.cpp +++ b/tesseract_state_solver/src/ofkt_state_solver.cpp @@ -162,6 +162,7 @@ OFKTStateSolver& OFKTStateSolver::operator=(const OFKTStateSolver& other) current_state_ = other.current_state_; joint_names_ = other.joint_names_; active_joint_names_ = other.active_joint_names_; + floating_joint_names_ = other.floating_joint_names_; link_names_ = other.link_names_; root_ = std::make_unique(other.root_->getLinkName()); link_map_[other.root_->getLinkName()] = root_.get(); @@ -196,6 +197,7 @@ void OFKTStateSolver::clear() current_state_ = SceneState(); joint_names_.clear(); active_joint_names_.clear(); + floating_joint_names_.clear(); link_names_.clear(); nodes_.clear(); link_map_.clear(); @@ -203,21 +205,28 @@ void OFKTStateSolver::clear() root_ = nullptr; } -void OFKTStateSolver::setState(const Eigen::Ref& joint_values) +void OFKTStateSolver::setState(const Eigen::Ref& joint_values, + const tesseract_common::TransformMap& floating_joint_values) { std::unique_lock lock(mutex_); assert(active_joint_names_.size() == static_cast(joint_values.size())); - Eigen::VectorXd jv = joint_values; for (std::size_t i = 0; i < active_joint_names_.size(); ++i) { nodes_[active_joint_names_[i]]->storeJointValue(joint_values(static_cast(i))); current_state_.joints[active_joint_names_[i]] = joint_values(static_cast(i)); } + for (const auto& floating_joint_value : floating_joint_values) + { + current_state_.floating_joints.at(floating_joint_value.first) = floating_joint_value.second; + nodes_[floating_joint_value.first]->setStaticTransformation(floating_joint_value.second); + } + update(root_.get(), false); } -void OFKTStateSolver::setState(const std::unordered_map& joint_values) +void OFKTStateSolver::setState(const std::unordered_map& joint_values, + const tesseract_common::TransformMap& floating_joint_values) { std::unique_lock lock(mutex_); @@ -227,11 +236,18 @@ void OFKTStateSolver::setState(const std::unordered_map& jo current_state_.joints[joint.first] = joint.second; } + for (const auto& floating_joint_value : floating_joint_values) + { + current_state_.floating_joints.at(floating_joint_value.first) = floating_joint_value.second; + nodes_[floating_joint_value.first]->setStaticTransformation(floating_joint_value.second); + } + update(root_.get(), false); } void OFKTStateSolver::setState(const std::vector& joint_names, - const Eigen::Ref& joint_values) + const Eigen::Ref& joint_values, + const tesseract_common::TransformMap& floating_joint_values) { std::unique_lock lock(mutex_); assert(joint_names.size() == static_cast(joint_values.size())); @@ -242,10 +258,17 @@ void OFKTStateSolver::setState(const std::vector& joint_names, current_state_.joints[joint_names[i]] = joint_values(static_cast(i)); } + for (const auto& floating_joint_value : floating_joint_values) + { + current_state_.floating_joints.at(floating_joint_value.first) = floating_joint_value.second; + nodes_[floating_joint_value.first]->setStaticTransformation(floating_joint_value.second); + } + update(root_.get(), false); } -SceneState OFKTStateSolver::getState(const Eigen::Ref& joint_values) const +SceneState OFKTStateSolver::getState(const Eigen::Ref& joint_values, + const tesseract_common::TransformMap& floating_joint_values) const { std::shared_lock lock(mutex_); assert(static_cast(active_joint_names_.size()) == joint_values.size()); @@ -253,22 +276,30 @@ SceneState OFKTStateSolver::getState(const Eigen::Ref& jo for (std::size_t i = 0; i < active_joint_names_.size(); ++i) state.joints[active_joint_names_[i]] = joint_values[static_cast(i)]; + for (const auto& floating_joint_value : floating_joint_values) + state.floating_joints.at(floating_joint_value.first) = floating_joint_value.second; + update(state, root_.get(), Eigen::Isometry3d::Identity(), false); return state; } -SceneState OFKTStateSolver::getState(const std::unordered_map& joint_values) const +SceneState OFKTStateSolver::getState(const std::unordered_map& joint_values, + const tesseract_common::TransformMap& floating_joint_values) const { auto state = SceneState(current_state_); for (const auto& joint : joint_values) state.joints[joint.first] = joint.second; + for (const auto& floating_joint_value : floating_joint_values) + state.floating_joints.at(floating_joint_value.first) = floating_joint_value.second; + update(state, root_.get(), Eigen::Isometry3d::Identity(), false); return state; } SceneState OFKTStateSolver::getState(const std::vector& joint_names, - const Eigen::Ref& joint_values) const + const Eigen::Ref& joint_values, + const tesseract_common::TransformMap& floating_joint_values) const { std::shared_lock lock(mutex_); assert(static_cast(joint_names.size()) == joint_values.size()); @@ -276,6 +307,9 @@ SceneState OFKTStateSolver::getState(const std::vector& joint_names for (std::size_t i = 0; i < joint_names.size(); ++i) state.joints[joint_names[i]] = joint_values[static_cast(i)]; + for (const auto& floating_joint_value : floating_joint_values) + state.floating_joints.at(floating_joint_value.first) = floating_joint_value.second; + update(state, root_.get(), Eigen::Isometry3d::Identity(), false); return state; } @@ -293,41 +327,57 @@ SceneState OFKTStateSolver::getRandomState() const } Eigen::MatrixXd OFKTStateSolver::getJacobian(const Eigen::Ref& joint_values, - const std::string& link_name) const + const std::string& link_name, + const tesseract_common::TransformMap& floating_joint_values) const { std::shared_lock lock(mutex_); std::unordered_map joints = current_state_.joints; for (Eigen::Index i = 0; i < joint_values.rows(); ++i) joints[active_joint_names_[static_cast(i)]] = joint_values[i]; - return calcJacobianHelper(joints, link_name); + tesseract_common::TransformMap floating_joints{ current_state_.floating_joints }; + for (const auto& floating_joint_value : floating_joint_values) + floating_joints.at(floating_joint_value.first) = floating_joint_value.second; + + return calcJacobianHelper(joints, link_name, floating_joints); } Eigen::MatrixXd OFKTStateSolver::getJacobian(const std::unordered_map& joints_values, - const std::string& link_name) const + const std::string& link_name, + const tesseract_common::TransformMap& floating_joint_values) const { std::shared_lock lock(mutex_); std::unordered_map joints = current_state_.joints; for (const auto& joint : joints_values) joints[joint.first] = joint.second; - return calcJacobianHelper(joints, link_name); + tesseract_common::TransformMap floating_joints{ current_state_.floating_joints }; + for (const auto& floating_joint_value : floating_joint_values) + floating_joints.at(floating_joint_value.first) = floating_joint_value.second; + + return calcJacobianHelper(joints, link_name, floating_joints); } Eigen::MatrixXd OFKTStateSolver::getJacobian(const std::vector& joint_names, const Eigen::Ref& joint_values, - const std::string& link_name) const + const std::string& link_name, + const tesseract_common::TransformMap& floating_joint_values) const { std::shared_lock lock(mutex_); std::unordered_map joints = current_state_.joints; for (Eigen::Index i = 0; i < joint_values.rows(); ++i) joints[joint_names[static_cast(i)]] = joint_values[i]; - return calcJacobianHelper(joints, link_name); + tesseract_common::TransformMap floating_joints{ current_state_.floating_joints }; + for (const auto& floating_joint_value : floating_joint_values) + floating_joints.at(floating_joint_value.first) = floating_joint_value.second; + + return calcJacobianHelper(joints, link_name, floating_joints); } Eigen::MatrixXd OFKTStateSolver::calcJacobianHelper(const std::unordered_map& joints, - const std::string& link_name) const + const std::string& link_name, + const tesseract_common::TransformMap& floating_joint_values) const { OFKTNode* node = link_map_.at(link_name); Eigen::MatrixXd jacobian = Eigen::MatrixXd::Zero(6, static_cast(active_joint_names_.size())); @@ -335,10 +385,14 @@ Eigen::MatrixXd OFKTStateSolver::calcJacobianHelper(const std::unordered_mapgetType() == JointType::FIXED || node->getType() == JointType::FLOATING) + if (node->getType() == JointType::FIXED) { total_tf = node->getLocalTransformation() * total_tf; } + else if (node->getType() == JointType::FLOATING) + { + total_tf = floating_joint_values.at(node->getJointName()) * total_tf; + } else { Eigen::Isometry3d local_tf = node->computeLocalTransformation(joints.at(node->getJointName())); @@ -366,6 +420,12 @@ std::vector OFKTStateSolver::getJointNames() const return joint_names_; } +std::vector OFKTStateSolver::getFloatingJointNames() const +{ + std::shared_lock lock(mutex_); + return floating_joint_names_; +} + std::vector OFKTStateSolver::getActiveJointNames() const { std::shared_lock lock(mutex_); @@ -834,6 +894,17 @@ void OFKTStateSolver::update(SceneState& state, { Eigen::Isometry3d updated_parent_world_tf{ Eigen::Isometry3d::Identity() }; if (node->getType() != tesseract_scene_graph::JointType::FIXED) + { + updated_parent_world_tf = parent_world_tf * node->getLocalTransformation(); + } + else if (node->getType() != tesseract_scene_graph::JointType::FLOATING) + { + const auto& tf = state.floating_joints.at(node->getJointName()); + updated_parent_world_tf = parent_world_tf * tf; + if (!tf.isApprox(node->getLocalTransformation(), 1e-8)) + update_required = true; + } + else { double jv = state.joints[node->getJointName()]; if (!tesseract_common::almostEqualRelativeAndAbs(node->getJointValue(), jv, 1e-8)) @@ -846,10 +917,6 @@ void OFKTStateSolver::update(SceneState& state, updated_parent_world_tf = parent_world_tf * node->getLocalTransformation(); } } - else - { - updated_parent_world_tf = parent_world_tf * node->getLocalTransformation(); - } if (update_required) { @@ -931,6 +998,7 @@ void OFKTStateSolver::moveLinkHelper(std::vector& removed_ removed_joints.end()); }), joint_names_.end()); + + floating_joint_names_.erase(std::remove_if(floating_joint_names_.begin(), + floating_joint_names_.end(), + [removed_joints](const std::string& joint_name) { + return (std::find(removed_joints.begin(), + removed_joints.end(), + joint_name) != removed_joints.end()); + }), + floating_joint_names_.end()); } if (!removed_active_joints.empty()) @@ -1105,6 +1182,22 @@ void OFKTStateSolver::addNode(const tesseract_scene_graph::Joint& joint, nodes_[joint_name] = std::move(n); break; } + case tesseract_scene_graph::JointType::FLOATING: + { + OFKTNode* parent_node = link_map_[parent_link_name]; + assert(parent_node != nullptr); + auto n = std::make_unique( + parent_node, child_link_name, joint_name, joint.parent_to_joint_origin_transform); + link_map_[child_link_name] = n.get(); + parent_node->addChild(n.get()); + current_state_.link_transforms[n->getLinkName()] = n->getWorldTransformation(); + current_state_.joint_transforms[n->getJointName()] = n->getWorldTransformation(); + joint_names_.push_back(joint_name); + floating_joint_names_.push_back(joint_name); + link_names_.push_back(n->getLinkName()); + nodes_[joint_name] = std::move(n); + break; + } // LCOV_EXCL_START default: { @@ -1132,10 +1225,11 @@ void OFKTStateSolver::removeNode(OFKTNode* node, current_state_.link_transforms.erase(node->getLinkName()); current_state_.joints.erase(node->getJointName()); + current_state_.floating_joints.erase(node->getJointName()); current_state_.joint_transforms.erase(node->getJointName()); std::vector children = node->getChildren(); - for (auto* child : node->getChildren()) + for (auto* child : children) removeNode(child, removed_links, removed_joints, removed_active_joints, removed_active_joints_indices); if (node->getParent() != nullptr)