Merge pull request #20 from PickNikRobotics/pr-gazebo-tuning

Stretch Gazebo for ROS1
3 years ago · c7522ee213
--- a/BIN
+++ b/BIN
--- a/BIN
+++ b/BIN
--- a/BIN
+++ b/BIN
--- a/BIN
+++ b/BIN
--- a/stretch_description/urdf/stretch_aruco.xacro
+++ b/stretch_description/urdf/stretch_aruco.xacro
@ -119,7 +119,7 @@
        xyz="-2.77555756156289E-17 2.56739074444567E-16 -0.000125000000000042"
        rpy="0 0 0" />
      <mass
        value="0.000399141592653546" />
        value="0" />
      <inertia
        ixx="0"
        ixy="0"
@ -174,7 +174,7 @@
        xyz="1.11022302462516E-16 3.05311331771918E-16 -0.00012499999999821"
        rpy="0 0 0" />
      <mass
        value="0.000224999999999975" />
        value="0" />
      <inertia
        ixx="0"
        ixy="0"
@ -229,7 +229,7 @@
        xyz="8.32667268468867E-17 1.77635683940025E-15 -0.000125000000000264"
        rpy="0 0 0" />
      <mass
        value="0.000225" />
        value="0" />
      <inertia
        ixx="0"
        ixy="0"
--- a/stretch_description/urdf/stretch_d435i.xacro
+++ b/stretch_description/urdf/stretch_d435i.xacro
@ -3,18 +3,18 @@

  <xacro:arg name="use_nominal_extrinsics" default="true"/>
  <xacro:arg name="add_plug" default="false"/>
  <xacro:include filename="$(find realsense2_description)/urdf/_d435.urdf.xacro"/>
  <xacro:include filename="$(find realsense2_description)/urdf/_d435i.urdf.xacro"/>

  <!-- xyz = "
       depth (- recessed into head / + protruding from front)
       up and down (- down / + up)
       sideways (- right / + left)
       " -->
  <xacro:sensor_d435 parent="link_head_tilt" use_nominal_extrinsics="$(arg use_nominal_extrinsics)">
  <xacro:sensor_d435i parent="link_head_tilt" use_nominal_extrinsics="$(arg use_nominal_extrinsics)">
    <origin
      xyz="0.03 -0.0122 0.0182"
      rpy="0.0 0.0 0.0" />
  </xacro:sensor_d435>
  </xacro:sensor_d435i>
  
 </robot>

--- a/stretch_description/urdf/stretch_gripper.xacro
+++ b/stretch_description/urdf/stretch_gripper.xacro
@ -12,12 +12,12 @@
      <mass
        value="0.101902711393094" />
      <inertia
        ixx="2.79241618017607E-05"
        ixy="-1.10819247449605E-05"
        ixz="-1.50343284036654E-05"
        iyy="3.67945130513069E-05"
        iyz="-4.33280448535579E-06"
        izz="4.14524691955231E-05" />
        ixx="0.001"
        ixy="0"
        ixz="0"
        iyy="0.001"
        iyz="0"
        izz="0.001" />
    </inertial>
    <visual>
      <origin
@ -69,12 +69,12 @@
      <mass
        value="0.0476207785199474" />
      <inertia
        ixx="0"
        ixx="0.001"
        ixy="0"
        ixz="0"
        iyy="0"
        iyy="0.001"
        iyz="0"
        izz="0" />
        izz="0.001" />
    </inertial>
    <visual>
      <origin
@ -125,12 +125,12 @@
      <mass
        value="0.00382159917455729" />
      <inertia
        ixx="0"
        ixx="0.001"
        ixy="0"
        ixz="0"
        iyy="0"
        iyy="0.001"
        iyz="0"
        izz="0" />
        izz="0.001" />
    </inertial>
    <visual>
      <origin
@ -180,12 +180,12 @@
      <mass
        value="0.0476207785199481" />
      <inertia
        ixx="0"
        ixx="0.001"
        ixy="0"
        ixz="0"
        iyy="0"
        iyy="0.001"
        iyz="0"
        izz="0" />
        izz="0.001" />
    </inertial>
    <visual>
      <origin
@ -236,12 +236,12 @@
      <mass
        value="0.00382160037319545" />
      <inertia
        ixx="0"
        ixx="0.001"
        ixy="0"
        ixz="0"
        iyy="0"
        iyy="0.001"
        iyz="0"
        izz="0" />
        izz="0.001" />
    </inertial>
    <visual>
      <origin
@ -300,6 +300,3 @@
  </joint>
  
 </robot>

  

--- a/stretch_description/urdf/stretch_laser_range_finder.xacro
+++ b/stretch_description/urdf/stretch_laser_range_finder.xacro
@ -58,5 +58,4 @@

 </robot>

  

--- a/stretch_description/urdf/stretch_main.xacro
+++ b/stretch_description/urdf/stretch_main.xacro
@ -1,6 +1,11 @@
 <?xml version="1.0"?>
 <robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="stretch_main">
  
  <xacro:property name="M_PI" value="3.1415926535897931" /> 
  <xacro:property name="joint_damping" value="21.75"/>
  <xacro:property name="joint_friction" value="10.48"/>
  <xacro:property name="joint_spring_stiffness" value="0"/>
  <xacro:property name="joint_spring_reference" value="0"/>

  <link
    name="base_link">
    <inertial>
@ -49,14 +54,14 @@
        xyz="1.25554620866719E-07 3.54748938447003E-07 0.0239581106165018"
        rpy="0 0 0" />
      <mass
        value="0.00356714564938475" />
        value="0.01" />
      <inertia
        ixx="0"
        ixx="0.001"
        ixy="0"
        ixz="0"
        iyy="0"
        iyy="0.001"
        iyz="0"
        izz="0" />
        izz="0.001" />
    </inertial>
    <visual>
      <origin
@ -87,7 +92,7 @@
    name="joint_right_wheel"
    type="continuous">
    <origin
      xyz="-0.00300000000000034 -0.15765 0.0508000000000004"
      xyz="-0.003 -0.15765 0.05"
      rpy="-1.57079632679489 -1.11022302462516E-16 7.28583859910277E-17" />
    <parent
      link="base_link" />
@ -95,6 +100,7 @@
      link="link_right_wheel" />
    <axis
      xyz="0 0 1" />
      <dynamics damping="${joint_damping}" friction="${joint_friction}" spring_reference="${joint_spring_reference}" spring_stiffness="{joint_spring_stiffness}"/>
  </joint>
  
  <link
@ -104,14 +110,14 @@
        xyz="1.2555462092223E-07 -3.54748938502514E-07 -0.0239581106165035"
        rpy="0 0 0" />
      <mass
        value="0.00356714564938459" />
        value="0.01" />
      <inertia
        ixx="0"
        ixx="0.001"
        ixy="0"
        ixz="0"
        iyy="0"
        iyy="0.001"
        iyz="0"
        izz="0" />
        izz="0.001" />
    </inertial>
    <visual>
      <origin
@ -142,7 +148,7 @@
    name="joint_left_wheel"
    type="continuous">
    <origin
      xyz="-0.00300000000001899 0.15765 0.0507999999999994"
      xyz="-0.003 0.15765 0.05"
      rpy="-1.5707963267949 3.16227047920818E-31 7.88745438253713E-16" />
    <parent
      link="base_link" />
@ -150,8 +156,60 @@
      link="link_left_wheel" />
    <axis
      xyz="0 0 1" />
      <dynamics damping="${joint_damping}" friction="${joint_friction}" spring_reference="${joint_spring_reference}" spring_stiffness="{joint_spring_stiffness}"/>
  </joint>
  

  <link name="caster_link">
    <collision>
      <geometry>
        <sphere radius="0.032"/>
      </geometry>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <surface>
        <friction>
          <ode>
            <mu>0</mu>
            <mu2>0</mu2>
            <slip1>1.0</slip1>
            <slip2>1.0</slip2>
          </ode>
        </friction>
      </surface>
    </collision>
    <visual>
      <origin
        xyz="0 0 0"
        rpy="0 0 0" />
      <geometry>
        <mesh
          filename="package://stretch_description/meshes/omni_wheel_m.STL" />
      </geometry>
      <material
        name="">
        <color
          rgba="0.792156862745098 0.819607843137255 0.933333333333333 1" />
      </material>
    </visual>
    <inertial>
      <mass value="0.01" />
      <origin xyz="0 0 0" />
      <inertia
        ixx="0.001"
        ixy="0"
        ixz="0"
        iyy="0.001"
        iyz="0"
        izz="0.001" />
    </inertial>
  </link>

  <joint name="caster_joint" type="fixed">
    <parent link="base_link"/>
    <child link="caster_link"/>
    <origin xyz="-0.245 0.0 0.032" rpy="${-M_PI/2} 0 0"/>
    <axis xyz="0 0 1" />
  </joint>

  <link
    name="link_mast">
    <inertial>
@ -252,7 +310,7 @@
    name="joint_lift"
    type="prismatic">
    <origin
      xyz="-0.0369217062323472 0.165471199999996 -0.000341653286793524"
      xyz="-0.0369217062323472 0.268771199999996 -0.000341653286793524"
      rpy="-1.57079632679552 1.5615431375292 -6.2942004366467E-13" />
    <parent
      link="link_mast" />
@ -260,10 +318,7 @@
      link="link_lift" />
    <axis
 	xyz="0 0 1" />
  <!-- for now: hand copied range_m: from lift: from ~/repos/stretch_fleet/stretch-re1-1001/stretch_re1_factory_params.yaml -->
  <!--<limit effort="100" lower="0.0" upper="1.095" velocity="1.0"/>-->
  <!-- copied value did not reach the top of mesh model with GUI sliders and RViz -->
  <limit effort="100" lower="0.0" upper="1.1" velocity="1.0"/>
  <limit effort="100" lower="-0.1033" upper="0.9967" velocity="1.0"/>
  </joint>
  
  <link
@ -330,12 +385,12 @@
      <mass
        value="0.0626921047058405" />
      <inertia
        ixx="0.000115246639451415"
        ixy="4.42139679831176E-14"
        ixz="1.89711279909763E-09"
        iyy="0.000115246639420046"
        iyz="-2.68592949587539E-09"
        izz="9.38270676408352E-14" />
        ixx="0.001"
        ixy="0"
        ixz="0"
        iyy="0.001"
        iyz="0"
        izz="0.001" />
    </inertial>
    <visual>
      <origin
@ -374,7 +429,6 @@
      link="link_arm_l3" />
    <axis
 	xyz="0 0 1" />
    <!-- 0.13 = 0.52/4-->
    <limit effort="100" lower="0.0" upper="0.13" velocity="1.0"/>
  </joint>
  
@ -387,12 +441,12 @@
      <mass
        value="0.0569074368576238" />
      <inertia
        ixx="0.000105206697151872"
        ixy="2.59896259031046E-14"
        ixz="1.69941454517918E-09"
        iyy="0.000105206697154717"
        iyz="-1.60895568923743E-09"
        izz="5.20570301686304E-14" />
        ixx="0.001"
        ixy="0"
        ixz="0"
        iyy="0.001"
        iyz="0"
        izz="0.001" />
    </inertial>
    <visual>
      <origin
@ -431,7 +485,6 @@
      link="link_arm_l2" />
    <axis
 	xyz="0 0 1" />
    <!-- 0.13 = 0.52/4-->
    <limit effort="100" lower="0.0" upper="0.13" velocity="1.0"/>
  </joint>
  
@ -444,12 +497,12 @@
      <mass
        value="0.0511763619538321" />
      <inertia
        ixx="9.58490021274768E-05"
        ixy="2.27446667584309E-14"
        ixz="1.54951672295709E-09"
        iyy="9.5849002131875E-05"
        iyz="-1.40692486873422E-09"
        izz="4.57014633924348E-14" />
        ixx="0.001"
        ixy="0"
        ixz="0"
        iyy="0.001"
        iyz="0"
        izz="0.001" />
    </inertial>
    <visual>
      <origin
@ -488,7 +541,6 @@
      link="link_arm_l1" />
    <axis
 	xyz="0 0 1" />
    <!-- 0.13 = 0.52/4-->
    <limit effort="100" lower="0.0" upper="0.13" velocity="1.0"/>
  </joint>
  
@ -501,12 +553,12 @@
      <mass
        value="0.101241250325294" />
      <inertia
        ixx="0.000227156999010352"
        ixy="8.90680620109423E-06"
        ixz="-7.91132210985695E-05"
        iyy="0.000300732790537324"
        iyz="7.21771824540896E-06"
        izz="9.35784422621597E-05" />
        ixx="0.001"
        ixy="0"
        ixz="0"
        iyy="0.001"
        iyz="0"
        izz="0.001" />
    </inertial>
    <visual>
      <origin
@ -545,7 +597,6 @@
      link="link_arm_l0" />
    <axis
 	xyz="0 0 1" />
    <!-- 0.13 = 0.52/4-->
    <limit effort="100" lower="0.0" upper="0.13" velocity="1.0"/>
  </joint>
  
@ -558,12 +609,12 @@
      <mass
        value="0.0405398981326229" />
      <inertia
        ixx="2.20062857967227E-09"
        ixy="1.15494401227051E-13"
        ixz="1.02598056874346E-11"
        iyy="2.20039759185237E-09"
        iyz="-2.47692875363392E-11"
        izz="3.26666802416509E-13" />
        ixx="0.001"
        ixy="0"
        ixz="0"
        iyy="0.001"
        iyz="0"
        izz="0.001" />
    </inertial>
    <visual>
      <origin
@ -576,7 +627,7 @@
      <material
        name="">
        <color
 	  rgba="0.792156862745098 0.819607843137255 0.933333333333333 1" />
 	rgba="0.792156862745098 0.819607843137255 0.933333333333333 1" />
      </material>
    </visual>
    <collision>
@ -602,12 +653,6 @@
      link="link_wrist_yaw" />
    <axis
      xyz="0 0 -1" />
    <!--
 	stowed to front ~225 deg: 3.15159 x 1.25 = 3.9395
 	using 4.0
 	stowed to back ~100 deg:  100 / 180 x 3.14159 = 1.7453
 	using -1.75
    -->
    <limit effort="100" lower="-1.75" upper="4.0" velocity="1.0"/>
  </joint>
  
@ -620,12 +665,12 @@
      <mass
        value="0.129454717596498" />
      <inertia
        ixx="2.9340596231093E-05"
        ixy="-7.89638931853666E-07"
        ixz="-4.74678128515878E-06"
        iyy="2.68283736177377E-05"
        iyz="1.80251508621876E-06"
        izz="1.4617291423745E-05" />
        ixx="0.001"
        ixy="0"
        ixz="0"
        iyy="0.001"
        iyz="0"
        izz="0.001" />
    </inertial>
    <visual>
      <origin
@ -675,12 +720,12 @@
      <mass
        value="0.035858341182617" />
      <inertia
        ixx="2.75847937559827E-05"
        ixy="5.25762707913649E-07"
        ixz="-9.28796751052893E-07"
        iyy="2.08815921106858E-05"
        iyz="1.17265055331737E-05"
        izz="6.78612505435872E-06" />
        ixx="0.001"
        ixy="0"
        ixz="0"
        iyy="0.001"
        iyz="0"
        izz="0.001" />
    </inertial>
    <visual>
      <origin
@ -719,7 +764,6 @@
      link="link_head_pan" />
    <axis
 	xyz="0 0 1" />
    <!-- unconstrained range for now -->
    <limit effort="100" lower="-3.9" upper="1.5" velocity="1.0"/>
  </joint>
  
@ -732,12 +776,12 @@
      <mass
        value="0.0701242408063442" />
      <inertia
        ixx="1.73265934767466E-05"
        ixy="2.38363792824069E-07"
        ixz="7.96430204321056E-08"
        iyy="4.11044416077217E-05"
        iyz="-9.41955638215997E-07"
        izz="2.38851808140542E-05" />
        ixx="0.001"
        ixy="0"
        ixz="0"
        iyy="0.001"
        iyz="0"
        izz="0.001" />
    </inertial>
    <visual>
      <origin
@ -776,11 +820,7 @@
      link="link_head_tilt" />
    <axis
      xyz="0 0 1" />
    <!-- unconstrained range for now -->
    <limit effort="100" lower="-1.53" upper="0.79" velocity="1.0"/>
  </joint>

 </robot>

  

--- a/stretch_gazebo/CMakeLists.txt
+++ b/stretch_gazebo/CMakeLists.txt
@ -0,0 +1,16 @@
 cmake_minimum_required(VERSION 3.0.2)
 project(stretch_gazebo)

 find_package(catkin REQUIRED)

 catkin_package()

 install(DIRECTORY config
    DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
 )
 install(DIRECTORY launch
    DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
 )
 install(DIRECTORY urdf
 DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
 )
--- a/stretch_gazebo/README.md
+++ b/stretch_gazebo/README.md
@ -0,0 +1,90 @@
 ![](../images/banner.png)

 ## Overview

 *stretch_gazebo* is an implementation of simulating a Stretch robot with [Gazebo](http://gazebosim.org/) simulator.

 ## Details

 The *urdf directory* contains [a xacro file](http://wiki.ros.org/xacro) that extends the capabilities of the original xacro files living in *stretch_description* package to include Gazebo functionality.

 The *config* directory contains rviz files and [ros_control](http://wiki.ros.org/ros_control) controller configuration files for various parts of the robot including:

 * Base: [diff_drive_controller/DiffDriveController](http://wiki.ros.org/diff_drive_controller)
 * Arm: [position_controllers/JointTrajectoryController](http://wiki.ros.org/joint_trajectory_controller)
 * Gripper: [position_controllers/JointTrajectoryController](http://wiki.ros.org/joint_trajectory_controller)
 * Head: [position_controllers/JointTrajectoryController](http://wiki.ros.org/joint_trajectory_controller)
 * Joints: [joint_state_controller/JointStateController](http://wiki.ros.org/joint_state_controller)

 The *launch* directory includes two files:

 * gazebo.launch: Opens up an empty Gazebo world and spawns the robot loading all the controllers, including all the sensors except Cliff sensors and respeaker.
 * teleop_joy.launch: Spawns a joy and teleop_twist_joy instance and remaps *cmd_vel* topics to */stretch_diff_drive_controller/cmd_vel*, which the robot is taking drive commands from. Note that the *teleop_twist_joy* package has a deadman switch by default which disables the drive commands to be published unless it is being pressed. For an Logitech F310 joystick this button is A.

 The *script* directory contains a single python file that publishes ground truth odometry of the robot from Gazebo.

 ## Gazebo Sensors vs Stretch Sensors

 |         Sensor        |       Gazebo       |       Stretch      |       Notes       |
 |:---------------------:|:------------------:|:------------------:|:-----------------:|
 |         LIDAR         | :heavy_check_mark: | :heavy_check_mark: |                   |
 |        Base IMU       | :heavy_check_mark: | :heavy_check_mark: |                   |
 |  Wrist Accelerometer  | :heavy_check_mark: | :heavy_check_mark: | Modeled as an IMU |
 |    Realsense D435i    | :heavy_check_mark: | :heavy_check_mark: |                   |
 | Respeaker (Mic Array) |         :x:        | :heavy_check_mark: |                   |
 |     Cliff Sensors     |         :x:        | :heavy_check_mark: |                   |

 *Notes:* Although there is no microphone in Gazebo, Respeaker can be represented with a ROS node that accesses compputer's microphone. Cliff sensors are not modeled but they can also be represented as 1D LIDAR sensors. See LIDAR definition in *stretch_gazebo.urdf.xacro* file.

 ## Gazebo - Moveit Controllers vs stretch_core

 Actuators are defined as *ros_control* transmission objects in Gazebo using [PositionJointInterfaces](http://docs.ros.org/en/melodic/api/hardware_interface/html/c++/classhardware__interface_1_1PositionJointInterface.html). Moveit is configured to use three different action servers to control the body parts of stretch in Gazebo through the srdf file in *stretch_moveit_config* package. See the section below about moveit for details. Please note that this behaviour is different than *stretch_core* as it works with a single Python interface to control all the joints.

 ## Setup

 Clone stretch_ros and realsense_gazebo_plugin packages to your catkin workspace. Then install dependencies and build the packages, with the following set of commands:

 ```bash
    cd $CATKIN_WS/src
    git clone https://github.com/hello-robot/stretch_ros
    git clone https://github.com/pal-robotics/realsense_gazebo_plugin
    rosdep install -r --from-paths . --ignore-src --rosdistro melodic -y
    cd $CATKIN_WS
    catkin build
 ```

 ## Running Demo

 ```bash
    # Terminal 1:
    roslaunch stretch_gazebo gazebo.launch rviz:=true
    # Terminal 2:
    roslaunch stretch_gazebo teleop_joy.launch
 ```

 This will launch an Rviz instance that visualizes the sensors and an empty world in Gazebo with Stretch and load all the controllers. Although, the base will be able to move with the joystick comamnds, there joystick won't give joint commands to arm, head or gripper. To move these joints see the next section about *Running Gazebo with MoveIt! and Stretch*.

 ![](../images/gazebo.png)

 ## Running Gazebo with MoveIt! and Stretch

 ```bash
    # Terminal 1:
    roslaunch stretch_gazebo gazebo.launch
    # Terminal 2:
    roslaunch stretch_gazebo teleop_joy.launch
    # Terminal 3
    roslaunch stretch_moveit_config demo_gazebo.launch
 ```

 This will launch an Rviz instance that visualizes the joints with markers and an empty world in Gazebo with Stretch and load all the controllers. There are pre-defined positions for each joint group for demonstration purposes. There are three joint groups, namely *stretch_arm*, *stretch_gripper* and *stretch_head* that can be controlled individually via Motion Planning Rviz plugin. Start and goal positions for joints can be selected similar to [this moveit tutorial](https://ros-planning.github.io/moveit_tutorials/doc/quickstart_in_rviz/quickstart_in_rviz_tutorial.html#choosing-specific-start-goal-states). A few notes to be kept in mind:

 * Planning group can be changed via *Planning Group* drop down in Planning tab of Motion Planning Rviz plugin.
 * Pre-defined start and goal states can be speciified in *Start State* and *Goal State* drop downs in Planning tab of Motion Planning Rviz plugin.  
 * *stretch_gripper* group does not show markers, and is intended to be controlled via the joints tab that is located in the very right of Motion Planning Rviz plugin.
 * When planning with *stretch_head* group make sure you select *Approx IK Solutions* in Planning tab of Motion Planning Rviz plugin.

 ![](../images/gazebo_moveit.gif)
 ## License

 For license information, please see the LICENSE files.
--- a/stretch_gazebo/config/arm.yaml
+++ b/stretch_gazebo/config/arm.yaml
@ -0,0 +1,23 @@
 stretch_arm_controller:
  type: "position_controllers/JointTrajectoryController"
  joints:
    - joint_lift
    - joint_arm_l3
    - joint_arm_l2
    - joint_arm_l1
    - joint_arm_l0
    - joint_wrist_yaw
  allow_partial_joints_goal: true

  constraints:
    goal_time: 0.6
    stopped_velocity_tolerance: 0.05
    joint_lift: {trajectory: 0.1, goal: 0.1}
    joint_arm_l3: {trajectory: 0.1, goal: 0.1}
    joint_arm_l2: {trajectory: 0.1, goal: 0.1}
    joint_arm_l1: {trajectory: 0.1, goal: 0.1}
    joint_arm_l0: {trajectory: 0.1, goal: 0.1}
    joint_wrist_yaw: {trajectory: 0.1, goal: 0.1}
    stop_trajectory_duration: 0.5
    state_publish_rate:  25
    action_monitor_rate: 10
--- a/stretch_gazebo/config/drive_config.yaml
+++ b/stretch_gazebo/config/drive_config.yaml
@ -0,0 +1,31 @@
 type: "diff_drive_controller/DiffDriveController"
 publish_rate: 50

 left_wheel: [joint_left_wheel]
 right_wheel: [joint_right_wheel]

 wheel_separation: 0.315
 wheel_radius: 0.05

 # Odometry covariances for the encoder output of the robot. These values should
 # be tuned to your robot's sample odometry data, but these values are a good place
 # to start
 pose_covariance_diagonal: [0.001, 0.001, 0.001, 0.001, 0.001, 0.03]
 twist_covariance_diagonal: [0.001, 0.001, 0.001, 0.001, 0.001, 0.03]

 # Top level frame (link) of the robot description
 base_frame_id: base_link

 # Velocity and acceleration limits for the robot
 linear:
  x:
    has_velocity_limits    : true
    max_velocity           : 0.5   # m/s
    has_acceleration_limits: true
    max_acceleration       : 1.0   # m/s^2
 angular:
  z:
    has_velocity_limits    : true
    max_velocity           : 2.0   # rad/s
    has_acceleration_limits: true
    max_acceleration       : 6.0   # rad/s^2
--- a/stretch_gazebo/config/gripper.yaml
+++ b/stretch_gazebo/config/gripper.yaml
@ -0,0 +1,15 @@
 stretch_gripper_controller:
  type: "position_controllers/JointTrajectoryController"
  joints:
    - joint_gripper_finger_right
    - joint_gripper_finger_left
  allow_partial_joints_goal: true

  constraints:
    goal_time: 0.6
    stopped_velocity_tolerance: 0.05
    joint_gripper_finger_right: {trajectory: 0.1, goal: 0.1}
    joint_gripper_finger_left: {trajectory: 0.1, goal: 0.1}
    stop_trajectory_duration: 0.5
    state_publish_rate:  25
    action_monitor_rate: 10
--- a/stretch_gazebo/config/head.yaml
+++ b/stretch_gazebo/config/head.yaml
@ -0,0 +1,15 @@
 stretch_head_controller:
  type: "position_controllers/JointTrajectoryController"
  joints:
    - joint_head_pan
    - joint_head_tilt
  allow_partial_joints_goal: true

  constraints:
    goal_time: 0.6
    stopped_velocity_tolerance: 0.05
    joint_head_pan: {trajectory: 0.1, goal: 0.1}
    joint_head_tilt: {trajectory: 0.1, goal: 0.1}
    stop_trajectory_duration: 0.5
    state_publish_rate:  25
    action_monitor_rate: 10
--- a/stretch_gazebo/config/joints.yaml
+++ b/stretch_gazebo/config/joints.yaml
@ -0,0 +1,2 @@
 type: "joint_state_controller/JointStateController"
 publish_rate: 50
--- a/stretch_gazebo/config/sim.rviz
+++ b/stretch_gazebo/config/sim.rviz
@ -0,0 +1,391 @@
 Panels:
  - Class: rviz/Displays
    Help Height: 78
    Name: Displays
    Property Tree Widget:
      Expanded:
        - /Global Options1
      Splitter Ratio: 0.5
    Tree Height: 755
  - Class: rviz/Selection
    Name: Selection
  - Class: rviz/Tool Properties
    Expanded:
      - /2D Pose Estimate1
      - /2D Nav Goal1
      - /Publish Point1
    Name: Tool Properties
    Splitter Ratio: 0.5886790156364441
  - Class: rviz/Views
    Expanded:
      - /Current View1
    Name: Views
    Splitter Ratio: 0.5
  - Class: rviz/Time
    Experimental: false
    Name: Time
    SyncMode: 0
    SyncSource: LaserScan
 Preferences:
  PromptSaveOnExit: true
 Toolbars:
  toolButtonStyle: 2
 Visualization Manager:
  Class: ""
  Displays:
    - Alpha: 0.5
      Cell Size: 1
      Class: rviz/Grid
      Color: 160; 160; 164
      Enabled: true
      Line Style:
        Line Width: 0.029999999329447746
        Value: Lines
      Name: Grid
      Normal Cell Count: 0
      Offset:
        X: 0
        Y: 0
        Z: 0
      Plane: XY
      Plane Cell Count: 10
      Reference Frame: <Fixed Frame>
      Value: true
    - Alpha: 1
      Class: rviz/RobotModel
      Collision Enabled: false
      Enabled: true
      Links:
        All Links Enabled: true
        Expand Joint Details: false
        Expand Link Details: false
        Expand Tree: false
        Link Tree Style: Links in Alphabetic Order
        base_link:
          Alpha: 1
          Show Axes: false
          Show Trail: false
          Value: true
        camera_accel_frame:
          Alpha: 1
          Show Axes: false
          Show Trail: false
        camera_accel_optical_frame:
          Alpha: 1
          Show Axes: false
          Show Trail: false
        camera_bottom_screw_frame:
          Alpha: 1
          Show Axes: false
          Show Trail: false
        camera_color_frame:
          Alpha: 1
          Show Axes: false
          Show Trail: false
        camera_color_optical_frame:
          Alpha: 1
          Show Axes: false
          Show Trail: false
        camera_depth_frame:
          Alpha: 1
          Show Axes: false
          Show Trail: false
        camera_depth_optical_frame:
          Alpha: 1
          Show Axes: false
          Show Trail: false
        camera_gyro_frame:
          Alpha: 1
          Show Axes: false
          Show Trail: false
        camera_gyro_optical_frame:
          Alpha: 1
          Show Axes: false
          Show Trail: false
        camera_infra1_frame:
          Alpha: 1
          Show Axes: false
          Show Trail: false
        camera_infra1_optical_frame:
          Alpha: 1
          Show Axes: false
          Show Trail: false
        camera_infra2_frame:
          Alpha: 1
          Show Axes: false
          Show Trail: false
        camera_infra2_optical_frame:
          Alpha: 1
          Show Axes: false
          Show Trail: false
        camera_link:
          Alpha: 1
          Show Axes: false
          Show Trail: false
          Value: true
        caster_link:
          Alpha: 1
          Show Axes: false
          Show Trail: false
          Value: true
        laser:
          Alpha: 1
          Show Axes: false
          Show Trail: false
          Value: true
        link_arm_l0:
          Alpha: 1
          Show Axes: false
          Show Trail: false
          Value: true
        link_arm_l1:
          Alpha: 1
          Show Axes: false
          Show Trail: false
          Value: true
        link_arm_l2:
          Alpha: 1
          Show Axes: false
          Show Trail: false
          Value: true
        link_arm_l3:
          Alpha: 1
          Show Axes: false
          Show Trail: false
          Value: true
        link_arm_l4:
          Alpha: 1
          Show Axes: false
          Show Trail: false
          Value: true
        link_aruco_inner_wrist:
          Alpha: 1
          Show Axes: false
          Show Trail: false
          Value: true
        link_aruco_left_base:
          Alpha: 1
          Show Axes: false
          Show Trail: false
          Value: true
        link_aruco_right_base:
          Alpha: 1
          Show Axes: false
          Show Trail: false
          Value: true
        link_aruco_shoulder:
          Alpha: 1
          Show Axes: false
          Show Trail: false
          Value: true
        link_aruco_top_wrist:
          Alpha: 1
          Show Axes: false
          Show Trail: false
          Value: true
        link_grasp_center:
          Alpha: 1
          Show Axes: false
          Show Trail: false
        link_gripper:
          Alpha: 1
          Show Axes: false
          Show Trail: false
          Value: true
        link_gripper_finger_left:
          Alpha: 1
          Show Axes: false
          Show Trail: false
          Value: true
        link_gripper_finger_right:
          Alpha: 1
          Show Axes: false
          Show Trail: false
          Value: true
        link_gripper_fingertip_left:
          Alpha: 1
          Show Axes: false
          Show Trail: false
          Value: true
        link_gripper_fingertip_right:
          Alpha: 1
          Show Axes: false
          Show Trail: false
          Value: true
        link_head:
          Alpha: 1
          Show Axes: false
          Show Trail: false
          Value: true
        link_head_pan:
          Alpha: 1
          Show Axes: false
          Show Trail: false
          Value: true
        link_head_tilt:
          Alpha: 1
          Show Axes: false
          Show Trail: false
          Value: true
        link_left_wheel:
          Alpha: 1
          Show Axes: false
          Show Trail: false
          Value: true
        link_lift:
          Alpha: 1
          Show Axes: false
          Show Trail: false
          Value: true
        link_mast:
          Alpha: 1
          Show Axes: false
          Show Trail: false
          Value: true
        link_right_wheel:
          Alpha: 1
          Show Axes: false
          Show Trail: false
          Value: true
        link_wrist_yaw:
          Alpha: 1
          Show Axes: false
          Show Trail: false
          Value: true
        respeaker_base:
          Alpha: 1
          Show Axes: false
          Show Trail: false
          Value: true
      Name: RobotModel
      Robot Description: robot_description
      TF Prefix: ""
      Update Interval: 0
      Value: true
      Visual Enabled: true
    - Alpha: 1
      Autocompute Intensity Bounds: true
      Autocompute Value Bounds:
        Max Value: 10
        Min Value: -10
        Value: true
      Axis: Z
      Channel Name: intensity
      Class: rviz/LaserScan
      Color: 255; 255; 255
      Color Transformer: Intensity
      Decay Time: 0
      Enabled: true
      Invert Rainbow: false
      Max Color: 255; 255; 255
      Max Intensity: 0
      Min Color: 0; 0; 0
      Min Intensity: 0
      Name: LaserScan
      Position Transformer: XYZ
      Queue Size: 10
      Selectable: true
      Size (Pixels): 3
      Size (m): 0.009999999776482582
      Style: Flat Squares
      Topic: /scan
      Unreliable: false
      Use Fixed Frame: true
      Use rainbow: true
      Value: true
    - Alpha: 1
      Autocompute Intensity Bounds: true
      Autocompute Value Bounds:
        Max Value: 10
        Min Value: -10
        Value: true
      Axis: Z
      Channel Name: intensity
      Class: rviz/PointCloud2
      Color: 255; 255; 255
      Color Transformer: RGB8
      Decay Time: 0
      Enabled: true
      Invert Rainbow: false
      Max Color: 255; 255; 255
      Max Intensity: 4096
      Min Color: 0; 0; 0
      Min Intensity: 0
      Name: PointCloud2
      Position Transformer: XYZ
      Queue Size: 10
      Selectable: true
      Size (Pixels): 3
      Size (m): 0.009999999776482582
      Style: Flat Squares
      Topic: /realsense/depth/color/points
      Unreliable: false
      Use Fixed Frame: true
      Use rainbow: true
      Value: true
  Enabled: true
  Global Options:
    Background Color: 48; 48; 48
    Default Light: true
    Fixed Frame: odom
    Frame Rate: 30
  Name: root
  Tools:
    - Class: rviz/Interact
      Hide Inactive Objects: true
    - Class: rviz/MoveCamera
    - Class: rviz/Select
    - Class: rviz/FocusCamera
    - Class: rviz/Measure
    - Class: rviz/SetInitialPose
      Theta std deviation: 0.2617993950843811
      Topic: /initialpose
      X std deviation: 0.5
      Y std deviation: 0.5
    - Class: rviz/SetGoal
      Topic: /move_base_simple/goal
    - Class: rviz/PublishPoint
      Single click: true
      Topic: /clicked_point
  Value: true
  Views:
    Current:
      Class: rviz/Orbit
      Distance: 3.7518186569213867
      Enable Stereo Rendering:
        Stereo Eye Separation: 0.05999999865889549
        Stereo Focal Distance: 1
        Swap Stereo Eyes: false
        Value: false
      Focal Point:
        X: 0.35893547534942627
        Y: -0.690294623374939
        Z: 0.8657690286636353
      Focal Shape Fixed Size: true
      Focal Shape Size: 0.05000000074505806
      Invert Z Axis: false
      Name: Current View
      Near Clip Distance: 0.009999999776482582
      Pitch: 0.5203983187675476
      Target Frame: <Fixed Frame>
      Value: Orbit (rviz)
      Yaw: 3.6135666370391846
    Saved: ~
 Window Geometry:
  Displays:
    collapsed: false
  Height: 1052
  Hide Left Dock: false
  Hide Right Dock: false
  QMainWindow State: 000000ff00000000fd0000000400000000000001560000037efc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed0000026100000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000037e000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f0000037efc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d0000037e000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007800000003efc0100000002fb0000000800540069006d0065010000000000000780000002eb00fffffffb0000000800540069006d006501000000000000045000000000000000000000050f0000037e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
  Selection:
    collapsed: false
  Time:
    collapsed: false
  Tool Properties:
    collapsed: false
  Views:
    collapsed: false
  Width: 1920
  X: 1920
  Y: 0
--- a/stretch_gazebo/launch/gazebo.launch
+++ b/stretch_gazebo/launch/gazebo.launch
@ -0,0 +1,54 @@
 <launch>

  <arg name="paused" default="false"/>
  <arg name="use_sim_time" default="true"/>
  <arg name="gui" default="true"/>
  <arg name="headless" default="false"/>
  <arg name="debug" default="false"/>
  <arg name="rviz" default="false"/>
  <arg name="model" default="$(find stretch_gazebo)/urdf/stretch_gazebo.urdf.xacro"/>

  <include file="$(find gazebo_ros)/launch/empty_world.launch">
    <arg name="debug" value="$(arg debug)" />
    <arg name="gui" value="$(arg gui)" />
    <arg name="paused" value="$(arg paused)"/>
    <arg name="use_sim_time" value="$(arg use_sim_time)"/>
    <arg name="headless" value="$(arg headless)"/>
    <arg name="verbose" value="true"/>
  </include>

  <param name="robot_description" command="$(find xacro)/xacro $(arg model)" />

  <!-- push robot_description to factory and spawn robot in gazebo -->
  <node name="urdf_spawner" pkg="gazebo_ros" type="spawn_model"
        args=" -urdf -model robot -param robot_description" respawn="false" output="screen" />

  <node pkg="robot_state_publisher" type="robot_state_publisher"  name="robot_state_publisher">
    <param name="publish_frequency" type="double" value="30.0" />
  </node>

  <node name="rviz" pkg="rviz" type="rviz" args="-d $(find stretch_gazebo)/config/sim.rviz" if="$(arg rviz)"/>

  <rosparam command="load"
            file="$(find stretch_gazebo)/config/joints.yaml" 
            ns="stretch_joint_state_controller" />

  <rosparam command="load"
            file="$(find stretch_gazebo)/config/drive_config.yaml"
            ns="stretch_diff_drive_controller" />

  <rosparam command="load"
            file="$(find stretch_gazebo)/config/arm.yaml"/>

  <rosparam command="load"
            file="$(find stretch_gazebo)/config/head.yaml" />

  <rosparam command="load"
            file="$(find stretch_gazebo)/config/gripper.yaml" />

  <node name="stretch_controller_spawner" pkg="controller_manager" type="spawner"
        args="stretch_joint_state_controller stretch_diff_drive_controller stretch_arm_controller stretch_head_controller stretch_gripper_controller"/>

  <node name="publish_ground_truth_odom" pkg="stretch_gazebo" type="publish_ground_truth_odom.py" />

 </launch>
--- a/stretch_gazebo/launch/teleop_joy.launch
+++ b/stretch_gazebo/launch/teleop_joy.launch
@ -0,0 +1,11 @@
 <?xml version="1.0"?>
 <launch>
    <node name="joy" pkg="joy" type="joy_node" output="screen">
        <param name="dev" value="/dev/input/js0"/>
        <param name="autorepeat_rate" value="20" />
    </node>

    <node name="teleop_twist_joy" pkg="teleop_twist_joy" type="teleop_node">
        <remap from="cmd_vel" to="/stretch_diff_drive_controller/cmd_vel"/>
    </node>
 </launch>
--- a/stretch_gazebo/package.xml
+++ b/stretch_gazebo/package.xml
@ -0,0 +1,29 @@
 <?xml version="1.0"?>
 <package format="2">
  <name>stretch_gazebo</name>
  <version>0.0.0</version>
  <description>The stretch_gazebo package</description>

  <maintainer email="davidvlu@gmail.com">David V. Lu!!</maintainer>
  <maintainer email="vatan@picknik.ai">Vatan Aksoy Tezer</maintainer>
  <maintainer email="support@hello-robot.com">Hello Robot Inc.</maintainer>

  <license>Apache License 2.0</license>

  <buildtool_depend>catkin</buildtool_depend>

  <depend>controller_manager</depend>
  <depend>gazebo_msgs</depend>
  <depend>gazebo_plugins</depend>
  <depend>gazebo_ros</depend>
  <depend>nav_msgs</depend>
  <depend>realsense_gazebo_plugin</depend>
  <depend>realsense2_camera</depend>
  <depend>realsense2_description</depend>
  <depend>robot_state_publisher</depend>
  <depend>rospy</depend>
  <depend>rviz</depend>
  <depend>std_msgs</depend>
  <depend>xacro</depend>

 </package>
--- a/stretch_gazebo/scripts/publish_ground_truth_odom.py
+++ b/stretch_gazebo/scripts/publish_ground_truth_odom.py
@ -0,0 +1,29 @@
 #! /usr/bin/env python

 from gazebo_msgs.srv import GetModelState, GetModelStateRequest
 from nav_msgs.msg import Odometry
 from std_msgs.msg import Header
 import rospy

 rospy.init_node('ground_truth_odometry_publisher')
 odom_pub=rospy.Publisher('ground_truth', Odometry, queue_size=10)

 rospy.wait_for_service ('/gazebo/get_model_state')
 get_model_srv = rospy.ServiceProxy('/gazebo/get_model_state', GetModelState)

 odom=Odometry()
 header = Header()
 header.frame_id='/ground_truth'
 model = GetModelStateRequest()
 model.model_name='robot'

 r = rospy.Rate(20)

 while not rospy.is_shutdown():
    result = get_model_srv(model)
    odom.pose.pose = result.pose
    odom.twist.twist = result.twist
    header.stamp = rospy.Time.now()
    odom.header = header
    odom_pub.publish(odom)
    r.sleep()
--- a/stretch_gazebo/urdf/stretch_gazebo.urdf.xacro
+++ b/stretch_gazebo/urdf/stretch_gazebo.urdf.xacro
@ -0,0 +1,560 @@
 <?xml version="1.0"?>
 <robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="stretch_description">
  <xacro:include filename="$(find stretch_description)/urdf/stretch_main.xacro" />
  <xacro:include filename="$(find stretch_description)/urdf/stretch_aruco.xacro" />
  <xacro:include filename="$(find stretch_description)/urdf/stretch_d435i.xacro" />
  <xacro:include filename="$(find stretch_description)/urdf/stretch_laser_range_finder.xacro" />
  <xacro:include filename="$(find stretch_description)/urdf/stretch_respeaker.xacro" />
  <xacro:include filename="$(find stretch_description)/urdf/stretch_gripper.xacro" />

  <gazebo>
    <plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
      <robotNamespace>/</robotNamespace>
    </plugin>
  </gazebo>

  <!-- Base and Drive -->
  <gazebo reference="base_link">
      <material>Gazebo/Gray</material>
  </gazebo>

  <gazebo reference="link_right_wheel">
      <mu1 value="100.0"/>
      <mu2 value="200.0"/>
      <kp value="10000000.0" />
      <kd value="100.0" />
      <material>Gazebo/Blue</material>
      <minDepth>0.001</minDepth>
  </gazebo>

  <gazebo reference="link_left_wheel">
      <mu1 value="100.0"/>
      <mu2 value="200.0"/>
      <kp value="10000000.0" />
      <kd value="100.0" />
      <material>Gazebo/Blue</material>
      <minDepth>0.001</minDepth>
  </gazebo>

  <gazebo reference="caster_link">
    <turnGravityOff>false</turnGravityOff>
    <minDepth>0.001</minDepth>
    <material>Gazebo/Blue</material>
    <mu1>0.0</mu1>
    <mu2>0.0</mu2>
  </gazebo>

  <transmission name="right_wheel_trans" type="SimpleTransmission">
    <type>transmission_interface/SimpleTransmission</type>
    <actuator name="right_wheel_motor">
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
    <joint name="joint_right_wheel">
      <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
    </joint>
 		</transmission>

  <transmission name="left_wheel_trans" type="SimpleTransmission">
    <type>transmission_interface/SimpleTransmission</type>
    <actuator name="left_wheel_motor">
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
    <joint name="joint_left_wheel">
      <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
    </joint>
 		</transmission>

  <!-- Sensors -->

  <!-- Realsense D435i -->
  <gazebo reference="camera_color_frame">
    <sensor name="color" type="camera">
      <pose frame="">0 0 0 0 0 0</pose>
      <camera name="__default__">
        <horizontal_fov>1.5009831567151233</horizontal_fov>
        <image>
          <width>640</width>
          <height>480</height>
          <format>RGB_INT8</format>
        </image>
        <clip>
          <near>0.1</near>
          <far>100</far>
        </clip>
        <noise>
          <type>gaussian</type>
          <mean>0.0</mean>
          <stddev>0.007</stddev>
        </noise>
      </camera>
      <always_on>1</always_on>
      <update_rate>30</update_rate>
      <visualize>0</visualize>
    </sensor>
  </gazebo>
  <gazebo reference="camera_infra1_frame">
    <sensor name="ired1" type="camera">
      <pose frame="">0 0 0 0 0 0</pose>
      <camera name="__default__">
        <horizontal_fov>1.5009831567151233</horizontal_fov>
        <image>
          <width>640</width>
          <height>480</height>
          <format>L_INT8</format>
        </image>
        <clip>
          <near>0.1</near>
          <far>100</far>
        </clip>
        <noise>
          <type>gaussian</type>
          <mean>0.0</mean>
          <stddev>0.007</stddev>
        </noise>
      </camera>
      <always_on>1</always_on>
      <update_rate>30</update_rate>
      <visualize>0</visualize>
    </sensor>
  </gazebo>
  <gazebo reference="camera_infra2_frame">
    <sensor name="ired2" type="camera">
      <pose frame="">0 0 0 0 0 0</pose>
      <camera name="__default__">
        <horizontal_fov>1.5009831567151233</horizontal_fov>
        <image>
          <width>640</width>
          <height>480</height>
          <format>L_INT8</format>
        </image>
        <clip>
          <near>0.1</near>
          <far>100</far>
        </clip>
        <noise>
          <type>gaussian</type>
          <mean>0.0</mean>
          <stddev>0.007</stddev>
        </noise>
      </camera>
      <always_on>1</always_on>
      <update_rate>30</update_rate>
      <visualize>0</visualize>
    </sensor>
  </gazebo>
  <gazebo reference="camera_depth_frame">
    <sensor name="depth" type="depth">
      <pose frame="">0 0 0 0 0 0</pose>
      <camera name="__default__">
        <horizontal_fov>1.5009831567151233</horizontal_fov>
        <image>
          <width>640</width>
          <height>480</height>
        </image>
        <clip>
          <near>0.1</near>
          <far>100</far>
        </clip>
        <noise>
          <type>gaussian</type>
          <mean>0.0</mean>
          <stddev>0.007</stddev>
        </noise>
      </camera>
      <always_on>1</always_on>
      <update_rate>30</update_rate>
      <visualize>0</visualize>
    </sensor>
  </gazebo>
  <gazebo>
      <plugin name="realsense" filename="librealsense_gazebo_plugin.so">
        <depthUpdateRate>30</depthUpdateRate>
        <colorUpdateRate>30</colorUpdateRate>
        <infraredUpdateRate>30</infraredUpdateRate>
        <depthTopicName>depth/image_raw</depthTopicName>
        <depthCameraInfoTopicName>depth/camera_info</depthCameraInfoTopicName>
        <colorTopicName>color/image_raw</colorTopicName>
        <colorCameraInfoTopicName>color/camera_info</colorCameraInfoTopicName>
        <infrared1TopicName>infrared/image_raw</infrared1TopicName>
        <infrared1CameraInfoTopicName>infrared/camera_info</infrared1CameraInfoTopicName>
        <infrared2TopicName>infrared2/image_raw</infrared2TopicName>
        <infrared2CameraInfoTopicName>infrared2/camera_info</infrared2CameraInfoTopicName>
        <colorOpticalframeName>camera_color_optical_frame</colorOpticalframeName>
        <depthOpticalframeName>camera_depth_optical_frame</depthOpticalframeName>
        <infrared1OpticalframeName>camera_left_ir_optical_frame</infrared1OpticalframeName>
        <infrared2OpticalframeName>camera_right_ir_optical_frame</infrared2OpticalframeName>
        <rangeMinDepth>0.1</rangeMinDepth>
        <rangeMaxDepth>10</rangeMaxDepth>
        <pointCloud>1</pointCloud>
        <pointCloudTopicName>depth/color/points</pointCloudTopicName>
        <pointCloudCutoff>0.15</pointCloudCutoff>
        <pointCloudCutoffMax>10</pointCloudCutoffMax>
      </plugin>
  </gazebo>

  <gazebo reference="camera_gyro_frame">
    <gravity>true</gravity>
    <sensor name="imu_sensor" type="imu">
      <always_on>true</always_on>
      <visualize>false</visualize>
      <plugin name="imu_plugin" filename="libgazebo_ros_imu_sensor.so">
        <topicName>realsense/imu/data</topicName>
        <bodyName>camera_gyro_frame</bodyName>
        <updateRateHZ>50.0</updateRateHZ>
        <gaussianNoise>0.001</gaussianNoise>
        <xyzOffset>0 0 0</xyzOffset>
        <rpyOffset>0 0 0</rpyOffset>
        <frameName>camera_gyro_frame</frameName>
        <initialOrientationAsReference>false</initialOrientationAsReference>
      </plugin>
      <pose>0 0 0 0 0 0</pose>
    </sensor>
  </gazebo>

  <!-- Aruco -->
  <gazebo reference="link_aruco_right_base">
      <material>Gazebo/Black</material>
  </gazebo>

  <gazebo reference="link_aruco_left_base">
      <material>Gazebo/Black</material>
  </gazebo>

  <gazebo reference="link_aruco_shoulder">
      <material>Gazebo/Black</material>
  </gazebo>

  <gazebo reference="link_aruco_top_wrist">
      <material>Gazebo/Black</material>
  </gazebo>

  <gazebo reference="link_aruco_inner_wrist">
      <material>Gazebo/Black</material>
  </gazebo>

  <!-- Respeaker -->
  <gazebo reference="respeaker_base">
      <material>Gazebo/Green</material>
  </gazebo>

  <!-- LIDAR -->
  <gazebo reference="laser">
    <material>Gazebo/Black</material>
    <sensor type="gpu_ray" name="laser_sensor">
      <pose>0 0 0 0 0 0</pose>
      <visualize>false</visualize>
      <update_rate>5.5</update_rate>
      <ray>
        <scan>
          <horizontal>
            <samples>2000</samples>
            <resolution>1</resolution>
            <min_angle>${-M_PI}</min_angle>
            <max_angle>${M_PI}</max_angle>
          </horizontal>
        </scan>
        <range>
          <min>0.08</min>
          <max>12.0</max>
          <resolution>0.01</resolution>
        </range>
        <noise>
          <type>gaussian</type>
          <mean>0.0</mean>
          <stddev>0.001</stddev>
        </noise>
      </ray>
      <plugin name="gazebo_ros_lidar_controller" filename="libgazebo_ros_gpu_laser.so">
        <topicName>scan</topicName>
        <frameName>laser</frameName>
      </plugin>
    </sensor>
  </gazebo>

  <!-- Base IMU -->
  <gazebo reference="base_link">
    <gravity>true</gravity>
    <sensor name="base_imu" type="imu">
      <always_on>true</always_on>
      <visualize>false</visualize>
      <plugin name="imu_plugin" filename="libgazebo_ros_imu_sensor.so">
        <topicName>imu/data</topicName>
        <bodyName>base_link</bodyName>
        <updateRateHZ>25.0</updateRateHZ>
        <gaussianNoise>0.001</gaussianNoise>
        <xyzOffset>0 0 0</xyzOffset>
        <rpyOffset>0 0 0</rpyOffset>
        <frameName>base_link</frameName>
        <initialOrientationAsReference>false</initialOrientationAsReference>
      </plugin>
      <pose>0 0 0 0 0 0</pose>
    </sensor>
  </gazebo>

  <!-- Wrist IMU -->
  <gazebo reference="link_wrist_yaw">
    <gravity>true</gravity>
    <sensor name="wrist_imu" type="imu">
      <always_on>true</always_on>
      <visualize>false</visualize>
      <plugin name="imu_plugin" filename="libgazebo_ros_imu_sensor.so">
        <topicName>wrist_imu/data</topicName>
        <bodyName>link_wrist_yaw</bodyName>
        <updateRateHZ>25.0</updateRateHZ>
        <gaussianNoise>0.001</gaussianNoise>
        <xyzOffset>0 0 0</xyzOffset>
        <rpyOffset>0 0 0</rpyOffset>
        <frameName>link_wrist_yaw</frameName>
        <initialOrientationAsReference>false</initialOrientationAsReference>
      </plugin>
      <pose>0 0 0 0 0 0</pose>
    </sensor>
  </gazebo>

  <!-- Lift -->
  <gazebo reference="link_lift">
      <mu1 value="10000.0"/>
      <mu2 value="10000.0"/>
      <kp value="10000000.0" />
      <kd value="100.0" />
      <material>Gazebo/Black</material>
      <minDepth>0.001</minDepth>
  </gazebo>

  <gazebo reference="link_mast">
      <material>Gazebo/Gray</material>
  </gazebo>

  <transmission name="trans_lift">
    <type>transmission_interface/SimpleTransmission</type>
    <actuator name="motor_lift">
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
    <joint name="joint_lift">
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
    </joint>
  </transmission>

  <!-- Arm -->
  <gazebo reference="link_arm_l0">
      <mu1 value="100.0"/>
      <mu2 value="200.0"/>
      <kp value="10000000.0" />
      <kd value="100.0" />
      <material>Gazebo/Gray</material>
      <minDepth>0.001</minDepth>
  </gazebo>

  <gazebo reference="link_arm_l1">
      <mu1 value="100.0"/>
      <mu2 value="200.0"/>
      <kp value="10000000.0" />
      <kd value="100.0" />
      <material>Gazebo/Gray</material>
      <minDepth>0.001</minDepth>
  </gazebo>

  <gazebo reference="link_arm_l2">
      <mu1 value="100.0"/>
      <mu2 value="200.0"/>
      <kp value="10000000.0" />
      <kd value="100.0" />
      <material>Gazebo/Gray</material>
      <minDepth>0.001</minDepth>
  </gazebo>

  <gazebo reference="link_arm_l3">
      <mu1 value="100.0"/>
      <mu2 value="200.0"/>
      <kp value="10000000.0" />
      <kd value="100.0" />
      <material>Gazebo/Gray</material>
      <minDepth>0.001</minDepth>
  </gazebo>

  <gazebo reference="link_arm_l4">
      <mu1 value="100.0"/>
      <mu2 value="200.0"/>
      <kp value="10000000.0" />
      <kd value="100.0" />
      <material>Gazebo/Gray</material>
      <minDepth>0.001</minDepth>
  </gazebo>

  <transmission name="trans_arm_l0">
    <type>transmission_interface/SimpleTransmission</type>
    <actuator name="motor_arm_l0" >
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
    <joint name="joint_arm_l0">
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
    </joint>
  </transmission>
  <transmission name="trans_arm_l1">
    <type>transmission_interface/SimpleTransmission</type>
    <actuator name="motor_arm_l1" >
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
    <joint name="joint_arm_l1">
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
    </joint>
  </transmission>
  <transmission name="trans_arm_l2">
    <type>transmission_interface/SimpleTransmission</type>
    <actuator name="motor_arm_l2" >
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
    <joint name="joint_arm_l2">
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
    </joint>
  </transmission>
  <transmission name="trans_arm_l3">
    <type>transmission_interface/SimpleTransmission</type>
    <actuator name="motor_arm_l3" >
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
    <joint name="joint_arm_l3">
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
    </joint>
  </transmission>

  <!-- Wrist -->
  <gazebo reference="link_wrist_yaw">
      <mu1 value="100.0"/>
      <mu2 value="200.0"/>
      <kp value="10000000.0" />
      <kd value="100.0" />
      <material>Gazebo/Black</material>
      <minDepth>0.001</minDepth>
  </gazebo>

  <transmission name="trans_wrist_yaw">
    <type>transmission_interface/SimpleTransmission</type>
    <actuator name="motor_wrist_yaw" >
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
    <joint name="joint_wrist_yaw">
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
    </joint>
  </transmission>

  <!-- Head -->
  <gazebo reference="link_head">
      <mu1 value="100.0"/>
      <mu2 value="200.0"/>
      <material>Gazebo/Gray</material>
  </gazebo>

  <gazebo reference="link_head_tilt">
      <mu1 value="100.0"/>
      <mu2 value="200.0"/>
      <kp value="10000000.0" />
      <kd value="100.0" />
      <material>Gazebo/Gray</material>
      <minDepth>0.001</minDepth>
  </gazebo>

  <gazebo reference="link_head_pan">
      <mu1 value="100.0"/>
      <mu2 value="200.0"/>
      <kp value="10000000.0" />
      <kd value="100.0" />
      <material>Gazebo/Gray</material>
      <minDepth>0.001</minDepth>
  </gazebo>

  <transmission name="trans_head_pan">
    <type>transmission_interface/SimpleTransmission</type>
    <actuator name="motor_head_pan">
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
    <joint name="joint_head_pan">
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
    </joint>
  </transmission>

  <transmission name="trans_head_tilt">
    <type>transmission_interface/SimpleTransmission</type>
    <actuator name="motor_head_tilt">
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
    <joint name="joint_head_tilt">
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
    </joint>
  </transmission>

  <!-- Gripper -->
  <gazebo reference="link_gripper_finger_left">
      <mu1 value="100.0"/>
      <mu2 value="200.0"/>
      <kp value="10000000.0" />
      <kd value="100.0" />
      <material>Gazebo/Gray</material>
      <minDepth>0.001</minDepth>
  </gazebo>

  <gazebo reference="link_gripper_finger_right">
      <mu1 value="100.0"/>
      <mu2 value="200.0"/>
      <kp value="10000000.0" />
      <kd value="100.0" />
      <material>Gazebo/Gray</material>
      <minDepth>0.001</minDepth>
  </gazebo>

  <gazebo reference="link_grasp_center">
      <mu1 value="100.0"/>
      <mu2 value="200.0"/>
      <kp value="10000000.0" />
      <kd value="100.0" />
      <material>Gazebo/Gray</material>
      <minDepth>0.001</minDepth>
  </gazebo>

  <gazebo reference="link_gripper_fingertip_left">
      <mu1 value="100.0"/>
      <mu2 value="200.0"/>
      <kp value="10000000.0" />
      <kd value="100.0" />
      <material>Gazebo/Black</material>
      <minDepth>0.001</minDepth>
  </gazebo>

  <gazebo reference="link_gripper_fingertip_right">
      <mu1 value="100.0"/>
      <mu2 value="200.0"/>
      <kp value="10000000.0" />
      <kd value="100.0" />
      <material>Gazebo/Black</material>
      <minDepth>0.001</minDepth>
  </gazebo>
  
  <gazebo reference="link_gripper">
      <mu1 value="100.0"/>
      <mu2 value="200.0"/>
      <kp value="10000000.0" />
      <kd value="100.0" />
      <material>Gazebo/Gray</material>
      <minDepth>0.001</minDepth>
  </gazebo>

  <transmission name="trans_gripper_finger_left">
    <type>transmission_interface/SimpleTransmission</type>
    <actuator name="motor_gripper_finger_left">
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
    <joint name="joint_gripper_finger_left">
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
    </joint>
  </transmission>
  <transmission name="trans_gripper_finger_right">
    <type>transmission_interface/SimpleTransmission</type>
    <actuator name="motor_gripper_finger_right">
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
    <joint name="joint_gripper_finger_right">
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
    </joint>
 </transmission>

 </robot>
--- a/stretch_moveit_config/.setup_assistant
+++ b/stretch_moveit_config/.setup_assistant
@ -0,0 +1,11 @@
 moveit_setup_assistant_config:
  URDF:
    package: stretch_description
    relative_path: urdf/stretch_description.xacro
    xacro_args: ""
  SRDF:
    relative_path: config/stretch_description.srdf
  CONFIG:
    author_name: David V. Lu!!
    author_email: davidvlu@gmail.com
    generated_timestamp: 1602791794
--- a/stretch_moveit_config/CMakeLists.txt
+++ b/stretch_moveit_config/CMakeLists.txt
@ -0,0 +1,10 @@
 cmake_minimum_required(VERSION 3.1.3)
 project(stretch_moveit_config)

 find_package(catkin REQUIRED)

 catkin_package()

 install(DIRECTORY launch DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
  PATTERN "setup_assistant.launch" EXCLUDE)
 install(DIRECTORY config DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
--- a/stretch_moveit_config/README.md
+++ b/stretch_moveit_config/README.md
@ -0,0 +1,23 @@
 ![](../images/HelloRobotLogoBar.png)

 ## Stretch & MoveIt!
 MoveIt is the standard ROS manipulation platform, and this package is the configuration for working with Stretch with the MoveIt framework.


 ### Offline Demo

 To experiment with the planning capabilities of MoveIt on Stretch, you can run a demo _without_ Stretch hardware.

    roslaunch stretch_moveit_config demo.launch

 This will allow you to move the robot around using interactive markers and create plans between poses.

 ### Hardware Integration

 To run MoveIt with the actual hardware, (assuming `stretch_driver` is already running) simply run

    roslaunch stretch_moveit_config move_group.launch

 This will runs all of the planning capabilities, but without the setup, simulation and interface that the above demo provides. In order to create plans for the robot with the same interface as the offline demo, you can run

    roslaunch stretch_moveit_config moveit_rviz.launch
--- a/stretch_moveit_config/config/chomp_planning.yaml
+++ b/stretch_moveit_config/config/chomp_planning.yaml
@ -0,0 +1,18 @@
 planning_time_limit: 10.0
 max_iterations: 200
 max_iterations_after_collision_free: 5
 smoothness_cost_weight: 0.1
 obstacle_cost_weight: 1.0
 learning_rate: 0.01
 smoothness_cost_velocity: 0.0
 smoothness_cost_acceleration: 1.0
 smoothness_cost_jerk: 0.0
 ridge_factor: 0.01
 use_pseudo_inverse: false
 pseudo_inverse_ridge_factor: 1e-4
 joint_update_limit: 0.1
 collision_clearence: 0.2
 collision_threshold: 0.07
 use_stochastic_descent: true
 enable_failure_recovery: true
 max_recovery_attempts: 5
--- a/stretch_moveit_config/config/fake_controllers.yaml
+++ b/stretch_moveit_config/config/fake_controllers.yaml
@ -0,0 +1,16 @@
 controller_list:
  - name: fake_stretch_arm_controller
    joints:
      - joint_lift
      - joint_arm_l3
      - joint_arm_l2
      - joint_arm_l1
      - joint_arm_l0
      - joint_wrist_yaw
  - name: fake_gripper_controller
    joints:
      - joint_gripper_finger_left
      - joint_gripper_finger_right
 initial:  # Define initial robot poses.
  - group: stretch_arm
    pose: home
--- a/stretch_moveit_config/config/joint_limits.yaml
+++ b/stretch_moveit_config/config/joint_limits.yaml
@ -0,0 +1,50 @@
 # joint_limits.yaml allows the dynamics properties specified in the URDF to be overwritten or augmented as needed

 # For beginners, we downscale velocity and acceleration limits.
 # You can always specify higher scaling factors (<= 1.0) in your motion requests.  # Increase the values below to 1.0 to always move at maximum speed.
 default_velocity_scaling_factor: 0.1
 default_acceleration_scaling_factor: 0.1

 # Specific joint properties can be changed with the keys [max_position, min_position, max_velocity, max_acceleration]
 # Joint limits can be turned off with [has_velocity_limits, has_acceleration_limits]
 joint_limits:
  joint_arm_l0:
    has_velocity_limits: true
    max_velocity: 1
    has_acceleration_limits: false
    max_acceleration: 0
  joint_arm_l1:
    has_velocity_limits: true
    max_velocity: 1
    has_acceleration_limits: false
    max_acceleration: 0
  joint_arm_l2:
    has_velocity_limits: true
    max_velocity: 1
    has_acceleration_limits: false
    max_acceleration: 0
  joint_arm_l3:
    has_velocity_limits: true
    max_velocity: 1
    has_acceleration_limits: false
    max_acceleration: 0
  joint_gripper_finger_left:
    has_velocity_limits: true
    max_velocity: 1
    has_acceleration_limits: false
    max_acceleration: 0
  joint_gripper_finger_right:
    has_velocity_limits: true
    max_velocity: 1
    has_acceleration_limits: false
    max_acceleration: 0
  joint_lift:
    has_velocity_limits: true
    max_velocity: 1
    has_acceleration_limits: false
    max_acceleration: 0
  joint_wrist_yaw:
    has_velocity_limits: true
    max_velocity: 1
    has_acceleration_limits: false
    max_acceleration: 0
--- a/stretch_moveit_config/config/kinematics.yaml
+++ b/stretch_moveit_config/config/kinematics.yaml
@ -0,0 +1,8 @@
 stretch_arm:
  kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin
  kinematics_solver_search_resolution: 0.005
  kinematics_solver_timeout: 0.005
 stretch_head:
  kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin
  kinematics_solver_search_resolution: 0.005
  kinematics_solver_timeout: 0.1
--- a/stretch_moveit_config/config/ompl_planning.yaml
+++ b/stretch_moveit_config/config/ompl_planning.yaml
@ -0,0 +1,210 @@
 planner_configs:
  AnytimePathShortening:
    type: geometric::AnytimePathShortening
    shortcut: true  # Attempt to shortcut all new solution paths
    hybridize: true  # Compute hybrid solution trajectories
    max_hybrid_paths: 24  # Number of hybrid paths generated per iteration
    num_planners: 4  # The number of default planners to use for planning
    planners: ""  # A comma-separated list of planner types (e.g., "PRM,EST,RRTConnect"Optionally, planner parameters can be passed to change the default:"PRM[max_nearest_neighbors=5],EST[goal_bias=.5],RRT[range=10. goal_bias=.1]"
  SBL:
    type: geometric::SBL
    range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
  EST:
    type: geometric::EST
    range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0 setup()
    goal_bias: 0.05  # When close to goal select goal, with this probability. default: 0.05
  LBKPIECE:
    type: geometric::LBKPIECE
    range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
    border_fraction: 0.9  # Fraction of time focused on boarder default: 0.9
    min_valid_path_fraction: 0.5  # Accept partially valid moves above fraction. default: 0.5
  BKPIECE:
    type: geometric::BKPIECE
    range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
    border_fraction: 0.9  # Fraction of time focused on boarder default: 0.9
    failed_expansion_score_factor: 0.5  # When extending motion fails, scale score by factor. default: 0.5
    min_valid_path_fraction: 0.5  # Accept partially valid moves above fraction. default: 0.5
  KPIECE:
    type: geometric::KPIECE
    range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
    goal_bias: 0.05  # When close to goal select goal, with this probability. default: 0.05
    border_fraction: 0.9  # Fraction of time focused on boarder default: 0.9 (0.0,1.]
    failed_expansion_score_factor: 0.5  # When extending motion fails, scale score by factor. default: 0.5
    min_valid_path_fraction: 0.5  # Accept partially valid moves above fraction. default: 0.5
  RRT:
    type: geometric::RRT
    range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
    goal_bias: 0.05  # When close to goal select goal, with this probability? default: 0.05
  RRTConnect:
    type: geometric::RRTConnect
    range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
  RRTstar:
    type: geometric::RRTstar
    range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
    goal_bias: 0.05  # When close to goal select goal, with this probability? default: 0.05
    delay_collision_checking: 1  # Stop collision checking as soon as C-free parent found. default 1
  TRRT:
    type: geometric::TRRT
    range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
    goal_bias: 0.05  # When close to goal select goal, with this probability? default: 0.05
    max_states_failed: 10  # when to start increasing temp. default: 10
    temp_change_factor: 2.0  # how much to increase or decrease temp. default: 2.0
    min_temperature: 10e-10  # lower limit of temp change. default: 10e-10
    init_temperature: 10e-6  # initial temperature. default: 10e-6
    frountier_threshold: 0.0  # dist new state to nearest neighbor to disqualify as frontier. default: 0.0 set in setup()
    frountierNodeRatio: 0.1  # 1/10, or 1 nonfrontier for every 10 frontier. default: 0.1
    k_constant: 0.0  # value used to normalize expresssion. default: 0.0 set in setup()
  PRM:
    type: geometric::PRM
    max_nearest_neighbors: 10  # use k nearest neighbors. default: 10
  PRMstar:
    type: geometric::PRMstar
  FMT:
    type: geometric::FMT
    num_samples: 1000  # number of states that the planner should sample. default: 1000
    radius_multiplier: 1.1  # multiplier used for the nearest neighbors search radius. default: 1.1
    nearest_k: 1  # use Knearest strategy. default: 1
    cache_cc: 1  # use collision checking cache. default: 1
    heuristics: 0  # activate cost to go heuristics. default: 0
    extended_fmt: 1  # activate the extended FMT*: adding new samples if planner does not finish successfully. default: 1
  BFMT:
    type: geometric::BFMT
    num_samples: 1000  # number of states that the planner should sample. default: 1000
    radius_multiplier: 1.0  # multiplier used for the nearest neighbors search radius. default: 1.0
    nearest_k: 1  # use the Knearest strategy. default: 1
    balanced: 0  # exploration strategy: balanced true expands one tree every iteration. False will select the tree with lowest maximum cost to go. default: 1
    optimality: 1  # termination strategy: optimality true finishes when the best possible path is found. Otherwise, the algorithm will finish when the first feasible path is found. default: 1
    heuristics: 1  # activates cost to go heuristics. default: 1
    cache_cc: 1  # use the collision checking cache. default: 1
    extended_fmt: 1  # Activates the extended FMT*: adding new samples if planner does not finish successfully. default: 1
  PDST:
    type: geometric::PDST
  STRIDE:
    type: geometric::STRIDE
    range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
    goal_bias: 0.05  # When close to goal select goal, with this probability. default: 0.05
    use_projected_distance: 0  # whether nearest neighbors are computed based on distances in a projection of the state rather distances in the state space itself. default: 0
    degree: 16  # desired degree of a node in the Geometric Near-neightbor Access Tree (GNAT). default: 16
    max_degree: 18  # max degree of a node in the GNAT. default: 12
    min_degree: 12  # min degree of a node in the GNAT. default: 12
    max_pts_per_leaf: 6  # max points per leaf in the GNAT. default: 6
    estimated_dimension: 0.0  # estimated dimension of the free space. default: 0.0
    min_valid_path_fraction: 0.2  # Accept partially valid moves above fraction. default: 0.2
  BiTRRT:
    type: geometric::BiTRRT
    range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
    temp_change_factor: 0.1  # how much to increase or decrease temp. default: 0.1
    init_temperature: 100  # initial temperature. default: 100
    frountier_threshold: 0.0  # dist new state to nearest neighbor to disqualify as frontier. default: 0.0 set in setup()
    frountier_node_ratio: 0.1  # 1/10, or 1 nonfrontier for every 10 frontier. default: 0.1
    cost_threshold: 1e300  # the cost threshold. Any motion cost that is not better will not be expanded. default: inf
  LBTRRT:
    type: geometric::LBTRRT
    range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
    goal_bias: 0.05  # When close to goal select goal, with this probability. default: 0.05
    epsilon: 0.4  # optimality approximation factor. default: 0.4
  BiEST:
    type: geometric::BiEST
    range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
  ProjEST:
    type: geometric::ProjEST
    range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
    goal_bias: 0.05  # When close to goal select goal, with this probability. default: 0.05
  LazyPRM:
    type: geometric::LazyPRM
    range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
  LazyPRMstar:
    type: geometric::LazyPRMstar
  SPARS:
    type: geometric::SPARS
    stretch_factor: 3.0  # roadmap spanner stretch factor. multiplicative upper bound on path quality. It does not make sense to make this parameter more than 3. default: 3.0
    sparse_delta_fraction: 0.25  # delta fraction for connection distance. This value represents the visibility range of sparse samples. default: 0.25
    dense_delta_fraction: 0.001  # delta fraction for interface detection. default: 0.001
    max_failures: 1000  # maximum consecutive failure limit. default: 1000
  SPARStwo:
    type: geometric::SPARStwo
    stretch_factor: 3.0  # roadmap spanner stretch factor. multiplicative upper bound on path quality. It does not make sense to make this parameter more than 3. default: 3.0
    sparse_delta_fraction: 0.25  # delta fraction for connection distance. This value represents the visibility range of sparse samples. default: 0.25
    dense_delta_fraction: 0.001  # delta fraction for interface detection. default: 0.001
    max_failures: 5000  # maximum consecutive failure limit. default: 5000
 stretch_arm:
  default_planner_config: None
  planner_configs:
    - AnytimePathShortening
    - SBL
    - EST
    - LBKPIECE
    - BKPIECE
    - KPIECE
    - RRT
    - RRTConnect
    - RRTstar
    - TRRT
    - PRM
    - PRMstar
    - FMT
    - BFMT
    - PDST
    - STRIDE
    - BiTRRT
    - LBTRRT
    - BiEST
    - ProjEST
    - LazyPRM
    - LazyPRMstar
    - SPARS
    - SPARStwo
 stretch_gripper:
  default_planner_config: ""
  planner_configs:
    - AnytimePathShortening
    - SBL
    - EST
    - LBKPIECE
    - BKPIECE
    - KPIECE
    - RRT
    - RRTConnect
    - RRTstar
    - TRRT
    - PRM
    - PRMstar
    - FMT
    - BFMT
    - PDST
    - STRIDE
    - BiTRRT
    - LBTRRT
    - BiEST
    - ProjEST
    - LazyPRM
    - LazyPRMstar
    - SPARS
    - SPARStwo
 stretch_head:
  default_planner_config: RRTConnect
  planner_configs:
    - AnytimePathShortening
    - SBL
    - EST
    - LBKPIECE
    - BKPIECE
    - KPIECE
    - RRT
    - RRTConnect
    - RRTstar
    - TRRT
    - PRM
    - PRMstar
    - FMT
    - BFMT
    - PDST
    - STRIDE
    - BiTRRT
    - LBTRRT
    - BiEST
    - ProjEST
    - LazyPRM
    - LazyPRMstar
    - SPARS
    - SPARStwo
--- a/stretch_moveit_config/config/ros_controllers.yaml
+++ b/stretch_moveit_config/config/ros_controllers.yaml
@ -0,0 +1,31 @@
 # Publish all joint states
 # Creates the /joint_states topic necessary in ROS
 joint_state_controller:
  type: joint_state_controller/JointStateController
  publish_rate: 50
 controller_list:
  - name: stretch_arm_controller
    action_ns: follow_joint_trajectory
    default: True
    type: FollowJointTrajectory
    joints:
      - joint_lift
      - joint_arm_l3
      - joint_arm_l2
      - joint_arm_l1
      - joint_arm_l0
      - joint_wrist_yaw
  - name: stretch_head_controller
    action_ns: follow_joint_trajectory
    default: True
    type: FollowJointTrajectory
    joints:
      - joint_head_pan
      - joint_head_tilt
  - name: stretch_gripper_controller
    action_ns: follow_joint_trajectory
    default: True
    type: FollowJointTrajectory
    joints:
      - joint_gripper_finger_left
      - joint_gripper_finger_right
--- a/stretch_moveit_config/config/sensors_3d.yaml
+++ b/stretch_moveit_config/config/sensors_3d.yaml
@ -0,0 +1,10 @@
 # The name of this file shouldn't be changed, or else the Setup Assistant won't detect it
 sensors:
  - filtered_cloud_topic: filtered_cloud
    max_range: 5.0
    max_update_rate: 1.0
    padding_offset: 0.1
    padding_scale: 1.0
    point_cloud_topic: /head_mount_kinect/depth_registered/points
    point_subsample: 1
    sensor_plugin: occupancy_map_monitor/PointCloudOctomapUpdater
--- a/stretch_moveit_config/config/stretch_description.srdf
+++ b/stretch_moveit_config/config/stretch_description.srdf
@ -0,0 +1,408 @@
 <?xml version="1.0" encoding="UTF-8"?>
 <!--This does not replace URDF, and is not an extension of URDF.
    This is a format for representing semantic information about the robot structure.
    A URDF file must exist for this robot as well, where the joints and the links that are referenced are defined
 -->
 <robot name="stretch_description">
    <!--GROUPS: Representation of a set of joints and links. This can be useful for specifying DOF to plan for, defining arms, end effectors, etc-->
    <!--LINKS: When a link is specified, the parent joint of that link (if it exists) is automatically included-->
    <!--JOINTS: When a joint is specified, the child link of that joint (which will always exist) is automatically included-->
    <!--CHAINS: When a chain is specified, all the links along the chain (including endpoints) are included in the group. Additionally, all the joints that are parents to included links are also included. This means that joints along the chain and the parent joint of the base link are included in the group-->
    <!--SUBGROUPS: Groups can also be formed by referencing to already defined group names-->
    <group name="stretch_arm">
        <joint name="joint_lift"/>
        <joint name="joint_arm_l4"/>
        <joint name="joint_arm_l3"/>
        <joint name="joint_arm_l2"/>
        <joint name="joint_arm_l1"/>
        <joint name="joint_arm_l0"/>
        <joint name="joint_wrist_yaw"/>
    </group>
    <group name="stretch_gripper">
        <link name="link_gripper"/>
        <link name="link_grasp_center"/>
        <link name="link_gripper_finger_left"/>
        <link name="link_gripper_finger_right"/>
        <link name="link_gripper_fingertip_left"/>
        <link name="link_gripper_fingertip_right"/>
    </group>
    <group name="stretch_head">
        <joint name="joint_head_pan" />
        <joint name="joint_head_tilt" />
        <link name="camera_link"/>
    </group>
    <!--GROUP STATES: Purpose: Define a named state for a particular group, in terms of joint values. This is useful to define states like 'folded arms'-->
    <group_state name="home" group="stretch_arm">
        <joint name="joint_arm_l0" value="0"/>
        <joint name="joint_arm_l1" value="0"/>
        <joint name="joint_arm_l2" value="0"/>
        <joint name="joint_arm_l3" value="0"/>
        <joint name="joint_lift" value="0"/>
        <joint name="joint_wrist_yaw" value="0"/>
    </group_state>
    <group_state name="extended" group="stretch_arm">
        <joint name="joint_arm_l0" value="0.13"/>
        <joint name="joint_arm_l1" value="0.13"/>
        <joint name="joint_arm_l2" value="0.13"/>
        <joint name="joint_arm_l3" value="0.13"/>
        <joint name="joint_lift" value="0.9967"/>
        <joint name="joint_wrist_yaw" value="4"/>
    </group_state>
    <group_state name="home" group="stretch_head">
        <joint name="joint_head_pan" value="0.0"/>
        <joint name="joint_head_tilt" value="0.0"/>
    </group_state>
    <group_state name="low_head" group="stretch_head">
        <joint name="joint_head_pan" value="-3.5"/>
        <joint name="joint_head_tilt" value="-1.2"/>
    </group_state>
    <group_state name="high_head" group="stretch_head">
        <joint name="joint_head_pan" value="1.0"/>
        <joint name="joint_head_tilt" value="0.5"/>
    </group_state>
    <group_state name="open" group="stretch_gripper">
        <joint name="joint_gripper_finger_left" value="0.3"/>
        <joint name="joint_gripper_finger_right" value="0.3"/>
    </group_state>
    <group_state name="closed" group="stretch_gripper">
        <joint name="joint_gripper_finger_left" value="0.0"/>
        <joint name="joint_gripper_finger_right" value="0.0"/>
    </group_state>
    <!--END EFFECTOR: Purpose: Represent information about an end effector.-->
    <end_effector name="gripper" parent_link="link_grasp_center" group="stretch_arm"/>
    <!--VIRTUAL JOINT: Purpose: this element defines a virtual joint between a robot link and an external frame of reference (considered fixed with respect to the robot)-->
    <!--virtual_joint name="position" type="floating" parent_frame="map" child_link="base_link"/-->
    <!--DISABLE COLLISIONS: By default it is assumed that any link of the robot could potentially come into collision with any other link in the robot. This tag disables collision checking between a specified pair of links. -->
    <disable_collisions link1="base_link" link2="camera_link" reason="Never"/>
    <disable_collisions link1="base_link" link2="laser" reason="Adjacent"/>
    <disable_collisions link1="base_link" link2="link_arm_l0" reason="Never"/>
    <disable_collisions link1="base_link" link2="link_arm_l1" reason="Never"/>
    <disable_collisions link1="base_link" link2="link_arm_l2" reason="Never"/>
    <disable_collisions link1="base_link" link2="link_arm_l3" reason="Never"/>
    <disable_collisions link1="base_link" link2="link_arm_l4" reason="Never"/>
    <disable_collisions link1="base_link" link2="link_aruco_inner_wrist" reason="Never"/>
    <disable_collisions link1="base_link" link2="link_aruco_left_base" reason="Adjacent"/>
    <disable_collisions link1="base_link" link2="link_aruco_right_base" reason="Adjacent"/>
    <disable_collisions link1="base_link" link2="link_aruco_shoulder" reason="Never"/>
    <disable_collisions link1="base_link" link2="link_aruco_top_wrist" reason="Never"/>
    <disable_collisions link1="base_link" link2="link_head" reason="Never"/>
    <disable_collisions link1="base_link" link2="link_head_pan" reason="Never"/>
    <disable_collisions link1="base_link" link2="link_head_tilt" reason="Never"/>
    <disable_collisions link1="base_link" link2="link_left_wheel" reason="Adjacent"/>
    <disable_collisions link1="base_link" link2="link_lift" reason="Never"/>
    <disable_collisions link1="base_link" link2="link_mast" reason="Adjacent"/>
    <disable_collisions link1="base_link" link2="link_right_wheel" reason="Adjacent"/>
    <disable_collisions link1="base_link" link2="link_wrist_yaw" reason="Never"/>
    <disable_collisions link1="base_link" link2="respeaker_base" reason="Never"/>
    <disable_collisions link1="camera_link" link2="laser" reason="Never"/>
    <disable_collisions link1="camera_link" link2="link_arm_l0" reason="Never"/>
    <disable_collisions link1="camera_link" link2="link_arm_l1" reason="Never"/>
    <disable_collisions link1="camera_link" link2="link_arm_l2" reason="Never"/>
    <disable_collisions link1="camera_link" link2="link_arm_l3" reason="Never"/>
    <disable_collisions link1="camera_link" link2="link_arm_l4" reason="Never"/>
    <disable_collisions link1="camera_link" link2="link_aruco_inner_wrist" reason="Never"/>
    <disable_collisions link1="camera_link" link2="link_aruco_left_base" reason="Never"/>
    <disable_collisions link1="camera_link" link2="link_aruco_right_base" reason="Never"/>
    <disable_collisions link1="camera_link" link2="link_aruco_shoulder" reason="Never"/>
    <disable_collisions link1="camera_link" link2="link_aruco_top_wrist" reason="Never"/>
    <disable_collisions link1="camera_link" link2="link_gripper" reason="Never"/>
    <disable_collisions link1="camera_link" link2="link_gripper_finger_left" reason="Never"/>
    <disable_collisions link1="camera_link" link2="link_gripper_finger_right" reason="Never"/>
    <disable_collisions link1="camera_link" link2="link_gripper_fingertip_left" reason="Never"/>
    <disable_collisions link1="camera_link" link2="link_gripper_fingertip_right" reason="Never"/>
    <disable_collisions link1="camera_link" link2="link_head_tilt" reason="Adjacent"/>
    <disable_collisions link1="camera_link" link2="link_left_wheel" reason="Never"/>
    <disable_collisions link1="camera_link" link2="link_lift" reason="Never"/>
    <disable_collisions link1="camera_link" link2="link_mast" reason="Never"/>
    <disable_collisions link1="camera_link" link2="link_right_wheel" reason="Never"/>
    <disable_collisions link1="camera_link" link2="link_wrist_yaw" reason="Never"/>
    <disable_collisions link1="camera_link" link2="respeaker_base" reason="Never"/>
    <disable_collisions link1="laser" link2="link_arm_l0" reason="Never"/>
    <disable_collisions link1="laser" link2="link_arm_l1" reason="Never"/>
    <disable_collisions link1="laser" link2="link_arm_l2" reason="Never"/>
    <disable_collisions link1="laser" link2="link_arm_l3" reason="Never"/>
    <disable_collisions link1="laser" link2="link_arm_l4" reason="Never"/>
    <disable_collisions link1="laser" link2="link_aruco_inner_wrist" reason="Never"/>
    <disable_collisions link1="laser" link2="link_aruco_left_base" reason="Never"/>
    <disable_collisions link1="laser" link2="link_aruco_right_base" reason="Never"/>
    <disable_collisions link1="laser" link2="link_aruco_shoulder" reason="Never"/>
    <disable_collisions link1="laser" link2="link_aruco_top_wrist" reason="Never"/>
    <disable_collisions link1="laser" link2="link_gripper" reason="Never"/>
    <disable_collisions link1="laser" link2="link_gripper_finger_left" reason="Never"/>
    <disable_collisions link1="laser" link2="link_gripper_finger_right" reason="Never"/>
    <disable_collisions link1="laser" link2="link_gripper_fingertip_left" reason="Never"/>
    <disable_collisions link1="laser" link2="link_gripper_fingertip_right" reason="Never"/>
    <disable_collisions link1="laser" link2="link_head" reason="Never"/>
    <disable_collisions link1="laser" link2="link_head_pan" reason="Never"/>
    <disable_collisions link1="laser" link2="link_head_tilt" reason="Never"/>
    <disable_collisions link1="laser" link2="link_left_wheel" reason="Never"/>
    <disable_collisions link1="laser" link2="link_lift" reason="Never"/>
    <disable_collisions link1="laser" link2="link_mast" reason="Never"/>
    <disable_collisions link1="laser" link2="link_right_wheel" reason="Never"/>
    <disable_collisions link1="laser" link2="link_wrist_yaw" reason="Never"/>
    <disable_collisions link1="laser" link2="respeaker_base" reason="Never"/>
    <disable_collisions link1="link_arm_l0" link2="link_arm_l1" reason="Adjacent"/>
    <disable_collisions link1="link_arm_l0" link2="link_arm_l2" reason="Never"/>
    <disable_collisions link1="link_arm_l0" link2="link_arm_l3" reason="Never"/>
    <disable_collisions link1="link_arm_l0" link2="link_arm_l4" reason="Never"/>
    <disable_collisions link1="link_arm_l0" link2="link_aruco_inner_wrist" reason="Adjacent"/>
    <disable_collisions link1="link_arm_l0" link2="link_aruco_left_base" reason="Never"/>
    <disable_collisions link1="link_arm_l0" link2="link_aruco_right_base" reason="Never"/>
    <disable_collisions link1="link_arm_l0" link2="link_aruco_shoulder" reason="Never"/>
    <disable_collisions link1="link_arm_l0" link2="link_aruco_top_wrist" reason="Adjacent"/>
    <disable_collisions link1="link_arm_l0" link2="link_gripper" reason="Never"/>
    <disable_collisions link1="link_arm_l0" link2="link_gripper_finger_left" reason="Never"/>
    <disable_collisions link1="link_arm_l0" link2="link_gripper_finger_right" reason="Never"/>
    <disable_collisions link1="link_arm_l0" link2="link_gripper_fingertip_left" reason="Never"/>
    <disable_collisions link1="link_arm_l0" link2="link_gripper_fingertip_right" reason="Never"/>
    <disable_collisions link1="link_arm_l0" link2="link_head" reason="Never"/>
    <disable_collisions link1="link_arm_l0" link2="link_head_pan" reason="Never"/>
    <disable_collisions link1="link_arm_l0" link2="link_head_tilt" reason="Never"/>
    <disable_collisions link1="link_arm_l0" link2="link_left_wheel" reason="Never"/>
    <disable_collisions link1="link_arm_l0" link2="link_lift" reason="Never"/>
    <disable_collisions link1="link_arm_l0" link2="link_mast" reason="Never"/>
    <disable_collisions link1="link_arm_l0" link2="link_right_wheel" reason="Never"/>
    <disable_collisions link1="link_arm_l0" link2="link_wrist_yaw" reason="Adjacent"/>
    <disable_collisions link1="link_arm_l0" link2="respeaker_base" reason="Never"/>
    <disable_collisions link1="link_arm_l1" link2="link_arm_l2" reason="Adjacent"/>
    <disable_collisions link1="link_arm_l1" link2="link_arm_l3" reason="Never"/>
    <disable_collisions link1="link_arm_l1" link2="link_arm_l4" reason="Never"/>
    <disable_collisions link1="link_arm_l1" link2="link_aruco_inner_wrist" reason="Never"/>
    <disable_collisions link1="link_arm_l1" link2="link_aruco_left_base" reason="Never"/>
    <disable_collisions link1="link_arm_l1" link2="link_aruco_right_base" reason="Never"/>
    <disable_collisions link1="link_arm_l1" link2="link_aruco_shoulder" reason="Never"/>
    <disable_collisions link1="link_arm_l1" link2="link_aruco_top_wrist" reason="Never"/>
    <disable_collisions link1="link_arm_l1" link2="link_gripper" reason="Never"/>
    <disable_collisions link1="link_arm_l1" link2="link_gripper_finger_left" reason="Never"/>
    <disable_collisions link1="link_arm_l1" link2="link_gripper_finger_right" reason="Never"/>
    <disable_collisions link1="link_arm_l1" link2="link_gripper_fingertip_left" reason="Never"/>
    <disable_collisions link1="link_arm_l1" link2="link_gripper_fingertip_right" reason="Never"/>
    <disable_collisions link1="link_arm_l1" link2="link_head" reason="Never"/>
    <disable_collisions link1="link_arm_l1" link2="link_head_pan" reason="Never"/>
    <disable_collisions link1="link_arm_l1" link2="link_head_tilt" reason="Never"/>
    <disable_collisions link1="link_arm_l1" link2="link_left_wheel" reason="Never"/>
    <disable_collisions link1="link_arm_l1" link2="link_lift" reason="Never"/>
    <disable_collisions link1="link_arm_l1" link2="link_mast" reason="Never"/>
    <disable_collisions link1="link_arm_l1" link2="link_right_wheel" reason="Never"/>
    <disable_collisions link1="link_arm_l1" link2="link_wrist_yaw" reason="Never"/>
    <disable_collisions link1="link_arm_l1" link2="respeaker_base" reason="Never"/>
    <disable_collisions link1="link_arm_l2" link2="link_arm_l3" reason="Adjacent"/>
    <disable_collisions link1="link_arm_l2" link2="link_arm_l4" reason="Never"/>
    <disable_collisions link1="link_arm_l2" link2="link_aruco_inner_wrist" reason="Never"/>
    <disable_collisions link1="link_arm_l2" link2="link_aruco_left_base" reason="Never"/>
    <disable_collisions link1="link_arm_l2" link2="link_aruco_right_base" reason="Never"/>
    <disable_collisions link1="link_arm_l2" link2="link_aruco_shoulder" reason="Never"/>
    <disable_collisions link1="link_arm_l2" link2="link_aruco_top_wrist" reason="Never"/>
    <disable_collisions link1="link_arm_l2" link2="link_gripper" reason="Never"/>
    <disable_collisions link1="link_arm_l2" link2="link_gripper_finger_left" reason="Never"/>
    <disable_collisions link1="link_arm_l2" link2="link_gripper_finger_right" reason="Never"/>
    <disable_collisions link1="link_arm_l2" link2="link_gripper_fingertip_left" reason="Never"/>
    <disable_collisions link1="link_arm_l2" link2="link_gripper_fingertip_right" reason="Never"/>
    <disable_collisions link1="link_arm_l2" link2="link_head" reason="Never"/>
    <disable_collisions link1="link_arm_l2" link2="link_head_pan" reason="Never"/>
    <disable_collisions link1="link_arm_l2" link2="link_head_tilt" reason="Never"/>
    <disable_collisions link1="link_arm_l2" link2="link_left_wheel" reason="Never"/>
    <disable_collisions link1="link_arm_l2" link2="link_lift" reason="Never"/>
    <disable_collisions link1="link_arm_l2" link2="link_mast" reason="Never"/>
    <disable_collisions link1="link_arm_l2" link2="link_right_wheel" reason="Never"/>
    <disable_collisions link1="link_arm_l2" link2="link_wrist_yaw" reason="Never"/>
    <disable_collisions link1="link_arm_l2" link2="respeaker_base" reason="Never"/>
    <disable_collisions link1="link_arm_l3" link2="link_arm_l4" reason="Adjacent"/>
    <disable_collisions link1="link_arm_l3" link2="link_aruco_inner_wrist" reason="Never"/>
    <disable_collisions link1="link_arm_l3" link2="link_aruco_left_base" reason="Never"/>
    <disable_collisions link1="link_arm_l3" link2="link_aruco_right_base" reason="Never"/>
    <disable_collisions link1="link_arm_l3" link2="link_aruco_shoulder" reason="Never"/>
    <disable_collisions link1="link_arm_l3" link2="link_aruco_top_wrist" reason="Never"/>
    <disable_collisions link1="link_arm_l3" link2="link_gripper" reason="Never"/>
    <disable_collisions link1="link_arm_l3" link2="link_gripper_finger_left" reason="Never"/>
    <disable_collisions link1="link_arm_l3" link2="link_gripper_finger_right" reason="Never"/>
    <disable_collisions link1="link_arm_l3" link2="link_gripper_fingertip_left" reason="Never"/>
    <disable_collisions link1="link_arm_l3" link2="link_gripper_fingertip_right" reason="Never"/>
    <disable_collisions link1="link_arm_l3" link2="link_head" reason="Never"/>
    <disable_collisions link1="link_arm_l3" link2="link_head_pan" reason="Never"/>
    <disable_collisions link1="link_arm_l3" link2="link_head_tilt" reason="Never"/>
    <disable_collisions link1="link_arm_l3" link2="link_left_wheel" reason="Never"/>
    <disable_collisions link1="link_arm_l3" link2="link_lift" reason="Never"/>
    <disable_collisions link1="link_arm_l3" link2="link_mast" reason="Never"/>
    <disable_collisions link1="link_arm_l3" link2="link_right_wheel" reason="Never"/>
    <disable_collisions link1="link_arm_l3" link2="link_wrist_yaw" reason="Never"/>
    <disable_collisions link1="link_arm_l3" link2="respeaker_base" reason="Never"/>
    <disable_collisions link1="link_arm_l4" link2="link_aruco_inner_wrist" reason="Never"/>
    <disable_collisions link1="link_arm_l4" link2="link_aruco_left_base" reason="Never"/>
    <disable_collisions link1="link_arm_l4" link2="link_aruco_right_base" reason="Never"/>
    <disable_collisions link1="link_arm_l4" link2="link_aruco_shoulder" reason="Never"/>
    <disable_collisions link1="link_arm_l4" link2="link_aruco_top_wrist" reason="Never"/>
    <disable_collisions link1="link_arm_l4" link2="link_gripper" reason="Never"/>
    <disable_collisions link1="link_arm_l4" link2="link_gripper_finger_left" reason="Never"/>
    <disable_collisions link1="link_arm_l4" link2="link_gripper_finger_right" reason="Never"/>
    <disable_collisions link1="link_arm_l4" link2="link_gripper_fingertip_left" reason="Never"/>
    <disable_collisions link1="link_arm_l4" link2="link_gripper_fingertip_right" reason="Never"/>
    <disable_collisions link1="link_arm_l4" link2="link_head" reason="Never"/>
    <disable_collisions link1="link_arm_l4" link2="link_head_pan" reason="Never"/>
    <disable_collisions link1="link_arm_l4" link2="link_head_tilt" reason="Never"/>
    <disable_collisions link1="link_arm_l4" link2="link_left_wheel" reason="Never"/>
    <disable_collisions link1="link_arm_l4" link2="link_lift" reason="Adjacent"/>
    <disable_collisions link1="link_arm_l4" link2="link_mast" reason="Never"/>
    <disable_collisions link1="link_arm_l4" link2="link_right_wheel" reason="Never"/>
    <disable_collisions link1="link_arm_l4" link2="link_wrist_yaw" reason="Never"/>
    <disable_collisions link1="link_arm_l4" link2="respeaker_base" reason="Never"/>
    <disable_collisions link1="link_aruco_inner_wrist" link2="link_aruco_left_base" reason="Never"/>
    <disable_collisions link1="link_aruco_inner_wrist" link2="link_aruco_right_base" reason="Never"/>
    <disable_collisions link1="link_aruco_inner_wrist" link2="link_aruco_shoulder" reason="Never"/>
    <disable_collisions link1="link_aruco_inner_wrist" link2="link_aruco_top_wrist" reason="Never"/>
    <disable_collisions link1="link_aruco_inner_wrist" link2="link_gripper" reason="Never"/>
    <disable_collisions link1="link_aruco_inner_wrist" link2="link_gripper_finger_left" reason="Never"/>
    <disable_collisions link1="link_aruco_inner_wrist" link2="link_gripper_finger_right" reason="Never"/>
    <disable_collisions link1="link_aruco_inner_wrist" link2="link_gripper_fingertip_left" reason="Never"/>
    <disable_collisions link1="link_aruco_inner_wrist" link2="link_gripper_fingertip_right" reason="Never"/>
    <disable_collisions link1="link_aruco_inner_wrist" link2="link_head" reason="Never"/>
    <disable_collisions link1="link_aruco_inner_wrist" link2="link_head_pan" reason="Never"/>
    <disable_collisions link1="link_aruco_inner_wrist" link2="link_head_tilt" reason="Never"/>
    <disable_collisions link1="link_aruco_inner_wrist" link2="link_left_wheel" reason="Never"/>
    <disable_collisions link1="link_aruco_inner_wrist" link2="link_lift" reason="Never"/>
    <disable_collisions link1="link_aruco_inner_wrist" link2="link_mast" reason="Never"/>
    <disable_collisions link1="link_aruco_inner_wrist" link2="link_right_wheel" reason="Never"/>
    <disable_collisions link1="link_aruco_inner_wrist" link2="link_wrist_yaw" reason="Never"/>
    <disable_collisions link1="link_aruco_inner_wrist" link2="respeaker_base" reason="Never"/>
    <disable_collisions link1="link_aruco_left_base" link2="link_aruco_right_base" reason="Never"/>
    <disable_collisions link1="link_aruco_left_base" link2="link_aruco_shoulder" reason="Never"/>
    <disable_collisions link1="link_aruco_left_base" link2="link_aruco_top_wrist" reason="Never"/>
    <disable_collisions link1="link_aruco_left_base" link2="link_gripper" reason="Never"/>
    <disable_collisions link1="link_aruco_left_base" link2="link_gripper_finger_left" reason="Never"/>
    <disable_collisions link1="link_aruco_left_base" link2="link_gripper_finger_right" reason="Never"/>
    <disable_collisions link1="link_aruco_left_base" link2="link_gripper_fingertip_left" reason="Never"/>
    <disable_collisions link1="link_aruco_left_base" link2="link_gripper_fingertip_right" reason="Never"/>
    <disable_collisions link1="link_aruco_left_base" link2="link_head" reason="Never"/>
    <disable_collisions link1="link_aruco_left_base" link2="link_head_pan" reason="Never"/>
    <disable_collisions link1="link_aruco_left_base" link2="link_head_tilt" reason="Never"/>
    <disable_collisions link1="link_aruco_left_base" link2="link_left_wheel" reason="Never"/>
    <disable_collisions link1="link_aruco_left_base" link2="link_lift" reason="Never"/>
    <disable_collisions link1="link_aruco_left_base" link2="link_mast" reason="Never"/>
    <disable_collisions link1="link_aruco_left_base" link2="link_right_wheel" reason="Never"/>
    <disable_collisions link1="link_aruco_left_base" link2="link_wrist_yaw" reason="Never"/>
    <disable_collisions link1="link_aruco_left_base" link2="respeaker_base" reason="Never"/>
    <disable_collisions link1="link_aruco_right_base" link2="link_aruco_shoulder" reason="Never"/>
    <disable_collisions link1="link_aruco_right_base" link2="link_aruco_top_wrist" reason="Never"/>
    <disable_collisions link1="link_aruco_right_base" link2="link_gripper" reason="Default"/>
    <disable_collisions link1="link_aruco_right_base" link2="link_head" reason="Never"/>
    <disable_collisions link1="link_aruco_right_base" link2="link_head_pan" reason="Never"/>
    <disable_collisions link1="link_aruco_right_base" link2="link_head_tilt" reason="Never"/>
    <disable_collisions link1="link_aruco_right_base" link2="link_left_wheel" reason="Never"/>
    <disable_collisions link1="link_aruco_right_base" link2="link_lift" reason="Never"/>
    <disable_collisions link1="link_aruco_right_base" link2="link_mast" reason="Never"/>
    <disable_collisions link1="link_aruco_right_base" link2="link_right_wheel" reason="Never"/>
    <disable_collisions link1="link_aruco_right_base" link2="link_wrist_yaw" reason="Never"/>
    <disable_collisions link1="link_aruco_right_base" link2="respeaker_base" reason="Never"/>
    <disable_collisions link1="link_aruco_shoulder" link2="link_aruco_top_wrist" reason="Never"/>
    <disable_collisions link1="link_aruco_shoulder" link2="link_gripper" reason="Never"/>
    <disable_collisions link1="link_aruco_shoulder" link2="link_gripper_finger_left" reason="Never"/>
    <disable_collisions link1="link_aruco_shoulder" link2="link_gripper_finger_right" reason="Never"/>
    <disable_collisions link1="link_aruco_shoulder" link2="link_gripper_fingertip_left" reason="Never"/>
    <disable_collisions link1="link_aruco_shoulder" link2="link_gripper_fingertip_right" reason="Never"/>
    <disable_collisions link1="link_aruco_shoulder" link2="link_head" reason="Never"/>
    <disable_collisions link1="link_aruco_shoulder" link2="link_head_pan" reason="Never"/>
    <disable_collisions link1="link_aruco_shoulder" link2="link_head_tilt" reason="Never"/>
    <disable_collisions link1="link_aruco_shoulder" link2="link_left_wheel" reason="Never"/>
    <disable_collisions link1="link_aruco_shoulder" link2="link_lift" reason="Adjacent"/>
    <disable_collisions link1="link_aruco_shoulder" link2="link_mast" reason="Never"/>
    <disable_collisions link1="link_aruco_shoulder" link2="link_right_wheel" reason="Never"/>
    <disable_collisions link1="link_aruco_shoulder" link2="link_wrist_yaw" reason="Never"/>
    <disable_collisions link1="link_aruco_shoulder" link2="respeaker_base" reason="Never"/>
    <disable_collisions link1="link_aruco_top_wrist" link2="link_gripper" reason="Never"/>
    <disable_collisions link1="link_aruco_top_wrist" link2="link_gripper_finger_left" reason="Never"/>
    <disable_collisions link1="link_aruco_top_wrist" link2="link_gripper_finger_right" reason="Never"/>
    <disable_collisions link1="link_aruco_top_wrist" link2="link_gripper_fingertip_left" reason="Never"/>
    <disable_collisions link1="link_aruco_top_wrist" link2="link_gripper_fingertip_right" reason="Never"/>
    <disable_collisions link1="link_aruco_top_wrist" link2="link_head" reason="Never"/>
    <disable_collisions link1="link_aruco_top_wrist" link2="link_head_pan" reason="Never"/>
    <disable_collisions link1="link_aruco_top_wrist" link2="link_head_tilt" reason="Never"/>
    <disable_collisions link1="link_aruco_top_wrist" link2="link_left_wheel" reason="Never"/>
    <disable_collisions link1="link_aruco_top_wrist" link2="link_lift" reason="Never"/>
    <disable_collisions link1="link_aruco_top_wrist" link2="link_mast" reason="Never"/>
    <disable_collisions link1="link_aruco_top_wrist" link2="link_right_wheel" reason="Never"/>
    <disable_collisions link1="link_aruco_top_wrist" link2="link_wrist_yaw" reason="Never"/>
    <disable_collisions link1="link_aruco_top_wrist" link2="respeaker_base" reason="Never"/>
    <disable_collisions link1="link_gripper" link2="link_gripper_finger_left" reason="Adjacent"/>
    <disable_collisions link1="link_gripper" link2="link_gripper_finger_right" reason="Adjacent"/>
    <disable_collisions link1="link_gripper" link2="link_gripper_fingertip_left" reason="Never"/>
    <disable_collisions link1="link_gripper" link2="link_gripper_fingertip_right" reason="Never"/>
    <disable_collisions link1="link_gripper" link2="link_head" reason="Never"/>
    <disable_collisions link1="link_gripper" link2="link_head_pan" reason="Never"/>
    <disable_collisions link1="link_gripper" link2="link_head_tilt" reason="Never"/>
    <disable_collisions link1="link_gripper" link2="link_left_wheel" reason="Never"/>
    <disable_collisions link1="link_gripper" link2="link_lift" reason="Never"/>
    <disable_collisions link1="link_gripper" link2="link_mast" reason="Never"/>
    <disable_collisions link1="link_gripper" link2="link_wrist_yaw" reason="Adjacent"/>
    <disable_collisions link1="link_gripper" link2="respeaker_base" reason="Never"/>
    <disable_collisions link1="link_gripper_finger_left" link2="link_gripper_fingertip_left" reason="Adjacent"/>
    <disable_collisions link1="link_gripper_finger_left" link2="link_head" reason="Never"/>
    <disable_collisions link1="link_gripper_finger_left" link2="link_head_pan" reason="Never"/>
    <disable_collisions link1="link_gripper_finger_left" link2="link_head_tilt" reason="Never"/>
    <disable_collisions link1="link_gripper_finger_left" link2="link_left_wheel" reason="Never"/>
    <disable_collisions link1="link_gripper_finger_left" link2="link_lift" reason="Never"/>
    <disable_collisions link1="link_gripper_finger_left" link2="link_mast" reason="Never"/>
    <disable_collisions link1="link_gripper_finger_left" link2="link_wrist_yaw" reason="Never"/>
    <disable_collisions link1="link_gripper_finger_left" link2="respeaker_base" reason="Never"/>
    <disable_collisions link1="link_gripper_finger_right" link2="link_gripper_fingertip_right" reason="Adjacent"/>
    <disable_collisions link1="link_gripper_finger_right" link2="link_head" reason="Never"/>
    <disable_collisions link1="link_gripper_finger_right" link2="link_head_pan" reason="Never"/>
    <disable_collisions link1="link_gripper_finger_right" link2="link_head_tilt" reason="Never"/>
    <disable_collisions link1="link_gripper_finger_right" link2="link_left_wheel" reason="Never"/>
    <disable_collisions link1="link_gripper_finger_right" link2="link_lift" reason="Never"/>
    <disable_collisions link1="link_gripper_finger_right" link2="link_mast" reason="Never"/>
    <disable_collisions link1="link_gripper_finger_right" link2="link_wrist_yaw" reason="Never"/>
    <disable_collisions link1="link_gripper_finger_right" link2="respeaker_base" reason="Never"/>
    <disable_collisions link1="link_gripper_fingertip_left" link2="link_head" reason="Never"/>
    <disable_collisions link1="link_gripper_fingertip_left" link2="link_head_pan" reason="Never"/>
    <disable_collisions link1="link_gripper_fingertip_left" link2="link_head_tilt" reason="Never"/>
    <disable_collisions link1="link_gripper_fingertip_left" link2="link_left_wheel" reason="Never"/>
    <disable_collisions link1="link_gripper_fingertip_left" link2="link_lift" reason="Never"/>
    <disable_collisions link1="link_gripper_fingertip_left" link2="link_mast" reason="Never"/>
    <disable_collisions link1="link_gripper_fingertip_left" link2="link_wrist_yaw" reason="Never"/>
    <disable_collisions link1="link_gripper_fingertip_left" link2="respeaker_base" reason="Never"/>
    <disable_collisions link1="link_gripper_fingertip_right" link2="link_head" reason="Never"/>
    <disable_collisions link1="link_gripper_fingertip_right" link2="link_head_pan" reason="Never"/>
    <disable_collisions link1="link_gripper_fingertip_right" link2="link_head_tilt" reason="Never"/>
    <disable_collisions link1="link_gripper_fingertip_right" link2="link_left_wheel" reason="Never"/>
    <disable_collisions link1="link_gripper_fingertip_right" link2="link_lift" reason="Never"/>
    <disable_collisions link1="link_gripper_fingertip_right" link2="link_mast" reason="Never"/>
    <disable_collisions link1="link_gripper_fingertip_right" link2="link_wrist_yaw" reason="Never"/>
    <disable_collisions link1="link_gripper_fingertip_right" link2="respeaker_base" reason="Never"/>
    <disable_collisions link1="link_head" link2="link_head_pan" reason="Adjacent"/>
    <disable_collisions link1="link_head" link2="link_head_tilt" reason="Never"/>
    <disable_collisions link1="link_head" link2="link_left_wheel" reason="Never"/>
    <disable_collisions link1="link_head" link2="link_lift" reason="Never"/>
    <disable_collisions link1="link_head" link2="link_mast" reason="Adjacent"/>
    <disable_collisions link1="link_head" link2="link_right_wheel" reason="Never"/>
    <disable_collisions link1="link_head" link2="link_wrist_yaw" reason="Never"/>
    <disable_collisions link1="link_head" link2="respeaker_base" reason="Default"/>
    <disable_collisions link1="link_head_pan" link2="link_head_tilt" reason="Adjacent"/>
    <disable_collisions link1="link_head_pan" link2="link_left_wheel" reason="Never"/>
    <disable_collisions link1="link_head_pan" link2="link_lift" reason="Never"/>
    <disable_collisions link1="link_head_pan" link2="link_mast" reason="Never"/>
    <disable_collisions link1="link_head_pan" link2="link_right_wheel" reason="Never"/>
    <disable_collisions link1="link_head_pan" link2="link_wrist_yaw" reason="Never"/>
    <disable_collisions link1="link_head_pan" link2="respeaker_base" reason="Never"/>
    <disable_collisions link1="link_head_tilt" link2="link_left_wheel" reason="Never"/>
    <disable_collisions link1="link_head_tilt" link2="link_lift" reason="Never"/>
    <disable_collisions link1="link_head_tilt" link2="link_mast" reason="Never"/>
    <disable_collisions link1="link_head_tilt" link2="link_right_wheel" reason="Never"/>
    <disable_collisions link1="link_head_tilt" link2="link_wrist_yaw" reason="Never"/>
    <disable_collisions link1="link_head_tilt" link2="respeaker_base" reason="Never"/>
    <disable_collisions link1="link_left_wheel" link2="link_lift" reason="Never"/>
    <disable_collisions link1="link_left_wheel" link2="link_mast" reason="Never"/>
    <disable_collisions link1="link_left_wheel" link2="link_right_wheel" reason="Never"/>
    <disable_collisions link1="link_left_wheel" link2="link_wrist_yaw" reason="Never"/>
    <disable_collisions link1="link_left_wheel" link2="respeaker_base" reason="Never"/>
    <disable_collisions link1="link_lift" link2="link_mast" reason="Adjacent"/>
    <disable_collisions link1="link_lift" link2="link_right_wheel" reason="Never"/>
    <disable_collisions link1="link_lift" link2="link_wrist_yaw" reason="Never"/>
    <disable_collisions link1="link_lift" link2="respeaker_base" reason="Never"/>
    <disable_collisions link1="link_mast" link2="link_right_wheel" reason="Never"/>
    <disable_collisions link1="link_mast" link2="link_wrist_yaw" reason="Never"/>
    <disable_collisions link1="link_mast" link2="respeaker_base" reason="Adjacent"/>
    <disable_collisions link1="link_right_wheel" link2="link_wrist_yaw" reason="Never"/>
    <disable_collisions link1="link_right_wheel" link2="respeaker_base" reason="Never"/>
    <disable_collisions link1="link_wrist_yaw" link2="respeaker_base" reason="Never"/>
    <disable_collisions link1="caster_link" link2="base_link" reason="Adjacent"/>
 </robot>
--- a/stretch_moveit_config/launch/chomp_planning_pipeline.launch.xml
+++ b/stretch_moveit_config/launch/chomp_planning_pipeline.launch.xml
@ -0,0 +1,21 @@
 <launch>
   <!-- CHOMP Plugin for MoveIt -->
   <arg name="planning_plugin" value="chomp_interface/CHOMPPlanner" />
   <arg name="start_state_max_bounds_error" value="0.1" />
   <!-- The request adapters (plugins) used when planning.
        ORDER MATTERS -->
   <arg name="planning_adapters"
        value="default_planner_request_adapters/AddTimeParameterization
               default_planner_request_adapters/ResolveConstraintFrames
               default_planner_request_adapters/FixWorkspaceBounds
               default_planner_request_adapters/FixStartStateBounds
               default_planner_request_adapters/FixStartStateCollision
               default_planner_request_adapters/FixStartStatePathConstraints"
               />

   <param name="planning_plugin" value="$(arg planning_plugin)" />
   <param name="request_adapters" value="$(arg planning_adapters)" />
   <param name="start_state_max_bounds_error" value="$(arg start_state_max_bounds_error)" />

   <rosparam command="load" file="$(find stretch_moveit_config)/config/chomp_planning.yaml" />
 </launch>
--- a/stretch_moveit_config/launch/default_warehouse_db.launch
+++ b/stretch_moveit_config/launch/default_warehouse_db.launch
@ -0,0 +1,15 @@
 <launch>

  <arg name="reset" default="false"/>
  <!-- If not specified, we'll use a default database location -->
  <arg name="moveit_warehouse_database_path" default="$(find stretch_moveit_config)/default_warehouse_mongo_db" />

  <!-- Launch the warehouse with the configured database location -->
  <include file="$(find stretch_moveit_config)/launch/warehouse.launch">
    <arg name="moveit_warehouse_database_path" value="$(arg moveit_warehouse_database_path)" />
  </include>

  <!-- If we want to reset the database, run this node -->
  <node if="$(arg reset)" name="$(anon moveit_default_db_reset)" type="moveit_init_demo_warehouse" pkg="moveit_ros_warehouse" respawn="false" output="screen" />

 </launch>
--- a/stretch_moveit_config/launch/demo.launch
+++ b/stretch_moveit_config/launch/demo.launch
@ -0,0 +1,65 @@
 <launch>

  <!-- specify the planning pipeline -->
  <arg name="pipeline" default="ompl" />

  <!-- By default, we do not start a database (it can be large) -->
  <arg name="db" default="false" />
  <!-- Allow user to specify database location -->
  <arg name="db_path" default="$(find stretch_moveit_config)/default_warehouse_mongo_db" />

  <!-- By default, we are not in debug mode -->
  <arg name="debug" default="false" />

  <!--
  By default, hide joint_state_publisher's GUI

  MoveIt's "demo" mode replaces the real robot driver with the joint_state_publisher.
  The latter one maintains and publishes the current joint configuration of the simulated robot.
  It also provides a GUI to move the simulated robot around "manually".
  This corresponds to moving around the real robot without the use of MoveIt.
  -->
  <arg name="use_gui" default="false" />
  <arg name="use_rviz" default="true" />

  <!-- Load the URDF, SRDF and other .yaml configuration files on the param server -->
  <include file="$(find stretch_moveit_config)/launch/planning_context.launch">
    <arg name="load_robot_description" value="true"/>
  </include>

  <!-- If needed, broadcast static tf for robot root -->
    <node pkg="tf2_ros" type="static_transform_publisher" name="virtual_joint_broadcaster_0" args="0 0 0 0 0 0 map base_link" />


  <!-- We do not have a robot connected, so publish fake joint states -->
  <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" unless="$(arg use_gui)">
    <rosparam param="source_list">[move_group/fake_controller_joint_states]</rosparam>
  </node>
  <node name="joint_state_publisher" pkg="joint_state_publisher_gui" type="joint_state_publisher_gui" if="$(arg use_gui)">
    <rosparam param="source_list">[move_group/fake_controller_joint_states]</rosparam>
  </node>

  <!-- Given the published joint states, publish tf for the robot links -->
  <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" respawn="true" output="screen" />

  <!-- Run the main MoveIt executable without trajectory execution (we do not have controllers configured by default) -->
  <include file="$(find stretch_moveit_config)/launch/move_group.launch">
    <arg name="allow_trajectory_execution" value="true"/>
    <arg name="fake_execution" value="true"/>
    <arg name="info" value="true"/>
    <arg name="debug" value="$(arg debug)"/>
    <arg name="pipeline" value="$(arg pipeline)"/>
  </include>

  <!-- Run Rviz and load the default config to see the state of the move_group node -->
  <include file="$(find stretch_moveit_config)/launch/moveit_rviz.launch" if="$(arg use_rviz)">
    <arg name="rviz_config" value="$(find stretch_moveit_config)/launch/moveit.rviz"/>
    <arg name="debug" value="$(arg debug)"/>
  </include>

  <!-- If database loading was enabled, start mongodb as well -->
  <include file="$(find stretch_moveit_config)/launch/default_warehouse_db.launch" if="$(arg db)">
    <arg name="moveit_warehouse_database_path" value="$(arg db_path)"/>
  </include>

 </launch>
--- a/stretch_moveit_config/launch/demo_gazebo.launch
+++ b/stretch_moveit_config/launch/demo_gazebo.launch
@ -0,0 +1,61 @@
 <launch>

  <!-- By default, we do not start a database (it can be large) -->
  <arg name="db" default="false" />
  <!-- Allow user to specify database location -->
  <arg name="db_path" default="$(find stretch_moveit_config)/default_warehouse_mongo_db" />

  <!-- By default, we are not in debug mode -->
  <arg name="debug" default="false" />

  <!--
  By default, hide joint_state_publisher's GUI

  MoveIt's "demo" mode replaces the real robot driver with the joint_state_publisher.
  The latter one maintains and publishes the current joint configuration of the simulated robot.
  It also provides a GUI to move the simulated robot around "manually".
  This corresponds to moving around the real robot without the use of MoveIt.
  -->
  <arg name="use_gui" default="false" />

  <!-- Gazebo specific options -->
  <arg name="gazebo_gui" default="true"/>
  <arg name="paused" default="false"/>
  <!-- By default, use the urdf location provided from the package -->
  <arg name="urdf_path" default="$(find stretch_description)/urdf/stretch_description.xacro"/>

  <!-- launch the gazebo simulator and spawn the robot -->
  <!-- <include file="$(find stretch_gazebo)/launch/gazebo.launch" >
    <arg name="rviz" value="false"/>
  </include> -->

  <!-- Load the URDF, SRDF and other .yaml configuration files on the param server -->
  <include file="$(find stretch_moveit_config)/launch/planning_context.launch">
    <arg name="load_robot_description" value="true"/>
  </include>

  <!-- If needed, broadcast static tf for robot root -->
    <node pkg="tf2_ros" type="static_transform_publisher" name="virtual_joint_broadcaster_0" args="0 0 0 0 0 0 map base_link" />

  <!-- Given the published joint states, publish tf for the robot links -->
  <!-- <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" respawn="true" output="screen" /> -->

  <!-- Run the main MoveIt executable without trajectory execution (we do not have controllers configured by default) -->
  <include file="$(find stretch_moveit_config)/launch/move_group.launch">
    <arg name="allow_trajectory_execution" value="true"/>
    <arg name="info" value="true"/>
    <arg name="debug" value="$(arg debug)"/>
  </include>  

  <!-- Run Rviz and load the default config to see the state of the move_group node -->
  <include file="$(find stretch_moveit_config)/launch/moveit_rviz.launch">
    <arg name="rviz_config" value="$(find stretch_moveit_config)/launch/moveit.rviz"/>
    <arg name="debug" value="$(arg debug)"/>
  </include>

  <!-- If database loading was enabled, start mongodb as well -->
  <include file="$(find stretch_moveit_config)/launch/default_warehouse_db.launch" if="$(arg db)">
    <arg name="moveit_warehouse_database_path" value="$(arg db_path)"/>
  </include>

 </launch>
--- a/stretch_moveit_config/launch/fake_moveit_controller_manager.launch.xml
+++ b/stretch_moveit_config/launch/fake_moveit_controller_manager.launch.xml
@ -0,0 +1,9 @@
 <launch>

  <!-- Set the param that trajectory_execution_manager needs to find the controller plugin -->
  <param name="moveit_controller_manager" value="moveit_fake_controller_manager/MoveItFakeControllerManager"/>

  <!-- The rest of the params are specific to this plugin -->
  <rosparam file="$(find stretch_moveit_config)/config/fake_controllers.yaml"/>

 </launch>
--- a/stretch_moveit_config/launch/gazebo.launch
+++ b/stretch_moveit_config/launch/gazebo.launch
@ -0,0 +1,23 @@
 <?xml version="1.0"?>
 <launch>
  <arg name="paused" default="false"/>
  <arg name="gazebo_gui" default="true"/>
  <arg name="urdf_path" default="$(find stretch_description)/urdf/stretch_description.xacro"/>

  <!-- startup simulated world -->
  <include file="$(find gazebo_ros)/launch/empty_world.launch">
    <arg name="world_name" default="worlds/empty.world"/>
    <arg name="paused" value="$(arg paused)"/>
    <arg name="gui" value="$(arg gazebo_gui)"/>
  </include>

  <!-- send robot urdf to param server -->
  <param name="robot_description" textfile="$(arg urdf_path)" />

  <!-- push robot_description to factory and spawn robot in gazebo at the origin, change x,y,z arguments to spawn in a different position -->
  <node name="spawn_gazebo_model" pkg="gazebo_ros" type="spawn_model" args="-urdf -param robot_description -model robot -x 0 -y 0 -z 0"
    respawn="false" output="screen" />

  <include file="$(find stretch_moveit_config)/launch/ros_controllers.launch"/>

 </launch>
--- a/stretch_moveit_config/launch/joystick_control.launch
+++ b/stretch_moveit_config/launch/joystick_control.launch
@ -0,0 +1,17 @@
 <launch>
  <!-- See moveit_ros/visualization/doc/joystick.rst for documentation -->

  <arg name="dev" default="/dev/input/js0" />

  <!-- Launch joy node -->
  <node pkg="joy" type="joy_node" name="joy">
    <param name="dev" value="$(arg dev)" /> <!-- Customize this to match the location your joystick is plugged in on-->
    <param name="deadzone" value="0.2" />
    <param name="autorepeat_rate" value="40" />
    <param name="coalesce_interval" value="0.025" />
  </node>

  <!-- Launch python interface -->
  <node pkg="moveit_ros_visualization" type="moveit_joy.py" output="screen" name="moveit_joy"/>

 </launch>
--- a/stretch_moveit_config/launch/move_group.launch
+++ b/stretch_moveit_config/launch/move_group.launch
@ -0,0 +1,82 @@
 <launch>

  <!-- GDB Debug Option -->
  <arg name="debug" default="false" />
  <arg unless="$(arg debug)" name="launch_prefix" value="" />
  <arg     if="$(arg debug)" name="launch_prefix"
 	   value="gdb -x $(find stretch_moveit_config)/launch/gdb_settings.gdb --ex run --args" />

  <!-- Verbose Mode Option -->
  <arg name="info" default="$(arg debug)" />
  <arg unless="$(arg info)" name="command_args" value="" />
  <arg     if="$(arg info)" name="command_args" value="--debug" />

  <!-- move_group settings -->
  <arg name="pipeline" default="ompl" />
  <arg name="allow_trajectory_execution" default="true"/>
  <arg name="fake_execution" default="false"/>
  <arg name="max_safe_path_cost" default="1"/>
  <arg name="jiggle_fraction" default="0.05" />
  <arg name="publish_monitored_planning_scene" default="true"/>

  <arg name="capabilities" default=""/>
  <arg name="disable_capabilities" default=""/>
  <!-- load these non-default MoveGroup capabilities (space seperated) -->
  <!--
  <arg name="capabilities" value="
                a_package/AwsomeMotionPlanningCapability
                another_package/GraspPlanningPipeline
                " />
  -->

  <!-- inhibit these default MoveGroup capabilities (space seperated) -->
  <!--
  <arg name="disable_capabilities" value="
                move_group/MoveGroupKinematicsService
                move_group/ClearOctomapService
                " />
  -->

  <arg name="load_robot_description" default="true" />
  <!-- load URDF, SRDF and joint_limits configuration -->
  <include file="$(find stretch_moveit_config)/launch/planning_context.launch">
    <arg name="load_robot_description" value="$(arg load_robot_description)" />
  </include>

  <!-- Planning Functionality -->
  <include ns="move_group" file="$(find stretch_moveit_config)/launch/planning_pipeline.launch.xml">
    <arg name="pipeline" value="$(arg pipeline)" />
  </include>

  <!-- Trajectory Execution Functionality -->
  <include ns="move_group" file="$(find stretch_moveit_config)/launch/trajectory_execution.launch.xml" if="$(arg allow_trajectory_execution)">
    <arg name="moveit_manage_controllers" value="true" />
    <arg name="moveit_controller_manager" value="stretch_description" unless="$(arg fake_execution)"/>
    <arg name="moveit_controller_manager" value="fake" if="$(arg fake_execution)"/>
  </include>

  <!-- Sensors Functionality -->
  <include ns="move_group" file="$(find stretch_moveit_config)/launch/sensor_manager.launch.xml" if="$(arg allow_trajectory_execution)">
    <arg name="moveit_sensor_manager" value="stretch_description" />
  </include>

  <!-- Start the actual move_group node/action server -->
  <node name="move_group" launch-prefix="$(arg launch_prefix)" pkg="moveit_ros_move_group" type="move_group" respawn="false" output="screen" args="$(arg command_args)">
    <!-- Set the display variable, in case OpenGL code is used internally -->
    <env name="DISPLAY" value="$(optenv DISPLAY :0)" />

    <param name="allow_trajectory_execution" value="$(arg allow_trajectory_execution)"/>
    <param name="max_safe_path_cost" value="$(arg max_safe_path_cost)"/>
    <param name="jiggle_fraction" value="$(arg jiggle_fraction)" />
    <param name="capabilities" value="$(arg capabilities)"/>
    <param name="disable_capabilities" value="$(arg disable_capabilities)"/>


    <!-- Publish the planning scene of the physical robot so that rviz plugin can know actual robot -->
    <param name="planning_scene_monitor/publish_planning_scene" value="$(arg publish_monitored_planning_scene)" />
    <param name="planning_scene_monitor/publish_geometry_updates" value="$(arg publish_monitored_planning_scene)" />
    <param name="planning_scene_monitor/publish_state_updates" value="$(arg publish_monitored_planning_scene)" />
    <param name="planning_scene_monitor/publish_transforms_updates" value="$(arg publish_monitored_planning_scene)" />
  </node>

 </launch>
--- a/stretch_moveit_config/launch/moveit.rviz
+++ b/stretch_moveit_config/launch/moveit.rviz
@ -0,0 +1,570 @@
 Panels:
  - Class: rviz/Displays
    Help Height: 84
    Name: Displays
    Property Tree Widget:
      Expanded:
        - /MotionPlanning1
      Splitter Ratio: 0.7425600290298462
    Tree Height: 351
  - Class: rviz/Help
    Name: Help
  - Class: rviz/Views
    Expanded:
      - /Current View1
    Name: Views
    Splitter Ratio: 0.5
 Preferences:
  PromptSaveOnExit: true
 Toolbars:
  toolButtonStyle: 2
 Visualization Manager:
  Class: ""
  Displays:
    - Alpha: 0.5
      Cell Size: 1
      Class: rviz/Grid
      Color: 160; 160; 164
      Enabled: true
      Line Style:
        Line Width: 0.029999999329447746
        Value: Lines
      Name: Grid
      Normal Cell Count: 0
      Offset:
        X: 0
        Y: 0
        Z: 0
      Plane: XY
      Plane Cell Count: 10
      Reference Frame: <Fixed Frame>
      Value: true
    - Acceleration_Scaling_Factor: 1
      Class: moveit_rviz_plugin/MotionPlanning
      Enabled: true
      Move Group Namespace: ""
      MoveIt_Allow_Approximate_IK: false
      MoveIt_Allow_External_Program: false
      MoveIt_Allow_Replanning: false
      MoveIt_Allow_Sensor_Positioning: false
      MoveIt_Planning_Attempts: 10
      MoveIt_Planning_Time: 5
      MoveIt_Use_Cartesian_Path: false
      MoveIt_Use_Constraint_Aware_IK: true
      MoveIt_Warehouse_Host: 127.0.0.1
      MoveIt_Warehouse_Port: 33829
      MoveIt_Workspace:
        Center:
          X: 0
          Y: 0
          Z: 0
        Size:
          X: 2
          Y: 2
          Z: 2
      Name: MotionPlanning
      Planned Path:
        Color Enabled: false
        Interrupt Display: false
        Links:
          All Links Enabled: true
          Expand Joint Details: false
          Expand Link Details: false
          Expand Tree: false
          Link Tree Style: Links in Alphabetic Order
          base_link:
            Alpha: 1
            Show Axes: false
            Show Trail: false
            Value: true
          camera_accel_frame:
            Alpha: 1
            Show Axes: false
            Show Trail: false
          camera_accel_optical_frame:
            Alpha: 1
            Show Axes: false
            Show Trail: false
          camera_bottom_screw_frame:
            Alpha: 1
            Show Axes: false
            Show Trail: false
          camera_color_frame:
            Alpha: 1
            Show Axes: false
            Show Trail: false
          camera_color_optical_frame:
            Alpha: 1
            Show Axes: false
            Show Trail: false
          camera_depth_frame:
            Alpha: 1
            Show Axes: false
            Show Trail: false
          camera_depth_optical_frame:
            Alpha: 1
            Show Axes: false
            Show Trail: false
          camera_gyro_frame:
            Alpha: 1
            Show Axes: false
            Show Trail: false
          camera_gyro_optical_frame:
            Alpha: 1
            Show Axes: false
            Show Trail: false
          camera_infra1_frame:
            Alpha: 1
            Show Axes: false
            Show Trail: false
          camera_infra1_optical_frame:
            Alpha: 1
            Show Axes: false
            Show Trail: false
          camera_infra2_frame:
            Alpha: 1
            Show Axes: false
            Show Trail: false
          camera_infra2_optical_frame:
            Alpha: 1
            Show Axes: false
            Show Trail: false
          camera_link:
            Alpha: 1
            Show Axes: false
            Show Trail: false
            Value: true
          caster_link:
            Alpha: 1
            Show Axes: false
            Show Trail: false
            Value: true
          laser:
            Alpha: 1
            Show Axes: false
            Show Trail: false
            Value: true
          link_arm_l0:
            Alpha: 1
            Show Axes: false
            Show Trail: false
            Value: true
          link_arm_l1:
            Alpha: 1
            Show Axes: false
            Show Trail: false
            Value: true
          link_arm_l2:
            Alpha: 1
            Show Axes: false
            Show Trail: false
            Value: true
          link_arm_l3:
            Alpha: 1
            Show Axes: false
            Show Trail: false
            Value: true
          link_arm_l4:
            Alpha: 1
            Show Axes: false
            Show Trail: false
            Value: true
          link_aruco_inner_wrist:
            Alpha: 1
            Show Axes: false
            Show Trail: false
            Value: true
          link_aruco_left_base:
            Alpha: 1
            Show Axes: false
            Show Trail: false
            Value: true
          link_aruco_right_base:
            Alpha: 1
            Show Axes: false
            Show Trail: false
            Value: true
          link_aruco_shoulder:
            Alpha: 1
            Show Axes: false
            Show Trail: false
            Value: true
          link_aruco_top_wrist:
            Alpha: 1
            Show Axes: false
            Show Trail: false
            Value: true
          link_grasp_center:
            Alpha: 1
            Show Axes: false
            Show Trail: false
          link_gripper:
            Alpha: 1
            Show Axes: false
            Show Trail: false
            Value: true
          link_gripper_finger_left:
            Alpha: 1
            Show Axes: false
            Show Trail: false
            Value: true
          link_gripper_finger_right:
            Alpha: 1
            Show Axes: false
            Show Trail: false
            Value: true
          link_gripper_fingertip_left:
            Alpha: 1
            Show Axes: false
            Show Trail: false
            Value: true
          link_gripper_fingertip_right:
            Alpha: 1
            Show Axes: false
            Show Trail: false
            Value: true
          link_head:
            Alpha: 1
            Show Axes: false
            Show Trail: false
            Value: true
          link_head_pan:
            Alpha: 1
            Show Axes: false
            Show Trail: false
            Value: true
          link_head_tilt:
            Alpha: 1
            Show Axes: false
            Show Trail: false
            Value: true
          link_left_wheel:
            Alpha: 1
            Show Axes: false
            Show Trail: false
            Value: true
          link_lift:
            Alpha: 1
            Show Axes: false
            Show Trail: false
            Value: true
          link_mast:
            Alpha: 1
            Show Axes: false
            Show Trail: false
            Value: true
          link_right_wheel:
            Alpha: 1
            Show Axes: false
            Show Trail: false
            Value: true
          link_wrist_yaw:
            Alpha: 1
            Show Axes: false
            Show Trail: false
            Value: true
          respeaker_base:
            Alpha: 1
            Show Axes: false
            Show Trail: false
            Value: true
        Loop Animation: false
        Robot Alpha: 0.5
        Robot Color: 150; 50; 150
        Show Robot Collision: false
        Show Robot Visual: true
        Show Trail: false
        State Display Time: 0.05 s
        Trail Step Size: 1
        Trajectory Topic: move_group/display_planned_path
      Planning Metrics:
        Payload: 1
        Show Joint Torques: false
        Show Manipulability: false
        Show Manipulability Index: false
        Show Weight Limit: false
        TextHeight: 0.07999999821186066
      Planning Request:
        Colliding Link Color: 255; 0; 0
        Goal State Alpha: 1
        Goal State Color: 250; 128; 0
        Interactive Marker Size: 0
        Joint Violation Color: 255; 0; 255
        Planning Group: stretch_arm
        Query Goal State: true
        Query Start State: false
        Show Workspace: false
        Start State Alpha: 1
        Start State Color: 0; 255; 0
      Planning Scene Topic: move_group/monitored_planning_scene
      Robot Description: robot_description
      Scene Geometry:
        Scene Alpha: 1
        Scene Color: 50; 230; 50
        Scene Display Time: 0.20000000298023224
        Show Scene Geometry: true
        Voxel Coloring: Z-Axis
        Voxel Rendering: Occupied Voxels
      Scene Robot:
        Attached Body Color: 150; 50; 150
        Links:
          All Links Enabled: true
          Expand Joint Details: false
          Expand Link Details: false
          Expand Tree: false
          Link Tree Style: Links in Alphabetic Order
          base_link:
            Alpha: 1
            Show Axes: false
            Show Trail: false
            Value: true
          camera_accel_frame:
            Alpha: 1
            Show Axes: false
            Show Trail: false
          camera_accel_optical_frame:
            Alpha: 1
            Show Axes: false
            Show Trail: false
          camera_bottom_screw_frame:
            Alpha: 1
            Show Axes: false
            Show Trail: false
          camera_color_frame:
            Alpha: 1
            Show Axes: false
            Show Trail: false
          camera_color_optical_frame:
            Alpha: 1
            Show Axes: false
            Show Trail: false
          camera_depth_frame:
            Alpha: 1
            Show Axes: false
            Show Trail: false
          camera_depth_optical_frame:
            Alpha: 1
            Show Axes: false
            Show Trail: false
          camera_gyro_frame:
            Alpha: 1
            Show Axes: false
            Show Trail: false
          camera_gyro_optical_frame:
            Alpha: 1
            Show Axes: false
            Show Trail: false
          camera_infra1_frame:
            Alpha: 1
            Show Axes: false
            Show Trail: false
          camera_infra1_optical_frame:
            Alpha: 1
            Show Axes: false
            Show Trail: false
          camera_infra2_frame:
            Alpha: 1
            Show Axes: false
            Show Trail: false
          camera_infra2_optical_frame:
            Alpha: 1
            Show Axes: false
            Show Trail: false
          camera_link:
            Alpha: 1
            Show Axes: false
            Show Trail: false
            Value: true
          caster_link:
            Alpha: 1
            Show Axes: false
            Show Trail: false
            Value: true
          laser:
            Alpha: 1
            Show Axes: false
            Show Trail: false
            Value: true
          link_arm_l0:
            Alpha: 1
            Show Axes: false
            Show Trail: false
            Value: true
          link_arm_l1:
            Alpha: 1
            Show Axes: false
            Show Trail: false
            Value: true
          link_arm_l2:
            Alpha: 1
            Show Axes: false
            Show Trail: false
            Value: true
          link_arm_l3:
            Alpha: 1
            Show Axes: false
            Show Trail: false
            Value: true
          link_arm_l4:
            Alpha: 1
            Show Axes: false
            Show Trail: false
            Value: true
          link_aruco_inner_wrist:
            Alpha: 1
            Show Axes: false
            Show Trail: false
            Value: true
          link_aruco_left_base:
            Alpha: 1
            Show Axes: false
            Show Trail: false
            Value: true
          link_aruco_right_base:
            Alpha: 1
            Show Axes: false
            Show Trail: false
            Value: true
          link_aruco_shoulder:
            Alpha: 1
            Show Axes: false
            Show Trail: false
            Value: true
          link_aruco_top_wrist:
            Alpha: 1
            Show Axes: false
            Show Trail: false
            Value: true
          link_grasp_center:
            Alpha: 1
            Show Axes: false
            Show Trail: false
          link_gripper:
            Alpha: 1
            Show Axes: false
            Show Trail: false
            Value: true
          link_gripper_finger_left:
            Alpha: 1
            Show Axes: false
            Show Trail: false
            Value: true
          link_gripper_finger_right:
            Alpha: 1
            Show Axes: false
            Show Trail: false
            Value: true
          link_gripper_fingertip_left:
            Alpha: 1
            Show Axes: false
            Show Trail: false
            Value: true
          link_gripper_fingertip_right:
            Alpha: 1
            Show Axes: false
            Show Trail: false
            Value: true
          link_head:
            Alpha: 1
            Show Axes: false
            Show Trail: false
            Value: true
          link_head_pan:
            Alpha: 1
            Show Axes: false
            Show Trail: false
            Value: true
          link_head_tilt:
            Alpha: 1
            Show Axes: false
            Show Trail: false
            Value: true
          link_left_wheel:
            Alpha: 1
            Show Axes: false
            Show Trail: false
            Value: true
          link_lift:
            Alpha: 1
            Show Axes: false
            Show Trail: false
            Value: true
          link_mast:
            Alpha: 1
            Show Axes: false
            Show Trail: false
            Value: true
          link_right_wheel:
            Alpha: 1
            Show Axes: false
            Show Trail: false
            Value: true
          link_wrist_yaw:
            Alpha: 1
            Show Axes: false
            Show Trail: false
            Value: true
          respeaker_base:
            Alpha: 1
            Show Axes: false
            Show Trail: false
            Value: true
        Robot Alpha: 0.5
        Show Robot Collision: false
        Show Robot Visual: true
      Value: true
      Velocity_Scaling_Factor: 1
  Enabled: true
  Global Options:
    Background Color: 48; 48; 48
    Default Light: true
    Fixed Frame: base_link
    Frame Rate: 30
  Name: root
  Tools:
    - Class: rviz/Interact
      Hide Inactive Objects: true
    - Class: rviz/MoveCamera
    - Class: rviz/Select
  Value: true
  Views:
    Current:
      Class: rviz/XYOrbit
      Distance: 2.5615234375
      Enable Stereo Rendering:
        Stereo Eye Separation: 0.05999999865889549
        Stereo Focal Distance: 1
        Swap Stereo Eyes: false
        Value: false
      Focal Point:
        X: 0.11356700211763382
        Y: 0.10592000186443329
        Z: 2.2351800055275817e-7
      Focal Shape Fixed Size: true
      Focal Shape Size: 0.05000000074505806
      Invert Z Axis: false
      Name: Current View
      Near Clip Distance: 0.009999999776482582
      Pitch: 0.5697969794273376
      Target Frame: base_link
      Value: XYOrbit (rviz)
      Yaw: 5.364950656890869
    Saved: ~
 Window Geometry:
  Displays:
    collapsed: false
  Height: 1052
  Help:
    collapsed: false
  Hide Left Dock: false
  Hide Right Dock: false
  MotionPlanning:
    collapsed: false
  MotionPlanning - Trajectory Slider:
    collapsed: false
  QMainWindow State: 000000ff00000000fd0000000100000000000002a2000003c2fc0200000007fb000000100044006900730070006c006100790073010000003d000001f0000000c900fffffffb0000000800480065006c00700000000342000000bb0000006e00fffffffb0000000a0056006900650077007300000003b0000000b0000000a400fffffffb0000000c00430061006d00650072006100000002ff000001610000000000000000fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000374000001890000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000004100fffffffb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e00670100000233000001cc0000017d00ffffff000004d8000003c200000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
  Views:
    collapsed: false
  Width: 1920
  X: 1920
  Y: 0
--- a/stretch_moveit_config/launch/moveit_rviz.launch
+++ b/stretch_moveit_config/launch/moveit_rviz.launch
@ -0,0 +1,15 @@
 <launch>

  <arg name="debug" default="false" />
  <arg unless="$(arg debug)" name="launch_prefix" value="" />
  <arg     if="$(arg debug)" name="launch_prefix" value="gdb --ex run --args" />

  <arg name="rviz_config" default="$(find stretch_moveit_config)/launch/moveit.launch" />
  <arg     if="$(eval rviz_config=='')" name="command_args" value="" />
  <arg unless="$(eval rviz_config=='')" name="command_args" value="-d $(arg rviz_config)" />

  <node name="$(anon rviz)" launch-prefix="$(arg launch_prefix)" pkg="rviz" type="rviz" respawn="false"
        args="$(arg command_args)" output="screen">
  </node>

 </launch>
--- a/stretch_moveit_config/launch/ompl_planning_pipeline.launch.xml
+++ b/stretch_moveit_config/launch/ompl_planning_pipeline.launch.xml
@ -0,0 +1,24 @@
 <launch>

  <!-- OMPL Plugin for MoveIt -->
  <arg name="planning_plugin" value="ompl_interface/OMPLPlanner" />

  <!-- The request adapters (plugins) used when planning with OMPL.
       ORDER MATTERS -->
  <arg name="planning_adapters"
       value="default_planner_request_adapters/AddTimeParameterization
              default_planner_request_adapters/FixWorkspaceBounds
              default_planner_request_adapters/FixStartStateBounds
              default_planner_request_adapters/FixStartStateCollision
              default_planner_request_adapters/FixStartStatePathConstraints"
              />

  <arg name="start_state_max_bounds_error" value="0.1" />

  <param name="planning_plugin" value="$(arg planning_plugin)" />
  <param name="request_adapters" value="$(arg planning_adapters)" />
  <param name="start_state_max_bounds_error" value="$(arg start_state_max_bounds_error)" />

  <rosparam command="load" file="$(find stretch_moveit_config)/config/ompl_planning.yaml"/>

 </launch>
--- a/stretch_moveit_config/launch/planning_context.launch
+++ b/stretch_moveit_config/launch/planning_context.launch
@ -0,0 +1,25 @@
 <launch>
  <!-- By default we do not overwrite the URDF. Change the following to true to change the default behavior -->
  <arg name="load_robot_description" default="false"/>

  <!-- The name of the parameter under which the URDF is loaded -->
  <arg name="robot_description" default="robot_description"/>

  <!-- Load universal robot description format (URDF) -->
  <param if="$(arg load_robot_description)" name="$(arg robot_description)" command="xacro  '$(find stretch_description)/urdf/stretch_description.xacro'"/>

  <!-- The semantic description that corresponds to the URDF -->
  <param name="$(arg robot_description)_semantic" textfile="$(find stretch_moveit_config)/config/stretch_description.srdf" />

  <!-- Load updated joint limits (override information from URDF) -->
  <group ns="$(arg robot_description)_planning">
    <rosparam command="load" file="$(find stretch_moveit_config)/config/joint_limits.yaml"/>
  </group>

  <!-- Load default settings for kinematics; these settings are overridden by settings in a node's namespace -->
  <group ns="$(arg robot_description)_kinematics">
    <rosparam command="load" file="$(find stretch_moveit_config)/config/kinematics.yaml"/>

  </group>

 </launch>
--- a/stretch_moveit_config/launch/planning_pipeline.launch.xml
+++ b/stretch_moveit_config/launch/planning_pipeline.launch.xml
@ -0,0 +1,10 @@
 <launch>

  <!-- This file makes it easy to include different planning pipelines;
       It is assumed that all planning pipelines are named XXX_planning_pipeline.launch  -->

  <arg name="pipeline" default="ompl" />

  <include file="$(find stretch_moveit_config)/launch/$(arg pipeline)_planning_pipeline.launch.xml" />

 </launch>
--- a/stretch_moveit_config/launch/ros_controllers.launch
+++ b/stretch_moveit_config/launch/ros_controllers.launch
@ -0,0 +1,11 @@
 <?xml version="1.0"?>
 <launch>

  <!-- Load joint controller configurations from YAML file to parameter server -->
  <rosparam file="$(find stretch_moveit_config)/config/ros_controllers.yaml" command="load"/>

  <!-- Load the controllers -->
  <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false"
    output="screen" args=""/>

 </launch>
--- a/stretch_moveit_config/launch/run_benchmark_ompl.launch
+++ b/stretch_moveit_config/launch/run_benchmark_ompl.launch
@ -0,0 +1,21 @@
 <launch>

  <!-- This argument must specify the list of .cfg files to process for benchmarking -->
  <arg name="cfg" />

  <!-- Load URDF -->
  <include file="$(find stretch_moveit_config)/launch/planning_context.launch">
    <arg name="load_robot_description" value="true"/>
  </include>

  <!-- Start the database -->
  <include file="$(find stretch_moveit_config)/launch/warehouse.launch">
    <arg name="moveit_warehouse_database_path" value="moveit_ompl_benchmark_warehouse"/>
  </include>

  <!-- Start Benchmark Executable -->
  <node name="$(anon moveit_benchmark)" pkg="moveit_ros_benchmarks" type="moveit_run_benchmark" args="$(arg cfg) --benchmark-planners" respawn="false" output="screen">
    <rosparam command="load" file="$(find stretch_moveit_config)/config/ompl_planning.yaml"/>
  </node>

 </launch>
--- a/stretch_moveit_config/launch/sensor_manager.launch.xml
+++ b/stretch_moveit_config/launch/sensor_manager.launch.xml
@ -0,0 +1,17 @@
 <launch>

  <!-- This file makes it easy to include the settings for sensor managers -->

  <!-- Params for 3D sensors config -->
  <rosparam command="load" file="$(find stretch_moveit_config)/config/sensors_3d.yaml" />

  <!-- Params for the octomap monitor -->
  <!--  <param name="octomap_frame" type="string" value="some frame in which the robot moves" /> -->
  <param name="octomap_resolution" type="double" value="0.025" />
  <param name="max_range" type="double" value="5.0" />

  <!-- Load the robot specific sensor manager; this sets the moveit_sensor_manager ROS parameter -->
  <arg name="moveit_sensor_manager" default="stretch_description" />
  <include file="$(find stretch_moveit_config)/launch/$(arg moveit_sensor_manager)_moveit_sensor_manager.launch.xml" />

 </launch>
--- a/stretch_moveit_config/launch/setup_assistant.launch
+++ b/stretch_moveit_config/launch/setup_assistant.launch
@ -0,0 +1,15 @@
 <!-- Re-launch the MoveIt Setup Assistant with this configuration package already loaded -->
 <launch>

  <!-- Debug Info -->
  <arg name="debug" default="false" />
  <arg unless="$(arg debug)" name="launch_prefix" value="" />
  <arg     if="$(arg debug)" name="launch_prefix" value="gdb --ex run --args" />

  <!-- Run -->
  <node pkg="moveit_setup_assistant" type="moveit_setup_assistant" name="moveit_setup_assistant"
 	args="--config_pkg=stretch_moveit_config"
 	launch-prefix="$(arg launch_prefix)"
        output="screen" />

 </launch>
--- a/stretch_moveit_config/launch/stretch_description_moveit_controller_manager.launch.xml
+++ b/stretch_moveit_config/launch/stretch_description_moveit_controller_manager.launch.xml
@ -0,0 +1,10 @@
 <launch>

  <!-- loads moveit_controller_manager on the parameter server which is taken as argument 
    if no argument is passed, moveit_simple_controller_manager will be set -->
  <arg name="moveit_controller_manager" default="moveit_simple_controller_manager/MoveItSimpleControllerManager" />
  <param name="moveit_controller_manager" value="$(arg moveit_controller_manager)"/>

  <!-- loads ros_controllers to the param server -->
  <rosparam file="$(find stretch_moveit_config)/config/ros_controllers.yaml"/>
 </launch>
--- a/stretch_moveit_config/launch/stretch_description_moveit_sensor_manager.launch.xml
+++ b/stretch_moveit_config/launch/stretch_description_moveit_sensor_manager.launch.xml
@ -0,0 +1,3 @@
 <launch>

 </launch>
--- a/stretch_moveit_config/launch/trajectory_execution.launch.xml
+++ b/stretch_moveit_config/launch/trajectory_execution.launch.xml
@ -0,0 +1,20 @@
 <launch>

  <!-- This file makes it easy to include the settings for trajectory execution  -->

  <!-- Flag indicating whether MoveIt is allowed to load/unload  or switch controllers -->
  <arg name="moveit_manage_controllers" default="true"/>
  <param name="moveit_manage_controllers" value="$(arg moveit_manage_controllers)"/>

  <!-- When determining the expected duration of a trajectory, this multiplicative factor is applied to get the allowed duration of execution -->
  <param name="trajectory_execution/allowed_execution_duration_scaling" value="1.2"/> <!-- default 1.2 -->
  <!-- Allow more than the expected execution time before triggering a trajectory cancel (applied after scaling) -->
  <param name="trajectory_execution/allowed_goal_duration_margin" value="0.5"/> <!-- default 0.5 -->
  <!-- Allowed joint-value tolerance for validation that trajectory's first point matches current robot state -->
  <param name="trajectory_execution/allowed_start_tolerance" value="0.01"/> <!-- default 0.01 -->

  <!-- Load the robot specific controller manager; this sets the moveit_controller_manager ROS parameter -->
  <arg name="moveit_controller_manager" default="stretch_description" />
  <include file="$(find stretch_moveit_config)/launch/$(arg moveit_controller_manager)_moveit_controller_manager.launch.xml" />

 </launch>
--- a/stretch_moveit_config/launch/warehouse.launch
+++ b/stretch_moveit_config/launch/warehouse.launch
@ -0,0 +1,15 @@
 <launch>

  <!-- The path to the database must be specified -->
  <arg name="moveit_warehouse_database_path" />

  <!-- Load warehouse parameters -->
  <include file="$(find stretch_moveit_config)/launch/warehouse_settings.launch.xml" />

  <!-- Run the DB server -->
  <node name="$(anon mongo_wrapper_ros)" cwd="ROS_HOME" type="mongo_wrapper_ros.py" pkg="warehouse_ros_mongo">
    <param name="overwrite" value="false"/>
    <param name="database_path" value="$(arg moveit_warehouse_database_path)" />
  </node>

 </launch>
--- a/stretch_moveit_config/launch/warehouse_settings.launch.xml
+++ b/stretch_moveit_config/launch/warehouse_settings.launch.xml
@ -0,0 +1,16 @@
 <launch>
  <!-- Set the parameters for the warehouse and run the mongodb server. -->

  <!-- The default DB port for moveit (not default MongoDB port to avoid potential conflicts) -->
  <arg name="moveit_warehouse_port" default="33829" />

  <!-- The default DB host for moveit -->
  <arg name="moveit_warehouse_host" default="localhost" />

  <!-- Set parameters for the warehouse -->
  <param name="warehouse_port" value="$(arg moveit_warehouse_port)"/>
  <param name="warehouse_host" value="$(arg moveit_warehouse_host)"/>
  <param name="warehouse_exec" value="mongod" />
  <param name="warehouse_plugin" value="warehouse_ros_mongo::MongoDatabaseConnection" />

 </launch>
--- a/stretch_moveit_config/package.xml
+++ b/stretch_moveit_config/package.xml
@ -0,0 +1,33 @@
 <package>

  <name>stretch_moveit_config</name>
  <version>0.3.0</version>
  <description>
     An automatically generated package with all the configuration and launch files for using the stretch_description with the MoveIt Motion Planning Framework
  </description>
  <author email="davidvlu@gmail.com">David V. Lu!!</author>
  <maintainer email="davidvlu@gmail.com">David V. Lu!!</maintainer>

  <license>BSD</license>

  <url type="website">http://moveit.ros.org/</url>
  <url type="bugtracker">https://github.com/ros-planning/moveit/issues</url>
  <url type="repository">https://github.com/ros-planning/moveit</url>

  <buildtool_depend>catkin</buildtool_depend>

  <run_depend>joint_state_publisher</run_depend>
  <run_depend>joint_state_publisher_gui</run_depend>
  <run_depend>moveit_fake_controller_manager</run_depend>
  <run_depend>moveit_kinematics</run_depend>
  <run_depend>moveit_planners_ompl</run_depend>
  <run_depend>moveit_ros_move_group</run_depend>
  <run_depend>moveit_ros_visualization</run_depend>
  <run_depend>moveit_setup_assistant</run_depend>
  <run_depend>robot_state_publisher</run_depend>
  <run_depend>stretch_description</run_depend>
  <run_depend>tf2_ros</run_depend>
  <run_depend>xacro</run_depend>
  <!-- This package is referenced in the warehouse launch files, but does not build out of the box at the moment. Commented the dependency until this works. -->
  <!-- <run_depend>warehouse_ros_mongo</run_depend> -->
 </package>