From 660d42d6bc6e9cdbab551c9a12e637875ee92321 Mon Sep 17 00:00:00 2001
From: Levi Armstrong <levi.armstrong@gmail.com>
Date: Tue, 28 May 2024 10:51:19 -0500
Subject: [PATCH] Add support for jerk limits

---
 .../tesseract_common/kinematic_limits.h       | 92 ++++++++++---------
 tesseract_common/src/kinematic_limits.cpp     | 63 ++++++-------
 .../tesseract_common_serialization_unit.cpp   |  4 +-
 .../test/tesseract_common_unit.cpp            | 38 ++++----
 .../include/tesseract_kinematics/core/utils.h |  8 +-
 tesseract_kinematics/core/src/joint_group.cpp | 12 ++-
 .../core/src/kinematic_group.cpp              |  4 +-
 .../test/kinematics_core_unit.cpp             |  8 +-
 .../test/kinematics_test_utils.h              | 32 +++++--
 .../include/tesseract_scene_graph/graph.h     |  8 ++
 .../include/tesseract_scene_graph/joint.h     |  3 +-
 tesseract_scene_graph/src/graph.cpp           | 29 +++++-
 tesseract_scene_graph/src/joint.cpp           | 10 +-
 ...sseract_scene_graph_serialization_unit.cpp |  8 +-
 .../test/tesseract_scene_graph_unit.cpp       | 43 ++++++---
 .../tesseract_srdf_serialization_unit.cpp     | 12 +--
 tesseract_srdf/test/tesseract_srdf_unit.cpp   | 38 ++++----
 .../mutable_state_solver.h                    |  8 ++
 .../ofkt/ofkt_state_solver.h                  |  2 +
 .../src/kdl_state_solver.cpp                  | 13 ++-
 .../src/ofkt_state_solver.cpp                 | 52 ++++++++---
 .../test/state_solver_test_suite.h            | 45 +++++++--
 tesseract_urdf/src/joint.cpp                  |  3 +-
 tesseract_urdf/src/limits.cpp                 | 11 +++
 24 files changed, 354 insertions(+), 192 deletions(-)

diff --git a/tesseract_common/include/tesseract_common/kinematic_limits.h b/tesseract_common/include/tesseract_common/kinematic_limits.h
index f6239139c0b..4eca0951137 100644
--- a/tesseract_common/include/tesseract_common/kinematic_limits.h
+++ b/tesseract_common/include/tesseract_common/kinematic_limits.h
@@ -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);
 
@@ -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();
 
@@ -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
 
diff --git a/tesseract_common/src/kinematic_limits.cpp b/tesseract_common/src/kinematic_limits.cpp
index 6a4c4b81e55..ad726b1683b 100644
--- a/tesseract_common/src/kinematic_limits.cpp
+++ b/tesseract_common/src/kinematic_limits.cpp
@@ -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
@@ -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;
 }
 
@@ -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>
diff --git a/tesseract_common/test/tesseract_common_serialization_unit.cpp b/tesseract_common/test/tesseract_common_serialization_unit.cpp
index fc1f9c3065a..d5e73a1ccf2 100644
--- a/tesseract_common/test/tesseract_common_serialization_unit.cpp
+++ b/tesseract_common/test/tesseract_common_serialization_unit.cpp
@@ -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");
 }
diff --git a/tesseract_common/test/tesseract_common_unit.cpp b/tesseract_common/test/tesseract_common_unit.cpp
index c3b25f244ad..869feccf72c 100644
--- a/tesseract_common/test/tesseract_common_unit.cpp
+++ b/tesseract_common/test/tesseract_common_unit.cpp
@@ -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
diff --git a/tesseract_kinematics/core/include/tesseract_kinematics/core/utils.h b/tesseract_kinematics/core/include/tesseract_kinematics/core/utils.h
index da142302f02..e6b012e5f74 100644
--- a/tesseract_kinematics/core/include/tesseract_kinematics/core/utils.h
+++ b/tesseract_kinematics/core/include/tesseract_kinematics/core/utils.h
@@ -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>());
           }
 
@@ -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>());
           }
 
diff --git a/tesseract_kinematics/core/src/joint_group.cpp b/tesseract_kinematics/core/src/joint_group.cpp
index 33c7076fc37..03f17545cfe 100644
--- a/tesseract_kinematics/core/src/joint_group.cpp
+++ b/tesseract_kinematics/core/src/joint_group.cpp
@@ -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)
@@ -283,8 +287,8 @@ tesseract_common::KinematicLimits JointGroup::getLimits() const { return limits_
 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)
+  if (limits.joint_limits.rows() != nj || limits.velocity_limits.rows() != nj ||
+      limits.acceleration_limits.rows() != nj || limits.jerk_limits.rows() != nj)
     throw std::runtime_error("Kinematics Group limits assigned are invalid!");
 
   limits_ = limits;
diff --git a/tesseract_kinematics/core/src/kinematic_group.cpp b/tesseract_kinematics/core/src/kinematic_group.cpp
index 0fc0f1b7da3..7e8df92bd27 100644
--- a/tesseract_kinematics/core/src/kinematic_group.cpp
+++ b/tesseract_kinematics/core/src/kinematic_group.cpp
@@ -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);
     }
 
@@ -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);
   }
 
diff --git a/tesseract_kinematics/test/kinematics_core_unit.cpp b/tesseract_kinematics/test/kinematics_core_unit.cpp
index 41e9da9c58d..d83005cede1 100644
--- a/tesseract_kinematics/test/kinematics_core_unit.cpp
+++ b/tesseract_kinematics/test/kinematics_core_unit.cpp
@@ -171,7 +171,7 @@ void runRedundantSolutionsTest()
     {  // Test when initial solution is at the lower limit
       std::vector<tesseract_kinematics::VectorX<FloatType>> solutions =
           tesseract_kinematics::getRedundantSolutions<FloatType>(q, limits, redundancy_capable_joints);
-      if (tesseract_common::satisfiesPositionLimits<double>(q.template cast<double>(), limits, max_diff))
+      if (tesseract_common::satisfiesLimits<double>(q.template cast<double>(), limits, max_diff))
         solutions.push_back(q);
 
       EXPECT_EQ(solutions.size(), 8);
@@ -182,7 +182,7 @@ void runRedundantSolutionsTest()
       limits << -2.0 * M_PI, 2.0 * M_PI, -2.0 * M_PI, 2.0 * M_PI, -2.0 * M_PI, 2.0 * M_PI;
       std::vector<tesseract_kinematics::VectorX<FloatType>> solutions =
           tesseract_kinematics::getRedundantSolutions<FloatType>(q, limits, redundancy_capable_joints);
-      if (tesseract_common::satisfiesPositionLimits<double>(q.template cast<double>(), limits, max_diff))
+      if (tesseract_common::satisfiesLimits<double>(q.template cast<double>(), limits, max_diff))
         solutions.push_back(q);
 
       EXPECT_EQ(solutions.size(), 27);
@@ -195,7 +195,7 @@ void runRedundantSolutionsTest()
 
       std::vector<tesseract_kinematics::VectorX<FloatType>> solutions =
           tesseract_kinematics::getRedundantSolutions<FloatType>(q, limits, redundancy_capable_joints);
-      if (tesseract_common::satisfiesPositionLimits<double>(q.template cast<double>(), limits, max_diff))
+      if (tesseract_common::satisfiesLimits<double>(q.template cast<double>(), limits, max_diff))
         solutions.push_back(q);
 
       EXPECT_EQ(solutions.size(), 27);
@@ -217,7 +217,7 @@ void runRedundantSolutionsTest()
 
     std::vector<tesseract_kinematics::VectorX<FloatType>> solutions =
         tesseract_kinematics::getRedundantSolutions<FloatType>(q, limits, redundancy_capable_joints);
-    if (tesseract_common::satisfiesPositionLimits<double>(q.template cast<double>(), limits, max_diff))
+    if (tesseract_common::satisfiesLimits<double>(q.template cast<double>(), limits, max_diff))
       solutions.push_back(q);
 
     EXPECT_EQ(solutions.size(), 27);
diff --git a/tesseract_kinematics/test/kinematics_test_utils.h b/tesseract_kinematics/test/kinematics_test_utils.h
index c14f7e168a9..b70562724d2 100644
--- a/tesseract_kinematics/test/kinematics_test_utils.h
+++ b/tesseract_kinematics/test/kinematics_test_utils.h
@@ -242,8 +242,12 @@ inline tesseract_common::KinematicLimits getTargetLimits(const tesseract_scene_g
     auto joint = scene_graph.getJoint(joint_names[static_cast<std::size_t>(i)]);
     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;
   }
 
   return limits;
@@ -372,14 +376,22 @@ inline void runKinJointLimitsTest(const tesseract_common::KinematicLimits& limit
   EXPECT_EQ(limits.joint_limits.rows(), target_limits.joint_limits.rows());
   EXPECT_EQ(limits.velocity_limits.rows(), target_limits.velocity_limits.rows());
   EXPECT_EQ(limits.acceleration_limits.rows(), target_limits.acceleration_limits.rows());
+  EXPECT_EQ(limits.jerk_limits.rows(), target_limits.jerk_limits.rows());
 
   // Check limits
   for (Eigen::Index i = 0; i < limits.joint_limits.rows(); ++i)
   {
     EXPECT_NEAR(limits.joint_limits(i, 0), target_limits.joint_limits(i, 0), 1e-6);
     EXPECT_NEAR(limits.joint_limits(i, 1), target_limits.joint_limits(i, 1), 1e-6);
-    EXPECT_NEAR(limits.velocity_limits(i), target_limits.velocity_limits(i), 1e-6);
-    EXPECT_NEAR(limits.acceleration_limits(i), target_limits.acceleration_limits(i), 1e-6);
+
+    EXPECT_NEAR(limits.velocity_limits(i, 0), target_limits.velocity_limits(i, 0), 1e-6);
+    EXPECT_NEAR(limits.velocity_limits(i, 1), target_limits.velocity_limits(i, 1), 1e-6);
+
+    EXPECT_NEAR(limits.acceleration_limits(i, 0), target_limits.acceleration_limits(i, 0), 1e-6);
+    EXPECT_NEAR(limits.acceleration_limits(i, 1), target_limits.acceleration_limits(i, 1), 1e-6);
+
+    EXPECT_NEAR(limits.jerk_limits(i, 0), target_limits.jerk_limits(i, 0), 1e-6);
+    EXPECT_NEAR(limits.jerk_limits(i, 1), target_limits.jerk_limits(i, 1), 1e-6);
   }
 }
 
@@ -396,14 +408,22 @@ inline void runKinSetJointLimitsTest(tesseract_kinematics::KinematicGroup& kin_g
   EXPECT_TRUE(limits.joint_limits.rows() > 0);
   EXPECT_TRUE(limits.velocity_limits.rows() > 0);
   EXPECT_TRUE(limits.acceleration_limits.rows() > 0);
+  EXPECT_TRUE(limits.jerk_limits.rows() > 0);
 
   // Check limits
   for (Eigen::Index i = 0; i < limits.joint_limits.rows(); ++i)
   {
     limits.joint_limits(i, 0) = -5.0 - double(i);
     limits.joint_limits(i, 1) = 5.0 + double(i);
-    limits.velocity_limits(i) = 10.0 + double(i);
-    limits.acceleration_limits(i) = 5.0 + double(i);
+
+    limits.velocity_limits(i, 0) = -10.0 - double(i);
+    limits.velocity_limits(i, 1) = 10.0 + double(i);
+
+    limits.acceleration_limits(i, 0) = -5.0 - double(i);
+    limits.acceleration_limits(i, 1) = 5.0 + double(i);
+
+    limits.jerk_limits(i, 0) = -5.0 - double(i);
+    limits.jerk_limits(i, 1) = 5.0 + double(i);
   }
 
   kin_group.setLimits(limits);
diff --git a/tesseract_scene_graph/include/tesseract_scene_graph/graph.h b/tesseract_scene_graph/include/tesseract_scene_graph/graph.h
index 839cfc541ef..107877e10ad 100644
--- a/tesseract_scene_graph/include/tesseract_scene_graph/graph.h
+++ b/tesseract_scene_graph/include/tesseract_scene_graph/graph.h
@@ -346,6 +346,14 @@ class SceneGraph
    */
   bool changeJointAccelerationLimits(const std::string& name, double limit);
 
+  /**
+   * @brief Changes the jerk limits associated with a joint
+   * @param joint_name Name of the joint to be updated
+   * @param limits New jerk limits to be set as the joint limits
+   * @return
+   */
+  bool changeJointJerkLimits(const std::string& name, double limit);
+
   /**
    * @brief Gets the limits of the joint specified by name
    * @param name Name of the joint which limits will be retrieved
diff --git a/tesseract_scene_graph/include/tesseract_scene_graph/joint.h b/tesseract_scene_graph/include/tesseract_scene_graph/joint.h
index 215574d5eb3..0fbccf5aa08 100644
--- a/tesseract_scene_graph/include/tesseract_scene_graph/joint.h
+++ b/tesseract_scene_graph/include/tesseract_scene_graph/joint.h
@@ -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;
@@ -106,6 +106,7 @@ class JointLimits
   double effort{ 0 };
   double velocity{ 0 };
   double acceleration{ 0 };
+  double jerk{ 0 };
 
   void clear();
 
diff --git a/tesseract_scene_graph/src/graph.cpp b/tesseract_scene_graph/src/graph.cpp
index 28e4840dd59..07712af9a67 100644
--- a/tesseract_scene_graph/src/graph.cpp
+++ b/tesseract_scene_graph/src/graph.cpp
@@ -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, 1000);
     }
     else if (tesseract_common::almostEqualRelativeAndAbs(joint_ptr->limits->lower, joint_ptr->limits->upper, 1e-5))
     {
@@ -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;
 }
@@ -726,6 +727,32 @@ bool SceneGraph::changeJointAccelerationLimits(const std::string& name, double l
   return true;
 }
 
+bool SceneGraph::changeJointJerkLimits(const std::string& name, double limit)
+{
+  auto found = joint_map_.find(name);
+
+  if (found == joint_map_.end())
+  {
+    CONSOLE_BRIDGE_logWarn("Tried to change Joint Jerk limit with name (%s) which does not exist in scene "
+                           "graph.",
+                           name.c_str());
+    return false;
+  }
+
+  if (found->second.first->type == JointType::FIXED || found->second.first->type == JointType::FLOATING)
+  {
+    CONSOLE_BRIDGE_logWarn("Tried to change Joint Jerk limit for a fixed or floating joint type.", name.c_str());
+    return false;
+  }
+
+  if (found->second.first->limits == nullptr)
+    found->second.first->limits = std::make_shared<JointLimits>();
+
+  found->second.first->limits->jerk = limit;
+
+  return true;
+}
+
 std::shared_ptr<const JointLimits> SceneGraph::getJointLimits(const std::string& name)
 {
   auto found = joint_map_.find(name);
diff --git a/tesseract_scene_graph/src/joint.cpp b/tesseract_scene_graph/src/joint.cpp
index 37f77f22beb..8b68c103c85 100644
--- a/tesseract_scene_graph/src/joint.cpp
+++ b/tesseract_scene_graph/src/joint.cpp
@@ -74,8 +74,8 @@ std::ostream& operator<<(std::ostream& os, const JointDynamics& dynamics)
 /******              JointLimits                     *****/
 /*********************************************************/
 
-JointLimits::JointLimits(double l, double u, double e, double v, double a)
-  : lower(l), upper(u), effort(e), velocity(v), acceleration(a)
+JointLimits::JointLimits(double l, double u, double e, double v, double a, double j)
+  : lower(l), upper(u), effort(e), velocity(v), acceleration(a), jerk(j)
 {
 }
 
@@ -86,6 +86,7 @@ void JointLimits::clear()
   effort = 0;
   velocity = 0;
   acceleration = 0;
+  jerk = 0;
 }
 
 bool JointLimits::operator==(const JointLimits& rhs) const
@@ -96,6 +97,7 @@ bool JointLimits::operator==(const JointLimits& rhs) const
   equal &= tesseract_common::almostEqualRelativeAndAbs(effort, rhs.effort);
   equal &= tesseract_common::almostEqualRelativeAndAbs(velocity, rhs.velocity);
   equal &= tesseract_common::almostEqualRelativeAndAbs(acceleration, rhs.acceleration);
+  equal &= tesseract_common::almostEqualRelativeAndAbs(jerk, rhs.jerk);
 
   return equal;
 }
@@ -109,12 +111,14 @@ void JointLimits::serialize(Archive& ar, const unsigned int /*version*/)
   ar& BOOST_SERIALIZATION_NVP(effort);
   ar& BOOST_SERIALIZATION_NVP(velocity);
   ar& BOOST_SERIALIZATION_NVP(acceleration);
+  ar& BOOST_SERIALIZATION_NVP(jerk);
 }
 
 std::ostream& operator<<(std::ostream& os, const JointLimits& limits)
 {
   os << "lower=" << limits.lower << " upper=" << limits.upper << " effort=" << limits.effort
-     << " velocity=" << limits.velocity << " acceleration=" << limits.acceleration;
+     << " velocity=" << limits.velocity << " acceleration=" << limits.acceleration << " jerk=" << limits.jerk;
+  ;
   return os;
 }
 
diff --git a/tesseract_scene_graph/test/tesseract_scene_graph_serialization_unit.cpp b/tesseract_scene_graph/test/tesseract_scene_graph_serialization_unit.cpp
index 0bcaec0f989..344ad7c75ab 100644
--- a/tesseract_scene_graph/test/tesseract_scene_graph_serialization_unit.cpp
+++ b/tesseract_scene_graph/test/tesseract_scene_graph_serialization_unit.cpp
@@ -52,7 +52,7 @@ TEST(TesseractSceneGraphSerializationUnit, JointDynamics)  // NOLINT
 
 TEST(TesseractSceneGraphSerializationUnit, JointLimits)  // NOLINT
 {
-  auto object = std::make_shared<JointLimits>(1.1, 2.2, 3.3, 4.4, 5.5);
+  auto object = std::make_shared<JointLimits>(1.1, 2.2, 3.3, 4.4, 5.5, 6.5);
   tesseract_common::testSerialization<JointLimits>(*object, "JointLimits");
 }
 
@@ -83,7 +83,7 @@ TEST(TesseractSceneGraphSerializationUnit, Joint)  // NOLINT
   object->parent_link_name = "parent_name";
   object->parent_to_joint_origin_transform.translate(Eigen::Vector3d(5.5, 6.6, 7.7));
   object->dynamics = std::make_shared<JointDynamics>(1.1, 2.2);
-  object->limits = std::make_shared<JointLimits>(1.1, 2.2, 3.3, 4.4, 5.5);
+  object->limits = std::make_shared<JointLimits>(1.1, 2.2, 3.3, 4.4, 5.5, 6.5);
   object->safety = std::make_shared<JointSafety>(1.1, 2.2, 3.3, 4.4);
   object->calibration = std::make_shared<JointCalibration>(1.1, 2.2, 3.3);
   object->mimic = std::make_shared<JointMimic>(1.1, 2.2, "mimic_name");
@@ -186,7 +186,7 @@ tesseract_scene_graph::SceneGraph createTestSceneGraph()
   joint_2.parent_link_name = "link_2";
   joint_2.child_link_name = "link_3";
   joint_2.type = JointType::PLANAR;
-  joint_2.limits = std::make_shared<JointLimits>(-1, 1, 0, 2, 3);
+  joint_2.limits = std::make_shared<JointLimits>(-1, 1, 0, 2, 3, 4);
   EXPECT_TRUE(g.addJoint(joint_2));
 
   Joint joint_3("joint_3");
@@ -201,7 +201,7 @@ tesseract_scene_graph::SceneGraph createTestSceneGraph()
   joint_4.parent_link_name = "link_2";
   joint_4.child_link_name = "link_5";
   joint_4.type = JointType::REVOLUTE;
-  joint_4.limits = std::make_shared<JointLimits>(-1, 1, 0, 2, 3);
+  joint_4.limits = std::make_shared<JointLimits>(-1, 1, 0, 2, 3, 4);
   EXPECT_TRUE(g.addJoint(joint_4));
 
   g.addAllowedCollision("link_1", "link_2", "Adjacent");
diff --git a/tesseract_scene_graph/test/tesseract_scene_graph_unit.cpp b/tesseract_scene_graph/test/tesseract_scene_graph_unit.cpp
index ec1ca5300b7..9d4f9762be5 100644
--- a/tesseract_scene_graph/test/tesseract_scene_graph_unit.cpp
+++ b/tesseract_scene_graph/test/tesseract_scene_graph_unit.cpp
@@ -66,7 +66,7 @@ tesseract_scene_graph::SceneGraph createTestSceneGraph()
   joint_2.parent_link_name = "link_2";
   joint_2.child_link_name = "link_3";
   joint_2.type = JointType::PLANAR;
-  joint_2.limits = std::make_shared<JointLimits>(-1, 1, 0, 2, 3);
+  joint_2.limits = std::make_shared<JointLimits>(-1, 1, 0, 2, 3, 6);
   EXPECT_TRUE(g.addJoint(joint_2));
 
   Joint joint_3("joint_3");
@@ -81,7 +81,7 @@ tesseract_scene_graph::SceneGraph createTestSceneGraph()
   joint_4.parent_link_name = "link_2";
   joint_4.child_link_name = "link_5";
   joint_4.type = JointType::REVOLUTE;
-  joint_4.limits = std::make_shared<JointLimits>(-1, 1, 0, 2, 3);
+  joint_4.limits = std::make_shared<JointLimits>(-1, 1, 0, 2, 3, 4);
   EXPECT_TRUE(g.addJoint(joint_4));
 
   g.addAllowedCollision("link_1", "link_2", "Adjacent");
@@ -631,6 +631,27 @@ TEST(TesseractSceneGraphUnit, TesseractSceneGraphChangeJointAccelerationLimitsUn
   EXPECT_FALSE(g.changeJointAccelerationLimits("joint_3", 20));
 }
 
+TEST(TesseractSceneGraphUnit, TesseractSceneGraphChangeJointJerkLimitsUnit)  // NOLINT
+{
+  using namespace tesseract_scene_graph;
+  SceneGraph g = createTestSceneGraph();
+
+  Joint::ConstPtr j = g.getJoint("joint_4");
+  EXPECT_EQ(g.getLinks().size(), 5);
+  EXPECT_EQ(g.getJoints().size(), 4);
+  EXPECT_TRUE(g.changeJointJerkLimits("joint_4", 20));
+  EXPECT_DOUBLE_EQ(j->limits->jerk, 20);
+  EXPECT_EQ(g.getLinks().size(), 5);
+  EXPECT_EQ(g.getJoints().size(), 4);
+
+  // Joint does not exist
+  EXPECT_FALSE(g.changeJointJerkLimits("joint_does_not_exist", 20));
+
+  // Cannot change limits of fixed or floating joint
+  EXPECT_FALSE(g.changeJointJerkLimits("joint_1", 20));
+  EXPECT_FALSE(g.changeJointJerkLimits("joint_3", 20));
+}
+
 TEST(TesseractSceneGraphUnit, TesseractSceneGraphMoveJointUnit)  // NOLINT
 {
   using namespace tesseract_scene_graph;
@@ -727,7 +748,7 @@ TEST(TesseractSceneGraphUnit, TesseractSceneGraphAddJointUnit)  // NOLINT
   joint_8.parent_link_name = "link_7";
   joint_8.child_link_name = "link_6";
   joint_8.type = JointType::CONTINUOUS;
-  joint_8.limits = std::make_shared<JointLimits>(0, 0, 0, 2, 1);
+  joint_8.limits = std::make_shared<JointLimits>(0, 0, 0, 2, 1, 0.5);
   EXPECT_TRUE(g.addJoint(joint_8));
   EXPECT_EQ(g.getJoints().size(), 6);
   EXPECT_TRUE(g.getJoint("joint_7")->limits != nullptr);
@@ -816,7 +837,7 @@ tesseract_scene_graph::SceneGraph buildTestSceneGraph()
   joint_1.parent_link_name = "link_1";
   joint_1.child_link_name = "link_2";
   joint_1.type = JointType::REVOLUTE;
-  joint_1.limits = std::make_shared<JointLimits>(-1, 1, 0, 2, 3);
+  joint_1.limits = std::make_shared<JointLimits>(-1, 1, 0, 2, 3, 4);
   EXPECT_TRUE(g.addJoint(joint_1));
 
   Joint joint_2("joint_2");
@@ -824,7 +845,7 @@ tesseract_scene_graph::SceneGraph buildTestSceneGraph()
   joint_2.parent_link_name = "link_2";
   joint_2.child_link_name = "link_3";
   joint_2.type = JointType::REVOLUTE;
-  joint_2.limits = std::make_shared<JointLimits>(-1, 1, 0, 2, 3);
+  joint_2.limits = std::make_shared<JointLimits>(-1, 1, 0, 2, 3, 4);
   EXPECT_TRUE(g.addJoint(joint_2));
 
   Joint joint_3("joint_3");
@@ -832,7 +853,7 @@ tesseract_scene_graph::SceneGraph buildTestSceneGraph()
   joint_3.parent_link_name = "link_3";
   joint_3.child_link_name = "link_4";
   joint_3.type = JointType::REVOLUTE;
-  joint_3.limits = std::make_shared<JointLimits>(-1, 1, 0, 2, 3);
+  joint_3.limits = std::make_shared<JointLimits>(-1, 1, 0, 2, 3, 4);
   EXPECT_TRUE(g.addJoint(joint_3));
 
   Joint joint_4("joint_4");
@@ -840,7 +861,7 @@ tesseract_scene_graph::SceneGraph buildTestSceneGraph()
   joint_4.parent_link_name = "link_2";
   joint_4.child_link_name = "link_5";
   joint_4.type = JointType::REVOLUTE;
-  joint_4.limits = std::make_shared<JointLimits>(-1, 1, 0, 2, 3);
+  joint_4.limits = std::make_shared<JointLimits>(-1, 1, 0, 2, 3, 4);
   EXPECT_TRUE(g.addJoint(joint_4));
 
   return g;
@@ -886,7 +907,7 @@ tesseract_scene_graph::SceneGraph buildTestSceneGraphForSubTree()
     joint_1.parent_link_name = p + "link_1";
     joint_1.child_link_name = p + "link_2";
     joint_1.type = JointType::REVOLUTE;
-    joint_1.limits = std::make_shared<JointLimits>(-1, 1, 0, 2, 3);
+    joint_1.limits = std::make_shared<JointLimits>(-1, 1, 0, 2, 3, 4);
     EXPECT_TRUE(g.addJoint(joint_1));
 
     Joint joint_2(p + "joint_2");
@@ -894,7 +915,7 @@ tesseract_scene_graph::SceneGraph buildTestSceneGraphForSubTree()
     joint_2.parent_link_name = p + "link_2";
     joint_2.child_link_name = p + "link_3";
     joint_2.type = JointType::REVOLUTE;
-    joint_2.limits = std::make_shared<JointLimits>(-1, 1, 0, 2, 3);
+    joint_2.limits = std::make_shared<JointLimits>(-1, 1, 0, 2, 3, 4);
     EXPECT_TRUE(g.addJoint(joint_2));
 
     Joint joint_3(p + "joint_3");
@@ -902,7 +923,7 @@ tesseract_scene_graph::SceneGraph buildTestSceneGraphForSubTree()
     joint_3.parent_link_name = p + "link_3";
     joint_3.child_link_name = p + "link_4";
     joint_3.type = JointType::REVOLUTE;
-    joint_3.limits = std::make_shared<JointLimits>(-1, 1, 0, 2, 3);
+    joint_3.limits = std::make_shared<JointLimits>(-1, 1, 0, 2, 3, 4);
     EXPECT_TRUE(g.addJoint(joint_3));
 
     Joint joint_4(p + "joint_4");
@@ -910,7 +931,7 @@ tesseract_scene_graph::SceneGraph buildTestSceneGraphForSubTree()
     joint_4.parent_link_name = p + "link_2";
     joint_4.child_link_name = p + "link_5";
     joint_4.type = JointType::REVOLUTE;
-    joint_4.limits = std::make_shared<JointLimits>(-1, 1, 0, 2, 3);
+    joint_4.limits = std::make_shared<JointLimits>(-1, 1, 0, 2, 3, 4);
     EXPECT_TRUE(g.addJoint(joint_4));
   }
 
diff --git a/tesseract_srdf/test/tesseract_srdf_serialization_unit.cpp b/tesseract_srdf/test/tesseract_srdf_serialization_unit.cpp
index fa3b7eff1ec..edc14bfd933 100644
--- a/tesseract_srdf/test/tesseract_srdf_serialization_unit.cpp
+++ b/tesseract_srdf/test/tesseract_srdf_serialization_unit.cpp
@@ -79,7 +79,7 @@ SceneGraph getSceneGraph()
   joint_2.parent_link_name = "link_1";
   joint_2.child_link_name = "link_2";
   joint_2.type = JointType::REVOLUTE;
-  joint_2.limits = std::make_shared<JointLimits>(-7, 7, 0, 5, 10);
+  joint_2.limits = std::make_shared<JointLimits>(-7, 7, 0, 5, 10, 20);
   EXPECT_TRUE(g.addJoint(joint_2));
 
   Joint joint_3("joint_a3");
@@ -87,7 +87,7 @@ SceneGraph getSceneGraph()
   joint_3.parent_link_name = "link_2";
   joint_3.child_link_name = "link_3";
   joint_3.type = JointType::REVOLUTE;
-  joint_3.limits = std::make_shared<JointLimits>(-7, 7, 0, 5, 10);
+  joint_3.limits = std::make_shared<JointLimits>(-7, 7, 0, 5, 10, 20);
   EXPECT_TRUE(g.addJoint(joint_3));
 
   Joint joint_4("joint_a4");
@@ -95,7 +95,7 @@ SceneGraph getSceneGraph()
   joint_4.parent_link_name = "link_3";
   joint_4.child_link_name = "link_4";
   joint_4.type = JointType::REVOLUTE;
-  joint_4.limits = std::make_shared<JointLimits>(-7, 7, 0, 5, 10);
+  joint_4.limits = std::make_shared<JointLimits>(-7, 7, 0, 5, 10, 20);
   EXPECT_TRUE(g.addJoint(joint_4));
 
   Joint joint_5("joint_a5");
@@ -103,7 +103,7 @@ SceneGraph getSceneGraph()
   joint_5.parent_link_name = "link_4";
   joint_5.child_link_name = "link_5";
   joint_5.type = JointType::REVOLUTE;
-  joint_5.limits = std::make_shared<JointLimits>(-7, 7, 0, 5, 10);
+  joint_5.limits = std::make_shared<JointLimits>(-7, 7, 0, 5, 10, 20);
   EXPECT_TRUE(g.addJoint(joint_5));
 
   Joint joint_6("joint_a6");
@@ -111,7 +111,7 @@ SceneGraph getSceneGraph()
   joint_6.parent_link_name = "link_5";
   joint_6.child_link_name = "link_6";
   joint_6.type = JointType::REVOLUTE;
-  joint_6.limits = std::make_shared<JointLimits>(-7, 7, 0, 5, 10);
+  joint_6.limits = std::make_shared<JointLimits>(-7, 7, 0, 5, 10, 20);
   EXPECT_TRUE(g.addJoint(joint_6));
 
   Joint joint_7("joint_a7");
@@ -119,7 +119,7 @@ SceneGraph getSceneGraph()
   joint_7.parent_link_name = "link_6";
   joint_7.child_link_name = "link_7";
   joint_7.type = JointType::REVOLUTE;
-  joint_7.limits = std::make_shared<JointLimits>(-7, 7, 0, 5, 10);
+  joint_7.limits = std::make_shared<JointLimits>(-7, 7, 0, 5, 10, 20);
   EXPECT_TRUE(g.addJoint(joint_7));
 
   Joint joint_tool0("joint_tool0");
diff --git a/tesseract_srdf/test/tesseract_srdf_unit.cpp b/tesseract_srdf/test/tesseract_srdf_unit.cpp
index 55468f86db9..d62b60f03a9 100644
--- a/tesseract_srdf/test/tesseract_srdf_unit.cpp
+++ b/tesseract_srdf/test/tesseract_srdf_unit.cpp
@@ -70,7 +70,7 @@ tesseract_scene_graph::SceneGraph::Ptr getABBSceneGraph(ABBConfig config = ABBCo
     joint_a.parent_link_name = "world";
     joint_a.child_link_name = "axis_1";
     joint_a.type = JointType::PRISMATIC;
-    joint_a.limits = std::make_shared<JointLimits>(-10, 10, 0, 5, 10);
+    joint_a.limits = std::make_shared<JointLimits>(-10, 10, 0, 5, 10, 20);
     EXPECT_TRUE(g->addJoint(joint_a));
 
     Joint joint_b("joint_axis_2");
@@ -78,7 +78,7 @@ tesseract_scene_graph::SceneGraph::Ptr getABBSceneGraph(ABBConfig config = ABBCo
     joint_b.parent_link_name = "axis_1";
     joint_b.child_link_name = "axis_2";
     joint_b.type = JointType::PRISMATIC;
-    joint_b.limits = std::make_shared<JointLimits>(-10, 10, 0, 5, 10);
+    joint_b.limits = std::make_shared<JointLimits>(-10, 10, 0, 5, 10, 20);
     EXPECT_TRUE(g->addJoint(joint_b));
 
     Joint joint_c("joint_base_link");
@@ -100,7 +100,7 @@ tesseract_scene_graph::SceneGraph::Ptr getABBSceneGraph(ABBConfig config = ABBCo
     joint_a.parent_link_name = "world";
     joint_a.child_link_name = "axis_1";
     joint_a.type = JointType::PRISMATIC;
-    joint_a.limits = std::make_shared<JointLimits>(-10, 10, 0, 5, 10);
+    joint_a.limits = std::make_shared<JointLimits>(-10, 10, 0, 5, 10, 20);
     EXPECT_TRUE(g->addJoint(joint_a));
 
     Joint joint_b("joint_axis_2");
@@ -109,7 +109,7 @@ tesseract_scene_graph::SceneGraph::Ptr getABBSceneGraph(ABBConfig config = ABBCo
     joint_b.parent_link_name = "axis_1";
     joint_b.child_link_name = "axis_2";
     joint_b.type = JointType::PRISMATIC;
-    joint_b.limits = std::make_shared<JointLimits>(-10, 10, 0, 5, 10);
+    joint_b.limits = std::make_shared<JointLimits>(-10, 10, 0, 5, 10, 20);
     EXPECT_TRUE(g->addJoint(joint_b));
 
     Joint joint_c("joint_base_link");
@@ -129,7 +129,7 @@ tesseract_scene_graph::SceneGraph::Ptr getABBSceneGraph(ABBConfig config = ABBCo
   joint_2.parent_link_name = "link_1";
   joint_2.child_link_name = "link_2";
   joint_2.type = JointType::REVOLUTE;
-  joint_2.limits = std::make_shared<JointLimits>(-7, 7, 0, 5, 10);
+  joint_2.limits = std::make_shared<JointLimits>(-7, 7, 0, 5, 10, 20);
   EXPECT_TRUE(g->addJoint(joint_2));
 
   Joint joint_3("joint_3");
@@ -137,7 +137,7 @@ tesseract_scene_graph::SceneGraph::Ptr getABBSceneGraph(ABBConfig config = ABBCo
   joint_3.parent_link_name = "link_2";
   joint_3.child_link_name = "link_3";
   joint_3.type = JointType::REVOLUTE;
-  joint_3.limits = std::make_shared<JointLimits>(-7, 7, 0, 5, 10);
+  joint_3.limits = std::make_shared<JointLimits>(-7, 7, 0, 5, 10, 20);
   EXPECT_TRUE(g->addJoint(joint_3));
 
   Joint joint_4("joint_4");
@@ -145,7 +145,7 @@ tesseract_scene_graph::SceneGraph::Ptr getABBSceneGraph(ABBConfig config = ABBCo
   joint_4.parent_link_name = "link_3";
   joint_4.child_link_name = "link_4";
   joint_4.type = JointType::REVOLUTE;
-  joint_4.limits = std::make_shared<JointLimits>(-7, 7, 0, 5, 10);
+  joint_4.limits = std::make_shared<JointLimits>(-7, 7, 0, 5, 10, 20);
   EXPECT_TRUE(g->addJoint(joint_4));
 
   Joint joint_5("joint_5");
@@ -153,7 +153,7 @@ tesseract_scene_graph::SceneGraph::Ptr getABBSceneGraph(ABBConfig config = ABBCo
   joint_5.parent_link_name = "link_4";
   joint_5.child_link_name = "link_5";
   joint_5.type = JointType::REVOLUTE;
-  joint_5.limits = std::make_shared<JointLimits>(-7, 7, 0, 5, 10);
+  joint_5.limits = std::make_shared<JointLimits>(-7, 7, 0, 5, 10, 20);
   EXPECT_TRUE(g->addJoint(joint_5));
 
   Joint joint_6("joint_6");
@@ -161,7 +161,7 @@ tesseract_scene_graph::SceneGraph::Ptr getABBSceneGraph(ABBConfig config = ABBCo
   joint_6.parent_link_name = "link_5";
   joint_6.child_link_name = "link_6";
   joint_6.type = JointType::REVOLUTE;
-  joint_6.limits = std::make_shared<JointLimits>(-7, 7, 0, 5, 10);
+  joint_6.limits = std::make_shared<JointLimits>(-7, 7, 0, 5, 10, 20);
   EXPECT_TRUE(g->addJoint(joint_6));
 
   Joint joint_tool0("joint_tool0");
@@ -202,7 +202,7 @@ tesseract_scene_graph::SceneGraph buildTestSceneGraph()
   joint_1.parent_link_name = "link_1";
   joint_1.child_link_name = "link_2";
   joint_1.type = JointType::REVOLUTE;
-  joint_1.limits = std::make_shared<JointLimits>(-7, 7, 0, 5, 10);
+  joint_1.limits = std::make_shared<JointLimits>(-7, 7, 0, 5, 10, 20);
   EXPECT_TRUE(g.addJoint(joint_1));
 
   Joint joint_2("joint_2");
@@ -210,7 +210,7 @@ tesseract_scene_graph::SceneGraph buildTestSceneGraph()
   joint_2.parent_link_name = "link_2";
   joint_2.child_link_name = "link_3";
   joint_2.type = JointType::REVOLUTE;
-  joint_2.limits = std::make_shared<JointLimits>(-7, 7, 0, 5, 10);
+  joint_2.limits = std::make_shared<JointLimits>(-7, 7, 0, 5, 10, 20);
   EXPECT_TRUE(g.addJoint(joint_2));
 
   Joint joint_3("joint_3");
@@ -218,7 +218,7 @@ tesseract_scene_graph::SceneGraph buildTestSceneGraph()
   joint_3.parent_link_name = "link_3";
   joint_3.child_link_name = "link_4";
   joint_3.type = JointType::REVOLUTE;
-  joint_3.limits = std::make_shared<JointLimits>(-7, 7, 0, 5, 10);
+  joint_3.limits = std::make_shared<JointLimits>(-7, 7, 0, 5, 10, 20);
   EXPECT_TRUE(g.addJoint(joint_3));
 
   Joint joint_4("joint_4");
@@ -226,7 +226,7 @@ tesseract_scene_graph::SceneGraph buildTestSceneGraph()
   joint_4.parent_link_name = "link_2";
   joint_4.child_link_name = "link_5";
   joint_4.type = JointType::REVOLUTE;
-  joint_4.limits = std::make_shared<JointLimits>(-7, 7, 0, 5, 10);
+  joint_4.limits = std::make_shared<JointLimits>(-7, 7, 0, 5, 10, 20);
   EXPECT_TRUE(g.addJoint(joint_4));
 
   return g;
@@ -275,7 +275,7 @@ TEST(TesseractSRDFUnit, LoadSRDFFileUnit)  // NOLINT
   joint_2.parent_link_name = "link_1";
   joint_2.child_link_name = "link_2";
   joint_2.type = JointType::REVOLUTE;
-  joint_2.limits = std::make_shared<JointLimits>(-7, 7, 0, 5, 10);
+  joint_2.limits = std::make_shared<JointLimits>(-7, 7, 0, 5, 10, 20);
   EXPECT_TRUE(g.addJoint(joint_2));
 
   Joint joint_3("joint_a3");
@@ -283,7 +283,7 @@ TEST(TesseractSRDFUnit, LoadSRDFFileUnit)  // NOLINT
   joint_3.parent_link_name = "link_2";
   joint_3.child_link_name = "link_3";
   joint_3.type = JointType::REVOLUTE;
-  joint_3.limits = std::make_shared<JointLimits>(-7, 7, 0, 5, 10);
+  joint_3.limits = std::make_shared<JointLimits>(-7, 7, 0, 5, 10, 20);
   EXPECT_TRUE(g.addJoint(joint_3));
 
   Joint joint_4("joint_a4");
@@ -291,7 +291,7 @@ TEST(TesseractSRDFUnit, LoadSRDFFileUnit)  // NOLINT
   joint_4.parent_link_name = "link_3";
   joint_4.child_link_name = "link_4";
   joint_4.type = JointType::REVOLUTE;
-  joint_4.limits = std::make_shared<JointLimits>(-7, 7, 0, 5, 10);
+  joint_4.limits = std::make_shared<JointLimits>(-7, 7, 0, 5, 10, 20);
   EXPECT_TRUE(g.addJoint(joint_4));
 
   Joint joint_5("joint_a5");
@@ -299,7 +299,7 @@ TEST(TesseractSRDFUnit, LoadSRDFFileUnit)  // NOLINT
   joint_5.parent_link_name = "link_4";
   joint_5.child_link_name = "link_5";
   joint_5.type = JointType::REVOLUTE;
-  joint_5.limits = std::make_shared<JointLimits>(-7, 7, 0, 5, 10);
+  joint_5.limits = std::make_shared<JointLimits>(-7, 7, 0, 5, 10, 20);
   EXPECT_TRUE(g.addJoint(joint_5));
 
   Joint joint_6("joint_a6");
@@ -307,7 +307,7 @@ TEST(TesseractSRDFUnit, LoadSRDFFileUnit)  // NOLINT
   joint_6.parent_link_name = "link_5";
   joint_6.child_link_name = "link_6";
   joint_6.type = JointType::REVOLUTE;
-  joint_6.limits = std::make_shared<JointLimits>(-7, 7, 0, 5, 10);
+  joint_6.limits = std::make_shared<JointLimits>(-7, 7, 0, 5, 10, 20);
   EXPECT_TRUE(g.addJoint(joint_6));
 
   Joint joint_7("joint_a7");
@@ -315,7 +315,7 @@ TEST(TesseractSRDFUnit, LoadSRDFFileUnit)  // NOLINT
   joint_7.parent_link_name = "link_6";
   joint_7.child_link_name = "link_7";
   joint_7.type = JointType::REVOLUTE;
-  joint_7.limits = std::make_shared<JointLimits>(-7, 7, 0, 5, 10);
+  joint_7.limits = std::make_shared<JointLimits>(-7, 7, 0, 5, 10, 20);
   EXPECT_TRUE(g.addJoint(joint_7));
 
   Joint joint_tool0("joint_tool0");
diff --git a/tesseract_state_solver/include/tesseract_state_solver/mutable_state_solver.h b/tesseract_state_solver/include/tesseract_state_solver/mutable_state_solver.h
index e5be31eafc3..09980465e47 100644
--- a/tesseract_state_solver/include/tesseract_state_solver/mutable_state_solver.h
+++ b/tesseract_state_solver/include/tesseract_state_solver/mutable_state_solver.h
@@ -139,6 +139,14 @@ class MutableStateSolver : public StateSolver
    */
   virtual bool changeJointAccelerationLimits(const std::string& name, double limit) = 0;
 
+  /**
+   * @brief Changes the jerk limits associated with a joint
+   * @param joint_name Name of the joint to be updated
+   * @param limits New jerk limits to be set as the joint limits
+   * @return
+   */
+  virtual bool changeJointJerkLimits(const std::string& name, double limit) = 0;
+
   /**
    * @brief Merge a scene into the current solver
    * @param scene_graph Const ref to the graph to be merged
diff --git a/tesseract_state_solver/include/tesseract_state_solver/ofkt/ofkt_state_solver.h b/tesseract_state_solver/include/tesseract_state_solver/ofkt/ofkt_state_solver.h
index 19833424c4c..f0602502b0d 100644
--- a/tesseract_state_solver/include/tesseract_state_solver/ofkt/ofkt_state_solver.h
+++ b/tesseract_state_solver/include/tesseract_state_solver/ofkt/ofkt_state_solver.h
@@ -145,6 +145,8 @@ class OFKTStateSolver : public MutableStateSolver
 
   bool changeJointAccelerationLimits(const std::string& name, double limit) override final;
 
+  bool changeJointJerkLimits(const std::string& name, double limit) override final;
+
   bool insertSceneGraph(const SceneGraph& scene_graph,
                         const Joint& joint,
                         const std::string& prefix = "") override final;
diff --git a/tesseract_state_solver/src/kdl_state_solver.cpp b/tesseract_state_solver/src/kdl_state_solver.cpp
index a3c82c80a00..7b146c5ca63 100644
--- a/tesseract_state_solver/src/kdl_state_solver.cpp
+++ b/tesseract_state_solver/src/kdl_state_solver.cpp
@@ -255,8 +255,9 @@ bool KDLStateSolver::processKDLData(const tesseract_scene_graph::SceneGraph& sce
   current_state_ = SceneState();
   kdl_jnt_array_.resize(data_.tree.getNrOfJoints());
   limits_.joint_limits.resize(static_cast<long int>(data_.tree.getNrOfJoints()), 2);
-  limits_.velocity_limits.resize(static_cast<long int>(data_.tree.getNrOfJoints()));
-  limits_.acceleration_limits.resize(static_cast<long int>(data_.tree.getNrOfJoints()));
+  limits_.velocity_limits.resize(static_cast<long int>(data_.tree.getNrOfJoints()), 2);
+  limits_.acceleration_limits.resize(static_cast<long int>(data_.tree.getNrOfJoints()), 2);
+  limits_.jerk_limits.resize(static_cast<long int>(data_.tree.getNrOfJoints()), 2);
   joint_qnr_.resize(data_.tree.getNrOfJoints());
   joint_to_qnr_.clear();
   size_t j = 0;
@@ -277,8 +278,12 @@ bool KDLStateSolver::processKDLData(const tesseract_scene_graph::SceneGraph& sce
     const auto& sj = scene_graph.getJoint(jnt.getName());
     limits_.joint_limits(static_cast<long>(j), 0) = sj->limits->lower;
     limits_.joint_limits(static_cast<long>(j), 1) = sj->limits->upper;
-    limits_.velocity_limits(static_cast<long>(j)) = sj->limits->velocity;
-    limits_.acceleration_limits(static_cast<long>(j)) = sj->limits->acceleration;
+    limits_.velocity_limits(static_cast<long>(j), 0) = -sj->limits->velocity;
+    limits_.velocity_limits(static_cast<long>(j), 1) = sj->limits->velocity;
+    limits_.acceleration_limits(static_cast<long>(j), 0) = -sj->limits->acceleration;
+    limits_.acceleration_limits(static_cast<long>(j), 1) = sj->limits->acceleration;
+    limits_.jerk_limits(static_cast<long>(j), 0) = -sj->limits->jerk;
+    limits_.jerk_limits(static_cast<long>(j), 1) = sj->limits->jerk;
 
     j++;
   }
diff --git a/tesseract_state_solver/src/ofkt_state_solver.cpp b/tesseract_state_solver/src/ofkt_state_solver.cpp
index 22e763a5798..c9dc15f3470 100644
--- a/tesseract_state_solver/src/ofkt_state_solver.cpp
+++ b/tesseract_state_solver/src/ofkt_state_solver.cpp
@@ -670,7 +670,8 @@ bool OFKTStateSolver::changeJointVelocityLimits(const std::string& name, double
 
   long idx = std::distance(active_joint_names_.begin(),
                            std::find(active_joint_names_.begin(), active_joint_names_.end(), name));
-  limits_.velocity_limits(idx) = limit;
+  limits_.velocity_limits(idx, 0) = -limit;
+  limits_.velocity_limits(idx, 1) = limit;
   return true;
 }
 
@@ -687,7 +688,26 @@ bool OFKTStateSolver::changeJointAccelerationLimits(const std::string& name, dou
 
   long idx = std::distance(active_joint_names_.begin(),
                            std::find(active_joint_names_.begin(), active_joint_names_.end(), name));
-  limits_.acceleration_limits(idx) = limit;
+  limits_.acceleration_limits(idx, 0) = -limit;
+  limits_.acceleration_limits(idx, 1) = limit;
+  return true;
+}
+
+bool OFKTStateSolver::changeJointJerkLimits(const std::string& name, double limit)
+{
+  std::unique_lock<std::shared_mutex> lock(mutex_);
+  auto it = nodes_.find(name);
+  if (it == nodes_.end())
+  {
+    CONSOLE_BRIDGE_logError("OFKTStateSolver, tried to change joint '%s' positioner limits which does not exist!",
+                            name.c_str());
+    return false;
+  }
+
+  long idx = std::distance(active_joint_names_.begin(),
+                           std::find(active_joint_names_.begin(), active_joint_names_.end(), name));
+  limits_.jerk_limits(idx, 0) = -limit;
+  limits_.jerk_limits(idx, 1) = limit;
   return true;
 }
 
@@ -986,8 +1006,9 @@ void OFKTStateSolver::removeJointHelper(const std::vector<std::string>& removed_
 
     tesseract_common::KinematicLimits l1;
     l1.joint_limits.resize(static_cast<long int>(active_joint_names_.size()), 2);
-    l1.velocity_limits.resize(static_cast<long int>(active_joint_names_.size()));
-    l1.acceleration_limits.resize(static_cast<long int>(active_joint_names_.size()));
+    l1.velocity_limits.resize(static_cast<long int>(active_joint_names_.size()), 2);
+    l1.acceleration_limits.resize(static_cast<long int>(active_joint_names_.size()), 2);
+    l1.jerk_limits.resize(static_cast<long int>(active_joint_names_.size()), 2);
 
     long cnt = 0;
     for (long i = 0; i < limits_.joint_limits.rows(); ++i)
@@ -996,8 +1017,9 @@ void OFKTStateSolver::removeJointHelper(const std::vector<std::string>& removed_
           removed_active_joints_indices.end())
       {
         l1.joint_limits.row(cnt) = limits_.joint_limits.row(i);
-        l1.velocity_limits(cnt) = limits_.velocity_limits(i);
-        l1.acceleration_limits(cnt) = limits_.acceleration_limits(i);
+        l1.velocity_limits.row(cnt) = limits_.velocity_limits.row(i);
+        l1.acceleration_limits.row(cnt) = limits_.acceleration_limits.row(i);
+        l1.jerk_limits.row(cnt) = limits_.jerk_limits.row(i);
         ++cnt;
       }
     }
@@ -1131,20 +1153,26 @@ void OFKTStateSolver::addNewJointLimits(const std::vector<std::shared_ptr<const
     tesseract_common::KinematicLimits l;
     long s = limits_.joint_limits.rows() + static_cast<long>(new_joint_limits.size());
     l.joint_limits.resize(s, 2);
-    l.velocity_limits.resize(s);
-    l.acceleration_limits.resize(s);
+    l.velocity_limits.resize(s, 2);
+    l.acceleration_limits.resize(s, 2);
+    l.jerk_limits.resize(s, 2);
 
     l.joint_limits.block(0, 0, limits_.joint_limits.rows(), 2) = limits_.joint_limits;
-    l.velocity_limits.head(limits_.joint_limits.rows()) = limits_.velocity_limits;
-    l.acceleration_limits.head(limits_.joint_limits.rows()) = limits_.acceleration_limits;
+    l.velocity_limits.block(0, 0, limits_.velocity_limits.rows(), 2) = limits_.velocity_limits;
+    l.acceleration_limits.block(0, 0, limits_.acceleration_limits.rows(), 2) = limits_.acceleration_limits;
+    l.jerk_limits.block(0, 0, limits_.jerk_limits.rows(), 2) = limits_.jerk_limits;
 
     long cnt = limits_.joint_limits.rows();
     for (const auto& limits : new_joint_limits)
     {
       l.joint_limits(cnt, 0) = limits->lower;
       l.joint_limits(cnt, 1) = limits->upper;
-      l.velocity_limits(cnt) = limits->velocity;
-      l.acceleration_limits(cnt) = limits->acceleration;
+      l.velocity_limits(cnt, 0) = -limits->velocity;
+      l.velocity_limits(cnt, 1) = limits->velocity;
+      l.acceleration_limits(cnt, 0) = -limits->acceleration;
+      l.acceleration_limits(cnt, 1) = limits->acceleration;
+      l.jerk_limits(cnt, 0) = -limits->jerk;
+      l.jerk_limits(cnt, 1) = limits->jerk;
       ++cnt;
     }
     limits_ = l;
diff --git a/tesseract_state_solver/test/state_solver_test_suite.h b/tesseract_state_solver/test/state_solver_test_suite.h
index fc6cc1a3cf9..484bd8e5fe1 100644
--- a/tesseract_state_solver/test/state_solver_test_suite.h
+++ b/tesseract_state_solver/test/state_solver_test_suite.h
@@ -181,8 +181,12 @@ inline void runCompareStateSolverLimits(const SceneGraph& scene_graph, const Sta
     const auto& scene_joint = scene_graph.getJoint(comp_joint_names[static_cast<std::size_t>(i)]);
     EXPECT_NEAR(limits.joint_limits(i, 0), scene_joint->limits->lower, 1e-5);
     EXPECT_NEAR(limits.joint_limits(i, 1), scene_joint->limits->upper, 1e-5);
-    EXPECT_NEAR(limits.velocity_limits(i), scene_joint->limits->velocity, 1e-5);
-    EXPECT_NEAR(limits.acceleration_limits(i), scene_joint->limits->acceleration, 1e-5);
+    EXPECT_NEAR(limits.velocity_limits(i, 0), -scene_joint->limits->velocity, 1e-5);
+    EXPECT_NEAR(limits.velocity_limits(i, 1), scene_joint->limits->velocity, 1e-5);
+    EXPECT_NEAR(limits.acceleration_limits(i, 0), -scene_joint->limits->acceleration, 1e-5);
+    EXPECT_NEAR(limits.acceleration_limits(i, 1), scene_joint->limits->acceleration, 1e-5);
+    EXPECT_NEAR(limits.jerk_limits(i, 0), -scene_joint->limits->jerk, 1e-5);
+    EXPECT_NEAR(limits.jerk_limits(i, 1), scene_joint->limits->jerk, 1e-5);
   }
 }
 
@@ -1464,14 +1468,17 @@ void runChangeJointLimitsTest()
   double new_upper = 2.0;
   double new_velocity = 3.0;
   double new_acceleration = 4.0;
+  double new_jerk = 5.0;
 
   scene_graph->changeJointPositionLimits("joint_a1", new_lower, new_upper);
   scene_graph->changeJointVelocityLimits("joint_a1", new_velocity);
   scene_graph->changeJointAccelerationLimits("joint_a1", new_acceleration);
+  scene_graph->changeJointJerkLimits("joint_a1", new_jerk);
 
   EXPECT_TRUE(state_solver.changeJointPositionLimits("joint_a1", new_lower, new_upper));
   EXPECT_TRUE(state_solver.changeJointVelocityLimits("joint_a1", new_velocity));
   EXPECT_TRUE(state_solver.changeJointAccelerationLimits("joint_a1", new_acceleration));
+  EXPECT_TRUE(state_solver.changeJointJerkLimits("joint_a1", new_jerk));
 
   {
     std::vector<std::string> joint_names = state_solver.getActiveJointNames();
@@ -1479,8 +1486,12 @@ void runChangeJointLimitsTest()
     auto limits = state_solver.getLimits();
     EXPECT_NEAR(limits.joint_limits(idx, 0), new_lower, 1e-5);
     EXPECT_NEAR(limits.joint_limits(idx, 1), new_upper, 1e-5);
-    EXPECT_NEAR(limits.velocity_limits(idx), new_velocity, 1e-5);
-    EXPECT_NEAR(limits.acceleration_limits(idx), new_acceleration, 1e-5);
+    EXPECT_NEAR(limits.velocity_limits(idx, 0), -new_velocity, 1e-5);
+    EXPECT_NEAR(limits.velocity_limits(idx, 1), new_velocity, 1e-5);
+    EXPECT_NEAR(limits.acceleration_limits(idx, 0), -new_acceleration, 1e-5);
+    EXPECT_NEAR(limits.acceleration_limits(idx, 1), new_acceleration, 1e-5);
+    EXPECT_NEAR(limits.jerk_limits(idx, 0), -new_jerk, 1e-5);
+    EXPECT_NEAR(limits.jerk_limits(idx, 1), new_jerk, 1e-5);
   }
 
   {  // Test Clone
@@ -1492,8 +1503,12 @@ void runChangeJointLimitsTest()
     auto limits = state_solver_clone.getLimits();
     EXPECT_NEAR(limits.joint_limits(idx, 0), new_lower, 1e-5);
     EXPECT_NEAR(limits.joint_limits(idx, 1), new_upper, 1e-5);
-    EXPECT_NEAR(limits.velocity_limits(idx), new_velocity, 1e-5);
-    EXPECT_NEAR(limits.acceleration_limits(idx), new_acceleration, 1e-5);
+    EXPECT_NEAR(limits.velocity_limits(idx, 0), -new_velocity, 1e-5);
+    EXPECT_NEAR(limits.velocity_limits(idx, 1), new_velocity, 1e-5);
+    EXPECT_NEAR(limits.acceleration_limits(idx, 0), -new_acceleration, 1e-5);
+    EXPECT_NEAR(limits.acceleration_limits(idx, 1), new_acceleration, 1e-5);
+    EXPECT_NEAR(limits.jerk_limits(idx, 0), -new_jerk, 1e-5);
+    EXPECT_NEAR(limits.jerk_limits(idx, 1), new_jerk, 1e-5);
   }
 
   // Joint does not exist
@@ -1501,9 +1516,11 @@ void runChangeJointLimitsTest()
   double new_upper_err = 2.0 * 10;
   double new_velocity_err = 3.0 * 10;
   double new_acceleration_err = 4.0 * 10;
+  double new_jerk_err = 5.0 * 10;
   EXPECT_FALSE(state_solver.changeJointPositionLimits("joint_does_not_exist", new_lower_err, new_upper_err));
   EXPECT_FALSE(state_solver.changeJointVelocityLimits("joint_does_not_exist", new_velocity_err));
   EXPECT_FALSE(state_solver.changeJointAccelerationLimits("joint_does_not_exist", new_acceleration_err));
+  EXPECT_FALSE(state_solver.changeJointJerkLimits("joint_does_not_exist", new_jerk_err));
 
   {
     std::vector<std::string> joint_names = state_solver.getActiveJointNames();
@@ -1511,8 +1528,12 @@ void runChangeJointLimitsTest()
     auto limits = state_solver.getLimits();
     EXPECT_NEAR(limits.joint_limits(idx, 0), new_lower, 1e-5);
     EXPECT_NEAR(limits.joint_limits(idx, 1), new_upper, 1e-5);
-    EXPECT_NEAR(limits.velocity_limits(idx), new_velocity, 1e-5);
-    EXPECT_NEAR(limits.acceleration_limits(idx), new_acceleration, 1e-5);
+    EXPECT_NEAR(limits.velocity_limits(idx, 0), -new_velocity, 1e-5);
+    EXPECT_NEAR(limits.velocity_limits(idx, 1), new_velocity, 1e-5);
+    EXPECT_NEAR(limits.acceleration_limits(idx, 0), -new_acceleration, 1e-5);
+    EXPECT_NEAR(limits.acceleration_limits(idx, 1), new_acceleration, 1e-5);
+    EXPECT_NEAR(limits.jerk_limits(idx, 0), -new_jerk, 1e-5);
+    EXPECT_NEAR(limits.jerk_limits(idx, 1), new_jerk, 1e-5);
   }
 
   {  // Test Clone
@@ -1524,8 +1545,12 @@ void runChangeJointLimitsTest()
     auto limits = state_solver_clone.getLimits();
     EXPECT_NEAR(limits.joint_limits(idx, 0), new_lower, 1e-5);
     EXPECT_NEAR(limits.joint_limits(idx, 1), new_upper, 1e-5);
-    EXPECT_NEAR(limits.velocity_limits(idx), new_velocity, 1e-5);
-    EXPECT_NEAR(limits.acceleration_limits(idx), new_acceleration, 1e-5);
+    EXPECT_NEAR(limits.velocity_limits(idx, 0), -new_velocity, 1e-5);
+    EXPECT_NEAR(limits.velocity_limits(idx, 1), new_velocity, 1e-5);
+    EXPECT_NEAR(limits.acceleration_limits(idx, 0), -new_acceleration, 1e-5);
+    EXPECT_NEAR(limits.acceleration_limits(idx, 1), new_acceleration, 1e-5);
+    EXPECT_NEAR(limits.jerk_limits(idx, 0), -new_jerk, 1e-5);
+    EXPECT_NEAR(limits.jerk_limits(idx, 1), new_jerk, 1e-5);
   }
 }
 
diff --git a/tesseract_urdf/src/joint.cpp b/tesseract_urdf/src/joint.cpp
index 85714f832a3..2e01f389b74 100644
--- a/tesseract_urdf/src/joint.cpp
+++ b/tesseract_urdf/src/joint.cpp
@@ -302,7 +302,8 @@ tinyxml2::XMLElement* tesseract_urdf::writeJoint(const std::shared_ptr<const tes
   {
     if (joint->limits != nullptr && (!tesseract_common::almostEqualRelativeAndAbs(joint->limits->effort, 0.0) ||
                                      !tesseract_common::almostEqualRelativeAndAbs(joint->limits->velocity, 0.0) ||
-                                     !tesseract_common::almostEqualRelativeAndAbs(joint->limits->acceleration, 0.0)))
+                                     !tesseract_common::almostEqualRelativeAndAbs(joint->limits->acceleration, 0.0) ||
+                                     !tesseract_common::almostEqualRelativeAndAbs(joint->limits->jerk, 0.0)))
     {
       tinyxml2::XMLElement* xml_limits = writeLimits(joint->limits, doc);
       xml_element->InsertEndChild(xml_limits);
diff --git a/tesseract_urdf/src/limits.cpp b/tesseract_urdf/src/limits.cpp
index 9764c6b995d..69a1af1a53e 100644
--- a/tesseract_urdf/src/limits.cpp
+++ b/tesseract_urdf/src/limits.cpp
@@ -61,6 +61,12 @@ tesseract_scene_graph::JointLimits::Ptr tesseract_urdf::parseLimits(const tinyxm
   else if (status != tinyxml2::XML_SUCCESS)
     std::throw_with_nested(std::runtime_error("Limits: Failed to parse attribute 'acceleration'!"));
 
+  status = xml_element->QueryDoubleAttribute("jerk", &(limits->jerk));
+  if (status == tinyxml2::XML_NO_ATTRIBUTE)
+    limits->jerk = 1000;
+  else if (status != tinyxml2::XML_SUCCESS)
+    std::throw_with_nested(std::runtime_error("Limits: Failed to parse attribute 'jerk'!"));
+
   return limits;
 }
 
@@ -89,5 +95,10 @@ tesseract_urdf::writeLimits(const std::shared_ptr<const tesseract_scene_graph::J
       !tesseract_common::almostEqualRelativeAndAbs(limits->acceleration, limits->velocity * 0.5))
     xml_element->SetAttribute("acceleration", toString(limits->acceleration).c_str());
 
+  // Write out nonzero jerk (Tesseract-exclusive)
+  if (!tesseract_common::almostEqualRelativeAndAbs(limits->jerk, 0.0) &&
+      !tesseract_common::almostEqualRelativeAndAbs(limits->jerk, 1000))
+    xml_element->SetAttribute("jerk", toString(limits->jerk).c_str());
+
   return xml_element;
 }