Make gripping force configurable in the pick and place demo

examples/kuka_iiwa_arm/pick_and_place/action.cc

@@ -69,24 +69,26 @@ bool IiwaMove::ActionFinished(const WorldState& est_state) const {
 WsgAction::WsgAction() {}
 
 void WsgAction::OpenGripper(const WorldState& est_state,
+                            double grip_force,
                             lcmt_schunk_wsg_command* msg) {
   StartAction(est_state.get_wsg_time());
   *msg = lcmt_schunk_wsg_command();
   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
+  msg->force = grip_force;
   last_command_ = kOpen;
 }
 
 void WsgAction::CloseGripper(const WorldState& est_state,
+                             double grip_force,
                              lcmt_schunk_wsg_command* msg) {
   StartAction(est_state.get_wsg_time());
   *msg = lcmt_schunk_wsg_command();
   msg->utime = est_state.get_wsg_time() * 1e6;
   msg->target_position_mm = 8;  // 0 would smash the fingers together
                                 // and keep applying force on a real
                                 // WSG when no object is grasped.
-  msg->force = 40;
+  msg->force = grip_force;
   last_command_ = kClose;
 }
 

examples/kuka_iiwa_arm/pick_and_place/action.h

@@ -124,16 +124,26 @@ class WsgAction : public Action {
 
   /**
    * Populates @p msg with an LCM message that tells the WSG gripper
-   * driver to fully open.
+   * driver to fully open.  @p grip_force is passed to the gripper,
+   * but since the gripper is opening and (hopefully) not pushing
+   * against anything in this direction, it mostly affects the
+   * velocity of the fingers.
    */
   void OpenGripper(const WorldState& est_state,
+                   double grip_force,
                    lcmt_schunk_wsg_command* msg);
 
   /**
    * Populates @p msg with an LCM message that tells the WSG gripper
-   * driver to fully close.
+   * driver to fully close.  @p grip_force specifies the target
+   * gripping force to be applied to the grasped object.  The gripper
+   * will internally attempt to maintain this amount of clamping force
+   * by driving the motor appropriately, though the result will not be
+   * precise (and the actual outcome may depend on the gripper
+   * configuration).
    */
   void CloseGripper(const WorldState& est_state,
+                    double grip_force,
                     lcmt_schunk_wsg_command* msg);
 
   // TODO(siyuanfeng): Implement something meaningful here like a check for a

examples/kuka_iiwa_arm/pick_and_place/configuration/yellow_posts.pick_and_place_configuration

@@ -148,6 +148,7 @@ object {
 # effector is attached to.
 task {
   end_effector_name: "iiwa_link_ee"
+  grip_force: 42
 }
 
 # Contact parameters

examples/kuka_iiwa_arm/pick_and_place/pick_and_place_configuration.h

@@ -46,6 +46,8 @@ struct PlannerConfiguration {
   /// Number of tables for which the planner should expect to receive pose
   /// inputs.
   int num_tables{0};
+  /// Gripping force to apply (in Newtons).
+  double grip_force{40.};
 
   /// Returns the absolute path for our @p drake_relative_model_path.
   std::string absolute_model_path() const {

examples/kuka_iiwa_arm/pick_and_place/pick_and_place_configuration.proto

@@ -52,7 +52,7 @@ message ModelInstance {
     optional Pose pose = 3;
 }
 
-// Next ID: 4
+// Next ID: 5
 message PickAndPlaceTask {
     // The index of the robot arm to be used for this task.
     // DEFAULT: 0
@@ -62,6 +62,8 @@ message PickAndPlaceTask {
     optional int32 target_index = 2;
     // The name of the end-effector body in the robot.
     required string end_effector_name = 3;
+    // The gripping force to apply (in Newtons).
+    optional double grip_force = 4;
 }
 
 // Next ID: 5

examples/kuka_iiwa_arm/pick_and_place/pick_and_place_configuration_parsing.cc

@@ -187,6 +187,10 @@ pick_and_place::PlannerConfiguration DoParsePlannerConfiguration(
           configuration.robot(planner_configuration.robot_index));
   planner_configuration.end_effector_name = end_effector_name;
 
+  if (task.grip_force() != 0) {
+    planner_configuration.grip_force = task.grip_force();
+  }
+
   // Extract number of tables
   planner_configuration.num_tables = configuration.table_size();
 

examples/kuka_iiwa_arm/pick_and_place/pick_and_place_state_machine.cc

@@ -258,14 +258,18 @@ PostureInterpolationResult PlanInterpolatingMotion(
   return result;
 }
 
-void OpenGripper(const WorldState& env_state, WsgAction* wsg_act,
+void OpenGripper(const WorldState& env_state,
+                 double grip_force,
+                 WsgAction* wsg_act,
                  lcmt_schunk_wsg_command* msg) {
-  wsg_act->OpenGripper(env_state, msg);
+  wsg_act->OpenGripper(env_state, grip_force, msg);
 }
 
-void CloseGripper(const WorldState& env_state, WsgAction* wsg_act,
+void CloseGripper(const WorldState& env_state,
+                  double grip_force,
+                  WsgAction* wsg_act,
                   lcmt_schunk_wsg_command* msg) {
-  wsg_act->CloseGripper(env_state, msg);
+  wsg_act->CloseGripper(env_state, grip_force, msg);
 }
 
 std::unique_ptr<RigidBodyTree<double>> BuildTree(
@@ -869,7 +873,8 @@ void PickAndPlaceStateMachine::Update(const WorldState& env_state,
     case PickAndPlaceState::kPlace: {
       if (!wsg_act_.ActionStarted()) {
         lcmt_schunk_wsg_command msg;
-        schunk_action(env_state, &wsg_act_, &msg);
+        schunk_action(env_state, configuration_.grip_force,
+                      &wsg_act_, &msg);
         wsg_callback(&msg);
 
         drake::log()->info("{} at {}", state_, env_state.get_iiwa_time());

examples/kuka_iiwa_arm/pick_and_place/test/pick_and_place_configuration_parsing_test.cc

@@ -243,6 +243,7 @@ class ConfigurationParsingTests : public ::testing::Test {
     planner_configuration_.end_effector_name = kEndEffectorName;
     planner_configuration_.target_dimensions = kTargetDimensions;
     planner_configuration_.num_tables = 6;
+    planner_configuration_.grip_force = 42;
   }
 
   SimulatedPlantConfiguration plant_configuration_;