Skip to content

Commit

Permalink
Add missing floating joint methods to environment monitor interface
Browse files Browse the repository at this point in the history
  • Loading branch information
@Levi-Armstrong
Levi-Armstrong committed Jan 18, 2025
1 parent b060e9f commit abe0613
Show file tree
Hide file tree
Showing 2 changed files with 92 additions and 19 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
30 changes: 22 additions & 8 deletions tesseract_environment/include/tesseract_environment/environment_monitor_interface.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -35,6 +35,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
TESSERACT_COMMON_IGNORE_WARNINGS_POP

#include <tesseract_scene_graph/fwd.h>
#include <tesseract_common/eigen_types.h>

namespace tesseract_environment
{
Expand Down Expand Up @@ -117,23 +118,36 @@ class EnvironmentMonitorInterface
* @return True if successful, otherwise false
*/
virtual bool setEnvironmentState(const std::string& monitor_namespace,
const std::unordered_map<std::string, double>& joints) const = 0;
const std::unordered_map<std::string, double>& joints,
const tesseract_common::TransformMap& floating_joints = {}) const = 0;
virtual bool setEnvironmentState(const std::string& monitor_namespace,
const std::vector<std::string>& joint_names,
const std::vector<double>& joint_values) const = 0;
const std::vector<double>& joint_values,
const tesseract_common::TransformMap& floating_joints = {}) const = 0;
virtual bool setEnvironmentState(const std::string& monitor_namespace,
const std::vector<std::string>& joint_names,
const Eigen::Ref<const Eigen::VectorXd>& joint_values) const = 0;
const Eigen::Ref<const Eigen::VectorXd>& joint_values,
const tesseract_common::TransformMap& floating_joints = {}) const = 0;
virtual bool setEnvironmentState(const std::string& monitor_namespace,
const tesseract_common::TransformMap& floating_joints) const = 0;

/**
* @brief Set environment state for all monitor namespaces
* @return A vector of failed namespace, if empty all namespace were updated successfully.
*/
virtual std::vector<std::string> setEnvironmentState(const std::unordered_map<std::string, double>& joints) const = 0;
virtual std::vector<std::string> setEnvironmentState(const std::vector<std::string>& joint_names,
const std::vector<double>& joint_values) const = 0;
virtual std::vector<std::string> setEnvironmentState(const std::vector<std::string>& joint_names,
const Eigen::Ref<const Eigen::VectorXd>& joint_values) const = 0;
virtual std::vector<std::string>
setEnvironmentState(const std::unordered_map<std::string, double>& joints,
const tesseract_common::TransformMap& floating_joints = {}) const = 0;
virtual std::vector<std::string>
setEnvironmentState(const std::vector<std::string>& joint_names,
const std::vector<double>& joint_values,
const tesseract_common::TransformMap& floating_joints = {}) const = 0;
virtual std::vector<std::string>
setEnvironmentState(const std::vector<std::string>& joint_names,
const Eigen::Ref<const Eigen::VectorXd>& joint_values,
const tesseract_common::TransformMap& floating_joints = {}) const = 0;
virtual std::vector<std::string>
setEnvironmentState(const tesseract_common::TransformMap& floating_joints = {}) const = 0;

/**
* @brief Pull information from the environment in the provided namespace and create a Environment Object
Expand Down

0 comments on commit abe0613

Please sign in to comment.