diff --git a/urdf/simple_humanoid.urdf b/urdf/simple_humanoid.urdf
index eb5f6b5f4decfe2e90295faa0a56cd0fb55ca460..0af20e26cfb25d5434daaafdf36c0f113ef1bf2c 100644
--- a/urdf/simple_humanoid.urdf
+++ b/urdf/simple_humanoid.urdf
@@ -271,6 +271,13 @@
       <mass value="3"/>
       <inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1" />
     </inertial>
+
+    <visual>
+      <origin xyz="0 0.05 0.0" rpy="0 0 0" />
+      <geometry>
+	<cylinder length="0.05" radius="0.025" />
+      </geometry>
+    </visual>
   </link>
 
   <link name="RARM_LINK2">
@@ -279,6 +286,20 @@
       <mass value="0.6"/>
       <inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1" />
     </inertial>
+    
+    <visual>
+      <origin xyz="0 0.0 0.0" rpy="0 0 1.5708" />
+      <geometry>
+	<cylinder length="0.05" radius="0.025" />
+      </geometry>
+    </visual>
+
+    <visual>
+      <origin xyz="0 0.0 -0.1065" rpy="0 0 0" />
+      <geometry>
+	<box size="0.05 0.05 0.163" />
+      </geometry>
+    </visual>
   </link>
 
   <link name="RARM_LINK3">
@@ -287,6 +308,13 @@
       <mass value="1"/>
       <inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1" />
     </inertial>
+
+    <visual>
+      <origin xyz="0 0.0 0.05" rpy="1.5708 0 0 " />
+      <geometry>
+	<cylinder length="0.05" radius="0.025" />
+      </geometry>
+    </visual>
   </link>
 
   <link name="RARM_LINK4">
@@ -295,6 +323,20 @@
       <mass value="0.6"/>
       <inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1" />
     </inertial>
+
+    <visual>
+      <geometry>
+	<cylinder length="0.05" radius="0.025" />
+      </geometry>
+      <material name="WAIST_LINK3_APP"/>      
+    </visual>
+    
+    <visual>
+      <origin xyz="0 0.0 -0.0985" rpy="0 0 0" />
+      <geometry>
+	<box size="0.05 0.05 0.147" />
+      </geometry>
+    </visual>
   </link>
 
   <link name="RARM_LINK5">
@@ -303,6 +345,13 @@
       <mass value="0.4"/>
       <inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1" />
     </inertial>
+    
+    <visual>
+      <origin xyz="0 0 0.05" rpy="1.5708 0 0"/>      
+      <geometry>
+	<cylinder length="0.05" radius="0.025" />
+      </geometry>
+    </visual>
   </link>
 
   <link name="RARM_LINK6">
@@ -311,6 +360,26 @@
       <mass value="0.4"/>
       <inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1" />
     </inertial>
+
+    <visual>
+      <geometry>
+	<cylinder length="0.05" radius="0.025" />
+      </geometry>
+    </visual>
+
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 1.5708"/>      
+      <geometry>
+	<cylinder length="0.05" radius="0.025" />
+      </geometry>
+    </visual>
+
+    <visual>
+      <origin xyz="0 0.03 -0.1125" rpy="0 0 0"/>      
+      <geometry>
+	<box size="0.05 0.03 0.175" />
+      </geometry>
+    </visual>
   </link>
 
   <!-- VRML link name="RARM_LINK7" -->
@@ -320,6 +389,13 @@
       <mass value="0.4"/>
       <inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1" />
     </inertial>
+
+    <visual>
+      <origin xyz="0 -0.03 -0.1" rpy="0 0 0"/>      
+      <geometry>
+	<box size="0.05 0.03 0.15" />
+      </geometry>
+    </visual>
   </link>
 
   <link name="LLEG_LINK1">