diff --git a/talos_description/urdf/arm/arm.urdf.xacro b/talos_description/urdf/arm/arm.urdf.xacro
index d9b7475..919772a 100644
--- a/talos_description/urdf/arm/arm.urdf.xacro
+++ b/talos_description/urdf/arm/arm.urdf.xacro
@@ -29,7 +29,7 @@
   <xacro:property name="arm_eps"            value="0.02" />
 
 
-  <xacro:macro name="talos_arm" params="name parent side reflect">
+  <xacro:macro name="talos_arm" params="name parent side reflect using_capsule_collision">
     <xacro:unless value="$(arg test)">
       <xacro:include filename="$(find talos_description_calibration)/urdf/calibration/arm_${side}.xacro" />
     </xacro:unless>
@@ -51,14 +51,27 @@
         <material name="DarkGrey" />
       </visual>
 
-      <collision>
-       <origin xyz="0 0 0" rpy="0 0 0" />
-        <geometry>
-          <mesh filename="package://talos_description/meshes/arm/arm_1_collision.STL" scale="1 ${reflect} 1"/>
-        </geometry>
-      </collision>
+      <xacro:if value="${using_capsule_collision}">
+        <xacro:if value="${side == 'left'}">
+          <xacro:capsule-collision e1="-0.030119 0.141768 0.0465263" e2="-0.00429634 0.0157203 0.0513477" rad="0.076661652397679497" clen="0.12875612953621868" rpy="1.53256 0.201924 0.194397" mid="-0.0172077 0.0787443 0.048937" />
+        </xacro:if>
+        <xacro:if value="${side == 'right'}">
+	  <xacro:capsule-collision e1="-0.030119 -0.141768 0.0465263" e2="-0.00429634 -0.0157203 0.0513477" rad="0.076661652397679497" clen="0.12875612953621868" rpy="-1.53256 0.201924 -0.194397" mid="-0.0172077 -0.0787443 0.048937" />
+        </xacro:if>
+      </xacro:if>
+
+      <xacro:unless value="${using_capsule_collision}">
+        <collision>
+          <origin xyz="0 0 0" rpy="0 0 0" />
+          <geometry>
+            <mesh filename="package://talos_description/meshes/arm/arm_1_collision.STL" scale="1 ${reflect} 1"/>
+          </geometry>
+        </collision>
+      </xacro:unless>
+
     </link>
 
+
     <joint name="${name}_${side}_1_joint" type="revolute">
       <parent link="${parent}" />
       <child link="${name}_${side}_1_link" />
@@ -85,13 +98,24 @@
         </geometry>
         <material name="DarkGrey" />
       </visual>
+      <xacro:if value="${using_capsule_collision}">
+        <xacro:if value="${side == 'left'}">
+          <xacro:capsule-collision e1="-8.22104e-05 0.00124357 -0.107621" e2="0.0268691 0.000255834 -0.00293965" rad="0.082464320229024751" clen="0.10809965507805776" rpy="0.00943534 0.251977 0.00119509" mid="0.0133935 0.000749701 -0.0552803" />
+        </xacro:if>
+        <xacro:if value="${side == 'right'}">
+	  <xacro:capsule-collision e1="-8.22104e-05 -0.00124357 -0.107621" e2="0.0268691 -0.000255834 -0.00293965" rad="0.082464320229024751" clen="0.10809965507805776" rpy="-0.00943534 0.251977 -0.00119509" mid="0.0133935 -0.000749701 -0.0552803" />
+        </xacro:if>
+      </xacro:if>
+
+      <xacro:unless value="${using_capsule_collision}">
+        <collision>
+          <origin xyz="0 0 0" rpy="0 0 0" />
+          <geometry>
+            <mesh filename="package://talos_description/meshes/arm/arm_2_collision.STL" scale="1 ${reflect} 1"/>
+          </geometry>
+        </collision>
+      </xacro:unless>
 
-      <collision>
-        <origin xyz="0 0 0" rpy="0 0 0" />
-        <geometry>
-          <mesh filename="package://talos_description/meshes/arm/arm_2_collision.STL" scale="1 ${reflect} 1"/>
-        </geometry>
-      </collision>
     </link>
 
     <joint name="${name}_${side}_2_joint" type="revolute">
@@ -121,12 +145,23 @@
         <material name="DarkGrey" />
       </visual>
 
-      <collision>
-        <origin xyz="0 0 0" rpy="0 0 0" />
-        <geometry>
-          <mesh filename="package://talos_description/meshes/arm/arm_3_collision.STL" scale="1 ${reflect} 1"/>
-        </geometry>
-      </collision>
+      <xacro:if value="${using_capsule_collision}">
+        <xacro:if value="${side == 'left'}">
+          <xacro:capsule-collision e1="0.0012938 0.00577828 -0.203827" e2="0.0206547 0.0332832 -0.277413" rad="0.077010074599207598" clen="0.080908820017687497" rpy="-2.78389 0.241638 -1.1828" mid="0.0109743 0.0195307 -0.24062" />
+        </xacro:if>
+        <xacro:if value="${side == 'right'}">
+	  <xacro:capsule-collision e1="0.0012938 -0.00577828 -0.203827" e2="0.0206547 -0.0332832 -0.277413" rad="0.077010074599207598" clen="0.080908820017687497" rpy="2.78389 0.241638 1.1828" mid="0.0109743 -0.0195307 -0.24062" />
+        </xacro:if>
+      </xacro:if>
+      <xacro:unless value="${using_capsule_collision}">
+        <collision>
+          <origin xyz="0 0 0" rpy="0 0 0" />
+          <geometry>
+            <mesh filename="package://talos_description/meshes/arm/arm_3_collision.STL" scale="1 ${reflect} 1"/>
+          </geometry>
+        </collision>
+      </xacro:unless>
+
     </link>
 
     <joint name="${name}_${side}_3_joint" type="revolute">
@@ -158,12 +193,23 @@
         <material name="DarkGrey" />
       </visual>
 
-      <collision>
-        <origin xyz="0 0 0" rpy="0 0 0" />
-        <geometry>
-          <mesh filename="package://talos_description/meshes/arm/arm_4_collision.STL" scale="1 ${reflect} 1"/>
-        </geometry>
-      </collision>
+      <xacro:if value="${using_capsule_collision}">
+        <xacro:if value="${side == 'left'}">
+	  <xacro:capsule-collision e1="-0.020217 -0.00152238 -0.0600083" e2="-0.000255625 -0.0316283 0.0000811275" rad="0.061638573989565293" clen="0.07011106838940788" rpy="0.464462 0.288704 0.0687292" mid="-0.0102363 -0.0165753 -0.0299636" />
+        </xacro:if>
+        <xacro:if value="${side == 'right'}">
+	  <xacro:capsule-collision e1="-0.020217 0.00152238 -0.0600083" e2="-0.000255625 0.0316283 0.0000811275" rad="0.061638573989565293" clen="0.07011106838940788" rpy="-0.464462 0.288704 -0.0687292" mid="-0.0102363 0.0165753 -0.0299636" />
+        </xacro:if>
+      </xacro:if>
+      <xacro:unless value="${using_capsule_collision}">
+        <collision>
+          <origin xyz="0 0 0" rpy="0 0 0" />
+          <geometry>
+            <mesh filename="package://talos_description/meshes/arm/arm_4_collision.STL" scale="1 ${reflect} 1"/>
+          </geometry>
+        </collision>
+      </xacro:unless>
+
     </link>
 
     <joint name="${name}_${side}_4_joint" type="revolute">
@@ -218,7 +264,7 @@
     <!--************************-->
     <!--        WRIST           -->
     <!--************************-->
-    <xacro:talos_wrist name="arm" parent="${name}_${side}_4_link" side="${side}" reflect="${reflect}" />
+    <xacro:talos_wrist name="arm" parent="${name}_${side}_4_link" side="${side}" reflect="${reflect}"  using_capsule_collision="${using_capsule_collision}" />
 
   </xacro:macro>
 
diff --git a/talos_description/urdf/arm/wrist.urdf.xacro b/talos_description/urdf/arm/wrist.urdf.xacro
index e4263be..4fb524e 100644
--- a/talos_description/urdf/arm/wrist.urdf.xacro
+++ b/talos_description/urdf/arm/wrist.urdf.xacro
@@ -26,7 +26,7 @@
   <xacro:property name="wrist_eps"          value="0.02" />
 
 
-  <xacro:macro name="talos_wrist" params="name parent side reflect">
+  <xacro:macro name="talos_wrist" params="name parent side reflect using_capsule_collision">
     <xacro:unless value="$(arg test)">
       <xacro:include filename="$(find talos_description_calibration)/urdf/calibration/arm_${side}.xacro" />
     </xacro:unless>
@@ -48,12 +48,24 @@
         <material name="LightGrey" />
       </visual>
 
-      <collision>
-        <origin xyz="0 0 0" rpy="0 0 0" />
-        <geometry>
-          <mesh filename="package://talos_description/meshes/arm/arm_5_collision.STL" scale="1 ${reflect} 1"/>
-        </geometry>
-      </collision>
+      <xacro:if value="${using_capsule_collision}">
+        <xacro:if value="${side == 'left'}">
+	  <xacro:capsule-collision e1="-2.13964e-05 0.000999035 0.0358616" e2="0.000145615 0.0012191 0.121451" rad="0.077412213480368891" clen="0.085589949413547098" rpy="-0.00257122 0.0019513 -2.50861e-06" mid="6.21094e-05 0.00110907 0.0786563" />
+        </xacro:if>
+        <xacro:if value="${side == 'right'}">
+	  <xacro:capsule-collision e1="-2.13964e-05 -0.000999035 0.0358616" e2="0.000145615 -0.0012191 0.121451" rad="0.077412213480368891" clen="0.085589949413547098" rpy="0.00257122 0.0019513 2.50861e-06" mid="6.21094e-05 -0.00110907 0.0786563" />
+        </xacro:if>
+      </xacro:if>
+
+      <xacro:unless value="${using_capsule_collision}">
+        <collision>
+          <origin xyz="0 0 0" rpy="0 0 0" />
+          <geometry>
+            <mesh filename="package://talos_description/meshes/arm/arm_5_collision.STL" scale="1 ${reflect} 1"/>
+          </geometry>
+        </collision>
+      </xacro:unless>
+
     </link>
 
     <joint name="${name}_${side}_5_joint" type="revolute">
@@ -82,12 +94,24 @@
         <material name="LightGrey" />
       </visual>
 
-      <collision>
-        <origin xyz="0 0 0" rpy="0 0 0" />
-        <geometry>
-          <mesh filename="package://talos_description/meshes/arm/arm_6_collision.STL" scale="1 ${reflect} 1"/>
-        </geometry>
-      </collision>
+      <xacro:if value="${using_capsule_collision}">
+        <xacro:if value="${side == 'left'}">
+	  <xacro:capsule-collision e1="3.71733e-07 -0.000248227 -0.00766634" e2="5.46154e-05 -8.61821e-05 0.00107298" rad="0.044402592090813625" clen="0.0087409825962203314" rpy="-0.0185399 0.00620572 -5.75286e-05" mid="2.74936e-05 -0.000167204 -0.00329668" />
+
+        </xacro:if>
+        <xacro:if value="${side == 'right'}">
+	  <xacro:capsule-collision e1="3.71733e-07 0.000248227 -0.00766634" e2="5.46154e-05 8.61821e-05 0.00107298" rad="0.044402592090813625" clen="0.0087409825962203314" rpy="0.0185399 0.00620572 5.75286e-05" mid="2.74936e-05 0.000167204 -0.00329668" />
+        </xacro:if>
+      </xacro:if>
+
+      <xacro:unless value="${using_capsule_collision}">
+        <collision>
+          <origin xyz="0 0 0" rpy="0 0 0" />
+          <geometry>
+            <mesh filename="package://talos_description/meshes/arm/arm_6_collision.STL" scale="1 ${reflect} 1"/>
+          </geometry>
+        </collision>
+      </xacro:unless>
     </link>
 
     <joint name="${name}_${side}_6_joint" type="revolute">
@@ -116,12 +140,24 @@
         <material name="DarkGrey" />
       </visual>
 
-      <collision>
-        <origin xyz="0 0 0" rpy="0 0 0" />
-        <geometry>
-          <mesh filename="package://talos_description/meshes/arm/arm_7_collision.STL" scale="1 ${reflect} 1"/>
-        </geometry>
-      </collision>
+      <xacro:if value="${using_capsule_collision}">
+        <xacro:if value="${side == 'left'}">
+	  <xacro:capsule-collision e1="-3.67844e-05 -8.9312e-05 -0.0167866" e2="1.80245e-05 -1.49283e-05 -0.0702192" rad="0.048804145966408037" clen="0.05343267786175529" rpy="-3.1402 0.00102576 -1.27005" mid="-9.37996e-06 -5.21201e-05 -0.0435029" />
+        </xacro:if>
+        <xacro:if value="${side == 'right'}">
+	  <xacro:capsule-collision e1="-3.67844e-05 8.9312e-05 -0.0167866" e2="1.80245e-05 1.49283e-05 -0.0702192" rad="0.048804145966408037" clen="0.05343267786175529" rpy="3.1402 0.00102576 1.27005" mid="-9.37996e-06 5.21201e-05 -0.0435029" />
+        </xacro:if>
+      </xacro:if>
+
+      <xacro:unless value="${using_capsule_collision}">
+        <collision>
+          <origin xyz="0 0 0" rpy="0 0 0" />
+          <geometry>
+            <mesh filename="package://talos_description/meshes/arm/arm_7_collision.STL" scale="1 ${reflect} 1"/>
+          </geometry>
+        </collision>
+      </xacro:unless>
+
     </link>
 
     <joint name="${name}_${side}_7_joint" type="revolute">
diff --git a/talos_description/urdf/capsule_collision.xacro b/talos_description/urdf/capsule_collision.xacro
new file mode 100644
index 0000000..80eb41e
--- /dev/null
+++ b/talos_description/urdf/capsule_collision.xacro
@@ -0,0 +1,22 @@
+<robot xmlns:xacro="http://ros.org/wiki/xacro"> 
+  <xacro:macro name="capsule-collision" params="e1 e2 rad clen rpy mid">
+    <collision>
+      <origin rpy="${rpy}" xyz="${e1}" />
+      <geometry>
+	<sphere radius="${rad}" />
+      </geometry>
+    </collision>
+    <collision>
+      <origin rpy="${rpy}" xyz="${mid}" />
+      <geometry>
+	<cylinder length="${clen}" radius="${rad}" />
+      </geometry>
+    </collision>
+    <collision>
+      <origin rpy="${rpy}" xyz="${e2}" />
+      <geometry>
+	<sphere radius="${rad}" />
+      </geometry>
+    </collision>
+  </xacro:macro>
+</robot>
diff --git a/talos_description/urdf/gripper/gripper.urdf.xacro b/talos_description/urdf/gripper/gripper.urdf.xacro
index 0085f5d..1c93ada 100644
--- a/talos_description/urdf/gripper/gripper.urdf.xacro
+++ b/talos_description/urdf/gripper/gripper.urdf.xacro
@@ -4,7 +4,7 @@
 
   <xacro:include filename="$(find talos_description)/urdf/deg_to_rad.xacro" />
 
-  <xacro:macro name="talos_gripper" params="name parent reflect">
+  <xacro:macro name="talos_gripper" params="name parent reflect using_capsule_collision">
 
             <xacro:macro name="dummy_mimic_joint" params="name parent">
               <link name="${name}_dummy_mimic_link"/>
@@ -20,8 +20,8 @@
               <xacro:dummy_mimic_joint name="${name}_fingertip_2" parent="${name}_base_link"/>
               <xacro:dummy_mimic_joint name="${name}_inner_single" parent="${name}_base_link"/>
               <xacro:dummy_mimic_joint name="${name}_fingertip_3" parent="${name}_base_link"/>
-              <xacro:dummy_mimic_joint name="${name}_motor_single" parent="${name}_base_link"/> 
-              <xacro:dummy_mimic_joint name="${name}_inner_double" parent="${name}_base_link"/>                         
+              <xacro:dummy_mimic_joint name="${name}_motor_single" parent="${name}_base_link"/>
+              <xacro:dummy_mimic_joint name="${name}_inner_double" parent="${name}_base_link"/>
             </xacro:macro>
 
             <link name="${name}_base_link">
@@ -35,12 +35,23 @@
                 <material name="DarkGrey"/>
               </visual>
 
-              <collision>
-                <origin xyz="0 0 0" rpy="0 0 0" />
-                <geometry>
-                  <mesh filename="package://talos_description/meshes/gripper/base_link_collision.STL" scale="1 1 1"/>
-                </geometry>
-              </collision>
+              <xacro:if value="${using_capsule_collision}">
+                <xacro:if value="${name == 'gripper_left'}">
+                  <xacro:capsule-collision e1="-0.00141404 0.0132339 -0.0265828" e2="-0.00133643 -0.0137463 -0.0261142" rad="0.060646620008233225" clen="0.026984385527609595" rpy="1.55343 0.00287592 0.00282641" mid="-0.00137524 -0.00025619 -0.0263485" />
+                </xacro:if>
+                <xacro:if value="${name == 'gripper_right'}">
+                  <xacro:capsule-collision e1="-0.00141404 0.0132339 -0.0265828" e2="-0.00133643 -0.0137463 -0.0261142" rad="0.060646620008233225" clen="0.026984385527609595" rpy="1.55343 0.00287592 0.00282641" mid="-0.00137524 -0.00025619 -0.0263485" />
+                </xacro:if>
+              </xacro:if>
+              <xacro:unless value="${using_capsule_collision}">
+                <collision>
+                  <origin xyz="0 0 0" rpy="0 0 0" />
+                  <geometry>
+                    <mesh filename="package://talos_description/meshes/gripper/base_link_collision.STL" scale="1 1 1"/>
+                  </geometry>
+                </collision>
+              </xacro:unless>
+
             </link>
 
             <joint name="${name}_base_link_joint" type="fixed">
@@ -63,12 +74,22 @@
               <material name="Orange"/>
               </visual>
 
-              <collision>
-                <origin xyz="0 0 0" rpy="0 0 0" />
-                <geometry>
-                  <mesh filename="package://talos_description/meshes/gripper/gripper_motor_double_collision.STL" scale="1 1 1"/>
-                </geometry>
-              </collision>
+              <xacro:if value="${using_capsule_collision}">
+                <xacro:if value="${name == 'gripper_left'}">
+		  <xacro:capsule-collision e1="-0.0265616 0.0150351 -0.0244879" e2="0.0170994 0.0176648 -0.0189716" rad="0.056117638554986304" clen="0.044086614202536976" rpy="-0.444846 1.43174 -0.388469" mid="-0.00473109 0.0163499 -0.0217297" />
+                </xacro:if>
+                <xacro:if value="${name == 'gripper_right'}">
+                  <xacro:capsule-collision e1="-0.0265616 0.0150351 -0.0244879" e2="0.0170994 0.0176648 -0.0189716" rad="0.056117638554986304" clen="0.044086614202536976" rpy="-0.444846 1.43174 -0.388469" mid="-0.00473109 0.0163499 -0.0217297" />
+                </xacro:if>
+              </xacro:if>
+              <xacro:unless value="${using_capsule_collision}">
+                <collision>
+                  <origin xyz="0 0 0" rpy="0 0 0" />
+                  <geometry>
+                    <mesh filename="package://talos_description/meshes/gripper/gripper_motor_double_collision.STL" scale="1 1 1"/>
+                  </geometry>
+                </collision>
+              </xacro:unless>
             </link>
 
             <joint name="${name}_joint" type="revolute">
@@ -93,12 +114,22 @@
               <material name="Orange"/>
               </visual>
 
-              <collision>
-                <origin xyz="0 0 0" rpy="0 0 0" />
-                <geometry>
-                  <mesh filename="package://talos_description/meshes/gripper/inner_double_collision.STL" scale="1 1 1"/>
-                </geometry>
-              </collision>
+              <xacro:if value="${using_capsule_collision}">
+                <xacro:if value="${name == 'gripper_left'}">
+		  <xacro:capsule-collision e1="-0.0179304 0.0279391 -0.034841" e2="0.0190611 0.0265842 -0.0355285" rad="0.048779412908590628" clen="0.03702274994466908" rpy="2.04039 1.52975 2.00344" mid="0.000565336 0.0272617 -0.0351848" />
+                </xacro:if>
+                <xacro:if value="${name == 'gripper_right'}">
+                  <xacro:capsule-collision e1="-0.0179304 0.0279391 -0.034841" e2="0.0190611 0.0265842 -0.0355285" rad="0.048779412908590628" clen="0.03702274994466908" rpy="2.04039 1.52975 2.00344" mid="0.000565336 0.0272617 -0.0351848" />
+                </xacro:if>
+              </xacro:if>
+              <xacro:unless value="${using_capsule_collision}">
+                <collision>
+                  <origin xyz="0 0 0" rpy="0 0 0" />
+                  <geometry>
+                    <mesh filename="package://talos_description/meshes/gripper/inner_double_collision.STL" scale="1 1 1"/>
+                  </geometry>
+                </collision>
+              </xacro:unless>
             </link>
 
             <joint name="${name}_inner_double_joint" type="revolute">
@@ -124,12 +155,22 @@
               <material name="DarkGrey"/>
               </visual>
 
-              <collision>
-                <origin xyz="0 0 0" rpy="0 0 0" />
-                <geometry>
-                  <mesh filename="package://talos_description/meshes/gripper/fingertip_collision.STL" scale="1 1 1"/>
-                </geometry>
-              </collision>
+              <xacro:if value="${using_capsule_collision}">
+                <xacro:if value="${name == 'gripper_left'}">
+		  <xacro:capsule-collision e1="-3.72021e-06 0.000302954 -0.026257" e2="-5.04286e-05 0.0140159 0.0163723" rad="0.016697853294401835" clen="0.044780558196911839" rpy="-0.311226 -0.00104305 0.000163635" mid="-2.70744e-05 0.00715944 -0.00494235" />
+                </xacro:if>
+                <xacro:if value="${name == 'gripper_right'}">
+		  <xacro:capsule-collision e1="-3.72021e-06 0.000302954 -0.026257" e2="-5.04286e-05 0.0140159 0.0163723" rad="0.016697853294401835" clen="0.044780558196911839" rpy="-0.311226 -0.00104305 0.000163635" mid="-2.70744e-05 0.00715944 -0.00494235" />
+                </xacro:if>
+              </xacro:if>
+              <xacro:unless value="${using_capsule_collision}">
+                <collision>
+                  <origin xyz="0 0 0" rpy="0 0 0" />
+                  <geometry>
+                    <mesh filename="package://talos_description/meshes/gripper/fingertip_collision.STL" scale="1 1 1"/>
+                  </geometry>
+                </collision>
+              </xacro:unless>
             </link>
 
 
@@ -155,12 +196,17 @@
                 <material name="DarkGrey"/>
               </visual>
 
-              <collision>
-                <origin xyz="0 0 0" rpy="0 0 0" />
-                <geometry>
-                  <mesh filename="package://talos_description/meshes/gripper/fingertip_collision.STL" scale="1 1 1"/>
-                </geometry>
-              </collision>
+              <xacro:if value="${using_capsule_collision}">
+		<xacro:capsule-collision e1="-3.72021e-06 0.000302954 -0.026257" e2="-5.04286e-05 0.0140159 0.0163723" rad="0.016697853294401835" clen="0.044780558196911839" rpy="-0.311226 -0.00104305 0.000163635" mid="-2.70744e-05 0.00715944 -0.00494235" />
+              </xacro:if>
+              <xacro:unless value="${using_capsule_collision}">
+                <collision>
+                  <origin xyz="0 0 0" rpy="0 0 0" />
+                  <geometry>
+                    <mesh filename="package://talos_description/meshes/gripper/fingertip_collision.STL" scale="1 1 1"/>
+                  </geometry>
+                </collision>
+              </xacro:unless>
             </link>
 
             <joint name="${name}_fingertip_2_joint" type="revolute">
@@ -185,12 +231,18 @@
               <material name="Orange"/>
               </visual>
 
-              <collision>
-                <origin xyz="0 0 0" rpy="0 0 0" />
-                <geometry>
-                  <mesh filename="package://talos_description/meshes/gripper/gripper_motor_single_collision.STL" scale="1 1 1"/>
-                </geometry>
-              </collision>
+
+              <xacro:if value="${using_capsule_collision}">
+		<xacro:capsule-collision e1="-0.00541609 -0.00889752 -0.0121105" e2="0.0100479 -0.0148793 -0.0200314" rad="0.060566804043534143" clen="0.018375470004515674" rpy="2.49478 1.00016 2.04115" mid="0.00231593 -0.0118884 -0.0160709" />
+              </xacro:if>
+              <xacro:unless value="${using_capsule_collision}">
+                <collision>
+                  <origin xyz="0 0 0" rpy="0 0 0" />
+                  <geometry>
+                    <mesh filename="package://talos_description/meshes/gripper/gripper_motor_single_collision.STL" scale="1 1 1"/>
+                  </geometry>
+                </collision>
+              </xacro:unless>
             </link>
 
             <joint name="${name}_motor_single_joint" type="revolute">
@@ -216,12 +268,17 @@
               <material name="Orange"/>
               </visual>
 
-              <collision>
-                <origin xyz="0 0 0" rpy="0 0 0" />
-                <geometry>
-                  <mesh filename="package://talos_description/meshes/gripper/inner_single_collision.STL" scale="1 1 1"/>
-                </geometry>
-              </collision>
+              <xacro:if value="${using_capsule_collision}">
+                <xacro:capsule-collision e1="-0.000202241 -0.0141496 0.000280576" e2="0.000808315 -0.0507867 -0.0515168" rad="0.023859684447196262" clen="0.063452821575460158" rpy="2.52597 0.0159268 0.0500884" mid="0.000303037 -0.0324682 -0.0256181" />
+              </xacro:if>
+              <xacro:unless value="${using_capsule_collision}">
+                <collision>
+                  <origin xyz="0 0 0" rpy="0 0 0" />
+                  <geometry>
+                    <mesh filename="package://talos_description/meshes/gripper/inner_single_collision.STL" scale="1 1 1"/>
+                  </geometry>
+                </collision>
+              </xacro:unless>
             </link>
 
             <joint name="${name}_inner_single_joint" type="revolute">
@@ -246,12 +303,17 @@
               <material name="DarkGrey"/>
               </visual>
 
-              <collision>
-                <origin xyz="0 0 0" rpy="0 0 0" />
-                <geometry>
+              <xacro:if value="${using_capsule_collision}">
+                <xacro:capsule-collision e1="-3.72021e-06 0.000302954 -0.026257" e2="-5.04286e-05 0.0140159 0.0163723" rad="0.016697853294401835" clen="0.044780558196911839" rpy="-0.311226 -0.00104305 0.000163635" mid="-2.70744e-05 0.00715944 -0.00494235" />
+              </xacro:if>
+              <xacro:unless value="${using_capsule_collision}">
+                <collision>
+                  <origin xyz="0 0 0" rpy="0 0 0" />
+                  <geometry>
                   <mesh filename="package://talos_description/meshes/gripper/fingertip_collision.STL" scale="1 1 1"/>
-                </geometry>
-              </collision>
+                  </geometry>
+                </collision>
+              </xacro:unless>
             </link>
 
             <joint name="${name}_fingertip_3_joint" type="revolute">
diff --git a/talos_description/urdf/head/head.urdf.xacro b/talos_description/urdf/head/head.urdf.xacro
index f2c4736..84427d5 100644
--- a/talos_description/urdf/head/head.urdf.xacro
+++ b/talos_description/urdf/head/head.urdf.xacro
@@ -23,7 +23,7 @@
   <!--************************-->
   <!--     HEAD_1 (TILT)      -->
   <!--************************-->
-  <xacro:macro name="talos_head_1" params="name parent">
+  <xacro:macro name="talos_head_1" params="name parent using_capsule_collision">
     <link name="${name}_link">
       <xacro:call macro="${name}_link_inertial" />
 
@@ -35,12 +35,17 @@
         <material name="DarkGrey" />
       </visual>
 
-      <collision>
-        <origin xyz="0 0 0" rpy="0 0 0" />
-        <geometry>
-          <mesh filename="package://talos_description/meshes/head/head_1_collision.stl" scale="1 1 1"/>
-        </geometry>
-      </collision>
+      <xacro:if value="${using_capsule_collision}">
+	<xacro:capsule-collision e1="9.25184e-05 -2.39435e-05 -0.000765958" e2="-0.0116532 0.000267479 0.0600432" rad="0.082632032931063207" clen="0.061933815385056032" rpy="-0.00479238 -0.190806 0.000458601" mid="-0.00578036 0.000121768 0.0296386" />
+      </xacro:if>
+      <xacro:unless value="${using_capsule_collision}">
+        <collision>
+          <origin xyz="0 0 0" rpy="0 0 0" />
+          <geometry>
+            <mesh filename="package://talos_description/meshes/head/head_1_collision.stl" scale="1 1 1"/>
+          </geometry>
+        </collision>
+      </xacro:unless>
     </link>
 
     <joint name="${name}_joint" type="revolute">
@@ -77,7 +82,7 @@
   <!--************************-->
   <!--      HEAD_2 (PAN)      -->
   <!--************************-->
-  <xacro:macro name="talos_head_2" params="name parent type:=default">
+  <xacro:macro name="talos_head_2" params="name parent using_capsule_collision=${using_capsule_collision} type:=default">
     <link name="${name}_link">
       <xacro:call macro="${name}_link_inertial" />
 
@@ -89,12 +94,17 @@
         <material name="LightGrey" />
       </visual>
 
-      <collision>
-        <origin xyz="0 0 0" rpy="0 0 0" />
-        <geometry>
-          <mesh filename="package://talos_description/meshes/head/head_2_${type}_collision.stl" scale="1 1 1"/>
-        </geometry>
-      </collision>
+      <xacro:if value="${using_capsule_collision}">
+        <xacro:capsule-collision e1="-0.0136558 6.17209e-05 0.196249" e2="0.0257541 0.000121936 0.169493" rad="0.10582827877003444" clen="0.047634451110799531" rpy="-3.13934 0.974356 -3.13734" mid="0.00604916 9.18284e-05 0.182871" />
+      </xacro:if>
+      <xacro:unless value="${using_capsule_collision}">
+        <collision>
+          <origin xyz="0 0 0" rpy="0 0 0" />
+          <geometry>
+            <mesh filename="package://talos_description/meshes/head/head_2_${type}_collision.stl" scale="1 1 1"/>
+          </geometry>
+        </collision>
+      </xacro:unless>
     </link>
 
     <joint name="${name}_joint" type="revolute">
@@ -128,9 +138,10 @@
 
   </xacro:macro>
 
-  <xacro:macro name="talos_head" params="name parent disable_gazebo_camera:=false head_type:=default">
-    <xacro:talos_head_1 name="${name}_1" parent="${parent}" />
-    <xacro:talos_head_2 name="${name}_2" parent="${name}_1_link" type="${head_type}"/>
+  <xacro:macro name="talos_head" params="name parent using_capsule_collision disable_gazebo_camera:=false head_type:=default">
+    <xacro:talos_head_1 name="${name}_1" parent="${parent}" using_capsule_collision="${using_capsule_collision}"/>
+    <xacro:talos_head_2 name="${name}_2" parent="${name}_1_link" type="${head_type}"
+                        using_capsule_collision="${using_capsule_collision}"/>
     <xacro:if value="${head_type == 'default'}">
       <xacro:orbbec_astra_pro name="rgbd" parent="${name}_2" disable_gazebo="${disable_gazebo_camera}">
         <!-- Pose of sensor frame wrt to base -->
diff --git a/talos_description/urdf/leg/leg.urdf.xacro b/talos_description/urdf/leg/leg.urdf.xacro
index 08d962c..2e94261 100644
--- a/talos_description/urdf/leg/leg.urdf.xacro
+++ b/talos_description/urdf/leg/leg.urdf.xacro
@@ -1,6 +1,7 @@
 <?xml version="1.0"?>
 
 <robot xmlns:xacro="http://ros.org/wiki/xacro" name="talos">
+
   
   <xacro:include filename="$(find talos_description)/urdf/leg/foot_frames.urdf.xacro" />
 
@@ -11,7 +12,7 @@
   <xacro:property name="leg_damping"      value="0.0" />
 
 
-  <xacro:macro name="talos_leg" params="prefix reflect flexibility:=false">
+  <xacro:macro name="talos_leg" params="prefix reflect flexibility:=false using_capsule_collision:=false">
     <xacro:unless value="$(arg test)">
       <xacro:include filename="$(find talos_description_calibration)/urdf/calibration/leg_${prefix}.xacro" />
     </xacro:unless>
@@ -80,13 +81,23 @@
         </geometry>
         <material name="DarkGrey" />
       </visual>
-
-      <collision>
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry>
-           <mesh filename="package://talos_description/meshes/v2/hip_z_collision.stl" scale="1 ${reflect*1} 1"/>
-        </geometry>
-      </collision>
+      <xacro:if value="${using_capsule_collision}">
+        <xacro:if value="${prefix == 'left'}">
+	  <xacro:capsule-collision e1="0.00653137 -0.00107636 0.0620231" e2="0.0148595 0.000151801 0.00431736" rad="0.10038884516307987" clen="0.058316498699378369" rpy="-3.12031 0.1433 -2.84723" mid="0.0106955 -0.000462282 0.0331702" />
+        </xacro:if>
+        <xacro:if value="${ prefix == 'right'}">
+	  <xacro:capsule-collision e1="0.00653137 0.00107636 0.0620231" e2="0.0148595 -0.000151801 0.00431736" rad="0.10038884516307987" clen="0.058316498699378369" rpy="3.12031 0.1433 2.84723" mid="0.0106955 0.000462282 0.0331702" />
+        </xacro:if>
+      </xacro:if>
+
+      <xacro:unless value="${using_capsule_collision}">
+        <collision>
+          <origin rpy="0 0 0" xyz="0 0 0"/>
+          <geometry>
+            <mesh filename="package://talos_description/meshes/v2/hip_z_collision.stl" scale="1 ${reflect*1} 1"/>
+          </geometry>
+        </collision>
+      </xacro:unless>
     </link>
 
     <!-- Joint hip_z (mesh link) ; child link hip_x -> parent link base_link -->
@@ -126,12 +137,24 @@
         <material name="DarkGrey" />
       </visual>
 
-      <collision>
-        <origin xyz="0 0 0.0" rpy="0  0  0" />
-        <geometry>
-          <mesh filename="package://talos_description/meshes/v2/hip_x_collision.stl" scale="1 ${reflect*1} 1"/>
-        </geometry>
-      </collision>
+      <xacro:if value="${using_capsule_collision}">
+        <xacro:if value="${prefix == 'left'}">
+	  <xacro:capsule-collision e1="-0.0163525 -0.0136729 0.000327785" e2="-0.0177494 0.072721 0.00192874" rad="0.065228014094609946" clen="0.086420044518244921" rpy="-1.55227 -0.0161651 0.0158683" mid="-0.0170509 0.029524 0.00112826" />
+        </xacro:if>
+        <xacro:if value="${prefix == 'right'}">
+		<xacro:capsule-collision e1="-0.0163525 0.0136729 0.000327785" e2="-0.0177494 -0.072721 0.00192874" rad="0.065228014094609946" clen="0.086420044518244921" rpy="1.55227 -0.0161651 -0.0158683" mid="-0.0170509 -0.029524 0.00112826" />
+        </xacro:if>
+      </xacro:if>
+
+      <xacro:unless value="${using_capsule_collision}">
+        <collision>
+          <origin xyz="0 0 0.0" rpy="0  0  0" />
+          <geometry>
+            <mesh filename="package://talos_description/meshes/v2/hip_x_collision.stl" scale="1 ${reflect*1} 1"/>
+          </geometry>
+        </collision>
+      </xacro:unless>
+
     </link>
 
     <joint name="leg_${prefix}_2_joint" type="revolute">
@@ -157,12 +180,24 @@
         <material name="DarkGrey" />
       </visual>
 
-      <collision>
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry>
-           <mesh filename="package://talos_description/meshes/v2/hip_y_collision.stl" scale="1 ${reflect*1} 1"/>
-        </geometry>
-      </collision>
+      <xacro:if value="${using_capsule_collision}">
+        <xacro:if value="${prefix == 'left'}">
+	  <xacro:capsule-collision e1="0.00475684 0.0387611 -0.305131" e2="0.0048036 0.0612963 -0.0132432" rad="0.14557912147210253" clen="0.29275626620252521" rpy="-0.077052 0.00015972 -6.15642e-06" mid="0.00478022 0.0500287 -0.159187" />
+        </xacro:if>
+        <xacro:if value="${prefix == 'right'}">
+	  <xacro:capsule-collision e1="0.00475684 -0.0387611 -0.305131" e2="0.0048036 -0.0612963 -0.0132432" rad="0.14557912147210253" clen="0.29275626620252521" rpy="0.077052 0.00015972 6.15642e-06" mid="0.00478022 -0.0500287 -0.159187" />
+        </xacro:if>
+      </xacro:if>
+
+      <xacro:unless value="${using_capsule_collision}">
+        <collision>
+          <origin rpy="0 0 0" xyz="0 0 0"/>
+          <geometry>
+            <mesh filename="package://talos_description/meshes/v2/hip_y_collision.stl" scale="1 ${reflect*1} 1"/>
+          </geometry>
+        </collision>
+      </xacro:unless>
+
     </link>
 
     <joint name="leg_${prefix}_3_joint" type="revolute">
@@ -188,12 +223,23 @@
         <material name="DarkGrey" />
       </visual>
 
-      <collision>
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry>
-           <mesh filename="package://talos_description/meshes/v2/knee_collision.stl" scale="1 ${reflect*1} 1"/>
-        </geometry>
-      </collision>
+      <xacro:if value="${using_capsule_collision}">
+        <xacro:if value="${prefix == 'left'}">
+	  <xacro:capsule-collision e1="0.00163993 0.0368989 -0.301909" e2="5.69681e-05 0.0588976 -0.00805678" rad="0.1184924950609377" clen="0.29467833417874939" rpy="-0.0747239 -0.00537185 0.000200797" mid="0.000848448 0.0478982 -0.154983" />
+        </xacro:if>
+        <xacro:if value="${prefix == 'right'}">
+	  <xacro:capsule-collision e1="0.00163993 -0.0368989 -0.301909" e2="5.69681e-05 -0.0588976 -0.00805678" rad="0.1184924950609377" clen="0.29467833417874939" rpy="0.0747239 -0.00537185 -0.000200797" mid="0.000848448 -0.0478982 -0.154983" />
+        </xacro:if>
+      </xacro:if>
+
+      <xacro:unless value="${using_capsule_collision}">
+        <collision>
+          <origin rpy="0 0 0" xyz="0 0 0"/>
+          <geometry>
+            <mesh filename="package://talos_description/meshes/v2/knee_collision.stl" scale="1 ${reflect*1} 1"/>
+          </geometry>
+        </collision>
+      </xacro:unless>
     </link>
 
     <joint name="leg_${prefix}_4_joint" type="revolute">
@@ -221,12 +267,24 @@
         <material name="Grey" />
       </visual>
 
-      <collision>
-        <origin xyz="0 0 0.0" rpy="0 0 0" />
-        <geometry>
-          <mesh filename="package://talos_description/meshes/v2/ankle_Y_collision.stl" scale="1 ${reflect*1} 1"/>
-        </geometry>
-      </collision>
+      <xacro:if value="${using_capsule_collision}">
+        <xacro:if value="${prefix == 'left'}">
+	  <xacro:capsule-collision e1="-0.0304137 0.105995 0.00632637" e2="0.00663845 -0.0117406 -0.00301129" rad="0.056285651716761462" clen="0.12378102303209611" rpy="1.64994 0.303997 0.32863" mid="-0.0118876 0.0471272 0.00165754" />
+        </xacro:if>
+        <xacro:if value="${prefix == 'right'}">
+	  <xacro:capsule-collision e1="-0.0304137 -0.105995 0.00632637" e2="0.00663845 0.0117406 -0.00301129" rad="0.056285651716761462" clen="0.12378102303209611" rpy="-1.64994 0.303997 -0.32863" mid="-0.0118876 -0.0471272 0.00165754" />
+        </xacro:if>
+      </xacro:if>
+
+      <xacro:unless value="${using_capsule_collision}">
+        <collision>
+          <origin xyz="0 0 0.0" rpy="0 0 0" />
+          <geometry>
+            <mesh filename="package://talos_description/meshes/v2/ankle_Y_collision.stl" scale="1 ${reflect*1} 1"/>
+          </geometry>
+        </collision>
+      </xacro:unless>
+
     </link>
 
     <joint name="leg_${prefix}_5_joint" type="revolute">
diff --git a/talos_description/urdf/talos_full_common.urdf.xacro b/talos_description/urdf/talos_full_common.urdf.xacro
index cb1c16f..3e2d28c 100644
--- a/talos_description/urdf/talos_full_common.urdf.xacro
+++ b/talos_description/urdf/talos_full_common.urdf.xacro
@@ -17,12 +17,15 @@
   <xacro:arg name="use_fixed_base" default="false"/>
   <xacro:arg name="use_sim" default="false"/>
   <xacro:arg name="multiple" default="false"/>
+  <xacro:arg name="use_capsule_collision" default="false"/>
+  <xacro:arg name="disable_gazebo_camera" default="false"/>
   <xacro:property name="is_multiple" value="$(arg multiple)" />
 
   <xacro:if value="$(arg enable_crane)">
     <xacro:include filename="$(find talos_description)/urdf/crane/crane.urdf.xacro" />
   </xacro:if>
 
+  <xacro:include filename="$(find talos_description)/urdf/capsule_collision.xacro" />
   <xacro:include filename="$(find talos_description)/urdf/torso/torso.urdf.xacro" />
   <xacro:include filename="$(find talos_description)/urdf/head/head.urdf.xacro" />
   <xacro:include filename="$(find talos_description)/urdf/arm/arm.urdf.xacro" />
@@ -50,24 +53,26 @@
     </joint>
   </xacro:if>
 
-  <xacro:talos_torso name="torso" />
+  <xacro:talos_torso name="torso" using_capsule_collision="$(arg use_capsule_collision)"/>
 
   <xacro:if value="$(arg enable_crane)">
     <xacro:crane parent="torso_2_link"/>
   </xacro:if>
 
-  <xacro:talos_head name="head" parent="torso_2_link" disable_gazebo_camera="$(arg disable_gazebo_camera)" head_type="$(arg head_type)"/>
+  <xacro:talos_head name="head" parent="torso_2_link" using_capsule_collision="$(arg use_capsule_collision)" disable_gazebo_camera="$(arg disable_gazebo_camera)" head_type="$(arg head_type)"/>
 
-  <xacro:talos_arm name="arm" parent="torso_2_link" side="left" reflect="1" />
-  <xacro:talos_arm name="arm" parent="torso_2_link" side="right" reflect="-1" />
+  <xacro:talos_arm name="arm" parent="torso_2_link" side="left" reflect="1" using_capsule_collision="$(arg use_capsule_collision)"/>
+  <xacro:talos_arm name="arm" parent="torso_2_link" side="right" reflect="-1" using_capsule_collision="$(arg use_capsule_collision)"/>
 
   <xacro:ft_sensor name="wrist" parent="arm_right_7_link" side="right" reflect="1"  />
   <xacro:ft_sensor name="wrist" parent="arm_left_7_link"  side="left"  reflect="-1" />
-  <xacro:talos_gripper name="gripper_left" parent="wrist_left_ft_tool_link" reflect="1" />
-  <xacro:talos_gripper name="gripper_right" parent="wrist_right_ft_tool_link" reflect="-1" />
+  <xacro:talos_gripper name="gripper_left" parent="wrist_left_ft_tool_link" reflect="1"
+                       using_capsule_collision="$(arg use_capsule_collision)"/>
+  <xacro:talos_gripper name="gripper_right" parent="wrist_right_ft_tool_link" reflect="-1"
+                       using_capsule_collision="$(arg use_capsule_collision)"/>
 
-  <xacro:talos_leg prefix="left"  reflect="1" flexibility="$(arg flexibility)"/>
-  <xacro:talos_leg prefix="right" reflect="-1" flexibility="$(arg flexibility)"/>
+  <xacro:talos_leg prefix="left"  reflect="1" flexibility="$(arg flexibility)" using_capsule_collision="$(arg use_capsule_collision)"/>
+  <xacro:talos_leg prefix="right" reflect="-1" flexibility="$(arg flexibility)" using_capsule_collision="$(arg use_capsule_collision)"/>
 
   <xacro:talos_addons/>
   <xacro:talos_gazebo/>
diff --git a/talos_description/urdf/torso/torso.urdf.xacro b/talos_description/urdf/torso/torso.urdf.xacro
index 60da5cc..0ec9478 100644
--- a/talos_description/urdf/torso/torso.urdf.xacro
+++ b/talos_description/urdf/torso/torso.urdf.xacro
@@ -25,7 +25,7 @@
   <xacro:property name="torso_max_effort"  value="200.0" />
   <xacro:property name="torso_eps"      value="0.02" />
 
-  <xacro:macro name="talos_torso" params="name">
+  <xacro:macro name="talos_torso" params="name using_capsule_collision">
     <!--************************-->
     <!--        TORSO_2  (TILT) -->
     <!--************************-->
@@ -40,12 +40,17 @@
         <material name="LightGrey" />
       </visual>
 
-      <collision>
-        <origin xyz="0 0 0" rpy="0 0 0" />
-        <geometry>
-          <mesh filename="package://talos_description/meshes/torso/torso_2_collision.STL" scale="1 1 1"/>
-        </geometry>
-      </collision>
+      <xacro:if value="${using_capsule_collision}">
+        <xacro:capsule-collision e1="-0.0381908 -4.93875e-05 0.108256" e2="-0.045126 -6.75323e-05 0.298264" rad="0.23584089766240193" clen="0.19013465851723285" rpy="9.54947e-05 -0.0364831 -1.74216e-06" mid="-0.0416584 -5.84599e-05 0.20326" />
+      </xacro:if>
+      <xacro:unless value="${using_capsule_collision}">
+        <collision>
+          <origin xyz="0 0 0" rpy="0 0 0" />
+          <geometry>
+            <mesh filename="package://talos_description/meshes/torso/torso_2_collision.STL" scale="1 1 1"/>
+          </geometry>
+        </collision>
+      </xacro:unless>
     </link>
 
 
@@ -63,12 +68,17 @@
         <material name="LightGrey" />
       </visual>
 
-      <collision>
-        <origin xyz="0 0 0" rpy="0 0 0" />
-        <geometry>
-          <mesh filename="package://talos_description/meshes/torso/torso_1.STL" scale="1 1 1"/>
+      <xacro:if value="${using_capsule_collision}">
+        <xacro:capsule-collision e1="-0.00232934 -0.0104948 -0.01235" e2="-0.00187524 0.0100705 -0.0118019" rad="0.091952314418329517" clen="0.020577608942189702" rpy="-1.54415 0.0220698 -0.0214895" mid="-0.00210229 -0.000212158 -0.0120759" />
+      </xacro:if>
+      <xacro:unless value="${using_capsule_collision}">
+        <collision>
+          <origin xyz="0 0 0" rpy="0 0 0" />
+          <geometry>
+            <mesh filename="package://talos_description/meshes/torso/torso_1.STL" scale="1 1 1"/>
         </geometry>
-      </collision>
+        </collision>
+      </xacro:unless>
     </link>
 
     <joint name="${name}_1_joint" type="revolute">
@@ -102,12 +112,17 @@
         <material name="LightGrey" />
       </visual>
 
-      <collision>
-        <origin xyz="0 0 0" rpy="0 0 0" />
-        <geometry>
-          <mesh filename="package://talos_description/meshes/torso/base_link_collision.STL" scale="1 1 1"/>
-        </geometry>
-      </collision>
+      <xacro:if value="${using_capsule_collision}">
+        <xacro:capsule-collision e1="-0.0485657 0.111601 -0.05811" e2="-0.0490399 -0.110849 -0.0575714" rad="0.16382936354183916" clen="0.22245096803647357" rpy="1.56838 -0.00213179 -0.00212663" mid="-0.0488028 0.000376009 -0.0578407" />
+      </xacro:if>
+      <xacro:unless value="${using_capsule_collision}">
+        <collision>
+          <origin xyz="0 0 0" rpy="0 0 0" />
+          <geometry>
+            <mesh filename="package://talos_description/meshes/torso/base_link_collision.STL" scale="1 1 1"/>
+          </geometry>
+        </collision>
+      </xacro:unless>
     </link>
 
     <joint name="${name}_2_joint" type="revolute">
@@ -157,4 +172,3 @@
   </xacro:macro>
 
 </robot>
-