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 f99cdc8
Show file tree
Hide file tree
Showing 12 changed files with 426 additions and 87 deletions.
81 changes: 70 additions & 11 deletions tesseract_collision/bullet/src/create_convex_hull.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -51,17 +51,76 @@ int main(int argc, char** argv)

namespace po = boost::program_options;
po::options_description desc("Options");
desc.add_options()("help,h", "Print help messages")(
"input,i", po::value<std::string>(&input)->required(), "File path to mesh used to create a convex hull.")(
"output,o", po::value<std::string>(&output)->required(), "File path to save the generated convex hull as a ply.")(
"shrink,s",
po::value<double>(&shrink),
"If positive, the convex hull is shrunken by that amount (each face is moved by 'shrink' length units towards "
"the center along its normal).")("clamp,c",
po::value<double>(&clamp),
"If positive, 'shrink' is clamped to not exceed 'clamp * innerRadius', where "
"'innerRadius' is the minimum distance of a face to the center of the convex "
"hull.");
desc.add_options()(
"help,h", "Print help messages")("input,i",
po::value<std::string>(&input)->required(),
"File path to mesh used to create a convex hull.")("output,o",
po::value<std::string>(
&output)
->required(),
"File path to save the "
"generated convex hull as a "
"ply.")("shrink,s",
po::value<double>(
&shrink),
"If positive, the "
"convex hull is "
"shrunken by that "
"amount (each face "
"is moved by "
"'shrink' length "
"units towards "
"the center along "
"its normal).")("clam"
"p,c",
po::value<
double>(
&clamp),
"If "
"posi"
"tive"
", "
"'shr"
"ink'"
" is "
"clam"
"ped "
"to "
"not "
"exce"
"ed "
"'cla"
"mp "
"* "
"inne"
"rRad"
"ius'"
", "
"wher"
"e "
"'inn"
"erRa"
"dius"
"' "
"is "
"the "
"mini"
"mum "
"dist"
"ance"
" of "
"a "
"face"
" to "
"the "
"cent"
"er "
"of "
"the "
"conv"
"ex "
"hull"
".");

po::variables_map vm;
try
Expand Down
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
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 f99cdc8

Please sign in to comment.