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 660d42d
Show file tree
Hide file tree
Showing 24 changed files with 354 additions and 192 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
4 changes: 2 additions & 2 deletions tesseract_common/test/tesseract_common_serialization_unit.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -60,8 +60,8 @@ TEST(TesseractCommonSerializeUnit, KinematicLimits) // NOLINT
EXPECT_EQ(limits.acceleration_limits.rows(), 3);

limits.joint_limits << -5, 5, -5, 5, -5, 5;
limits.velocity_limits = Eigen::VectorXd::Constant(3, 6);
limits.acceleration_limits = Eigen::VectorXd::Constant(3, 7);
limits.velocity_limits << -6, 6, -6, 6, -6, 6;
limits.acceleration_limits << -7, 7, -7, 7, -7, 7;

tesseract_common::testSerialization<KinematicLimits>(limits, "KinematicLimits");
}
Expand Down
38 changes: 19 additions & 19 deletions tesseract_common/test/tesseract_common_unit.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -420,45 +420,45 @@ TEST(TesseractCommonUnit, boundsUnit) // NOLINT
limits.col(0) = -Eigen::VectorXd::Ones(6);
limits.col(1) = Eigen::VectorXd::Ones(6);

EXPECT_FALSE(tesseract_common::isWithinPositionLimits<double>(v, limits));
EXPECT_TRUE(tesseract_common::satisfiesPositionLimits<double>(v, limits, std::numeric_limits<float>::epsilon()));
EXPECT_FALSE(tesseract_common::satisfiesPositionLimits<double>(v, limits, std::numeric_limits<double>::epsilon()));
tesseract_common::enforcePositionLimits<double>(v, limits);
EXPECT_TRUE(tesseract_common::satisfiesPositionLimits<double>(v, limits, std::numeric_limits<double>::epsilon()));
EXPECT_FALSE(tesseract_common::isWithinLimits<double>(v, limits));
EXPECT_TRUE(tesseract_common::satisfiesLimits<double>(v, limits, std::numeric_limits<float>::epsilon()));
EXPECT_FALSE(tesseract_common::satisfiesLimits<double>(v, limits, std::numeric_limits<double>::epsilon()));
tesseract_common::enforceLimits<double>(v, limits);
EXPECT_TRUE(tesseract_common::satisfiesLimits<double>(v, limits, std::numeric_limits<double>::epsilon()));

v = -Eigen::VectorXd::Ones(6);
v = v.array() - std::numeric_limits<float>::epsilon();

EXPECT_FALSE(tesseract_common::isWithinPositionLimits<double>(v, limits));
EXPECT_TRUE(tesseract_common::satisfiesPositionLimits<double>(v, limits, std::numeric_limits<float>::epsilon()));
EXPECT_FALSE(tesseract_common::satisfiesPositionLimits<double>(v, limits, std::numeric_limits<double>::epsilon()));
tesseract_common::enforcePositionLimits<double>(v, limits);
EXPECT_TRUE(tesseract_common::satisfiesPositionLimits<double>(v, limits, std::numeric_limits<double>::epsilon()));
EXPECT_FALSE(tesseract_common::isWithinLimits<double>(v, limits));
EXPECT_TRUE(tesseract_common::satisfiesLimits<double>(v, limits, std::numeric_limits<float>::epsilon()));
EXPECT_FALSE(tesseract_common::satisfiesLimits<double>(v, limits, std::numeric_limits<double>::epsilon()));
tesseract_common::enforceLimits<double>(v, limits);
EXPECT_TRUE(tesseract_common::satisfiesLimits<double>(v, limits, std::numeric_limits<double>::epsilon()));

// Check that clamp is done correctly on both sides
v = Eigen::VectorXd::Constant(6, -2);
EXPECT_FALSE(tesseract_common::isWithinPositionLimits<double>(v, limits));
EXPECT_FALSE(tesseract_common::satisfiesPositionLimits<double>(v, limits, std::numeric_limits<double>::epsilon()));
tesseract_common::enforcePositionLimits<double>(v, limits);
EXPECT_FALSE(tesseract_common::isWithinLimits<double>(v, limits));
EXPECT_FALSE(tesseract_common::satisfiesLimits<double>(v, limits, std::numeric_limits<double>::epsilon()));
tesseract_common::enforceLimits<double>(v, limits);
ASSERT_EQ((v - limits.col(0)).norm(), 0);

v = Eigen::VectorXd::Constant(6, 2);
EXPECT_FALSE(tesseract_common::isWithinPositionLimits<double>(v, limits));
EXPECT_FALSE(tesseract_common::satisfiesPositionLimits<double>(v, limits, std::numeric_limits<double>::epsilon()));
tesseract_common::enforcePositionLimits<double>(v, limits);
EXPECT_FALSE(tesseract_common::isWithinLimits<double>(v, limits));
EXPECT_FALSE(tesseract_common::satisfiesLimits<double>(v, limits, std::numeric_limits<double>::epsilon()));
tesseract_common::enforceLimits<double>(v, limits);
ASSERT_EQ((v - limits.col(1)).norm(), 0);

v = Eigen::VectorXd::Ones(6);
v = v.array() - std::numeric_limits<float>::epsilon();
EXPECT_TRUE(tesseract_common::isWithinPositionLimits<double>(v, limits));
EXPECT_TRUE(tesseract_common::isWithinLimits<double>(v, limits));

v = Eigen::VectorXd::Ones(6);
v(3) = v(3) + std::numeric_limits<float>::epsilon();
EXPECT_FALSE(tesseract_common::isWithinPositionLimits<double>(v, limits));
EXPECT_FALSE(tesseract_common::isWithinLimits<double>(v, limits));

v = -Eigen::VectorXd::Ones(6);
v(3) = v(3) - std::numeric_limits<float>::epsilon();
EXPECT_FALSE(tesseract_common::isWithinPositionLimits<double>(v, limits));
EXPECT_FALSE(tesseract_common::isWithinLimits<double>(v, limits));
}

TEST(TesseractCommonUnit, isIdenticalUnit) // NOLINT
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
Loading

0 comments on commit 660d42d

Please sign in to comment.