diff --git a/.gitattributes b/.gitattributes
index 8392c70c123e4c97c0c87c81e32bdf8298657e51..100e7b6d54d53375ebef9c6d6a4171707de6a95c 100644
--- a/.gitattributes
+++ b/.gitattributes
@@ -38,3 +38,4 @@ saved_model/**/* filter=lfs diff=lfs merge=lfs -text
 *.dae filter=lfs diff=lfs merge=lfs -text
 *.glb filter=lfs diff=lfs merge=lfs -text
 *.gltf filter=lfs diff=lfs merge=lfs -text
+*.STL filter=lfs diff=lfs merge=lfs -text
diff --git a/mobile/marvin_pro/meshes/marvin_pro/meshes/45omnigripper/Left_Finger_Link.STL b/mobile/marvin_pro/meshes/marvin_pro/meshes/45omnigripper/Left_Finger_Link.STL
new file mode 100644
index 0000000000000000000000000000000000000000..cb72f648a6ce1347780b242f49ff200a22f48fa0
--- /dev/null
+++ b/mobile/marvin_pro/meshes/marvin_pro/meshes/45omnigripper/Left_Finger_Link.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:86d3e7f07472d420fa83d082da4578f1348d6a558a81a7d9e86037145371fe0d
+size 106484
diff --git a/mobile/marvin_pro/meshes/marvin_pro/meshes/45omnigripper/Right_Finger_Link.STL b/mobile/marvin_pro/meshes/marvin_pro/meshes/45omnigripper/Right_Finger_Link.STL
new file mode 100644
index 0000000000000000000000000000000000000000..d00ddb38a5d375c8a39e77a4ed5518e5c02f5dd6
--- /dev/null
+++ b/mobile/marvin_pro/meshes/marvin_pro/meshes/45omnigripper/Right_Finger_Link.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:c527529e800eb5664ce28b0fb6272c0f5d0c19b5c353510afebfb6cbf7fbc8ea
+size 106484
diff --git a/mobile/marvin_pro/meshes/marvin_pro/meshes/45omnigripper/base_link.STL b/mobile/marvin_pro/meshes/marvin_pro/meshes/45omnigripper/base_link.STL
new file mode 100644
index 0000000000000000000000000000000000000000..8ec2001a6a8501c847127c3aebb696fe655db457
--- /dev/null
+++ b/mobile/marvin_pro/meshes/marvin_pro/meshes/45omnigripper/base_link.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:c12b10e4c3f9e8e729e59baee13bc73658aa24b21b7024f044e140e388aa4677
+size 1152084
diff --git a/mobile/marvin_pro/meshes/marvin_pro/meshes/SRS/BASE.STL b/mobile/marvin_pro/meshes/marvin_pro/meshes/SRS/BASE.STL
new file mode 100644
index 0000000000000000000000000000000000000000..5480ebe2340c77abb9bf7f2f1e4cebefceedf31f
--- /dev/null
+++ b/mobile/marvin_pro/meshes/marvin_pro/meshes/SRS/BASE.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:5fc43b60972223c9e54c86e7c66fc2b12bd918054475aeb61017d67216256d4a
+size 1999584
diff --git a/mobile/marvin_pro/meshes/marvin_pro/meshes/SRS/Link1.STL b/mobile/marvin_pro/meshes/marvin_pro/meshes/SRS/Link1.STL
new file mode 100644
index 0000000000000000000000000000000000000000..d129e153691ae340e5e078a4214adb00924cb3e0
--- /dev/null
+++ b/mobile/marvin_pro/meshes/marvin_pro/meshes/SRS/Link1.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:8eada9ea3a2b6f13f209387f75bc2ff47c3034197acfbffdc515ff3c5efb304a
+size 2802384
diff --git a/mobile/marvin_pro/meshes/marvin_pro/meshes/SRS/Link2.STL b/mobile/marvin_pro/meshes/marvin_pro/meshes/SRS/Link2.STL
new file mode 100644
index 0000000000000000000000000000000000000000..4d844843d537e0e4048de616079c120d5983e604
--- /dev/null
+++ b/mobile/marvin_pro/meshes/marvin_pro/meshes/SRS/Link2.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:879f693fbb0650aa753ada082e69f5e501947af0fa348e012178aa7b1bb4dc1f
+size 6239484
diff --git a/mobile/marvin_pro/meshes/marvin_pro/meshes/SRS/Link3.STL b/mobile/marvin_pro/meshes/marvin_pro/meshes/SRS/Link3.STL
new file mode 100644
index 0000000000000000000000000000000000000000..37f2e34ac82c685cdb3d97d111305ad044da9a25
--- /dev/null
+++ b/mobile/marvin_pro/meshes/marvin_pro/meshes/SRS/Link3.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:3ea48260f977963e0d28f4e57bee0f0cf79f3f647c96cdbb5172e503a388988e
+size 954384
diff --git a/mobile/marvin_pro/meshes/marvin_pro/meshes/SRS/Link4.STL b/mobile/marvin_pro/meshes/marvin_pro/meshes/SRS/Link4.STL
new file mode 100644
index 0000000000000000000000000000000000000000..8db4b799f93dc7142f9f96233a64289ef31b5771
--- /dev/null
+++ b/mobile/marvin_pro/meshes/marvin_pro/meshes/SRS/Link4.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:3cb13f8332f98b553d79534fe91576963671cb63b9eb862ff046ce013818edf7
+size 3076784
diff --git a/mobile/marvin_pro/meshes/marvin_pro/meshes/SRS/Link5.STL b/mobile/marvin_pro/meshes/marvin_pro/meshes/SRS/Link5.STL
new file mode 100644
index 0000000000000000000000000000000000000000..00c407e81847f7d73ebd515421b13a637e710fea
--- /dev/null
+++ b/mobile/marvin_pro/meshes/marvin_pro/meshes/SRS/Link5.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:b849fe3edcb4d71bdab7b1572a70fa94eef3297f1d146901d03e50ce76bc2adc
+size 784984
diff --git a/mobile/marvin_pro/meshes/marvin_pro/meshes/SRS/Link6.STL b/mobile/marvin_pro/meshes/marvin_pro/meshes/SRS/Link6.STL
new file mode 100644
index 0000000000000000000000000000000000000000..154edad3bd68f81e9dadb41e235249c64dc86754
--- /dev/null
+++ b/mobile/marvin_pro/meshes/marvin_pro/meshes/SRS/Link6.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:49077a236418cd5a2d46ef4571981be1e4c2a42ec5ffe8535acbdc308cd6040f
+size 2724984
diff --git a/mobile/marvin_pro/meshes/marvin_pro/meshes/SRS/Link7.STL b/mobile/marvin_pro/meshes/marvin_pro/meshes/SRS/Link7.STL
new file mode 100644
index 0000000000000000000000000000000000000000..cf0b76d3d8948e3a1a7fe5820c5896bf5d1860d6
--- /dev/null
+++ b/mobile/marvin_pro/meshes/marvin_pro/meshes/SRS/Link7.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:598526a1d0863068b8e7dba414839fc06d48808f620bdfe9ddf1a82fca0d945b
+size 1142084
diff --git a/mobile/marvin_pro/meshes/marvin_pro/meshes/base/ZJ_Robot_link.STL b/mobile/marvin_pro/meshes/marvin_pro/meshes/base/ZJ_Robot_link.STL
new file mode 100644
index 0000000000000000000000000000000000000000..cae72399142852379380611c50f6e3702d94342e
--- /dev/null
+++ b/mobile/marvin_pro/meshes/marvin_pro/meshes/base/ZJ_Robot_link.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:186d243916654a45c943838f47b5cf73832695cd366145fd7b6347aa46f4cea0
+size 9721584
diff --git a/mobile/marvin_pro/meshes/marvin_pro/meshes/base/base_link.STL b/mobile/marvin_pro/meshes/marvin_pro/meshes/base/base_link.STL
new file mode 100644
index 0000000000000000000000000000000000000000..1747d4215ed6725baebeaa9c1290c9aa32cd5731
--- /dev/null
+++ b/mobile/marvin_pro/meshes/marvin_pro/meshes/base/base_link.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:2385c27e73f5a86ddb85d6c0491cf6c26deaf30eeb351c22fb05e53fe93c966f
+size 348984
diff --git a/mobile/marvin_pro/meshes/marvin_pro/meshes/base/stand.STL b/mobile/marvin_pro/meshes/marvin_pro/meshes/base/stand.STL
new file mode 100644
index 0000000000000000000000000000000000000000..a657277c0c2159a589f7aaf18efe10e9a56f612f
--- /dev/null
+++ b/mobile/marvin_pro/meshes/marvin_pro/meshes/base/stand.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:0a9f193351137f937c099219ca02b4e3fe40c10f2cf31ef98276381493f9da3e
+size 7738584
diff --git a/mobile/marvin_pro/meshes/marvin_pro/meshes/base/support_link.STL b/mobile/marvin_pro/meshes/marvin_pro/meshes/base/support_link.STL
new file mode 100644
index 0000000000000000000000000000000000000000..a21dbe8894384f1f88a266b4486fefa027344bb6
--- /dev/null
+++ b/mobile/marvin_pro/meshes/marvin_pro/meshes/base/support_link.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:b915194e0546fb7e174fba1dfb8993fac5bf492dfc6a34e73c908d514f48c158
+size 689984
diff --git a/mobile/marvin_pro/meshes/marvin_pro/meshes/m3/Base_L.STL b/mobile/marvin_pro/meshes/marvin_pro/meshes/m3/Base_L.STL
new file mode 100644
index 0000000000000000000000000000000000000000..b30fbde595186305cef741539ffa0e9aa2bb429e
--- /dev/null
+++ b/mobile/marvin_pro/meshes/marvin_pro/meshes/m3/Base_L.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:f89c8cf1c772bb31939bf4d25f21537f1123b618f5efe652e05a9512782e4491
+size 320484
diff --git a/mobile/marvin_pro/meshes/marvin_pro/meshes/m3/Base_R.STL b/mobile/marvin_pro/meshes/marvin_pro/meshes/m3/Base_R.STL
new file mode 100644
index 0000000000000000000000000000000000000000..b30fbde595186305cef741539ffa0e9aa2bb429e
--- /dev/null
+++ b/mobile/marvin_pro/meshes/marvin_pro/meshes/m3/Base_R.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:f89c8cf1c772bb31939bf4d25f21537f1123b618f5efe652e05a9512782e4491
+size 320484
diff --git a/mobile/marvin_pro/meshes/marvin_pro/meshes/m3/Link1_L.STL b/mobile/marvin_pro/meshes/marvin_pro/meshes/m3/Link1_L.STL
new file mode 100644
index 0000000000000000000000000000000000000000..23e18e54acaa5898880e8a7360c9389199d7789f
--- /dev/null
+++ b/mobile/marvin_pro/meshes/marvin_pro/meshes/m3/Link1_L.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:9454d5043557c61b83ae95e8106f24c5e0404b0b87c0a909fb1fb42136493484
+size 900484
diff --git a/mobile/marvin_pro/meshes/marvin_pro/meshes/m3/Link1_R.STL b/mobile/marvin_pro/meshes/marvin_pro/meshes/m3/Link1_R.STL
new file mode 100644
index 0000000000000000000000000000000000000000..23e18e54acaa5898880e8a7360c9389199d7789f
--- /dev/null
+++ b/mobile/marvin_pro/meshes/marvin_pro/meshes/m3/Link1_R.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:9454d5043557c61b83ae95e8106f24c5e0404b0b87c0a909fb1fb42136493484
+size 900484
diff --git a/mobile/marvin_pro/meshes/marvin_pro/meshes/m3/Link2_L.STL b/mobile/marvin_pro/meshes/marvin_pro/meshes/m3/Link2_L.STL
new file mode 100644
index 0000000000000000000000000000000000000000..d2efba2f083b84565c8afc1634bd72efbae8c55b
--- /dev/null
+++ b/mobile/marvin_pro/meshes/marvin_pro/meshes/m3/Link2_L.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:c58c7594051b379dea48bd388ef21bbd2babce6b3a8d872f3e32ab97871e01df
+size 5772784
diff --git a/mobile/marvin_pro/meshes/marvin_pro/meshes/m3/Link2_R.STL b/mobile/marvin_pro/meshes/marvin_pro/meshes/m3/Link2_R.STL
new file mode 100644
index 0000000000000000000000000000000000000000..d2efba2f083b84565c8afc1634bd72efbae8c55b
--- /dev/null
+++ b/mobile/marvin_pro/meshes/marvin_pro/meshes/m3/Link2_R.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:c58c7594051b379dea48bd388ef21bbd2babce6b3a8d872f3e32ab97871e01df
+size 5772784
diff --git a/mobile/marvin_pro/meshes/marvin_pro/meshes/m3/Link3_L.STL b/mobile/marvin_pro/meshes/marvin_pro/meshes/m3/Link3_L.STL
new file mode 100644
index 0000000000000000000000000000000000000000..2215d205bc4e2c5363660ec1ceda3c7f74f2c938
--- /dev/null
+++ b/mobile/marvin_pro/meshes/marvin_pro/meshes/m3/Link3_L.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:16e0171418fea520f458932775756d8cb0204cb776d656a4dfb153f07c46517c
+size 2412484
diff --git a/mobile/marvin_pro/meshes/marvin_pro/meshes/m3/Link3_R.STL b/mobile/marvin_pro/meshes/marvin_pro/meshes/m3/Link3_R.STL
new file mode 100644
index 0000000000000000000000000000000000000000..2215d205bc4e2c5363660ec1ceda3c7f74f2c938
--- /dev/null
+++ b/mobile/marvin_pro/meshes/marvin_pro/meshes/m3/Link3_R.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:16e0171418fea520f458932775756d8cb0204cb776d656a4dfb153f07c46517c
+size 2412484
diff --git a/mobile/marvin_pro/meshes/marvin_pro/meshes/m3/Link4_L.STL b/mobile/marvin_pro/meshes/marvin_pro/meshes/m3/Link4_L.STL
new file mode 100644
index 0000000000000000000000000000000000000000..dad4cec899033db95aee0e66dbb7a989f057e37a
--- /dev/null
+++ b/mobile/marvin_pro/meshes/marvin_pro/meshes/m3/Link4_L.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:44b8ac7d9c789b967078318e7c25192fa7a3fb7c4a04cf3ae1383fd45ef1d4a0
+size 5726084
diff --git a/mobile/marvin_pro/meshes/marvin_pro/meshes/m3/Link4_R.STL b/mobile/marvin_pro/meshes/marvin_pro/meshes/m3/Link4_R.STL
new file mode 100644
index 0000000000000000000000000000000000000000..dad4cec899033db95aee0e66dbb7a989f057e37a
--- /dev/null
+++ b/mobile/marvin_pro/meshes/marvin_pro/meshes/m3/Link4_R.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:44b8ac7d9c789b967078318e7c25192fa7a3fb7c4a04cf3ae1383fd45ef1d4a0
+size 5726084
diff --git a/mobile/marvin_pro/meshes/marvin_pro/meshes/m3/Link5_L.STL b/mobile/marvin_pro/meshes/marvin_pro/meshes/m3/Link5_L.STL
new file mode 100644
index 0000000000000000000000000000000000000000..0657c82fe7416e583eb2f8ac3a0129ba119a4a8c
--- /dev/null
+++ b/mobile/marvin_pro/meshes/marvin_pro/meshes/m3/Link5_L.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:47202c8b234ed28f2a01f043b84e7e59f968e74c80e59240fa043b4d25c397b1
+size 6051784
diff --git a/mobile/marvin_pro/meshes/marvin_pro/meshes/m3/Link5_R.STL b/mobile/marvin_pro/meshes/marvin_pro/meshes/m3/Link5_R.STL
new file mode 100644
index 0000000000000000000000000000000000000000..f0e739b123266cc3bd59724b7f31d8972b40fdd8
--- /dev/null
+++ b/mobile/marvin_pro/meshes/marvin_pro/meshes/m3/Link5_R.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:2930459babed4ff1168ad59fc9a04f592135740b99270c0c84004fa5d9ffe229
+size 6052284
diff --git a/mobile/marvin_pro/meshes/marvin_pro/meshes/m3/Link6_L.STL b/mobile/marvin_pro/meshes/marvin_pro/meshes/m3/Link6_L.STL
new file mode 100644
index 0000000000000000000000000000000000000000..e941ff3a4170be7fc74b64ac82997c1377e904e0
--- /dev/null
+++ b/mobile/marvin_pro/meshes/marvin_pro/meshes/m3/Link6_L.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:86543f5bd00a407d77205c6507b4c935f182dc8f8f586c7cebb0b4945a7eb4f1
+size 407384
diff --git a/mobile/marvin_pro/meshes/marvin_pro/meshes/m3/Link6_R.STL b/mobile/marvin_pro/meshes/marvin_pro/meshes/m3/Link6_R.STL
new file mode 100644
index 0000000000000000000000000000000000000000..5cba36b6aea925b7a93a72d9b40f3a5d62bf0b5b
--- /dev/null
+++ b/mobile/marvin_pro/meshes/marvin_pro/meshes/m3/Link6_R.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:852432bdb2cc62abda9c85d7a6b7ce8e0c5b33358929f8e13e8b18f6b3412d9a
+size 407384
diff --git a/mobile/marvin_pro/meshes/marvin_pro/meshes/m3/Link7_L.STL b/mobile/marvin_pro/meshes/marvin_pro/meshes/m3/Link7_L.STL
new file mode 100644
index 0000000000000000000000000000000000000000..03113110047872a0fd09069ea5782501af6066d7
--- /dev/null
+++ b/mobile/marvin_pro/meshes/marvin_pro/meshes/m3/Link7_L.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:484217736ffb8889ccf51aa3db0ad443fc1df5ca04d813827c269dc567b6b917
+size 4566484
diff --git a/mobile/marvin_pro/meshes/marvin_pro/meshes/m3/Link7_R.STL b/mobile/marvin_pro/meshes/marvin_pro/meshes/m3/Link7_R.STL
new file mode 100644
index 0000000000000000000000000000000000000000..59a2a8601ae337ea7f0df1224871d0e467b1c7a5
--- /dev/null
+++ b/mobile/marvin_pro/meshes/marvin_pro/meshes/m3/Link7_R.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:d4b74570036f431428e589fa93487dfa80e29a2e3ff36797741e9efbeb75dc76
+size 4590384
diff --git a/mobile/marvin_pro/meshes/marvin_pro/meshes/m6/Arm_L1_Link.STL b/mobile/marvin_pro/meshes/marvin_pro/meshes/m6/Arm_L1_Link.STL
new file mode 100644
index 0000000000000000000000000000000000000000..1435476217b009bdba22cbf9a01ff2ab07dd555c
--- /dev/null
+++ b/mobile/marvin_pro/meshes/marvin_pro/meshes/m6/Arm_L1_Link.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:d00097eec49600a4111ee5a124d6636f50ea09a7a1dc8486d98f4370dde7a932
+size 563584
diff --git a/mobile/marvin_pro/meshes/marvin_pro/meshes/m6/Arm_L2_Link.STL b/mobile/marvin_pro/meshes/marvin_pro/meshes/m6/Arm_L2_Link.STL
new file mode 100644
index 0000000000000000000000000000000000000000..9f6455cc53a7562b39881b0e287ec603fc47f28c
--- /dev/null
+++ b/mobile/marvin_pro/meshes/marvin_pro/meshes/m6/Arm_L2_Link.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:4776a6a2086ddc61b1643aff93386e545baccef2751db29d863fe027a76f325e
+size 1278784
diff --git a/mobile/marvin_pro/meshes/marvin_pro/meshes/m6/Arm_L3_Link.STL b/mobile/marvin_pro/meshes/marvin_pro/meshes/m6/Arm_L3_Link.STL
new file mode 100644
index 0000000000000000000000000000000000000000..7112f2af5fa3be51f248b75c5b2ba924a19b500c
--- /dev/null
+++ b/mobile/marvin_pro/meshes/marvin_pro/meshes/m6/Arm_L3_Link.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:d58f1039bd3eb2ccc1a1959b89d2de7ae714344ad8cb8b78ef97b92dcd317d26
+size 635484
diff --git a/mobile/marvin_pro/meshes/marvin_pro/meshes/m6/Arm_L4_Link.STL b/mobile/marvin_pro/meshes/marvin_pro/meshes/m6/Arm_L4_Link.STL
new file mode 100644
index 0000000000000000000000000000000000000000..8f4597538ac46750e2a1987b4da93338c59ab628
--- /dev/null
+++ b/mobile/marvin_pro/meshes/marvin_pro/meshes/m6/Arm_L4_Link.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:77f18566339f38acac1785fae517b05ff1e5247b95404ce26536d215bbed9cd7
+size 1263484
diff --git a/mobile/marvin_pro/meshes/marvin_pro/meshes/m6/Arm_L5_Link.STL b/mobile/marvin_pro/meshes/marvin_pro/meshes/m6/Arm_L5_Link.STL
new file mode 100644
index 0000000000000000000000000000000000000000..c1aa6276ac302a81174e07283cc0d9d347596598
--- /dev/null
+++ b/mobile/marvin_pro/meshes/marvin_pro/meshes/m6/Arm_L5_Link.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:573f15ee62b0a9b2d62aceeb756800fd86969a685535e91416b73d51f1c57613
+size 1176084
diff --git a/mobile/marvin_pro/meshes/marvin_pro/meshes/m6/Arm_L6_Link.STL b/mobile/marvin_pro/meshes/marvin_pro/meshes/m6/Arm_L6_Link.STL
new file mode 100644
index 0000000000000000000000000000000000000000..ceae9067670eb0e06783e27bf0df6e7fc6a86fdd
--- /dev/null
+++ b/mobile/marvin_pro/meshes/marvin_pro/meshes/m6/Arm_L6_Link.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:b6b7570d8ebf1d2acb01baf8941b892f5a5f72aaa05503ab8340da8a2a7b2c13
+size 310884
diff --git a/mobile/marvin_pro/meshes/marvin_pro/meshes/m6/Arm_L7_Link.STL b/mobile/marvin_pro/meshes/marvin_pro/meshes/m6/Arm_L7_Link.STL
new file mode 100644
index 0000000000000000000000000000000000000000..57f3248dbd0c6bc991960389878228cee5a97a2a
--- /dev/null
+++ b/mobile/marvin_pro/meshes/marvin_pro/meshes/m6/Arm_L7_Link.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:126cc7d41c37049ce34d2636bfe2d460c21e3429ed080877d1c5e983109ef798
+size 2557184
diff --git a/mobile/marvin_pro/meshes/marvin_pro/meshes/m6/Arm_R1_Link.STL b/mobile/marvin_pro/meshes/marvin_pro/meshes/m6/Arm_R1_Link.STL
new file mode 100644
index 0000000000000000000000000000000000000000..dd4fe6008a88524a1aa5cc8cf695f173c77c3f39
--- /dev/null
+++ b/mobile/marvin_pro/meshes/marvin_pro/meshes/m6/Arm_R1_Link.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:fdb1075fe0b41e5c2cb7c2fe13c9d2e62ff4dd6eb540ea841b8603db28369dd0
+size 563584
diff --git a/mobile/marvin_pro/meshes/marvin_pro/meshes/m6/Arm_R2_Link.STL b/mobile/marvin_pro/meshes/marvin_pro/meshes/m6/Arm_R2_Link.STL
new file mode 100644
index 0000000000000000000000000000000000000000..d3367e1e8ca851169809fff238e6eb0782bbde8d
--- /dev/null
+++ b/mobile/marvin_pro/meshes/marvin_pro/meshes/m6/Arm_R2_Link.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:53eb9fc69c939d2ebd4f32c1df341c1c42bc3d9d0911db0c9fcee70aa2701b34
+size 1278784
diff --git a/mobile/marvin_pro/meshes/marvin_pro/meshes/m6/Arm_R3_Link.STL b/mobile/marvin_pro/meshes/marvin_pro/meshes/m6/Arm_R3_Link.STL
new file mode 100644
index 0000000000000000000000000000000000000000..595fb3451228409296d26d9105f1ccc976b47e15
--- /dev/null
+++ b/mobile/marvin_pro/meshes/marvin_pro/meshes/m6/Arm_R3_Link.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:122456d8fc3e81694f352fc1f29d0bc19066f7f3d7572857a1be1e7c030dec9c
+size 635484
diff --git a/mobile/marvin_pro/meshes/marvin_pro/meshes/m6/Arm_R4_Link.STL b/mobile/marvin_pro/meshes/marvin_pro/meshes/m6/Arm_R4_Link.STL
new file mode 100644
index 0000000000000000000000000000000000000000..8f4597538ac46750e2a1987b4da93338c59ab628
--- /dev/null
+++ b/mobile/marvin_pro/meshes/marvin_pro/meshes/m6/Arm_R4_Link.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:77f18566339f38acac1785fae517b05ff1e5247b95404ce26536d215bbed9cd7
+size 1263484
diff --git a/mobile/marvin_pro/meshes/marvin_pro/meshes/m6/Arm_R5_Link.STL b/mobile/marvin_pro/meshes/marvin_pro/meshes/m6/Arm_R5_Link.STL
new file mode 100644
index 0000000000000000000000000000000000000000..a4670db26a36acb184caabc752c3928ce25bb77a
--- /dev/null
+++ b/mobile/marvin_pro/meshes/marvin_pro/meshes/m6/Arm_R5_Link.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:2de51a340695c89b2fd58b1171dfacc4d85aa4e6cd50879e021a2d0bde8599a8
+size 1196684
diff --git a/mobile/marvin_pro/meshes/marvin_pro/meshes/m6/Arm_R6_Link.STL b/mobile/marvin_pro/meshes/marvin_pro/meshes/m6/Arm_R6_Link.STL
new file mode 100644
index 0000000000000000000000000000000000000000..0a6b53a16a42c37ba81799e4b75dc02bf8746b61
--- /dev/null
+++ b/mobile/marvin_pro/meshes/marvin_pro/meshes/m6/Arm_R6_Link.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:49722d14b9ac34a84a41c95f49bd0b2a8859d4a05f66b247822bf36b0a703cd5
+size 310884
diff --git a/mobile/marvin_pro/meshes/marvin_pro/meshes/m6/Arm_R7_Link.STL b/mobile/marvin_pro/meshes/marvin_pro/meshes/m6/Arm_R7_Link.STL
new file mode 100644
index 0000000000000000000000000000000000000000..b82c7ec9475ba314d93388a18b016f5ae1dd70c7
--- /dev/null
+++ b/mobile/marvin_pro/meshes/marvin_pro/meshes/m6/Arm_R7_Link.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:6eb75b059ff383e90a153be840bcce84b23d8f8afd074b1bc9dfec1b18c2351f
+size 2577384
diff --git a/mobile/marvin_pro/meshes/marvin_pro/meshes/m6/Base_L.STL b/mobile/marvin_pro/meshes/marvin_pro/meshes/m6/Base_L.STL
new file mode 100644
index 0000000000000000000000000000000000000000..f997bf9096d8cd4fc160297ff9559cabbde8c401
--- /dev/null
+++ b/mobile/marvin_pro/meshes/marvin_pro/meshes/m6/Base_L.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:f22c309ed8cacd2fc776e2f72a2fc5618d9da84315267377edd3205e91ebbcb5
+size 204584
diff --git a/mobile/marvin_pro/meshes/marvin_pro/meshes/m6/Base_R.STL b/mobile/marvin_pro/meshes/marvin_pro/meshes/m6/Base_R.STL
new file mode 100644
index 0000000000000000000000000000000000000000..f997bf9096d8cd4fc160297ff9559cabbde8c401
--- /dev/null
+++ b/mobile/marvin_pro/meshes/marvin_pro/meshes/m6/Base_R.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:f22c309ed8cacd2fc776e2f72a2fc5618d9da84315267377edd3205e91ebbcb5
+size 204584
diff --git a/mobile/marvin_pro/meshes/marvin_pro/meshes/omnigripper/Left_Chipper_Base.STL b/mobile/marvin_pro/meshes/marvin_pro/meshes/omnigripper/Left_Chipper_Base.STL
new file mode 100644
index 0000000000000000000000000000000000000000..6cf827cfd8fe6715757dca16af00d1a330aa4421
--- /dev/null
+++ b/mobile/marvin_pro/meshes/marvin_pro/meshes/omnigripper/Left_Chipper_Base.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:1417bf23a88dc7add0275e16840baf8678d2f4da775679cc9619d938b00290bf
+size 106484
diff --git a/mobile/marvin_pro/meshes/marvin_pro/meshes/omnigripper/Right_Chipper_Base.STL b/mobile/marvin_pro/meshes/marvin_pro/meshes/omnigripper/Right_Chipper_Base.STL
new file mode 100644
index 0000000000000000000000000000000000000000..22cdadb3ea59439b371d230c979ea2232a94d7d1
--- /dev/null
+++ b/mobile/marvin_pro/meshes/marvin_pro/meshes/omnigripper/Right_Chipper_Base.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:0b05030f3f1ac56f0ea412343f38a569fe3d54e796a23ae7c7576753a343a7f3
+size 106484
diff --git a/mobile/marvin_pro/meshes/marvin_pro/meshes/omnigripper/omnigripper_straight.STL b/mobile/marvin_pro/meshes/marvin_pro/meshes/omnigripper/omnigripper_straight.STL
new file mode 100644
index 0000000000000000000000000000000000000000..87f902a145fb3fab23e25a1fec73323ac7a87be5
--- /dev/null
+++ b/mobile/marvin_pro/meshes/marvin_pro/meshes/omnigripper/omnigripper_straight.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:4589db3be5e1d72b46a4c8433ca245199bf0657847a4994457c8f68a3e35a93d
+size 1297284
diff --git a/mobile/marvin_pro/mjcf/marvin_pro_mink_with_gripper.xml b/mobile/marvin_pro/mjcf/marvin_pro_mink_with_gripper.xml
new file mode 100644
index 0000000000000000000000000000000000000000..b3eec9c4e118233dee0637e8228ca52a993a158b
--- /dev/null
+++ b/mobile/marvin_pro/mjcf/marvin_pro_mink_with_gripper.xml
@@ -0,0 +1,260 @@
+<mujoco model="marvin_robot">
+  <compiler angle="radian" meshdir="meshes/marvin_pro" />
+
+  <option timestep="0.001" integrator="implicitfast" />
+
+
+  <default>
+  <default class="ccs_m6">
+  
+    <joint damping="2" armature="0.1" frictionloss="0.2" />
+    <position inheritrange="1" />
+    <default class="visual">
+      <geom group="2" type="mesh" contype="0" conaffinity="0" rgba="0.3 0.3 0.3 1" />
+    </default>
+    <default class="collision">
+      <geom type="capsule" group="3" mass="0" density="0" />
+    </default>
+  </default>
+</default>
+
+  <asset>
+    <mesh name="Base_L" content_type="model/stl" file="meshes/m6/Base_L.STL" />
+    <mesh name="Arm_L1_Link" content_type="model/stl" file="meshes/m6/Arm_L1_Link.STL" />
+    <mesh name="Arm_L2_Link" content_type="model/stl" file="meshes/m6/Arm_L2_Link.STL" />
+    <mesh name="Arm_L3_Link" content_type="model/stl" file="meshes/m6/Arm_L3_Link.STL" />
+    <mesh name="Arm_L4_Link" content_type="model/stl" file="meshes/m6/Arm_L4_Link.STL" />
+    <mesh name="Arm_L5_Link" content_type="model/stl" file="meshes/m6/Arm_L5_Link.STL" />
+    <mesh name="Arm_L6_Link" content_type="model/stl" file="meshes/m6/Arm_L6_Link.STL" />
+    <mesh name="Arm_L7_Link" content_type="model/stl" file="meshes/m6/Arm_L7_Link.STL" />
+    <mesh name="Base_R" content_type="model/stl" file="meshes/m6/Base_R.STL" />
+    <mesh name="Arm_R1_Link" content_type="model/stl" file="meshes/m6/Arm_R1_Link.STL" />
+    <mesh name="Arm_R2_Link" content_type="model/stl" file="meshes/m6/Arm_R2_Link.STL" />
+    <mesh name="Arm_R3_Link" content_type="model/stl" file="meshes/m6/Arm_R3_Link.STL" />
+    <mesh name="Arm_R4_Link" content_type="model/stl" file="meshes/m6/Arm_R4_Link.STL" />
+    <mesh name="Arm_R5_Link" content_type="model/stl" file="meshes/m6/Arm_R5_Link.STL" />
+    <mesh name="Arm_R6_Link" content_type="model/stl" file="meshes/m6/Arm_R6_Link.STL" />
+    <mesh name="Arm_R7_Link" content_type="model/stl" file="meshes/m6/Arm_R7_Link.STL" />
+    <mesh name="ZJ_Robot_link" content_type="model/stl" file="meshes/base/ZJ_Robot_link.STL" />
+      <mesh name="left_gripper_base_mesh" content_type="model/stl" file="meshes/omnigripper/omnigripper_straight.STL" />
+      <mesh name="left_gripper_left_finger_mesh" content_type="model/stl" file="meshes/omnigripper/Left_Chipper_Base.STL" />
+      <mesh name="left_gripper_right_finger_mesh" content_type="model/stl" file="meshes/omnigripper/Right_Chipper_Base.STL" />
+      <mesh name="right_gripper_base_mesh" content_type="model/stl" file="meshes/omnigripper/omnigripper_straight.STL" />
+      <mesh name="right_gripper_left_finger_mesh" content_type="model/stl" file="meshes/omnigripper/Left_Chipper_Base.STL" />
+      <mesh name="right_gripper_right_finger_mesh" content_type="model/stl" file="meshes/omnigripper/Right_Chipper_Base.STL" />
+    <material name="gripper_base_mat" rgba="0.7922 0.8196 0.9294 1" specular="0" />
+    <material name="gripper_finger_mat" rgba="0.6039 0.6039 0.6039 1" specular="0" />
+  </asset>
+  <contact>
+    <exclude body1="base_link" body2="Base_L" />
+    <exclude body1="base_link" body2="Base_R" />
+    <exclude body1="Base_L" body2="Arm_L1_Link" />
+    <exclude body1="Arm_L1_Link" body2="Arm_L2_Link" />
+    <exclude body1="Arm_L2_Link" body2="Arm_L3_Link" />
+    <exclude body1="Arm_L3_Link" body2="Arm_L4_Link" />
+    <exclude body1="Arm_L4_Link" body2="Arm_L5_Link" />
+    <exclude body1="Arm_L5_Link" body2="Arm_L6_Link" />
+    <exclude body1="Arm_L6_Link" body2="Arm_L7_Link" />
+    <exclude body1="Base_R" body2="Arm_R1_Link" />
+    <exclude body1="Arm_R1_Link" body2="Arm_R2_Link" />
+    <exclude body1="Arm_R2_Link" body2="Arm_R3_Link" />
+    <exclude body1="Arm_R3_Link" body2="Arm_R4_Link" />
+    <exclude body1="Arm_R4_Link" body2="Arm_R5_Link" />
+    <exclude body1="Arm_R5_Link" body2="Arm_R6_Link" />
+    <exclude body1="Arm_R6_Link" body2="Arm_R7_Link" />
+    <exclude body1="Arm_L7_Link" body2="left_gripper_base" />
+    <exclude body1="Arm_L7_Link" body2="left_gripper_left_finger" />
+    <exclude body1="Arm_L7_Link" body2="left_gripper_right_finger" />
+    <exclude body1="left_gripper_base" body2="left_gripper_left_finger" />
+    <exclude body1="left_gripper_base" body2="left_gripper_right_finger" />
+    <exclude body1="left_gripper_left_finger" body2="left_gripper_right_finger" />
+    <exclude body1="Arm_R7_Link" body2="right_gripper_base" />
+    <exclude body1="Arm_R7_Link" body2="right_gripper_left_finger" />
+    <exclude body1="Arm_R7_Link" body2="right_gripper_right_finger" />
+    <exclude body1="right_gripper_base" body2="right_gripper_left_finger" />
+    <exclude body1="right_gripper_base" body2="right_gripper_right_finger" />
+    <exclude body1="right_gripper_left_finger" body2="right_gripper_right_finger" />
+  </contact>
+  <equality>
+    <joint joint1="left_gripper_right_finger_joint" joint2="left_gripper_left_finger_joint" polycoef="0 1 0 0 0" />
+    <joint joint1="right_gripper_right_finger_joint" joint2="right_gripper_left_finger_joint" polycoef="0 1 0 0 0" />
+  </equality>
+  <worldbody>
+    <body name="base_link" childclass="ccs_m6">
+      <inertial pos="0.0081994 -5.1239e-06 0.47835" quat="0.999994 2.57224e-05 0.0031361 0.00173711" mass="18.846" diaginertia="0.463463 0.46325 0.377657" />
+      <geom class="visual" rgba="0.8 0.8 0.8 1" mesh="ZJ_Robot_link" />
+      <geom name="base_link_collision" class="collision" rgba="1 0 0 1" fromto="0 0 0 0 0 1.3" size="0.10" />
+      <body name="tracking_base_link" pos="0 0 -0.1" />
+        <camera name="head_left_camera" pos="0.086 0.015 1.218" xyaxes="0 -1 0 0.5 0 0.85" fovy="105" />
+        <camera name="head_right_camera" pos="0.086 -0.015 1.218" xyaxes="0 -1 0 0.5 0 0.85" fovy="105" />
+      <body name="Base_L" pos="0 0.026 1.121" quat="0.707388 -0.706825 0 0">
+        <inertial pos="0.00032267 4.798e-05 0.0406635" quat="0.582787 0.36644 -0.620273 0.375956" mass="1.1962" diaginertia="0.00127375 0.00107367 0.00106904" />
+        <geom class="visual" rgba="1 0 0 1" mesh="Base_L" />
+        <geom class="collision" rgba="1 0 0 1" mesh="Base_L" />
+        <body name="Arm_L1_Link" pos="0 0 0.1745">
+          <inertial pos="2.347e-05 0.0055715 -0.03076" quat="0.711599 0.0343691 -0.0341702 0.700912" mass="1.9705" diaginertia="0.00529754 0.00492285 0.00184874" />
+          <joint name="Arm_L1_Joint" pos="0 0 0" axis="0 0 1" range="-3.1067 3.1067" actuatorfrcrange="-108 108" damping="3" armature="0.15" frictionloss="0.3" />
+          <geom class="visual" rgba="0 1 0 1" mesh="Arm_L1_Link" />
+          <geom class="collision" rgba="0 1 0 1" mesh="Arm_L1_Link" />
+          <body name="Arm_L2_Link" quat="0.707105 0.707108 0 0">
+            <inertial pos="0.00013483 0.0617046 0.00950639" quat="0.643375 0.765551 -3.57328e-05 0.000294349" mass="2.24398" diaginertia="0.0119493 0.0112156 0.00338374" />
+            <joint name="Arm_L2_Joint" pos="0 0 0" axis="0 0 1" range="-2.0944 -0.1" actuatorfrcrange="-108 108" damping="3" armature="0.15" frictionloss="0.3" />
+            <geom class="visual" rgba="0 0 1 1" mesh="Arm_L2_Link" />
+            <geom class="collision" rgba="0 0 1 1" mesh="Arm_L2_Link" />
+            <body name="Arm_L3_Link" pos="0 0.287 0" quat="0.707105 -0.707108 0 0">
+              <inertial pos="0.00968562 0.00246599 -0.046273" quat="0.782446 -0.0355285 -0.0735516 0.617337" mass="1.9242" diaginertia="0.00765215 0.00730789 0.0016338" />
+              <joint name="Arm_L3_Joint" pos="0 0 0" axis="0 0 1" range="-3.1067 3.1067" actuatorfrcrange="-66 66" damping="2.5" armature="0.12" frictionloss="0.25" />
+              <geom class="visual" rgba="1 0 0 1" mesh="Arm_L3_Link" />
+              <geom class="collision" rgba="1 0 0 1" mesh="Arm_L3_Link" />
+              <body name="Arm_L4_Link" pos="0.018 0 0" quat="-2.59734e-06 2.59735e-06 -0.707108 0.707105">
+                <inertial pos="0.00850411 -0.0471822 0.00941509" quat="0.741754 0.628694 0.0774999 -0.220317" mass="1.6085" diaginertia="0.00603244 0.00535875 0.00198228" />
+                <joint name="Arm_L4_Joint" pos="0 0 0" axis="0 0 1" range="-2.5307 -0.1" actuatorfrcrange="-66 66" damping="2.5" armature="0.12" frictionloss="0.25" />
+                <geom class="visual" rgba="0 1 0 1" mesh="Arm_L4_Link" />
+                <geom name="Arm_L4_Link_collision" class="collision" rgba="0 1 0 1" mesh="Arm_L4_Link" />
+                <body name="Arm_L5_Link" pos="0.018 -0.314 0" quat="-2.59734e-06 2.59735e-06 -0.707108 0.707105">
+                  <inertial pos="0.00051127 0.00101064 -0.112717" quat="0.999438 -0.00995046 -0.00675781 0.0312997" mass="2.0883" diaginertia="0.00626924 0.00564262 0.00243436" />
+                  <joint name="Arm_L5_Joint" pos="0 0 0" axis="0 0 1" range="-3.1067 3.1067" actuatorfrcrange="-18 18" damping="1.5" armature="0.08" frictionloss="0.15" />
+                  <geom class="visual" rgba="0 0 1 1" mesh="Arm_L5_Link" />
+                  <geom name="Arm_L5_Link_collision" class="collision" rgba="0 0 1 1" fromto="0 0 0 0 0 -0.15" size="0.06" />
+                  <body name="Arm_L6_Link" quat="0.499998 0.5 -0.5 0.500002">
+                    <inertial pos="0.00032659 0.00531336 -0.00030182" quat="0.718188 -0.0225408 0.00407878 0.695472" mass="0.72496" diaginertia="0.000517351 0.000460362 0.000425057" />
+                    <joint name="Arm_L6_Joint" pos="0 0 0" axis="0 0 1" range="-1.0472 1.0472" actuatorfrcrange="-18 18" damping="1.2" armature="0.06" frictionloss="0.12" />
+                    <geom class="visual" rgba="1 0 0 1" mesh="Arm_L6_Link" />
+                    <geom class="collision" rgba="1 0 0 1" mesh="Arm_L6_Link" />
+                    <body name="Arm_L7_Link" quat="0.499998 0.5 -0.5 0.500002">
+                      <inertial pos="-9.108e-05 -0.0315325 0.00889371" quat="0.84481 0.535026 0.00229584 -0.00609725" mass="0.68606" diaginertia="0.00173691 0.00153911 0.000745263" />
+                      <joint name="Arm_L7_Joint" pos="0 0 0" axis="0 0 1" range="-1.5708 1.5708" actuatorfrcrange="-18 18" damping="1.2" armature="0.05" frictionloss="0.1" />
+                      <geom class="visual" rgba="0 1 0 1" mesh="Arm_L7_Link" />
+                      <geom class="collision" rgba="0 1 0 1" mesh="Arm_L7_Link" />
+                      <body name="flange_L" pos="0 -0.088 0" quat="0.499998 0.5 -0.5 0.500002">
+                        <body name="left_gripper_mount" pos="0 0 0" quat="0.7071068 0 0 -0.7071068">
+                          <body name="left_gripper_base" pos="0 0 0">
+                            <inertial pos="-0.000142 0.005641 0.0488811" mass="0.4064271" fullinertia="0.0002976 0.0004831 0.0004721 -0.0000024 0.0000011 -0.0000721" />
+                            <geom group="2" type="mesh" contype="0" conaffinity="0" material="gripper_base_mat" mesh="left_gripper_base_mesh" />
+                            <geom group="3" type="mesh" rgba="0.9373 0.2667 0.2667 1" contype="1" conaffinity="1" condim="4" friction="0.8 0.05 0.01" mesh="left_gripper_base_mesh" />
+                            <camera name="left_wrist_camera" pos="0.01 0.01 0.115" xyaxes="-1 0 0 0 0.7 0.866" fovy="105" />
+                            <body name="left_gripper_left_finger" pos="0.105 -0.01 0.0916374" quat="0.0061005 0.0025269 -0.3826751 -0.38">
+                              <joint name="left_gripper_left_finger_joint" type="slide" axis="1 0 0" limited="true" range="0 0.05" damping="8" armature="0.01" frictionloss="0.2" />
+                              <inertial pos="-0.0166753 -0.0028108 0.0127051" mass="0.031395" fullinertia="0.0000116 0.0000048 0.0000102 -0.0000013 0 0" />
+                              <geom group="2" type="mesh" contype="0" conaffinity="0" material="gripper_finger_mat" mesh="left_gripper_left_finger_mesh" />
+                              <geom group="3" type="mesh" rgba="0.9373 0.2667 0.2667 1" contype="1" conaffinity="1" condim="4" friction="0.8 0.05 0.01" mesh="left_gripper_left_finger_mesh" />
+                            </body>
+                            <body name="left_gripper_right_finger" pos="-0.05 -0.002 0.1105" quat="0.3826751 0.37 0.0061006 0.0025269">
+                              <joint name="left_gripper_right_finger_joint" type="slide" axis="1 0 0" limited="true" range="0 0.05" damping="8" armature="0.01" frictionloss="0.2" />
+                              <inertial pos="0.0089293 -0.0028108 0.0127051" mass="0.031395" fullinertia="0.0000116 0.0000048 0.0000102 -0.0000013 0 0" />
+                              <geom group="2" type="mesh" contype="0" conaffinity="0" material="gripper_finger_mat" mesh="left_gripper_right_finger_mesh" />
+                              <geom group="3" type="mesh" rgba="0.9373 0.2667 0.2667 1" contype="1" conaffinity="1" condim="4" friction="0.8 0.05 0.01" mesh="left_gripper_right_finger_mesh" />
+                            </body>
+                            <body name="left_tool" pos="0 0 0" quat="0.5 -0.5 -0.5 -0.5">
+                              <site name="left_tool_site" pos="0 0 0" size="0.005" />
+                            </body>
+                          </body>
+                        </body>
+                      </body>
+                    </body>
+                  </body>
+                </body>
+              </body>
+            </body>
+          </body>
+        </body>
+      </body>
+      <body name="Base_R" pos="0 -0.026 1.121" quat="0.707388 0.706825 0 0">
+        <inertial pos="0.00032267 4.798e-05 0.0406635" quat="0.582787 0.36644 -0.620273 0.375956" mass="1.1962" diaginertia="0.00127375 0.00107367 0.00106904" />
+        <geom class="visual" rgba="1 0 0 1" mesh="Base_R" />
+        <geom class="collision" rgba="1 0 0 1" mesh="Base_R" />
+        <body name="Arm_R1_Link" pos="0 0 0.1745">
+          <inertial pos="2.347e-05 0.0055715 -0.03076" quat="0.711599 0.0343691 -0.0341702 0.700912" mass="1.9705" diaginertia="0.00529754 0.00492285 0.00184874" />
+          <joint name="Arm_R1_Joint" pos="0 0 0" axis="0 0 1" range="-3.1067 3.1067" actuatorfrcrange="-108 108" damping="3" armature="0.15" frictionloss="0.3" />
+          <geom class="visual" rgba="0 1 0 1" mesh="Arm_R1_Link" />
+          <geom class="collision" rgba="0 1 0 1" mesh="Arm_R1_Link" />
+          <body name="Arm_R2_Link" quat="0.707105 0.707108 0 0">
+            <inertial pos="0.00013483 0.0617046 0.00950639" quat="0.643375 0.765551 -3.57328e-05 0.000294349" mass="2.24398" diaginertia="0.0119493 0.0112156 0.00338374" />
+            <joint name="Arm_R2_Joint" pos="0 0 0" axis="0 0 1" range="-2.0944 -0.1" actuatorfrcrange="-108 108" damping="3" armature="0.15" frictionloss="0.3" />
+            <geom class="visual" rgba="0 0 1 1" mesh="Arm_R2_Link" />
+            <geom class="collision" rgba="0 0 1 1" mesh="Arm_R2_Link" />
+            <body name="Arm_R3_Link" pos="0 0.287 0" quat="0.707105 -0.707108 0 0">
+              <inertial pos="0.00968562 0.00246599 -0.046273" quat="0.782446 -0.0355285 -0.0735516 0.617337" mass="1.9242" diaginertia="0.00765215 0.00730789 0.0016338" />
+              <joint name="Arm_R3_Joint" pos="0 0 0" axis="0 0 1" range="-3.1067 3.1067" actuatorfrcrange="-66 66" damping="2.5" armature="0.12" frictionloss="0.25" />
+              <geom class="visual" rgba="1 0 0 1" mesh="Arm_R3_Link" />
+              <geom class="collision" rgba="1 0 0 1" mesh="Arm_R3_Link" />
+              <body name="Arm_R4_Link" pos="0.018 0 0" quat="-2.59734e-06 2.59735e-06 -0.707108 0.707105">
+                <inertial pos="0.00850411 -0.0471822 0.00941509" quat="0.741754 0.628694 0.0774999 -0.220317" mass="1.6085" diaginertia="0.00603244 0.00535875 0.00198228" />
+                <joint name="Arm_R4_Joint" pos="0 0 0" axis="0 0 1" range="-2.5307 -0.1" actuatorfrcrange="-66 66" damping="2.5" armature="0.12" frictionloss="0.25" />
+                <geom class="visual" rgba="0 1 0 1" mesh="Arm_R4_Link" />
+                <geom name="Arm_R4_Link_collision" class="collision" rgba="0 1 0 1" mesh="Arm_R4_Link" />
+                <body name="Arm_R5_Link" pos="0.018 -0.314 0" quat="-2.59734e-06 2.59735e-06 -0.707108 0.707105">
+                  <inertial pos="-0.00051127 -0.00101064 -0.112717" quat="0.999438 0.00995046 0.00675781 0.0312997" mass="2.0883" diaginertia="0.00626924 0.00564262 0.00243436" />
+                  <joint name="Arm_R5_Joint" pos="0 0 0" axis="0 0 1" range="-3.1067 3.1067" actuatorfrcrange="-18 18" damping="1.5" armature="0.08" frictionloss="0.15" />
+                  <geom class="visual" rgba="0 0 1 1" mesh="Arm_R5_Link" />
+                  <geom name="Arm_R5_Link_collision" class="collision" rgba="0 0 1 1" fromto="0 0 0 0 0 -0.15" size="0.06" />
+                  <body name="Arm_R6_Link" quat="0.499998 0.5 -0.5 0.500002">
+                    <inertial pos="0.00032659 -0.00531336 0.00030182" quat="0.695472 -0.00407878 0.0225408 0.718188" mass="0.72496" diaginertia="0.000517351 0.000460362 0.000425057" />
+                    <joint name="Arm_R6_Joint" pos="0 0 0" axis="0 0 1" range="-1.0472 1.0472" actuatorfrcrange="-18 18" damping="1.2" armature="0.06" frictionloss="0.12" />
+                    <geom class="visual" rgba="1 0 0 1" mesh="Arm_R6_Link" />
+                    <geom class="collision" rgba="1 0 0 1" mesh="Arm_R6_Link" />
+                    <body name="Arm_R7_Link" quat="0.499998 0.5 -0.5 0.500002">
+                      <inertial pos="-9.108e-05 -0.0315325 0.00889371" quat="0.84481 0.535026 0.00229584 -0.00609725" mass="0.68606" diaginertia="0.00173691 0.00153911 0.000745263" />
+                      <joint name="Arm_R7_Joint" pos="0 0 0" axis="0 0 1" range="-1.5708 1.5708" actuatorfrcrange="-18 18" damping="1.2" armature="0.05" frictionloss="0.1" />
+                      <geom class="visual" rgba="0 1 0 1" mesh="Arm_R7_Link" />
+                      <geom class="collision" rgba="0 1 0 1" mesh="Arm_R7_Link" />
+                      <body name="flange_R" pos="0 -0.088 0" quat="0.499998 0.5 -0.5 0.500002">
+                        <body name="right_gripper_mount" pos="0 0 0" quat="0.7071068 0 0 -0.7071068">
+                          <body name="right_gripper_base" pos="0 0 0">
+                            <inertial pos="-0.000142 0.005641 0.0488811" mass="0.4064271" fullinertia="0.0002976 0.0004831 0.0004721 -0.0000024 0.0000011 -0.0000721" />
+                            <geom group="2" type="mesh" contype="0" conaffinity="0" material="gripper_base_mat" mesh="right_gripper_base_mesh" />
+                            <geom group="3" type="mesh" rgba="0.9373 0.2667 0.2667 1" contype="1" conaffinity="1" condim="4" friction="0.8 0.05 0.01" mesh="right_gripper_base_mesh" />
+                            <camera name="right_wrist_camera" pos="0.01 0.01 0.115" xyaxes="-1 0 0 0 0.7 0.866" fovy="105" />
+                            <body name="right_gripper_left_finger" pos="0.105 -0.01 0.0916374" quat="0.0061005 0.0025269 -0.3826751 -0.38">
+                              <joint name="right_gripper_left_finger_joint" type="slide" axis="1 0 0" limited="true" range="0 0.05" damping="8" armature="0.01" frictionloss="0.2" />
+                              <inertial pos="-0.0166753 -0.0028108 0.0127051" mass="0.031395" fullinertia="0.0000116 0.0000048 0.0000102 -0.0000013 0 0" />
+                              <geom group="2" type="mesh" contype="0" conaffinity="0" material="gripper_finger_mat" mesh="right_gripper_left_finger_mesh" />
+                              <geom group="3" type="mesh" rgba="0.9373 0.2667 0.2667 1" contype="1" conaffinity="1" condim="4" friction="0.8 0.05 0.01" mesh="right_gripper_left_finger_mesh" />
+                            </body>
+                            <body name="right_gripper_right_finger" pos="-0.05 -0.002 0.1105" quat="0.3826751 0.37 0.0061006 0.0025269">
+                              <joint name="right_gripper_right_finger_joint" type="slide" axis="1 0 0" limited="true" range="0 0.05" damping="8" armature="0.01" frictionloss="0.2" />
+                              <inertial pos="0.0089293 -0.0028108 0.0127051" mass="0.031395" fullinertia="0.0000116 0.0000048 0.0000102 -0.0000013 0 0" />
+                              <geom group="2" type="mesh" contype="0" conaffinity="0" material="gripper_finger_mat" mesh="right_gripper_right_finger_mesh" />
+                              <geom group="3" type="mesh" rgba="0.9373 0.2667 0.2667 1" contype="1" conaffinity="1" condim="4" friction="0.8 0.05 0.01" mesh="right_gripper_right_finger_mesh" />
+                            </body>
+                            <body name="right_tool" pos="0 0 0" quat="0.5 -0.5 -0.5 -0.5">
+                              <site name="right_tool_site" pos="0 0 0" size="0.005" />
+                            </body>
+                          </body>
+                        </body> 
+                      </body>
+                    </body>
+                  </body>
+                </body>
+              </body>
+            </body>
+          </body>
+        </body>
+      </body>
+    </body>
+  </worldbody>
+
+  <actuator>
+    <position name="Arm_L1_Joint" joint="Arm_L1_Joint" kp="700" kv="140" ctrlrange="-3.1067 3.1067" />
+    <position name="Arm_L2_Joint" joint="Arm_L2_Joint" kp="700" kv="140" ctrlrange="-2.0944 -0.1" />
+    <position name="Arm_L3_Joint" joint="Arm_L3_Joint" kp="650" kv="120" ctrlrange="-3.1067 3.1067" />
+    <position name="Arm_L4_Joint" joint="Arm_L4_Joint" kp="650" kv="120" ctrlrange="-2.5307 -0.1" />
+    <position name="Arm_L5_Joint" joint="Arm_L5_Joint" kp="350" kv="70" ctrlrange="-3.1067 3.1067" />
+    <position name="Arm_L6_Joint" joint="Arm_L6_Joint" kp="250" kv="50" ctrlrange="-1.0472 1.0472" />
+    <position name="Arm_L7_Joint" joint="Arm_L7_Joint" kp="250" kv="50" ctrlrange="-1.5708 1.5708" />
+
+    <position name="Arm_R1_Joint" joint="Arm_R1_Joint" kp="700" kv="140" ctrlrange="-3.1067 3.1067" />
+    <position name="Arm_R2_Joint" joint="Arm_R2_Joint" kp="700" kv="140" ctrlrange="-2.0944 -0.1" />
+    <position name="Arm_R3_Joint" joint="Arm_R3_Joint" kp="650" kv="120" ctrlrange="-3.1067 3.1067" />
+    <position name="Arm_R4_Joint" joint="Arm_R4_Joint" kp="650" kv="120" ctrlrange="-2.5307 -0.1" />
+    <position name="Arm_R5_Joint" joint="Arm_R5_Joint" kp="350" kv="70" ctrlrange="-3.1067 3.1067" />
+    <position name="Arm_R6_Joint" joint="Arm_R6_Joint" kp="250" kv="50" ctrlrange="-1.0472 1.0472" />
+    <position name="Arm_R7_Joint" joint="Arm_R7_Joint" kp="250" kv="50" ctrlrange="-1.5708 1.5708" />
+    <position name="left_gripper" joint="left_gripper_left_finger_joint" kp="100" kv="10" ctrlrange="0 0.05" forcerange="-30 30" />
+    <position name="right_gripper" joint="right_gripper_left_finger_joint" kp="100" kv="10" ctrlrange="0 0.05" forcerange="-30 30" />
+  </actuator>
+
+  <keyframe>
+    <key name="home" qpos="1.7 -1.1 -1.1 -2 -0.37 0.13 0.55 0 0 -1.7 -1.1 1.1 -2 0.37 0.13 -0.55 0 0" ctrl="1.7 -1.1 -1.1 -2 -0.37 0.13 0.55 -1.7 -1.1 1.1 -2 0.37 0.13 -0.55 0 0" />
+  </keyframe>
+</mujoco>
\ No newline at end of file
diff --git a/mobile/marvin_pro/urdf/marvin_pro_mink_with_gripper.urdf b/mobile/marvin_pro/urdf/marvin_pro_mink_with_gripper.urdf
new file mode 100644
index 0000000000000000000000000000000000000000..47ced0a7bcfdf667676f88acad97bb4d4d7b2b2f
--- /dev/null
+++ b/mobile/marvin_pro/urdf/marvin_pro_mink_with_gripper.urdf
@@ -0,0 +1,818 @@
+<robot name="marvin_pro_mink_with_gripper">
+
+  <link name="base_link">
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="package://ace_description/urdf/marvin_pro/meshes/marvin_pro/meshes/base/ZJ_Robot_link.STL" />
+      </geometry>
+      <material name="base_link_visual_0_material_0">
+        <color rgba="0.80000392 0.80000392 0.80000392 1.00000000" />
+      </material>
+    </visual>
+    <collision name="base_link_collision">
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <cylinder radius="0.1" length="0.4" />
+      </geometry>
+    </collision>
+    <inertial>
+      <origin xyz="0.0081994 -0.0000051 0.47835" rpy="0.0000623 0.0062721 0.0034744" />
+      <mass value="18.846" />
+      <inertia ixx="0.463463" ixy="0" ixz="0" iyy="0.46325" iyz="0" izz="0.377657" />
+    </inertial>
+  </link>
+
+  <link name="tracking_base_link">
+    <inertial>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <mass value="0" />
+      <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
+    </inertial>
+  </link>
+
+  <link name="Base_L">
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="package://ace_description/urdf/marvin_pro/meshes/marvin_pro/meshes/m6/Base_L.STL" />
+      </geometry>
+      <material name="Base_L_visual_0_material_0">
+        <color rgba="1.00000000 0.00000392 0.00000392 1.00000000" />
+      </material>
+    </visual>
+    <collision name="Base_L_geom_1">
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="package://ace_description/urdf/marvin_pro/meshes/marvin_pro/meshes/m6/Base_L.STL" />
+      </geometry>
+    </collision>
+    <inertial>
+      <origin xyz="0.0003227 0.000048 0.0406635" rpy="-2.3400985 -1.5160948 -2.8373052" />
+      <mass value="1.1962" />
+      <inertia ixx="0.0012737" ixy="0" ixz="0" iyy="0.0010737" iyz="0" izz="0.001069" />
+    </inertial>
+  </link>
+
+  <link name="Arm_L1_Link">
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="package://ace_description/urdf/marvin_pro/meshes/marvin_pro/meshes/m6/Arm_L1_Link.STL" />
+      </geometry>
+      <material name="Arm_L1_Link_visual_0_material_0">
+        <color rgba="0.00000392 1.00000000 0.00000392 1.00000000" />
+      </material>
+    </visual>
+    <collision name="Arm_L1_Link_geom_1">
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="package://ace_description/urdf/marvin_pro/meshes/marvin_pro/meshes/m6/Arm_L1_Link.STL" />
+      </geometry>
+    </collision>
+    <inertial>
+      <origin xyz="0.0000235 0.0055715 -0.03076" rpy="0.0010182 -0.0969623 1.5556153" />
+      <mass value="1.9705" />
+      <inertia ixx="0.0052975" ixy="0" ixz="0" iyy="0.0049229" iyz="0" izz="0.0018487" />
+    </inertial>
+  </link>
+
+  <link name="Arm_L2_Link">
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="package://ace_description/urdf/marvin_pro/meshes/marvin_pro/meshes/m6/Arm_L2_Link.STL" />
+      </geometry>
+      <material name="Arm_L2_Link_visual_0_material_0">
+        <color rgba="0.00000392 0.00000392 1.00000000 1.00000000" />
+      </material>
+    </visual>
+    <collision name="Arm_L2_Link_geom_1">
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="package://ace_description/urdf/marvin_pro/meshes/marvin_pro/meshes/m6/Arm_L2_Link.STL" />
+      </geometry>
+    </collision>
+    <inertial>
+      <origin xyz="0.0001348 0.0617046 0.0095064" rpy="1.7437949 -0.0004967 0.000324" />
+      <mass value="2.24398" />
+      <inertia ixx="0.0119493" ixy="0" ixz="0" iyy="0.0112156" iyz="0" izz="0.0033837" />
+    </inertial>
+  </link>
+
+  <link name="Arm_L3_Link">
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="package://ace_description/urdf/marvin_pro/meshes/marvin_pro/meshes/m6/Arm_L3_Link.STL" />
+      </geometry>
+      <material name="Arm_L3_Link_visual_0_material_0">
+        <color rgba="1.00000000 0.00000392 0.00000392 1.00000000" />
+      </material>
+    </visual>
+    <collision name="Arm_L3_Link_geom_1">
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="package://ace_description/urdf/marvin_pro/meshes/marvin_pro/meshes/m6/Arm_L3_Link.STL" />
+      </geometry>
+    </collision>
+    <inertial>
+      <origin xyz="0.0096856 0.002466 -0.046273" rpy="-0.1473158 -0.0712947 1.3412379" />
+      <mass value="1.9242" />
+      <inertia ixx="0.0076521" ixy="0" ixz="0" iyy="0.0073079" iyz="0" izz="0.0016338" />
+    </inertial>
+  </link>
+
+  <link name="Arm_L4_Link">
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="package://ace_description/urdf/marvin_pro/meshes/marvin_pro/meshes/m6/Arm_L4_Link.STL" />
+      </geometry>
+      <material name="Arm_L4_Link_visual_0_material_0">
+        <color rgba="0.00000392 1.00000000 0.00000392 1.00000000" />
+      </material>
+    </visual>
+    <collision name="Arm_L4_Link_collision">
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="package://ace_description/urdf/marvin_pro/meshes/marvin_pro/meshes/m6/Arm_L4_Link.STL" />
+      </geometry>
+    </collision>
+    <inertial>
+      <origin xyz="0.0085041 -0.0471822 0.0094151" rpy="1.3544573 0.4027995 -0.2520096" />
+      <mass value="1.6085" />
+      <inertia ixx="0.0060324" ixy="0" ixz="0" iyy="0.0053587" iyz="0" izz="0.0019823" />
+    </inertial>
+  </link>
+
+  <link name="Arm_L5_Link">
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="package://ace_description/urdf/marvin_pro/meshes/marvin_pro/meshes/m6/Arm_L5_Link.STL" />
+      </geometry>
+      <material name="Arm_L5_Link_visual_0_material_0">
+        <color rgba="0.00000392 0.00000392 1.00000000 1.00000000" />
+      </material>
+    </visual>
+    <collision name="Arm_L5_Link_collision">
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <cylinder radius="0.06" length="0.32" />
+      </geometry>
+    </collision>
+    <inertial>
+      <origin xyz="0.0005113 0.0010106 -0.112717" rpy="-0.0203158 -0.0128855 0.062745" />
+      <mass value="2.0883" />
+      <inertia ixx="0.0062692" ixy="0" ixz="0" iyy="0.0056426" iyz="0" izz="0.0024344" />
+    </inertial>
+  </link>
+
+  <link name="Arm_L6_Link">
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="package://ace_description/urdf/marvin_pro/meshes/marvin_pro/meshes/m6/Arm_L6_Link.STL" />
+      </geometry>
+      <material name="Arm_L6_Link_visual_0_material_0">
+        <color rgba="1.00000000 0.00000392 0.00000392 1.00000000" />
+      </material>
+    </visual>
+    <collision name="Arm_L6_Link_geom_1">
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="package://ace_description/urdf/marvin_pro/meshes/marvin_pro/meshes/m6/Arm_L6_Link.STL" />
+      </geometry>
+    </collision>
+    <inertial>
+      <origin xyz="0.0003266 0.0053134 -0.0003018" rpy="-0.0267254 0.0372202 1.5381638" />
+      <mass value="0.72496" />
+      <inertia ixx="0.0005174" ixy="0" ixz="0" iyy="0.0004604" iyz="0" izz="0.0004251" />
+    </inertial>
+  </link>
+
+  <link name="Arm_L7_Link">
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="package://ace_description/urdf/marvin_pro/meshes/marvin_pro/meshes/m6/Arm_L7_Link.STL" />
+      </geometry>
+      <material name="Arm_L7_Link_visual_0_material_0">
+        <color rgba="0.00000392 1.00000000 0.00000392 1.00000000" />
+      </material>
+    </visual>
+    <collision name="Arm_L7_Link_geom_1">
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="package://ace_description/urdf/marvin_pro/meshes/marvin_pro/meshes/m6/Arm_L7_Link.STL" />
+      </geometry>
+    </collision>
+    <inertial>
+      <origin xyz="-0.0000911 -0.0315325 0.0088937" rpy="1.1290636 0.0104037 -0.0078459" />
+      <mass value="0.68606" />
+      <inertia ixx="0.0017369" ixy="0" ixz="0" iyy="0.0015391" iyz="0" izz="0.0007453" />
+    </inertial>
+  </link>
+
+  <link name="flange_L">
+    <inertial>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <mass value="0" />
+      <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
+    </inertial>
+  </link>
+
+  <link name="left_gripper_mount">
+    <inertial>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <mass value="0" />
+      <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
+    </inertial>
+  </link>
+
+  <link name="left_gripper_base">
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="package://ace_description/urdf/marvin_pro/meshes/marvin_pro/meshes/omnigripper/omnigripper_straight.STL" />
+      </geometry>
+      <material name="gripper_base_mat">
+        <color rgba="0.79216078 0.81961176 0.92941569 1.00000000" />
+      </material>
+    </visual>
+    <collision name="left_gripper_base_geom_1">
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="package://ace_description/urdf/marvin_pro/meshes/marvin_pro/meshes/omnigripper/omnigripper_straight.STL" />
+      </geometry>
+    </collision>
+    <inertial>
+      <origin xyz="-0.000142 0.005641 0.0488811" rpy="0 0 0" />
+      <mass value="0.4064271" />
+      <inertia ixx="0.0002976" ixy="-0.0000024" ixz="0.0000011" iyy="0.0004831" iyz="-0.0000721" izz="0.0004721" />
+    </inertial>
+  </link>
+
+  <link name="left_gripper_left_finger">
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="package://ace_description/urdf/marvin_pro/meshes/marvin_pro/meshes/omnigripper/Left_Chipper_Base.STL" />
+      </geometry>
+      <material name="gripper_finger_mat">
+        <color rgba="0.60392549 0.60392549 0.60392549 1.00000000" />
+      </material>
+    </visual>
+    <collision name="left_gripper_left_finger_geom_1">
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="package://ace_description/urdf/marvin_pro/meshes/marvin_pro/meshes/omnigripper/Left_Chipper_Base.STL" />
+      </geometry>
+    </collision>
+    <inertial>
+      <origin xyz="-0.0166753 -0.0028108 0.0127051" rpy="0 0 0" />
+      <mass value="0.031395" />
+      <inertia ixx="0.0000116" ixy="-0.0000013" ixz="0" iyy="0.0000048" iyz="0" izz="0.0000102" />
+    </inertial>
+  </link>
+
+  <link name="left_gripper_right_finger">
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="package://ace_description/urdf/marvin_pro/meshes/marvin_pro/meshes/omnigripper/Right_Chipper_Base.STL" />
+      </geometry>
+      <material name="gripper_finger_mat">
+        <color rgba="0.60392549 0.60392549 0.60392549 1.00000000" />
+      </material>
+    </visual>
+    <collision name="left_gripper_right_finger_geom_1">
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="package://ace_description/urdf/marvin_pro/meshes/marvin_pro/meshes/omnigripper/Right_Chipper_Base.STL" />
+      </geometry>
+    </collision>
+    <inertial>
+      <origin xyz="0.0089293 -0.0028108 0.0127051" rpy="0 0 0" />
+      <mass value="0.031395" />
+      <inertia ixx="0.0000116" ixy="-0.0000013" ixz="0" iyy="0.0000048" iyz="0" izz="0.0000102" />
+    </inertial>
+  </link>
+
+  <link name="left_tool">
+    <inertial>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <mass value="0" />
+      <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
+    </inertial>
+  </link>
+
+  <link name="Base_R">
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="package://ace_description/urdf/marvin_pro/meshes/marvin_pro/meshes/m6/Base_R.STL" />
+      </geometry>
+      <material name="Base_R_visual_0_material_0">
+        <color rgba="1.00000000 0.00000392 0.00000392 1.00000000" />
+      </material>
+    </visual>
+    <collision name="Base_R_geom_1">
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="package://ace_description/urdf/marvin_pro/meshes/marvin_pro/meshes/m6/Base_R.STL" />
+      </geometry>
+    </collision>
+    <inertial>
+      <origin xyz="0.0003227 0.000048 0.0406635" rpy="-2.3400985 -1.5160948 -2.8373052" />
+      <mass value="1.1962" />
+      <inertia ixx="0.0012737" ixy="0" ixz="0" iyy="0.0010737" iyz="0" izz="0.001069" />
+    </inertial>
+  </link>
+
+  <link name="Arm_R1_Link">
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="package://ace_description/urdf/marvin_pro/meshes/marvin_pro/meshes/m6/Arm_R1_Link.STL" />
+      </geometry>
+      <material name="Arm_R1_Link_visual_0_material_0">
+        <color rgba="0.00000392 1.00000000 0.00000392 1.00000000" />
+      </material>
+    </visual>
+    <collision name="Arm_R1_Link_geom_1">
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="package://ace_description/urdf/marvin_pro/meshes/marvin_pro/meshes/m6/Arm_R1_Link.STL" />
+      </geometry>
+    </collision>
+    <inertial>
+      <origin xyz="0.0000235 0.0055715 -0.03076" rpy="0.0010182 -0.0969623 1.5556153" />
+      <mass value="1.9705" />
+      <inertia ixx="0.0052975" ixy="0" ixz="0" iyy="0.0049229" iyz="0" izz="0.0018487" />
+    </inertial>
+  </link>
+
+  <link name="Arm_R2_Link">
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="package://ace_description/urdf/marvin_pro/meshes/marvin_pro/meshes/m6/Arm_R2_Link.STL" />
+      </geometry>
+      <material name="Arm_R2_Link_visual_0_material_0">
+        <color rgba="0.00000392 0.00000392 1.00000000 1.00000000" />
+      </material>
+    </visual>
+    <collision name="Arm_R2_Link_geom_1">
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="package://ace_description/urdf/marvin_pro/meshes/marvin_pro/meshes/m6/Arm_R2_Link.STL" />
+      </geometry>
+    </collision>
+    <inertial>
+      <origin xyz="0.0001348 0.0617046 0.0095064" rpy="1.7437949 -0.0004967 0.000324" />
+      <mass value="2.24398" />
+      <inertia ixx="0.0119493" ixy="0" ixz="0" iyy="0.0112156" iyz="0" izz="0.0033837" />
+    </inertial>
+  </link>
+
+  <link name="Arm_R3_Link">
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="package://ace_description/urdf/marvin_pro/meshes/marvin_pro/meshes/m6/Arm_R3_Link.STL" />
+      </geometry>
+      <material name="Arm_R3_Link_visual_0_material_0">
+        <color rgba="1.00000000 0.00000392 0.00000392 1.00000000" />
+      </material>
+    </visual>
+    <collision name="Arm_R3_Link_geom_1">
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="package://ace_description/urdf/marvin_pro/meshes/marvin_pro/meshes/m6/Arm_R3_Link.STL" />
+      </geometry>
+    </collision>
+    <inertial>
+      <origin xyz="0.0096856 0.002466 -0.046273" rpy="-0.1473158 -0.0712947 1.3412379" />
+      <mass value="1.9242" />
+      <inertia ixx="0.0076521" ixy="0" ixz="0" iyy="0.0073079" iyz="0" izz="0.0016338" />
+    </inertial>
+  </link>
+
+  <link name="Arm_R4_Link">
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="package://ace_description/urdf/marvin_pro/meshes/marvin_pro/meshes/m6/Arm_R4_Link.STL" />
+      </geometry>
+      <material name="Arm_R4_Link_visual_0_material_0">
+        <color rgba="0.00000392 1.00000000 0.00000392 1.00000000" />
+      </material>
+    </visual>
+    <collision name="Arm_R4_Link_collision">
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="package://ace_description/urdf/marvin_pro/meshes/marvin_pro/meshes/m6/Arm_R4_Link.STL" />
+      </geometry>
+    </collision>
+    <inertial>
+      <origin xyz="0.0085041 -0.0471822 0.0094151" rpy="1.3544573 0.4027995 -0.2520096" />
+      <mass value="1.6085" />
+      <inertia ixx="0.0060324" ixy="0" ixz="0" iyy="0.0053587" iyz="0" izz="0.0019823" />
+    </inertial>
+  </link>
+
+  <link name="Arm_R5_Link">
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="package://ace_description/urdf/marvin_pro/meshes/marvin_pro/meshes/m6/Arm_R5_Link.STL" />
+      </geometry>
+      <material name="Arm_R5_Link_visual_0_material_0">
+        <color rgba="0.00000392 0.00000392 1.00000000 1.00000000" />
+      </material>
+    </visual>
+    <collision name="Arm_R5_Link_collision">
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <cylinder radius="0.06" length="0.32" />
+      </geometry>
+    </collision>
+    <inertial>
+      <origin xyz="-0.0005113 -0.0010106 -0.112717" rpy="0.0203158 0.0128855 0.062745" />
+      <mass value="2.0883" />
+      <inertia ixx="0.0062692" ixy="0" ixz="0" iyy="0.0056426" iyz="0" izz="0.0024344" />
+    </inertial>
+  </link>
+
+  <link name="Arm_R6_Link">
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="package://ace_description/urdf/marvin_pro/meshes/marvin_pro/meshes/m6/Arm_R6_Link.STL" />
+      </geometry>
+      <material name="Arm_R6_Link_visual_0_material_0">
+        <color rgba="1.00000000 0.00000392 0.00000392 1.00000000" />
+      </material>
+    </visual>
+    <collision name="Arm_R6_Link_geom_1">
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="package://ace_description/urdf/marvin_pro/meshes/marvin_pro/meshes/m6/Arm_R6_Link.STL" />
+      </geometry>
+    </collision>
+    <inertial>
+      <origin xyz="0.0003266 -0.0053134 0.0003018" rpy="0.0267254 0.0372202 1.6034289" />
+      <mass value="0.72496" />
+      <inertia ixx="0.0005174" ixy="0" ixz="0" iyy="0.0004604" iyz="0" izz="0.0004251" />
+    </inertial>
+  </link>
+
+  <link name="Arm_R7_Link">
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="package://ace_description/urdf/marvin_pro/meshes/marvin_pro/meshes/m6/Arm_R7_Link.STL" />
+      </geometry>
+      <material name="Arm_R7_Link_visual_0_material_0">
+        <color rgba="0.00000392 1.00000000 0.00000392 1.00000000" />
+      </material>
+    </visual>
+    <collision name="Arm_R7_Link_geom_1">
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="package://ace_description/urdf/marvin_pro/meshes/marvin_pro/meshes/m6/Arm_R7_Link.STL" />
+      </geometry>
+    </collision>
+    <inertial>
+      <origin xyz="-0.0000911 -0.0315325 0.0088937" rpy="1.1290636 0.0104037 -0.0078459" />
+      <mass value="0.68606" />
+      <inertia ixx="0.0017369" ixy="0" ixz="0" iyy="0.0015391" iyz="0" izz="0.0007453" />
+    </inertial>
+  </link>
+
+  <link name="flange_R">
+    <inertial>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <mass value="0" />
+      <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
+    </inertial>
+  </link>
+
+  <link name="right_gripper_mount">
+    <inertial>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <mass value="0" />
+      <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
+    </inertial>
+  </link>
+
+  <link name="right_gripper_base">
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="package://ace_description/urdf/marvin_pro/meshes/marvin_pro/meshes/omnigripper/omnigripper_straight.STL" />
+      </geometry>
+      <material name="gripper_base_mat">
+        <color rgba="0.79216078 0.81961176 0.92941569 1.00000000" />
+      </material>
+    </visual>
+    <collision name="right_gripper_base_geom_1">
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="package://ace_description/urdf/marvin_pro/meshes/marvin_pro/meshes/omnigripper/omnigripper_straight.STL" />
+      </geometry>
+    </collision>
+    <inertial>
+      <origin xyz="-0.000142 0.005641 0.0488811" rpy="0 0 0" />
+      <mass value="0.4064271" />
+      <inertia ixx="0.0002976" ixy="-0.0000024" ixz="0.0000011" iyy="0.0004831" iyz="-0.0000721" izz="0.0004721" />
+    </inertial>
+  </link>
+
+  <link name="right_gripper_left_finger">
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="package://ace_description/urdf/marvin_pro/meshes/marvin_pro/meshes/omnigripper/Left_Chipper_Base.STL" />
+      </geometry>
+      <material name="gripper_finger_mat">
+        <color rgba="0.60392549 0.60392549 0.60392549 1.00000000" />
+      </material>
+    </visual>
+    <collision name="right_gripper_left_finger_geom_1">
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="package://ace_description/urdf/marvin_pro/meshes/marvin_pro/meshes/omnigripper/Left_Chipper_Base.STL" />
+      </geometry>
+    </collision>
+    <inertial>
+      <origin xyz="-0.0166753 -0.0028108 0.0127051" rpy="0 0 0" />
+      <mass value="0.031395" />
+      <inertia ixx="0.0000116" ixy="-0.0000013" ixz="0" iyy="0.0000048" iyz="0" izz="0.0000102" />
+    </inertial>
+  </link>
+
+  <link name="right_gripper_right_finger">
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="package://ace_description/urdf/marvin_pro/meshes/marvin_pro/meshes/omnigripper/Right_Chipper_Base.STL" />
+      </geometry>
+      <material name="gripper_finger_mat">
+        <color rgba="0.60392549 0.60392549 0.60392549 1.00000000" />
+      </material>
+    </visual>
+    <collision name="right_gripper_right_finger_geom_1">
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="package://ace_description/urdf/marvin_pro/meshes/marvin_pro/meshes/omnigripper/Right_Chipper_Base.STL" />
+      </geometry>
+    </collision>
+    <inertial>
+      <origin xyz="0.0089293 -0.0028108 0.0127051" rpy="0 0 0" />
+      <mass value="0.031395" />
+      <inertia ixx="0.0000116" ixy="-0.0000013" ixz="0" iyy="0.0000048" iyz="0" izz="0.0000102" />
+    </inertial>
+  </link>
+
+  <link name="right_tool">
+    <inertial>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <mass value="0" />
+      <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
+    </inertial>
+  </link>
+
+  <joint name="base_link_to_tracking_base_link" type="fixed">
+    <parent link="base_link" />
+    <child link="tracking_base_link" />
+    <origin xyz="0 0 -0.1" rpy="0 0 0" />
+  </joint>
+
+  <joint name="base_link_to_Base_L" type="fixed">
+    <parent link="base_link" />
+    <child link="Base_L" />
+    <origin xyz="0 0.026 1.121" rpy="-1.5700001 0 0" />
+  </joint>
+
+  <joint name="Arm_L1_Joint" type="revolute">
+    <parent link="Base_L" />
+    <child link="Arm_L1_Link" />
+    <origin xyz="0 0 0.1745" rpy="0 0 0" />
+    <axis xyz="0 0 1" />
+    <limit lower="-3.1067" upper="3.1067" effort="108" velocity="0" />
+    <dynamics damping="3" friction="0.3" />
+  </joint>
+
+  <joint name="Arm_L2_Joint" type="revolute">
+    <parent link="Arm_L1_Link" />
+    <child link="Arm_L2_Link" />
+    <origin xyz="0 0 0" rpy="1.5708006 0 0" />
+    <axis xyz="0 0 1" />
+    <limit lower="-2.0944" upper="-0.1" effort="108" velocity="0" />
+    <dynamics damping="3" friction="0.3" />
+  </joint>
+
+  <joint name="Arm_L3_Joint" type="revolute">
+    <parent link="Arm_L2_Link" />
+    <child link="Arm_L3_Link" />
+    <origin xyz="0 0.287 0" rpy="-1.5708006 0 0" />
+    <axis xyz="0 0 1" />
+    <limit lower="-3.1067" upper="3.1067" effort="66" velocity="0" />
+    <dynamics damping="2.5" friction="0.25" />
+  </joint>
+
+  <joint name="Arm_L4_Joint" type="revolute">
+    <parent link="Arm_L3_Link" />
+    <child link="Arm_L4_Link" />
+    <origin xyz="0.018 0 0" rpy="-1.5708006 0 -3.1415853" />
+    <axis xyz="0 0 1" />
+    <limit lower="-2.5307" upper="-0.1" effort="66" velocity="0" />
+    <dynamics damping="2.5" friction="0.25" />
+  </joint>
+
+  <joint name="Arm_L5_Joint" type="revolute">
+    <parent link="Arm_L4_Link" />
+    <child link="Arm_L5_Link" />
+    <origin xyz="0.018 -0.314 0" rpy="-1.5708006 0 -3.1415853" />
+    <axis xyz="0 0 1" />
+    <limit lower="-3.1067" upper="3.1067" effort="18" velocity="0" />
+    <dynamics damping="1.5" friction="0.15" />
+  </joint>
+
+  <joint name="Arm_L6_Joint" type="revolute">
+    <parent link="Arm_L5_Link" />
+    <child link="Arm_L6_Link" />
+    <origin xyz="0 0 0" rpy="0 -1.5707923 1.5708003" />
+    <axis xyz="0 0 1" />
+    <limit lower="-1.0472" upper="1.0472" effort="18" velocity="0" />
+    <dynamics damping="1.2" friction="0.12" />
+  </joint>
+
+  <joint name="Arm_L7_Joint" type="revolute">
+    <parent link="Arm_L6_Link" />
+    <child link="Arm_L7_Link" />
+    <origin xyz="0 0 0" rpy="0 -1.5707923 1.5708003" />
+    <axis xyz="0 0 1" />
+    <limit lower="-1.5708" upper="1.5708" effort="18" velocity="0" />
+    <dynamics damping="1.2" friction="0.1" />
+  </joint>
+
+  <joint name="Arm_L7_Link_to_flange_L" type="fixed">
+    <parent link="Arm_L7_Link" />
+    <child link="flange_L" />
+    <origin xyz="0 -0.088 0" rpy="0 -1.5707923 1.5708003" />
+  </joint>
+
+  <joint name="flange_L_to_left_gripper_mount" type="fixed">
+    <parent link="flange_L" />
+    <child link="left_gripper_mount" />
+    <origin xyz="0 0 0" rpy="0 0 -1.5707963" />
+  </joint>
+
+  <joint name="left_gripper_mount_to_left_gripper_base" type="fixed">
+    <parent link="left_gripper_mount" />
+    <child link="left_gripper_base" />
+    <origin xyz="0 0 0" rpy="0 0 0" />
+  </joint>
+
+  <joint name="left_gripper_left_finger_joint" type="prismatic">
+    <parent link="left_gripper_base" />
+    <child link="left_gripper_left_finger" />
+    <origin xyz="0.105 -0.01 0.0916374" rpy="1.5777046 -0.0094492 -3.1190022" />
+    <axis xyz="1 0 0" />
+    <limit lower="0" upper="0.05" effort="30" velocity="0" />
+    <dynamics damping="8" friction="0.2" />
+  </joint>
+
+  <joint name="left_gripper_right_finger_joint" type="prismatic">
+    <parent link="left_gripper_base" />
+    <child link="left_gripper_right_finger" />
+    <origin xyz="-0.05 -0.002 0.1105" rpy="1.5372318 0.0098779 0.0227581" />
+    <axis xyz="1 0 0" />
+    <limit lower="0" upper="0.05" effort="0" velocity="0" />
+    <dynamics damping="8" friction="0.2" />
+    <mimic joint="left_gripper_left_finger_joint" multiplier="1" offset="0" />
+  </joint>
+
+  <joint name="left_gripper_base_to_left_tool" type="fixed">
+    <parent link="left_gripper_base" />
+    <child link="left_tool" />
+    <origin xyz="0 0 0" rpy="0 -1.5707963 -1.5707963" />
+  </joint>
+
+  <joint name="base_link_to_Base_R" type="fixed">
+    <parent link="base_link" />
+    <child link="Base_R" />
+    <origin xyz="0 -0.026 1.121" rpy="1.5700001 0 0" />
+  </joint>
+
+  <joint name="Arm_R1_Joint" type="revolute">
+    <parent link="Base_R" />
+    <child link="Arm_R1_Link" />
+    <origin xyz="0 0 0.1745" rpy="0 0 0" />
+    <axis xyz="0 0 1" />
+    <limit lower="-3.1067" upper="3.1067" effort="108" velocity="0" />
+    <dynamics damping="3" friction="0.3" />
+  </joint>
+
+  <joint name="Arm_R2_Joint" type="revolute">
+    <parent link="Arm_R1_Link" />
+    <child link="Arm_R2_Link" />
+    <origin xyz="0 0 0" rpy="1.5708006 0 0" />
+    <axis xyz="0 0 1" />
+    <limit lower="-2.0944" upper="-0.1" effort="108" velocity="0" />
+    <dynamics damping="3" friction="0.3" />
+  </joint>
+
+  <joint name="Arm_R3_Joint" type="revolute">
+    <parent link="Arm_R2_Link" />
+    <child link="Arm_R3_Link" />
+    <origin xyz="0 0.287 0" rpy="-1.5708006 0 0" />
+    <axis xyz="0 0 1" />
+    <limit lower="-3.1067" upper="3.1067" effort="66" velocity="0" />
+    <dynamics damping="2.5" friction="0.25" />
+  </joint>
+
+  <joint name="Arm_R4_Joint" type="revolute">
+    <parent link="Arm_R3_Link" />
+    <child link="Arm_R4_Link" />
+    <origin xyz="0.018 0 0" rpy="-1.5708006 0 -3.1415853" />
+    <axis xyz="0 0 1" />
+    <limit lower="-2.5307" upper="-0.1" effort="66" velocity="0" />
+    <dynamics damping="2.5" friction="0.25" />
+  </joint>
+
+  <joint name="Arm_R5_Joint" type="revolute">
+    <parent link="Arm_R4_Link" />
+    <child link="Arm_R5_Link" />
+    <origin xyz="0.018 -0.314 0" rpy="-1.5708006 0 -3.1415853" />
+    <axis xyz="0 0 1" />
+    <limit lower="-3.1067" upper="3.1067" effort="18" velocity="0" />
+    <dynamics damping="1.5" friction="0.15" />
+  </joint>
+
+  <joint name="Arm_R6_Joint" type="revolute">
+    <parent link="Arm_R5_Link" />
+    <child link="Arm_R6_Link" />
+    <origin xyz="0 0 0" rpy="0 -1.5707923 1.5708003" />
+    <axis xyz="0 0 1" />
+    <limit lower="-1.0472" upper="1.0472" effort="18" velocity="0" />
+    <dynamics damping="1.2" friction="0.12" />
+  </joint>
+
+  <joint name="Arm_R7_Joint" type="revolute">
+    <parent link="Arm_R6_Link" />
+    <child link="Arm_R7_Link" />
+    <origin xyz="0 0 0" rpy="0 -1.5707923 1.5708003" />
+    <axis xyz="0 0 1" />
+    <limit lower="-1.5708" upper="1.5708" effort="18" velocity="0" />
+    <dynamics damping="1.2" friction="0.1" />
+  </joint>
+
+  <joint name="Arm_R7_Link_to_flange_R" type="fixed">
+    <parent link="Arm_R7_Link" />
+    <child link="flange_R" />
+    <origin xyz="0 -0.088 0" rpy="0 -1.5707923 1.5708003" />
+  </joint>
+
+  <joint name="flange_R_to_right_gripper_mount" type="fixed">
+    <parent link="flange_R" />
+    <child link="right_gripper_mount" />
+    <origin xyz="0 0 0" rpy="0 0 -1.5707963" />
+  </joint>
+
+  <joint name="right_gripper_mount_to_right_gripper_base" type="fixed">
+    <parent link="right_gripper_mount" />
+    <child link="right_gripper_base" />
+    <origin xyz="0 0 0" rpy="0 0 0" />
+  </joint>
+
+  <joint name="right_gripper_left_finger_joint" type="prismatic">
+    <parent link="right_gripper_base" />
+    <child link="right_gripper_left_finger" />
+    <origin xyz="0.105 -0.01 0.0916374" rpy="1.5777046 -0.0094492 -3.1190022" />
+    <axis xyz="1 0 0" />
+    <limit lower="0" upper="0.05" effort="30" velocity="0" />
+    <dynamics damping="8" friction="0.2" />
+  </joint>
+
+  <joint name="right_gripper_right_finger_joint" type="prismatic">
+    <parent link="right_gripper_base" />
+    <child link="right_gripper_right_finger" />
+    <origin xyz="-0.05 -0.002 0.1105" rpy="1.5372318 0.0098779 0.0227581" />
+    <axis xyz="1 0 0" />
+    <limit lower="0" upper="0.05" effort="0" velocity="0" />
+    <dynamics damping="8" friction="0.2" />
+    <mimic joint="right_gripper_left_finger_joint" multiplier="1" offset="0" />
+  </joint>
+
+  <joint name="right_gripper_base_to_right_tool" type="fixed">
+    <parent link="right_gripper_base" />
+    <child link="right_tool" />
+    <origin xyz="0 0 0" rpy="0 -1.5707963 -1.5707963" />
+  </joint>
+
+</robot>
\ No newline at end of file