Set type of joint dynamics limits to Vector7

Author: TimSchneider42

include/franky/dynamics_limit.hpp

@@ -71,25 +71,14 @@ class DynamicsLimit {
     value_ = value;
   }
 
-  /**
-   * @brief Get the current value of the limit as a different type.
-   *
-   * Retrieves the current value of the limit and converts it to a different type.
-   *
-   * @tparam AlternativeType The type to which the value should be converted.
-   * @return The current value of the limit converted to the specified type.
-   */
-  template <typename AlternativeType>
-  AlternativeType getAs();
-
   /**
    * @brief Get the current value of the limit.
    *
    * Retrieves the current value stored in this limit.
    *
    * @return The current value of the limit.
    */
-  LimitType get() { return value_; }
+  [[nodiscard]] LimitType get() const { return value_; }
 
   /**
    * @brief The maximum value this limit can take as defined by Franka.
@@ -114,7 +103,7 @@ class DynamicsLimit {
    *
    * @param value The value to check.
    */
-  void check(const LimitType& value);
+  void check(const LimitType& value) const;
 
   std::shared_ptr<std::mutex> write_mutex_; /**< Mutex for synchronizing writes to the limit. */
   LimitType value_; /**< Current value of the limit. */

include/franky/robot.hpp

@@ -24,6 +24,7 @@
 #include "franky/relative_dynamics_factor.hpp"
 #include "franky/joint_state.hpp"
 #include "franky/dynamics_limit.hpp"
+#include "franky/util.hpp"
 
 namespace franky {
 
@@ -37,13 +38,6 @@ struct InvalidMotionTypeException : std::runtime_error {
   using std::runtime_error::runtime_error;
 };
 
-constexpr std::array<double, 7> scaleArr7(const std::array<double, 7> &arr, const double factor) {
-  std::array<double, arr.size()> result{};
-  for (size_t i = 0; i < arr.size(); ++i)
-    result[i] = arr[i] * factor;
-  return result;
-}
-
 /**
  * @brief A class representing a Franka robot.
  *
@@ -155,31 +149,30 @@ class Robot : public franka::Robot {
   DynamicsLimit<double> elbow_jerk_limit{
     "elbow jerk", 5000.0, 800.0, control_mutex_, std::bind(&Robot::is_in_control_unsafe, this)};
 
-
   /**
    * @brief Joint velocity limit [rad/s].
    */
-#define MAX_JOINT_VEL {2.175, 2.175, 2.175, 2.175, 2.610, 2.610, 2.610}
-  DynamicsLimit<std::array<double, 7>> joint_velocity_limit{
+#define MAX_JOINT_VEL toEigenD<7>({2.175, 2.175, 2.175, 2.175, 2.610, 2.610, 2.610})
+  DynamicsLimit<Vector7d> joint_velocity_limit{
     "joint_velocity", MAX_JOINT_VEL, MAX_JOINT_VEL, control_mutex_,
     std::bind(&Robot::is_in_control_unsafe, this)
   };
 
   /**
    * @brief Joint acceleration limit [rad/s²].
    */
-#define MAX_JOINT_ACC {15.0, 7.5, 10.0, 12.5, 15.0, 20.0, 20.0}
-  DynamicsLimit<std::array<double, 7>> joint_acceleration_limit{
-    "joint_acceleration", MAX_JOINT_ACC, scaleArr7(MAX_JOINT_ACC, 0.3), control_mutex_,
+#define MAX_JOINT_ACC toEigenD<7>({15.0, 7.5, 10.0, 12.5, 15.0, 20.0, 20.0})
+  DynamicsLimit<Vector7d> joint_acceleration_limit{
+    "joint_acceleration", MAX_JOINT_ACC, MAX_JOINT_ACC * 0.3, control_mutex_,
     std::bind(&Robot::is_in_control_unsafe, this)
   };
 
   /**
    * @brief Joint jerk limit [rad/s³].
    */
-#define MAX_JOINT_JERK {7500.0, 3750.0, 5000.0, 6250.0, 7500.0, 10000.0, 10000.0}
-  DynamicsLimit<std::array<double, 7>> joint_jerk_limit{
-    "joint_jerk", MAX_JOINT_JERK, scaleArr7(MAX_JOINT_JERK, 0.3), control_mutex_,
+#define MAX_JOINT_JERK toEigenD<7>({7500.0, 3750.0, 5000.0, 6250.0, 7500.0, 10000.0, 10000.0})
+  DynamicsLimit<Vector7d> joint_jerk_limit{
+    "joint_jerk", MAX_JOINT_JERK, MAX_JOINT_JERK * 0.3, control_mutex_,
     std::bind(&Robot::is_in_control_unsafe, this)
   };
 

include/franky/util.hpp

@@ -22,6 +22,13 @@ Eigen::Matrix<double, dims, 1> toEigenD(const std::array<double, dims> &vector)
   return Eigen::Matrix<double, dims, 1>::Map(vector.data());
 }
 
+template<size_t dims>
+Eigen::Vector<double, dims> ensureEigen(const Array<dims> &input) {
+  if (std::holds_alternative<Eigen::Vector<double, dims>>(input))
+    return std::get<Eigen::Vector<double, dims>>(input);
+  return toEigenD<dims>(std::get<std::array<double, dims >>(input));
+}
+
 template<size_t dims>
 std::array<double, dims> ensureStd(const Array<dims> &input) {
   if (std::holds_alternative<std::array<double, dims >>(input))

python/bind_robot.cpp

@@ -34,15 +34,15 @@ void robotMove(Robot &robot, const std::shared_ptr<Motion<ControlSignalType>> &m
 }
 
 void bind_robot(py::module &m) {
-  py::class_<DynamicsLimit<std::array<double, 7>>>(m, "VectorDynamicsLimit")
-      .def("set", &DynamicsLimit<std::array<double, 7>>::setFrom<Array<7>>, "value"_a)
-      .def("get", &DynamicsLimit<std::array<double, 7>>::getAs<Vector7d>)
+  py::class_<DynamicsLimit<Vector7d>>(m, "VectorDynamicsLimit")
+      .def("set", &DynamicsLimit<Vector7d>::setFrom<Array<7>>, "value"_a)
+      .def("get", &DynamicsLimit<Vector7d>::get)
       .def_property_readonly(
           "max",
-          [](const DynamicsLimit<std::array<double, 7>> &dynamics_limit) {
+          [](const DynamicsLimit<Vector7d> &dynamics_limit) {
             return Vector7d::Map(dynamics_limit.max.data());
           })
-      .def_readonly("desc", &DynamicsLimit<std::array<double, 7>>::desc);
+      .def_readonly("desc", &DynamicsLimit<Vector7d>::desc);
 
   py::class_<DynamicsLimit<double>>(m, "DoubleDynamicsLimit")
       .def("set", &DynamicsLimit<double>::set, "value"_a)

src/dynamics_limit.cpp

@@ -7,8 +7,8 @@
 
 namespace franky {
 
-template <>
-void DynamicsLimit<double>::check(const double &value) {
+template<>
+void DynamicsLimit<double>::check(const double &value) const {
   if (value < 0) {
     std::stringstream ss;
     ss << desc << " limit cannot be negative.";
@@ -21,20 +21,14 @@ void DynamicsLimit<double>::check(const double &value) {
   }
 }
 
-template <>
-template <>
-void DynamicsLimit<std::array<double, 7>>::setFrom<Array<7>>(const Array<7> &value) {
-  set(ensureStd<7>(value));
+template<>
+template<>
+void DynamicsLimit<Vector7d>::setFrom<Array<7>>(const Array<7> &value) {
+  set(ensureEigen<7>(value));
 }
 
-template <>
-template <>
-Vector7d DynamicsLimit<std::array<double, 7>>::getAs<Vector7d>() {
-  return Vector7d::Map(value_.data());
-}
-
-template <>
-void DynamicsLimit<std::array<double, 7>>::check(const std::array<double, 7> &value) {
+template<>
+void DynamicsLimit<Vector7d>::check(const Vector7d &value) const {
   for (int i = 0; i < value.size(); i++) {
     if (value[i] < 0) {
       std::stringstream ss;

src/motion/joint_velocity_waypoint_motion.cpp

@@ -47,9 +47,9 @@ void JointVelocityWaypointMotion::setNewWaypoint(const franka::RobotState &robot
 std::tuple<Vector7d, Vector7d, Vector7d> JointVelocityWaypointMotion::getAbsoluteInputLimits() const {
   const auto r = robot();
   return {
-    r->joint_velocity_limit.getAs<Vector7d>(),
-    r->joint_acceleration_limit.getAs<Vector7d>(),
-    r->joint_jerk_limit.getAs<Vector7d>()
+    r->joint_velocity_limit.get(),
+    r->joint_acceleration_limit.get(),
+    r->joint_jerk_limit.get()
   };
 }
 }  // namespace franky

src/motion/joint_waypoint_motion.cpp

@@ -50,9 +50,9 @@ void JointWaypointMotion::setNewWaypoint(
 std::tuple<Vector7d, Vector7d, Vector7d> JointWaypointMotion::getAbsoluteInputLimits() const {
   const auto r = robot();
   return {
-    r->joint_velocity_limit.getAs<Vector7d>(),
-    r->joint_acceleration_limit.getAs<Vector7d>(),
-    r->joint_jerk_limit.getAs<Vector7d>()
+      r->joint_velocity_limit.get(),
+      r->joint_acceleration_limit.get(),
+      r->joint_jerk_limit.get()
   };
 }
 }  // namespace franky