Sawyer Sim Examples Pkg: IK Pick example

- Adds sawyer_sim_examples package

- Adds pick_and_place example to show:
-- Spawning of models in Gazebo
-- An example of how to call the Inverse Kinematics service
   through the Limb() interface
-- An example of incredibly simple linearly-interpolated
   Cartesian movements
-- Simple object manipulation

sawyer_gazebo/include/sawyer_gazebo/arm_controller_interface.h

@@ -37,13 +37,11 @@ namespace sawyer_gazebo {
     std::mutex mtx_;
     int current_mode_;
     std::string side_;
-    ros::Subscriber speed_ratio_sub_;
     ros::Subscriber joint_command_timeout_sub_;
     ros::Subscriber joint_command_sub_;
     boost::shared_ptr<controller_manager::ControllerManager> controller_manager_;
 
   protected:
-    void speedRatioCallback(const std_msgs::Float64 msg);
     void jointCommandTimeoutCallback(const std_msgs::Float64 msg);
     void jointCommandCallback(const intera_core_msgs::JointCommandConstPtr& msg);
     std::string getControllerString(std::string mode_str);

sawyer_gazebo/src/arm_controller_interface.cpp

@@ -24,18 +24,12 @@ void ArmControllerInterface::init(ros::NodeHandle& nh, std::string side,
   current_mode_ = -1;
   side_ = side;
   controller_manager_ = controller_manager;
-  speed_ratio_sub_ = nh.subscribe("limb/"+side_+"/set_speed_ratio", 1,
-                       &ArmControllerInterface::speedRatioCallback, this);
   joint_command_timeout_sub_ = nh.subscribe("limb/"+side_+"/joint_command_timeout", 1,
                        &ArmControllerInterface::jointCommandTimeoutCallback, this);
   joint_command_sub_ = nh.subscribe("limb/"+side_+"/joint_command", 1,
                        &ArmControllerInterface::jointCommandCallback, this);
 }
 
-void ArmControllerInterface::speedRatioCallback(const std_msgs::Float64 msg) {
-  ROS_INFO_STREAM_NAMED("ros_control_plugin", "Data: " << msg.data);
-}
-
 void ArmControllerInterface::jointCommandTimeoutCallback(const std_msgs::Float64 msg) {
   ROS_INFO_STREAM_NAMED("sawyer_control_plugin", "Joint command timeout: " << msg.data);
 }

sawyer_sim_controllers/config/sawyer_sim_controllers.yaml

@@ -13,7 +13,7 @@ robot:
     joints:
           head_controller:
             joint: head_pan
-            pid: {p: 700.0, i: 0.01, d: 100.0}
+            pid: {p: 100.0, i: 1.0, d: 10.0}
 
   # Sawyer SDK Controllers: Position --------------------------
   right_joint_position_controller:
@@ -26,22 +26,22 @@ robot:
         pid: {p: 700,  i: 0.01, d: 100}
       right_j1_controller:
         joint: right_j1
-        pid: {p: 100,  i: 100, d: 100}
+        pid: {p: 700,  i: 0.01, d: 100}
       right_j2_controller:
         joint: right_j2
-        pid: {p: 100,  i: 100, d: 100}
+        pid: {p: 700,  i: 0.01, d: 100}
       right_j3_controller:
         joint: right_j3
-        pid: {p: 100,  i: 100, d: 100}
+        pid: {p: 700,  i: 0.01, d: 100}
       right_j4_controller:
         joint: right_j4
-        pid: {p: 100,  i: 100, d: 100}
+        pid: {p: 700,  i: 0.01, d: 100}
       right_j5_controller:
         joint: right_j5
-        pid: {p: 100,  i: 100, d: 100}
+        pid: {p: 700,  i: 0.01, d: 100}
       right_j6_controller:
         joint: right_j6
-        pid: {p: 100,  i: 100, d: 100}
+        pid: {p: 700,  i: 0.01, d: 100}
 
   # Sawyer SDK Controllers: Velocity --------------------------
   right_joint_velocity_controller:
@@ -123,4 +123,3 @@ robot:
       right_j6_controller:
         joint: right_j6
         pid: {p: 100,  i: 100, d: 100}
-

sawyer_sim_examples/CMakeLists.txt

@@ -0,0 +1,30 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(sawyer_sim_examples)
+
+find_package(catkin REQUIRED COMPONENTS
+  rospy
+  intera_core_msgs
+  gazebo_msgs
+)
+
+catkin_package(
+  CATKIN_DEPENDS
+  rospy
+  intera_core_msgs
+  gazebo_msgs
+)
+
+
+#############
+## Install ##
+#############
+
+install(PROGRAMS
+   scripts/ik_pick_and_place_demo.py
+   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
+
+foreach(dir launch models)
+   install(DIRECTORY ${dir}/
+      DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/${dir})
+endforeach(dir)

sawyer_sim_examples/launch/sawyer_pick_and_place_demo.launch

@@ -0,0 +1,12 @@
+<?xml version="1.0" encoding="utf-8"?>
+<launch>
+
+  <!-- We resume the logic in empty_world.launch, changing the name of the world to be launched -->
+  <include file="$(find sawyer_gazebo)/launch/sawyer_world.launch">
+    <arg name="electric_gripper" value="true"/>
+  </include>
+
+  <!-- Start the Sawyer pick and place demo -->
+  <node pkg="sawyer_sim_examples" type="ik_pick_and_place_demo.py" name="ik_pick_and_place_demo" />
+
+</launch>

sawyer_sim_examples/models/block/model.urdf

@@ -0,0 +1,29 @@
+<robot name="block">
+  <link name="block">
+    <inertial>
+      <origin xyz="0.025 0.025 0.025" />
+      <mass value="0.5" />
+      <!-- (1/12)*mass*(h^2 + d^2)  (1/12)*mass*(w^2 + d^2) (1/12)*mass*(w^2 + h^2) -->
+      <!-- (1/12)*0.5*(0.045^2 + 0.045^2)  (1/12)*0.5*(0.045^2 + 0.045^2) (1/12)*0.5*(0.045^2 + 0.045^2) -->
+      <inertia  ixx="0.00016874999" ixy="0.0"  ixz="0.0"  iyy="0.00016874999"  iyz="0.0"  izz="0.00016874999" />
+
+    </inertial>
+    <visual>
+      <origin xyz="0.025 0.025 0.025"/>
+      <geometry>
+        <box size="0.045 0.045 0.045" />
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="0.025 0.025 0.025"/>
+      <geometry>
+        <box size="0.045 0.045 0.045" />
+      </geometry>
+    </collision>
+  </link>
+  <gazebo reference="block">
+    <material>Gazebo/Red</material>
+     <mu1>1000</mu1>
+     <mu2>1000</mu2>
+  </gazebo>
+</robot>