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

fix windows build for b3ResourcePath

Browse Source 
replace print -> b3Printf in URDF2Bullet.cpp
This commit is contained in:
erwincoumans
2015-07-03 18:54:41 -07:00
parent 8517e85b21
commit 4630d0abb1
3 changed files with 44 additions and 30 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -10,6 +10,8 @@
#include "URDFImporterInterface.h"
#include "MultiBodyCreationInterface.h"
#include <string>
#include "Bullet3Common/b3Logging.h"
static int bodyCollisionFilterGroup=btBroadphaseProxy::CharacterFilter;
static int bodyCollisionFilterMask=btBroadphaseProxy::AllFilter&(~btBroadphaseProxy::CharacterFilter);
static bool enableConstraints = true;
@@ -41,11 +43,11 @@ void printTree(const URDFImporterInterface& u2b, int linkIndex, int indentationL
u2b.getMassAndInertia(linkIndex,mass,localInertia,inertialFrame);
std::string name = u2b.getLinkName(linkIndex);
for(int j=0;j<indentationLevel;j++)
printf(" "); //indent
printf("link %s mass=%f\n",name.c_str(),mass);
b3Printf(" "); //indent
b3Printf("link %s mass=%f\n",name.c_str(),mass);
for(int j=0;j<indentationLevel;j++)
printf(" "); //indent
printf("local inertia:%f,%f,%f\n",localInertia[0],
b3Printf(" "); //indent
b3Printf("local inertia:%f,%f,%f\n",localInertia[0],
localInertia[1],
localInertia[2]);
@@ -62,8 +64,9 @@ void printTree(const URDFImporterInterface& u2b, int linkIndex, int indentationL
std::string name = u2b.getLinkName(childLinkIndex);
for(int j=0;j<indentationLevel;j++) printf(" "); //indent
printf("child(%d).name=%s with childIndex=%d\n",(count++)+1, name.c_str(),childLinkIndex);
for(int j=0;j<indentationLevel;j++)
b3Printf(" "); //indent
b3Printf("child(%d).name=%s with childIndex=%d\n",(count++)+1, name.c_str(),childLinkIndex);
// first grandchild
printTree(u2b,childLinkIndex,indentationLevel);
}
@@ -129,11 +132,11 @@ void ComputeTotalNumberOfJoints(const URDFImporterInterface& u2b, URDF2BulletCac
{
btAlignedObjectArray<int> childIndices;
u2b.getLinkChildIndices(linkIndex,childIndices);
printf("link %s has %d children\n", u2b.getLinkName(linkIndex).c_str(),childIndices.size());
for (int i=0;i<childIndices.size();i++)
{
printf("child %d has childIndex%d=%s\n",i,childIndices[i],u2b.getLinkName(childIndices[i]).c_str());
}
//b3Printf("link %s has %d children\n", u2b.getLinkName(linkIndex).c_str(),childIndices.size());
//for (int i=0;i<childIndices.size();i++)
//{
// b3Printf("child %d has childIndex%d=%s\n",i,childIndices[i],u2b.getLinkName(childIndices[i]).c_str());
//}
cache.m_totalNumJoints1 += childIndices.size();
for (int i=0;i<childIndices.size();i++)
{
@@ -179,7 +182,7 @@ void InitURDF2BulletCache(const URDFImporterInterface& u2b, URDF2BulletCachedDat
void ConvertURDF2BulletInternal(const URDFImporterInterface& u2b, MultiBodyCreationInterface& creation, URDF2BulletCachedData& cache, int urdfLinkIndex, const btTransform& parentTransformInWorldSpace, btMultiBodyDynamicsWorld* world1,bool createMultiBody, const char* pathPrefix)
{
printf("start converting/extracting data from URDF interface\n");
b3Printf("start converting/extracting data from URDF interface\n");
btTransform linkTransformInWorldSpace;
linkTransformInWorldSpace.setIdentity();
@@ -192,8 +195,8 @@ void ConvertURDF2BulletInternal(const URDFImporterInterface& u2b, MultiBodyCreat
btRigidBody* parentRigidBody = 0;
std::string name = u2b.getLinkName(urdfLinkIndex);
printf("link name=%s urdf link index=%d\n",name.c_str(),urdfLinkIndex);
printf("mb link index = %d\n",mbLinkIndex);
b3Printf("link name=%s urdf link index=%d\n",name.c_str(),urdfLinkIndex);
b3Printf("mb link index = %d\n",mbLinkIndex);
btTransform parentLocalInertialFrame;
parentLocalInertialFrame.setIdentity();
@@ -202,11 +205,11 @@ void ConvertURDF2BulletInternal(const URDFImporterInterface& u2b, MultiBodyCreat
if (urdfParentIndex==-2)
{
printf("root link has no parent\n");
b3Printf("root link has no parent\n");
} else
{
printf("urdf parent index = %d\n",urdfParentIndex);
printf("mb parent index = %d\n",mbParentIndex);
b3Printf("urdf parent index = %d\n",urdfParentIndex);
b3Printf("mb parent index = %d\n",mbParentIndex);
parentRigidBody = cache.getRigidBodyFromLink(urdfParentIndex);
u2b.getMassAndInertia(urdfParentIndex, parentMass,parentLocalInertiaDiagonal,parentLocalInertialFrame);
@@ -302,7 +305,7 @@ void ConvertURDF2BulletInternal(const URDFImporterInterface& u2b, MultiBodyCreat
{
//todo: adjust the center of mass transform and pivot axis properly
printf("Fixed joint (btMultiBody)\n");
b3Printf("Fixed joint (btMultiBody)\n");
btQuaternion rot = offsetInA.inverse().getRotation();//parent2joint.inverse().getRotation();
cache.m_bulletMultiBody->setupFixed(mbLinkIndex, mass, localInertiaDiagonal, mbParentIndex,
rot*offsetInB.getRotation(), offsetInA.getOrigin(),-offsetInB.getOrigin(),disableParentCollision);
@@ -311,7 +314,7 @@ void ConvertURDF2BulletInternal(const URDFImporterInterface& u2b, MultiBodyCreat
} else
{
printf("Fixed joint\n");
b3Printf("Fixed joint\n");
btGeneric6DofSpring2Constraint* dof6 = creation.createFixedJoint(urdfLinkIndex,*parentRigidBody, *linkRigidBody, offsetInA, offsetInB);
@@ -340,7 +343,7 @@ void ConvertURDF2BulletInternal(const URDFImporterInterface& u2b, MultiBodyCreat
if (enableConstraints)
world1->addConstraint(dof6,true);
printf("Revolute/Continuous joint\n");
b3Printf("Revolute/Continuous joint\n");
}
break;
}
@@ -368,13 +371,13 @@ void ConvertURDF2BulletInternal(const URDFImporterInterface& u2b, MultiBodyCreat
if (enableConstraints)
world1->addConstraint(dof6,true);
printf("Prismatic\n");
b3Printf("Prismatic\n");
}
break;
}
default:
{
printf("Error: unsupported joint type in URDF (%d)\n", jointType);
b3Printf("Error: unsupported joint type in URDF (%d)\n", jointType);
btAssert(0);
}
}