Skip to content

Commit

Permalink
Add floating joint support
Browse files Browse the repository at this point in the history
  • Loading branch information
@Levi-Armstrong
Levi-Armstrong committed Jan 5, 2025
1 parent ea05dde commit dd8c088
Show file tree
Hide file tree
Showing 17 changed files with 424 additions and 93 deletions.
25 changes: 21 additions & 4 deletions tesseract_environment/include/tesseract_environment/environment.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -296,8 +296,11 @@ class Environment
* will update the contact managers transforms
*
*/
void setState(const std::unordered_map<std::string, double>& joints);
void setState(const std::vector<std::string>& joint_names, const Eigen::Ref<const Eigen::VectorXd>& joint_values);
void setState(const std::unordered_map<std::string, double>& joints,
const tesseract_common::TransformMap& floating_joints = {});
void setState(const std::vector<std::string>& joint_names,
const Eigen::Ref<const Eigen::VectorXd>& joint_values,
const tesseract_common::TransformMap& floating_joints = {});

/**
* @brief Get the state of the environment for a given set or subset of joint values.
Expand All @@ -307,9 +310,11 @@ class Environment
* @param joints A map of joint names to joint values to change.
* @return A the state of the environment
*/
tesseract_scene_graph::SceneState getState(const std::unordered_map<std::string, double>& joints) const;
tesseract_scene_graph::SceneState getState(const std::unordered_map<std::string, double>& joints,
const tesseract_common::TransformMap& floating_joints = {}) const;
tesseract_scene_graph::SceneState getState(const std::vector<std::string>& joint_names,
const Eigen::Ref<const Eigen::VectorXd>& joint_values) const;
const Eigen::Ref<const Eigen::VectorXd>& joint_values,
const tesseract_common::TransformMap& floating_joints = {}) const;

/** @brief Get the current state of the environment */
tesseract_scene_graph::SceneState getState() const;
Expand Down Expand Up @@ -389,6 +394,18 @@ class Environment
*/
Eigen::VectorXd getCurrentJointValues(const std::vector<std::string>& joint_names) const;

/**
* @brief Get the current floating joint values
* @return The joint origin transform for the floating joint
*/
tesseract_common::TransformMap getCurrentFloatingJointValues() const;

/**
* @brief Get the current floating joint values
* @return The joint origin transform for the floating joint
*/
tesseract_common::TransformMap getCurrentFloatingJointValues(const std::vector<std::string>& joint_names) const;

/**
* @brief Get the root link name
* @return String
Expand Down
71 changes: 56 additions & 15 deletions tesseract_environment/src/environment.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -289,14 +289,21 @@ struct Environment::Implementation

bool initHelper(const std::vector<std::shared_ptr<const Command>>& commands);

void setState(const std::unordered_map<std::string, double>& joints);
void setState(const std::unordered_map<std::string, double>& joints,
const tesseract_common::TransformMap& floating_joints = {});

void setState(const std::vector<std::string>& joint_names, const Eigen::Ref<const Eigen::VectorXd>& joint_values);
void setState(const std::vector<std::string>& joint_names,
const Eigen::Ref<const Eigen::VectorXd>& joint_values,
const tesseract_common::TransformMap& floating_joints = {});

Eigen::VectorXd getCurrentJointValues() const;

Eigen::VectorXd getCurrentJointValues(const std::vector<std::string>& joint_names) const;

tesseract_common::TransformMap getCurrentFloatingJointValues() const;

tesseract_common::TransformMap getCurrentFloatingJointValues(const std::vector<std::string>& joint_names) const;

std::vector<std::string> getStaticLinkNames(const std::vector<std::string>& joint_names) const;

void clear();
Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -567,16 +574,18 @@ bool Environment::Implementation::initHelper(const std::vector<std::shared_ptr<c
return initialized;
}

void Environment::Implementation::setState(const std::unordered_map<std::string, double>& joints)
void Environment::Implementation::setState(const std::unordered_map<std::string, double>& joints,
const tesseract_common::TransformMap& floating_joints)
{
state_solver->setState(joints);
state_solver->setState(joints, floating_joints);
currentStateChanged();
}

void Environment::Implementation::setState(const std::vector<std::string>& joint_names,
const Eigen::Ref<const Eigen::VectorXd>& joint_values)
const Eigen::Ref<const Eigen::VectorXd>& joint_values,
const tesseract_common::TransformMap& floating_joints)
{
state_solver->setState(joint_names, joint_values);
state_solver->setState(joint_names, joint_values, floating_joints);
currentStateChanged();
}

Expand All @@ -601,6 +610,21 @@ Eigen::VectorXd Environment::Implementation::getCurrentJointValues(const std::ve
return jv;
}

tesseract_common::TransformMap Environment::Implementation::getCurrentFloatingJointValues() const
{
return current_state.floating_joints;
}

tesseract_common::TransformMap
Environment::Implementation::getCurrentFloatingJointValues(const std::vector<std::string>& joint_names) const
{
tesseract_common::TransformMap fjv;
for (const auto& joint_name : joint_names)
fjv[joint_name] = current_state.floating_joints.at(joint_name);

return fjv;
}

std::vector<std::string>
Environment::Implementation::getStaticLinkNames(const std::vector<std::string>& joint_names) const
{
Expand Down Expand Up @@ -2427,40 +2451,44 @@ const std::string& Environment::getName() const
return std::as_const<Implementation>(*impl_).scene_graph->getName();
}

void Environment::setState(const std::unordered_map<std::string, double>& joints)
void Environment::setState(const std::unordered_map<std::string, double>& joints,
const tesseract_common::TransformMap& floating_joints)
{
{
std::unique_lock<std::shared_mutex> lock(mutex_);
impl_->setState(joints);
impl_->setState(joints, floating_joints);
}

std::shared_lock<std::shared_mutex> lock(mutex_);
impl_->triggerCurrentStateChangedCallbacks();
}

void Environment::setState(const std::vector<std::string>& joint_names,
const Eigen::Ref<const Eigen::VectorXd>& joint_values)
const Eigen::Ref<const Eigen::VectorXd>& joint_values,
const tesseract_common::TransformMap& floating_joints)
{
{
std::unique_lock<std::shared_mutex> lock(mutex_);
impl_->setState(joint_names, joint_values);
impl_->setState(joint_names, joint_values, floating_joints);
}

std::shared_lock<std::shared_mutex> lock(mutex_);
impl_->triggerCurrentStateChangedCallbacks();
}

tesseract_scene_graph::SceneState Environment::getState(const std::unordered_map<std::string, double>& joints) const
tesseract_scene_graph::SceneState Environment::getState(const std::unordered_map<std::string, double>& joints,
const tesseract_common::TransformMap& floating_joints) const
{
std::shared_lock<std::shared_mutex> lock(mutex_);
return std::as_const<Implementation>(*impl_).state_solver->getState(joints);
return std::as_const<Implementation>(*impl_).state_solver->getState(joints, floating_joints);
}

tesseract_scene_graph::SceneState Environment::getState(const std::vector<std::string>& joint_names,
const Eigen::Ref<const Eigen::VectorXd>& joint_values) const
const Eigen::Ref<const Eigen::VectorXd>& joint_values,
const tesseract_common::TransformMap& floating_joints) const
{
std::shared_lock<std::shared_mutex> lock(mutex_);
return std::as_const<Implementation>(*impl_).state_solver->getState(joint_names, joint_values);
return std::as_const<Implementation>(*impl_).state_solver->getState(joint_names, joint_values, floating_joints);
}

tesseract_scene_graph::SceneState Environment::getState() const
Expand Down Expand Up @@ -2543,6 +2571,19 @@ Eigen::VectorXd Environment::getCurrentJointValues(const std::vector<std::string
return std::as_const<Implementation>(*impl_).getCurrentJointValues(joint_names);
}

tesseract_common::TransformMap Environment::getCurrentFloatingJointValues() const
{
std::shared_lock<std::shared_mutex> lock(mutex_);
return std::as_const<Implementation>(*impl_).getCurrentFloatingJointValues();
}

tesseract_common::TransformMap
Environment::getCurrentFloatingJointValues(const std::vector<std::string>& joint_names) const
{
std::shared_lock<std::shared_mutex> lock(mutex_);
return std::as_const<Implementation>(*impl_).getCurrentFloatingJointValues(joint_names);
}

std::string Environment::getRootLinkName() const
{
std::shared_lock<std::shared_mutex> lock(mutex_);
Expand Down
4 changes: 2 additions & 2 deletions tesseract_kinematics/core/src/joint_group.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -52,8 +52,8 @@ JointGroup::JointGroup(std::string name,
throw std::runtime_error("Joint name '" + joint_name + "' does not exist in the provided scene graph!");
}

tesseract_scene_graph::KDLTreeData data =
tesseract_scene_graph::parseSceneGraph(scene_graph, joint_names_, scene_state.joints);
tesseract_scene_graph::KDLTreeData data = tesseract_scene_graph::parseSceneGraph(
scene_graph, joint_names_, scene_state.joints, scene_state.floating_joints);
state_solver_ = std::make_unique<tesseract_scene_graph::KDLStateSolver>(scene_graph, data);
jacobian_map_.reserve(joint_names_.size());
std::vector<std::string> solver_jn = state_solver_->getActiveJointNames();
Expand Down
9 changes: 8 additions & 1 deletion tesseract_scene_graph/include/tesseract_scene_graph/kdl_parser.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -49,6 +49,8 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
#include <kdl/jacobian.hpp>
TESSERACT_COMMON_IGNORE_WARNINGS_POP

#include <tesseract_common/eigen_types.h>

namespace tesseract_scene_graph
{
class SceneGraph;
Expand Down Expand Up @@ -122,13 +124,17 @@ KDL::RigidBodyInertia convert(const std::shared_ptr<const Inertial>& inertial);
/** @brief The KDLTreeData populated when parsing scene graph */
struct KDLTreeData
{
EIGEN_MAKE_ALIGNED_OPERATOR_NEW

KDL::Tree tree;
std::string base_link_name;
std::vector<std::string> joint_names;
std::vector<std::string> active_joint_names;
std::vector<std::string> floating_joint_names;
std::vector<std::string> link_names;
std::vector<std::string> active_link_names;
std::vector<std::string> static_link_names;
tesseract_common::TransformMap floating_joint_values;
};

/**
Expand All @@ -153,7 +159,8 @@ KDLTreeData parseSceneGraph(const SceneGraph& scene_graph);
*/
KDLTreeData parseSceneGraph(const SceneGraph& scene_graph,
const std::vector<std::string>& joint_names,
const std::unordered_map<std::string, double>& joint_values);
const std::unordered_map<std::string, double>& joint_values,
const tesseract_common::TransformMap& floating_joint_values);

} // namespace tesseract_scene_graph

Expand Down
5 changes: 5 additions & 0 deletions tesseract_scene_graph/include/tesseract_scene_graph/scene_state.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -69,6 +69,9 @@ struct SceneState
/** @brief The joint values used for calculating the joint and link transforms */
std::unordered_map<std::string, double> joints;

/** @brief The floating joint values used for calculating the joint and link transforms */
tesseract_common::TransformMap floating_joints;

/** @brief The link transforms in world coordinate system */
tesseract_common::TransformMap link_transforms;

Expand All @@ -77,6 +80,8 @@ struct SceneState

Eigen::VectorXd getJointValues(const std::vector<std::string>& joint_names) const;

tesseract_common::TransformMap getFloatingJointValues(const std::vector<std::string>& joint_names) const;

bool operator==(const SceneState& rhs) const;
bool operator!=(const SceneState& rhs) const;

Expand Down
39 changes: 28 additions & 11 deletions tesseract_scene_graph/src/kdl_parser.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -43,6 +43,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP
#include <tesseract_scene_graph/graph.h>
#include <tesseract_scene_graph/joint.h>
#include <tesseract_scene_graph/link.h>
#include <tesseract_common/eigen_types.h>

namespace tesseract_scene_graph
{
Expand Down Expand Up @@ -136,6 +137,7 @@ KDL::Joint convert(const std::shared_ptr<const Joint>& joint)
switch (joint->type)
{
case JointType::FIXED:
case JointType::FLOATING:
{
return KDL::Joint(name, KDL::Joint::None);
}
Expand Down Expand Up @@ -219,6 +221,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());
Expand All @@ -234,6 +237,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)
{
Expand Down Expand Up @@ -269,9 +278,12 @@ struct kdl_sub_tree_builder : public boost::dfs_visitor<>
{
kdl_sub_tree_builder(KDLTreeData& data,
const std::vector<std::string>& joint_names,
const std::unordered_map<std::string, double>& joint_values)
: data_(data), joint_names_(joint_names), joint_values_(joint_values)
const std::unordered_map<std::string, double>& 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 <class u, class g>
Expand Down Expand Up @@ -310,15 +322,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)
{
Expand Down Expand Up @@ -376,10 +391,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);
}
Expand Down Expand Up @@ -413,6 +428,7 @@ struct kdl_sub_tree_builder : public boost::dfs_visitor<>

const std::vector<std::string>& joint_names_;
const std::unordered_map<std::string, double>& joint_values_;
const tesseract_common::TransformMap& floating_joint_values_;
};

KDLTreeData parseSceneGraph(const SceneGraph& scene_graph)
Expand Down Expand Up @@ -457,15 +473,16 @@ KDLTreeData parseSceneGraph(const SceneGraph& scene_graph)

KDLTreeData parseSceneGraph(const SceneGraph& scene_graph,
const std::vector<std::string>& joint_names,
const std::unordered_map<std::string, double>& joint_values)
const std::unordered_map<std::string, double>& joint_values,
const tesseract_common::TransformMap& floating_joint_values)
{
if (!scene_graph.isTree())
throw std::runtime_error("parseSubSceneGraph: currently only works if the scene graph is a tree.");

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<SceneGraph::Vertex, size_t> index_map;
boost::associative_property_map<std::map<SceneGraph::Vertex, size_t>> prop_index_map(index_map);
Expand Down
Loading

0 comments on commit dd8c088

Please sign in to comment.