diff --git a/.gitattributes b/.gitattributes
new file mode 100644
index 0000000..5605974
--- /dev/null
+++ b/.gitattributes
@@ -0,0 +1,33 @@
+# LFS configuration
+*.bmp filter=lfs merge=lfs -text
+*.BMP filter=lfs merge=lfs -text
+*.png filter=lfs merge=lfs -text
+*.PNG filter=lfs merge=lfs -text
+*.pdf filter=lfs merge=lfs -text
+*.PDF filter=lfs merge=lfs -text
+*.stl filter=lfs merge=lfs -text
+*.STL filter=lfs merge=lfs -text
+*.jpg filter=lfs merge=lfs -text
+*.JPG filter=lfs merge=lfs -text
+*.zip filter=lfs merge=lfs -text
+*.ZIP filter=lfs merge=lfs -text
+*.tar.gz filter=lfs merge=lfs -text
+*.TAR.GZ filter=lfs merge=lfs -text
+*.ompldb filter=lfs merge=lfs -text
+*.OMPLDB filter=lfs merge=lfs -text
+*.npy filter=lfs merge=lfs -text
+*.NPY filter=lfs merge=lfs -text
+*.a filter=lfs merge=lfs -text
+*.pcd filter=lfs merge=lfs -text
+*.PCD filter=lfs merge=lfs -text
+*.ply filter=lfs merge=lfs -text
+*.PLY filter=lfs merge=lfs -text
+*.svg filter=lfs merge=lfs -text
+*.SVG filter=lfs merge=lfs -text
+*.dae filter=lfs merge=lfs -text
+*.DAE filter=lfs merge=lfs -text
+*.blend filter=lfs merge=lfs -text
+*.BLEND filter=lfs merge=lfs -text
+*.gif filter=lfs merge=lfs -text
+*.GIF filter=lfs merge=lfs -text
+.dae filter=lfs diff=lfs merge=lfs -text
diff --git a/src/stretch_re1_pro_config/CMakeLists.txt b/src/stretch_re1_pro_config/CMakeLists.txt
index fcfdbf5..83944b0 100644
--- a/src/stretch_re1_pro_config/CMakeLists.txt
+++ b/src/stretch_re1_pro_config/CMakeLists.txt
@@ -8,6 +8,7 @@ install(
     config
     description
     launch
+    meshes
     objectives
     waypoints
   DESTINATION
diff --git a/src/stretch_re1_pro_config/description/stretch.xacro b/src/stretch_re1_pro_config/description/stretch.xacro
index 0abe177..7bf969f 100644
--- a/src/stretch_re1_pro_config/description/stretch.xacro
+++ b/src/stretch_re1_pro_config/description/stretch.xacro
@@ -2,19 +2,178 @@
 <robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="stretch_description">
     <xacro:arg name="use_fake_controller" default="false"/>
 
+    <link name="world"/>
+    <link name="odom"/>
+
+    <joint name="world_to_odom" type="fixed">
+        <parent link="world"/>
+        <child link="odom"/>
+    </joint>
+
+    <joint name="position" type="planar">
+        <origin rpy="0 0 0" xyz="0 0 0"/>
+        <parent link="odom"/>
+        <child link="base_link"/>
+    </joint>
+
+    <link name="coffee_shop">
+        <visual>
+            <origin rpy="0 0 0" xyz="0 0 0"/>
+            <geometry>
+                <mesh filename="file://$(find stretch_re1_pro_config)/meshes/coffee_shop/coffee_shop.dae"/>
+            </geometry>
+        </visual>
+    </link>
+
+    <joint name="coffer_shop_joint" type="fixed">
+        <origin rpy="0 0 0" xyz="0 0 0"/>
+        <parent link="world"/>
+        <child link="coffee_shop"/>
+    </joint>
+
+    <link name="table_1">
+        <collision>
+            <geometry>
+                <cylinder length="1.08" radius="0.872821"/>
+            </geometry>
+        </collision>
+    </link>
+
+    <joint name="table_1_joint" type="fixed">
+        <origin rpy="0 0 0" xyz="-1.78826  2.16121  0.527411"/>
+        <parent link="coffee_shop"/>
+        <child link="table_1"/>
+    </joint>
+
+    <link name="counter">
+        <collision>
+            <geometry>
+                <box size="2.25864 4.70984 1.10145"/>
+            </geometry>
+        </collision>
+    </link>
+
+    <joint name="counter_joint" type="fixed">
+        <origin rpy="0 0 0" xyz="-1.70269  7.15  0.32"/>
+        <parent link="coffee_shop"/>
+        <child link="counter"/>
+    </joint>
+
+    <link name="seating">
+        <collision>
+            <geometry>
+                <box size="1.667808 8.85682 1.10145"/>
+            </geometry>
+        </collision>
+    </link>
+
+    <joint name="seating_joint" type="fixed">
+        <origin rpy="0 0 0" xyz="1.7163 6.25859 0.534589"/>
+        <parent link="coffee_shop"/>
+        <child link="seating"/>
+    </joint>
+
+    <link name="table_2">
+        <collision>
+            <geometry>
+                <box size="1.659276 0.777632 1.10145"/>
+            </geometry>
+        </collision>
+    </link>
+
+    <joint name="table_2_joint" type="fixed">
+        <origin rpy="0 0 0" xyz="1.80086  -0.227837 0.214425"/>
+        <parent link="coffee_shop"/>
+        <child link="table_2"/>
+    </joint>
+
+    <link name="chair">
+        <collision>
+            <geometry>
+                <box size="0.880082 1.182248 1.10145"/>
+            </geometry>
+        </collision>
+    </link>
+
+    <joint name="chair_joint" type="fixed">
+        <origin rpy="0 0 0" xyz="-2.40742  0.120353 0.534589"/>
+        <parent link="coffee_shop"/>
+        <child link="chair"/>
+    </joint>
+
+    <link name="wall_1">
+        <collision>
+            <geometry>
+                <box size="5.98744 1.182248 3.51658"/>
+            </geometry>
+        </collision>
+    </link>
+
+    <joint name="wall_1_joint" type="fixed">
+        <origin rpy="0 0 0" xyz="-0.179149 -1.3163 1.74082"/>
+        <parent link="coffee_shop"/>
+        <child link="wall_1"/>
+    </joint>
+
+    <link name="wall_2">
+        <collision>
+            <geometry>
+                <box size="5.98744 1.182248 3.51658"/>
+            </geometry>
+        </collision>
+    </link>
+
+    <joint name="wall_2_joint" type="fixed">
+        <origin rpy="0 0 0" xyz="-0.179149 11.3328 1.74082"/>
+        <parent link="coffee_shop"/>
+        <child link="wall_2"/>
+    </joint>
+
+    <link name="wall_3">
+        <collision>
+            <geometry>
+                <box size="1.182248 12.84594 3.51658"/>
+            </geometry>
+        </collision>
+    </link>
+
+    <joint name="wall_3_joint" type="fixed">
+        <origin rpy="0 0 0" xyz="3.42022 4.99722 1.74082"/>
+        <parent link="coffee_shop"/>
+        <child link="wall_3"/>
+    </joint>
+
+    <link name="wall_4">
+        <collision>
+            <geometry>
+                <box size="1.182248 12.84594 3.51658"/>
+            </geometry>
+        </collision>
+    </link>
+
+    <joint name="wall_4_joint" type="fixed">
+        <origin rpy="0 0 0" xyz="-3.78447 4.99722 1.74082"/>
+        <parent link="coffee_shop"/>
+        <child link="wall_4"/>
+    </joint>
+
+
     <!-- Import stretch urdf file -->
-    <xacro:include filename="$(find stretch_description)/urdf/stretch_description.xacro" />
+    <xacro:include filename="$(find stretch_description)/urdf/stretch_description.xacro"/>
+
 
     <xacro:if value="$(arg use_fake_controller)">
         <!-- Import stretch ros2_control description -->
-        <xacro:include filename="stretch_arm.ros2_control.xacro" />
-        <xacro:include filename="stretch_head.ros2_control.xacro" />
-        <xacro:include filename="gripper.ros2_control.xacro" />
-        <xacro:include filename="stretch_base.ros2_control.xacro" />
-
-        <xacro:stretch_arm_ros2_control name="StretchArmFakeJointDriver" />
-        <xacro:stretch_head_ros2_control name="StretchHeadFakeJointDriver" />
-        <xacro:stretch_base_ros2_control name="StretchBaseFakeJointDriver" />
-        <xacro:gripper_ros2_control name="StretchGripperFakeJointDriver" />
+        <xacro:include filename="stretch_arm.ros2_control.xacro"/>
+        <xacro:include filename="stretch_head.ros2_control.xacro"/>
+        <xacro:include filename="gripper.ros2_control.xacro"/>
+        <xacro:include filename="stretch_base.ros2_control.xacro"/>
+
+        <xacro:stretch_arm_ros2_control name="StretchArmFakeJointDriver"/>
+        <xacro:stretch_head_ros2_control name="StretchHeadFakeJointDriver"/>
+        <xacro:stretch_base_ros2_control name="StretchBaseFakeJointDriver"/>
+        <xacro:gripper_ros2_control name="StretchGripperFakeJointDriver"/>
     </xacro:if>
+
+
 </robot>
diff --git a/src/stretch_re1_pro_config/meshes/coffee_shop/09a5391d9df403266afbfbf5f42312aa.jpg b/src/stretch_re1_pro_config/meshes/coffee_shop/09a5391d9df403266afbfbf5f42312aa.jpg
new file mode 100644
index 0000000..c554a20
--- /dev/null
+++ b/src/stretch_re1_pro_config/meshes/coffee_shop/09a5391d9df403266afbfbf5f42312aa.jpg
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:5c17c198ac23309aefa4aae61959f3ae98287951872024b5c7d66ed72daf64ca
+size 31480
diff --git a/src/stretch_re1_pro_config/meshes/coffee_shop/AI52_001_Ceiling_Light_Diffuse-AI52_001_Ceiling_Light_Opacity.p.png b/src/stretch_re1_pro_config/meshes/coffee_shop/AI52_001_Ceiling_Light_Diffuse-AI52_001_Ceiling_Light_Opacity.p.png
new file mode 100644
index 0000000..6dae17f
--- /dev/null
+++ b/src/stretch_re1_pro_config/meshes/coffee_shop/AI52_001_Ceiling_Light_Diffuse-AI52_001_Ceiling_Light_Opacity.p.png
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:97b40891df1be67711938ee54a3c31e7a4dc2be0c9d68121a1aa8d47eefcec44
+size 9017725
diff --git a/src/stretch_re1_pro_config/meshes/coffee_shop/AI52_001_Ceiling_Light_Diffuse-AI52_001_Ceiling_Light_Opacity.png b/src/stretch_re1_pro_config/meshes/coffee_shop/AI52_001_Ceiling_Light_Diffuse-AI52_001_Ceiling_Light_Opacity.png
new file mode 100644
index 0000000..6dae17f
--- /dev/null
+++ b/src/stretch_re1_pro_config/meshes/coffee_shop/AI52_001_Ceiling_Light_Diffuse-AI52_001_Ceiling_Light_Opacity.png
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:97b40891df1be67711938ee54a3c31e7a4dc2be0c9d68121a1aa8d47eefcec44
+size 9017725
diff --git a/src/stretch_re1_pro_config/meshes/coffee_shop/AI52_003_Chair_V1_001_Diffuse.jpg b/src/stretch_re1_pro_config/meshes/coffee_shop/AI52_003_Chair_V1_001_Diffuse.jpg
new file mode 100644
index 0000000..51cbddb
--- /dev/null
+++ b/src/stretch_re1_pro_config/meshes/coffee_shop/AI52_003_Chair_V1_001_Diffuse.jpg
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:2a5794f0977e74447e824541bdf33919e1bd1acdeffddaea20693eff1e992c45
+size 271511
diff --git a/src/stretch_re1_pro_config/meshes/coffee_shop/AI52_003_ceiling_001_Diffuse.jpg b/src/stretch_re1_pro_config/meshes/coffee_shop/AI52_003_ceiling_001_Diffuse.jpg
new file mode 100644
index 0000000..3beba0b
--- /dev/null
+++ b/src/stretch_re1_pro_config/meshes/coffee_shop/AI52_003_ceiling_001_Diffuse.jpg
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:3c5d22dca9297c287cf3232f0ff6b074eda8368ee9dde980eacce44b619e539d
+size 49497
diff --git a/src/stretch_re1_pro_config/meshes/coffee_shop/AI52_003_shutters_V1_001_Diffuse.jpg b/src/stretch_re1_pro_config/meshes/coffee_shop/AI52_003_shutters_V1_001_Diffuse.jpg
new file mode 100644
index 0000000..50b1e37
--- /dev/null
+++ b/src/stretch_re1_pro_config/meshes/coffee_shop/AI52_003_shutters_V1_001_Diffuse.jpg
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:a818194511c48d1122db5358c39937bac89c5b1e33157d3645e9588c50d5348c
+size 278534
diff --git a/src/stretch_re1_pro_config/meshes/coffee_shop/AI52_003_sofa_V1_001_Diffuse.jpg b/src/stretch_re1_pro_config/meshes/coffee_shop/AI52_003_sofa_V1_001_Diffuse.jpg
new file mode 100644
index 0000000..60ba151
--- /dev/null
+++ b/src/stretch_re1_pro_config/meshes/coffee_shop/AI52_003_sofa_V1_001_Diffuse.jpg
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:c2e657305e7afb91859a6cf5650f7ee48022d4c4205003d5a713c20fe115e4cd
+size 278483
diff --git a/src/stretch_re1_pro_config/meshes/coffee_shop/AI52_003_table_V1_001_Diffuse.jpg b/src/stretch_re1_pro_config/meshes/coffee_shop/AI52_003_table_V1_001_Diffuse.jpg
new file mode 100644
index 0000000..17f056e
--- /dev/null
+++ b/src/stretch_re1_pro_config/meshes/coffee_shop/AI52_003_table_V1_001_Diffuse.jpg
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:222ce23be586f07ee70b295d40dfb8cf35f034f5e872499255ee6177b0ad6b4f
+size 425300
diff --git a/src/stretch_re1_pro_config/meshes/coffee_shop/AI52_003_table_V2_001_Diffuse.jpg b/src/stretch_re1_pro_config/meshes/coffee_shop/AI52_003_table_V2_001_Diffuse.jpg
new file mode 100644
index 0000000..b90d273
--- /dev/null
+++ b/src/stretch_re1_pro_config/meshes/coffee_shop/AI52_003_table_V2_001_Diffuse.jpg
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:c378363d489b37ca4f6b1ff7bcc8eb71b792a270dac4da4a4ded2416e36825c3
+size 426071
diff --git a/src/stretch_re1_pro_config/meshes/coffee_shop/AI52_003_wall_V1_001_Diffuse.jpg b/src/stretch_re1_pro_config/meshes/coffee_shop/AI52_003_wall_V1_001_Diffuse.jpg
new file mode 100644
index 0000000..95ea22f
--- /dev/null
+++ b/src/stretch_re1_pro_config/meshes/coffee_shop/AI52_003_wall_V1_001_Diffuse.jpg
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:da1992003166810177749dc9adf184e2076f5a49cf4cc8fdb73ba03c53d147a5
+size 267109
diff --git a/src/stretch_re1_pro_config/meshes/coffee_shop/AI52_008_Wall_Light_Diffuse-AI52_008_Wall_Light_Opacity.png b/src/stretch_re1_pro_config/meshes/coffee_shop/AI52_008_Wall_Light_Diffuse-AI52_008_Wall_Light_Opacity.png
new file mode 100644
index 0000000..5f9a0ab
--- /dev/null
+++ b/src/stretch_re1_pro_config/meshes/coffee_shop/AI52_008_Wall_Light_Diffuse-AI52_008_Wall_Light_Opacity.png
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:6ec207d558260e9dec8b5c51f0e6b5e3b2df86a38f322e5a9089005641bc34aa
+size 2608644
diff --git a/src/stretch_re1_pro_config/meshes/coffee_shop/AI52_008_Wood_Wall_Diffuse.jpg b/src/stretch_re1_pro_config/meshes/coffee_shop/AI52_008_Wood_Wall_Diffuse.jpg
new file mode 100644
index 0000000..f782a20
--- /dev/null
+++ b/src/stretch_re1_pro_config/meshes/coffee_shop/AI52_008_Wood_Wall_Diffuse.jpg
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:5bbb3e0bf2f19146c37cff0d0790e2ae4a69cd3f57423a53f06e13f3419b4524
+size 457745
diff --git a/src/stretch_re1_pro_config/meshes/coffee_shop/AI52_008_flower_diff_01.jpg b/src/stretch_re1_pro_config/meshes/coffee_shop/AI52_008_flower_diff_01.jpg
new file mode 100644
index 0000000..8f4565b
--- /dev/null
+++ b/src/stretch_re1_pro_config/meshes/coffee_shop/AI52_008_flower_diff_01.jpg
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:17b37956dc3e38ff3dc6434658d23d1bf97221f6a5107c0ab64d00e41455c9bd
+size 21718
diff --git a/src/stretch_re1_pro_config/meshes/coffee_shop/AI52_008_leaf_diff_01-AI52_008_leaf_opacity_01.png b/src/stretch_re1_pro_config/meshes/coffee_shop/AI52_008_leaf_diff_01-AI52_008_leaf_opacity_01.png
new file mode 100644
index 0000000..2bbe090
--- /dev/null
+++ b/src/stretch_re1_pro_config/meshes/coffee_shop/AI52_008_leaf_diff_01-AI52_008_leaf_opacity_01.png
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:b2a1e542ce4226fcf203d134388a01ac4ac97e1f7783a0f3154343cf527e3a7a
+size 583598
diff --git a/src/stretch_re1_pro_config/meshes/coffee_shop/AI52_008_petal_diff_02_png-AI52_008_petal_opacity_02_png.png b/src/stretch_re1_pro_config/meshes/coffee_shop/AI52_008_petal_diff_02_png-AI52_008_petal_opacity_02_png.png
new file mode 100644
index 0000000..aa66ee0
--- /dev/null
+++ b/src/stretch_re1_pro_config/meshes/coffee_shop/AI52_008_petal_diff_02_png-AI52_008_petal_opacity_02_png.png
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:90ea3969d5a2b4a8039987a08f89e61481a13413b5d24e400c7708ec1ea68dd2
+size 346945
diff --git a/src/stretch_re1_pro_config/meshes/coffee_shop/AI52_008_petal_diff_03-AI52_008_petal_opacity_01.png b/src/stretch_re1_pro_config/meshes/coffee_shop/AI52_008_petal_diff_03-AI52_008_petal_opacity_01.png
new file mode 100644
index 0000000..5f1c798
--- /dev/null
+++ b/src/stretch_re1_pro_config/meshes/coffee_shop/AI52_008_petal_diff_03-AI52_008_petal_opacity_01.png
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:f8d44525026af78bb9ac5c008dd62fcdd620ddd9beebd28fc8dc2b4f21a88e91
+size 438049
diff --git a/src/stretch_re1_pro_config/meshes/coffee_shop/AI52_008_steam_diff_01.jpg b/src/stretch_re1_pro_config/meshes/coffee_shop/AI52_008_steam_diff_01.jpg
new file mode 100644
index 0000000..f6952c1
--- /dev/null
+++ b/src/stretch_re1_pro_config/meshes/coffee_shop/AI52_008_steam_diff_01.jpg
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:ee2111b251da98c6762fd49f8d7c10c99c8c0fee8335191ad19b8c6aacc67d8f
+size 9207
diff --git a/src/stretch_re1_pro_config/meshes/coffee_shop/AI52_008_steam_diff_02.jpg b/src/stretch_re1_pro_config/meshes/coffee_shop/AI52_008_steam_diff_02.jpg
new file mode 100644
index 0000000..d5e5d17
--- /dev/null
+++ b/src/stretch_re1_pro_config/meshes/coffee_shop/AI52_008_steam_diff_02.jpg
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:babc90a12d9858b3b9eae2df8b1a3d916252d8bf3e0cc714d630bdef69ba3d02
+size 9215
diff --git a/src/stretch_re1_pro_config/meshes/coffee_shop/AI52_010_brick_V1_001_Diffuse.jpg b/src/stretch_re1_pro_config/meshes/coffee_shop/AI52_010_brick_V1_001_Diffuse.jpg
new file mode 100644
index 0000000..84c3734
--- /dev/null
+++ b/src/stretch_re1_pro_config/meshes/coffee_shop/AI52_010_brick_V1_001_Diffuse.jpg
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:1b414b465765456bbf45a7ebb5c7a33d8412e1e3b20bac1881f536c57bfeab87
+size 880982
diff --git a/src/stretch_re1_pro_config/meshes/coffee_shop/AI52_010_marble_Diffuse.jpg b/src/stretch_re1_pro_config/meshes/coffee_shop/AI52_010_marble_Diffuse.jpg
new file mode 100644
index 0000000..98a6037
--- /dev/null
+++ b/src/stretch_re1_pro_config/meshes/coffee_shop/AI52_010_marble_Diffuse.jpg
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:b3cde581bfd61a3163e6610aa8ca3ba8d61248ec4d71fc0f8f5ca65cc84025f7
+size 792796
diff --git a/src/stretch_re1_pro_config/meshes/coffee_shop/AI52_010_plant_V1_004_Diffuse.jpg b/src/stretch_re1_pro_config/meshes/coffee_shop/AI52_010_plant_V1_004_Diffuse.jpg
new file mode 100644
index 0000000..1ddecd5
--- /dev/null
+++ b/src/stretch_re1_pro_config/meshes/coffee_shop/AI52_010_plant_V1_004_Diffuse.jpg
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:0a4912ee87a071461093f70578ac704dccf7c5aefc2388bffb188b179f60aec1
+size 8579
diff --git a/src/stretch_re1_pro_config/meshes/coffee_shop/AI52_010_plant_V1_005_Diffuse.jpg b/src/stretch_re1_pro_config/meshes/coffee_shop/AI52_010_plant_V1_005_Diffuse.jpg
new file mode 100644
index 0000000..54bba72
--- /dev/null
+++ b/src/stretch_re1_pro_config/meshes/coffee_shop/AI52_010_plant_V1_005_Diffuse.jpg
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:4d3bdb621f6292abcf5aa80d133afe2830944aeb35f3c76ef7ff2da692d63a3d
+size 29381
diff --git a/src/stretch_re1_pro_config/meshes/coffee_shop/AI52_010_plant_V2_001_Diffuse.jpg b/src/stretch_re1_pro_config/meshes/coffee_shop/AI52_010_plant_V2_001_Diffuse.jpg
new file mode 100644
index 0000000..b070aa0
--- /dev/null
+++ b/src/stretch_re1_pro_config/meshes/coffee_shop/AI52_010_plant_V2_001_Diffuse.jpg
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:78d47f23bee5923ffbe592bc5deb7fdb1cecf7ec14427b1426b21ed45d61c955
+size 12826
diff --git a/src/stretch_re1_pro_config/meshes/coffee_shop/AI52_010_plant_V2_002_Diffuse.jpg b/src/stretch_re1_pro_config/meshes/coffee_shop/AI52_010_plant_V2_002_Diffuse.jpg
new file mode 100644
index 0000000..afad072
--- /dev/null
+++ b/src/stretch_re1_pro_config/meshes/coffee_shop/AI52_010_plant_V2_002_Diffuse.jpg
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:adad9aac6afac23af5a1c113b41b4f1019f58408f12ee8e34ea6abdc0db7668e
+size 26897
diff --git a/src/stretch_re1_pro_config/meshes/coffee_shop/AI52_010_stone_V1_001_Diffuse.jpg b/src/stretch_re1_pro_config/meshes/coffee_shop/AI52_010_stone_V1_001_Diffuse.jpg
new file mode 100644
index 0000000..149bb2f
--- /dev/null
+++ b/src/stretch_re1_pro_config/meshes/coffee_shop/AI52_010_stone_V1_001_Diffuse.jpg
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:77e6c7c379b7c4f3e6adb3a2f48f1ac3ef6013647ce71e8ff72841abc925938a
+size 4348
diff --git a/src/stretch_re1_pro_config/meshes/coffee_shop/AI52_010_stone_V1_002_Diffuse.jpg b/src/stretch_re1_pro_config/meshes/coffee_shop/AI52_010_stone_V1_002_Diffuse.jpg
new file mode 100644
index 0000000..65e733d
--- /dev/null
+++ b/src/stretch_re1_pro_config/meshes/coffee_shop/AI52_010_stone_V1_002_Diffuse.jpg
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:e622b20cdc4143daf6398c756edc049feeff698382de3bf077e0af95b2779dfd
+size 3823
diff --git a/src/stretch_re1_pro_config/meshes/coffee_shop/AI52_010_stone_V1_003_Diffuse.jpg b/src/stretch_re1_pro_config/meshes/coffee_shop/AI52_010_stone_V1_003_Diffuse.jpg
new file mode 100644
index 0000000..cf9403a
--- /dev/null
+++ b/src/stretch_re1_pro_config/meshes/coffee_shop/AI52_010_stone_V1_003_Diffuse.jpg
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:5e9faf6c19317455a9e71bedf9602e3616a6ad5111a74a191edd3115d0dc5981
+size 3520
diff --git a/src/stretch_re1_pro_config/meshes/coffee_shop/AI52_010_stone_V1_004_Diffuse.jpg b/src/stretch_re1_pro_config/meshes/coffee_shop/AI52_010_stone_V1_004_Diffuse.jpg
new file mode 100644
index 0000000..9498922
--- /dev/null
+++ b/src/stretch_re1_pro_config/meshes/coffee_shop/AI52_010_stone_V1_004_Diffuse.jpg
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:cca1b781f9b65e8c30af65a4799b9008a3bee2fac1e5316382a98b217af15502
+size 3321
diff --git a/src/stretch_re1_pro_config/meshes/coffee_shop/Architecture-pictures-I.jpg b/src/stretch_re1_pro_config/meshes/coffee_shop/Architecture-pictures-I.jpg
new file mode 100644
index 0000000..4e4f4cc
--- /dev/null
+++ b/src/stretch_re1_pro_config/meshes/coffee_shop/Architecture-pictures-I.jpg
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:81dbb0a0d14677f98b809d3f7cc5aadb1c75a9b368f66b57fb8bb1eb05a5564e
+size 259660
diff --git a/src/stretch_re1_pro_config/meshes/coffee_shop/Architecture.jpg b/src/stretch_re1_pro_config/meshes/coffee_shop/Architecture.jpg
new file mode 100644
index 0000000..02d9a62
--- /dev/null
+++ b/src/stretch_re1_pro_config/meshes/coffee_shop/Architecture.jpg
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:bb5bc4472c52845f5b0683afb090f31062268500ebb55024c504e7475f086602
+size 167293
diff --git a/src/stretch_re1_pro_config/meshes/coffee_shop/AstersKitchen_Marble.jpg b/src/stretch_re1_pro_config/meshes/coffee_shop/AstersKitchen_Marble.jpg
new file mode 100644
index 0000000..844ffc5
--- /dev/null
+++ b/src/stretch_re1_pro_config/meshes/coffee_shop/AstersKitchen_Marble.jpg
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:d5e467aa283f98577bbfb1b05512158fa5f1e4407e6e0accb1389fb35e460976
+size 324763
diff --git a/src/stretch_re1_pro_config/meshes/coffee_shop/BB Brnch 01.jpg b/src/stretch_re1_pro_config/meshes/coffee_shop/BB Brnch 01.jpg
new file mode 100644
index 0000000..49b1375
--- /dev/null
+++ b/src/stretch_re1_pro_config/meshes/coffee_shop/BB Brnch 01.jpg	
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:da04fe5b50dab8624c9a733c42ec900df25464ab52253189d0a824ce25de65a9
+size 13316
diff --git a/src/stretch_re1_pro_config/meshes/coffee_shop/Black see.jpg b/src/stretch_re1_pro_config/meshes/coffee_shop/Black see.jpg
new file mode 100644
index 0000000..45a297d
--- /dev/null
+++ b/src/stretch_re1_pro_config/meshes/coffee_shop/Black see.jpg	
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:1626b4ea58464282441a41b7674deb1fa8e98d1034e0b2d8bc458357a8045ba8
+size 629358
diff --git a/src/stretch_re1_pro_config/meshes/coffee_shop/Fabric_valvet_01_ambientocclusion.jpg b/src/stretch_re1_pro_config/meshes/coffee_shop/Fabric_valvet_01_ambientocclusion.jpg
new file mode 100644
index 0000000..2730681
--- /dev/null
+++ b/src/stretch_re1_pro_config/meshes/coffee_shop/Fabric_valvet_01_ambientocclusion.jpg
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:c3712569da81e7d4c3187274724e2e7f031b87e73b627ad5d602676510365a95
+size 1065514
diff --git a/src/stretch_re1_pro_config/meshes/coffee_shop/Forest.jpg b/src/stretch_re1_pro_config/meshes/coffee_shop/Forest.jpg
new file mode 100644
index 0000000..e3ee34d
--- /dev/null
+++ b/src/stretch_re1_pro_config/meshes/coffee_shop/Forest.jpg
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:250d33f791a398f814c027f3b94e1a759b9f3a93178479be631f7190536e0ade
+size 387865
diff --git a/src/stretch_re1_pro_config/meshes/coffee_shop/Institution 2.jpg b/src/stretch_re1_pro_config/meshes/coffee_shop/Institution 2.jpg
new file mode 100644
index 0000000..fc519a2
--- /dev/null
+++ b/src/stretch_re1_pro_config/meshes/coffee_shop/Institution 2.jpg	
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:11722101f49a38d0421d9b2e36c3ba7a84bf0e9356a0fc790830b90ae08a449e
+size 58286
diff --git a/src/stretch_re1_pro_config/meshes/coffee_shop/Institution.jpg b/src/stretch_re1_pro_config/meshes/coffee_shop/Institution.jpg
new file mode 100644
index 0000000..ca5c054
--- /dev/null
+++ b/src/stretch_re1_pro_config/meshes/coffee_shop/Institution.jpg
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:c792a36196dd6177c065b0b41d449ed0fddbe359fc6c500ea49a2d61a8cea5a4
+size 57163
diff --git a/src/stretch_re1_pro_config/meshes/coffee_shop/MetalBronzeRepolished001_REFL_4K_SPECULAR.jpg b/src/stretch_re1_pro_config/meshes/coffee_shop/MetalBronzeRepolished001_REFL_4K_SPECULAR.jpg
new file mode 100644
index 0000000..5824ba8
--- /dev/null
+++ b/src/stretch_re1_pro_config/meshes/coffee_shop/MetalBronzeRepolished001_REFL_4K_SPECULAR.jpg
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:3ae45e3c18cec9ed7a334b12aae7f86c38424441b27389e0a2119a41569ff85b
+size 1975399
diff --git a/src/stretch_re1_pro_config/meshes/coffee_shop/None_BaseColor.jpg b/src/stretch_re1_pro_config/meshes/coffee_shop/None_BaseColor.jpg
new file mode 100644
index 0000000..e485f12
--- /dev/null
+++ b/src/stretch_re1_pro_config/meshes/coffee_shop/None_BaseColor.jpg
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:f94201a1f0ecd7a8117b51873a024f45babff4dd1b6b188e6a13edc81ec497d5
+size 66601
diff --git a/src/stretch_re1_pro_config/meshes/coffee_shop/Old Tiles_Albedo.jpg b/src/stretch_re1_pro_config/meshes/coffee_shop/Old Tiles_Albedo.jpg
new file mode 100644
index 0000000..0f156ba
--- /dev/null
+++ b/src/stretch_re1_pro_config/meshes/coffee_shop/Old Tiles_Albedo.jpg	
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:d773832789dae1ea4b279c367e269ecdd43fbefc3265b4428d68704639f148b1
+size 301931
diff --git a/src/stretch_re1_pro_config/meshes/coffee_shop/Old parquet - Single Herringbone pattern_BaseColor.jpg b/src/stretch_re1_pro_config/meshes/coffee_shop/Old parquet - Single Herringbone pattern_BaseColor.jpg
new file mode 100644
index 0000000..a44c6c2
--- /dev/null
+++ b/src/stretch_re1_pro_config/meshes/coffee_shop/Old parquet - Single Herringbone pattern_BaseColor.jpg	
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:46d13b3610659072f6a154e8a900c354c0640b72fc9cfc534ba8570672bb9c81
+size 311174
diff --git a/src/stretch_re1_pro_config/meshes/coffee_shop/Pictures III - House.jpg b/src/stretch_re1_pro_config/meshes/coffee_shop/Pictures III - House.jpg
new file mode 100644
index 0000000..e5b8cd9
--- /dev/null
+++ b/src/stretch_re1_pro_config/meshes/coffee_shop/Pictures III - House.jpg	
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:2d2ec07eb334619ca31b39a735a6124da62fd1c1fb259812aa34285ddaaa92b8
+size 17786
diff --git a/src/stretch_re1_pro_config/meshes/coffee_shop/Porcelain002_2K_Color.jpg b/src/stretch_re1_pro_config/meshes/coffee_shop/Porcelain002_2K_Color.jpg
new file mode 100644
index 0000000..3f2f18d
--- /dev/null
+++ b/src/stretch_re1_pro_config/meshes/coffee_shop/Porcelain002_2K_Color.jpg
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:6abba973c4a1dff729de6031d14bda01fff7c3f810fc9934da58645833305e3a
+size 49498
diff --git a/src/stretch_re1_pro_config/meshes/coffee_shop/Sculpture.jpg b/src/stretch_re1_pro_config/meshes/coffee_shop/Sculpture.jpg
new file mode 100644
index 0000000..b404e1d
--- /dev/null
+++ b/src/stretch_re1_pro_config/meshes/coffee_shop/Sculpture.jpg
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:38448a708033f1ad24e004ba5bca139fa655d5baed4452c974b9020a02c01a55
+size 676473
diff --git a/src/stretch_re1_pro_config/meshes/coffee_shop/USA - Washigton.jpg b/src/stretch_re1_pro_config/meshes/coffee_shop/USA - Washigton.jpg
new file mode 100644
index 0000000..d66b625
--- /dev/null
+++ b/src/stretch_re1_pro_config/meshes/coffee_shop/USA - Washigton.jpg	
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:c52cfc6e08a9ab5534d3c914e6cb223f5cc247db22f5980deac40bab3bfcf078
+size 263590
diff --git a/src/stretch_re1_pro_config/meshes/coffee_shop/Wood051_2K_Color.jpg b/src/stretch_re1_pro_config/meshes/coffee_shop/Wood051_2K_Color.jpg
new file mode 100644
index 0000000..fca9c7c
--- /dev/null
+++ b/src/stretch_re1_pro_config/meshes/coffee_shop/Wood051_2K_Color.jpg
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:faa47385c259e4d169a26b6beaa241cc56344eea66a7c845363c868a0cfc6afb
+size 104532
diff --git a/src/stretch_re1_pro_config/meshes/coffee_shop/WoodFineDark003_COL_4K.jpg b/src/stretch_re1_pro_config/meshes/coffee_shop/WoodFineDark003_COL_4K.jpg
new file mode 100644
index 0000000..d64d934
--- /dev/null
+++ b/src/stretch_re1_pro_config/meshes/coffee_shop/WoodFineDark003_COL_4K.jpg
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:22f58bf2a626893fd2da91dc5a45dd56b0d8a542a9da3d38e20e9238f8012bfa
+size 421413
diff --git a/src/stretch_re1_pro_config/meshes/coffee_shop/coffee_shop.dae b/src/stretch_re1_pro_config/meshes/coffee_shop/coffee_shop.dae
new file mode 100644
index 0000000..52eb122
--- /dev/null
+++ b/src/stretch_re1_pro_config/meshes/coffee_shop/coffee_shop.dae
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:ad7d3116079ae2b89776eb03160a8ef05817ea501e12cd94104b3a3c0c3e84d0
+size 194208844
diff --git a/src/stretch_re1_pro_config/meshes/coffee_shop/fortress.jpg b/src/stretch_re1_pro_config/meshes/coffee_shop/fortress.jpg
new file mode 100644
index 0000000..0328809
--- /dev/null
+++ b/src/stretch_re1_pro_config/meshes/coffee_shop/fortress.jpg
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:f6a2d1db34282066a08eee9504467e6f6a227f49734e82d2e42ebf2c709a6ff2
+size 554425
diff --git a/src/stretch_re1_pro_config/meshes/coffee_shop/image_fly.jpg b/src/stretch_re1_pro_config/meshes/coffee_shop/image_fly.jpg
new file mode 100644
index 0000000..c06a73b
--- /dev/null
+++ b/src/stretch_re1_pro_config/meshes/coffee_shop/image_fly.jpg
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:3dfcab193b353969cd8702a64a096822e87e608e477271118eb8916530823bfe
+size 70268
diff --git a/src/stretch_re1_pro_config/meshes/coffee_shop/paper coffee cup base color.png b/src/stretch_re1_pro_config/meshes/coffee_shop/paper coffee cup base color.png
new file mode 100644
index 0000000..ca851fe
--- /dev/null
+++ b/src/stretch_re1_pro_config/meshes/coffee_shop/paper coffee cup base color.png	
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:55a69bccbe22cb242b1cf421fbdffeb86f85ad870e5e13ee26a1a3dfe8d628c8
+size 483631
diff --git a/src/stretch_re1_pro_config/meshes/coffee_shop/vai-ren.jpg b/src/stretch_re1_pro_config/meshes/coffee_shop/vai-ren.jpg
new file mode 100644
index 0000000..de0b830
--- /dev/null
+++ b/src/stretch_re1_pro_config/meshes/coffee_shop/vai-ren.jpg
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:95582638da8dac161664e471d335fa43306393abca33249e7b6ded4703d26715
+size 19978
diff --git a/src/stretch_re1_pro_config/objectives/move_to_joint_state.xml b/src/stretch_re1_pro_config/objectives/move_to_joint_state.xml
index d13a25c..68d0a4f 100644
--- a/src/stretch_re1_pro_config/objectives/move_to_joint_state.xml
+++ b/src/stretch_re1_pro_config/objectives/move_to_joint_state.xml
@@ -6,7 +6,7 @@
         <Action ID="RetrieveJointStateParameter" timeout_sec="-1" joint_state="{target_joint_state}"/>
         <Action ID="InitializeMTCTask" task_id="move_to_joint_state" controller_names="stretch_base_controller stretch_controller" task="{move_to_waypoint_task}"/>
         <Action ID="SetupMTCCurrentState" task="{move_to_waypoint_task}"/>
-	<Action ID="SetupMTCMoveToJointState" joint_state="{target_joint_state}" name="SetupMTCMoveToJointState_First" planning_group_name="position" task="{move_to_waypoint_task}" use_all_planners="false"/>
+        <Action ID="SetupMTCMoveToJointState" joint_state="{target_joint_state}" name="SetupMTCMoveToJointState_First" task="{move_to_waypoint_task}" use_all_planners="false" planning_group_name="mobile_base_arm"/>
         <Action ID="PlanMTCTask" solution="{move_to_waypoint_solution}" task="{move_to_waypoint_task}"/>
         <Action ID="ExecuteMTCTask" solution="{move_to_waypoint_solution}"/>
         <Action ID="PublishEmpty" topic="/studio_ui/motion_ended" queue_size="1" use_best_effort="false"/>
diff --git a/src/stretch_re1_pro_config/objectives/serve_coffee.xml b/src/stretch_re1_pro_config/objectives/serve_coffee.xml
new file mode 100644
index 0000000..238007a
--- /dev/null
+++ b/src/stretch_re1_pro_config/objectives/serve_coffee.xml
@@ -0,0 +1,19 @@
+<?xml version='1.0' encoding='UTF-8'?>
+<root BTCPP_format="4" main_tree_to_execute="Serve Coffee">
+  <!--//////////-->
+  <BehaviorTree ID="Serve Coffee" _description="" _favorite="true" _hardcoded="false">
+    <Control ID="Sequence" name="TopLevelSequence">
+      <Action ID="InitializeMTCTask" task="{mtc_task}" controller_names="stretch_base_controller stretch_controller"/>
+      <Action ID="SetupMTCCurrentState" task="{mtc_task}"/>
+      <Action ID="SetupMTCMoveToNamedState" task="{mtc_task}" use_all_planners="false" goal_state_name="closed" planning_group_name="gripper"/>
+      <Action ID="CreateStampedPose" stamped_pose="{target_pose}" reference_frame="coffee_shop" orientation_xyzw="0;0;0.707107;0.707107" position_xyz="-0.835354;4.90836;1.02"/>
+      <Action ID="SetupMTCMoveToPose" task="{mtc_task}" use_all_planners="false" target_pose="{target_pose}" planning_group_name="mobile_base_arm" ik_frame="link_grasp_center"/>
+      <Action ID="SetupMTCMoveToNamedState" task="{mtc_task}" use_all_planners="false" goal_state_name="closed" planning_group_name="gripper"/>
+      <Action ID="CreateStampedPose" stamped_pose="{target_pose_2}" reference_frame="coffee_shop" orientation_xyzw="0;0;-0.707107;0.707107" position_xyz="1.53989;0.073977;0.879106"/>
+      <Action ID="SetupMTCMoveToPose" task="{mtc_task}" use_all_planners="false" target_pose="{target_pose_2}" planning_group_name="mobile_base_arm" ik_frame="link_grasp_center"/>
+      <Action ID="SetupMTCMoveToNamedState" task="{mtc_task}" use_all_planners="false" goal_state_name="open" planning_group_name="gripper"/>
+      <Action ID="PlanMTCTask" solution="{mtc_solution}" task="{mtc_task}"/>
+      <Action ID="ExecuteMTCTask" solution="{mtc_solution}"/>
+    </Control>
+  </BehaviorTree>
+</root>