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 6, 2025
1 parent ea05dde commit 7a2b83d
Show file tree
Hide file tree
Showing 17 changed files with 592 additions and 105 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
12 changes: 11 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,20 @@ KDL::RigidBodyInertia convert(const std::shared_ptr<const Inertial>& inertial);
/** @brief The KDLTreeData populated when parsing scene graph */
struct KDLTreeData
{
EIGEN_MAKE_ALIGNED_OPERATOR_NEW

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

bool operator==(const KDLTreeData& rhs) const;
bool operator!=(const KDLTreeData& rhs) const;
};

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

} // namespace tesseract_scene_graph

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
Loading

0 comments on commit 7a2b83d

Please sign in to comment.