Merge pull request RobotLocomotion#7392 from avalenzu/wsg-action-chec…

…k-position Check gripper position in WsgAction.
Tomato1107 · Nov 10, 2017 · 206c270 · 206c270
2 parents 401edc6 + c351521
commit 206c270
Show file tree

Hide file tree

Showing 2 changed files with 22 additions and 7 deletions.
diff --git a/drake/examples/kuka_iiwa_arm/pick_and_place/action.cc b/drake/examples/kuka_iiwa_arm/pick_and_place/action.cc
@@ -77,6 +77,7 @@ void WsgAction::OpenGripper(const WorldState& est_state,
   msg->utime = est_state.get_wsg_time() * 1e6;
   msg->target_position_mm = 100;  // Maximum aperture for WSG
   msg->force = 40;  // Force in center of WSG range
+  last_command_ = kOpen;
 }
 
 void WsgAction::CloseGripper(const WorldState& est_state,
@@ -88,15 +89,21 @@ void WsgAction::CloseGripper(const WorldState& est_state,
                                 // and keep applying force on a real
                                 // WSG when no object is grasped.
   msg->force = 40;
+  last_command_ = kClose;
 }
 
 bool WsgAction::ActionFinished(const WorldState& est_state) const {
   if (!ActionStarted()) return false;
-
-  const double max_finished_velocity = 1e-2;
-  if (std::abs(est_state.get_wsg_v()) < max_finished_velocity &&
-      (get_time_since_action_start(est_state.get_wsg_time()) > 0.5))
-    return true;
+  if (std::abs(est_state.get_wsg_v()) < kFinalSpeedThreshold &&
+      (get_time_since_action_start(est_state.get_wsg_time()) > 0.5)) {
+    if (last_command_ == kOpen &&
+        est_state.get_wsg_q() > kOpenPositionThreshold) {
+      return true;
+    } else if (last_command_ == kClose &&
+               est_state.get_wsg_q() < kOpenPositionThreshold) {
+      return true;
+    }
+  }
   return false;
 }
 

diff --git a/drake/examples/kuka_iiwa_arm/pick_and_place/action.h b/drake/examples/kuka_iiwa_arm/pick_and_place/action.h
@@ -143,10 +143,18 @@ class WsgAction : public Action {
   }
 
   /**
-   * Returns true if the gripper stopped moving, and it is at least 0.5 seconds
-   * after an Open / Close command was last issued.
+   * Returns true if the following criteria are satisfied:
+   *  - The gripper speed is less than the final speed threshold.
+   *  - 0.5 s have elapsed since the last Open/Close command was issued.
+   *  - The gripper position is greater than (for an Open command) or less than
+   *    (for a Close command) the open position threshold.
    */
   bool ActionFinished(const WorldState& est_state) const override;
+
+ private:
+  enum { kOpen, kClose } last_command_{kOpen};
+  static constexpr double kFinalSpeedThreshold = 1e-2;  // m/s
+  static constexpr double kOpenPositionThreshold = .095;   // m
 };
 
 }  // namespace pick_and_place