AuroraMiddleware/bullet3
1
0
Fork 0
mirror of https://github.com/bulletphysics/bullet3 synced 2024-12-14 22:00:05 +00:00
Code Issues Releases Wiki Activity

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

Browse Source
This commit is contained in:
Erwin Coumans 2014-07-31 16:54:28 -07:00
parent af446c79b5
commit a08d4a94e3
19 changed files with 219 additions and 41 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
Demos/CommonRigidBodySetup.h
View File

@ -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
int i;
for (i = m_dynamicsWorld->getNumConstraints() - 1; i >= 0; i--)
{
m_dynamicsWorld->removeConstraint(m_dynamicsWorld->getConstraint(i));
}
if (m_dynamicsWorld)
{
int i;
for (i = m_dynamicsWorld->getNumConstraints() - 1; i >= 0; i--)
{
m_dynamicsWorld->removeConstraint(m_dynamicsWorld->getConstraint(i));
}
for (i = m_dynamicsWorld->getNumCollisionObjects() - 1; i >= 0; i--)
{
btCollisionObject* obj = m_dynamicsWorld->getCollisionObjectArray()[i];

9
Demos3/AllBullet2Demos/BulletDemoEntries.h
View File

@ -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,7 +60,8 @@ static BulletDemoEntry allDemos[]=
{ 1, "CcdDemo", MyCcdPhysicsDemoCreateFunc },
{ 1, "Kinematic", MyKinematicObjectCreateFunc },
{ 1, "Constraints", MyConstraintCreateFunc },
{ 1, "URDF", MyImportUrdfCreateFunc },
/* {1,"ChainDemo",ChainDemo::MyCreateFunc},
// {0, "Stress tests", 0 },

25
Demos3/AllBullet2Demos/CMakeLists.txt
View File

@ -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
)

133
Demos3/ImportURDFDemo/ImportURDFSetup.cpp Normal file
View File

@ -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);
}

16
Demos3/ImportURDFDemo/ImportURDFSetup.h Normal file
View File

@ -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

4
btgui/urdf/urdfdom/urdf_parser/include/urdf_parser/urdf_parser.h
View File

@ -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>

12
btgui/urdf/urdfdom/urdf_parser/src/joint.cpp
View File

@ -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 <urdf_parser/urdf_parser.h>
#include <tinyxml/tinyxml.h>
#include <urdf/urdfdom/urdf_parser/include/urdf_parser/urdf_parser.h>
namespace urdf{

10
btgui/urdf/urdfdom/urdf_parser/src/link.cpp
View File

@ -35,22 +35,22 @@
/* Author: Wim Meeussen */
#include <urdf_parser/urdf_parser.h>
#include <urdf_model/link.h>
#include <urdf/urdfdom/urdf_parser/include/urdf_parser/urdf_parser.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

4
btgui/urdf/urdfdom/urdf_parser/src/model.cpp
View File

@ -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{

8
btgui/urdf/urdfdom/urdf_parser/src/pose.cpp
View File

@ -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 <urdf_exception/exception.h>
#include <tinyxml/tinyxml.h>
#include <urdf/urdfdom_headers/urdf_exception/include/urdf_exception/exception.h>
namespace urdf{

2
btgui/urdf/urdfdom/urdf_parser/src/twist.cpp
View File

@ -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>

2
btgui/urdf/urdfdom/urdf_parser/src/urdf_model_state.cpp
View File

@ -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>

2
btgui/urdf/urdfdom/urdf_parser/src/urdf_sensor.cpp
View File

@ -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>

4
btgui/urdf/urdfdom/urdf_parser/src/world.cpp
View File

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

4
btgui/urdf/urdfdom_headers/urdf_model/include/urdf_model/joint.h
View File

@ -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{

2
btgui/urdf/urdfdom_headers/urdf_model/include/urdf_model/link.h
View File

@ -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"

4
btgui/urdf/urdfdom_headers/urdf_model/include/urdf_model/model.h
View File

@ -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 {

6
btgui/urdf/urdfdom_headers/urdf_model/include/urdf_model/pose.h
View File

@ -49,11 +49,11 @@
#include <boost/algorithm/string.hpp>
#include <boost/lexical_cast.hpp>
#else
#include <string_split.h>
#include <lexical_cast.h>
#include <urdf/boost_replacement/string_split.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{

2
btgui/urdf/urdfdom_headers/urdf_model/include/urdf_model/twist.h
View File

@ -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{