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_kinematics/core/src/joint_group.cpp b/tesseract_kinematics/core/src/joint_group.cpp index 03f17545cfe..fd7781561c0 100644 --- a/tesseract_kinematics/core/src/joint_group.cpp +++ b/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(scene_graph, data); jacobian_map_.reserve(joint_names_.size()); std::vector solver_jn = state_solver_->getActiveJointNames(); diff --git a/tesseract_scene_graph/include/tesseract_scene_graph/kdl_parser.h b/tesseract_scene_graph/include/tesseract_scene_graph/kdl_parser.h index 64fe6bc0a57..e69e2dfbe7a 100644 --- a/tesseract_scene_graph/include/tesseract_scene_graph/kdl_parser.h +++ b/tesseract_scene_graph/include/tesseract_scene_graph/kdl_parser.h @@ -49,6 +49,8 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include TESSERACT_COMMON_IGNORE_WARNINGS_POP +#include + namespace tesseract_scene_graph { class SceneGraph; @@ -122,13 +124,20 @@ KDL::RigidBodyInertia convert(const std::shared_ptr& 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 joint_names; std::vector active_joint_names; + std::vector floating_joint_names; std::vector link_names; std::vector active_link_names; std::vector 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& joint_names, - const std::unordered_map& joint_values); + const std::unordered_map& joint_values, + const tesseract_common::TransformMap& floating_joint_values); } // namespace tesseract_scene_graph 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/kdl_parser.cpp b/tesseract_scene_graph/src/kdl_parser.cpp index b71fe5ce5a1..63ad237e409 100644 --- a/tesseract_scene_graph/src/kdl_parser.cpp +++ b/tesseract_scene_graph/src/kdl_parser.cpp @@ -43,6 +43,8 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include #include +#include +#include namespace tesseract_scene_graph { @@ -136,6 +138,7 @@ KDL::Joint convert(const std::shared_ptr& joint) switch (joint->type) { case JointType::FIXED: + case JointType::FLOATING: { return KDL::Joint(name, KDL::Joint::None); } @@ -190,6 +193,28 @@ KDL::RigidBodyInertia convert(const std::shared_ptr& inertial) return KDL::RigidBodyInertia(kdl_mass, kdl_com, kdl_inertia_wrt_com); } +bool KDLTreeData::operator==(const KDLTreeData& rhs) const +{ + bool equal = true; + equal &= (base_link_name == rhs.base_link_name); + equal &= (joint_names == rhs.joint_names); + equal &= (active_joint_names == rhs.active_joint_names); + equal &= (floating_joint_names == rhs.floating_joint_names); + equal &= (link_names == rhs.link_names); + equal &= (active_link_names == rhs.active_link_names); + equal &= (static_link_names == rhs.static_link_names); + + auto isometry_equal = [](const Eigen::Isometry3d& iso_1, const Eigen::Isometry3d& iso_2) { + return iso_1.isApprox(iso_2, 1e-5); + }; + equal &= tesseract_common::isIdenticalMap( + floating_joint_values, rhs.floating_joint_values, isometry_equal); + + return equal; +} + +bool KDLTreeData::operator!=(const KDLTreeData& rhs) const { return !operator==(rhs); } + /** * @brief Every time a vertex is visited for the first time add a new * segment to the KDL Tree; @@ -219,6 +244,7 @@ struct kdl_tree_builder : public boost::dfs_visitor<> data_.static_link_names.reserve(num_v); data_.joint_names.reserve(num_e); data_.active_joint_names.reserve(num_e); + data_.floating_joint_names.reserve(num_e); data_.link_names.push_back(link->getName()); data_.static_link_names.push_back(link->getName()); @@ -234,6 +260,12 @@ struct kdl_tree_builder : public boost::dfs_visitor<> const Joint::ConstPtr& parent_joint = boost::get(boost::edge_joint, graph)[e]; data_.joint_names.push_back(parent_joint->getName()); + if (parent_joint->type == JointType::FLOATING) + { + data_.floating_joint_names.push_back(parent_joint->getName()); + data_.floating_joint_values[parent_joint->getName()] = parent_joint->parent_to_joint_origin_transform; + } + KDL::Joint kdl_jnt = convert(parent_joint); if (kdl_jnt.getType() != KDL::Joint::None) { @@ -269,9 +301,12 @@ struct kdl_sub_tree_builder : public boost::dfs_visitor<> { kdl_sub_tree_builder(KDLTreeData& data, const std::vector& joint_names, - const std::unordered_map& joint_values) - : data_(data), joint_names_(joint_names), joint_values_(joint_values) + const std::unordered_map& joint_values, + const tesseract_common::TransformMap& floating_joint_values) + : data_(data), joint_names_(joint_names), joint_values_(joint_values), floating_joint_values_(floating_joint_values) { + for (const auto& floating_joint_value : floating_joint_values) + data_.floating_joint_values.at(floating_joint_value.first) = floating_joint_value.second; } template @@ -310,15 +345,18 @@ struct kdl_sub_tree_builder : public boost::dfs_visitor<> const Joint::ConstPtr& parent_joint = boost::get(boost::edge_joint, graph)[e]; bool found = (std::find(joint_names_.begin(), joint_names_.end(), parent_joint->getName()) != joint_names_.end()); KDL::Joint kdl_jnt = convert(parent_joint); - KDL::Frame parent_to_joint = convert(parent_joint->parent_to_joint_origin_transform); + KDL::Frame parent_to_joint = (parent_joint->type == JointType::FLOATING) ? + convert(data_.floating_joint_values.at(parent_joint->getName())) : + convert(parent_joint->parent_to_joint_origin_transform); + KDL::Segment kdl_sgm(link->getName(), kdl_jnt, parent_to_joint, inert); std::string parent_link_name = parent_joint->parent_link_name; - if (parent_joint->type != JointType::FIXED) + if (parent_joint->type == JointType::FIXED || parent_joint->type == JointType::FLOATING) + segment_transforms_[parent_joint->child_link_name] = segment_transforms_[parent_link_name] * kdl_sgm.pose(0.0); + else segment_transforms_[parent_joint->child_link_name] = segment_transforms_[parent_link_name] * kdl_sgm.pose(joint_values_.at(parent_joint->getName())); - else - segment_transforms_[parent_joint->child_link_name] = segment_transforms_[parent_link_name] * kdl_sgm.pose(0.0); if (!started_ && found) { @@ -376,10 +414,10 @@ struct kdl_sub_tree_builder : public boost::dfs_visitor<> } else if (it != link_names_.end() && !found) { - if (parent_joint->type != JointType::FIXED) - parent_to_joint = kdl_sgm.pose(joint_values_.at(parent_joint->getName())); - else + if (parent_joint->type == JointType::FIXED || parent_joint->type == JointType::FLOATING) parent_to_joint = kdl_sgm.pose(0.0); + else + parent_to_joint = kdl_sgm.pose(joint_values_.at(parent_joint->getName())); kdl_jnt = KDL::Joint(parent_joint->getName(), KDL::Joint::None); } @@ -413,6 +451,7 @@ struct kdl_sub_tree_builder : public boost::dfs_visitor<> const std::vector& joint_names_; const std::unordered_map& joint_values_; + const tesseract_common::TransformMap& floating_joint_values_; }; KDLTreeData parseSceneGraph(const SceneGraph& scene_graph) @@ -457,7 +496,8 @@ KDLTreeData parseSceneGraph(const SceneGraph& scene_graph) KDLTreeData parseSceneGraph(const SceneGraph& scene_graph, const std::vector& joint_names, - const std::unordered_map& joint_values) + const std::unordered_map& joint_values, + const tesseract_common::TransformMap& floating_joint_values) { if (!scene_graph.isTree()) throw std::runtime_error("parseSubSceneGraph: currently only works if the scene graph is a tree."); @@ -465,7 +505,7 @@ KDLTreeData parseSceneGraph(const SceneGraph& scene_graph, KDLTreeData data; data.tree = KDL::Tree(scene_graph.getRoot()); - kdl_sub_tree_builder builder(data, joint_names, joint_values); + kdl_sub_tree_builder builder(data, joint_names, joint_values, floating_joint_values); std::map index_map; boost::associative_property_map> prop_index_map(index_map); 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_scene_graph/test/tesseract_scene_graph_unit.cpp b/tesseract_scene_graph/test/tesseract_scene_graph_unit.cpp index 9d4f9762be5..1bc7d64a270 100644 --- a/tesseract_scene_graph/test/tesseract_scene_graph_unit.cpp +++ b/tesseract_scene_graph/test/tesseract_scene_graph_unit.cpp @@ -1238,7 +1238,7 @@ TEST(TesseractSceneGraphUnit, LoadSubKDLUnit) // NOLINT { KDLTreeData full_data = parseSceneGraph(g); - KDLTreeData data = parseSceneGraph(g, sub_joint_names, joint_values); + KDLTreeData data = parseSceneGraph(g, sub_joint_names, joint_values, tesseract_common::TransformMap()); EXPECT_TRUE(tesseract_common::isIdentical(data.link_names, link_names, false)); EXPECT_TRUE(tesseract_common::isIdentical(data.static_link_names, static_link_names, false)); @@ -1269,7 +1269,7 @@ TEST(TesseractSceneGraphUnit, LoadSubKDLUnit) // NOLINT { KDLTreeData full_data = parseSceneGraph(g); - KDLTreeData data = parseSceneGraph(*g_clone, sub_joint_names, joint_values); + KDLTreeData data = parseSceneGraph(*g_clone, sub_joint_names, joint_values, tesseract_common::TransformMap()); EXPECT_TRUE(tesseract_common::isIdentical(data.link_names, link_names, false)); EXPECT_TRUE(tesseract_common::isIdentical(data.static_link_names, static_link_names, false)); 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..0556065d192 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 data_.floating_joint_names; } + std::vector KDLStateSolver::getActiveJointNames() const { return data_.active_joint_names; } std::string KDLStateSolver::getBaseLinkName() const { return data_.base_link_name; } @@ -253,6 +264,7 @@ tesseract_common::KinematicLimits KDLStateSolver::getLimits() const { return lim bool KDLStateSolver::processKDLData(const tesseract_scene_graph::SceneGraph& scene_graph) { current_state_ = SceneState(); + current_state_.floating_joints = data_.floating_joint_values; kdl_jnt_array_.resize(data_.tree.getNrOfJoints()); limits_.joint_limits.resize(static_cast(data_.tree.getNrOfJoints()), 2); limits_.velocity_limits.resize(static_cast(data_.tree.getNrOfJoints()), 2); 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..a6fa9b0784e 100644 --- a/tesseract_state_solver/src/ofkt_state_solver.cpp +++ b/tesseract_state_solver/src/ofkt_state_solver.cpp @@ -91,6 +91,14 @@ void OFKTStateSolver::cloneHelper(OFKTStateSolver& cloned, const OFKTNode* node) parent_node->addChild(n.get()); cloned.nodes_[child->getJointName()] = std::move(n); } + else if (child->getType() == tesseract_scene_graph::JointType::FLOATING) + { + auto n = std::make_unique( + parent_node, child->getLinkName(), child->getJointName(), child->getStaticTransformation()); + cloned.link_map_[child->getLinkName()] = n.get(); + parent_node->addChild(n.get()); + cloned.nodes_[child->getJointName()] = std::move(n); + } else if (child->getType() == tesseract_scene_graph::JointType::REVOLUTE) { const auto* cn = static_cast(child); @@ -162,6 +170,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 +205,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 +213,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 +244,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 +266,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 +284,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 +315,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 +335,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 +393,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 +428,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_); @@ -633,6 +701,8 @@ bool OFKTStateSolver::changeJointOrigin(const std::string& name, const Eigen::Is } it->second->setStaticTransformation(new_origin); + if (it->second->getType() == JointType::FLOATING) + current_state_.floating_joints[name] = new_origin; update(root_.get(), false); @@ -833,7 +903,18 @@ void OFKTStateSolver::update(SceneState& state, bool update_required) const { Eigen::Isometry3d updated_parent_world_tf{ Eigen::Isometry3d::Identity() }; - if (node->getType() != tesseract_scene_graph::JointType::FIXED) + 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 +927,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 +1008,7 @@ void OFKTStateSolver::moveLinkHelper(std::vectorgetParent()->removeChild(n.get()); n->setStaticTransformation(joint.parent_to_joint_origin_transform); + if (n->getType() == JointType::FLOATING) + current_state_.floating_joints[joint.getName()] = joint.parent_to_joint_origin_transform; + OFKTNode* new_parent = link_map_[joint.parent_link_name]; n->setParent(new_parent); new_parent->addChild(n.get()); @@ -991,6 +1072,15 @@ void OFKTStateSolver::removeJointHelper(const 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 +1195,23 @@ 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(); + current_state_.floating_joints[n->getJointName()] = n->getLocalTransformation(); + 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 +1239,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) diff --git a/tesseract_state_solver/test/state_solver_test_suite.h b/tesseract_state_solver/test/state_solver_test_suite.h index 9a71f9adc38..84f762b3074 100644 --- a/tesseract_state_solver/test/state_solver_test_suite.h +++ b/tesseract_state_solver/test/state_solver_test_suite.h @@ -68,6 +68,11 @@ inline void runCompareSceneStates(const SceneState& base_state, const SceneState EXPECT_NEAR(pair.second, compare_state.joints.at(pair.first), 1e-6); } + for (const auto& pair : base_state.floating_joints) + { + EXPECT_TRUE(pair.second.isApprox(compare_state.floating_joints.at(pair.first), 1e-6)); + } + for (const auto& pair : base_state.joint_transforms) { EXPECT_TRUE(pair.second.isApprox(compare_state.joint_transforms.at(pair.first), 1e-6)); @@ -88,6 +93,8 @@ inline void runCompareStateSolver(const StateSolver& base_solver, StateSolver& c EXPECT_TRUE(tesseract_common::isIdentical(base_solver.getLinkNames(), comp_solver.getLinkNames(), false)); EXPECT_TRUE(tesseract_common::isIdentical(base_solver.getActiveLinkNames(), comp_solver.getActiveLinkNames(), false)); EXPECT_TRUE(tesseract_common::isIdentical(base_solver.getStaticLinkNames(), comp_solver.getStaticLinkNames(), false)); + EXPECT_TRUE( + tesseract_common::isIdentical(base_solver.getFloatingJointNames(), comp_solver.getFloatingJointNames(), false)); for (const auto& active_link_name : base_solver.getActiveLinkNames()) { @@ -767,7 +774,7 @@ void runAddandRemoveLinkTest() joint_2.parent_to_joint_origin_transform.translation()(0) = 1.25; joint_2.parent_link_name = link_name1; joint_2.child_link_name = link_name2; - joint_2.type = JointType::FIXED; + joint_2.type = JointType::FLOATING; // Test adding link @@ -791,6 +798,7 @@ void runAddandRemoveLinkTest() EXPECT_TRUE(state.joint_transforms.find(joint_name1) != state.joint_transforms.end()); // Fixed joints are not listed EXPECT_TRUE(state.joints.find(joint_name1) == state.joints.end()); + EXPECT_TRUE(state.floating_joints.empty()); EXPECT_TRUE(scene_graph->addLink(link_2, joint_2)); EXPECT_TRUE(state_solver.addLink(link_2, joint_2)); @@ -810,6 +818,7 @@ void runAddandRemoveLinkTest() EXPECT_TRUE(std::find(joint_names.begin(), joint_names.end(), joint_name2) == joint_names.end()); EXPECT_TRUE(state.link_transforms.find(link_name2) != state.link_transforms.end()); EXPECT_TRUE(state.joint_transforms.find(joint_name2) != state.joint_transforms.end()); + EXPECT_TRUE(state.floating_joints.find(joint_name2) != state.floating_joints.end()); // Fixed joints are not listed EXPECT_TRUE(state.joints.find(joint_name2) == state.joints.end()); @@ -837,6 +846,8 @@ void runAddandRemoveLinkTest() EXPECT_TRUE(state.link_transforms.find(link_name2) == state.link_transforms.end()); EXPECT_TRUE(state.joint_transforms.find(joint_name2) == state.joint_transforms.end()); EXPECT_TRUE(state.joints.find(joint_name2) == state.joints.end()); + EXPECT_TRUE(state.floating_joints.find(joint_name2) == state.floating_joints.end()); + EXPECT_TRUE(state.floating_joints.empty()); scene_graph->saveDOT(tesseract_common::getTempPath() + "state_solver_after_remove_link_unit.dot"); @@ -917,6 +928,7 @@ void runAddandRemoveLinkTest() EXPECT_TRUE(std::find(joint_names.begin(), joint_names.end(), joint_name2) == joint_names.end()); EXPECT_TRUE(state.link_transforms.find(link_name2) != state.link_transforms.end()); EXPECT_TRUE(state.joint_transforms.find(joint_name2) != state.joint_transforms.end()); + EXPECT_TRUE(state.floating_joints.find(joint_name2) != state.floating_joints.end()); // Fixed joints are not listed EXPECT_TRUE(state.joints.find(joint_name2) == state.joints.end()); @@ -943,7 +955,9 @@ void runAddandRemoveLinkTest() EXPECT_TRUE(state.joints.find(joint_name1) == state.joints.end()); EXPECT_TRUE(state.link_transforms.find(link_name2) == state.link_transforms.end()); EXPECT_TRUE(state.joint_transforms.find(joint_name2) == state.joint_transforms.end()); + EXPECT_TRUE(state.floating_joints.find(joint_name2) == state.floating_joints.end()); EXPECT_TRUE(state.joints.find(joint_name2) == state.joints.end()); + EXPECT_TRUE(state.floating_joints.empty()); scene_graph->saveDOT(tesseract_common::getTempPath() + "state_solver_after_remove_link_unit2.dot"); @@ -1019,7 +1033,7 @@ void runAddSceneGraphTest() Joint joint_1_empty("provided_subgraph_joint"); joint_1_empty.parent_link_name = "base_link"; joint_1_empty.child_link_name = "prefix_subgraph_base_link"; - joint_1_empty.type = JointType::FIXED; + joint_1_empty.type = JointType::FLOATING; EXPECT_FALSE(scene_graph->insertSceneGraph(*subgraph, joint_1_empty)); EXPECT_FALSE(state_solver.insertSceneGraph(*subgraph, joint_1_empty)); @@ -1079,7 +1093,7 @@ void runAddSceneGraphTest() Joint prefix_joint(prefix + subgraph_joint_name); prefix_joint.parent_link_name = scene_graph->getRoot(); prefix_joint.child_link_name = prefix + subgraph->getRoot(); - prefix_joint.type = JointType::FIXED; + prefix_joint.type = JointType::FLOATING; EXPECT_TRUE(scene_graph->insertSceneGraph(*subgraph, prefix_joint, prefix)); EXPECT_TRUE(state_solver.insertSceneGraph(*subgraph, prefix_joint, prefix)); @@ -1098,6 +1112,7 @@ void runAddSceneGraphTest() EXPECT_TRUE(std::find(joint_names.begin(), joint_names.end(), prefix + subgraph_joint_name) == joint_names.end()); EXPECT_TRUE(state.link_transforms.find(prefix + subgraph->getRoot()) != state.link_transforms.end()); EXPECT_TRUE(state.joint_transforms.find(prefix + subgraph_joint_name) != state.joint_transforms.end()); + EXPECT_TRUE(state.floating_joints.find(prefix + subgraph_joint_name) != state.floating_joints.end()); EXPECT_TRUE(state.joints.find(prefix + subgraph_joint_name) == state.joints.end()); // Add subgraph with prefix and joint @@ -1167,6 +1182,19 @@ void runChangeJointOriginTest() EXPECT_TRUE(scene_graph->addLink(link_1, joint_1)); EXPECT_TRUE(state_solver.addLink(link_1, joint_1)); + const std::string link_name2 = "link_n2"; + const std::string joint_name2 = "joint_n2"; + Link link_2(link_name2); + + Joint joint_2(joint_name2); + joint_2.parent_link_name = scene_graph->getRoot(); + joint_2.child_link_name = link_name2; + joint_2.type = JointType::FLOATING; + joint_2.parent_to_joint_origin_transform.translation() = Eigen::Vector3d(1, 1, 1); + + EXPECT_TRUE(scene_graph->addLink(link_2, joint_2)); + EXPECT_TRUE(state_solver.addLink(link_2, joint_2)); + auto base_state_solver = std::make_shared(*scene_graph); runCompareStateSolver(*base_state_solver, state_solver); @@ -1180,14 +1208,24 @@ void runChangeJointOriginTest() EXPECT_TRUE(state.link_transforms.find(link_name1) != state.link_transforms.end()); EXPECT_TRUE(state.joint_transforms.find(joint_name1) != state.joint_transforms.end()); EXPECT_TRUE(state.joints.find(joint_name1) == state.joints.end()); + EXPECT_TRUE(state.joints.find(joint_name2) == state.joints.end()); + EXPECT_TRUE(state.joint_transforms.at(joint_name2).isApprox(joint_2.parent_to_joint_origin_transform)); + EXPECT_TRUE(state.floating_joints.at(joint_name2).isApprox(joint_2.parent_to_joint_origin_transform)); + EXPECT_TRUE(state.floating_joints.find(joint_name2) != state.floating_joints.end()); + EXPECT_EQ(state.floating_joints.size(), 1); scene_graph->saveDOT(tesseract_common::getTempPath() + "state_solver_before_change_joint_origin_unit.dot"); - Eigen::Isometry3d new_origin = Eigen::Isometry3d::Identity(); - new_origin.translation()(0) += 1.234; + Eigen::Isometry3d new_origin1 = Eigen::Isometry3d::Identity(); + new_origin1.translation()(0) += 1.234; + + Eigen::Isometry3d new_origin2 = Eigen::Isometry3d::Identity(); + new_origin2.translation()(1) += 1.234; - EXPECT_TRUE(scene_graph->changeJointOrigin(joint_name1, new_origin)); - EXPECT_TRUE(state_solver.changeJointOrigin(joint_name1, new_origin)); + EXPECT_TRUE(scene_graph->changeJointOrigin(joint_name1, new_origin1)); + EXPECT_TRUE(state_solver.changeJointOrigin(joint_name1, new_origin1)); + EXPECT_TRUE(scene_graph->changeJointOrigin(joint_name2, new_origin2)); + EXPECT_TRUE(state_solver.changeJointOrigin(joint_name2, new_origin2)); base_state_solver = std::make_shared(*scene_graph); @@ -1200,13 +1238,16 @@ void runChangeJointOriginTest() // Check that the origin got updated state = state_solver.getState(); - EXPECT_TRUE(state.link_transforms.at(link_name1).isApprox(new_origin)); - EXPECT_TRUE(state.joint_transforms.at(joint_name1).isApprox(new_origin)); + EXPECT_TRUE(state.link_transforms.at(link_name1).isApprox(new_origin1)); + EXPECT_TRUE(state.joint_transforms.at(joint_name1).isApprox(new_origin1)); + EXPECT_TRUE(state.link_transforms.at(link_name2).isApprox(new_origin2)); + EXPECT_TRUE(state.joint_transforms.at(joint_name2).isApprox(new_origin2)); + EXPECT_TRUE(state.floating_joints.at(joint_name2).isApprox(new_origin2)); scene_graph->saveDOT(tesseract_common::getTempPath() + "state_solver_after_change_joint_origin_unit.dot"); // Joint does not eixist - EXPECT_FALSE(state_solver.changeJointOrigin("joint_does_not_exist", new_origin)); + EXPECT_FALSE(state_solver.changeJointOrigin("joint_does_not_exist", new_origin1)); base_state_solver = std::make_shared(*scene_graph); @@ -1242,7 +1283,7 @@ void runMoveJointTest() joint_2.parent_to_joint_origin_transform.translation()(0) = 1.25; joint_2.parent_link_name = link_name1; joint_2.child_link_name = link_name2; - joint_2.type = JointType::FIXED; + joint_2.type = JointType::FLOATING; EXPECT_TRUE(scene_graph->addLink(link_1, joint_1)); EXPECT_TRUE(state_solver.addLink(link_1, joint_1)); @@ -1281,6 +1322,7 @@ void runMoveJointTest() EXPECT_TRUE(state.joints.find(joint_name1) == state.joints.end()); EXPECT_TRUE(state.link_transforms.find(link_name2) != state.link_transforms.end()); EXPECT_TRUE(state.joint_transforms.find(joint_name2) != state.joint_transforms.end()); + EXPECT_TRUE(state.floating_joints.find(joint_name2) != state.floating_joints.end()); EXPECT_TRUE(state.joints.find(joint_name2) == state.joints.end()); scene_graph->saveDOT(tesseract_common::getTempPath() + "state_solver_before_move_joint_unit.dot"); @@ -1306,6 +1348,7 @@ void runMoveJointTest() EXPECT_TRUE(state.joints.find(joint_name1) == state.joints.end()); EXPECT_TRUE(state.link_transforms.find(link_name2) != state.link_transforms.end()); EXPECT_TRUE(state.joint_transforms.find(joint_name2) != state.joint_transforms.end()); + EXPECT_TRUE(state.floating_joints.find(joint_name2) != state.floating_joints.end()); EXPECT_TRUE(state.joints.find(joint_name2) == state.joints.end()); scene_graph->saveDOT(tesseract_common::getTempPath() + "state_solver_after_move_joint_unit.dot"); @@ -1353,7 +1396,7 @@ void runMoveLinkTest() Joint joint_1(joint_name1); joint_1.parent_link_name = scene_graph->getRoot(); joint_1.child_link_name = link_name1; - joint_1.type = JointType::FIXED; + joint_1.type = JointType::FLOATING; Joint joint_2(joint_name2); joint_2.parent_to_joint_origin_transform.translation()(0) = 1.25; @@ -1376,6 +1419,7 @@ void runMoveLinkTest() SceneState state = state_solver.getState(); EXPECT_TRUE(state.link_transforms.find(link_name1) != state.link_transforms.end()); EXPECT_TRUE(state.joint_transforms.find(joint_name1) != state.joint_transforms.end()); + EXPECT_TRUE(state.floating_joints.find(joint_name1) != state.floating_joints.end()); EXPECT_TRUE(state.joints.find(joint_name1) == state.joints.end()); EXPECT_TRUE(scene_graph->addLink(link_2, joint_2)); @@ -1396,6 +1440,7 @@ void runMoveLinkTest() EXPECT_TRUE(std::find(joint_names.begin(), joint_names.end(), joint_name2) == joint_names.end()); EXPECT_TRUE(state.link_transforms.find(link_name1) != state.link_transforms.end()); EXPECT_TRUE(state.joint_transforms.find(joint_name1) != state.joint_transforms.end()); + EXPECT_TRUE(state.floating_joints.find(joint_name1) != state.floating_joints.end()); EXPECT_TRUE(state.joints.find(joint_name1) == state.joints.end()); EXPECT_TRUE(state.link_transforms.find(link_name2) != state.link_transforms.end()); EXPECT_TRUE(state.joint_transforms.find(joint_name2) != state.joint_transforms.end()); @@ -1406,6 +1451,7 @@ void runMoveLinkTest() std::string moved_joint_name = joint_name1 + "_moved"; Joint move_link_joint = joint_1.clone(moved_joint_name); move_link_joint.parent_link_name = "tool0"; + move_link_joint.type = JointType::FIXED; EXPECT_TRUE(scene_graph->moveLink(move_link_joint)); EXPECT_TRUE(state_solver.moveLink(move_link_joint)); @@ -1427,6 +1473,8 @@ void runMoveLinkTest() EXPECT_TRUE(state.link_transforms.find(link_name1) != state.link_transforms.end()); EXPECT_TRUE(state.joint_transforms.find(joint_name1) == state.joint_transforms.end()); + EXPECT_TRUE(state.floating_joints.find(joint_name1) == state.floating_joints.end()); + EXPECT_TRUE(state.floating_joints.empty()); EXPECT_TRUE(state.joint_transforms.find(moved_joint_name) != state.joint_transforms.end()); EXPECT_TRUE(state.joints.find(joint_name1) == state.joints.end()); EXPECT_TRUE(state.link_transforms.find(link_name2) != state.link_transforms.end()); @@ -1655,6 +1703,81 @@ void runReplaceJointTest() runCompareStateSolver(base_state_solver, *state_solver_clone); } + { // Replace joint with different type (Floating) + // Get the scene graph + auto scene_graph = getSceneGraph(locator); + auto state_solver = S(*scene_graph); + + { + Joint new_joint_a1 = scene_graph->getJoint("joint_a1")->clone(); + new_joint_a1.parent_to_joint_origin_transform.translation()(0) = 1.25; + new_joint_a1.type = JointType::FLOATING; + + EXPECT_TRUE(scene_graph->removeJoint("joint_a1")); + EXPECT_TRUE(scene_graph->addJoint(new_joint_a1)); + EXPECT_TRUE(state_solver.replaceJoint(new_joint_a1)); + + KDLStateSolver base_state_solver(*scene_graph); + + auto state = state_solver.getState(); + EXPECT_TRUE(state.floating_joints.find("joint_a1") != state.floating_joints.end()); + EXPECT_FALSE(state.floating_joints.empty()); + + runCompareStateSolver(base_state_solver, state_solver); + runCompareStateSolverLimits(*scene_graph, base_state_solver); + + // Test Clone + StateSolver::UPtr state_solver_clone = state_solver.clone(); + runCompareStateSolver(base_state_solver, *state_solver_clone); + } + + { // Replace floating joint with floating joint but different origin + Joint new_joint_a1 = scene_graph->getJoint("joint_a1")->clone(); + new_joint_a1.parent_to_joint_origin_transform.translation()(1) = 1.25; + new_joint_a1.type = JointType::FLOATING; + + EXPECT_TRUE(scene_graph->removeJoint("joint_a1")); + EXPECT_TRUE(scene_graph->addJoint(new_joint_a1)); + EXPECT_TRUE(state_solver.replaceJoint(new_joint_a1)); + + KDLStateSolver base_state_solver(*scene_graph); + + auto state = state_solver.getState(); + EXPECT_TRUE(state.floating_joints.find("joint_a1") != state.floating_joints.end()); + EXPECT_FALSE(state.floating_joints.empty()); + + runCompareStateSolver(base_state_solver, state_solver); + runCompareStateSolverLimits(*scene_graph, base_state_solver); + + // Test Clone + StateSolver::UPtr state_solver_clone = state_solver.clone(); + runCompareStateSolver(base_state_solver, *state_solver_clone); + } + + { // Replace floating joint with another joint type + Joint new_joint_a1 = scene_graph->getJoint("joint_a1")->clone(); + new_joint_a1.parent_to_joint_origin_transform.translation()(0) = 1.25; + new_joint_a1.type = JointType::FIXED; + + EXPECT_TRUE(scene_graph->removeJoint("joint_a1")); + EXPECT_TRUE(scene_graph->addJoint(new_joint_a1)); + EXPECT_TRUE(state_solver.replaceJoint(new_joint_a1)); + + KDLStateSolver base_state_solver(*scene_graph); + + auto state = state_solver.getState(); + EXPECT_TRUE(state.floating_joints.find("joint_a1") == state.floating_joints.end()); + EXPECT_TRUE(state.floating_joints.empty()); + + runCompareStateSolver(base_state_solver, state_solver); + runCompareStateSolverLimits(*scene_graph, base_state_solver); + + // Test Clone + StateSolver::UPtr state_solver_clone = state_solver.clone(); + runCompareStateSolver(base_state_solver, *state_solver_clone); + } + } + { // Replace joint with different type (Continuous) // Get the scene graph auto scene_graph = getSceneGraph(locator); diff --git a/tesseract_state_solver/test/tesseract_state_solver_ofkt_unit.cpp b/tesseract_state_solver/test/tesseract_state_solver_ofkt_unit.cpp index 89d0be1b324..ae46079d74b 100644 --- a/tesseract_state_solver/test/tesseract_state_solver_ofkt_unit.cpp +++ b/tesseract_state_solver/test/tesseract_state_solver_ofkt_unit.cpp @@ -53,6 +53,25 @@ TEST(TesseractStateSolverUnit, OFKTNodeBaseAndFailuresUnit) // NOLINT EXPECT_TRUE(node.getStaticTransformation().isApprox(static_tf, 1e-6)); } + { // OFKTFloatingNode + OFKTRootNode root_node("base_link"); + OFKTFloatingNode node(&root_node, "base_link", "joint_a1", Eigen::Isometry3d::Identity()); + const OFKTFloatingNode& const_node = node; + EXPECT_TRUE(const_node.getParent() == &root_node); + EXPECT_ANY_THROW(node.storeJointValue(M_PI_2)); // NOLINT + EXPECT_ANY_THROW(node.getJointValue()); // NOLINT + EXPECT_FALSE(node.updateWorldTransformationRequired()); + EXPECT_TRUE(Eigen::Isometry3d::Identity().isApprox(node.computeLocalTransformation(0), 1e-6)); + EXPECT_TRUE(node.getStaticTransformation().isApprox(Eigen::Isometry3d::Identity(), 1e-6)); + node.computeAndStoreLocalTransformation(); + EXPECT_TRUE(Eigen::Isometry3d::Identity().isApprox(node.getLocalTransformation(), 1e-6)); + + Eigen::Isometry3d static_tf = Eigen::Isometry3d::Identity(); + static_tf.translation() = Eigen::Vector3d(1, 2, 3); + node.setStaticTransformation(static_tf); + EXPECT_TRUE(node.getStaticTransformation().isApprox(static_tf, 1e-6)); + } + { // OFKTRevoluteNode auto check = Eigen::Isometry3d::Identity() * Eigen::AngleAxisd(M_PI_2, Eigen::Vector3d(0, 0, 1)); OFKTRootNode root_node("base_link");