start of URDF parsing (extremely preliminary)

Demos3/AllBullet2Demos/main.cpp

@@ -330,9 +330,11 @@ int main(int argc, char* argv[])
 		GLint err = glGetError();
 		assert(err==GL_NO_ERROR);
 		app->m_instancingRenderer->init();
-		app->m_instancingRenderer->updateCamera();
+        DrawGridData dg;
-
+//        dg.upAxis = 2;
-		app->drawGrid();
+        
+		app->m_instancingRenderer->updateCamera(dg.upAxis);
+        app->drawGrid(dg);
 
 		static int frameCount = 0;
 		frameCount++;

Demos3/ImportURDFDemo/ImportURDFSetup.cpp

@@ -45,7 +45,7 @@ void printTree(my_shared_ptr<const Link> link,int level = 0)
 #define MSTRINGIFY(A) #A
 
 
-const char* urdf_char = MSTRINGIFY(
+const char* urdf_char2 = MSTRINGIFY(
                                    <robot name="test_robot">
                                    <link name="link1" />
                                    <link name="link2" />
@@ -68,7 +68,7 @@ const char* urdf_char = MSTRINGIFY(
                                    </joint>
                                    </robot>);
 
-const char* urdf_char2 = MSTRINGIFY(
+const char* urdf_char1 = MSTRINGIFY(
                                    <?xml version="1.0"?>
                                    <robot name="myfirst">
                                    <link name="base_link">
@@ -81,8 +81,174 @@ const char* urdf_char2 = MSTRINGIFY(
                                    </robot>
                                    );
 
-int main2(int argc, char** argv)
+const char* urdf_char3 = MSTRINGIFY(<?xml version="1.0"?>
+                                   <robot name="multipleshapes">
+                                   <link name="base_link">
+                                   <visual>
+                                   <geometry>
+                                   <cylinder length="0.6" radius="0.2"/>
+                                   </geometry>
+                                   </visual>
+                                   </link>
+                                   
+                                   <link name="right_leg">
+                                   <visual>
+                                   <geometry>
+                                   <box size="0.6 .2 .1"/>
+                                   </geometry>
+                                   </visual>
+                                   </link>
+                                   
+                                   <joint name="base_to_right_leg" type="fixed">
+                                   <parent link="base_link"/>
+                                   <child link="right_leg"/>
+                                   </joint>
+                                   
+                                   </robot>);
+
+const char* urdf_char = MSTRINGIFY(<?xml version="1.0"?>
+                                   <robot name="materials">
+                                   <link name="base_link">
+                                   <visual>
+                                   <geometry>
+                                   <cylinder length="0.6" radius="0.2"/>
+                                   </geometry>
+                                   <material name="blue">
+                                   <color rgba="0 0 .8 1"/>
+                                   </material>
+                                   </visual>
+                                   </link>
+                                   
+                                   <link name="right_leg">
+                                   <visual>
+                                   <geometry>
+                                   <box size="0.6 .2 .1"/>
+                                   </geometry>
+                                   <origin rpy="0 1.57075 0" xyz="0 0 -0.3"/>
+                                   <material name="white">
+                                   <color rgba="1 1 1 1"/>
+                                   </material>
+                                   </visual>
+                                   </link>
+                                   
+                                   <joint name="base_to_right_leg" type="fixed">
+                                   <parent link="base_link"/>
+                                   <child link="right_leg"/>
+                                   <origin xyz="0.22 0 .25"/>
+                                   </joint>
+                                   
+                                   <link name="left_leg">
+                                   <visual>
+                                   <geometry>
+                                   <box size="0.6 .2 .1"/>
+                                   </geometry>
+                                   <origin rpy="0 1.57075 0" xyz="0 0 -0.3"/>
+                                   <material name="white"/>
+                                   </visual>
+                                   </link>
+                                   
+                                   <joint name="base_to_left_leg" type="fixed">
+                                   <parent link="base_link"/>
+                                   <child link="left_leg"/>
+                                   <origin xyz="-0.22 0 .25"/>
+                                   </joint>
+                                   
+                                   </robot>);
+
+
+#include "BulletCollision/CollisionShapes/btCylinderShape.h"
+
+void URDFvisual2BulletCollisionShape(my_shared_ptr<const Link> link, GraphicsPhysicsBridge& gfxBridge)
 {
+    btCollisionShape* shape = 0;
+
+    {
+        printf("converting link %s",link->name.c_str());
+        
+        for (int v=0;v<link->visual_array.size();v++)
+        {
+            const Visual* visual = link->visual_array[v].get();
+            
+            
+            switch (visual->geometry->type)
+            {
+                    //            , BOX, CYLINDER, MESH:
+                case Geometry::CYLINDER:
+                {
+                    printf("processing a cylinder\n");
+                    urdf::Cylinder* cyl = (urdf::Cylinder*)visual->geometry.get();
+                    btVector3 halfExtents(cyl->radius,cyl->radius,cyl->length);
+                    btCylinderShapeZ* cylZShape = new btCylinderShapeZ(halfExtents);
+                    shape = cylZShape;
+                    break;
+                }
+                case Geometry::BOX:
+                {
+                    printf("processing a box\n");
+                    urdf::Box* box = (urdf::Box*)visual->geometry.get();
+                    btVector3 halfExtents(box->dim.x,box->dim.y,box->dim.z);
+                    btBoxShape* boxShape = new btBoxShape(halfExtents);
+                    shape = boxShape;
+                    break;
+                }
+                case Geometry::SPHERE:
+                {
+                    break;
+                }
+                case Geometry::MESH:
+                {
+                    break;
+                }
+                default:
+                {
+                    printf("Error: unknown visual geometry type\n");
+                }
+            }
+            
+            if (shape)
+            {
+                gfxBridge.createCollisionShapeGraphicsObject(shape);
+                
+                btVector3 color(0,0,1);
+                if (visual->material.get())
+                {
+                    color.setValue(visual->material->color.r,visual->material->color.g,visual->material->color.b);//,visual->material->color.a);
+                }
+                //            btScalar mass, btMotionState* motionState, btCollisionShape* collisionShape, const btVector3& localInertia=btVector3(0,0,0)):
+                btScalar mass = 0.f;
+                btVector3 localInertia(0,0,0);
+                if (mass)
+                {
+                    shape->calculateLocalInertia(mass,localInertia);
+                }
+                btRigidBody::btRigidBodyConstructionInfo rbci(mass,0,shape,localInertia);
+                btRigidBody* body = new btRigidBody(rbci);
+                gfxBridge.createRigidBodyGraphicsObject(body,color);
+            }
+        }
+    }
+    
+    for (std::vector<my_shared_ptr<Link> >::const_iterator child = link->child_links.begin(); child != link->child_links.end(); child++)
+    {
+        if (*child)
+        {
+            URDFvisual2BulletCollisionShape(*child,gfxBridge);
+            
+        }
+        else
+        {
+            std::cout << "root link: " << link->name << " has a null child!" << *child << std::endl;
+        }
+    }
+    
+  
+
+ 
+}
+void ImportUrdfDemo::initPhysics(GraphicsPhysicsBridge& gfxBridge)
+{
+    int argc=0;
+    char* filename="somefile.urdf";
     
     std::string xml_string;
     
@@ -95,7 +261,7 @@ int main2(int argc, char** argv)
     {
         
         
-        std::fstream xml_file(argv[1], std::fstream::in);
+        std::fstream xml_file(filename, std::fstream::in);
         while ( xml_file.good() )
         {
             std::string line;
@@ -120,14 +286,12 @@ int main2(int argc, char** argv)
     
     std::cout << "root Link: " << root_link->name << " has " << root_link->child_links.size() << " child(ren)" << std::endl;
     
-    
     // print entire tree
     printTree(root_link);
-    return 0;
+    
-}
+    {
-
+        URDFvisual2BulletCollisionShape(root_link, gfxBridge);
-
+    }
-void ImportUrdfDemo::initPhysics(GraphicsPhysicsBridge& gfxBridge)
+    
-{
+  
-    main2(0,0);
 }

btgui/OpenGLWindow/GLInstancingRenderer.cpp

@@ -1311,6 +1311,10 @@ void writeTextureToPng(int textureWidth, int textureHeight, const char* fileName
 
 void GLInstancingRenderer::renderScene()
 {
+	//avoid some Intel driver on a Macbook Pro to lock-up
+	//todo: figure out what is going on on that machine
+
+	glFlush();
 
 	if (useShadowMap)
 	{

btgui/OpenGLWindow/SimpleOpenGL3App.cpp

@@ -319,6 +319,7 @@ int	SimpleOpenGL3App::registerGraphicsSphereShape(float radius, bool usePointSpr
 	return graphicsShapeIndex;
 }
 
+
 void SimpleOpenGL3App::drawGrid(DrawGridData data)
 {
     int gridSize = data.gridSize;

btgui/OpenGLWindow/SimpleOpenGL3App.h

@@ -41,7 +41,7 @@ struct SimpleOpenGL3App
 
     void dumpNextFrameToPng(const char* pngFilename);
     void dumpFramesToVideo(const char* mp4Filename);
-
+    
 	void drawGrid(DrawGridData data=DrawGridData());
 	void swapBuffer();
 	void drawText( const char* txt, int posX, int posY);

btgui/urdf/urdfdom_headers/urdf_model/include/urdf_model/color.h

@@ -65,7 +65,7 @@ public:
     this->clear();
     std::vector<std::string> pieces;
     std::vector<float> rgba;
-#if 0
+
     boost::split( pieces, vector_str, boost::is_any_of(" "));
     for (unsigned int i = 0; i < pieces.size(); ++i)
     {
@@ -86,7 +86,6 @@ public:
     {
       return false;
     }
-#endif
     this->r = rgba[0];
     this->g = rgba[1];
     this->b = rgba[2];