Merge pull request #33 from RethinkRobotics/adds_disable_gravity

Adds disabling of Gravity Compensation Torques
RethinkRobotics · Mar 9, 2018 · beca9f6 · beca9f6
2 parents 23e661d + 24b7c55
commit beca9f6
Show file tree

Hide file tree

Showing 3 changed files with 38 additions and 9 deletions.
diff --git a/sawyer_sim_controllers/config/sawyer_sim_controllers.yaml b/sawyer_sim_controllers/config/sawyer_sim_controllers.yaml
@@ -100,7 +100,9 @@ robot:
   # Sawyer SDK Controllers: Gravity Compensation ------------
   right_joint_gravity_controller:
     type: sawyer_sim_controllers/SawyerGravityController
-    topic: /robot/limb/right/gravity_compensation_torques
+    command_topic: /robot/limb/right/gravity_compensation_torques
+    disable_topic: /robot/limb/right/suppress_gravity_compensation
+    disable_timeout: 0.2
     joints:
       right_j0_controller:
         joint: right_j0

diff --git a/sawyer_sim_controllers/include/sawyer_sim_controllers/sawyer_gravity_controller.h b/sawyer_sim_controllers/include/sawyer_sim_controllers/sawyer_gravity_controller.h
@@ -19,8 +19,11 @@
 
 #include <sawyer_sim_controllers/joint_array_controller.h>
 #include <intera_core_msgs/SEAJointState.h>
+#include <std_msgs/Empty.h>
+#include <realtime_tools/realtime_box.h>
 #include <sawyer_sim_controllers/sawyer_joint_effort_controller.h>
 #include <ros/node_handle.h>
+#include <ros/time.h>
 
 #include <control_toolbox/pid.h>
 
@@ -37,8 +40,12 @@ namespace sawyer_sim_controllers
 
   private:
     ros::Subscriber sub_joint_command_;
+    ros::Subscriber sub_gravity_disable_;
+    realtime_tools::RealtimeBox< std::shared_ptr<const ros::Time > > box_disable_time_;
+    ros::Duration gravity_disable_timeout_;
 
     void gravityCommandCB(const intera_core_msgs::SEAJointStateConstPtr& msg);
+    void gravityDisableCB(const std_msgs::Empty& msg);
   };
 }
 

diff --git a/sawyer_sim_controllers/src/sawyer_gravity_controller.cpp b/sawyer_sim_controllers/src/sawyer_gravity_controller.cpp
@@ -23,28 +23,48 @@ namespace sawyer_sim_controllers {
     // TODO: use constant, don't hardcode ctrl_subtype ("GRAVITY_COMPENSATION")
     if(!sawyer_sim_controllers::JointArrayController<sawyer_effort_controllers::JointEffortController>::init(hw, n, "GRAVITY_COMPENSATION")) {
       return false;
+    }
+    std::string command_topic;
+    if (n.getParam("command_topic", command_topic)) {
+      ros::NodeHandle nh("~");
+      sub_joint_command_ = nh.subscribe(command_topic, 1, &SawyerGravityController::gravityCommandCB, this);
     } else {
-      std::string topic_name;
-      if (n.getParam("topic", topic_name)) {
-        ros::NodeHandle nh("~");
-        sub_joint_command_ = nh.subscribe(topic_name, 1, &SawyerGravityController::gravityCommandCB, this);
-      } else {
-        sub_joint_command_ = n.subscribe("gravity_command", 1, &SawyerGravityController::gravityCommandCB, this);
-      }
+      sub_joint_command_ = n.subscribe("gravity_command", 1, &SawyerGravityController::gravityCommandCB, this);
+    }
+    std::string disable_topic;
+    if (n.getParam("disable_topic", disable_topic)) {
+      ros::NodeHandle nh("~");
+      sub_gravity_disable_ = nh.subscribe(disable_topic, 1, &SawyerGravityController::gravityDisableCB, this);
+    } else {
+      sub_gravity_disable_ = n.subscribe("gravity_disable", 1, &SawyerGravityController::gravityDisableCB, this);
     }
+    // In order to disable the gravity compensation torques,
+    // an empty message should be published at a frequency greater than (1/disable_timeout) Hz
+    double disable_timeout;
+    n.param<double>("disable_timeout", disable_timeout, 0.2);
+    gravity_disable_timeout_ = ros::Duration(disable_timeout);
     return true;
   }
 
+  void SawyerGravityController::gravityDisableCB(const std_msgs::Empty& msg) {
+      auto p_disable_msg_time = std::make_shared<ros::Time>(ros::Time::now());
+      box_disable_time_.set(p_disable_msg_time);
+      ROS_INFO_STREAM_THROTTLE(60, "Gravity compensation torques are disabled...");
+  }
+
   void SawyerGravityController::gravityCommandCB(const intera_core_msgs::SEAJointStateConstPtr& msg) {
 
       std::vector<Command> commands;
       if (msg->name.size() != msg->gravity_only.size()) {
         ROS_ERROR_STREAM_NAMED(JOINT_ARRAY_CONTROLLER_NAME, "Gravity commands size does not match joints size");
       }
+      std::shared_ptr<const ros::Time>  p_disable_msg_time;
+      box_disable_time_.get(p_disable_msg_time);
+      bool enable_gravity = !p_disable_msg_time || ((ros::Time::now() - *p_disable_msg_time.get()) > gravity_disable_timeout_);
       for (int i = 0; i < msg->name.size(); i++) {
         Command cmd = Command();
         cmd.name_ = msg->name[i];
-        cmd.effort_ = msg->gravity_only[i];
+        cmd.effort_ = enable_gravity ? msg->gravity_only[i] : 0.0;
         commands.push_back(cmd);
       }
       command_buffer_.writeFromNonRT(commands);