Skip to content

Commit

Permalink
Add support for jerk limits
Browse files Browse the repository at this point in the history
  • Loading branch information
@Levi-Armstrong
Levi-Armstrong committed May 28, 2024
1 parent a6bfa92 commit 490f33f
Show file tree
Hide file tree
Showing 12 changed files with 140 additions and 108 deletions.
92 changes: 47 additions & 45 deletions tesseract_common/include/tesseract_common/kinematic_limits.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -45,10 +45,13 @@ struct KinematicLimits
Eigen::MatrixX2d joint_limits;

/** @brief The velocity limits */
Eigen::VectorXd velocity_limits;
Eigen::MatrixX2d velocity_limits;

/** @brief The acceleration limits */
Eigen::VectorXd acceleration_limits;
Eigen::MatrixX2d acceleration_limits;

/** @brief The jerk limits */
Eigen::MatrixX2d jerk_limits;

void resize(Eigen::Index size);

Expand All @@ -62,40 +65,40 @@ struct KinematicLimits
};

/**
* @brief Check if within position limits
* @param joint_positions The joint position to check
* @param position_limits The joint limits to perform check
* @brief Check if within limits
* @param joint_positions The values to check
* @param position_limits The limits to perform check
* @return
*/
template <typename FloatType>
bool isWithinPositionLimits(const Eigen::Ref<const Eigen::Matrix<FloatType, Eigen::Dynamic, 1>>& joint_positions,
const Eigen::Ref<const Eigen::Matrix<FloatType, Eigen::Dynamic, 2>>& position_limits)
bool isWithinLimits(const Eigen::Ref<const Eigen::Matrix<FloatType, Eigen::Dynamic, 1>>& values,
const Eigen::Ref<const Eigen::Matrix<FloatType, Eigen::Dynamic, 2>>& limits)
{
auto p = joint_positions.array();
auto l0 = position_limits.col(0).array();
auto l1 = position_limits.col(1).array();
auto p = values.array();
auto l0 = limits.col(0).array();
auto l1 = limits.col(1).array();
return (!(p > l1).any() && !(p < l0).any());
}

/**
* @brief Check if joint position is within bounds or relatively equal to a limit
* @param joint_positions The joint position to check
* @param joint_limits The joint limits to perform check
* @param max_diff The max diff when comparing position to limit value max(abs(position - limit)) <= max_diff, if true
* @brief Check if values are within bounds or relatively equal to a limit
* @param values The values to check
* @param limits The limits to perform check
* @param max_diff The max diff when comparing value to limit value max(abs(value - limit)) <= max_diff, if true
* they are considered equal
* @param max_rel_diff The max relative diff between position and limit abs(position - limit) <= largest * max_rel_diff,
* if true considered equal. The largest is the largest of the absolute values of position and limit.
* @return True if the all position are within the limits or relatively equal to the limit, otherwise false.
* @param max_rel_diff The max relative diff between value and limit abs(value - limit) <= largest * max_rel_diff,
* if true considered equal. The largest is the largest of the absolute values of value and limit.
* @return True if the all values are within the limits or relatively equal to the limit, otherwise false.
*/
template <typename FloatType>
bool satisfiesPositionLimits(const Eigen::Ref<const Eigen::Matrix<FloatType, Eigen::Dynamic, 1>>& joint_positions,
const Eigen::Ref<const Eigen::Matrix<FloatType, Eigen::Dynamic, 2>>& position_limits,
const Eigen::Ref<const Eigen::Matrix<FloatType, Eigen::Dynamic, 1>>& max_diff,
const Eigen::Ref<const Eigen::Matrix<FloatType, Eigen::Dynamic, 1>>& max_rel_diff)
bool satisfiesLimits(const Eigen::Ref<const Eigen::Matrix<FloatType, Eigen::Dynamic, 1>>& values,
const Eigen::Ref<const Eigen::Matrix<FloatType, Eigen::Dynamic, 2>>& limits,
const Eigen::Ref<const Eigen::Matrix<FloatType, Eigen::Dynamic, 1>>& max_diff,
const Eigen::Ref<const Eigen::Matrix<FloatType, Eigen::Dynamic, 1>>& max_rel_diff)
{
auto p = joint_positions.array();
auto l0 = position_limits.col(0).array();
auto l1 = position_limits.col(1).array();
auto p = values.array();
auto l0 = limits.col(0).array();
auto l1 = limits.col(1).array();
auto md = max_diff.array();
auto mrd = max_rel_diff.array();

Expand All @@ -113,38 +116,37 @@ bool satisfiesPositionLimits(const Eigen::Ref<const Eigen::Matrix<FloatType, Eig
}

/**
* @brief Check if joint position is within bounds or relatively equal to a limit
* @param joint_positions The joint position to check
* @param joint_limits The joint limits to perform check
* @param max_diff The max diff when comparing position to limit value max(abs(position - limit)) <= max_diff, if true
* @brief Check if values is within bounds or relatively equal to a limit
* @param values The values to check
* @param limits The limits to perform check
* @param max_diff The max diff when comparing value to limit value max(abs(value - limit)) <= max_diff, if true
* they are considered equal
* @param max_rel_diff The max relative diff between position and limit abs(position - limit) <= largest * max_rel_diff,
* if true considered equal. The largest is the largest of the absolute values of position and limit.
* @return True if the all position are within the limits or relatively equal to the limit, otherwise false.
* @param max_rel_diff The max relative diff between value and limit abs(value - limit) <= largest * max_rel_diff,
* if true considered equal. The largest is the largest of the absolute values of value and limit.
* @return True if the all values are within the limits or relatively equal to the limit, otherwise false.
*/
template <typename FloatType>
bool satisfiesPositionLimits(const Eigen::Ref<const Eigen::Matrix<FloatType, Eigen::Dynamic, 1>>& joint_positions,
const Eigen::Ref<const Eigen::Matrix<FloatType, Eigen::Dynamic, 2>>& position_limits,
FloatType max_diff = static_cast<FloatType>(1e-6),
FloatType max_rel_diff = std::numeric_limits<FloatType>::epsilon())
bool satisfiesLimits(const Eigen::Ref<const Eigen::Matrix<FloatType, Eigen::Dynamic, 1>>& values,
const Eigen::Ref<const Eigen::Matrix<FloatType, Eigen::Dynamic, 2>>& limits,
FloatType max_diff = static_cast<FloatType>(1e-6),
FloatType max_rel_diff = std::numeric_limits<FloatType>::epsilon())
{
const auto eigen_max_diff = Eigen::Matrix<FloatType, Eigen::Dynamic, 1>::Constant(joint_positions.size(), max_diff);
const auto eigen_max_rel_diff =
Eigen::Matrix<FloatType, Eigen::Dynamic, 1>::Constant(joint_positions.size(), max_rel_diff);
const auto eigen_max_diff = Eigen::Matrix<FloatType, Eigen::Dynamic, 1>::Constant(values.size(), max_diff);
const auto eigen_max_rel_diff = Eigen::Matrix<FloatType, Eigen::Dynamic, 1>::Constant(values.size(), max_rel_diff);
// NOLINTNEXTLINE(clang-analyzer-core.uninitialized.UndefReturn)
return satisfiesPositionLimits<FloatType>(joint_positions, position_limits, eigen_max_diff, eigen_max_rel_diff);
return satisfiesLimits<FloatType>(values, limits, eigen_max_diff, eigen_max_rel_diff);
}

/**
* @brief Enforce position to be within the provided limits
* @param joint_positions The joint position to enforce bounds on
* @param joint_limits The limits to perform check
* @brief Enforce values to be within the provided limits
* @param values The values to enforce bounds on
* @param limits The limits to perform check
*/
template <typename FloatType>
void enforcePositionLimits(Eigen::Ref<Eigen::Matrix<FloatType, Eigen::Dynamic, 1>> joint_positions,
const Eigen::Ref<const Eigen::Matrix<FloatType, Eigen::Dynamic, 2>>& position_limits)
void enforceLimits(Eigen::Ref<Eigen::Matrix<FloatType, Eigen::Dynamic, 1>> values,
const Eigen::Ref<const Eigen::Matrix<FloatType, Eigen::Dynamic, 2>>& limits)
{
joint_positions = joint_positions.array().min(position_limits.col(1).array()).max(position_limits.col(0).array());
values = values.array().min(limits.col(1).array()).max(limits.col(0).array());
}
} // namespace tesseract_common

Expand Down
63 changes: 29 additions & 34 deletions tesseract_common/src/kinematic_limits.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -37,8 +37,9 @@ namespace tesseract_common
void KinematicLimits::resize(Eigen::Index size)
{
joint_limits.resize(size, 2);
velocity_limits.resize(size);
acceleration_limits.resize(size);
velocity_limits.resize(size, 2);
acceleration_limits.resize(size, 2);
jerk_limits.resize(size, 2);
}

bool KinematicLimits::operator==(const KinematicLimits& rhs) const
Expand All @@ -47,6 +48,7 @@ bool KinematicLimits::operator==(const KinematicLimits& rhs) const
ret_val &= (joint_limits.isApprox(rhs.joint_limits, 1e-5));
ret_val &= (velocity_limits.isApprox(rhs.velocity_limits, 1e-5));
ret_val &= (acceleration_limits.isApprox(rhs.acceleration_limits, 1e-5));
ret_val &= (jerk_limits.isApprox(rhs.jerk_limits, 1e-5));
return ret_val;
}

Expand All @@ -58,47 +60,40 @@ void KinematicLimits::serialize(Archive& ar, const unsigned int /*version*/) //
ar& BOOST_SERIALIZATION_NVP(joint_limits);
ar& BOOST_SERIALIZATION_NVP(velocity_limits);
ar& BOOST_SERIALIZATION_NVP(acceleration_limits);
ar& BOOST_SERIALIZATION_NVP(jerk_limits);
}

template bool
isWithinPositionLimits<float>(const Eigen::Ref<const Eigen::Matrix<float, Eigen::Dynamic, 1>>& joint_positions,
const Eigen::Ref<const Eigen::Matrix<float, Eigen::Dynamic, 2>>& position_limits);
template bool isWithinLimits<float>(const Eigen::Ref<const Eigen::Matrix<float, Eigen::Dynamic, 1>>& values,
const Eigen::Ref<const Eigen::Matrix<float, Eigen::Dynamic, 2>>& limits);

template bool
isWithinPositionLimits<double>(const Eigen::Ref<const Eigen::Matrix<double, Eigen::Dynamic, 1>>& joint_positions,
const Eigen::Ref<const Eigen::Matrix<double, Eigen::Dynamic, 2>>& position_limits);
template bool isWithinLimits<double>(const Eigen::Ref<const Eigen::Matrix<double, Eigen::Dynamic, 1>>& values,
const Eigen::Ref<const Eigen::Matrix<double, Eigen::Dynamic, 2>>& limits);

template bool
satisfiesPositionLimits<float>(const Eigen::Ref<const Eigen::Matrix<float, Eigen::Dynamic, 1>>& joint_positions,
const Eigen::Ref<const Eigen::Matrix<float, Eigen::Dynamic, 2>>& position_limits,
float max_diff,
float max_rel_diff);
template bool satisfiesLimits<float>(const Eigen::Ref<const Eigen::Matrix<float, Eigen::Dynamic, 1>>& values,
const Eigen::Ref<const Eigen::Matrix<float, Eigen::Dynamic, 2>>& limits,
float max_diff,
float max_rel_diff);

template bool
satisfiesPositionLimits<double>(const Eigen::Ref<const Eigen::Matrix<double, Eigen::Dynamic, 1>>& joint_positions,
const Eigen::Ref<const Eigen::Matrix<double, Eigen::Dynamic, 2>>& position_limits,
double max_diff,
double max_rel_diff);
template bool satisfiesLimits<double>(const Eigen::Ref<const Eigen::Matrix<double, Eigen::Dynamic, 1>>& values,
const Eigen::Ref<const Eigen::Matrix<double, Eigen::Dynamic, 2>>& limits,
double max_diff,
double max_rel_diff);

template bool
satisfiesPositionLimits<float>(const Eigen::Ref<const Eigen::Matrix<float, Eigen::Dynamic, 1>>& joint_positions,
const Eigen::Ref<const Eigen::Matrix<float, Eigen::Dynamic, 2>>& position_limits,
const Eigen::Ref<const Eigen::Matrix<float, Eigen::Dynamic, 1>>& max_diff,
const Eigen::Ref<const Eigen::Matrix<float, Eigen::Dynamic, 1>>& max_rel_diff);
template bool satisfiesLimits<float>(const Eigen::Ref<const Eigen::Matrix<float, Eigen::Dynamic, 1>>& values,
const Eigen::Ref<const Eigen::Matrix<float, Eigen::Dynamic, 2>>& limits,
const Eigen::Ref<const Eigen::Matrix<float, Eigen::Dynamic, 1>>& max_diff,
const Eigen::Ref<const Eigen::Matrix<float, Eigen::Dynamic, 1>>& max_rel_diff);

template bool
satisfiesPositionLimits<double>(const Eigen::Ref<const Eigen::Matrix<double, Eigen::Dynamic, 1>>& joint_positions,
const Eigen::Ref<const Eigen::Matrix<double, Eigen::Dynamic, 2>>& position_limits,
const Eigen::Ref<const Eigen::Matrix<double, Eigen::Dynamic, 1>>& max_diff,
const Eigen::Ref<const Eigen::Matrix<double, Eigen::Dynamic, 1>>& max_rel_diff);
template bool satisfiesLimits<double>(const Eigen::Ref<const Eigen::Matrix<double, Eigen::Dynamic, 1>>& values,
const Eigen::Ref<const Eigen::Matrix<double, Eigen::Dynamic, 2>>& limits,
const Eigen::Ref<const Eigen::Matrix<double, Eigen::Dynamic, 1>>& max_diff,
const Eigen::Ref<const Eigen::Matrix<double, Eigen::Dynamic, 1>>& max_rel_diff);

template void
enforcePositionLimits<float>(Eigen::Ref<Eigen::Matrix<float, Eigen::Dynamic, 1>> joint_positions,
const Eigen::Ref<const Eigen::Matrix<float, Eigen::Dynamic, 2>>& position_limits);
template void enforceLimits<float>(Eigen::Ref<Eigen::Matrix<float, Eigen::Dynamic, 1>> values,
const Eigen::Ref<const Eigen::Matrix<float, Eigen::Dynamic, 2>>& limits);

template void
enforcePositionLimits<double>(Eigen::Ref<Eigen::Matrix<double, Eigen::Dynamic, 1>> joint_positions,
const Eigen::Ref<const Eigen::Matrix<double, Eigen::Dynamic, 2>>& position_limits);
template void enforceLimits<double>(Eigen::Ref<Eigen::Matrix<double, Eigen::Dynamic, 1>> values,
const Eigen::Ref<const Eigen::Matrix<double, Eigen::Dynamic, 2>>& limits);
} // namespace tesseract_common

#include <tesseract_common/serialization.h>
Expand Down
8 changes: 4 additions & 4 deletions tesseract_kinematics/core/include/tesseract_kinematics/core/utils.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -207,9 +207,9 @@ inline void getRedundantSolutionsHelper(std::vector<VectorX<FloatType>>& redunda
Eigen::VectorXd new_sol = sol;
new_sol[*current_index] = val;

if (tesseract_common::satisfiesPositionLimits<double>(new_sol, limits))
if (tesseract_common::satisfiesLimits<double>(new_sol, limits))
{
tesseract_common::enforcePositionLimits<double>(new_sol, limits);
tesseract_common::enforceLimits<double>(new_sol, limits);
redundant_sols.push_back(new_sol.template cast<FloatType>());
}

Expand Down Expand Up @@ -238,9 +238,9 @@ inline void getRedundantSolutionsHelper(std::vector<VectorX<FloatType>>& redunda
Eigen::VectorXd new_sol = sol;
new_sol[*current_index] = val;

if (tesseract_common::satisfiesPositionLimits<double>(new_sol, limits))
if (tesseract_common::satisfiesLimits<double>(new_sol, limits))
{
tesseract_common::enforcePositionLimits<double>(new_sol, limits);
tesseract_common::enforceLimits<double>(new_sol, limits);
redundant_sols.push_back(new_sol.template cast<FloatType>());
}

Expand Down
10 changes: 7 additions & 3 deletions tesseract_kinematics/core/src/joint_group.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -81,8 +81,12 @@ JointGroup::JointGroup(std::string name,
// Set limits
limits_.joint_limits(i, 0) = joint->limits->lower;
limits_.joint_limits(i, 1) = joint->limits->upper;
limits_.velocity_limits(i) = joint->limits->velocity;
limits_.acceleration_limits(i) = joint->limits->acceleration;
limits_.velocity_limits(i, 0) = -joint->limits->velocity;
limits_.velocity_limits(i, 1) = joint->limits->velocity;
limits_.acceleration_limits(i, 0) = -joint->limits->acceleration;
limits_.acceleration_limits(i, 1) = joint->limits->acceleration;
limits_.jerk_limits(i, 0) = -joint->limits->jerk;
limits_.jerk_limits(i, 1) = joint->limits->jerk;

// Set redundancy indices
switch (joint->type)
Expand Down Expand Up @@ -284,7 +288,7 @@ void JointGroup::setLimits(const tesseract_common::KinematicLimits& limits)
{
Eigen::Index nj = numJoints();
if (limits.joint_limits.rows() != nj || limits.velocity_limits.size() != nj ||
limits.acceleration_limits.size() != nj)
limits.acceleration_limits.size() != nj || limits.jerk_limits.size() != nj)
throw std::runtime_error("Kinematics Group limits assigned are invalid!");

limits_ = limits;
Expand Down
4 changes: 2 additions & 2 deletions tesseract_kinematics/core/src/kinematic_group.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -167,7 +167,7 @@ IKSolutions KinematicGroup::calcInvKin(const KinGroupIKInputs& tip_link_poses,
ordered_sol(i) = solution(inv_kin_joint_map_[static_cast<std::size_t>(i)]);

tesseract_kinematics::harmonizeTowardMedian<double>(ordered_sol, redundancy_indices_, limits_.joint_limits);
if (tesseract_common::satisfiesPositionLimits<double>(ordered_sol, limits_.joint_limits))
if (tesseract_common::satisfiesLimits<double>(ordered_sol, limits_.joint_limits))
solutions_filtered.push_back(ordered_sol);
}

Expand All @@ -180,7 +180,7 @@ IKSolutions KinematicGroup::calcInvKin(const KinGroupIKInputs& tip_link_poses,
for (auto& solution : solutions)
{
tesseract_kinematics::harmonizeTowardMedian<double>(solution, redundancy_indices_, limits_.joint_limits);
if (tesseract_common::satisfiesPositionLimits<double>(solution, limits_.joint_limits))
if (tesseract_common::satisfiesLimits<double>(solution, limits_.joint_limits))
solutions_filtered.push_back(solution);
}

Expand Down
3 changes: 2 additions & 1 deletion tesseract_scene_graph/include/tesseract_scene_graph/joint.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -93,7 +93,7 @@ class JointLimits
using ConstPtr = std::shared_ptr<const JointLimits>;

JointLimits() = default;
JointLimits(double l, double u, double e, double v, double a);
JointLimits(double l, double u, double e, double v, double a, double j);
~JointLimits() = default;

JointLimits(const JointLimits&) = default;
Expand All @@ -106,6 +106,7 @@ class JointLimits
double effort{ 0 };
double velocity{ 0 };
double acceleration{ 0 };
double jerk{ 0 };

void clear();

Expand Down
3 changes: 2 additions & 1 deletion tesseract_scene_graph/src/graph.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -500,7 +500,7 @@ bool SceneGraph::addJointHelper(const std::shared_ptr<Joint>& joint_ptr)
{
if (joint_ptr->limits == nullptr)
{
joint_ptr->limits = std::make_shared<JointLimits>(-4 * M_PI, 4 * M_PI, 0, 2, 1);
joint_ptr->limits = std::make_shared<JointLimits>(-4 * M_PI, 4 * M_PI, 0, 2, 1, 0.5);
}
else if (tesseract_common::almostEqualRelativeAndAbs(joint_ptr->limits->lower, joint_ptr->limits->upper, 1e-5))
{
Expand Down Expand Up @@ -651,6 +651,7 @@ bool SceneGraph::changeJointLimits(const std::string& name, const JointLimits& l
found->second.first->limits->effort = limits.effort;
found->second.first->limits->velocity = limits.velocity;
found->second.first->limits->acceleration = limits.acceleration;
found->second.first->limits->jerk = limits.jerk;

return true;
}
Expand Down
Loading

0 comments on commit 490f33f

Please sign in to comment.