create URDF skeleton for a demo (only loading data, not converting it yet)

Demos/CommonRigidBodySetup.h

@@ -70,14 +70,15 @@ struct CommonRigidBodySetup : public CommonPhysicsSetup
 		//cleanup in the reverse order of creation/initialization
 
 		//remove the rigidbodies from the dynamics world and delete them
+		
+		if (m_dynamicsWorld)
+		{
+
             int i;
             for (i = m_dynamicsWorld->getNumConstraints() - 1; i >= 0; i--)
             {
                 m_dynamicsWorld->removeConstraint(m_dynamicsWorld->getConstraint(i));
             }
-		if (m_dynamicsWorld)
-		{
-
 			for (i = m_dynamicsWorld->getNumCollisionObjects() - 1; i >= 0; i--)
 			{
 				btCollisionObject* obj = m_dynamicsWorld->getCollisionObjectArray()[i];

Demos3/AllBullet2Demos/BulletDemoEntries.h

@@ -15,6 +15,7 @@
 #include "../bullet2/ChainDemo/ChainDemo.h"
 #include "../../Demos/CcdPhysicsDemo/CcdPhysicsSetup.h"
 #include "../../Demos/ConstraintDemo/ConstraintPhysicsSetup.h"
+#include "../ImportURDFDemo/ImportUrdfSetup.h"
 
 
 static BulletDemoInterface* MyCcdPhysicsDemoCreateFunc(SimpleOpenGL3App* app)
@@ -35,6 +36,11 @@ static BulletDemoInterface* MyConstraintCreateFunc(SimpleOpenGL3App* app)
 	return new BasicDemo(app, physicsSetup);
 }
 
+static BulletDemoInterface* MyImportUrdfCreateFunc(SimpleOpenGL3App* app)
+{
+    CommonPhysicsSetup* physicsSetup = new ImportUrdfDemo();
+	return new BasicDemo(app, physicsSetup);
+}
 
 struct BulletDemoEntry
 {
@@ -54,6 +60,7 @@ static BulletDemoEntry allDemos[]=
 	{ 1, "CcdDemo", MyCcdPhysicsDemoCreateFunc },
 	{ 1, "Kinematic", MyKinematicObjectCreateFunc },
 	{ 1, "Constraints", MyConstraintCreateFunc },
+    { 1, "URDF", MyImportUrdfCreateFunc },
     
 /*	{1,"ChainDemo",ChainDemo::MyCreateFunc},
 //	{0, "Stress tests", 0 },

Demos3/AllBullet2Demos/CMakeLists.txt

@@ -27,7 +27,7 @@ SET(App_AllBullet2Demos_SRCS
 	../bullet2/BasicDemo/BasicDemo.cpp
 	../bullet2/BasicDemo/BasicDemo.h
 # the next few demos are not converted to 'newer' structure yet
-# target is to convert all Bullet 2 demos in new structure, but need to settle down on features
+# target is to convert all Bullet 2 demos in new structure but need to settle down on features
 #	../bullet2/BasicDemo/HingeDemo.cpp
 #	../bullet2/BasicDemo/HingeDemo.h
 #	../bullet2/ChainDemo/ChainDemo.cpp
@@ -38,9 +38,30 @@ SET(App_AllBullet2Demos_SRCS
 #	../bullet2/LuaDemo/LuaDemo.h
 	../GpuDemos/gwenUserInterface.cpp
 	../GpuDemos/gwenUserInterface.h
+	../ImportURDFDemo/ImportURDFSetup.cpp
+	../ImportURDFDemo/ImportURDFSetup.h
 	../../btgui/Timing/b3Clock.cpp
 	../../btgui/Timing/b3Clock.h
-
+        ../../btgui/urdf/urdfdom/urdf_parser/src/pose.cpp
+                        ../../btgui/urdf/urdfdom/urdf_parser/src/model.cpp
+                        ../../btgui/urdf/urdfdom/urdf_parser/src/link.cpp
+                        ../../btgui/urdf/urdfdom/urdf_parser/src/joint.cpp
+                        ../../btgui/urdf/urdfdom/urdf_parser/include/urdf_parser/urdf_parser.h
+                        ../../btgui/urdf/urdfdom_headers/urdf_exception/include/urdf_exception/exception.h
+                        ../../btgui/urdf/urdfdom_headers/urdf_model/include/urdf_model/pose.h
+                        ../../btgui/urdf/urdfdom_headers/urdf_model/include/urdf_model/model.h
+                        ../../btgui/urdf/urdfdom_headers/urdf_model/include/urdf_model/link.h
+                        ../../btgui/urdf/urdfdom_headers/urdf_model/include/urdf_model/joint.h
+                        ../../btgui/tinyxml/tinystr.cpp
+                        ../../btgui/tinyxml/tinyxml.cpp
+                        ../../btgui/tinyxml/tinyxmlerror.cpp
+                        ../../btgui/tinyxml/tinyxmlparser.cpp
+                        ../../btgui/urdf/boost_replacement/lexical_cast.h
+                        ../../btgui/urdf/boost_replacement/shared_ptr.h
+                        ../../btgui/urdf/boost_replacement/printf_console.cpp
+                        ../../btgui/urdf/boost_replacement/printf_console.h
+                        ../../btgui/urdf/boost_replacement/string_split.cpp
+                        ../../btgui/urdf/boost_replacement/string_split.h
 	${BULLET_PHYSICS_SOURCE_DIR}/build3/bullet.rc
 )
 

Demos3/ImportURDFDemo/ImportURDFSetup.cpp

@@ -0,0 +1,133 @@
+#include "ImportURDFSetup.h"
+
+ImportUrdfDemo::ImportUrdfDemo()
+{
+    
+}
+
+ImportUrdfDemo::~ImportUrdfDemo()
+{
+    
+}
+
+
+
+
+#include "urdf/urdfdom/urdf_parser/include/urdf_parser/urdf_parser.h"
+#include <iostream>
+#include <fstream>
+
+using namespace urdf;
+
+void printTree(my_shared_ptr<const Link> link,int level = 0)
+{
+    level+=2;
+    int count = 0;
+    for (std::vector<my_shared_ptr<Link> >::const_iterator child = link->child_links.begin(); child != link->child_links.end(); child++)
+    {
+        if (*child)
+        {
+            for(int j=0;j<level;j++) std::cout << "  "; //indent
+            std::cout << "child(" << (count++)+1 << "):  " << (*child)->name  << std::endl;
+            // first grandchild
+            printTree(*child,level);
+        }
+        else
+        {
+            for(int j=0;j<level;j++) std::cout << " "; //indent
+            std::cout << "root link: " << link->name << " has a null child!" << *child << std::endl;
+        }
+    }
+    
+}
+
+
+#define MSTRINGIFY(A) #A
+
+
+const char* urdf_char = MSTRINGIFY(
+                                   <robot name="test_robot">
+                                   <link name="link1" />
+                                   <link name="link2" />
+                                   <link name="link3" />
+                                   <link name="link4" />
+                                   
+                                   <joint name="joint1" type="continuous">
+                                   <parent link="link1"/>
+                                   <child link="link2"/>
+                                   </joint>
+                                   
+                                   <joint name="joint2" type="continuous">
+                                   <parent link="link1"/>
+                                   <child link="link3"/>
+                                   </joint>
+                                   
+                                   <joint name="joint3" type="continuous">
+                                   <parent link="link3"/>
+                                   <child link="link4"/>
+                                   </joint>
+                                   </robot>);
+
+const char* urdf_char2 = MSTRINGIFY(
+                                   <?xml version="1.0"?>
+                                   <robot name="myfirst">
+                                   <link name="base_link">
+                                   <visual>
+                                   <geometry>
+                                   <cylinder length="0.6" radius="0.2"/>
+                                   </geometry>
+                                   </visual>
+                                   </link>
+                                   </robot>
+                                   );
+
+int main2(int argc, char** argv)
+{
+    
+    std::string xml_string;
+    
+    if (argc < 2){
+        std::cerr << "No URDF file name provided, using a dummy test URDF" << std::endl;
+        
+        xml_string = std::string(urdf_char);
+        
+    } else
+    {
+        
+        
+        std::fstream xml_file(argv[1], std::fstream::in);
+        while ( xml_file.good() )
+        {
+            std::string line;
+            std::getline( xml_file, line);
+            xml_string += (line + "\n");
+        }
+        xml_file.close();
+    }
+    
+    my_shared_ptr<ModelInterface> robot = parseURDF(xml_string);
+    if (!robot){
+        std::cerr << "ERROR: Model Parsing the xml failed" << std::endl;
+        return -1;
+    }
+    std::cout << "robot name is: " << robot->getName() << std::endl;
+    
+    // get info from parser
+    std::cout << "---------- Successfully Parsed XML ---------------" << std::endl;
+    // get root link
+    my_shared_ptr<const Link> root_link=robot->getRoot();
+    if (!root_link) return -1;
+    
+    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;
+}
+
+
+void ImportUrdfDemo::initPhysics(GraphicsPhysicsBridge& gfxBridge)
+{
+    main2(0,0);
+}

Demos3/ImportURDFDemo/ImportURDFSetup.h

@@ -0,0 +1,16 @@
+#ifndef IMPORT_URDF_SETUP_H
+#define IMPORT_URDF_SETUP_H
+
+
+#include "../../Demos/CommonRigidBodySetup.h"
+
+class ImportUrdfDemo : public CommonRigidBodySetup
+{
+public:
+    ImportUrdfDemo();
+    virtual ~ImportUrdfDemo();
+    
+	virtual void initPhysics(GraphicsPhysicsBridge& gfxBridge);
+};
+
+#endif //IMPORT_URDF_SETUP_H

btgui/urdf/urdfdom/urdf_parser/include/urdf_parser/urdf_parser.h

@@ -40,7 +40,7 @@
 #include <string>
 
 #include <map>
-#include <tinyxml.h>
+#include "tinyxml/tinyxml.h"
 
 //#include <boost/function.hpp>
 
@@ -49,7 +49,7 @@
 #endif //M_PI
 
 
-#include <urdf_model/model.h>
+#include <urdf/urdfdom_headers/urdf_model/include/urdf_model/model.h>
 
 
 

btgui/urdf/urdfdom/urdf_parser/src/joint.cpp

@@ -35,22 +35,22 @@
 /* Author: John Hsu */
 
 #include <sstream>
-#include <urdf_model/joint.h>
+#include <urdf/urdfdom_headers/urdf_model/include/urdf_model/joint.h>
 #ifdef URDF_USE_BOOST
 #include <boost/lexical_cast.hpp>
 #else
-#include <lexical_cast.h>
+#include <urdf/boost_replacement/lexical_cast.h>
 #endif
-#include <urdf_model/pose.h>
+#include <urdf/urdfdom_headers/urdf_model/include/urdf_model/pose.h>
 
 #ifdef URDF_USE_CONSOLE_BRIDGE
 	#include <console_bridge/console.h>
 #else
-	#include "printf_console.h"
+	#include "urdf/boost_replacement/printf_console.h"
 #endif
 
-#include <tinyxml.h>
+#include <tinyxml/tinyxml.h>
-#include <urdf_parser/urdf_parser.h>
+#include <urdf/urdfdom/urdf_parser/include/urdf_parser/urdf_parser.h>
 
 namespace urdf{
 

btgui/urdf/urdfdom/urdf_parser/src/link.cpp

@@ -35,22 +35,22 @@
 /* Author: Wim Meeussen */
 
 
-#include <urdf_parser/urdf_parser.h>
+#include <urdf/urdfdom/urdf_parser/include/urdf_parser/urdf_parser.h>
-#include <urdf_model/link.h>
+#include <urdf/urdfdom_headers/urdf_model/include/urdf_model/link.h>
 //#include <fstream>
 //#include <sstream>
 #ifdef URDF_USE_BOOST
 #include <boost/lexical_cast.hpp>
 #else
-#include <lexical_cast.h>
+#include <urdf/boost_replacement/lexical_cast.h>
 #endif
 
 #include <algorithm>
-#include <tinyxml.h>
+#include <tinyxml/tinyxml.h>
 #ifdef URDF_USE_CONSOLE_BRIDGE
 #include <console_bridge/console.h>
 #else
-#include "printf_console.h"
+#include "urdf/boost_replacement/printf_console.h"
 #endif
 
 

btgui/urdf/urdfdom/urdf_parser/src/model.cpp

@@ -35,11 +35,11 @@
 /* Author: Wim Meeussen */
 //#include <boost/algorithm/string.hpp>
 #include <vector>
-#include "urdf_parser/urdf_parser.h"
+#include "urdf/urdfdom/urdf_parser/include/urdf_parser/urdf_parser.h"
 #ifdef URDF_USE_CONSOLE_BRIDGE
 	#include <console_bridge/console.h>
 #else
-	#include "printf_console.h"
+	#include "urdf/boost_replacement/printf_console.h"
 #endif
 
 namespace urdf{

btgui/urdf/urdfdom/urdf_parser/src/pose.cpp

@@ -35,7 +35,7 @@
 /* Author: Wim Meeussen, John Hsu */
 
 
-#include <urdf_model/pose.h>
+#include <urdf/urdfdom_headers/urdf_model/include/urdf_model/pose.h>
 #include <fstream>
 #include <sstream>
 //#include <boost/lexical_cast.hpp>
@@ -44,11 +44,11 @@
 #ifdef URDF_USE_CONSOLE_BRIDGE
 #include <console_bridge/console.h>
 #else
-#include "printf_console.h"
+#include "urdf/boost_replacement/printf_console.h"
 #endif
 
-#include <tinyxml.h>
+#include <tinyxml/tinyxml.h>
-#include <urdf_exception/exception.h>
+#include <urdf/urdfdom_headers/urdf_exception/include/urdf_exception/exception.h>
 
 
 namespace urdf{

btgui/urdf/urdfdom/urdf_parser/src/twist.cpp

@@ -35,7 +35,7 @@
 /* Author: John Hsu */
 
 
-#include <urdf_model/twist.h>
+#include <urdf/urdfdom_headers/urdf_model/include/urdf_model/twist.h>
 #include <fstream>
 #include <sstream>
 #include <boost/lexical_cast.hpp>

btgui/urdf/urdfdom/urdf_parser/src/urdf_model_state.cpp

@@ -35,7 +35,7 @@
 /* Author: John Hsu */
 
 
-#include <urdf_model_state/model_state.h>
+#include <urdf/urdfdom_headers/urdf_model_state/include/urdf_model_state/model_state.h>
 #include <fstream>
 #include <sstream>
 #include <boost/lexical_cast.hpp>

btgui/urdf/urdfdom/urdf_parser/src/urdf_sensor.cpp

@@ -35,7 +35,7 @@
 /* Author: John Hsu */
 
 
-#include <urdf_sensor/sensor.h>
+#include <urdf/urdfdom_headers/urdf_sensor/include/urdf_sensor/sensor.h>
 #include <fstream>
 #include <sstream>
 #include <boost/lexical_cast.hpp>

btgui/urdf/urdfdom/urdf_parser/src/world.cpp

@@ -35,8 +35,8 @@
 /* Author: Wim Meeussen */
 
 
-#include <urdf_world/world.h>
+#include <urdf/urdfdom_headers/urdf_world/include/urdf_world/world.h>
-#include <urdf_model/model.h>
+#include <urdf/urdfdom_headers/urdf_model/include/urdf_model/model.h>
 #include <urdf_parser/urdf_parser.h>
 #include <fstream>
 #include <sstream>

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

@@ -43,10 +43,10 @@
 #include <boost/shared_ptr.hpp>
 #define my_shared_ptr my_shared_ptr
 #else
-#include <shared_ptr.h>
+#include <urdf/boost_replacement/shared_ptr.h>
 #endif
 
-#include "urdf_model/pose.h"
+#include <urdf/urdfdom_headers/urdf_model/include/urdf_model/pose.h>
 
 
 namespace urdf{

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

@@ -46,7 +46,7 @@
 	#include <boost/shared_ptr.hpp>
 	#include <boost/weak_ptr.hpp>
 #else
-	#include <shared_ptr.h>
+	#include <urdf/boost_replacement/shared_ptr.h>
 #endif
 
 #include "joint.h"

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

@@ -40,9 +40,9 @@
 #include <string>
 #include <map>
 //#include <boost/function.hpp>
-#include <urdf_model/link.h>
+#include <urdf/urdfdom_headers/urdf_model/include/urdf_model/link.h>
 
-#include <urdf_exception/exception.h>
+#include <urdf/urdfdom_headers/urdf_exception/include/urdf_exception/exception.h>
 
 namespace urdf {
 

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

@@ -49,11 +49,11 @@
 	#include <boost/algorithm/string.hpp>
 	#include <boost/lexical_cast.hpp>
 #else
-	#include <string_split.h>
+	#include <urdf/boost_replacement/string_split.h>
-	#include <lexical_cast.h>
+	#include <urdf/boost_replacement/lexical_cast.h>
 #endif //URDF_USE_BOOST
 
-#include <urdf_exception/exception.h>
+#include <urdf/urdfdom_headers/urdf_exception/include/urdf_exception/exception.h>
 #include <assert.h>
 
 namespace urdf{

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

@@ -41,7 +41,7 @@
 #include <sstream>
 #include <vector>
 #include <math.h>
-#include <urdf_model/pose.h>
+#include <urdf/urdfdom_headers/urdf_model/include/urdf_model/pose.h>
 
 namespace urdf{