diff --git a/.gitmodules b/.gitmodules
index e69de29..d81d5e3 100644
--- a/.gitmodules
+++ b/.gitmodules
@@ -0,0 +1,6 @@
+[submodule "koopman-op"]
+ path = koopman-op
+ url = git@github.com:psu-hcr/koopman-op.git
+[submodule "iiwa_ros/lus-rosscripts"]
+ path = iiwa_ros/lus-rosscripts
+ url = git@github.com:psu-hcr/lus-rosscripts.git
diff --git a/iiwa_description/launch/iiwa15_upload.launch b/iiwa_description/launch/iiwa15_upload.launch
new file mode 100644
index 0000000..b33c272
--- /dev/null
+++ b/iiwa_description/launch/iiwa15_upload.launch
@@ -0,0 +1,37 @@
+
+
+
+
+
+
+
+
+
+
diff --git a/iiwa_description/launch/iiwa16_upload.launch b/iiwa_description/launch/iiwa16_upload.launch
new file mode 100644
index 0000000..73ff789
--- /dev/null
+++ b/iiwa_description/launch/iiwa16_upload.launch
@@ -0,0 +1,37 @@
+
+
+
+
+
+
+
+
+
+
diff --git a/iiwa_description/launch/iiwa17_upload.launch b/iiwa_description/launch/iiwa17_upload.launch
new file mode 100644
index 0000000..8501638
--- /dev/null
+++ b/iiwa_description/launch/iiwa17_upload.launch
@@ -0,0 +1,37 @@
+
+
+
+
+
+
+
+
+
+
diff --git a/iiwa_description/meshes/iiwa14/collision/Brush2.stl b/iiwa_description/meshes/iiwa14/collision/Brush2.stl
new file mode 100644
index 0000000..2e92227
Binary files /dev/null and b/iiwa_description/meshes/iiwa14/collision/Brush2.stl differ
diff --git a/iiwa_description/meshes/iiwa14/collision/BrushScaled.stl b/iiwa_description/meshes/iiwa14/collision/BrushScaled.stl
new file mode 100644
index 0000000..860017a
Binary files /dev/null and b/iiwa_description/meshes/iiwa14/collision/BrushScaled.stl differ
diff --git a/iiwa_description/meshes/iiwa14/collision/PegScaled.stl b/iiwa_description/meshes/iiwa14/collision/PegScaled.stl
new file mode 100644
index 0000000..211c1a7
Binary files /dev/null and b/iiwa_description/meshes/iiwa14/collision/PegScaled.stl differ
diff --git a/iiwa_description/meshes/iiwa14/visual/Brush2.stl b/iiwa_description/meshes/iiwa14/visual/Brush2.stl
new file mode 100644
index 0000000..2e92227
Binary files /dev/null and b/iiwa_description/meshes/iiwa14/visual/Brush2.stl differ
diff --git a/iiwa_description/meshes/iiwa14/visual/BrushScaled.stl b/iiwa_description/meshes/iiwa14/visual/BrushScaled.stl
new file mode 100644
index 0000000..860017a
Binary files /dev/null and b/iiwa_description/meshes/iiwa14/visual/BrushScaled.stl differ
diff --git a/iiwa_description/meshes/iiwa14/visual/PegScaled.stl b/iiwa_description/meshes/iiwa14/visual/PegScaled.stl
new file mode 100644
index 0000000..211c1a7
Binary files /dev/null and b/iiwa_description/meshes/iiwa14/visual/PegScaled.stl differ
diff --git a/iiwa_description/urdf/iiwa14.xacro b/iiwa_description/urdf/iiwa14.xacro
index b32fe88..a871f4d 100644
--- a/iiwa_description/urdf/iiwa14.xacro
+++ b/iiwa_description/urdf/iiwa14.xacro
@@ -386,7 +386,9 @@
-
+
+
+
@@ -399,6 +401,25 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/iiwa_description/urdf/iiwa15.urdf.xacro b/iiwa_description/urdf/iiwa15.urdf.xacro
new file mode 100644
index 0000000..979b6ac
--- /dev/null
+++ b/iiwa_description/urdf/iiwa15.urdf.xacro
@@ -0,0 +1,55 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/iiwa_description/urdf/iiwa15.xacro b/iiwa_description/urdf/iiwa15.xacro
new file mode 100644
index 0000000..bea3da1
--- /dev/null
+++ b/iiwa_description/urdf/iiwa15.xacro
@@ -0,0 +1,430 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/iiwa_description/urdf/iiwa16.urdf.xacro b/iiwa_description/urdf/iiwa16.urdf.xacro
new file mode 100644
index 0000000..ff02731
--- /dev/null
+++ b/iiwa_description/urdf/iiwa16.urdf.xacro
@@ -0,0 +1,55 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/iiwa_description/urdf/iiwa16.xacro b/iiwa_description/urdf/iiwa16.xacro
new file mode 100644
index 0000000..2e451b9
--- /dev/null
+++ b/iiwa_description/urdf/iiwa16.xacro
@@ -0,0 +1,431 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/iiwa_description/urdf/iiwa17.urdf.xacro b/iiwa_description/urdf/iiwa17.urdf.xacro
new file mode 100644
index 0000000..8dab76f
--- /dev/null
+++ b/iiwa_description/urdf/iiwa17.urdf.xacro
@@ -0,0 +1,55 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/iiwa_description/urdf/iiwa17.xacro b/iiwa_description/urdf/iiwa17.xacro
new file mode 100644
index 0000000..43b229c
--- /dev/null
+++ b/iiwa_description/urdf/iiwa17.xacro
@@ -0,0 +1,431 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/iiwa_gazebo/launch/iiwa_gazebo.launch b/iiwa_gazebo/launch/iiwa_gazebo.launch
index 1e51de8..4625000 100644
--- a/iiwa_gazebo/launch/iiwa_gazebo.launch
+++ b/iiwa_gazebo/launch/iiwa_gazebo.launch
@@ -34,6 +34,7 @@
+
diff --git a/iiwa_ros/lus-rosscripts b/iiwa_ros/lus-rosscripts
new file mode 160000
index 0000000..3f293fd
--- /dev/null
+++ b/iiwa_ros/lus-rosscripts
@@ -0,0 +1 @@
+Subproject commit 3f293fdc62368755629cb026f0750e74512e151f
diff --git a/koopman-op b/koopman-op
new file mode 160000
index 0000000..e2a6d61
--- /dev/null
+++ b/koopman-op
@@ -0,0 +1 @@
+Subproject commit e2a6d61a1873247c8bca58fdf2f415867a272db1