Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Add floating joint support #1060

Draft
wants to merge 1 commit into
base: master
Choose a base branch
from
+247 −60
Draft

Add floating joint support #1060

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
81 changes: 70 additions & 11 deletions tesseract_collision/bullet/src/create_convex_hull.cpp
View file
Open in desktop
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
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -295,8 +295,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 @@ -306,9 +309,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 @@ -388,6 +393,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
67 changes: 54 additions & 13 deletions tesseract_environment/src/environment.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -264,14 +264,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 @@ -398,7 +405,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);

ar& boost::serialization::make_nvp("timestamp",
boost::serialization::make_binary_object(&timestamp, sizeof(timestamp)));
Expand Down Expand Up @@ -545,16 +552,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 @@ -579,6 +588,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 @@ -2403,7 +2427,8 @@ 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_);
Expand All @@ -2415,7 +2440,8 @@ void Environment::setState(const std::unordered_map<std::string, double>& joints
}

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_);
Expand All @@ -2426,17 +2452,19 @@ void Environment::setState(const std::vector<std::string>& joint_names,
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 @@ -2519,6 +2547,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
Open in desktop
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
Loading