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

added LICENSE.txt and AUTHORS.txt file

Browse Source 
add MultiBody Custom Creation example, to show how to import data from a URDF file and fill up your own data structures.
add btMultiBody::setBaseWorldTransform method
todo: fix cmake build, this patch is premake only
This commit is contained in:
erwin coumans
2015-04-23 15:41:17 -07:00
parent 51938d642e
commit 05bf86d95f
19 changed files with 501 additions and 196 deletions
Download Patch File Download Diff File Expand all files Collapse all files

239
examples/Importers/ImportURDFDemo/ImportURDFSetup.cpp
View File

@@ -11,22 +11,14 @@
#include "../CommonInterfaces/CommonParameterInterface.h"
#include "MyURDFImporter.h"
static int bodyCollisionFilterGroup=btBroadphaseProxy::CharacterFilter;
static int bodyCollisionFilterMask=btBroadphaseProxy::AllFilter&(~btBroadphaseProxy::CharacterFilter);
static bool enableConstraints = true;//false;
#include "URDF2Bullet.h"
#include "urdf/urdfdom/urdf_parser/include/urdf_parser/urdf_parser.h"
#include "urdf_samples.h"
//#include "urdf_samples.h"
//#include "BulletCollision/CollisionShapes/btCylinderShape.h"
//#define USE_BARREL_VERTICES
//#include "OpenGLWindow/ShapeData.h"
#include <iostream>
#include <fstream>
using namespace urdf;
#include "../CommonInterfaces/CommonMultiBodyBase.h"
@@ -148,32 +140,6 @@ void ImportUrdfSetup::setFileName(const char* urdfFileName)
void printTree1(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
printTree1(*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;
}
}
}
void ImportUrdfSetup::initPhysics()
{
@@ -194,153 +160,100 @@ void ImportUrdfSetup::initPhysics()
gravity[upAxis]=-9.8;
m_dynamicsWorld->setGravity(gravity);
//int argc=0;
char relativeFileName[1024];
b3FileUtils fu;
printf("m_fileName=%s\n", m_fileName);
bool fileFound = fu.findFile(m_fileName, relativeFileName, 1024);
std::string xml_string;
char pathPrefix[1024];
pathPrefix[0] = 0;
if (!fileFound){
std::cerr << "URDF file not found, using a dummy test URDF" << std::endl;
xml_string = std::string(urdf_char);
} else
{
int maxPathLen = 1024;
fu.extractPath(relativeFileName,pathPrefix,maxPathLen);
std::fstream xml_file(relativeFileName, 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 ;
}
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 ;
std::cout << "root Link: " << root_link->name << " has " << root_link->child_links.size() << " child(ren)" << std::endl;
// print entire tree
printTree1(root_link);
printf("now using new interface\n");
std::cout << "root Link: " << root_link->name << " has " << root_link->child_links.size() << " child(ren)" << std::endl;
//now print the tree using the new interface
MyURDFImporter u2b(robot,m_guiHelper);
printTree(u2b, 0);
btTransform identityTrans;
identityTrans.setIdentity();
int numJoints = (*robot).m_numJoints;
MyURDFImporter u2b(m_guiHelper);
bool loadOk = u2b.loadURDF(m_fileName);
bool useUrdfInterfaceClass = true;
if (loadOk)
{
u2b.printTree();
{
btTransform identityTrans;
identityTrans.setIdentity();
{
btMultiBody* mb = 0;
btMultiBody* mb = 0;
//todo: move these internal API called inside the 'ConvertURDF2Bullet' call, hidden from the user
int rootLinkIndex = u2b.getRootLinkIndex();
printf("urdf root link index = %d\n",rootLinkIndex);
MyMultiBodyCreator creation(m_guiHelper);
//todo: move these internal API called inside the 'ConvertURDF2Bullet' call, hidden from the user
int rootLinkIndex = u2b.getRootLinkIndex();
printf("urdf root link index = %d\n",rootLinkIndex);
MyMultiBodyCreator creation(m_guiHelper);
ConvertURDF2Bullet(u2b,creation, identityTrans,m_dynamicsWorld,m_useMultiBody,pathPrefix);
mb = creation.getBulletMultiBody();
ConvertURDF2Bullet(u2b,creation, identityTrans,m_dynamicsWorld,m_useMultiBody,u2b.getPathPrefix());
mb = creation.getBulletMultiBody();
if (m_useMultiBody)
{
mb->setHasSelfCollision(false);
mb->finalizeMultiDof();
m_dynamicsWorld->addMultiBody(mb);
if (m_useMultiBody)
{
//create motors for each joint
//create motors for each joint
for (int i=0;i<mb->getNumLinks();i++)
{
int mbLinkIndex = i;
if (mb->getLink(mbLinkIndex).m_jointType==btMultibodyLink::eRevolute)
{
if (m_data->m_numMotors<MAX_NUM_MOTORS)
{
int urdfLinkIndex = creation.m_mb2urdfLink[mbLinkIndex];
for (int i=0;i<mb->getNumLinks();i++)
{
int mbLinkIndex = i;
if (mb->getLink(mbLinkIndex).m_jointType==btMultibodyLink::eRevolute)
{
if (m_data->m_numMotors<MAX_NUM_MOTORS)
{
int urdfLinkIndex = creation.m_mb2urdfLink[mbLinkIndex];
std::string jointName = u2b.getJointName(urdfLinkIndex);
char motorName[1024];
sprintf(motorName,"%s q'", jointName.c_str());
btScalar* motorVel = &m_data->m_motorTargetVelocities[m_data->m_numMotors];
*motorVel = 0.f;
SliderParams slider(motorName,motorVel);
slider.m_minVal=-4;
slider.m_maxVal=4;
m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
float maxMotorImpulse = 0.1f;
btMultiBodyJointMotor* motor = new btMultiBodyJointMotor(mb,mbLinkIndex,0,0,maxMotorImpulse);
m_data->m_jointMotors[m_data->m_numMotors]=motor;
m_dynamicsWorld->addMultiBodyConstraint(motor);
m_data->m_numMotors++;
}
}
std::string jointName = u2b.getJointName(urdfLinkIndex);
char motorName[1024];
sprintf(motorName,"%s q'", jointName.c_str());
btScalar* motorVel = &m_data->m_motorTargetVelocities[m_data->m_numMotors];
*motorVel = 0.f;
SliderParams slider(motorName,motorVel);
slider.m_minVal=-4;
slider.m_maxVal=4;
m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
float maxMotorImpulse = 0.1f;
btMultiBodyJointMotor* motor = new btMultiBodyJointMotor(mb,mbLinkIndex,0,0,maxMotorImpulse);
m_data->m_jointMotors[m_data->m_numMotors]=motor;
m_dynamicsWorld->addMultiBodyConstraint(motor);
m_data->m_numMotors++;
}
}
}
}
}
}
}
}
//the btMultiBody support is work-in-progress :-)
//the btMultiBody support is work-in-progress :-)
printf("numJoints/DOFS = %d\n", numJoints);
bool createGround=true;
if (createGround)
{
btVector3 groundHalfExtents(20,20,20);
groundHalfExtents[upAxis]=1.f;
btBoxShape* box = new btBoxShape(groundHalfExtents);
box->initializePolyhedralFeatures();
bool createGround=true;
if (createGround)
{
btVector3 groundHalfExtents(20,20,20);
groundHalfExtents[upAxis]=1.f;
btBoxShape* box = new btBoxShape(groundHalfExtents);
box->initializePolyhedralFeatures();
m_guiHelper->createCollisionShapeGraphicsObject(box);
btTransform start; start.setIdentity();
btVector3 groundOrigin(0,0,0);
groundOrigin[upAxis]=-2;//.5;
start.setOrigin(groundOrigin);
btRigidBody* body = createRigidBody(0,start,box);
//m_dynamicsWorld->removeRigidBody(body);
// m_dynamicsWorld->addRigidBody(body,2,1);
btVector3 color(0.5,0.5,0.5);
m_guiHelper->createRigidBodyGraphicsObject(body,color);
}
m_guiHelper->createCollisionShapeGraphicsObject(box);
btTransform start; start.setIdentity();
btVector3 groundOrigin(0,0,0);
groundOrigin[upAxis]=-2;//.5;
start.setOrigin(groundOrigin);
btRigidBody* body = createRigidBody(0,start,box);
//m_dynamicsWorld->removeRigidBody(body);
// m_dynamicsWorld->addRigidBody(body,2,1);
btVector3 color(0.5,0.5,0.5);
m_guiHelper->createRigidBodyGraphicsObject(body,color);
}
///this extra stepSimulation call makes sure that all the btMultibody transforms are properly propagates.
m_dynamicsWorld->stepSimulation(1. / 240., 0);// 1., 10, 1. / 240.);
///this extra stepSimulation call makes sure that all the btMultibody transforms are properly propagates.
m_dynamicsWorld->stepSimulation(1. / 240., 0);// 1., 10, 1. / 240.);
}
}
void ImportUrdfSetup::stepSimulation(float deltaTime)

1
examples/Importers/ImportURDFDemo/MultiBodyCreationInterface.h
View File

@@ -24,6 +24,7 @@ public:
virtual class btMultiBodyLinkCollider* allocateMultiBodyLinkCollider(int urdfLinkIndex, int mbLinkIndex, btMultiBody* body) = 0;
virtual void addLinkMapping(int urdfLinkIndex, int mbLinkIndex) = 0;
};
#endif //MULTIBODY_CREATION_INTERFACE_H

1
examples/Importers/ImportURDFDemo/MyMultiBodyCreator.cpp
View File

@@ -69,3 +69,4 @@ btMultiBody* MyMultiBodyCreator::getBulletMultiBody()
{
return m_bulletMultiBody;
}

1
examples/Importers/ImportURDFDemo/MyMultiBodyCreator.h
View File

@@ -43,6 +43,7 @@ public:
virtual void addLinkMapping(int urdfLinkIndex, int mbLinkIndex);
btMultiBody* getBulletMultiBody();
};
#endif //MY_MULTIBODY_CREATOR

127
examples/Importers/ImportURDFDemo/MyURDFImporter.cpp
View File

@@ -1,3 +1,17 @@
/* Copyright (C) 2015 Google
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
#include "MyURDFImporter.h"
@@ -9,21 +23,58 @@
#include "BulletCollision/CollisionShapes/btShapeHull.h"//to create a tesselation of a generic btConvexShape
#include "../CommonInterfaces/CommonGUIHelperInterface.h"
#include "Bullet3Common/b3FileUtils.h"
#include <string>
#include "urdf/urdfdom/urdf_parser/include/urdf_parser/urdf_parser.h"
#include <iostream>
#include <fstream>
using namespace urdf;
void convertURDFToVisualShape(const Visual* visual, const char* pathPrefix, const btTransform& visualTransform, btAlignedObjectArray<GLInstanceVertex>& verticesOut, btAlignedObjectArray<int>& indicesOut);
btCollisionShape* convertURDFToCollisionShape(const Collision* visual, const char* pathPrefix);
static void printTreeInternal(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
printTreeInternal(*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;
}
}
}
struct MyURDFInternalData
{
my_shared_ptr<ModelInterface> m_robot;
std::vector<my_shared_ptr<Link> > m_links;
struct GUIHelperInterface* m_guiHelper;
const char* m_pathPrefix;
};
void MyURDFImporter::printTree()
{
printTreeInternal(m_data->m_robot->getRoot(),0);
}
enum MyFileType
{
FILE_STL=1,
@@ -33,11 +84,72 @@ enum MyFileType
MyURDFImporter::MyURDFImporter(my_shared_ptr<ModelInterface> robot,struct GUIHelperInterface* helper)
MyURDFImporter::MyURDFImporter(struct GUIHelperInterface* helper)
{
m_data = new MyURDFInternalData;
m_data->m_robot = robot;
m_data->m_robot = 0;
m_data->m_guiHelper = helper;
m_data->m_pathPrefix=0;
}
bool MyURDFImporter::loadURDF(const char* fileName)
{
//int argc=0;
char relativeFileName[1024];
b3FileUtils fu;
bool fileFound = fu.findFile(fileName, relativeFileName, 1024);
std::string xml_string;
char pathPrefix[1024];
pathPrefix[0] = 0;
if (!fileFound){
std::cerr << "URDF file not found" << std::endl;
return false;
} else
{
int maxPathLen = 1024;
fu.extractPath(relativeFileName,pathPrefix,maxPathLen);
std::fstream xml_file(relativeFileName, 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 false;
}
std::cout << "robot name is: " << robot->getName() << std::endl;
// get info from parser
std::cout << "Successfully Parsed URDF" << std::endl;
// get root link
my_shared_ptr<const Link> root_link=robot->getRoot();
if (!root_link)
{
std::cout << "Failed to find root link in URDF" << std::endl;
return false;
}
m_data->m_robot = robot;
m_data->m_robot->getLinks(m_data->m_links);
//initialize the 'index' of each link
@@ -45,9 +157,16 @@ MyURDFImporter::MyURDFImporter(my_shared_ptr<ModelInterface> robot,struct GUIHel
{
m_data->m_links[i]->m_link_index = i;
}
return true;
}
const char* MyURDFImporter::getPathPrefix()
{
return m_data->m_pathPrefix;
}
MyURDFImporter::~MyURDFImporter()
{
delete m_data;

17
examples/Importers/ImportURDFDemo/MyURDFImporter.h
View File

@@ -2,9 +2,7 @@
#define MY_URDF_IMPORTER_H
#include "URDFImporterInterface.h"
#include <vector> //temp, replace by btAlignedObjectArray
#include "urdf/urdfdom/urdf_parser/include/urdf_parser/urdf_parser.h"
class MyURDFImporter : public URDFImporterInterface
{
@@ -13,13 +11,18 @@ class MyURDFImporter : public URDFImporterInterface
public:
MyURDFImporter(my_shared_ptr<urdf::ModelInterface> robot,struct GUIHelperInterface* helper);
MyURDFImporter(struct GUIHelperInterface* guiHelper);
virtual ~MyURDFImporter();
virtual bool loadURDF(const char* fileName);
const char* getPathPrefix();
void printTree(); //for debugging
virtual int getRootLinkIndex() const;
virtual int getRootLinkIndex() const;
virtual void getLinkChildIndices(int linkIndex, btAlignedObjectArray<int>& childLinkIndices) const;
@@ -31,7 +34,7 @@ public:
virtual bool getJointInfo(int urdfLinkIndex, btTransform& parent2joint, btVector3& jointAxisInJointSpace, int& jointType, btScalar& jointLowerLimit, btScalar& jointUpperLimit) const;
virtual int convertLinkVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& inertialFrame) const;
virtual int convertLinkVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame) const;
virtual class btCompoundShape* convertLinkCollisionShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame) const;

16
examples/Importers/ImportURDFDemo/URDF2Bullet.cpp
View File

@@ -290,11 +290,7 @@ void ConvertURDF2BulletInternal(const URDFImporterInterface& u2b, MultiBodyCreat
rot*offsetInB.getRotation(), offsetInA.getOrigin(),-offsetInB.getOrigin(),disableParentCollision);
creation.addLinkMapping(urdfLinkIndex,mbLinkIndex);
btMatrix3x3 rm(rot);
btScalar y,p,r;
rm.getEulerZYX(y,p,r);
printf("y=%f,p=%f,r=%f\n", y,p,r);
} else
{
printf("Fixed joint\n");
@@ -505,6 +501,16 @@ void ConvertURDF2Bullet(const URDFImporterInterface& u2b, MultiBodyCreationInter
int urdfLinkIndex = u2b.getRootLinkIndex();
ConvertURDF2BulletInternal(u2b, creation, cache, urdfLinkIndex,rootTransformInWorldSpace,world1,createMultiBody,pathPrefix);
if (world1 && cache.m_bulletMultiBody)
{
btMultiBody* mb = cache.m_bulletMultiBody;
mb->setHasSelfCollision(false);
mb->finalizeMultiDof();
mb->setBaseWorldTransform(rootTransformInWorldSpace);
world1->addMultiBody(mb);
}
}

12
examples/Importers/ImportURDFDemo/URDF2Bullet.h
View File

@@ -16,13 +16,13 @@ class MultiBodyCreationInterface;
void printTree(const URDFImporterInterface& u2b, int linkIndex, int identationLevel=0);
void ConvertURDF2Bullet(const URDFImporterInterface& u2b,
MultiBodyCreationInterface& creationCallback,
const btTransform& rootTransformInWorldSpace,
btMultiBodyDynamicsWorld* world,
bool createMultiBody,
const char* pathPrefix);
MultiBodyCreationInterface& creationCallback,
const btTransform& rootTransformInWorldSpace,
btMultiBodyDynamicsWorld* world,
bool createMultiBody,
const char* pathPrefix);
#endif //_URDF2BULLET_H