Repository: bulletphysics/bullet3

Issue:
pybullet: getVisualShapeData() returns wrong link indexes
Here is isolated example:

```python
import pybullet as p
p.connect(p.GUI)

huskypos = [0,0,0.1]
husky = p.loadURDF(sys.argv[1], huskypos[0],huskypos[1],huskypos[2])
cnt = p.getNumJoints(husky)

for joint in range(cnt) :
	print(p.getJointInfo(husky, joint))
	p.setJointMotorControl(husky, joint, p.VELOCITY_CONTROL, 0, 0)

viz = [""]*cnt
to_parse_array = p.getVisualShapeData(husky)
for uid, link_idx, geom, dims, filename, pos, orient in to_parse_array:
	if link_idx==-1: continue
	viz[link_idx] = str(filename)
	print("link %i have filename '%s'" % (link_idx, filename))

debug_text = []
for step in range (4000000):
	p.stepSimulation()

	for tid in debug_text:
		p.removeUserDebugItem(tid)
	debug_text = []
	for j in range(-1, cnt):
		if j==-1:
			p.getBasePositionAndOrientation(husky)
			continue
		pos, orient, inert_pos, inert_orient, frame_pos, frame_orient = p.getLinkState(husky, j)
		debug_text.append( p.addUserDebugText("O:" + viz[j][-12:], (pos[0]+1, pos[1], pos[2]) ) )
```

Output for this:

```
(0, b'back_bkz', 0, 7, 6, 1, 0.1, 0.0)
(1, b'back_bky', 0, 8, 7, 1, 0.1, 0.0)
(2, b'back_bkx', 0, 9, 8, 1, 0.1, 0.0)
(3, b'l_arm_shz', 0, 10, 9, 1, 0.1, 0.0)
(4, b'l_arm_shx', 0, 11, 10, 1, 0.1, 0.0)
...
link 23 have filename 'b'unplugged/pelvis.dae''
link 21 have filename 'b'unplugged/ltorso.dae''
link 22 have filename 'b'unplugged/mtorso.dae''
link 37 have filename 'b'unplugged/utorso.dae''
link 8 have filename 'b'unplugged/r_clav.dae''
link 16 have filename 'b'unplugged/r_scap.dae''
link 18 have filename 'b'unplugged/r_uarm.dae''
link 13 have filename 'b'meshes_v3/r_larm.dae''
```

First several joints connect parts of body (torso, not arms or legs). Seems improbable link 21 or 22 (which apparently are parts of left arm) should load torso models.

Here is how it looks:

![image](https://cloud.githubusercontent.com/assets/18546213/23656351/6ebb6508-034a-11e7-840c-8be221200dfe.png)

First models returned by getVisualShapeData() are body, first joints connect parts of body together, so it's logical to try link_index==array_index, but that attempt was also unsuccessful.




Discussion:
It could be an issue with the particular URDF file. Do you have a link to the URDF file? (possibly with any modifications you needed to make)
It is `roboschool/models_robot/atlas_description/urdf/atlas_v4_with_multisense.urdf`, with removed "package://". Also symlinks necessary for it to find `.dae`s.

Here is what I found, for R2D2:

```
link 0 have filename 'b'''
link 1 have filename 'b'''
link 2 have filename 'b'''
link 3 have filename 'b'''
link 4 have filename 'b'''
link 5 have filename 'b'''
link 6 have filename 'b'''
link 7 have filename 'b'''
link 8 have filename 'b'''
link 9 have filename 'b'''
link 10 have filename 'b's_robot/l_finger.stl''
link 11 have filename 'b'bot/l_finger_tip.stl''
link 12 have filename 'b's_robot/l_finger.stl''
link 13 have filename 'b'bot/l_finger_tip.stl''
link 14 have filename 'b'''
link 15 have filename 'b'''
```

Link numbers increase monotonically.

And for Atlas:

```
link 23 have filename 'b'unplugged/pelvis.dae''
link 21 have filename 'b'unplugged/ltorso.dae''
link 22 have filename 'b'unplugged/mtorso.dae''
link 37 have filename 'b'unplugged/utorso.dae''
link 8 have filename 'b'unplugged/r_clav.dae''
link 16 have filename 'b'unplugged/r_scap.dae''
link 18 have filename 'b'unplugged/r_uarm.dae''
link 13 have filename 'b'meshes_v3/r_larm.dae''
link 9 have filename 'b'meshes_v3/r_farm.dae''
link 11 have filename 'b'meshes_v3/r_hand.dae''
link 12 have filename 'b'''
link 0 have filename 'b'tion/meshes/head.dae''
link 1 have filename 'b'shes/head_camera.dae''
link 3 have filename 'b'''
link 5 have filename 'b'''
link 7 have filename 'b'''
link 24 have filename 'b'unplugged/r_clav.dae''
link 32 have filename 'b'unplugged/r_scap.dae''
link 34 have filename 'b'unplugged/r_uarm.dae''
link 29 have filename 'b'meshes_v3/r_larm.dae''
link 25 have filename 'b'meshes_v3/r_farm.dae''
link 27 have filename 'b'meshes_v3/r_hand.dae''
link 28 have filename 'b'''
link 38 have filename 'b'''
link 40 have filename 'b'''
link 19 have filename 'b'nplugged/l_uglut.dae''
link 14 have filename 'b'nplugged/l_lglut.dae''
link 20 have filename 'b'unplugged/l_uleg.dae''
link 15 have filename 'b'unplugged/l_lleg.dae''
link 17 have filename 'b'nplugged/l_talus.dae''
link 10 have filename 'b'unplugged/l_foot.dae''
link 35 have filename 'b'nplugged/r_uglut.dae''
link 30 have filename 'b'nplugged/r_lglut.dae''
link 36 have filename 'b'unplugged/r_uleg.dae''
link 31 have filename 'b'unplugged/r_lleg.dae''
link 33 have filename 'b'nplugged/r_talus.dae''
link 26 have filename 'b'unplugged/r_foot.dae''
```

You see, link 2 missing, link 4 is missing. Not in order. Strange.

Ok, I'll have a look at it later. By the way, the Atlas is one of the most complex robots out there, it might require quite some work to get the simulation in a nice shape.
Well it's regular URDF, links and joints... Rigid bodies with constraints kind of worked:

![photo_2017-03-07_17-41-08](https://cloud.githubusercontent.com/assets/18546213/23661275/4f262ba2-035d-11e7-9145-130fffde855d.jpg)

![photo_2017-03-07_17-41-00](https://cloud.githubusercontent.com/assets/18546213/23661274/4f24fa0c-035d-11e7-9494-0907b7cec0df.jpg)

Oh wait! 

It's not that Atlas is hard. It is rendered by internal rendering pipeline just fine! And simulated too, as far as I can tell. It is C API bug (present without using python as well).

This 'package://' stuff is ROS being overly complex (at least you don't use the xacro version)
Don't you have a version with simple relative paths to assets? We shouldn't need to worry about those details (symlinks, parsing or ignoring 'package://' etc), when we use paths relative to the URDF file.

I was going to modify URDF load to eat these files without modification.
(if you want to test Atlas, just kill `package://` and make a lot of symlinks to make valid paths it tries to use)
Wow. Same code, except:

```python
p.loadMJCF("gym/envs/mujoco/assets/humanoid.xml")
```

For all objects (floor or humanoid in humanoid.xml, or other objects in other files) `getVisualShapeData()` returns array of length 1 with uninitialized values.

(Joints work for MJCF, and URDF work with bug above, at least it's initialized)

Thanks for the report, I'll look at it soon.
(there hasn't been much effort in the 'getVisualShapeData' yet)

Patch:
diff --git a/data/MPL/mesh/index0.STL b/data/MPL/mesh/index0.stl
similarity index 100%
rename from data/MPL/mesh/index0.STL
rename to data/MPL/mesh/index0.stl
diff --git a/data/MPL/mesh/index0_collision.STL b/data/MPL/mesh/index0_collision.stl
similarity index 100%
rename from data/MPL/mesh/index0_collision.STL
rename to data/MPL/mesh/index0_collision.stl
diff --git a/data/MPL/mesh/index1.STL b/data/MPL/mesh/index1.stl
similarity index 100%
rename from data/MPL/mesh/index1.STL
rename to data/MPL/mesh/index1.stl
diff --git a/data/MPL/mesh/index1_collision.STL b/data/MPL/mesh/index1_collision.stl
similarity index 100%
rename from data/MPL/mesh/index1_collision.STL
rename to data/MPL/mesh/index1_collision.stl
diff --git a/data/MPL/mesh/index2.STL b/data/MPL/mesh/index2.stl
similarity index 100%
rename from data/MPL/mesh/index2.STL
rename to data/MPL/mesh/index2.stl
diff --git a/data/MPL/mesh/index2_collision.STL b/data/MPL/mesh/index2_collision.stl
similarity index 100%
rename from data/MPL/mesh/index2_collision.STL
rename to data/MPL/mesh/index2_collision.stl
diff --git a/data/MPL/mesh/index3.STL b/data/MPL/mesh/index3.stl
similarity index 100%
rename from data/MPL/mesh/index3.STL
rename to data/MPL/mesh/index3.stl
diff --git a/data/MPL/mesh/index3_collision.STL b/data/MPL/mesh/index3_collision.stl
similarity index 100%
rename from data/MPL/mesh/index3_collision.STL
rename to data/MPL/mesh/index3_collision.stl
diff --git a/data/MPL/mesh/middle0.STL b/data/MPL/mesh/middle0.stl
similarity index 100%
rename from data/MPL/mesh/middle0.STL
rename to data/MPL/mesh/middle0.stl
diff --git a/data/MPL/mesh/middle0_collision.STL b/data/MPL/mesh/middle0_collision.stl
similarity index 100%
rename from data/MPL/mesh/middle0_collision.STL
rename to data/MPL/mesh/middle0_collision.stl
diff --git a/data/MPL/mesh/middle1.STL b/data/MPL/mesh/middle1.stl
similarity index 100%
rename from data/MPL/mesh/middle1.STL
rename to data/MPL/mesh/middle1.stl
diff --git a/data/MPL/mesh/middle1_collision.STL b/data/MPL/mesh/middle1_collision.stl
similarity index 100%
rename from data/MPL/mesh/middle1_collision.STL
rename to data/MPL/mesh/middle1_collision.stl
diff --git a/data/MPL/mesh/middle2.STL b/data/MPL/mesh/middle2.stl
similarity index 100%
rename from data/MPL/mesh/middle2.STL
rename to data/MPL/mesh/middle2.stl
diff --git a/data/MPL/mesh/middle2_collision.STL b/data/MPL/mesh/middle2_collision.stl
similarity index 100%
rename from data/MPL/mesh/middle2_collision.STL
rename to data/MPL/mesh/middle2_collision.stl
diff --git a/data/MPL/mesh/middle3.STL b/data/MPL/mesh/middle3.stl
similarity index 100%
rename from data/MPL/mesh/middle3.STL
rename to data/MPL/mesh/middle3.stl
diff --git a/data/MPL/mesh/middle3_collision.STL b/data/MPL/mesh/middle3_collision.stl
similarity index 100%
rename from data/MPL/mesh/middle3_collision.STL
rename to data/MPL/mesh/middle3_collision.stl
diff --git a/data/MPL/mesh/palm.STL b/data/MPL/mesh/palm.stl
similarity index 100%
rename from data/MPL/mesh/palm.STL
rename to data/MPL/mesh/palm.stl
diff --git a/data/MPL/mesh/palm_collision.STL b/data/MPL/mesh/palm_collision.stl
similarity index 100%
rename from data/MPL/mesh/palm_collision.STL
rename to data/MPL/mesh/palm_collision.stl
diff --git a/data/MPL/mesh/pinky0.STL b/data/MPL/mesh/pinky0.stl
similarity index 100%
rename from data/MPL/mesh/pinky0.STL
rename to data/MPL/mesh/pinky0.stl
diff --git a/data/MPL/mesh/pinky0_collision.STL b/data/MPL/mesh/pinky0_collision.stl
similarity index 100%
rename from data/MPL/mesh/pinky0_collision.STL
rename to data/MPL/mesh/pinky0_collision.stl
diff --git a/data/MPL/mesh/pinky1.STL b/data/MPL/mesh/pinky1.stl
similarity index 100%
rename from data/MPL/mesh/pinky1.STL
rename to data/MPL/mesh/pinky1.stl
diff --git a/data/MPL/mesh/pinky1_collision.STL b/data/MPL/mesh/pinky1_collision.stl
similarity index 100%
rename from data/MPL/mesh/pinky1_collision.STL
rename to data/MPL/mesh/pinky1_collision.stl
diff --git a/data/MPL/mesh/pinky2.STL b/data/MPL/mesh/pinky2.stl
similarity index 100%
rename from data/MPL/mesh/pinky2.STL
rename to data/MPL/mesh/pinky2.stl
diff --git a/data/MPL/mesh/pinky2_collision.STL b/data/MPL/mesh/pinky2_collision.stl
similarity index 100%
rename from data/MPL/mesh/pinky2_collision.STL
rename to data/MPL/mesh/pinky2_collision.stl
diff --git a/data/MPL/mesh/pinky3.STL b/data/MPL/mesh/pinky3.stl
similarity index 100%
rename from data/MPL/mesh/pinky3.STL
rename to data/MPL/mesh/pinky3.stl
diff --git a/data/MPL/mesh/pinky3_collision.STL b/data/MPL/mesh/pinky3_collision.stl
similarity index 100%
rename from data/MPL/mesh/pinky3_collision.STL
rename to data/MPL/mesh/pinky3_collision.stl
diff --git a/data/MPL/mesh/ring0.STL b/data/MPL/mesh/ring0.stl
similarity index 100%
rename from data/MPL/mesh/ring0.STL
rename to data/MPL/mesh/ring0.stl
diff --git a/data/MPL/mesh/ring0_collision.STL b/data/MPL/mesh/ring0_collision.stl
similarity index 100%
rename from data/MPL/mesh/ring0_collision.STL
rename to data/MPL/mesh/ring0_collision.stl
diff --git a/data/MPL/mesh/ring1.STL b/data/MPL/mesh/ring1.stl
similarity index 100%
rename from data/MPL/mesh/ring1.STL
rename to data/MPL/mesh/ring1.stl
diff --git a/data/MPL/mesh/ring1_collision.STL b/data/MPL/mesh/ring1_collision.stl
similarity index 100%
rename from data/MPL/mesh/ring1_collision.STL
rename to data/MPL/mesh/ring1_collision.stl
diff --git a/data/MPL/mesh/ring2.STL b/data/MPL/mesh/ring2.stl
similarity index 100%
rename from data/MPL/mesh/ring2.STL
rename to data/MPL/mesh/ring2.stl
diff --git a/data/MPL/mesh/ring2_collision.STL b/data/MPL/mesh/ring2_collision.stl
similarity index 100%
rename from data/MPL/mesh/ring2_collision.STL
rename to data/MPL/mesh/ring2_collision.stl
diff --git a/data/MPL/mesh/ring3.STL b/data/MPL/mesh/ring3.stl
similarity index 100%
rename from data/MPL/mesh/ring3.STL
rename to data/MPL/mesh/ring3.stl
diff --git a/data/MPL/mesh/ring3_collision.STL b/data/MPL/mesh/ring3_collision.stl
similarity index 100%
rename from data/MPL/mesh/ring3_collision.STL
rename to data/MPL/mesh/ring3_collision.stl
diff --git a/data/MPL/mesh/thumb0.STL b/data/MPL/mesh/thumb0.stl
similarity index 100%
rename from data/MPL/mesh/thumb0.STL
rename to data/MPL/mesh/thumb0.stl
diff --git a/data/MPL/mesh/thumb0_collision.STL b/data/MPL/mesh/thumb0_collision.stl
similarity index 100%
rename from data/MPL/mesh/thumb0_collision.STL
rename to data/MPL/mesh/thumb0_collision.stl
diff --git a/data/MPL/mesh/thumb1.STL b/data/MPL/mesh/thumb1.stl
similarity index 100%
rename from data/MPL/mesh/thumb1.STL
rename to data/MPL/mesh/thumb1.stl
diff --git a/data/MPL/mesh/thumb1_collision.STL b/data/MPL/mesh/thumb1_collision.stl
similarity index 100%
rename from data/MPL/mesh/thumb1_collision.STL
rename to data/MPL/mesh/thumb1_collision.stl
diff --git a/data/MPL/mesh/thumb2.STL b/data/MPL/mesh/thumb2.stl
similarity index 100%
rename from data/MPL/mesh/thumb2.STL
rename to data/MPL/mesh/thumb2.stl
diff --git a/data/MPL/mesh/thumb2_collision.STL b/data/MPL/mesh/thumb2_collision.stl
similarity index 100%
rename from data/MPL/mesh/thumb2_collision.STL
rename to data/MPL/mesh/thumb2_collision.stl
diff --git a/data/MPL/mesh/thumb3.STL b/data/MPL/mesh/thumb3.stl
similarity index 100%
rename from data/MPL/mesh/thumb3.STL
rename to data/MPL/mesh/thumb3.stl
diff --git a/data/MPL/mesh/thumb3_collision.STL b/data/MPL/mesh/thumb3_collision.stl
similarity index 100%
rename from data/MPL/mesh/thumb3_collision.STL
rename to data/MPL/mesh/thumb3_collision.stl
diff --git a/data/MPL/mesh/wristx.STL b/data/MPL/mesh/wristx.stl
similarity index 100%
rename from data/MPL/mesh/wristx.STL
rename to data/MPL/mesh/wristx.stl
diff --git a/data/MPL/mesh/wristx_collision.STL b/data/MPL/mesh/wristx_collision.stl
similarity index 100%
rename from data/MPL/mesh/wristx_collision.STL
rename to data/MPL/mesh/wristx_collision.stl
diff --git a/data/MPL/mesh/wristy.STL b/data/MPL/mesh/wristy.stl
similarity index 100%
rename from data/MPL/mesh/wristy.STL
rename to data/MPL/mesh/wristy.stl
diff --git a/data/MPL/mesh/wristy_collision.STL b/data/MPL/mesh/wristy_collision.stl
similarity index 100%
rename from data/MPL/mesh/wristy_collision.STL
rename to data/MPL/mesh/wristy_collision.stl
diff --git a/data/MPL/mesh/wristz.STL b/data/MPL/mesh/wristz.stl
similarity index 100%
rename from data/MPL/mesh/wristz.STL
rename to data/MPL/mesh/wristz.stl
diff --git a/data/MPL/mesh/wristz_collision.STL b/data/MPL/mesh/wristz_collision.stl
similarity index 100%
rename from data/MPL/mesh/wristz_collision.STL
rename to data/MPL/mesh/wristz_collision.stl
diff --git a/examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp b/examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp
index 4d225c05b3..563c657a4d 100644
--- a/examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp
+++ b/examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp
@@ -30,12 +30,6 @@
 
 #include <vector>
 
-enum eMJCF_FILE_TYPE_ENUMS
-{
-	MJCF_FILE_STL = 1,
-	MJCF_FILE_OBJ = 2
-};
-
 enum ePARENT_LINK_ENUMS
 {
 	BASE_LINK_INDEX=-1,
@@ -137,9 +131,11 @@ struct MyMJCFAsset
 struct BulletMJCFImporterInternalData
 {
 	GUIHelperInterface* m_guiHelper;
+	struct LinkVisualShapesConverter* m_customVisualShapesConverter;
 	char m_pathPrefix[1024];
 
-	std::string m_fileModelName;
+	std::string m_sourceFileName; // with path
+	std::string m_fileModelName;  // without path
 	btHashMap<btHashString,MyMJCFAsset> m_assets;
 
 	btAlignedObjectArray<UrdfModel*>	m_models;
@@ -150,22 +146,37 @@ struct BulletMJCFImporterInternalData
 
 
 	int m_activeModel;
+	int m_activeBodyUniqueId;
 	//todo: for full MJCF compatibility, we would need a stack of default values
 	int m_defaultCollisionGroup;
 	int m_defaultCollisionMask;
 	btScalar m_defaultCollisionMargin;
 
+	// joint defaults
+	std::string m_defaultJointLimited;
+
+	// geom defaults
+	std::string m_defaultGeomRgba;
+
 	//those collision shapes are deleted by caller (todo: make sure this happens!)
 	btAlignedObjectArray<btCollisionShape*> m_allocatedCollisionShapes;
 
 	BulletMJCFImporterInternalData()
 		:m_activeModel(-1),
+		m_activeBodyUniqueId(-1),
 		m_defaultCollisionGroup(1),
 		m_defaultCollisionMask(1),
 		m_defaultCollisionMargin(0.001)//assume unit meters, margin is 1mm
 	{
 		m_pathPrefix[0] = 0;
 	}
+
+	std::string sourceFileLocation(TiXmlElement* e)
+	{
+		char buf[1024];
+		snprintf(buf, sizeof(buf), "%s:%i", m_sourceFileName.c_str(), e->Row());
+		return buf;
+	}
 	
 	const UrdfLink* getLink(int modelIndex, int linkIndex) const
 	{
@@ -238,6 +249,17 @@ struct BulletMJCFImporterInternalData
 			{
 				parseAssets(child_xml,logger);
 			}
+			if (n=="joint")
+			{
+				// Other attributes here:
+				// armature="1"
+				// damping="1"
+				// limited="true"
+				if (const char* conTypeStr = child_xml->Attribute("limited"))
+				{
+					m_defaultJointLimited = child_xml->Attribute("limited");
+				}
+			}
 			if (n=="geom")
 			{
 				//contype, conaffinity 
@@ -251,6 +273,11 @@ struct BulletMJCFImporterInternalData
 				{
 					m_defaultCollisionMask = urdfLexicalCast<int>(conAffinityStr);
 				}
+				const char* rgba = child_xml->Attribute("rgba");
+				if (rgba)
+				{
+					m_defaultGeomRgba = rgba;
+				}
 			}
 		}
 		handled=true;
@@ -361,40 +388,58 @@ struct BulletMJCFImporterInternalData
 		bool isLimited = false;
 		double range[2] = {1,0};
 
+		std::string lim = m_defaultJointLimited;
 		if (limitedStr)
 		{
-			std::string lim = limitedStr;
-			if (lim=="true")
+			lim = limitedStr;
+		}
+		if (lim=="true")
+		{
+			isLimited = true;
+			//parse the 'range' field
+			btArray<std::string> pieces;
+			btArray<float> sizes;
+			btAlignedObjectArray<std::string> strArray;
+			urdfIsAnyOf(" ", strArray);
+			urdfStringSplit(pieces, rangeStr, strArray);
+			for (int i = 0; i < pieces.size(); ++i)
 			{
-				isLimited = true;
-				//parse the 'range' field
-				btArray<std::string> pieces;
-				btArray<float> sizes;
-				btAlignedObjectArray<std::string> strArray;
-				urdfIsAnyOf(" ", strArray);
-				urdfStringSplit(pieces, rangeStr, strArray);
-				for (int i = 0; i < pieces.size(); ++i)
+				if (!pieces[i].empty())
 				{
-					if (!pieces[i].empty())
-					{
-						sizes.push_back(urdfLexicalCast<double>(pieces[i].c_str()));
-					}
-				}
-				if (sizes.size()==2)
-				{
-					range[0] = sizes[0];
-					range[1] = sizes[1];
-				} else
-				{
-					logger->reportWarning("Expected range[2] in joint with limits");
+					sizes.push_back(urdfLexicalCast<double>(pieces[i].c_str()));
 				}
-
 			}
-		} else
-		{
-//			logger->reportWarning("joint without limited field");
+			if (sizes.size()==2)
+			{
+				// TODO angle units are in "<compiler angle="degree" inertiafromgeom="true"/>
+				range[0] = sizes[0] * B3_PI / 180;
+				range[1] = sizes[1] * B3_PI / 180;
+			} else
+			{
+				logger->reportWarning("Expected range[2] in joint with limits");
+			}
 		}
 
+		// TODO armature : real, "0" Armature inertia (or rotor inertia) of all
+		// degrees of freedom created by this joint. These are constants added to the
+		// diagonal of the inertia matrix in generalized coordinates. They make the
+		// simulation more stable, and often increase physical realism. This is because
+		// when a motor is attached to the system with a transmission that amplifies
+		// the motor force by c, the inertia of the rotor (i.e. the moving part of the
+		// motor) is amplified by c*c. The same holds for gears in the early stages of
+		// planetary gear boxes. These extra inertias often dominate the inertias of
+		// the robot parts that are represented explicitly in the model, and the
+		// armature attribute is the way to model them.
+
+		// TODO damping : real, "0" Damping applied to all degrees of
+		// freedom created by this joint. Unlike friction loss
+		// which is computed by the constraint solver, damping is
+		// simply a force linear in velocity. It is included in
+		// the passive forces. Despite this simplicity, larger
+		// damping values can make numerical integrators unstable,
+		// which is why our Euler integrator handles damping
+		// implicitly. See Integration in the Computation chapter.
+
 		bool jointHandled = false;
 		const UrdfLink* linkPtr = getLink(modelIndex,linkIndex);
 		
@@ -443,11 +488,11 @@ struct BulletMJCFImporterInternalData
 			jointPtr->m_parentLinkToJointTransform = parentLinkToJointTransform;
 			jointPtr->m_type = ejtype;
 			int numJoints = m_models[modelIndex]->m_joints.size();
-			
+
 			//range
 			jointPtr->m_lowerLimit = range[0];
 			jointPtr->m_upperLimit = range[1];
-			
+
 			if (nameStr)
 			{
 				jointPtr->m_name =nameStr; 
@@ -489,10 +534,23 @@ struct BulletMJCFImporterInternalData
 
 		
 
-//		const char* rgba = link_xml->Attribute("rgba");
 		const char* gType = link_xml->Attribute("type");
 		const char* sz = link_xml->Attribute("size");
 		const char* posS = link_xml->Attribute("pos");
+
+		std::string rgba = m_defaultGeomRgba;
+		if (const char* rgbattr = link_xml->Attribute("rgba"))
+		{
+			rgba = rgbattr;
+		}
+		if (!rgba.empty())
+		{
+			// "0 0.7 0.7 1"
+			parseVector4(geom.m_localMaterial.m_rgbaColor, rgba);
+			geom.m_hasLocalMaterial = true;
+			geom.m_localMaterial.m_name = rgba;
+		}
+
 		if (posS)
 		{
 			btVector3 pos(0,0,0);
@@ -560,10 +618,10 @@ struct BulletMJCFImporterInternalData
 				handledGeomType = true;
 			}
 
-			//todo: capsule, cylinder, meshes or heightfields etc
-			if (geomType == "capsule")
+			if (geomType == "capsule" || geomType == "cylinder")
 			{
-				geom.m_type = URDF_GEOM_CAPSULE;
+				// <geom conaffinity="0" contype="0" fromto="0 0 0 0 0 0.02" name="root" rgba="0.9 0.4 0.6 1" size=".011" type="cylinder"/>
+				geom.m_type = geomType=="cylinder" ? URDF_GEOM_CYLINDER : URDF_GEOM_CAPSULE;
 
 				btArray<std::string> pieces;
 				btArray<float> sizes;
@@ -621,9 +679,14 @@ struct BulletMJCFImporterInternalData
 					MyMJCFAsset* assetPtr = m_assets[meshStr];
 					if (assetPtr)
 					{
-						handledGeomType = true;
 						geom.m_type = URDF_GEOM_MESH;
 						geom.m_meshFileName = assetPtr->m_fileName;
+						bool exists = findExistingMeshFile(
+							m_sourceFileName, assetPtr->m_fileName, sourceFileLocation(link_xml),
+							&geom.m_meshFileName,
+							&geom.m_meshFileType);
+						handledGeomType = exists;
+
 						geom.m_meshScale.setValue(1,1,1);
 						//todo: parse mesh scale
 						if (sz)
@@ -632,13 +695,6 @@ struct BulletMJCFImporterInternalData
 					}
 				}
 			}
-			#if 0
-			if (geomType == "cylinder")
-			{
-				geom.m_type = URDF_GEOM_CYLINDER;
-				handledGeomType = true;
-			}
-#endif
 			if (handledGeomType)
 			{
 						
@@ -803,6 +859,7 @@ struct BulletMJCFImporterInternalData
 
 		return orgChildLinkIndex;
 	}
+
 	bool parseBody(TiXmlElement* link_xml, int modelIndex, int orgParentLinkIndex, MJCFErrorLogger* logger)
 	{
 		int newParentLinkIndex = orgParentLinkIndex;
@@ -964,10 +1021,6 @@ struct BulletMJCFImporterInternalData
 		}
 
 		linkPtr->m_linkTransformInWorld = linkTransform;
-		if (bodyN == "cart1")//front_left_leg")
-		{
-			printf("found!\n");
-		}
 		if ((newParentLinkIndex != INVALID_LINK_INDEX) && !skipFixedJoint)
 		{
 			//linkPtr->m_linkTransformInWorld.setIdentity();
@@ -1113,10 +1166,11 @@ struct BulletMJCFImporterInternalData
 
 };
 
-BulletMJCFImporter::BulletMJCFImporter(struct GUIHelperInterface* helper)
+BulletMJCFImporter::BulletMJCFImporter(struct GUIHelperInterface* helper, LinkVisualShapesConverter* customConverter)
 {
 	m_data = new BulletMJCFImporterInternalData();
 	m_data->m_guiHelper = helper;
+	m_data->m_customVisualShapesConverter = customConverter;
 }
 
 BulletMJCFImporter::~BulletMJCFImporter()
@@ -1135,7 +1189,8 @@ bool BulletMJCFImporter::loadMJCF(const char* fileName, MJCFErrorLogger* logger,
 	b3FileUtils fu;
 	
 	//bool fileFound = fu.findFile(fileName, relativeFileName, 1024);
-  	bool fileFound = (b3ResourcePath::findResourcePath(fileName,relativeFileName,1024)>0);
+	bool fileFound = (b3ResourcePath::findResourcePath(fileName,relativeFileName,1024)>0);
+	m_data->m_sourceFileName = relativeFileName;
 	
 	std::string xml_string;
 	m_data->m_pathPrefix[0] = 0;
@@ -1399,21 +1454,26 @@ bool BulletMJCFImporter::getLinkContactInfo(int linkIndex, URDFLinkContactInfo&
 	return false;
 }
 
-
-void BulletMJCFImporter::convertLinkVisualShapes2(int linkIndex, const char* pathPrefix, const btTransform& inertialFrame, class btCollisionObject* colObj, int objectIndex) const
+void BulletMJCFImporter::convertLinkVisualShapes2(int linkIndex, int urdfIndex, const char* pathPrefix, const btTransform& inertialFrame, class btCollisionObject* colObj, int objectIndex) const
 {
+	if (m_data->m_customVisualShapesConverter)
+	{
+		const UrdfLink* link = m_data->getLink(m_data->m_activeModel, urdfIndex);
+		m_data->m_customVisualShapesConverter->convertVisualShapes(linkIndex,pathPrefix,inertialFrame, link, 0, colObj, objectIndex);
+	}
 }
+
 void BulletMJCFImporter::setBodyUniqueId(int bodyId)
 {
-
+	m_data->m_activeBodyUniqueId = bodyId;
 }
 
 int BulletMJCFImporter::getBodyUniqueId() const 
 { 
-	return 0;
+	b3Assert(m_data->m_activeBodyUniqueId != -1);
+	return m_data->m_activeBodyUniqueId;
 }
 
-
 static btCollisionShape* MjcfCreateConvexHullFromShapes(std::vector<tinyobj::shape_t>& shapes, const btVector3& geomScale, btScalar collisionMargin)
 {
 	btCompoundShape* compound = new btCompoundShape();
@@ -1496,132 +1556,87 @@ class btCompoundShape* BulletMJCFImporter::convertLinkCollisionShapes(int linkIn
 			}
 			case URDF_GEOM_MESH:
 			{
-				//////////////////////
-				if (1)
-			{
-				if (col->m_geometry.m_meshFileName.length())
+				GLInstanceGraphicsShape* glmesh = 0;
+				switch (col->m_geometry.m_meshFileType)
 				{
-					const char* filename = col->m_geometry.m_meshFileName.c_str();
-					//b3Printf("mesh->filename=%s\n",filename);
-					char fullPath[1024];
-					int fileType = 0;
-					sprintf(fullPath,"%s%s",pathPrefix,filename);
-					b3FileUtils::toLower(fullPath);
-                    char tmpPathPrefix[1024];
-                    int maxPathLen = 1024;
-                    b3FileUtils::extractPath(filename,tmpPathPrefix,maxPathLen);
-                    
-                    char collisionPathPrefix[1024];
-                    sprintf(collisionPathPrefix,"%s%s",pathPrefix,tmpPathPrefix);
-                    
-					if (strstr(fullPath,".stl"))
+				case UrdfGeometry::FILE_OBJ:
 					{
-						fileType = MJCF_FILE_STL;
-					}
-                    if (strstr(fullPath,".obj"))
-                   {
-                       fileType = MJCF_FILE_OBJ;
-                   }
-
-					sprintf(fullPath,"%s%s",pathPrefix,filename);
-					FILE* f = fopen(fullPath,"rb");
-					if (f)
-					{
-						fclose(f);
-						GLInstanceGraphicsShape* glmesh = 0;
-						
-						
-						switch (fileType)
+						if (col->m_flags & URDF_FORCE_CONCAVE_TRIMESH)
 						{
-                            case MJCF_FILE_OBJ:
-                            {
-								if (col->m_flags & URDF_FORCE_CONCAVE_TRIMESH)
-								{
-									glmesh = LoadMeshFromObj(fullPath, collisionPathPrefix);
-								}
-								else
-								{
-									std::vector<tinyobj::shape_t> shapes;
-									std::string err = tinyobj::LoadObj(shapes, fullPath, collisionPathPrefix);
-									//create a convex hull for each shape, and store it in a btCompoundShape
-
-									childShape = MjcfCreateConvexHullFromShapes(shapes, col->m_geometry.m_meshScale, m_data->m_defaultCollisionMargin);
-									
-								}
-                                break;
-                            }
-						case MJCF_FILE_STL:
-							{
-								glmesh = LoadMeshFromSTL(fullPath);
-							break;
-							}
-						
-						default:
-							{
-                                b3Warning("Unsupported file type in Collision: %s\n",fullPath);
-                               
-							}
+							glmesh = LoadMeshFromObj(col->m_geometry.m_meshFileName.c_str(), 0);
 						}
-					
-
-						if (!childShape && glmesh && (glmesh->m_numvertices>0))
+						else
 						{
-							//b3Printf("extracted %d verticed from STL file %s\n", glmesh->m_numvertices,fullPath);
-							//int shapeId = m_glApp->m_instancingRenderer->registerShape(&gvertices[0].pos[0],gvertices.size(),&indices[0],indices.size());
-							//convex->setUserIndex(shapeId);
-							btAlignedObjectArray<btVector3> convertedVerts;
-							convertedVerts.reserve(glmesh->m_numvertices);
-							for (int i=0;i<glmesh->m_numvertices;i++)
-							{
-								convertedVerts.push_back(btVector3(
-                                           glmesh->m_vertices->at(i).xyzw[0]*col->m_geometry.m_meshScale[0],
-                                           glmesh->m_vertices->at(i).xyzw[1]*col->m_geometry.m_meshScale[1],
-                                           glmesh->m_vertices->at(i).xyzw[2]*col->m_geometry.m_meshScale[2]));
-							}
-							
-							if (col->m_flags & URDF_FORCE_CONCAVE_TRIMESH)
-							{
-								
-								btTriangleMesh* meshInterface = new btTriangleMesh();
-								for (int i=0;i<glmesh->m_numIndices/3;i++)
-								{
-									float* v0 = glmesh->m_vertices->at(glmesh->m_indices->at(i*3)).xyzw;
-									float* v1 = glmesh->m_vertices->at(glmesh->m_indices->at(i*3+1)).xyzw;
-									float* v2 = glmesh->m_vertices->at(glmesh->m_indices->at(i*3+2)).xyzw;
-									meshInterface->addTriangle(btVector3(v0[0],v0[1],v0[2]),
-																btVector3(v1[0],v1[1],v1[2]),
-															btVector3(v2[0],v2[1],v2[2]));
-								}
-								
-								btBvhTriangleMeshShape* trimesh = new btBvhTriangleMeshShape(meshInterface,true,true);
-								childShape = trimesh;
-							} else
-							{
-								btConvexHullShape* convexHull = new btConvexHullShape(&convertedVerts[0].getX(), convertedVerts.size(), sizeof(btVector3));
-								convexHull->optimizeConvexHull();
-								//convexHull->initializePolyhedralFeatures();
-								convexHull->setMargin(m_data->m_defaultCollisionMargin);
-								childShape = convexHull;
-							}
-						} else
+							std::vector<tinyobj::shape_t> shapes;
+							std::string err = tinyobj::LoadObj(shapes, col->m_geometry.m_meshFileName.c_str());
+							//create a convex hull for each shape, and store it in a btCompoundShape
+
+							childShape = MjcfCreateConvexHullFromShapes(shapes, col->m_geometry.m_meshScale, m_data->m_defaultCollisionMargin);
+
+						}
+						break;
+					}
+				case UrdfGeometry::FILE_STL:
+					{
+						glmesh = LoadMeshFromSTL(col->m_geometry.m_meshFileName.c_str());
+						break;
+					}
+				default:
+					b3Warning("%s: Unsupported file type in Collision: %s (maybe .dae?)\n", col->m_sourceFileLocation.c_str(), col->m_geometry.m_meshFileType);
+				}
+
+				if (childShape)
+				{
+					// okay!
+				}
+				else if (!glmesh || glmesh->m_numvertices<=0)
+				{
+					b3Warning("%s: cannot extract anything useful from mesh '%s'\n", col->m_sourceFileLocation.c_str(), col->m_geometry.m_meshFileName.c_str());
+				}
+				else
+				{
+					//b3Printf("extracted %d verticed from STL file %s\n", glmesh->m_numvertices,fullPath);
+					//int shapeId = m_glApp->m_instancingRenderer->registerShape(&gvertices[0].pos[0],gvertices.size(),&indices[0],indices.size());
+					//convex->setUserIndex(shapeId);
+					btAlignedObjectArray<btVector3> convertedVerts;
+					convertedVerts.reserve(glmesh->m_numvertices);
+					for (int i=0;i<glmesh->m_numvertices;i++)
+					{
+						convertedVerts.push_back(btVector3(
+							glmesh->m_vertices->at(i).xyzw[0]*col->m_geometry.m_meshScale[0],
+							glmesh->m_vertices->at(i).xyzw[1]*col->m_geometry.m_meshScale[1],
+							glmesh->m_vertices->at(i).xyzw[2]*col->m_geometry.m_meshScale[2]));
+					}
+
+					if (col->m_flags & URDF_FORCE_CONCAVE_TRIMESH)
+					{
+
+						btTriangleMesh* meshInterface = new btTriangleMesh();
+						for (int i=0;i<glmesh->m_numIndices/3;i++)
 						{
-							b3Warning("issue extracting mesh from STL file %s\n", fullPath);
+							float* v0 = glmesh->m_vertices->at(glmesh->m_indices->at(i*3)).xyzw;
+							float* v1 = glmesh->m_vertices->at(glmesh->m_indices->at(i*3+1)).xyzw;
+							float* v2 = glmesh->m_vertices->at(glmesh->m_indices->at(i*3+2)).xyzw;
+							meshInterface->addTriangle(btVector3(v0[0],v0[1],v0[2]),
+														btVector3(v1[0],v1[1],v1[2]),
+													btVector3(v2[0],v2[1],v2[2]));
 						}
 
-                        delete glmesh;
-                       
+						btBvhTriangleMeshShape* trimesh = new btBvhTriangleMeshShape(meshInterface,true,true);
+						childShape = trimesh;
 					} else
 					{
-						b3Warning("mesh geometry not found %s\n",fullPath);
+						btConvexHullShape* convexHull = new btConvexHullShape(&convertedVerts[0].getX(), convertedVerts.size(), sizeof(btVector3));
+						convexHull->optimizeConvexHull();
+						//convexHull->initializePolyhedralFeatures();
+						convexHull->setMargin(m_data->m_defaultCollisionMargin);
+						childShape = convexHull;
 					}
-							
 				}
-			}
 
-				//////////////////////
+				delete glmesh;
 				break;
 			}
-			
 			case URDF_GEOM_CAPSULE:
 			{
 				//todo: convert fromto to btCapsuleShape + local btTransform
@@ -1636,7 +1651,7 @@ class btCompoundShape* BulletMJCFImporter::convertLinkCollisionShapes(int linkIn
 					btVector3 fromto[2] = {f,t};
 					btScalar radii[2] = {btScalar(col->m_geometry.m_capsuleRadius)
 										,btScalar(col->m_geometry.m_capsuleRadius)};
-			
+
 					btMultiSphereShape* ms = new btMultiSphereShape(fromto,radii,2);
 					childShape = ms;
 				} else
@@ -1647,11 +1662,8 @@ class btCompoundShape* BulletMJCFImporter::convertLinkCollisionShapes(int linkIn
 				}
 				break;
 			}
-			default:
-			{
+			} // switch geom
 
-			}
-			}
 			if (childShape)
 			{
 				m_data->m_allocatedCollisionShapes.push_back(childShape);
diff --git a/examples/Importers/ImportMJCFDemo/BulletMJCFImporter.h b/examples/Importers/ImportMJCFDemo/BulletMJCFImporter.h
index f1b4396901..a719c984ea 100644
--- a/examples/Importers/ImportMJCFDemo/BulletMJCFImporter.h
+++ b/examples/Importers/ImportMJCFDemo/BulletMJCFImporter.h
@@ -18,9 +18,8 @@ class BulletMJCFImporter : public URDFImporterInterface
 {
 	struct BulletMJCFImporterInternalData* m_data;
 
-
 public:
-	BulletMJCFImporter(struct GUIHelperInterface* helper);
+	BulletMJCFImporter(struct GUIHelperInterface* helper, LinkVisualShapesConverter* customConverter);
 	virtual ~BulletMJCFImporter();
 	
 	virtual bool parseMJCFString(const char* xmlString, MJCFErrorLogger* logger);
@@ -66,7 +65,7 @@ class BulletMJCFImporter : public URDFImporterInterface
     
 	virtual int convertLinkVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& inertialFrame) const;
     
-    virtual void convertLinkVisualShapes2(int linkIndex, const char* pathPrefix, const btTransform& inertialFrame, class btCollisionObject* colObj, int objectIndex) const;
+    virtual void convertLinkVisualShapes2(int linkIndex, int urdfIndex, const char* pathPrefix, const btTransform& inertialFrame, class btCollisionObject* colObj, int objectIndex) const;
     virtual void setBodyUniqueId(int bodyId);
     virtual int getBodyUniqueId() const;
     
diff --git a/examples/Importers/ImportMJCFDemo/ImportMJCFSetup.cpp b/examples/Importers/ImportMJCFDemo/ImportMJCFSetup.cpp
index 79fff963f1..b5e8cda6cb 100644
--- a/examples/Importers/ImportMJCFDemo/ImportMJCFSetup.cpp
+++ b/examples/Importers/ImportMJCFDemo/ImportMJCFSetup.cpp
@@ -188,12 +188,12 @@ void ImportMJCFSetup::initPhysics()
 	m_filterCallback->m_filterMode = FILTER_GROUPAMASKB_OR_GROUPBMASKA2;
 	
 	//m_dynamicsWorld->getSolverInfo().m_numIterations = 100;
-    m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
-    m_dynamicsWorld->getDebugDrawer()->setDebugMode(
-    btIDebugDraw::DBG_DrawConstraints
-    +btIDebugDraw::DBG_DrawContactPoints
-    +btIDebugDraw::DBG_DrawAabb
-        );//+btIDebugDraw::DBG_DrawConstraintLimits);
+	m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
+	m_dynamicsWorld->getDebugDrawer()->setDebugMode(
+		btIDebugDraw::DBG_DrawConstraints
+		+btIDebugDraw::DBG_DrawContactPoints
+		+btIDebugDraw::DBG_DrawAabb
+	);//+btIDebugDraw::DBG_DrawConstraintLimits);
 
 
 	if (m_guiHelper->getParameterInterface())
@@ -203,20 +203,23 @@ void ImportMJCFSetup::initPhysics()
 		slider.m_maxVal = 10;
 		m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
 	}
-	
-	BulletMJCFImporter importer(m_guiHelper);
+
+	BulletMJCFImporter importer(m_guiHelper, 0);
 	MyMJCFLogger logger;
 	bool result = importer.loadMJCF(m_fileName,&logger);
 	if (result)
 	{
-		 btTransform rootTrans;
-        rootTrans.setIdentity();
-        
-        for (int m =0; m<importer.getNumModels();m++)
+		btTransform rootTrans;
+		rootTrans.setIdentity();
+
+		for (int m =0; m<importer.getNumModels();m++)
 		{
+			importer.activateModel(m);
+
+			// normally used with PhysicsServerCommandProcessor that allocates unique ids to multibodies,
+			// emulate this behavior here:
+			importer.setBodyUniqueId(m);
 
-            importer.activateModel(m);
-            
 			btMultiBody* mb = 0;
 
 
@@ -226,18 +229,17 @@ void ImportMJCFSetup::initPhysics()
 			MyMultiBodyCreator creation(m_guiHelper);
 
 			rootTrans.setIdentity();
-           importer.getRootTransformInWorld(rootTrans);
+			importer.getRootTransformInWorld(rootTrans);
 
 			ConvertURDF2Bullet(importer,creation, rootTrans,m_dynamicsWorld,m_useMultiBody,importer.getPathPrefix(),CUF_USE_MJCF);
 
 			mb = creation.getBulletMultiBody();
-                        if (mb)
-                        {
-                          printf("first MJCF file converted!\n");
-                          std::string* name =
-                              new std::string(importer.getLinkName(
-                                  importer.getRootLinkIndex()));
-                          m_nameMemory.push_back(name);
+			if (mb)
+			{
+				std::string* name =
+				new std::string(importer.getLinkName(
+					importer.getRootLinkIndex()));
+					m_nameMemory.push_back(name);
 #ifdef TEST_MULTIBODY_SERIALIZATION
 				s->registerNameForPointer(name->c_str(),name->c_str());
 #endif//TEST_MULTIBODY_SERIALIZATION
@@ -287,10 +289,11 @@ void ImportMJCFSetup::initPhysics()
 							m_data->m_numMotors++;
 						}
 					}
-
 				}
+
 			} else
 			{
+				// not multibody
 				if (1)
 				{
 					//create motors for each generic joint
diff --git a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp
index ae33643346..5ba817b51e 100644
--- a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp
+++ b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp
@@ -75,16 +75,6 @@ void BulletURDFImporter::printTree()
 //	btAssert(0);
 }
 
-
-enum MyFileType
-{
-	FILE_STL=1,
-	FILE_COLLADA=2,
-    FILE_OBJ=3,
-};
-
-
-    
 BulletURDFImporter::BulletURDFImporter(struct GUIHelperInterface* helper, LinkVisualShapesConverter* customConverter)
 {
 	m_data = new BulletURDFInternalData;
@@ -572,8 +562,8 @@ btCollisionShape* convertURDFToCollisionShape(const UrdfCollision* collision, co
 
         case URDF_GEOM_CYLINDER:
         {
-			btScalar cylRadius = collision->m_geometry.m_cylinderRadius;
-			btScalar cylLength = collision->m_geometry.m_cylinderLength;
+			btScalar cylRadius = collision->m_geometry.m_capsuleRadius;
+			btScalar cylLength = collision->m_geometry.m_capsuleHalfHeight;
 			
             btAlignedObjectArray<btVector3> vertices;
             //int numVerts = sizeof(barrel_vertices)/(9*sizeof(float));
@@ -624,7 +614,7 @@ btCollisionShape* convertURDFToCollisionShape(const UrdfCollision* collision, co
 		GLInstanceGraphicsShape* glmesh = 0;
 		switch (collision->m_geometry.m_meshFileType)
 		{
-		case FILE_OBJ:
+		case UrdfGeometry::FILE_OBJ:
 			if (collision->m_flags & URDF_FORCE_CONCAVE_TRIMESH)
 			{
 				glmesh = LoadMeshFromObj(collision->m_geometry.m_meshFileName.c_str(), 0);
@@ -640,11 +630,11 @@ btCollisionShape* convertURDFToCollisionShape(const UrdfCollision* collision, co
 			}
 			break;
 
-		case FILE_STL:
+		case UrdfGeometry::FILE_STL:
 			glmesh = LoadMeshFromSTL(collision->m_geometry.m_meshFileName.c_str());
 			break;
 
-		case FILE_COLLADA:
+		case UrdfGeometry::FILE_COLLADA:
 			{
 				btAlignedObjectArray<GLInstanceGraphicsShape> visualShapes;
 				btAlignedObjectArray<ColladaGraphicsInstance> visualShapeInstances;
@@ -795,8 +785,8 @@ static void convertURDFToVisualShapeInternal(const UrdfVisual* visual, const cha
 			for (int i = 0; i<numSteps; i++)
 			{
 
-				btScalar cylRadius = visual->m_geometry.m_cylinderRadius;
-				btScalar cylLength = visual->m_geometry.m_cylinderLength;
+				btScalar cylRadius = visual->m_geometry.m_capsuleRadius;
+				btScalar cylLength = visual->m_geometry.m_capsuleHalfHeight;
 				
 				btVector3 vert(cylRadius*btSin(SIMD_2_PI*(float(i) / numSteps)), cylRadius*btCos(SIMD_2_PI*(float(i) / numSteps)), cylLength / 2.);
 				vertices.push_back(vert);
@@ -833,7 +823,7 @@ static void convertURDFToVisualShapeInternal(const UrdfVisual* visual, const cha
 		{
 			switch (visual->m_geometry.m_meshFileType)
 			{
-			case FILE_OBJ:
+			case UrdfGeometry::FILE_OBJ:
 				{
 					b3ImportMeshData meshData;
 					if (b3ImportMeshUtility::loadAndRegisterMeshFromFileInternal(visual->m_geometry.m_meshFileName, meshData))
@@ -852,13 +842,13 @@ static void convertURDFToVisualShapeInternal(const UrdfVisual* visual, const cha
 					break;
 				}
 
-			case FILE_STL:
+			case UrdfGeometry::FILE_STL:
 				{
 					glmesh = LoadMeshFromSTL(visual->m_geometry.m_meshFileName.c_str());
 					break;
 				}
 
-			case FILE_COLLADA:
+			case UrdfGeometry::FILE_COLLADA:
 				{
 					btAlignedObjectArray<GLInstanceGraphicsShape> visualShapes;
 					btAlignedObjectArray<ColladaGraphicsInstance> visualShapeInstances;
@@ -1128,15 +1118,17 @@ bool BulletURDFImporter::getLinkContactInfo(int linkIndex, URDFLinkContactInfo&
 	return false;
 }
 
-void BulletURDFImporter::convertLinkVisualShapes2(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame, class btCollisionObject* colObj, int bodyUniqueId) const
+void BulletURDFImporter::convertLinkVisualShapes2(int linkIndex, int urdfIndex, const char* pathPrefix, const btTransform& localInertiaFrame, class btCollisionObject* colObj, int bodyUniqueId) const
 {
-
   	if (m_data->m_customVisualShapesConverter)
 	{
 		const UrdfModel& model = m_data->m_urdfParser.getModel();
-		m_data->m_customVisualShapesConverter->convertVisualShapes(linkIndex,pathPrefix,localInertiaFrame, model, colObj, bodyUniqueId);
+		UrdfLink*const* linkPtr = model.m_links.getAtIndex(urdfIndex);
+		if (linkPtr)
+		{
+			m_data->m_customVisualShapesConverter->convertVisualShapes(linkIndex,pathPrefix,localInertiaFrame, *linkPtr, &model, colObj, bodyUniqueId);
+		}
 	}
-
 }
 
 int BulletURDFImporter::getNumAllocatedCollisionShapes() const
diff --git a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.h b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.h
index 26b5a66e2a..2ceb8e0d7d 100644
--- a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.h
+++ b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.h
@@ -51,7 +51,7 @@ class BulletURDFImporter : public URDFImporterInterface
 
     virtual int convertLinkVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& inertialFrame) const;
 
-    virtual void convertLinkVisualShapes2(int linkIndex, const char* pathPrefix, const btTransform& inertialFrame, class btCollisionObject* colObj, int bodyUniqueId) const;
+    virtual void convertLinkVisualShapes2(int linkIndex, int urdfIndex, const char* pathPrefix, const btTransform& inertialFrame, class btCollisionObject* colObj, int bodyUniqueId) const;
 
     ///todo(erwincoumans) refactor this convertLinkCollisionShapes/memory allocation
     
diff --git a/examples/Importers/ImportURDFDemo/LinkVisualShapesConverter.h b/examples/Importers/ImportURDFDemo/LinkVisualShapesConverter.h
index 814838cedc..8f3d3c4116 100644
--- a/examples/Importers/ImportURDFDemo/LinkVisualShapesConverter.h
+++ b/examples/Importers/ImportURDFDemo/LinkVisualShapesConverter.h
@@ -1,9 +1,14 @@
 #ifndef LINK_VISUAL_SHAPES_CONVERTER_H
 #define LINK_VISUAL_SHAPES_CONVERTER_H
 
+struct UrdfLink;
+struct UrdfModel;
+class btTransform;
+class btCollisionObject;
+
 struct LinkVisualShapesConverter
 {
-	virtual void convertVisualShapes(int linkIndex, const char* pathPrefix, const class btTransform& localInertiaFrame, const struct UrdfModel& model, class btCollisionObject* colObj, int objectIndex)=0;
+	virtual void convertVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame, const UrdfLink* linkPtr, const UrdfModel* model, class btCollisionObject* colShape, int objectIndex) =0;
 };
 
 #endif //LINK_VISUAL_SHAPES_CONVERTER_H
diff --git a/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp b/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp
index 30f35963af..a99477d93f 100644
--- a/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp
+++ b/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp
@@ -311,7 +311,7 @@ void ConvertURDF2BulletInternal(
             
 
 
-            //untested: u2b.convertLinkVisualShapes2(urdfLinkIndex,pathPrefix,localInertialFrame,body);
+            //untested: u2b.convertLinkVisualShapes2(linkIndex,urdfLinkIndex,pathPrefix,localInertialFrame,body);
         } else
         {
             if (cache.m_bulletMultiBody==0)
@@ -469,8 +469,8 @@ void ConvertURDF2BulletInternal(
                 btVector4 color = selectColor2();//(0.0,0.0,0.5);
 				u2b.getLinkColor(urdfLinkIndex,color);
                 creation.createCollisionObjectGraphicsInstance(urdfLinkIndex,col,color);
-                
-                u2b.convertLinkVisualShapes2(urdfLinkIndex,pathPrefix,localInertialFrame,col, u2b.getBodyUniqueId());
+
+                u2b.convertLinkVisualShapes2(mbLinkIndex, urdfLinkIndex, pathPrefix, localInertialFrame,col, u2b.getBodyUniqueId());
 
 				URDFLinkContactInfo contactInfo;
 				u2b.getLinkContactInfo(urdfLinkIndex,contactInfo);
@@ -487,7 +487,7 @@ void ConvertURDF2BulletInternal(
             }
         } else
         {
-            //u2b.convertLinkVisualShapes2(urdfLinkIndex,pathPrefix,localInertialFrame,compoundShape);
+            //u2b.convertLinkVisualShapes2(urdfLinkIndex,urdfIndex,pathPrefix,localInertialFrame,compoundShape);
         }
     }
 
diff --git a/examples/Importers/ImportURDFDemo/URDFImporterInterface.h b/examples/Importers/ImportURDFDemo/URDFImporterInterface.h
index 6d4c98e57e..427f98b3b8 100644
--- a/examples/Importers/ImportURDFDemo/URDFImporterInterface.h
+++ b/examples/Importers/ImportURDFDemo/URDFImporterInterface.h
@@ -49,7 +49,7 @@ class URDFImporterInterface
 	///quick hack: need to rethink the API/dependencies of this
     virtual int convertLinkVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& inertialFrame) const { return -1;}
     
-    virtual void convertLinkVisualShapes2(int linkIndex, const char* pathPrefix, const btTransform& inertialFrame, class btCollisionObject* colObj, int objectIndex) const  { }
+    virtual void convertLinkVisualShapes2(int linkIndex, int urdfIndex, const char* pathPrefix, const btTransform& inertialFrame, class btCollisionObject* colObj, int objectIndex) const  { }
     virtual void setBodyUniqueId(int bodyId) {}
     virtual int getBodyUniqueId() const { return 0;}
 
diff --git a/examples/Importers/ImportURDFDemo/UrdfParser.cpp b/examples/Importers/ImportURDFDemo/UrdfParser.cpp
index ad5247710e..cbbc481c2d 100644
--- a/examples/Importers/ImportURDFDemo/UrdfParser.cpp
+++ b/examples/Importers/ImportURDFDemo/UrdfParser.cpp
@@ -401,8 +401,9 @@ bool UrdfParser::parseGeometry(UrdfGeometry& geom, TiXmlElement* g, ErrorLogger*
 		  logger->reportError("Cylinder shape must have both length and radius attributes");
 		  return false;
 	  }
-		geom.m_cylinderRadius = urdfLexicalCast<double>(shape->Attribute("radius"));
-		geom.m_cylinderLength = urdfLexicalCast<double>(shape->Attribute("length"));
+		geom.m_hasFromTo = false;
+		geom.m_capsuleRadius = urdfLexicalCast<double>(shape->Attribute("radius"));
+		geom.m_capsuleHalfHeight = urdfLexicalCast<double>(shape->Attribute("length"));
 		
 	}
 	else if (type_name == "capsule")
@@ -575,7 +576,7 @@ bool UrdfParser::parseVisual(UrdfModel& model, UrdfVisual& visual, TiXmlElement*
   if (name_char)
 	  visual.m_name = name_char;
 
-	visual.m_hasLocalMaterial = false;
+	visual.m_geometry.m_hasLocalMaterial = false;
 	
   // Material
   TiXmlElement *mat = config->FirstChildElement("material");
@@ -597,7 +598,7 @@ bool UrdfParser::parseVisual(UrdfModel& model, UrdfVisual& visual, TiXmlElement*
             matPtr->m_rgbaColor = rgba;
             
             visual.m_materialName = matPtr->m_name;
-            visual.m_hasLocalMaterial = true;
+            visual.m_geometry.m_hasLocalMaterial = true;
         }
     } 
     else
@@ -616,11 +617,11 @@ bool UrdfParser::parseVisual(UrdfModel& model, UrdfVisual& visual, TiXmlElement*
           TiXmlElement *c = mat->FirstChildElement("color");
           if (t||c)
           {
-              if (parseMaterial(visual.m_localMaterial, mat,logger))
+              if (parseMaterial(visual.m_geometry.m_localMaterial, mat,logger))
               {
-                  UrdfMaterial* matPtr = new UrdfMaterial(visual.m_localMaterial);
+                  UrdfMaterial* matPtr = new UrdfMaterial(visual.m_geometry.m_localMaterial);
                   model.m_materials.insert(matPtr->m_name.c_str(),matPtr);
-                  visual.m_hasLocalMaterial = true;
+                  visual.m_geometry.m_hasLocalMaterial = true;
               }
           }
       }
@@ -1408,12 +1409,12 @@ bool UrdfParser::loadUrdf(const char* urdfText, ErrorLogger* logger, bool forceF
 				for (int i=0;i<link->m_visualArray.size();i++)
 				{
 					UrdfVisual& vis = link->m_visualArray.at(i);
-					if (!vis.m_hasLocalMaterial && vis.m_materialName.c_str())
+					if (!vis.m_geometry.m_hasLocalMaterial && vis.m_materialName.c_str())
 					{
 						UrdfMaterial** mat = m_urdf2Model.m_materials.find(vis.m_materialName.c_str());
 						if (mat && *mat)
 						{
-							vis.m_localMaterial = **mat;
+							vis.m_geometry.m_localMaterial = **mat;
 						} else
 						{
 							//logger->reportError("Cannot find material with name:");
@@ -1603,12 +1604,12 @@ bool UrdfParser::loadSDF(const char* sdfText, ErrorLogger* logger)
                     for (int i=0;i<link->m_visualArray.size();i++)
                     {
                         UrdfVisual& vis = link->m_visualArray.at(i);
-                        if (!vis.m_hasLocalMaterial && vis.m_materialName.c_str())
+                        if (!vis.m_geometry.m_hasLocalMaterial && vis.m_materialName.c_str())
                         {
                             UrdfMaterial** mat = localModel->m_materials.find(vis.m_materialName.c_str());
                             if (mat && *mat)
                             {
-                                vis.m_localMaterial = **mat;
+                                vis.m_geometry.m_localMaterial = **mat;
                             } else
                             {
                                 //logger->reportError("Cannot find material with name:");
diff --git a/examples/Importers/ImportURDFDemo/UrdfParser.h b/examples/Importers/ImportURDFDemo/UrdfParser.h
index 505e2658fc..015342a0a2 100644
--- a/examples/Importers/ImportURDFDemo/UrdfParser.h
+++ b/examples/Importers/ImportURDFDemo/UrdfParser.h
@@ -18,9 +18,13 @@ struct ErrorLogger
 
 struct UrdfMaterial
 {
-	std::string m_name;	
+	std::string m_name;
 	std::string m_textureFilename;
-	btVector4 m_rgbaColor;
+	btVector4 m_rgbaColor; // [0]==r [1]==g [2]==b [3]==a
+	UrdfMaterial():
+		m_rgbaColor(0.8, 0.8, 0.8, 1)
+	{
+	}
 };
 
 struct UrdfInertia
@@ -66,9 +70,6 @@ struct UrdfGeometry
 	btVector3 m_capsuleFrom;
 	btVector3 m_capsuleTo;
 
-	double m_cylinderRadius;
-	double m_cylinderLength;
-
 	btVector3 m_planeNormal;
     
 	enum {
@@ -79,32 +80,30 @@ struct UrdfGeometry
 	int         m_meshFileType;
 	std::string m_meshFileName;
 	btVector3   m_meshScale;
+
+	UrdfMaterial m_localMaterial;
+	bool m_hasLocalMaterial;
 };
 
 bool findExistingMeshFile(const std::string& urdf_path, std::string fn,
 	const std::string& error_message_prefix,
 	std::string* out_found_filename, int* out_type); // intended to fill UrdfGeometry::m_meshFileName and Type, but can be used elsewhere
 
-struct UrdfVisual
+struct UrdfShape
 {
 	std::string m_sourceFileLocation;
 	btTransform m_linkLocalFrame;
 	UrdfGeometry m_geometry;
 	std::string m_name;
-	std::string m_materialName;
-	bool m_hasLocalMaterial;
-	UrdfMaterial m_localMaterial;
 };
 
+struct UrdfVisual: UrdfShape
+{
+	std::string m_materialName;
+};
 
-
-
-struct UrdfCollision
+struct UrdfCollision: UrdfShape
 {
-	std::string m_sourceFileLocation;
-	btTransform m_linkLocalFrame;
-	UrdfGeometry m_geometry;
-	std::string m_name;
 	int m_flags;
 	int m_collisionGroup;
 	int m_collisionMask;
diff --git a/examples/SharedMemory/PhysicsDirect.cpp b/examples/SharedMemory/PhysicsDirect.cpp
index 0989a49620..5ca0f35a2b 100644
--- a/examples/SharedMemory/PhysicsDirect.cpp
+++ b/examples/SharedMemory/PhysicsDirect.cpp
@@ -1067,4 +1067,4 @@ void PhysicsDirect::setTimeOut(double timeOutInSeconds)
 double PhysicsDirect::getTimeOut() const
 {
 	return m_data->m_timeOutInSeconds;
-}
\ No newline at end of file
+}
diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp
index c5a48efaa3..077dba179f 100644
--- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp
+++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp
@@ -1497,7 +1497,7 @@ bool PhysicsServerCommandProcessor::loadMjcf(const char* fileName, char* bufferS
 
 	m_data->m_sdfRecentLoadedBodies.clear();
 
-    BulletMJCFImporter u2b(m_data->m_guiHelper);	//, &m_data->m_visualConverter
+    BulletMJCFImporter u2b(m_data->m_guiHelper, &m_data->m_visualConverter);
 
 	bool useFixedBase = false;
 	MyMJCFLogger2 logger;
@@ -4610,20 +4610,19 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
 					int remain = totalNumVisualShapes - clientCmd.m_requestVisualShapeDataArguments.m_startingVisualShapeIndex;
 					int shapeIndex = clientCmd.m_requestVisualShapeDataArguments.m_startingVisualShapeIndex;
 
-					m_data->m_visualConverter.getVisualShapesData(clientCmd.m_requestVisualShapeDataArguments.m_bodyUniqueId,
+					int success = m_data->m_visualConverter.getVisualShapesData(clientCmd.m_requestVisualShapeDataArguments.m_bodyUniqueId,
 						shapeIndex,
 						visualShapeStoragePtr);
-
-
-                    //m_visualConverter
-					serverCmd.m_sendVisualShapeArgs.m_numRemainingVisualShapes = remain-1;
-					serverCmd.m_sendVisualShapeArgs.m_numVisualShapesCopied = 1;
-					serverCmd.m_sendVisualShapeArgs.m_startingVisualShapeIndex = clientCmd.m_requestVisualShapeDataArguments.m_startingVisualShapeIndex;
-					serverCmd.m_sendVisualShapeArgs.m_bodyUniqueId = clientCmd.m_requestVisualShapeDataArguments.m_bodyUniqueId;
-					serverCmd.m_numDataStreamBytes = sizeof(b3VisualShapeData)*serverCmd.m_sendVisualShapeArgs.m_numVisualShapesCopied;
-                    serverCmd.m_type =CMD_VISUAL_SHAPE_INFO_COMPLETED;
-                    hasStatus = true;
-                    break;
+					if (success) {
+						serverCmd.m_sendVisualShapeArgs.m_numRemainingVisualShapes = remain-1;
+						serverCmd.m_sendVisualShapeArgs.m_numVisualShapesCopied = 1;
+						serverCmd.m_sendVisualShapeArgs.m_startingVisualShapeIndex = clientCmd.m_requestVisualShapeDataArguments.m_startingVisualShapeIndex;
+						serverCmd.m_sendVisualShapeArgs.m_bodyUniqueId = clientCmd.m_requestVisualShapeDataArguments.m_bodyUniqueId;
+						serverCmd.m_numDataStreamBytes = sizeof(b3VisualShapeData)*serverCmd.m_sendVisualShapeArgs.m_numVisualShapesCopied;
+						serverCmd.m_type = CMD_VISUAL_SHAPE_INFO_COMPLETED;
+					}
+					hasStatus = true;
+					break;
                 }
                 case CMD_UPDATE_VISUAL_SHAPE:
                 {
diff --git a/examples/SharedMemory/TinyRendererVisualShapeConverter.cpp b/examples/SharedMemory/TinyRendererVisualShapeConverter.cpp
index d2c3b79bdf..520ca67104 100644
--- a/examples/SharedMemory/TinyRendererVisualShapeConverter.cpp
+++ b/examples/SharedMemory/TinyRendererVisualShapeConverter.cpp
@@ -170,7 +170,7 @@ void TinyRendererVisualShapeConverter::setLightSpecularCoeff(float specularCoeff
     m_data->m_hasLightSpecularCoeff = true;
 }
 
-void convertURDFToVisualShape(const UrdfVisual* visual, const char* urdfPathPrefix, const btTransform& visualTransform, btAlignedObjectArray<GLInstanceVertex>& verticesOut, btAlignedObjectArray<int>& indicesOut, btAlignedObjectArray<MyTexture2>& texturesOut, b3VisualShapeData& visualShapeOut)
+void convertURDFToVisualShape(const UrdfShape* visual, const char* urdfPathPrefix, const btTransform& visualTransform, btAlignedObjectArray<GLInstanceVertex>& verticesOut, btAlignedObjectArray<int>& indicesOut, btAlignedObjectArray<MyTexture2>& texturesOut, b3VisualShapeData& visualShapeOut)
 {
 
 	visualShapeOut.m_visualGeometryType = visual->m_geometry.m_type;
@@ -178,6 +178,12 @@ void convertURDFToVisualShape(const UrdfVisual* visual, const char* urdfPathPref
 	visualShapeOut.m_dimensions[1] = 0;
 	visualShapeOut.m_dimensions[2] = 0;
 	visualShapeOut.m_meshAssetFileName[0] = 0;
+	if (visual->m_geometry.m_hasLocalMaterial) {
+		visualShapeOut.m_rgbaColor[0] = visual->m_geometry.m_localMaterial.m_rgbaColor[0];
+		visualShapeOut.m_rgbaColor[1] = visual->m_geometry.m_localMaterial.m_rgbaColor[1];
+		visualShapeOut.m_rgbaColor[2] = visual->m_geometry.m_localMaterial.m_rgbaColor[2];
+		visualShapeOut.m_rgbaColor[3] = visual->m_geometry.m_localMaterial.m_rgbaColor[3];
+	}
 	
 	GLInstanceGraphicsShape* glmesh = 0;
 
@@ -186,25 +192,63 @@ void convertURDFToVisualShape(const UrdfVisual* visual, const char* urdfPathPref
 	switch (visual->m_geometry.m_type)
 	{
 		case URDF_GEOM_CYLINDER:
+		case URDF_GEOM_CAPSULE:
 		{
+			btVector3 p1 = visual->m_geometry.m_capsuleFrom;
+			btVector3 p2 = visual->m_geometry.m_capsuleTo;
+			btTransform tr;
+			tr.setIdentity();
+			btScalar rad, len;
+			if (visual->m_geometry.m_hasFromTo) {
+				btVector3 v      =  p2 - p1;
+				btVector3 center = (p2 + p1) * 0.5;
+				btVector3 up_vector(0,0,1);
+				btVector3 dir  = v.normalized();
+				btVector3 axis = dir.cross(up_vector);
+				if (axis.fuzzyZero())
+				{
+					axis = btVector3(0,0,1);
+				}
+				else
+				{
+					axis.normalize();
+				}
+				btQuaternion q(axis, -acos(dir.dot(up_vector)));
+				btTransform capsule_orient(q, center);
+				tr = visual->m_linkLocalFrame * capsule_orient;
+				len = v.length();
+				rad = visual->m_geometry.m_capsuleRadius;
+			} else {
+				tr = visual->m_linkLocalFrame;
+				len = visual->m_geometry.m_capsuleHalfHeight;
+				rad = visual->m_geometry.m_capsuleRadius;
+			}
+			visualShapeOut.m_localVisualFrame[0] = tr.getOrigin()[0];
+			visualShapeOut.m_localVisualFrame[1] = tr.getOrigin()[1];
+			visualShapeOut.m_localVisualFrame[2] = tr.getOrigin()[2];
+			visualShapeOut.m_localVisualFrame[3] = tr.getRotation()[0];
+			visualShapeOut.m_localVisualFrame[4] = tr.getRotation()[1];
+			visualShapeOut.m_localVisualFrame[5] = tr.getRotation()[2];
+			visualShapeOut.m_localVisualFrame[6] = tr.getRotation()[3];
+			visualShapeOut.m_dimensions[0] = len;
+			visualShapeOut.m_dimensions[1] = rad;
+
 			btAlignedObjectArray<btVector3> vertices;
-		
-			visualShapeOut.m_dimensions[0] = visual->m_geometry.m_cylinderLength;
-			visualShapeOut.m_dimensions[1] = visual->m_geometry.m_cylinderRadius;
-			
-			//int numVerts = sizeof(barrel_vertices)/(9*sizeof(float));
 			int numSteps = 32;
 			for (int i = 0; i<numSteps; i++)
 			{
-
-				btScalar cylRadius = visual->m_geometry.m_cylinderRadius;
-				btScalar cylLength = visual->m_geometry.m_cylinderLength;
-				
-				btVector3 vert(cylRadius*btSin(SIMD_2_PI*(float(i) / numSteps)), cylRadius*btCos(SIMD_2_PI*(float(i) / numSteps)), cylLength / 2.);
+				btVector3 vert(rad*btSin(SIMD_2_PI*(float(i) / numSteps)), rad*btCos(SIMD_2_PI*(float(i) / numSteps)), len / 2.);
 				vertices.push_back(vert);
-				vert[2] = -cylLength / 2.;
+				vert[2] = -len / 2.;
 				vertices.push_back(vert);
 			}
+			if (visual->m_geometry.m_type==URDF_GEOM_CAPSULE) {
+				// TODO: check if tiny renderer works with that, didn't check -- Oleg
+				btVector3 pole1(0, 0, + len / 2. + rad);
+				btVector3 pole2(0, 0, - len / 2. - rad);
+				vertices.push_back(pole1);
+				vertices.push_back(pole2);
+			}
 
 			btConvexHullShape* cylZShape = new btConvexHullShape(&vertices[0].x(), vertices.size(), sizeof(btVector3));
 			cylZShape->setMargin(0.001);
@@ -228,7 +272,7 @@ void convertURDFToVisualShape(const UrdfVisual* visual, const char* urdfPathPref
 		case URDF_GEOM_SPHERE:
 		{
 			visualShapeOut.m_dimensions[0] = visual->m_geometry.m_sphereRadius;
-			
+
 			btScalar radius = visual->m_geometry.m_sphereRadius;
 			btSphereShape* sphereShape = new btSphereShape(radius);
 			convexColShape = sphereShape;
@@ -244,14 +288,6 @@ void convertURDFToVisualShape(const UrdfVisual* visual, const char* urdfPathPref
 			visualShapeOut.m_dimensions[1] = visual->m_geometry.m_meshScale[1];
 			visualShapeOut.m_dimensions[2] = visual->m_geometry.m_meshScale[2];
 
-			visualShapeOut.m_localVisualFrame[0] = visual->m_linkLocalFrame.getOrigin()[0];
-			visualShapeOut.m_localVisualFrame[1] = visual->m_linkLocalFrame.getOrigin()[1];
-			visualShapeOut.m_localVisualFrame[2] = visual->m_linkLocalFrame.getOrigin()[2];
-			visualShapeOut.m_localVisualFrame[3] = visual->m_linkLocalFrame.getRotation()[0];
-			visualShapeOut.m_localVisualFrame[4] = visual->m_linkLocalFrame.getRotation()[1];
-			visualShapeOut.m_localVisualFrame[5] = visual->m_linkLocalFrame.getRotation()[2];
-			visualShapeOut.m_localVisualFrame[6] = visual->m_linkLocalFrame.getRotation()[3];
-
 			switch (visual->m_geometry.m_meshFileType)
 			{
 			case UrdfGeometry::FILE_OBJ:
@@ -467,17 +503,30 @@ void convertURDFToVisualShape(const UrdfVisual* visual, const char* urdfPathPref
 
 
 
-void TinyRendererVisualShapeConverter::convertVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame, const UrdfModel& model, class btCollisionObject* colObj, int bodyUniqueId)
+void TinyRendererVisualShapeConverter::convertVisualShapes(
+	int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame,
+	const UrdfLink* linkPtr, const UrdfModel* model,
+	class btCollisionObject* colObj, int bodyUniqueId)
 {
-    
-	
-	UrdfLink* const* linkPtr = model.m_links.getAtIndex(linkIndex);
+	btAssert(linkPtr); // TODO: remove if (not doing it now, because diff will be 50+ lines)
 	if (linkPtr)
 	{
+		const btArray<UrdfVisual>* shapeArray;
+		bool useVisual;
+		int cnt = 0;
+		if (linkPtr->m_visualArray.size() > 0)
+		{
+			useVisual = true;
+			cnt = linkPtr->m_visualArray.size();
+		}
+		else
+		{
+			// We have to see something, take collision shape. Useful for MuJoCo xml, where there is not visual shape.
+			useVisual = false;
+			cnt = linkPtr->m_collisionArray.size();
+		}
 
-		const UrdfLink* link = *linkPtr;
-	
-		for (int v1 = 0; v1 < link->m_visualArray.size();v1++)
+		for (int v1=0; v1<cnt; v1++)
 		{
 			btAlignedObjectArray<MyTexture2> textures;
 			btAlignedObjectArray<GLInstanceVertex> vertices;
@@ -485,20 +534,28 @@ void TinyRendererVisualShapeConverter::convertVisualShapes(int linkIndex, const
 			btTransform startTrans; startTrans.setIdentity();
 			//int graphicsIndex = -1;
 
-			const UrdfVisual& vis = link->m_visualArray[v1];
-			btTransform childTrans = vis.m_linkLocalFrame;
-			btHashString matName(vis.m_materialName.c_str());
-			UrdfMaterial *const * matPtr = model.m_materials[matName];
-            
-            float rgbaColor[4] = {1,1,1,1};
-            
-			if (matPtr)
+			const UrdfShape* vis;
+			if (useVisual) {
+				vis = &linkPtr->m_visualArray[v1];
+			} else {
+				vis = &linkPtr->m_collisionArray[v1];
+			}
+			btTransform childTrans = vis->m_linkLocalFrame;
+
+			float rgbaColor[4] = {1,1,1,1};
+			if (model && useVisual)
 			{
-				UrdfMaterial *const  mat = *matPtr;
-                for (int i=0;i<4;i++)
-                    rgbaColor[i] = mat->m_rgbaColor[i];
-				//printf("UrdfMaterial %s, rgba = %f,%f,%f,%f\n",mat->m_name.c_str(),mat->m_rgbaColor[0],mat->m_rgbaColor[1],mat->m_rgbaColor[2],mat->m_rgbaColor[3]);
-				//m_data->m_linkColors.insert(linkIndex,mat->m_rgbaColor);
+				btHashString matName(linkPtr->m_visualArray[v1].m_materialName.c_str());
+				UrdfMaterial*const* matPtr = model->m_materials[matName];
+				if (matPtr)
+				{
+					for (int i=0; i<4; i++)
+					{
+						rgbaColor[i] = (*matPtr)->m_rgbaColor[i];
+					}
+					//printf("UrdfMaterial %s, rgba = %f,%f,%f,%f\n",mat->m_name.c_str(),mat->m_rgbaColor[0],mat->m_rgbaColor[1],mat->m_rgbaColor[2],mat->m_rgbaColor[3]);
+					//m_data->m_linkColors.insert(linkIndex,mat->m_rgbaColor);
+				}
 			}
 			
 			TinyRendererObjectArray** visualsPtr = m_data->m_swRenderInstances[colObj];
@@ -513,19 +570,19 @@ void TinyRendererVisualShapeConverter::convertVisualShapes(int linkIndex, const
 			b3VisualShapeData visualShape;
 			visualShape.m_objectUniqueId = bodyUniqueId;
 			visualShape.m_linkIndex = linkIndex;
-            visualShape.m_localVisualFrame[0] = vis.m_linkLocalFrame.getOrigin()[0];
-            visualShape.m_localVisualFrame[1] = vis.m_linkLocalFrame.getOrigin()[1];
-			visualShape.m_localVisualFrame[2] = vis.m_linkLocalFrame.getOrigin()[2];
-			visualShape.m_localVisualFrame[3] = vis.m_linkLocalFrame.getRotation()[0];
-			visualShape.m_localVisualFrame[4] = vis.m_linkLocalFrame.getRotation()[1];
-			visualShape.m_localVisualFrame[5] = vis.m_linkLocalFrame.getRotation()[2];
-			visualShape.m_localVisualFrame[6] = vis.m_linkLocalFrame.getRotation()[3];
-            visualShape.m_rgbaColor[0] = rgbaColor[0];
-            visualShape.m_rgbaColor[1] = rgbaColor[1];
-            visualShape.m_rgbaColor[2] = rgbaColor[2];
-            visualShape.m_rgbaColor[3] = rgbaColor[3];
+			visualShape.m_localVisualFrame[0] = vis->m_linkLocalFrame.getOrigin()[0];
+			visualShape.m_localVisualFrame[1] = vis->m_linkLocalFrame.getOrigin()[1];
+			visualShape.m_localVisualFrame[2] = vis->m_linkLocalFrame.getOrigin()[2];
+			visualShape.m_localVisualFrame[3] = vis->m_linkLocalFrame.getRotation()[0];
+			visualShape.m_localVisualFrame[4] = vis->m_linkLocalFrame.getRotation()[1];
+			visualShape.m_localVisualFrame[5] = vis->m_linkLocalFrame.getRotation()[2];
+			visualShape.m_localVisualFrame[6] = vis->m_linkLocalFrame.getRotation()[3];
+			visualShape.m_rgbaColor[0] = rgbaColor[0];
+			visualShape.m_rgbaColor[1] = rgbaColor[1];
+			visualShape.m_rgbaColor[2] = rgbaColor[2];
+			visualShape.m_rgbaColor[3] = rgbaColor[3];
             
-			convertURDFToVisualShape(&vis, pathPrefix, localInertiaFrame.inverse()*childTrans, vertices, indices,textures, visualShape);
+			convertURDFToVisualShape(vis, pathPrefix, localInertiaFrame.inverse()*childTrans, vertices, indices,textures, visualShape);
 			m_data->m_visualShapes.push_back(visualShape);
 
             if (vertices.size() && indices.size())
diff --git a/examples/SharedMemory/TinyRendererVisualShapeConverter.h b/examples/SharedMemory/TinyRendererVisualShapeConverter.h
index 78b4c62d65..e9e60948fb 100644
--- a/examples/SharedMemory/TinyRendererVisualShapeConverter.h
+++ b/examples/SharedMemory/TinyRendererVisualShapeConverter.h
@@ -16,7 +16,7 @@ struct TinyRendererVisualShapeConverter : public LinkVisualShapesConverter
 	
 	virtual ~TinyRendererVisualShapeConverter();
 	
-	virtual void convertVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame, const UrdfModel& model, class btCollisionObject* colShape, int objectIndex);
+	virtual void convertVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame, const UrdfLink* linkPtr, const UrdfModel* model, class btCollisionObject* colShape, int objectIndex);
 	
 	virtual int getNumVisualShapes(int bodyUniqueId);