Improve sim-real correspondence

Add Gazebo plugin
This plug in is a modified version of the original ros_control plugin and that plugin's "default hardware plugin" (confusing, I know)
The modified model plugin provides a /runstop service to match the real robot's driver, and hooks this up to ros_controls existing estop implementation
The modified hardware plugin provides the mode switching services that the real robot has (mocked for now) and publishes joint states that include the virtual joints used by the real platform. The wrist extension joint behaves the same as the driver implementation. The rotation and translation joints aren't hooked up yet, and it may be difficult to provide their functionality without forking the actual "follow_joint_trajectory" implementation, which expects only joints that are present in the URDF
Mirror the real platform setup and use a joint state publisher node to republish the subset of joints that match the URDF

Add controller shim that mimics the fake joints used by the real robot

Use effort control for all arm joints
Increase mass of base
Makes tipping behavior in simulation more plausible
I assume that the mass of an assembled base is at least this heavy, given that the robot is around 50lbs total
Use single controller for the simulation
This better matches the physical robot, though this interface doesn't handle the virtual joints that the robot uses

This commit tracks the changes proposed in hello-robot/stretch_ros#51

CMakeLists.txt

@@ -2,12 +2,62 @@ cmake_minimum_required(VERSION 2.8.3)
 project(hcrl_gazebo)
 
 find_package(catkin REQUIRED COMPONENTS
-  gazebo_ros
+        gazebo_dev
+        roscpp
+        std_msgs
+        control_toolbox
+        controller_manager
+        hardware_interface
+        transmission_interface
+        pluginlib
+        joint_limits_interface
+        urdf
+        angles
 )
 
-catkin_package()
+catkin_package(
+        INCLUDE_DIRS include
+        LIBRARIES ${PROJECT_NAME}
+        CATKIN_DEPENDS
+        roscpp
+        std_msgs
+        controller_manager
+        control_toolbox
+        pluginlib
+        hardware_interface
+        transmission_interface
+        joint_limits_interface
+        urdf
+        angles
+        hardware_interface)
 
 install(DIRECTORY launch models worlds maps photos robots
   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
 )
 
+
+include_directories(include ${catkin_INCLUDE_DIRS} ${GAZEBO_INCLUDE_DIRS})
+
+add_library(${PROJECT_NAME}_ros_control_plugin src/stretch_hardware_gazebo_plugin.cpp)
+target_link_libraries(${PROJECT_NAME}_ros_control_plugin ${catkin_LIBRARIES})
+
+add_library(${PROJECT_NAME} src/stretch_hardware_gazebo.cpp)
+target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ${GAZEBO_LIBRARIES})
+
+
+install(TARGETS ${PROJECT_NAME}
+        ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+        LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+        RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+        )
+
+install(DIRECTORY include/
+        DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}
+        FILES_MATCHING PATTERN "*.h"
+        )
+install (FILES hcrl_gazebo_plugins.xml
+        DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
+
+
+catkin_install_python(PROGRAMS scripts/follow_joint_trajectory_server scripts/unpause_after_wait
+        DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})

config/stretch/body.yaml

@@ -0,0 +1,35 @@
+stretch_controller_raw:
+  type: "effort_controllers/JointTrajectoryController"
+  joints:
+    - joint_head_pan
+    - joint_head_tilt
+    - joint_lift
+    - joint_arm_l3
+    - joint_arm_l2
+    - joint_arm_l1
+    - joint_arm_l0
+    - joint_wrist_yaw
+    - joint_gripper_finger_right
+    - joint_gripper_finger_left
+  allow_partial_joints_goal: true
+
+  constraints:
+    goal_time: 0.6
+    stopped_velocity_tolerance: 0.05
+
+    stop_trajectory_duration: 0.5
+    state_publish_rate:  25
+    action_monitor_rate: 10
+
+
+  gains:
+    joint_head_pan: {p: 10, d: 1, i: 0, i_clamp: 1}
+    joint_head_tilt: {p: 10, d: 1, i: 0, i_clamp: 1}
+    joint_lift: {p: 500, d: 100, i: 0, i_clamp: 1}
+    joint_arm_l3: {p: 100, d: 30, i: 0, i_clamp: 1}
+    joint_arm_l2: {p: 100, d: 30, i: 0, i_clamp: 1}
+    joint_arm_l1: {p: 100, d: 30, i: 0, i_clamp: 1}
+    joint_arm_l0: {p: 100, d: 30, i: 0, i_clamp: 1}
+    joint_wrist_yaw: {p: 100, d: 1, i: 0, i_clamp: 1}
+    joint_gripper_finger_right: {p: 100, d: 1, i: 0, i_clamp: 1}
+    joint_gripper_finger_left: {p: 100, d: 1, i: 0, i_clamp: 1}

hcrl_gazebo_plugins.xml

@@ -0,0 +1,12 @@
+<library path="lib/libhcrl_gazebo">
+
+    <class
+            name="hcrl_gazebo/StretchHardwareGazebo"
+            type="hcrl_gazebo::StretchHardwareGazebo"
+            base_class_type="gazebo_ros_control::RobotHWSim">
+        <description>
+            Stretch robot simulation interface.
+        </description>
+    </class>
+</library>
+

include/hcrl_gazebo/stretch_hardware_gazebo.h

@@ -0,0 +1,175 @@
+#pragma once
+
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2013, Open Source Robotics Foundation
+ *  Copyright (c) 2013, The Johns Hopkins University
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of the Open Source Robotics Foundation
+ *     nor the names of its contributors may be
+ *     used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *********************************************************************/
+
+/* Author: Dave Coleman, Jonathan Bohren
+   Desc:   Hardware Interface for any simulated robot in Gazebo
+*/
+
+
+// ros_control
+#include <control_toolbox/pid.h>
+#include <hardware_interface/joint_command_interface.h>
+#include <hardware_interface/robot_hw.h>
+#include <joint_limits_interface/joint_limits.h>
+#include <joint_limits_interface/joint_limits_interface.h>
+#include <joint_limits_interface/joint_limits_rosparam.h>
+#include <joint_limits_interface/joint_limits_urdf.h>
+
+// Gazebo
+#include <gazebo/common/common.hh>
+#include <gazebo/physics/physics.hh>
+#include <gazebo/gazebo.hh>
+
+// ROS
+#include <ros/ros.h>
+#include <angles/angles.h>
+#include <pluginlib/class_list_macros.h>
+#include <sensor_msgs/JointState.h>
+#include <std_srvs/SetBool.h>
+
+// gazebo_ros_control
+#include <gazebo_ros_control/robot_hw_sim.h>
+
+// URDF
+#include <urdf/model.h>
+
+
+
+namespace hcrl_gazebo
+{
+
+class StretchHardwareGazebo : public gazebo_ros_control::RobotHWSim
+{
+public:
+
+  virtual bool initSim(
+      const std::string& robot_namespace,
+      ros::NodeHandle model_nh,
+      gazebo::physics::ModelPtr parent_model,
+      const urdf::Model *const urdf_model,
+      std::vector<transmission_interface::TransmissionInfo> transmissions);
+
+  virtual void readSim(ros::Time time, ros::Duration period);
+
+  virtual void writeSim(ros::Time time, ros::Duration period);
+
+  virtual void eStopActive(const bool active);
+
+protected:
+  // Methods used to control a joint.
+  enum ControlMethod {EFFORT, POSITION, POSITION_PID, VELOCITY, VELOCITY_PID};
+
+  enum RobotMode {MANIPULATION, NAVIGATION, POSITION_MODE};
+
+
+  // Register the limits of the joint specified by joint_name and joint_handle. The limits are
+  // retrieved from joint_limit_nh. If urdf_model is not NULL, limits are retrieved from it also.
+  // Return the joint's type, lower position limit, upper position limit, and effort limit.
+  void registerJointLimits(const std::string& joint_name,
+                           const hardware_interface::JointHandle& joint_handle,
+                           const ControlMethod ctrl_method,
+                           const ros::NodeHandle& joint_limit_nh,
+                           const urdf::Model *const urdf_model,
+                           int *const joint_type, double *const lower_limit,
+                           double *const upper_limit, double *const effort_limit);
+
+  bool setManipulationModeCB(std_srvs::SetBool::Request  &req,
+                    std_srvs::SetBool::Response &res);
+
+  bool setNavigationModeCB(std_srvs::SetBool::Request  &req,
+                             std_srvs::SetBool::Response &res);
+
+  bool setPositionModeCB(std_srvs::SetBool::Request  &req,
+                             std_srvs::SetBool::Response &res);
+
+  void updateJointStates(const ros::TimerEvent& e);
+
+  unsigned int n_dof_;
+
+  hardware_interface::JointStateInterface    js_interface_;
+  hardware_interface::EffortJointInterface   ej_interface_;
+  hardware_interface::PositionJointInterface pj_interface_;
+  hardware_interface::VelocityJointInterface vj_interface_;
+
+  joint_limits_interface::EffortJointSaturationInterface   ej_sat_interface_;
+  joint_limits_interface::EffortJointSoftLimitsInterface   ej_limits_interface_;
+  joint_limits_interface::PositionJointSaturationInterface pj_sat_interface_;
+  joint_limits_interface::PositionJointSoftLimitsInterface pj_limits_interface_;
+  joint_limits_interface::VelocityJointSaturationInterface vj_sat_interface_;
+  joint_limits_interface::VelocityJointSoftLimitsInterface vj_limits_interface_;
+
+  std::vector<std::string> joint_names_;
+  std::vector<int> joint_types_;
+  std::vector<double> joint_lower_limits_;
+  std::vector<double> joint_upper_limits_;
+  std::vector<double> joint_effort_limits_;
+  std::vector<ControlMethod> joint_control_methods_;
+  std::vector<control_toolbox::Pid> pid_controllers_;
+  std::vector<double> joint_position_;
+  std::vector<double> joint_velocity_;
+  std::vector<double> joint_effort_;
+  std::vector<double> joint_effort_command_;
+  std::vector<double> joint_position_command_;
+  std::vector<double> last_joint_position_command_;
+  std::vector<double> joint_velocity_command_;
+
+  int telescoping_joint_indices_[4];
+
+  std::vector<gazebo::physics::JointPtr> sim_joints_;
+
+  std::string physics_type_;
+
+  // e_stop_active_ is true if the emergency stop is active.
+  bool e_stop_active_, last_e_stop_active_;
+
+  ros::ServiceServer manipulation_mode_srv_;
+  ros::ServiceServer navigation_mode_srv_;
+  ros::ServiceServer position_mode_srv_;
+
+  RobotMode current_mode_;
+
+  ros::Timer joint_states_timer_;
+
+  std::shared_ptr<realtime_tools::RealtimePublisher<sensor_msgs::JointState> > realtime_pub_;
+  ros::Time last_publish_time_;
+  double publish_rate_;
+};
+
+typedef boost::shared_ptr<StretchHardwareGazebo> StretchHardwareGazeboPtr;
+
+}