Merge pull request #194 from ipa-mdl/limit_reset

reset functionality for stateful position limit handles
ros-controls · Jan 22, 2015 · 3bde688 · 3bde688
2 parents 8c6a1b7 + b16ca02
commit 3bde688
Show file tree

Hide file tree

Showing 2 changed files with 97 additions and 2 deletions.
diff --git a/joint_limits_interface/include/joint_limits_interface/joint_limits_interface.h b/joint_limits_interface/include/joint_limits_interface/joint_limits_interface.h
@@ -115,6 +115,13 @@ class PositionJointSaturationHandle
     prev_cmd_ = cmd;
   }
 
+  /**
+   * \brief Reset state, in case of mode switch or e-stop
+   */
+  void reset(){
+    prev_cmd_ = std::numeric_limits<double>::quiet_NaN();
+  }
+
 private:
   hardware_interface::JointHandle jh_;
   JointLimits limits_;
@@ -239,6 +246,13 @@ class PositionJointSoftLimitsHandle
     prev_cmd_ = jh_.getCommand();
   }
 
+  /**
+   * \brief Reset state, in case of mode switch or e-stop
+   */
+  void reset(){
+    prev_cmd_ = std::numeric_limits<double>::quiet_NaN();
+  }
+
 private:
   hardware_interface::JointHandle jh_;
   JointLimits limits_;
@@ -554,10 +568,38 @@ class JointLimitsInterface : public hardware_interface::ResourceManager<HandleTy
 };
 
 /** Interface for enforcing limits on a position-controlled joint through saturation. */
-class PositionJointSaturationInterface : public JointLimitsInterface<PositionJointSaturationHandle> {};
+class PositionJointSaturationInterface : public JointLimitsInterface<PositionJointSaturationHandle> {
+public:
+  /** \name Real-Time Safe Functions
+   *\{*/
+  /** \brief Reset all managed handles. */
+  void reset()
+  {
+    typedef hardware_interface::ResourceManager<PositionJointSaturationHandle>::ResourceMap::iterator ItratorType;
+    for (ItratorType it = this->resource_map_.begin(); it != this->resource_map_.end(); ++it)
+    {
+      it->second.reset();
+    }
+  }
+  /*\}*/
+};
 
 /** Interface for enforcing limits on a position-controlled joint with soft position limits. */
-class PositionJointSoftLimitsInterface : public JointLimitsInterface<PositionJointSoftLimitsHandle> {};
+class PositionJointSoftLimitsInterface : public JointLimitsInterface<PositionJointSoftLimitsHandle> {
+public:
+  /** \name Real-Time Safe Functions
+   *\{*/
+  /** \brief Reset all managed handles. */
+  void reset()
+  {
+    typedef hardware_interface::ResourceManager<PositionJointSoftLimitsHandle>::ResourceMap::iterator ItratorType;
+    for (ItratorType it = this->resource_map_.begin(); it != this->resource_map_.end(); ++it)
+    {
+      it->second.reset();
+    }
+  }
+  /*\}*/
+};
 
 /** Interface for enforcing limits on an effort-controlled joint through saturation. */
 class EffortJointSaturationInterface : public JointLimitsInterface<EffortJointSaturationHandle> {};

diff --git a/joint_limits_interface/test/joint_limits_interface_test.cpp b/joint_limits_interface/test/joint_limits_interface_test.cpp
@@ -454,6 +454,59 @@ TEST_F(JointLimitsInterfaceTest, InterfaceRegistration)
   EXPECT_GT(cmd_handle2.getPosition(), cmd_handle2.getCommand());
 }
 
+TEST_F(JointLimitsHandleTest, ResetSaturationInterface)
+{
+  // Populate interface
+  PositionJointSaturationHandle limits_handle1(cmd_handle, limits);
+
+  PositionJointSaturationInterface iface;
+  iface.registerHandle(limits_handle1);
+
+  iface.enforceLimits(period); // initialize limit handles
+
+  const double max_increment = period.toSec() * limits.max_velocity;
+
+  cmd_handle.setCommand(limits.max_position);
+  iface.enforceLimits(period);
+
+  EXPECT_NEAR(cmd_handle.getCommand(),  max_increment, EPS);
+
+  iface.reset();
+  pos = limits.max_position;
+  cmd_handle.setCommand(limits.max_position);
+  iface.enforceLimits(period);
+
+  EXPECT_NEAR(cmd_handle.getCommand(),  limits.max_position, EPS);
+
+}
+
+
+TEST_F(JointLimitsHandleTest, ResetSoftLimitsInterface)
+{
+  // Populate interface
+  PositionJointSoftLimitsHandle limits_handle1(cmd_handle, limits, soft_limits);
+
+  PositionJointSoftLimitsInterface iface;
+  iface.registerHandle(limits_handle1);
+
+  iface.enforceLimits(period); // initialize limit handles
+
+  const double max_increment = period.toSec() * limits.max_velocity;
+
+  cmd_handle.setCommand(limits.max_position);
+  iface.enforceLimits(period);
+
+  EXPECT_NEAR(cmd_handle.getCommand(),  max_increment, EPS);
+
+  iface.reset();
+  pos = limits.max_position;
+  cmd_handle.setCommand(soft_limits.max_position);
+  iface.enforceLimits(period);
+
+  EXPECT_NEAR(cmd_handle.getCommand(),  soft_limits.max_position, EPS);
+
+}
+
 int main(int argc, char** argv)
 {
   testing::InitGoogleTest(&argc, argv);