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

188
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;
}
void ImportUrdfDemo::initPhysics(GraphicsPhysicsBridge& gfxBridge)
{
main2(0,0);
{
URDFvisual2BulletCollisionShape(root_link, gfxBridge);
}
}