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

Jump to bottom

Add JointModel::satisfiesAccelerationBounds() #2092

Merged
merged 4 commits into from
Apr 13, 2023
Merged

Add JointModel::satisfiesAccelerationBounds() #2092

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
c8e4545
Add JointModel::satisfiesAccelerationBounds()
AndyZe Apr 7, 2023
b66678f
Check Jerk bounds too
AndyZe Apr 7, 2023
879f4cb
Check if bounds exist
AndyZe Apr 7, 2023
7723c25
Merge branch 'main' into andyz/accel_bounds
sjahr Apr 13, 2023
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
18 changes: 18 additions & 0 deletions moveit_core/robot_model/include/moveit/robot_model/joint_model.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -325,6 +325,24 @@ class JointModel
/** \brief Force the specified velocities to be inside bounds. Return true if changes were made. */
virtual bool enforceVelocityBounds(double* values, const Bounds& other_bounds) const;

/** \brief Check if the set of accelerations for the variables of this joint are within bounds. */
bool satisfiesAccelerationBounds(const double* values, double margin = 0.0) const
{
return satisfiesAccelerationBounds(values, variable_bounds_, margin);
}

/** \brief Check if the set of accelerations for the variables of this joint are within bounds, up to some margin. */
virtual bool satisfiesAccelerationBounds(const double* values, const Bounds& other_bounds, double margin) const;

/** \brief Check if the set of jerks for the variables of this joint are within bounds. */
bool satisfiesJerkBounds(const double* values, double margin = 0.0) const
{
return satisfiesJerkBounds(values, variable_bounds_, margin);
}

/** \brief Check if the set of jerks for the variables of this joint are within bounds, up to some margin. */
virtual bool satisfiesJerkBounds(const double* values, const Bounds& other_bounds, double margin) const;

/** \brief Get the bounds for a variable. Throw an exception if the variable was not found */
const VariableBounds& getVariableBounds(const std::string& variable) const;

Expand Down
36 changes: 36 additions & 0 deletions moveit_core/robot_model/src/joint_model.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -118,6 +118,10 @@ bool JointModel::satisfiesVelocityBounds(const double* values, const Bounds& oth
{
for (std::size_t i = 0; i < other_bounds.size(); ++i)
{
if (!other_bounds[i].velocity_bounded_)
{
continue;
}
Comment on lines +121 to +124
Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Not having this previously was a bug

if (other_bounds[i].max_velocity_ + margin < values[i])
{
return false;
Expand All @@ -130,6 +134,38 @@ bool JointModel::satisfiesVelocityBounds(const double* values, const Bounds& oth
return true;
}

bool JointModel::satisfiesAccelerationBounds(const double* values, const Bounds& other_bounds, double margin) const
{
for (std::size_t i = 0; i < other_bounds.size(); ++i)
{
if (!other_bounds[i].acceleration_bounded_)
{
continue;
}
if (other_bounds[i].max_acceleration_ + margin < values[i])
return false;
else if (other_bounds[i].min_acceleration_ - margin > values[i])
return false;
}
return true;
}

bool JointModel::satisfiesJerkBounds(const double* values, const Bounds& other_bounds, double margin) const
{
for (std::size_t i = 0; i < other_bounds.size(); ++i)
{
if (!other_bounds[i].jerk_bounded_)
{
continue;
}
if (other_bounds[i].max_jerk_ + margin < values[i])
return false;
else if (other_bounds[i].min_jerk_ - margin > values[i])
return false;
}
return true;
}

const VariableBounds& JointModel::getVariableBounds(const std::string& variable) const
{
return variable_bounds_[getLocalVariableIndex(variable)];
Expand Down