Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

start of URDF parsing (extremely preliminary)

Browse Source
This commit is contained in:
Erwin Coumans
2014-08-01 13:01:31 -07:00
parent 070802f4de
commit 86f793a6ae
6 changed files with 188 additions and 18 deletions
Download Patch File Download Diff File Expand all files Collapse all files

6
Demos3/AllBullet2Demos/main.cpp
View File

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

184
Demos3/ImportURDFDemo/ImportURDFSetup.cpp
View File

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

4
btgui/OpenGLWindow/GLInstancingRenderer.cpp
View File

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

1
btgui/OpenGLWindow/SimpleOpenGL3App.cpp
View File

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

3
btgui/urdf/urdfdom_headers/urdf_model/include/urdf_model/color.h
View File

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