Add a Gazebo plugin to better match driver interfaces

stretch_description/urdf/stretch_main.xacro

@@ -13,7 +13,7 @@
         xyz="-0.109461304328163 -0.000741018909047708 0.0914915269429946"
         rpy="0 0 0" />
       <mass
-        value="1.0723782659782" />
+        value="6.0" />
       <inertia
         ixx="0.00310580907710135"
         ixy="1.5182848191076E-06"

stretch_gazebo/CMakeLists.txt

@@ -1,16 +1,50 @@
 cmake_minimum_required(VERSION 3.0.2)
 project(stretch_gazebo)
 
-find_package(catkin REQUIRED)
+find_package(catkin REQUIRED COMPONENTS
+        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}_ros_control_plugin ${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 config
-    DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
-)
-install(DIRECTORY launch
+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(DIRECTORY config launch urdf
     DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
-)
-install(DIRECTORY urdf
-DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
-)
+        )
+
+install(TARGETS ${PROJECT_NAME}_ros_control_plugin ${PROJECT_NAME}
+        ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+        LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+        RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+        )

stretch_gazebo/config/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}

stretch_gazebo/config/gazebo_ros_control_params.yaml

@@ -0,0 +1,14 @@
+gazebo_ros_control/pid_gains:
+  #joint_right_wheel: {p: 10000, i: 0.0, d: 0.4}
+  #joint_left_wheel: {p: 10000, i: 0.0, d: 0.4}
+  joint_lift: {p: 5000, i: 0.0, d: 200}
+  joint_arm_l0: {p: 100, i: 0.0, d: 30}
+  joint_arm_l1: {p: 100, i: 0.0, d: 30}
+  joint_arm_l2: {p: 100, i: 0.0, d: 30}
+  joint_arm_l3: {p: 100, i: 0.0, d: 30}
+  joint_wrist_yaw: {p: 20, i: 5.0, d: 0.4}
+  joint_head_pan: {p: 2, i: 0.0, d: 0.4}
+  joint_head_tilt: {p: 2, i: 0.0, d: 0.4}
+  joint_gripper_finger_left: {p: 0.5, i: 0.0, d: 0.4}
+  joint_gripper_finger_right: {p: 0.5, i: 0.0, d: 0.4}
+

stretch_gazebo/include/stretch_gazebo/stretch_hardware_gazebo.h

@@ -0,0 +1,173 @@
+/*********************************************************************
+ * 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
+*/
+
+#pragma once
+// 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 stretch_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;
+
+}