From e3870e402ab1c8565faf2eeb6b39dfcf13ec9be8 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 ++++++-------
 .../include/tesseract_kinematics/core/utils.h |  8 +-
 tesseract_kinematics/core/src/joint_group.cpp | 10 +-
 .../core/src/kinematic_group.cpp              |  4 +-
 .../include/tesseract_scene_graph/joint.h     |  3 +-
 tesseract_scene_graph/src/graph.cpp           |  3 +-
 tesseract_scene_graph/src/joint.cpp           | 10 +-
 .../src/kdl_state_solver.cpp                  | 13 ++-
 .../src/ofkt_state_solver.cpp                 | 28 ++++--
 tesseract_urdf/src/joint.cpp                  |  3 +-
 tesseract_urdf/src/limits.cpp                 | 11 +++
 12 files changed, 140 insertions(+), 108 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_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..ec37256532b 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)
@@ -284,7 +288,7 @@ void JointGroup::setLimits(const tesseract_common::KinematicLimits& limits)
 {
   Eigen::Index nj = numJoints();
   if (limits.joint_limits.rows() != nj || limits.velocity_limits.size() != nj ||
-      limits.acceleration_limits.size() != nj)
+      limits.acceleration_limits.size() != nj || limits.jerk_limits.size() != nj)
     throw std::runtime_error("Kinematics Group limits assigned are invalid!");
 
   limits_ = limits;
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_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..4d793411262 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, 0.5);
     }
     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;
 }
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_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..8fed193c387 100644
--- a/tesseract_state_solver/src/ofkt_state_solver.cpp
+++ b/tesseract_state_solver/src/ofkt_state_solver.cpp
@@ -986,8 +986,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 +997,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 +1133,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_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..157c68a27f0 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 = 0.5 * limits->acceleration;
+  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, limits->acceleration * 0.5))
+    xml_element->SetAttribute("jerk", toString(limits->jerk).c_str());
+
   return xml_element;
 }