Code-style consistency improvement:
Apply clang-format-all.sh using the _clang-format file through all the cpp/.h files. make sure not to apply it to certain serialization structures, since some parser expects the * as part of the name, instead of type. This commit contains no other changes aside from adding and applying clang-format-all.sh
This commit is contained in:
erwincoumans
File diff suppressed because it is too large
Load Diff
@@ -1,5 +1,5 @@
|
||||
#ifndef BULLET_URDF_IMPORTER_H
|
||||
#define BULLET_URDF_IMPORTER_H
|
||||
#define BULLET_URDF_IMPORTER_H
|
||||
|
||||
#include "URDFImporterInterface.h"
|
||||
|
||||
@@ -13,21 +13,16 @@ struct BulletURDFTexture
|
||||
bool m_isCached;
|
||||
};
|
||||
|
||||
|
||||
///BulletURDFImporter can deal with URDF and (soon) SDF files
|
||||
class BulletURDFImporter : public URDFImporterInterface
|
||||
{
|
||||
|
||||
struct BulletURDFInternalData* m_data;
|
||||
|
||||
|
||||
public:
|
||||
|
||||
BulletURDFImporter(struct GUIHelperInterface* helper, UrdfRenderingInterface* customConverter, double globalScaling=1, int flags=0);
|
||||
BulletURDFImporter(struct GUIHelperInterface* helper, UrdfRenderingInterface* customConverter, double globalScaling = 1, int flags = 0);
|
||||
|
||||
virtual ~BulletURDFImporter();
|
||||
|
||||
|
||||
virtual bool loadURDF(const char* fileName, bool forceFixedBase = false);
|
||||
|
||||
//warning: some quick test to load SDF: we 'activate' a model, so we can re-use URDF code path
|
||||
@@ -38,15 +33,15 @@ public:
|
||||
virtual int getBodyUniqueId() const;
|
||||
const char* getPathPrefix();
|
||||
|
||||
void printTree(); //for debugging
|
||||
|
||||
void printTree(); //for debugging
|
||||
|
||||
virtual int getRootLinkIndex() const;
|
||||
|
||||
virtual void getLinkChildIndices(int linkIndex, btAlignedObjectArray<int>& childLinkIndices) const;
|
||||
|
||||
|
||||
virtual void getLinkChildIndices(int linkIndex, btAlignedObjectArray<int>& childLinkIndices) const;
|
||||
|
||||
virtual std::string getBodyName() const;
|
||||
|
||||
virtual std::string getLinkName(int linkIndex) const;
|
||||
virtual std::string getLinkName(int linkIndex) const;
|
||||
|
||||
virtual bool getLinkColor(int linkIndex, btVector4& colorRGBA) const;
|
||||
|
||||
@@ -54,50 +49,46 @@ public:
|
||||
|
||||
virtual void setLinkColor2(int linkIndex, struct UrdfMaterialColor& matCol) const;
|
||||
|
||||
virtual bool getLinkContactInfo(int urdflinkIndex, URDFLinkContactInfo& contactInfo ) const;
|
||||
|
||||
virtual bool getLinkContactInfo(int urdflinkIndex, URDFLinkContactInfo& contactInfo) const;
|
||||
|
||||
virtual bool getLinkAudioSource(int linkIndex, SDFAudioSource& audioSource) const;
|
||||
|
||||
virtual std::string getJointName(int linkIndex) const;
|
||||
|
||||
virtual void getMassAndInertia(int linkIndex, btScalar& mass,btVector3& localInertiaDiagonal, btTransform& inertialFrame) const;
|
||||
virtual void getMassAndInertia2(int urdfLinkIndex, btScalar& mass, btVector3& localInertiaDiagonal, btTransform& inertialFrame, int flags) const;
|
||||
virtual std::string getJointName(int linkIndex) const;
|
||||
|
||||
|
||||
virtual bool getJointInfo(int urdfLinkIndex, btTransform& parent2joint, btTransform& linkTransformInWorld, btVector3& jointAxisInJointSpace, int& jointType, btScalar& jointLowerLimit, btScalar& jointUpperLimit, btScalar& jointDamping, btScalar& jointFriction) const;
|
||||
virtual bool getJointInfo2(int urdfLinkIndex, btTransform& parent2joint, btTransform& linkTransformInWorld, btVector3& jointAxisInJointSpace, int& jointType, btScalar& jointLowerLimit, btScalar& jointUpperLimit, btScalar& jointDamping, btScalar& jointFriction, btScalar& jointMaxForce, btScalar& jointMaxVelocity) const;
|
||||
virtual void getMassAndInertia(int linkIndex, btScalar& mass, btVector3& localInertiaDiagonal, btTransform& inertialFrame) const;
|
||||
virtual void getMassAndInertia2(int urdfLinkIndex, btScalar& mass, btVector3& localInertiaDiagonal, btTransform& inertialFrame, int flags) const;
|
||||
|
||||
virtual bool getRootTransformInWorld(btTransform& rootTransformInWorld) const;
|
||||
virtual bool getJointInfo(int urdfLinkIndex, btTransform& parent2joint, btTransform& linkTransformInWorld, btVector3& jointAxisInJointSpace, int& jointType, btScalar& jointLowerLimit, btScalar& jointUpperLimit, btScalar& jointDamping, btScalar& jointFriction) const;
|
||||
virtual bool getJointInfo2(int urdfLinkIndex, btTransform& parent2joint, btTransform& linkTransformInWorld, btVector3& jointAxisInJointSpace, int& jointType, btScalar& jointLowerLimit, btScalar& jointUpperLimit, btScalar& jointDamping, btScalar& jointFriction, btScalar& jointMaxForce, btScalar& jointMaxVelocity) const;
|
||||
|
||||
virtual bool getRootTransformInWorld(btTransform& rootTransformInWorld) const;
|
||||
virtual void setRootTransformInWorld(const btTransform& rootTransformInWorld);
|
||||
|
||||
virtual int convertLinkVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& inertialFrame) const;
|
||||
virtual int convertLinkVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& inertialFrame) const;
|
||||
|
||||
virtual void convertLinkVisualShapes2(int linkIndex, int urdfIndex, const char* pathPrefix, const btTransform& inertialFrame, class btCollisionObject* colObj, int bodyUniqueId) const;
|
||||
virtual void convertLinkVisualShapes2(int linkIndex, int urdfIndex, const char* pathPrefix, const btTransform& inertialFrame, class btCollisionObject* colObj, int bodyUniqueId) const;
|
||||
|
||||
class btCollisionShape* convertURDFToCollisionShape(const struct UrdfCollision* collision, const char* urdfPathPrefix) const;
|
||||
|
||||
virtual int getUrdfFromCollisionShape(const btCollisionShape* collisionShape, UrdfCollision& collision) const;
|
||||
|
||||
///todo(erwincoumans) refactor this convertLinkCollisionShapes/memory allocation
|
||||
|
||||
///todo(erwincoumans) refactor this convertLinkCollisionShapes/memory allocation
|
||||
|
||||
virtual class btCompoundShape* convertLinkCollisionShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame) const;
|
||||
|
||||
virtual int getCollisionGroupAndMask(int linkIndex, int& colGroup, int& colMask) const;
|
||||
|
||||
virtual int getNumAllocatedCollisionShapes() const;
|
||||
virtual class btCollisionShape* getAllocatedCollisionShape(int index);
|
||||
|
||||
virtual int getNumAllocatedCollisionShapes() const;
|
||||
virtual class btCollisionShape* getAllocatedCollisionShape(int index);
|
||||
|
||||
virtual int getNumAllocatedMeshInterfaces() const;
|
||||
virtual class btStridingMeshInterface* getAllocatedMeshInterface(int index);
|
||||
|
||||
virtual int getNumAllocatedTextures() const;
|
||||
virtual int getAllocatedTexture(int index) const;
|
||||
|
||||
|
||||
virtual void setEnableTinyRenderer(bool enable);
|
||||
void convertURDFToVisualShapeInternal(const struct UrdfVisual* visual, const char* urdfPathPrefix, const class btTransform& visualTransform, btAlignedObjectArray<struct GLInstanceVertex>& verticesOut, btAlignedObjectArray<int>& indicesOut, btAlignedObjectArray<struct BulletURDFTexture>& texturesOut) const;
|
||||
|
||||
|
||||
};
|
||||
|
||||
|
||||
#endif //BULLET_URDF_IMPORTER_H
|
||||
#endif //BULLET_URDF_IMPORTER_H
|
||||
|
||||
@@ -5,10 +5,9 @@ struct ConvertRigidBodies2MultiBody
|
||||
{
|
||||
btAlignedObjectArray<btRigidBody*> m_bodies;
|
||||
btAlignedObjectArray<btTypedConstraint*> m_constraints;
|
||||
|
||||
virtual void addRigidBody(btRigidBody* body);
|
||||
virtual void addConstraint(btTypedConstraint* constraint, bool disableCollisionsBetweenLinkedBodies=false);
|
||||
|
||||
virtual void addRigidBody(btRigidBody* body);
|
||||
virtual void addConstraint(btTypedConstraint* constraint, bool disableCollisionsBetweenLinkedBodies = false);
|
||||
virtual btMultiBody* convertToMultiBody();
|
||||
|
||||
};
|
||||
#endif //CONVERT_RIGIDBODIES_2_MULTIBODY_H
|
||||
#endif //CONVERT_RIGIDBODIES_2_MULTIBODY_H
|
||||
|
||||
@@ -11,93 +11,82 @@
|
||||
#include "../CommonInterfaces/CommonParameterInterface.h"
|
||||
#include "../../Utils/b3ResourcePath.h"
|
||||
|
||||
|
||||
#include "BulletUrdfImporter.h"
|
||||
|
||||
|
||||
#include "URDF2Bullet.h"
|
||||
|
||||
|
||||
//#include "urdf_samples.h"
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
#include "../CommonInterfaces/CommonMultiBodyBase.h"
|
||||
|
||||
#include "MyMultiBodyCreator.h"
|
||||
|
||||
|
||||
class ImportUrdfSetup : public CommonMultiBodyBase
|
||||
{
|
||||
char m_fileName[1024];
|
||||
char m_fileName[1024];
|
||||
|
||||
struct ImportUrdfInternalData* m_data;
|
||||
struct ImportUrdfInternalData* m_data;
|
||||
bool m_useMultiBody;
|
||||
btAlignedObjectArray<std::string* > m_nameMemory;
|
||||
btAlignedObjectArray<std::string*> m_nameMemory;
|
||||
btScalar m_grav;
|
||||
int m_upAxis;
|
||||
|
||||
public:
|
||||
ImportUrdfSetup(struct GUIHelperInterface* helper, int option, const char* fileName);
|
||||
virtual ~ImportUrdfSetup();
|
||||
ImportUrdfSetup(struct GUIHelperInterface* helper, int option, const char* fileName);
|
||||
virtual ~ImportUrdfSetup();
|
||||
|
||||
virtual void initPhysics();
|
||||
virtual void stepSimulation(float deltaTime);
|
||||
|
||||
void setFileName(const char* urdfFileName);
|
||||
void setFileName(const char* urdfFileName);
|
||||
|
||||
virtual void resetCamera()
|
||||
{
|
||||
float dist = 3.5;
|
||||
float pitch = -28;
|
||||
float yaw = -136;
|
||||
float targetPos[3]={0.47,0,-0.64};
|
||||
m_guiHelper->resetCamera(dist,yaw,pitch,targetPos[0],targetPos[1],targetPos[2]);
|
||||
float targetPos[3] = {0.47, 0, -0.64};
|
||||
m_guiHelper->resetCamera(dist, yaw, pitch, targetPos[0], targetPos[1], targetPos[2]);
|
||||
}
|
||||
};
|
||||
|
||||
|
||||
btAlignedObjectArray<std::string> gFileNameArray;
|
||||
|
||||
|
||||
#define MAX_NUM_MOTORS 1024
|
||||
|
||||
struct ImportUrdfInternalData
|
||||
{
|
||||
ImportUrdfInternalData()
|
||||
:m_numMotors(0),
|
||||
m_mb(0)
|
||||
{
|
||||
for (int i=0;i<MAX_NUM_MOTORS;i++)
|
||||
ImportUrdfInternalData()
|
||||
: m_numMotors(0),
|
||||
m_mb(0)
|
||||
{
|
||||
for (int i = 0; i < MAX_NUM_MOTORS; i++)
|
||||
{
|
||||
m_jointMotors[i] = 0;
|
||||
m_generic6DofJointMotors[i] = 0;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
btScalar m_motorTargetVelocities[MAX_NUM_MOTORS];
|
||||
btMultiBodyJointMotor* m_jointMotors [MAX_NUM_MOTORS];
|
||||
btGeneric6DofSpring2Constraint* m_generic6DofJointMotors [MAX_NUM_MOTORS];
|
||||
int m_numMotors;
|
||||
btMultiBody* m_mb;
|
||||
btRigidBody* m_rb;
|
||||
}
|
||||
|
||||
btScalar m_motorTargetVelocities[MAX_NUM_MOTORS];
|
||||
btMultiBodyJointMotor* m_jointMotors[MAX_NUM_MOTORS];
|
||||
btGeneric6DofSpring2Constraint* m_generic6DofJointMotors[MAX_NUM_MOTORS];
|
||||
int m_numMotors;
|
||||
btMultiBody* m_mb;
|
||||
btRigidBody* m_rb;
|
||||
};
|
||||
|
||||
|
||||
ImportUrdfSetup::ImportUrdfSetup(struct GUIHelperInterface* helper, int option, const char* fileName)
|
||||
:CommonMultiBodyBase(helper),
|
||||
m_grav(-10),
|
||||
m_upAxis(2)
|
||||
: CommonMultiBodyBase(helper),
|
||||
m_grav(-10),
|
||||
m_upAxis(2)
|
||||
{
|
||||
m_data = new ImportUrdfInternalData;
|
||||
|
||||
if (option==1)
|
||||
if (option == 1)
|
||||
{
|
||||
m_useMultiBody = true;
|
||||
} else
|
||||
}
|
||||
else
|
||||
{
|
||||
m_useMultiBody = false;
|
||||
}
|
||||
@@ -106,15 +95,14 @@ ImportUrdfSetup::ImportUrdfSetup(struct GUIHelperInterface* helper, int option,
|
||||
if (fileName)
|
||||
{
|
||||
setFileName(fileName);
|
||||
} else
|
||||
}
|
||||
else
|
||||
{
|
||||
gFileNameArray.clear();
|
||||
|
||||
|
||||
|
||||
//load additional urdf file names from file
|
||||
|
||||
FILE* f = fopen("urdf_files.txt","r");
|
||||
FILE* f = fopen("urdf_files.txt", "r");
|
||||
if (f)
|
||||
{
|
||||
int result;
|
||||
@@ -122,68 +110,56 @@ ImportUrdfSetup::ImportUrdfSetup(struct GUIHelperInterface* helper, int option,
|
||||
char fileName[1024];
|
||||
do
|
||||
{
|
||||
result = fscanf(f,"%s",fileName);
|
||||
b3Printf("urdf_files.txt entry %s",fileName);
|
||||
if (result==1)
|
||||
result = fscanf(f, "%s", fileName);
|
||||
b3Printf("urdf_files.txt entry %s", fileName);
|
||||
if (result == 1)
|
||||
{
|
||||
gFileNameArray.push_back(fileName);
|
||||
}
|
||||
} while (result==1);
|
||||
} while (result == 1);
|
||||
|
||||
fclose(f);
|
||||
}
|
||||
|
||||
if (gFileNameArray.size()==0)
|
||||
|
||||
if (gFileNameArray.size() == 0)
|
||||
{
|
||||
gFileNameArray.push_back("r2d2.urdf");
|
||||
|
||||
}
|
||||
|
||||
int numFileNames = gFileNameArray.size();
|
||||
|
||||
if (count>=numFileNames)
|
||||
if (count >= numFileNames)
|
||||
{
|
||||
count=0;
|
||||
count = 0;
|
||||
}
|
||||
sprintf(m_fileName,"%s",gFileNameArray[count++].c_str());
|
||||
sprintf(m_fileName, "%s", gFileNameArray[count++].c_str());
|
||||
}
|
||||
}
|
||||
|
||||
ImportUrdfSetup::~ImportUrdfSetup()
|
||||
{
|
||||
for (int i=0;i<m_nameMemory.size();i++)
|
||||
for (int i = 0; i < m_nameMemory.size(); i++)
|
||||
{
|
||||
delete m_nameMemory[i];
|
||||
}
|
||||
m_nameMemory.clear();
|
||||
delete m_data;
|
||||
delete m_data;
|
||||
}
|
||||
|
||||
|
||||
|
||||
void ImportUrdfSetup::setFileName(const char* urdfFileName)
|
||||
{
|
||||
memcpy(m_fileName,urdfFileName,strlen(urdfFileName)+1);
|
||||
memcpy(m_fileName, urdfFileName, strlen(urdfFileName) + 1);
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
void ImportUrdfSetup::initPhysics()
|
||||
{
|
||||
|
||||
|
||||
m_guiHelper->setUpAxis(m_upAxis);
|
||||
|
||||
|
||||
this->createEmptyDynamicsWorld();
|
||||
//m_dynamicsWorld->getSolverInfo().m_numIterations = 100;
|
||||
m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
|
||||
m_dynamicsWorld->getDebugDrawer()->setDebugMode(
|
||||
btIDebugDraw::DBG_DrawConstraints
|
||||
+btIDebugDraw::DBG_DrawContactPoints
|
||||
+btIDebugDraw::DBG_DrawAabb
|
||||
);//+btIDebugDraw::DBG_DrawConstraintLimits);
|
||||
|
||||
m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
|
||||
m_dynamicsWorld->getDebugDrawer()->setDebugMode(
|
||||
btIDebugDraw::DBG_DrawConstraints + btIDebugDraw::DBG_DrawContactPoints + btIDebugDraw::DBG_DrawAabb); //+btIDebugDraw::DBG_DrawConstraintLimits);
|
||||
|
||||
if (m_guiHelper->getParameterInterface())
|
||||
{
|
||||
@@ -192,19 +168,17 @@ void ImportUrdfSetup::initPhysics()
|
||||
slider.m_maxVal = 10;
|
||||
m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
|
||||
}
|
||||
|
||||
|
||||
int flags=0;
|
||||
double globalScaling=1;
|
||||
BulletURDFImporter u2b(m_guiHelper, 0,globalScaling,flags);
|
||||
|
||||
|
||||
int flags = 0;
|
||||
double globalScaling = 1;
|
||||
BulletURDFImporter u2b(m_guiHelper, 0, globalScaling, flags);
|
||||
|
||||
bool loadOk = u2b.loadURDF(m_fileName);
|
||||
|
||||
#ifdef TEST_MULTIBODY_SERIALIZATION
|
||||
#ifdef TEST_MULTIBODY_SERIALIZATION
|
||||
//test to serialize a multibody to disk or shared memory, with base, link and joint names
|
||||
btSerializer* s = new btDefaultSerializer;
|
||||
#endif //TEST_MULTIBODY_SERIALIZATION
|
||||
#endif //TEST_MULTIBODY_SERIALIZATION
|
||||
|
||||
if (loadOk)
|
||||
{
|
||||
@@ -215,18 +189,13 @@ void ImportUrdfSetup::initPhysics()
|
||||
btTransform identityTrans;
|
||||
identityTrans.setIdentity();
|
||||
|
||||
|
||||
{
|
||||
|
||||
|
||||
|
||||
|
||||
//todo: move these internal API called inside the 'ConvertURDF2Bullet' call, hidden from the user
|
||||
//int rootLinkIndex = u2b.getRootLinkIndex();
|
||||
//b3Printf("urdf root link index = %d\n",rootLinkIndex);
|
||||
MyMultiBodyCreator creation(m_guiHelper);
|
||||
|
||||
ConvertURDF2Bullet(u2b,creation, identityTrans,m_dynamicsWorld,m_useMultiBody,u2b.getPathPrefix());
|
||||
ConvertURDF2Bullet(u2b, creation, identityTrans, m_dynamicsWorld, m_useMultiBody, u2b.getPathPrefix());
|
||||
m_data->m_rb = creation.getRigidBody();
|
||||
m_data->m_mb = creation.getBulletMultiBody();
|
||||
btMultiBody* mb = m_data->m_mb;
|
||||
@@ -235,146 +204,137 @@ void ImportUrdfSetup::initPhysics()
|
||||
m_collisionShapes.push_back(u2b.getAllocatedCollisionShape(i));
|
||||
}
|
||||
|
||||
if (m_useMultiBody && mb )
|
||||
if (m_useMultiBody && mb)
|
||||
{
|
||||
std::string* name = new std::string(u2b.getLinkName(u2b.getRootLinkIndex()));
|
||||
std::string* name = new std::string(u2b.getLinkName(u2b.getRootLinkIndex()));
|
||||
m_nameMemory.push_back(name);
|
||||
#ifdef TEST_MULTIBODY_SERIALIZATION
|
||||
s->registerNameForPointer(name->c_str(),name->c_str());
|
||||
#endif//TEST_MULTIBODY_SERIALIZATION
|
||||
s->registerNameForPointer(name->c_str(), name->c_str());
|
||||
#endif //TEST_MULTIBODY_SERIALIZATION
|
||||
mb->setBaseName(name->c_str());
|
||||
//create motors for each btMultiBody joint
|
||||
int numLinks = mb->getNumLinks();
|
||||
for (int i=0;i<numLinks;i++)
|
||||
for (int i = 0; i < numLinks; i++)
|
||||
{
|
||||
int mbLinkIndex = i;
|
||||
int urdfLinkIndex = creation.m_mb2urdfLink[mbLinkIndex];
|
||||
|
||||
std::string* jointName = new std::string(u2b.getJointName(urdfLinkIndex));
|
||||
std::string* linkName = new std::string(u2b.getLinkName(urdfLinkIndex).c_str());
|
||||
#ifdef TEST_MULTIBODY_SERIALIZATION
|
||||
s->registerNameForPointer(jointName->c_str(),jointName->c_str());
|
||||
s->registerNameForPointer(linkName->c_str(),linkName->c_str());
|
||||
#endif//TEST_MULTIBODY_SERIALIZATION
|
||||
#ifdef TEST_MULTIBODY_SERIALIZATION
|
||||
s->registerNameForPointer(jointName->c_str(), jointName->c_str());
|
||||
s->registerNameForPointer(linkName->c_str(), linkName->c_str());
|
||||
#endif //TEST_MULTIBODY_SERIALIZATION
|
||||
m_nameMemory.push_back(jointName);
|
||||
m_nameMemory.push_back(linkName);
|
||||
|
||||
mb->getLink(i).m_linkName = linkName->c_str();
|
||||
mb->getLink(i).m_jointName = jointName->c_str();
|
||||
|
||||
if (mb->getLink(mbLinkIndex).m_jointType==btMultibodyLink::eRevolute
|
||||
||mb->getLink(mbLinkIndex).m_jointType==btMultibodyLink::ePrismatic
|
||||
)
|
||||
|
||||
if (mb->getLink(mbLinkIndex).m_jointType == btMultibodyLink::eRevolute || mb->getLink(mbLinkIndex).m_jointType == btMultibodyLink::ePrismatic)
|
||||
{
|
||||
if (m_data->m_numMotors<MAX_NUM_MOTORS)
|
||||
if (m_data->m_numMotors < MAX_NUM_MOTORS)
|
||||
{
|
||||
|
||||
char motorName[1024];
|
||||
sprintf(motorName,"%s q'", jointName->c_str());
|
||||
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;
|
||||
SliderParams slider(motorName, motorVel);
|
||||
slider.m_minVal = -4;
|
||||
slider.m_maxVal = 4;
|
||||
m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
|
||||
float maxMotorImpulse = 10.1f;
|
||||
btMultiBodyJointMotor* motor = new btMultiBodyJointMotor(mb,mbLinkIndex,0,0,maxMotorImpulse);
|
||||
btMultiBodyJointMotor* motor = new btMultiBodyJointMotor(mb, mbLinkIndex, 0, 0, maxMotorImpulse);
|
||||
//motor->setMaxAppliedImpulse(0);
|
||||
m_data->m_jointMotors[m_data->m_numMotors]=motor;
|
||||
m_data->m_jointMotors[m_data->m_numMotors] = motor;
|
||||
m_dynamicsWorld->addMultiBodyConstraint(motor);
|
||||
m_data->m_numMotors++;
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
} else
|
||||
}
|
||||
else
|
||||
{
|
||||
if (1)
|
||||
{
|
||||
//create motors for each generic joint
|
||||
int num6Dof = creation.getNum6DofConstraints();
|
||||
for (int i=0;i<num6Dof;i++)
|
||||
for (int i = 0; i < num6Dof; i++)
|
||||
{
|
||||
btGeneric6DofSpring2Constraint* c = creation.get6DofConstraint(i);
|
||||
if (c->getUserConstraintPtr())
|
||||
{
|
||||
GenericConstraintUserInfo* jointInfo = (GenericConstraintUserInfo*)c->getUserConstraintPtr();
|
||||
if ((jointInfo->m_urdfJointType ==URDFRevoluteJoint) ||
|
||||
(jointInfo->m_urdfJointType ==URDFPrismaticJoint) ||
|
||||
(jointInfo->m_urdfJointType ==URDFContinuousJoint))
|
||||
if ((jointInfo->m_urdfJointType == URDFRevoluteJoint) ||
|
||||
(jointInfo->m_urdfJointType == URDFPrismaticJoint) ||
|
||||
(jointInfo->m_urdfJointType == URDFContinuousJoint))
|
||||
{
|
||||
int urdfLinkIndex = jointInfo->m_urdfIndex;
|
||||
std::string jointName = u2b.getJointName(urdfLinkIndex);
|
||||
char motorName[1024];
|
||||
sprintf(motorName,"%s q'", jointName.c_str());
|
||||
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;
|
||||
SliderParams slider(motorName, motorVel);
|
||||
slider.m_minVal = -4;
|
||||
slider.m_maxVal = 4;
|
||||
m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
|
||||
m_data->m_generic6DofJointMotors[m_data->m_numMotors]=c;
|
||||
m_data->m_generic6DofJointMotors[m_data->m_numMotors] = c;
|
||||
bool motorOn = true;
|
||||
c->enableMotor(jointInfo->m_jointAxisIndex,motorOn);
|
||||
c->setMaxMotorForce(jointInfo->m_jointAxisIndex,10000);
|
||||
c->setTargetVelocity(jointInfo->m_jointAxisIndex,0);
|
||||
|
||||
c->enableMotor(jointInfo->m_jointAxisIndex, motorOn);
|
||||
c->setMaxMotorForce(jointInfo->m_jointAxisIndex, 10000);
|
||||
c->setTargetVelocity(jointInfo->m_jointAxisIndex, 0);
|
||||
|
||||
m_data->m_numMotors++;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
//the btMultiBody support is work-in-progress :-)
|
||||
|
||||
for (int i=0;i<m_dynamicsWorld->getNumMultiBodyConstraints();i++)
|
||||
{
|
||||
m_dynamicsWorld->getMultiBodyConstraint(i)->finalizeMultiDof();
|
||||
}
|
||||
for (int i = 0; i < m_dynamicsWorld->getNumMultiBodyConstraints(); i++)
|
||||
{
|
||||
m_dynamicsWorld->getMultiBodyConstraint(i)->finalizeMultiDof();
|
||||
}
|
||||
|
||||
|
||||
|
||||
bool createGround=true;
|
||||
bool createGround = true;
|
||||
if (createGround)
|
||||
{
|
||||
btVector3 groundHalfExtents(20,20,20);
|
||||
groundHalfExtents[m_upAxis]=1.f;
|
||||
btVector3 groundHalfExtents(20, 20, 20);
|
||||
groundHalfExtents[m_upAxis] = 1.f;
|
||||
btBoxShape* box = new btBoxShape(groundHalfExtents);
|
||||
m_collisionShapes.push_back(box);
|
||||
box->initializePolyhedralFeatures();
|
||||
|
||||
m_guiHelper->createCollisionShapeGraphicsObject(box);
|
||||
btTransform start; start.setIdentity();
|
||||
btVector3 groundOrigin(0,0,0);
|
||||
groundOrigin[m_upAxis]=-2.5;
|
||||
btTransform start;
|
||||
start.setIdentity();
|
||||
btVector3 groundOrigin(0, 0, 0);
|
||||
groundOrigin[m_upAxis] = -2.5;
|
||||
start.setOrigin(groundOrigin);
|
||||
btRigidBody* body = createRigidBody(0,start,box);
|
||||
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_dynamicsWorld->addRigidBody(body,2,1);
|
||||
btVector3 color(0.5, 0.5, 0.5);
|
||||
m_guiHelper->createRigidBodyGraphicsObject(body, color);
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
|
||||
|
||||
#ifdef TEST_MULTIBODY_SERIALIZATION
|
||||
m_dynamicsWorld->serialize(s);
|
||||
b3ResourcePath p;
|
||||
char resourcePath[1024];
|
||||
if (p.findResourcePath("r2d2_multibody.bullet",resourcePath,1024))
|
||||
if (p.findResourcePath("r2d2_multibody.bullet", resourcePath, 1024))
|
||||
{
|
||||
FILE* f = fopen(resourcePath,"wb");
|
||||
fwrite(s->getBufferPointer(),s->getCurrentBufferSize(),1,f);
|
||||
FILE* f = fopen(resourcePath, "wb");
|
||||
fwrite(s->getBufferPointer(), s->getCurrentBufferSize(), 1, f);
|
||||
fclose(f);
|
||||
}
|
||||
#endif//TEST_MULTIBODY_SERIALIZATION
|
||||
|
||||
#endif //TEST_MULTIBODY_SERIALIZATION
|
||||
}
|
||||
|
||||
void ImportUrdfSetup::stepSimulation(float deltaTime)
|
||||
@@ -385,8 +345,8 @@ void ImportUrdfSetup::stepSimulation(float deltaTime)
|
||||
gravity[m_upAxis] = m_grav;
|
||||
m_dynamicsWorld->setGravity(gravity);
|
||||
|
||||
for (int i=0;i<m_data->m_numMotors;i++)
|
||||
{
|
||||
for (int i = 0; i < m_data->m_numMotors; i++)
|
||||
{
|
||||
if (m_data->m_jointMotors[i])
|
||||
{
|
||||
m_data->m_jointMotors[i]->setVelocityTarget(m_data->m_motorTargetVelocities[i]);
|
||||
@@ -394,18 +354,17 @@ void ImportUrdfSetup::stepSimulation(float deltaTime)
|
||||
if (m_data->m_generic6DofJointMotors[i])
|
||||
{
|
||||
GenericConstraintUserInfo* jointInfo = (GenericConstraintUserInfo*)m_data->m_generic6DofJointMotors[i]->getUserConstraintPtr();
|
||||
m_data->m_generic6DofJointMotors[i]->setTargetVelocity(jointInfo->m_jointAxisIndex,m_data->m_motorTargetVelocities[i]);
|
||||
m_data->m_generic6DofJointMotors[i]->setTargetVelocity(jointInfo->m_jointAxisIndex, m_data->m_motorTargetVelocities[i]);
|
||||
//jointInfo->
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
//the maximal coordinates/iterative MLCP solver requires a smallish timestep to converge
|
||||
m_dynamicsWorld->stepSimulation(deltaTime,10,1./240.);
|
||||
m_dynamicsWorld->stepSimulation(deltaTime, 10, 1. / 240.);
|
||||
}
|
||||
}
|
||||
|
||||
class CommonExampleInterface* ImportURDFCreateFunc(struct CommonExampleOptions& options)
|
||||
class CommonExampleInterface* ImportURDFCreateFunc(struct CommonExampleOptions& options)
|
||||
{
|
||||
|
||||
return new ImportUrdfSetup(options.m_guiHelper, options.m_option,options.m_fileName);
|
||||
return new ImportUrdfSetup(options.m_guiHelper, options.m_option, options.m_fileName);
|
||||
}
|
||||
|
||||
@@ -1,8 +1,6 @@
|
||||
#ifndef IMPORT_URDF_SETUP_H
|
||||
#define IMPORT_URDF_SETUP_H
|
||||
|
||||
class CommonExampleInterface* ImportURDFCreateFunc(struct CommonExampleOptions& options);
|
||||
|
||||
class CommonExampleInterface* ImportURDFCreateFunc(struct CommonExampleOptions& options);
|
||||
|
||||
|
||||
#endif //IMPORT_URDF_SETUP_H
|
||||
#endif //IMPORT_URDF_SETUP_H
|
||||
|
||||
@@ -6,40 +6,37 @@
|
||||
class MultiBodyCreationInterface
|
||||
{
|
||||
public:
|
||||
|
||||
virtual ~MultiBodyCreationInterface() {}
|
||||
|
||||
|
||||
virtual void createRigidBodyGraphicsInstance(int linkIndex, class btRigidBody* body, const btVector3& colorRgba, int graphicsIndex) = 0;
|
||||
virtual void createRigidBodyGraphicsInstance(int linkIndex, class btRigidBody* body, const btVector3& colorRgba, int graphicsIndex) = 0;
|
||||
virtual void createRigidBodyGraphicsInstance2(int linkIndex, class btRigidBody* body, const btVector3& colorRgba, const btVector3& specularColor, int graphicsIndex)
|
||||
{
|
||||
createRigidBodyGraphicsInstance(linkIndex,body,colorRgba,graphicsIndex);
|
||||
createRigidBodyGraphicsInstance(linkIndex, body, colorRgba, graphicsIndex);
|
||||
}
|
||||
|
||||
///optionally create some graphical representation from a collision object, usually for visual debugging purposes.
|
||||
virtual void createCollisionObjectGraphicsInstance(int linkIndex, class btCollisionObject* col, const btVector3& colorRgba) = 0;
|
||||
virtual void createCollisionObjectGraphicsInstance2(int linkIndex, class btCollisionObject* col, const btVector4& colorRgba, const btVector3& specularColor)
|
||||
///optionally create some graphical representation from a collision object, usually for visual debugging purposes.
|
||||
virtual void createCollisionObjectGraphicsInstance(int linkIndex, class btCollisionObject* col, const btVector3& colorRgba) = 0;
|
||||
virtual void createCollisionObjectGraphicsInstance2(int linkIndex, class btCollisionObject* col, const btVector4& colorRgba, const btVector3& specularColor)
|
||||
{
|
||||
createCollisionObjectGraphicsInstance(linkIndex,col,colorRgba);
|
||||
createCollisionObjectGraphicsInstance(linkIndex, col, colorRgba);
|
||||
}
|
||||
|
||||
virtual class btMultiBody* allocateMultiBody(int urdfLinkIndex, int totalNumJoints,btScalar mass, const btVector3& localInertiaDiagonal, bool isFixedBase, bool canSleep) =0;
|
||||
|
||||
virtual class btRigidBody* allocateRigidBody(int urdfLinkIndex, btScalar mass, const btVector3& localInertiaDiagonal, const btTransform& initialWorldTrans, class btCollisionShape* colShape) = 0;
|
||||
|
||||
virtual class btGeneric6DofSpring2Constraint* allocateGeneric6DofSpring2Constraint(int urdfLinkIndex, btRigidBody& rbA /*parent*/, btRigidBody& rbB, const btTransform& offsetInA, const btTransform& offsetInB, int rotateOrder=0) = 0;
|
||||
|
||||
|
||||
virtual class btMultiBody* allocateMultiBody(int urdfLinkIndex, int totalNumJoints, btScalar mass, const btVector3& localInertiaDiagonal, bool isFixedBase, bool canSleep) = 0;
|
||||
|
||||
virtual class btRigidBody* allocateRigidBody(int urdfLinkIndex, btScalar mass, const btVector3& localInertiaDiagonal, const btTransform& initialWorldTrans, class btCollisionShape* colShape) = 0;
|
||||
|
||||
virtual class btGeneric6DofSpring2Constraint* allocateGeneric6DofSpring2Constraint(int urdfLinkIndex, btRigidBody& rbA /*parent*/, btRigidBody& rbB, const btTransform& offsetInA, const btTransform& offsetInB, int rotateOrder = 0) = 0;
|
||||
|
||||
virtual class btGeneric6DofSpring2Constraint* createPrismaticJoint(int urdfLinkIndex, btRigidBody& rbA /*parent*/, btRigidBody& rbB, const btTransform& offsetInA, const btTransform& offsetInB,
|
||||
const btVector3& jointAxisInJointSpace,btScalar jointLowerLimit,btScalar jointUpperLimit) = 0;
|
||||
virtual class btGeneric6DofSpring2Constraint* createRevoluteJoint(int urdfLinkIndex, btRigidBody& rbA /*parent*/, btRigidBody& rbB, const btTransform& offsetInA, const btTransform& offsetInB,
|
||||
const btVector3& jointAxisInJointSpace,btScalar jointLowerLimit,btScalar jointUpperLimit) = 0;
|
||||
const btVector3& jointAxisInJointSpace, btScalar jointLowerLimit, btScalar jointUpperLimit) = 0;
|
||||
virtual class btGeneric6DofSpring2Constraint* createRevoluteJoint(int urdfLinkIndex, btRigidBody& rbA /*parent*/, btRigidBody& rbB, const btTransform& offsetInA, const btTransform& offsetInB,
|
||||
const btVector3& jointAxisInJointSpace, btScalar jointLowerLimit, btScalar jointUpperLimit) = 0;
|
||||
|
||||
virtual class btGeneric6DofSpring2Constraint* createFixedJoint(int urdfLinkIndex, btRigidBody& rbA /*parent*/, btRigidBody& rbB, const btTransform& offsetInA, const btTransform& offsetInB) = 0;
|
||||
|
||||
virtual class btMultiBodyLinkCollider* allocateMultiBodyLinkCollider(int urdfLinkIndex, int mbLinkIndex, btMultiBody* body) = 0;
|
||||
|
||||
virtual void addLinkMapping(int urdfLinkIndex, int mbLinkIndex) = 0;
|
||||
virtual class btGeneric6DofSpring2Constraint* createFixedJoint(int urdfLinkIndex, btRigidBody& rbA /*parent*/, btRigidBody& rbB, const btTransform& offsetInA, const btTransform& offsetInB) = 0;
|
||||
|
||||
virtual class btMultiBodyLinkCollider* allocateMultiBodyLinkCollider(int urdfLinkIndex, int mbLinkIndex, btMultiBody* body) = 0;
|
||||
|
||||
virtual void addLinkMapping(int urdfLinkIndex, int mbLinkIndex) = 0;
|
||||
};
|
||||
|
||||
#endif //MULTIBODY_CREATION_INTERFACE_H
|
||||
#endif //MULTIBODY_CREATION_INTERFACE_H
|
||||
@@ -12,56 +12,51 @@
|
||||
#include "URDFJointTypes.h"
|
||||
|
||||
MyMultiBodyCreator::MyMultiBodyCreator(GUIHelperInterface* guiHelper)
|
||||
:m_bulletMultiBody(0),
|
||||
m_rigidBody(0),
|
||||
m_guiHelper(guiHelper)
|
||||
: m_bulletMultiBody(0),
|
||||
m_rigidBody(0),
|
||||
m_guiHelper(guiHelper)
|
||||
{
|
||||
}
|
||||
|
||||
|
||||
class btMultiBody* MyMultiBodyCreator::allocateMultiBody(int /* urdfLinkIndex */, int totalNumJoints,btScalar mass, const btVector3& localInertiaDiagonal, bool isFixedBase, bool canSleep)
|
||||
class btMultiBody* MyMultiBodyCreator::allocateMultiBody(int /* urdfLinkIndex */, int totalNumJoints, btScalar mass, const btVector3& localInertiaDiagonal, bool isFixedBase, bool canSleep)
|
||||
{
|
||||
// m_urdf2mbLink.resize(totalNumJoints+1,-2);
|
||||
m_mb2urdfLink.resize(totalNumJoints+1,-2);
|
||||
// m_urdf2mbLink.resize(totalNumJoints+1,-2);
|
||||
m_mb2urdfLink.resize(totalNumJoints + 1, -2);
|
||||
|
||||
m_bulletMultiBody = new btMultiBody(totalNumJoints,mass,localInertiaDiagonal,isFixedBase,canSleep);
|
||||
m_bulletMultiBody = new btMultiBody(totalNumJoints, mass, localInertiaDiagonal, isFixedBase, canSleep);
|
||||
//if (canSleep)
|
||||
// m_bulletMultiBody->goToSleep();
|
||||
return m_bulletMultiBody;
|
||||
return m_bulletMultiBody;
|
||||
}
|
||||
|
||||
class btRigidBody* MyMultiBodyCreator::allocateRigidBody(int urdfLinkIndex, btScalar mass, const btVector3& localInertiaDiagonal, const btTransform& initialWorldTrans, class btCollisionShape* colShape)
|
||||
class btRigidBody* MyMultiBodyCreator::allocateRigidBody(int urdfLinkIndex, btScalar mass, const btVector3& localInertiaDiagonal, const btTransform& initialWorldTrans, class btCollisionShape* colShape)
|
||||
{
|
||||
btRigidBody::btRigidBodyConstructionInfo rbci(mass, 0, colShape, localInertiaDiagonal);
|
||||
rbci.m_startWorldTransform = initialWorldTrans;
|
||||
m_rigidBody = new btRigidBody(rbci);
|
||||
|
||||
|
||||
return m_rigidBody;
|
||||
}
|
||||
|
||||
class btMultiBodyLinkCollider* MyMultiBodyCreator::allocateMultiBodyLinkCollider(int /*urdfLinkIndex*/, int mbLinkIndex, btMultiBody* multiBody)
|
||||
{
|
||||
btMultiBodyLinkCollider* mbCol= new btMultiBodyLinkCollider(multiBody, mbLinkIndex);
|
||||
return mbCol;
|
||||
btRigidBody::btRigidBodyConstructionInfo rbci(mass, 0, colShape, localInertiaDiagonal);
|
||||
rbci.m_startWorldTransform = initialWorldTrans;
|
||||
m_rigidBody = new btRigidBody(rbci);
|
||||
|
||||
return m_rigidBody;
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
class btGeneric6DofSpring2Constraint* MyMultiBodyCreator::allocateGeneric6DofSpring2Constraint(int urdfLinkIndex, btRigidBody& rbA /*parent*/, btRigidBody& rbB, const btTransform& offsetInA, const btTransform& offsetInB, int rotateOrder)
|
||||
class btMultiBodyLinkCollider* MyMultiBodyCreator::allocateMultiBodyLinkCollider(int /*urdfLinkIndex*/, int mbLinkIndex, btMultiBody* multiBody)
|
||||
{
|
||||
btGeneric6DofSpring2Constraint* c = new btGeneric6DofSpring2Constraint(rbA,rbB,offsetInA, offsetInB, (RotateOrder)rotateOrder);
|
||||
|
||||
return c;
|
||||
btMultiBodyLinkCollider* mbCol = new btMultiBodyLinkCollider(multiBody, mbLinkIndex);
|
||||
return mbCol;
|
||||
}
|
||||
|
||||
class btGeneric6DofSpring2Constraint* MyMultiBodyCreator::allocateGeneric6DofSpring2Constraint(int urdfLinkIndex, btRigidBody& rbA /*parent*/, btRigidBody& rbB, const btTransform& offsetInA, const btTransform& offsetInB, int rotateOrder)
|
||||
{
|
||||
btGeneric6DofSpring2Constraint* c = new btGeneric6DofSpring2Constraint(rbA, rbB, offsetInA, offsetInB, (RotateOrder)rotateOrder);
|
||||
|
||||
return c;
|
||||
}
|
||||
|
||||
class btGeneric6DofSpring2Constraint* MyMultiBodyCreator::createPrismaticJoint(int urdfLinkIndex, btRigidBody& rbA /*parent*/, btRigidBody& rbB, const btTransform& offsetInA, const btTransform& offsetInB,
|
||||
const btVector3& jointAxisInJointSpace,btScalar jointLowerLimit,btScalar jointUpperLimit)
|
||||
const btVector3& jointAxisInJointSpace, btScalar jointLowerLimit, btScalar jointUpperLimit)
|
||||
{
|
||||
int rotateOrder=0;
|
||||
btGeneric6DofSpring2Constraint* dof6 = allocateGeneric6DofSpring2Constraint(urdfLinkIndex, rbA , rbB, offsetInA, offsetInB, rotateOrder);
|
||||
//todo(erwincoumans) for now, we only support principle axis along X, Y or Z
|
||||
int rotateOrder = 0;
|
||||
btGeneric6DofSpring2Constraint* dof6 = allocateGeneric6DofSpring2Constraint(urdfLinkIndex, rbA, rbB, offsetInA, offsetInB, rotateOrder);
|
||||
//todo(erwincoumans) for now, we only support principle axis along X, Y or Z
|
||||
int principleAxis = jointAxisInJointSpace.closestAxis();
|
||||
|
||||
GenericConstraintUserInfo* userInfo = new GenericConstraintUserInfo;
|
||||
@@ -74,89 +69,86 @@ class btGeneric6DofSpring2Constraint* MyMultiBodyCreator::createPrismaticJoint(i
|
||||
userInfo->m_urdfIndex = urdfLinkIndex;
|
||||
dof6->setUserConstraintPtr(userInfo);
|
||||
|
||||
|
||||
switch (principleAxis)
|
||||
{
|
||||
case 0:
|
||||
{
|
||||
dof6->setLinearLowerLimit(btVector3(jointLowerLimit,0,0));
|
||||
dof6->setLinearUpperLimit(btVector3(jointUpperLimit,0,0));
|
||||
dof6->setLinearLowerLimit(btVector3(jointLowerLimit, 0, 0));
|
||||
dof6->setLinearUpperLimit(btVector3(jointUpperLimit, 0, 0));
|
||||
break;
|
||||
}
|
||||
case 1:
|
||||
{
|
||||
dof6->setLinearLowerLimit(btVector3(0,jointLowerLimit,0));
|
||||
dof6->setLinearUpperLimit(btVector3(0,jointUpperLimit,0));
|
||||
dof6->setLinearLowerLimit(btVector3(0, jointLowerLimit, 0));
|
||||
dof6->setLinearUpperLimit(btVector3(0, jointUpperLimit, 0));
|
||||
break;
|
||||
}
|
||||
case 2:
|
||||
default:
|
||||
{
|
||||
dof6->setLinearLowerLimit(btVector3(0,0,jointLowerLimit));
|
||||
dof6->setLinearUpperLimit(btVector3(0,0,jointUpperLimit));
|
||||
dof6->setLinearLowerLimit(btVector3(0, 0, jointLowerLimit));
|
||||
dof6->setLinearUpperLimit(btVector3(0, 0, jointUpperLimit));
|
||||
}
|
||||
};
|
||||
|
||||
dof6->setAngularLowerLimit(btVector3(0,0,0));
|
||||
dof6->setAngularUpperLimit(btVector3(0,0,0));
|
||||
dof6->setAngularLowerLimit(btVector3(0, 0, 0));
|
||||
dof6->setAngularUpperLimit(btVector3(0, 0, 0));
|
||||
m_6DofConstraints.push_back(dof6);
|
||||
return dof6;
|
||||
}
|
||||
|
||||
class btGeneric6DofSpring2Constraint* MyMultiBodyCreator::createRevoluteJoint(int urdfLinkIndex, btRigidBody& rbA /*parent*/, btRigidBody& rbB, const btTransform& offsetInA, const btTransform& offsetInB,
|
||||
const btVector3& jointAxisInJointSpace,btScalar jointLowerLimit,btScalar jointUpperLimit)
|
||||
const btVector3& jointAxisInJointSpace, btScalar jointLowerLimit, btScalar jointUpperLimit)
|
||||
{
|
||||
btGeneric6DofSpring2Constraint* dof6 = 0;
|
||||
|
||||
//only handle principle axis at the moment,
|
||||
//@todo(erwincoumans) orient the constraint for non-principal axis
|
||||
int principleAxis = jointAxisInJointSpace.closestAxis();
|
||||
switch (principleAxis)
|
||||
{
|
||||
case 0:
|
||||
{
|
||||
dof6 = allocateGeneric6DofSpring2Constraint(urdfLinkIndex,rbA, rbB, offsetInA, offsetInB,RO_ZYX);
|
||||
dof6->setLinearLowerLimit(btVector3(0,0,0));
|
||||
dof6->setLinearUpperLimit(btVector3(0,0,0));
|
||||
//only handle principle axis at the moment,
|
||||
//@todo(erwincoumans) orient the constraint for non-principal axis
|
||||
int principleAxis = jointAxisInJointSpace.closestAxis();
|
||||
switch (principleAxis)
|
||||
{
|
||||
case 0:
|
||||
{
|
||||
dof6 = allocateGeneric6DofSpring2Constraint(urdfLinkIndex, rbA, rbB, offsetInA, offsetInB, RO_ZYX);
|
||||
dof6->setLinearLowerLimit(btVector3(0, 0, 0));
|
||||
dof6->setLinearUpperLimit(btVector3(0, 0, 0));
|
||||
|
||||
dof6->setAngularLowerLimit(btVector3(jointLowerLimit,0,0));
|
||||
dof6->setAngularUpperLimit(btVector3(jointUpperLimit,0,0));
|
||||
dof6->setAngularLowerLimit(btVector3(jointLowerLimit, 0, 0));
|
||||
dof6->setAngularUpperLimit(btVector3(jointUpperLimit, 0, 0));
|
||||
|
||||
break;
|
||||
}
|
||||
case 1:
|
||||
{
|
||||
dof6 = allocateGeneric6DofSpring2Constraint(urdfLinkIndex,rbA, rbB, offsetInA, offsetInB,RO_XZY);
|
||||
dof6->setLinearLowerLimit(btVector3(0,0,0));
|
||||
dof6->setLinearUpperLimit(btVector3(0,0,0));
|
||||
break;
|
||||
}
|
||||
case 1:
|
||||
{
|
||||
dof6 = allocateGeneric6DofSpring2Constraint(urdfLinkIndex, rbA, rbB, offsetInA, offsetInB, RO_XZY);
|
||||
dof6->setLinearLowerLimit(btVector3(0, 0, 0));
|
||||
dof6->setLinearUpperLimit(btVector3(0, 0, 0));
|
||||
|
||||
|
||||
dof6->setAngularLowerLimit(btVector3(0,jointLowerLimit,0));
|
||||
dof6->setAngularUpperLimit(btVector3(0,jointUpperLimit,0));
|
||||
break;
|
||||
}
|
||||
case 2:
|
||||
default:
|
||||
{
|
||||
dof6 = allocateGeneric6DofSpring2Constraint(urdfLinkIndex,rbA, rbB, offsetInA, offsetInB,RO_XYZ);
|
||||
dof6->setLinearLowerLimit(btVector3(0,0,0));
|
||||
dof6->setLinearUpperLimit(btVector3(0,0,0));
|
||||
dof6->setAngularLowerLimit(btVector3(0, jointLowerLimit, 0));
|
||||
dof6->setAngularUpperLimit(btVector3(0, jointUpperLimit, 0));
|
||||
break;
|
||||
}
|
||||
case 2:
|
||||
default:
|
||||
{
|
||||
dof6 = allocateGeneric6DofSpring2Constraint(urdfLinkIndex, rbA, rbB, offsetInA, offsetInB, RO_XYZ);
|
||||
dof6->setLinearLowerLimit(btVector3(0, 0, 0));
|
||||
dof6->setLinearUpperLimit(btVector3(0, 0, 0));
|
||||
|
||||
|
||||
dof6->setAngularLowerLimit(btVector3(0,0,jointLowerLimit));
|
||||
dof6->setAngularUpperLimit(btVector3(0,0,jointUpperLimit));
|
||||
|
||||
}
|
||||
};
|
||||
dof6->setAngularLowerLimit(btVector3(0, 0, jointLowerLimit));
|
||||
dof6->setAngularUpperLimit(btVector3(0, 0, jointUpperLimit));
|
||||
}
|
||||
};
|
||||
|
||||
GenericConstraintUserInfo* userInfo = new GenericConstraintUserInfo;
|
||||
userInfo->m_jointAxisInJointSpace = jointAxisInJointSpace;
|
||||
userInfo->m_jointAxisIndex = 3+principleAxis;
|
||||
userInfo->m_jointAxisIndex = 3 + principleAxis;
|
||||
|
||||
if (jointLowerLimit > jointUpperLimit)
|
||||
{
|
||||
userInfo->m_urdfJointType = URDFContinuousJoint;
|
||||
} else
|
||||
}
|
||||
else
|
||||
{
|
||||
userInfo->m_urdfJointType = URDFRevoluteJoint;
|
||||
userInfo->m_lowerJointLimit = jointLowerLimit;
|
||||
@@ -168,73 +160,64 @@ class btGeneric6DofSpring2Constraint* MyMultiBodyCreator::createRevoluteJoint(in
|
||||
return dof6;
|
||||
}
|
||||
|
||||
|
||||
class btGeneric6DofSpring2Constraint* MyMultiBodyCreator::createFixedJoint(int urdfLinkIndex, btRigidBody& rbA /*parent*/, btRigidBody& rbB, const btTransform& offsetInA, const btTransform& offsetInB)
|
||||
{
|
||||
btGeneric6DofSpring2Constraint* dof6 = allocateGeneric6DofSpring2Constraint(urdfLinkIndex, rbA, rbB, offsetInA, offsetInB);
|
||||
|
||||
|
||||
GenericConstraintUserInfo* userInfo = new GenericConstraintUserInfo;
|
||||
userInfo->m_urdfIndex = urdfLinkIndex;
|
||||
userInfo->m_urdfJointType = URDFFixedJoint;
|
||||
|
||||
dof6->setUserConstraintPtr(userInfo);
|
||||
|
||||
dof6->setLinearLowerLimit(btVector3(0,0,0));
|
||||
dof6->setLinearUpperLimit(btVector3(0,0,0));
|
||||
dof6->setLinearLowerLimit(btVector3(0, 0, 0));
|
||||
dof6->setLinearUpperLimit(btVector3(0, 0, 0));
|
||||
|
||||
dof6->setAngularLowerLimit(btVector3(0,0,0));
|
||||
dof6->setAngularUpperLimit(btVector3(0,0,0));
|
||||
dof6->setAngularLowerLimit(btVector3(0, 0, 0));
|
||||
dof6->setAngularUpperLimit(btVector3(0, 0, 0));
|
||||
m_6DofConstraints.push_back(dof6);
|
||||
return dof6;
|
||||
}
|
||||
|
||||
|
||||
|
||||
void MyMultiBodyCreator::addLinkMapping(int urdfLinkIndex, int mbLinkIndex)
|
||||
void MyMultiBodyCreator::addLinkMapping(int urdfLinkIndex, int mbLinkIndex)
|
||||
{
|
||||
if (m_mb2urdfLink.size()<(mbLinkIndex+1))
|
||||
if (m_mb2urdfLink.size() < (mbLinkIndex + 1))
|
||||
{
|
||||
m_mb2urdfLink.resize((mbLinkIndex+1),-2);
|
||||
m_mb2urdfLink.resize((mbLinkIndex + 1), -2);
|
||||
}
|
||||
// m_urdf2mbLink[urdfLinkIndex] = mbLinkIndex;
|
||||
m_mb2urdfLink[mbLinkIndex] = urdfLinkIndex;
|
||||
// m_urdf2mbLink[urdfLinkIndex] = mbLinkIndex;
|
||||
m_mb2urdfLink[mbLinkIndex] = urdfLinkIndex;
|
||||
}
|
||||
|
||||
void MyMultiBodyCreator::createRigidBodyGraphicsInstance(int linkIndex, btRigidBody* body, const btVector3& colorRgba, int graphicsIndex)
|
||||
void MyMultiBodyCreator::createRigidBodyGraphicsInstance(int linkIndex, btRigidBody* body, const btVector3& colorRgba, int graphicsIndex)
|
||||
{
|
||||
m_guiHelper->createRigidBodyGraphicsObject(body, colorRgba);
|
||||
m_guiHelper->createRigidBodyGraphicsObject(body, colorRgba);
|
||||
}
|
||||
|
||||
void MyMultiBodyCreator::createRigidBodyGraphicsInstance2(int linkIndex, class btRigidBody* body, const btVector3& colorRgba, const btVector3& specularColor, int graphicsIndex)
|
||||
{
|
||||
m_guiHelper->createRigidBodyGraphicsObject(body, colorRgba);
|
||||
{
|
||||
m_guiHelper->createRigidBodyGraphicsObject(body, colorRgba);
|
||||
int graphicsInstanceId = body->getUserIndex();
|
||||
btVector3DoubleData speculard;
|
||||
specularColor.serializeDouble(speculard);
|
||||
m_guiHelper->changeSpecularColor(graphicsInstanceId,speculard.m_floats);
|
||||
m_guiHelper->changeSpecularColor(graphicsInstanceId, speculard.m_floats);
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
void MyMultiBodyCreator::createCollisionObjectGraphicsInstance(int linkIndex, class btCollisionObject* colObj, const btVector3& colorRgba)
|
||||
void MyMultiBodyCreator::createCollisionObjectGraphicsInstance(int linkIndex, class btCollisionObject* colObj, const btVector3& colorRgba)
|
||||
{
|
||||
m_guiHelper->createCollisionObjectGraphicsObject(colObj,colorRgba);
|
||||
m_guiHelper->createCollisionObjectGraphicsObject(colObj, colorRgba);
|
||||
}
|
||||
|
||||
void MyMultiBodyCreator::createCollisionObjectGraphicsInstance2(int linkIndex, class btCollisionObject* col, const btVector4& colorRgba, const btVector3& specularColor)
|
||||
{
|
||||
createCollisionObjectGraphicsInstance(linkIndex,col,colorRgba);
|
||||
createCollisionObjectGraphicsInstance(linkIndex, col, colorRgba);
|
||||
int graphicsInstanceId = col->getUserIndex();
|
||||
btVector3DoubleData speculard;
|
||||
specularColor.serializeDouble(speculard);
|
||||
m_guiHelper->changeSpecularColor(graphicsInstanceId,speculard.m_floats);
|
||||
|
||||
m_guiHelper->changeSpecularColor(graphicsInstanceId, speculard.m_floats);
|
||||
}
|
||||
|
||||
|
||||
btMultiBody* MyMultiBodyCreator::getBulletMultiBody()
|
||||
{
|
||||
return m_bulletMultiBody;
|
||||
return m_bulletMultiBody;
|
||||
}
|
||||
|
||||
|
||||
@@ -11,74 +11,70 @@ class btMultiBody;
|
||||
|
||||
struct GenericConstraintUserInfo
|
||||
{
|
||||
int m_urdfIndex;
|
||||
int m_urdfIndex;
|
||||
int m_urdfJointType;
|
||||
btVector3 m_jointAxisInJointSpace;
|
||||
int m_jointAxisIndex;
|
||||
int m_jointAxisIndex;
|
||||
btScalar m_lowerJointLimit;
|
||||
btScalar m_upperJointLimit;
|
||||
|
||||
};
|
||||
|
||||
class MyMultiBodyCreator : public MultiBodyCreationInterface
|
||||
{
|
||||
protected:
|
||||
|
||||
btMultiBody* m_bulletMultiBody;
|
||||
btRigidBody* m_rigidBody;
|
||||
|
||||
|
||||
struct GUIHelperInterface* m_guiHelper;
|
||||
|
||||
btAlignedObjectArray<btGeneric6DofSpring2Constraint*> m_6DofConstraints;
|
||||
|
||||
public:
|
||||
|
||||
btAlignedObjectArray<int> m_mb2urdfLink;
|
||||
|
||||
btAlignedObjectArray<int> m_mb2urdfLink;
|
||||
|
||||
MyMultiBodyCreator(GUIHelperInterface* guiHelper);
|
||||
|
||||
virtual ~MyMultiBodyCreator() {}
|
||||
|
||||
virtual void createRigidBodyGraphicsInstance(int linkIndex, class btRigidBody* body, const btVector3& colorRgba, int graphicsIndex) ;
|
||||
virtual void createRigidBodyGraphicsInstance2(int linkIndex, class btRigidBody* body, const btVector3& colorRgba, const btVector3& specularColor, int graphicsIndex) ;
|
||||
|
||||
///optionally create some graphical representation from a collision object, usually for visual debugging purposes.
|
||||
virtual void createCollisionObjectGraphicsInstance(int linkIndex, class btCollisionObject* col, const btVector3& colorRgba);
|
||||
virtual void createCollisionObjectGraphicsInstance2(int linkIndex, class btCollisionObject* col, const btVector4& colorRgba, const btVector3& specularColor);
|
||||
virtual void createRigidBodyGraphicsInstance(int linkIndex, class btRigidBody* body, const btVector3& colorRgba, int graphicsIndex);
|
||||
virtual void createRigidBodyGraphicsInstance2(int linkIndex, class btRigidBody* body, const btVector3& colorRgba, const btVector3& specularColor, int graphicsIndex);
|
||||
|
||||
///optionally create some graphical representation from a collision object, usually for visual debugging purposes.
|
||||
virtual void createCollisionObjectGraphicsInstance(int linkIndex, class btCollisionObject* col, const btVector3& colorRgba);
|
||||
virtual void createCollisionObjectGraphicsInstance2(int linkIndex, class btCollisionObject* col, const btVector4& colorRgba, const btVector3& specularColor);
|
||||
|
||||
virtual class btMultiBody* allocateMultiBody(int urdfLinkIndex, int totalNumJoints, btScalar mass, const btVector3& localInertiaDiagonal, bool isFixedBase, bool canSleep);
|
||||
|
||||
virtual class btRigidBody* allocateRigidBody(int urdfLinkIndex, btScalar mass, const btVector3& localInertiaDiagonal, const btTransform& initialWorldTrans, class btCollisionShape* colShape);
|
||||
|
||||
virtual class btGeneric6DofSpring2Constraint* allocateGeneric6DofSpring2Constraint(int urdfLinkIndex, btRigidBody& rbA /*parent*/, btRigidBody& rbB, const btTransform& offsetInA, const btTransform& offsetInB, int rotateOrder = 0);
|
||||
|
||||
virtual class btMultiBody* allocateMultiBody(int urdfLinkIndex, int totalNumJoints,btScalar mass, const btVector3& localInertiaDiagonal, bool isFixedBase, bool canSleep);
|
||||
|
||||
virtual class btRigidBody* allocateRigidBody(int urdfLinkIndex, btScalar mass, const btVector3& localInertiaDiagonal, const btTransform& initialWorldTrans, class btCollisionShape* colShape);
|
||||
|
||||
virtual class btGeneric6DofSpring2Constraint* allocateGeneric6DofSpring2Constraint(int urdfLinkIndex, btRigidBody& rbA /*parent*/, btRigidBody& rbB, const btTransform& offsetInA, const btTransform& offsetInB, int rotateOrder=0);
|
||||
|
||||
virtual class btGeneric6DofSpring2Constraint* createPrismaticJoint(int urdfLinkIndex, btRigidBody& rbA /*parent*/, btRigidBody& rbB, const btTransform& offsetInA, const btTransform& offsetInB,
|
||||
const btVector3& jointAxisInJointSpace,btScalar jointLowerLimit,btScalar jointUpperLimit);
|
||||
virtual class btGeneric6DofSpring2Constraint* createRevoluteJoint(int urdfLinkIndex, btRigidBody& rbA /*parent*/, btRigidBody& rbB, const btTransform& offsetInA, const btTransform& offsetInB,
|
||||
const btVector3& jointAxisInJointSpace,btScalar jointLowerLimit,btScalar jointUpperLimit);
|
||||
const btVector3& jointAxisInJointSpace, btScalar jointLowerLimit, btScalar jointUpperLimit);
|
||||
virtual class btGeneric6DofSpring2Constraint* createRevoluteJoint(int urdfLinkIndex, btRigidBody& rbA /*parent*/, btRigidBody& rbB, const btTransform& offsetInA, const btTransform& offsetInB,
|
||||
const btVector3& jointAxisInJointSpace, btScalar jointLowerLimit, btScalar jointUpperLimit);
|
||||
|
||||
virtual class btGeneric6DofSpring2Constraint* createFixedJoint(int urdfLinkIndex, btRigidBody& rbA /*parent*/, btRigidBody& rbB, const btTransform& offsetInA, const btTransform& offsetInB);
|
||||
|
||||
virtual class btMultiBodyLinkCollider* allocateMultiBodyLinkCollider(int urdfLinkIndex, int mbLinkIndex, btMultiBody* body);
|
||||
|
||||
virtual void addLinkMapping(int urdfLinkIndex, int mbLinkIndex);
|
||||
virtual class btGeneric6DofSpring2Constraint* createFixedJoint(int urdfLinkIndex, btRigidBody& rbA /*parent*/, btRigidBody& rbB, const btTransform& offsetInA, const btTransform& offsetInB);
|
||||
|
||||
virtual class btMultiBodyLinkCollider* allocateMultiBodyLinkCollider(int urdfLinkIndex, int mbLinkIndex, btMultiBody* body);
|
||||
|
||||
virtual void addLinkMapping(int urdfLinkIndex, int mbLinkIndex);
|
||||
|
||||
btMultiBody* getBulletMultiBody();
|
||||
btRigidBody* getRigidBody()
|
||||
{
|
||||
return m_rigidBody;
|
||||
}
|
||||
|
||||
int getNum6DofConstraints() const
|
||||
return m_rigidBody;
|
||||
}
|
||||
|
||||
int getNum6DofConstraints() const
|
||||
{
|
||||
return m_6DofConstraints.size();
|
||||
}
|
||||
|
||||
btGeneric6DofSpring2Constraint* get6DofConstraint(int index)
|
||||
|
||||
btGeneric6DofSpring2Constraint* get6DofConstraint(int index)
|
||||
{
|
||||
return m_6DofConstraints[index];
|
||||
}
|
||||
};
|
||||
|
||||
#endif //MY_MULTIBODY_CREATOR
|
||||
#endif //MY_MULTIBODY_CREATOR
|
||||
|
||||
@@ -3,43 +3,42 @@
|
||||
|
||||
#include <string>
|
||||
|
||||
|
||||
///See audio_source element in http://sdformat.org/spec?ver=1.6&elem=link
|
||||
struct SDFAudioSource
|
||||
{
|
||||
enum
|
||||
{
|
||||
SDFAudioSourceValid=1,
|
||||
SDFAudioSourceLooping=2,
|
||||
SDFAudioSourceValid = 1,
|
||||
SDFAudioSourceLooping = 2,
|
||||
};
|
||||
|
||||
int m_flags; //repeat mode (0 = no repeat, 1 = loop forever)
|
||||
int m_flags; //repeat mode (0 = no repeat, 1 = loop forever)
|
||||
|
||||
std::string m_uri; //media filename of the sound, .wav file
|
||||
double m_pitch; //1 = regular rate, -1 play in reverse
|
||||
double m_gain; //normalized volume in range [0..1] where 0 is silent, 1 is most loud
|
||||
std::string m_uri; //media filename of the sound, .wav file
|
||||
double m_pitch; //1 = regular rate, -1 play in reverse
|
||||
double m_gain; //normalized volume in range [0..1] where 0 is silent, 1 is most loud
|
||||
|
||||
double m_attackRate;
|
||||
double m_decayRate;
|
||||
double m_sustainLevel;
|
||||
double m_releaseRate;
|
||||
|
||||
double m_collisionForceThreshold; //force that will trigger the audio, in Newton. If < 0, audio source is invalid
|
||||
|
||||
double m_collisionForceThreshold; //force that will trigger the audio, in Newton. If < 0, audio source is invalid
|
||||
|
||||
int m_userIndex;
|
||||
|
||||
SDFAudioSource()
|
||||
: m_flags(0),
|
||||
m_pitch(1),
|
||||
m_gain(1),
|
||||
m_attackRate(0.0001),
|
||||
m_decayRate(0.00001),
|
||||
m_sustainLevel(0.5),
|
||||
m_releaseRate(0.0005),
|
||||
m_collisionForceThreshold(0.5),
|
||||
m_userIndex(-1)
|
||||
{
|
||||
}
|
||||
: m_flags(0),
|
||||
m_pitch(1),
|
||||
m_gain(1),
|
||||
m_attackRate(0.0001),
|
||||
m_decayRate(0.00001),
|
||||
m_sustainLevel(0.5),
|
||||
m_releaseRate(0.0005),
|
||||
m_collisionForceThreshold(0.5),
|
||||
m_userIndex(-1)
|
||||
{
|
||||
}
|
||||
};
|
||||
|
||||
#endif //SDF_AUDIO_TYPES_H
|
||||
#endif //SDF_AUDIO_TYPES_H
|
||||
|
||||
@@ -4,7 +4,6 @@
|
||||
#include "BulletCollision/CollisionShapes/btCompoundShape.h"
|
||||
#include "BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h"
|
||||
|
||||
|
||||
#include "BulletDynamics/Dynamics/btRigidBody.h"
|
||||
#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h"
|
||||
#include "BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.h"
|
||||
@@ -20,131 +19,122 @@
|
||||
static bool enableConstraints = true;
|
||||
|
||||
static btVector4 colors[4] =
|
||||
{
|
||||
btVector4(1,0,0,1),
|
||||
btVector4(0,1,0,1),
|
||||
btVector4(0,1,1,1),
|
||||
btVector4(1,1,0,1),
|
||||
{
|
||||
btVector4(1, 0, 0, 1),
|
||||
btVector4(0, 1, 0, 1),
|
||||
btVector4(0, 1, 1, 1),
|
||||
btVector4(1, 1, 0, 1),
|
||||
};
|
||||
|
||||
|
||||
static btVector4 selectColor2()
|
||||
{
|
||||
|
||||
static int curColor = 0;
|
||||
btVector4 color = colors[curColor];
|
||||
curColor++;
|
||||
curColor&=3;
|
||||
return color;
|
||||
static int curColor = 0;
|
||||
btVector4 color = colors[curColor];
|
||||
curColor++;
|
||||
curColor &= 3;
|
||||
return color;
|
||||
}
|
||||
|
||||
|
||||
|
||||
struct URDF2BulletCachedData
|
||||
{
|
||||
URDF2BulletCachedData()
|
||||
:
|
||||
m_currentMultiBodyLinkIndex(-1),
|
||||
m_bulletMultiBody(0),
|
||||
m_totalNumJoints1(0)
|
||||
{
|
||||
URDF2BulletCachedData()
|
||||
: m_currentMultiBodyLinkIndex(-1),
|
||||
m_bulletMultiBody(0),
|
||||
m_totalNumJoints1(0)
|
||||
{
|
||||
}
|
||||
//these arrays will be initialized in the 'InitURDF2BulletCache'
|
||||
|
||||
}
|
||||
//these arrays will be initialized in the 'InitURDF2BulletCache'
|
||||
btAlignedObjectArray<int> m_urdfLinkParentIndices;
|
||||
btAlignedObjectArray<int> m_urdfLinkIndices2BulletLinkIndices;
|
||||
btAlignedObjectArray<class btRigidBody*> m_urdfLink2rigidBodies;
|
||||
btAlignedObjectArray<btTransform> m_urdfLinkLocalInertialFrames;
|
||||
|
||||
btAlignedObjectArray<int> m_urdfLinkParentIndices;
|
||||
btAlignedObjectArray<int> m_urdfLinkIndices2BulletLinkIndices;
|
||||
btAlignedObjectArray<class btRigidBody*> m_urdfLink2rigidBodies;
|
||||
btAlignedObjectArray<btTransform> m_urdfLinkLocalInertialFrames;
|
||||
int m_currentMultiBodyLinkIndex;
|
||||
|
||||
int m_currentMultiBodyLinkIndex;
|
||||
class btMultiBody* m_bulletMultiBody;
|
||||
|
||||
class btMultiBody* m_bulletMultiBody;
|
||||
//this will be initialized in the constructor
|
||||
int m_totalNumJoints1;
|
||||
int getParentUrdfIndex(int linkIndex) const
|
||||
{
|
||||
return m_urdfLinkParentIndices[linkIndex];
|
||||
}
|
||||
int getMbIndexFromUrdfIndex(int urdfIndex) const
|
||||
{
|
||||
if (urdfIndex == -2)
|
||||
return -2;
|
||||
return m_urdfLinkIndices2BulletLinkIndices[urdfIndex];
|
||||
}
|
||||
|
||||
//this will be initialized in the constructor
|
||||
int m_totalNumJoints1;
|
||||
int getParentUrdfIndex(int linkIndex) const
|
||||
{
|
||||
return m_urdfLinkParentIndices[linkIndex];
|
||||
}
|
||||
int getMbIndexFromUrdfIndex(int urdfIndex) const
|
||||
{
|
||||
if (urdfIndex==-2)
|
||||
return -2;
|
||||
return m_urdfLinkIndices2BulletLinkIndices[urdfIndex];
|
||||
}
|
||||
void registerMultiBody(int urdfLinkIndex, class btMultiBody* body, const btTransform& worldTransform, btScalar mass, const btVector3& localInertiaDiagonal, const class btCollisionShape* compound, const btTransform& localInertialFrame)
|
||||
{
|
||||
m_urdfLinkLocalInertialFrames[urdfLinkIndex] = localInertialFrame;
|
||||
}
|
||||
|
||||
class btRigidBody* getRigidBodyFromLink(int urdfLinkIndex)
|
||||
{
|
||||
return m_urdfLink2rigidBodies[urdfLinkIndex];
|
||||
}
|
||||
|
||||
void registerMultiBody( int urdfLinkIndex, class btMultiBody* body, const btTransform& worldTransform, btScalar mass, const btVector3& localInertiaDiagonal, const class btCollisionShape* compound, const btTransform& localInertialFrame)
|
||||
{
|
||||
m_urdfLinkLocalInertialFrames[urdfLinkIndex] = localInertialFrame;
|
||||
}
|
||||
|
||||
class btRigidBody* getRigidBodyFromLink(int urdfLinkIndex)
|
||||
{
|
||||
return m_urdfLink2rigidBodies[urdfLinkIndex];
|
||||
}
|
||||
|
||||
void registerRigidBody( int urdfLinkIndex, class btRigidBody* body, const btTransform& worldTransform, btScalar mass, const btVector3& localInertiaDiagonal, const class btCollisionShape* compound, const btTransform& localInertialFrame)
|
||||
{
|
||||
btAssert(m_urdfLink2rigidBodies[urdfLinkIndex]==0);
|
||||
|
||||
m_urdfLink2rigidBodies[urdfLinkIndex] = body;
|
||||
m_urdfLinkLocalInertialFrames[urdfLinkIndex] = localInertialFrame;
|
||||
}
|
||||
void registerRigidBody(int urdfLinkIndex, class btRigidBody* body, const btTransform& worldTransform, btScalar mass, const btVector3& localInertiaDiagonal, const class btCollisionShape* compound, const btTransform& localInertialFrame)
|
||||
{
|
||||
btAssert(m_urdfLink2rigidBodies[urdfLinkIndex] == 0);
|
||||
|
||||
m_urdfLink2rigidBodies[urdfLinkIndex] = body;
|
||||
m_urdfLinkLocalInertialFrames[urdfLinkIndex] = localInertialFrame;
|
||||
}
|
||||
};
|
||||
|
||||
void ComputeTotalNumberOfJoints(const URDFImporterInterface& u2b, URDF2BulletCachedData& cache, int linkIndex)
|
||||
{
|
||||
btAlignedObjectArray<int> childIndices;
|
||||
u2b.getLinkChildIndices(linkIndex,childIndices);
|
||||
//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++)
|
||||
{
|
||||
int childIndex =childIndices[i];
|
||||
ComputeTotalNumberOfJoints(u2b,cache,childIndex);
|
||||
}
|
||||
btAlignedObjectArray<int> childIndices;
|
||||
u2b.getLinkChildIndices(linkIndex, childIndices);
|
||||
//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++)
|
||||
{
|
||||
int childIndex = childIndices[i];
|
||||
ComputeTotalNumberOfJoints(u2b, cache, childIndex);
|
||||
}
|
||||
}
|
||||
|
||||
void ComputeParentIndices(const URDFImporterInterface& u2b, URDF2BulletCachedData& cache, int urdfLinkIndex, int urdfParentIndex)
|
||||
{
|
||||
cache.m_urdfLinkParentIndices[urdfLinkIndex]=urdfParentIndex;
|
||||
cache.m_urdfLinkIndices2BulletLinkIndices[urdfLinkIndex]=cache.m_currentMultiBodyLinkIndex++;
|
||||
cache.m_urdfLinkParentIndices[urdfLinkIndex] = urdfParentIndex;
|
||||
cache.m_urdfLinkIndices2BulletLinkIndices[urdfLinkIndex] = cache.m_currentMultiBodyLinkIndex++;
|
||||
|
||||
btAlignedObjectArray<int> childIndices;
|
||||
u2b.getLinkChildIndices(urdfLinkIndex,childIndices);
|
||||
for (int i=0;i<childIndices.size();i++)
|
||||
{
|
||||
ComputeParentIndices(u2b,cache,childIndices[i],urdfLinkIndex);
|
||||
}
|
||||
btAlignedObjectArray<int> childIndices;
|
||||
u2b.getLinkChildIndices(urdfLinkIndex, childIndices);
|
||||
for (int i = 0; i < childIndices.size(); i++)
|
||||
{
|
||||
ComputeParentIndices(u2b, cache, childIndices[i], urdfLinkIndex);
|
||||
}
|
||||
}
|
||||
|
||||
void InitURDF2BulletCache(const URDFImporterInterface& u2b, URDF2BulletCachedData& cache)
|
||||
{
|
||||
//compute the number of links, and compute parent indices array (and possibly other cached data?)
|
||||
cache.m_totalNumJoints1 = 0;
|
||||
//compute the number of links, and compute parent indices array (and possibly other cached data?)
|
||||
cache.m_totalNumJoints1 = 0;
|
||||
|
||||
int rootLinkIndex = u2b.getRootLinkIndex();
|
||||
if (rootLinkIndex>=0)
|
||||
{
|
||||
ComputeTotalNumberOfJoints(u2b,cache,rootLinkIndex);
|
||||
int numTotalLinksIncludingBase = 1+cache.m_totalNumJoints1;
|
||||
int rootLinkIndex = u2b.getRootLinkIndex();
|
||||
if (rootLinkIndex >= 0)
|
||||
{
|
||||
ComputeTotalNumberOfJoints(u2b, cache, rootLinkIndex);
|
||||
int numTotalLinksIncludingBase = 1 + cache.m_totalNumJoints1;
|
||||
|
||||
cache.m_urdfLinkParentIndices.resize(numTotalLinksIncludingBase);
|
||||
cache.m_urdfLinkIndices2BulletLinkIndices.resize(numTotalLinksIncludingBase);
|
||||
cache.m_urdfLink2rigidBodies.resize(numTotalLinksIncludingBase);
|
||||
cache.m_urdfLinkParentIndices.resize(numTotalLinksIncludingBase);
|
||||
cache.m_urdfLinkIndices2BulletLinkIndices.resize(numTotalLinksIncludingBase);
|
||||
cache.m_urdfLink2rigidBodies.resize(numTotalLinksIncludingBase);
|
||||
cache.m_urdfLinkLocalInertialFrames.resize(numTotalLinksIncludingBase);
|
||||
|
||||
cache.m_currentMultiBodyLinkIndex = -1;//multi body base has 'link' index -1
|
||||
ComputeParentIndices(u2b,cache,rootLinkIndex,-2);
|
||||
}
|
||||
|
||||
cache.m_currentMultiBodyLinkIndex = -1; //multi body base has 'link' index -1
|
||||
ComputeParentIndices(u2b, cache, rootLinkIndex, -2);
|
||||
}
|
||||
}
|
||||
|
||||
void processContactParameters(const URDFLinkContactInfo& contactInfo, btCollisionObject* col)
|
||||
@@ -176,106 +166,96 @@ void processContactParameters(const URDFLinkContactInfo& contactInfo, btCollisio
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
btScalar tmpUrdfScaling=2;
|
||||
|
||||
btScalar tmpUrdfScaling = 2;
|
||||
|
||||
void ConvertURDF2BulletInternal(
|
||||
const URDFImporterInterface& u2b, MultiBodyCreationInterface& creation,
|
||||
URDF2BulletCachedData& cache, int urdfLinkIndex,
|
||||
const btTransform& parentTransformInWorldSpace, btMultiBodyDynamicsWorld* world1,
|
||||
bool createMultiBody, const char* pathPrefix,
|
||||
int flags = 0, UrdfVisualShapeCache* cachedLinkGraphicsShapesIn=0, UrdfVisualShapeCache* cachedLinkGraphicsShapesOut=0)
|
||||
const URDFImporterInterface& u2b, MultiBodyCreationInterface& creation,
|
||||
URDF2BulletCachedData& cache, int urdfLinkIndex,
|
||||
const btTransform& parentTransformInWorldSpace, btMultiBodyDynamicsWorld* world1,
|
||||
bool createMultiBody, const char* pathPrefix,
|
||||
int flags = 0, UrdfVisualShapeCache* cachedLinkGraphicsShapesIn = 0, UrdfVisualShapeCache* cachedLinkGraphicsShapesOut = 0)
|
||||
{
|
||||
B3_PROFILE("ConvertURDF2BulletInternal2");
|
||||
//b3Printf("start converting/extracting data from URDF interface\n");
|
||||
//b3Printf("start converting/extracting data from URDF interface\n");
|
||||
|
||||
btTransform linkTransformInWorldSpace;
|
||||
linkTransformInWorldSpace.setIdentity();
|
||||
btTransform linkTransformInWorldSpace;
|
||||
linkTransformInWorldSpace.setIdentity();
|
||||
|
||||
int mbLinkIndex = cache.getMbIndexFromUrdfIndex(urdfLinkIndex);
|
||||
|
||||
int mbLinkIndex =cache.getMbIndexFromUrdfIndex(urdfLinkIndex);
|
||||
int urdfParentIndex = cache.getParentUrdfIndex(urdfLinkIndex);
|
||||
int mbParentIndex = cache.getMbIndexFromUrdfIndex(urdfParentIndex);
|
||||
btRigidBody* parentRigidBody = 0;
|
||||
|
||||
int urdfParentIndex = cache.getParentUrdfIndex(urdfLinkIndex);
|
||||
int mbParentIndex = cache.getMbIndexFromUrdfIndex(urdfParentIndex);
|
||||
btRigidBody* parentRigidBody = 0;
|
||||
|
||||
//b3Printf("mb link index = %d\n",mbLinkIndex);
|
||||
//b3Printf("mb link index = %d\n",mbLinkIndex);
|
||||
|
||||
btTransform parentLocalInertialFrame;
|
||||
parentLocalInertialFrame.setIdentity();
|
||||
btScalar parentMass(1);
|
||||
btVector3 parentLocalInertiaDiagonal(1,1,1);
|
||||
btVector3 parentLocalInertiaDiagonal(1, 1, 1);
|
||||
|
||||
if (urdfParentIndex==-2)
|
||||
{
|
||||
//b3Printf("root link has no parent\n");
|
||||
} else
|
||||
{
|
||||
//b3Printf("urdf parent index = %d\n",urdfParentIndex);
|
||||
//b3Printf("mb parent index = %d\n",mbParentIndex);
|
||||
parentRigidBody = cache.getRigidBodyFromLink(urdfParentIndex);
|
||||
u2b.getMassAndInertia2(urdfParentIndex, parentMass,parentLocalInertiaDiagonal,parentLocalInertialFrame, flags);
|
||||
if (urdfParentIndex == -2)
|
||||
{
|
||||
//b3Printf("root link has no parent\n");
|
||||
}
|
||||
else
|
||||
{
|
||||
//b3Printf("urdf parent index = %d\n",urdfParentIndex);
|
||||
//b3Printf("mb parent index = %d\n",mbParentIndex);
|
||||
parentRigidBody = cache.getRigidBodyFromLink(urdfParentIndex);
|
||||
u2b.getMassAndInertia2(urdfParentIndex, parentMass, parentLocalInertiaDiagonal, parentLocalInertialFrame, flags);
|
||||
}
|
||||
|
||||
}
|
||||
btScalar mass = 0;
|
||||
btTransform localInertialFrame;
|
||||
localInertialFrame.setIdentity();
|
||||
btVector3 localInertiaDiagonal(0, 0, 0);
|
||||
u2b.getMassAndInertia2(urdfLinkIndex, mass, localInertiaDiagonal, localInertialFrame, flags);
|
||||
|
||||
btScalar mass = 0;
|
||||
btTransform localInertialFrame;
|
||||
localInertialFrame.setIdentity();
|
||||
btVector3 localInertiaDiagonal(0,0,0);
|
||||
u2b.getMassAndInertia2(urdfLinkIndex, mass,localInertiaDiagonal,localInertialFrame, flags);
|
||||
btTransform parent2joint;
|
||||
parent2joint.setIdentity();
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
btTransform parent2joint;
|
||||
parent2joint.setIdentity();
|
||||
|
||||
int jointType;
|
||||
btVector3 jointAxisInJointSpace;
|
||||
btScalar jointLowerLimit;
|
||||
btScalar jointUpperLimit;
|
||||
btScalar jointDamping;
|
||||
btScalar jointFriction;
|
||||
int jointType;
|
||||
btVector3 jointAxisInJointSpace;
|
||||
btScalar jointLowerLimit;
|
||||
btScalar jointUpperLimit;
|
||||
btScalar jointDamping;
|
||||
btScalar jointFriction;
|
||||
btScalar jointMaxForce;
|
||||
btScalar jointMaxVelocity;
|
||||
|
||||
|
||||
bool hasParentJoint = u2b.getJointInfo2(urdfLinkIndex, parent2joint, linkTransformInWorldSpace, jointAxisInJointSpace, jointType,jointLowerLimit,jointUpperLimit, jointDamping, jointFriction,jointMaxForce,jointMaxVelocity);
|
||||
bool hasParentJoint = u2b.getJointInfo2(urdfLinkIndex, parent2joint, linkTransformInWorldSpace, jointAxisInJointSpace, jointType, jointLowerLimit, jointUpperLimit, jointDamping, jointFriction, jointMaxForce, jointMaxVelocity);
|
||||
std::string linkName = u2b.getLinkName(urdfLinkIndex);
|
||||
|
||||
if (flags & CUF_USE_SDF)
|
||||
{
|
||||
parent2joint =parentTransformInWorldSpace.inverse()*linkTransformInWorldSpace;
|
||||
}
|
||||
else
|
||||
{
|
||||
|
||||
if (flags & CUF_USE_SDF)
|
||||
{
|
||||
parent2joint = parentTransformInWorldSpace.inverse() * linkTransformInWorldSpace;
|
||||
}
|
||||
else
|
||||
{
|
||||
if (flags & CUF_USE_MJCF)
|
||||
{
|
||||
linkTransformInWorldSpace =parentTransformInWorldSpace*linkTransformInWorldSpace;
|
||||
} else
|
||||
{
|
||||
linkTransformInWorldSpace =parentTransformInWorldSpace*parent2joint;
|
||||
linkTransformInWorldSpace = parentTransformInWorldSpace * linkTransformInWorldSpace;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
else
|
||||
{
|
||||
linkTransformInWorldSpace = parentTransformInWorldSpace * parent2joint;
|
||||
}
|
||||
}
|
||||
|
||||
btCompoundShape* tmpShape = u2b.convertLinkCollisionShapes(urdfLinkIndex,pathPrefix,localInertialFrame);
|
||||
btCompoundShape* tmpShape = u2b.convertLinkCollisionShapes(urdfLinkIndex, pathPrefix, localInertialFrame);
|
||||
btCollisionShape* compoundShape = tmpShape;
|
||||
if (tmpShape->getNumChildShapes() == 1 && tmpShape->getChildTransform(0)==btTransform::getIdentity())
|
||||
if (tmpShape->getNumChildShapes() == 1 && tmpShape->getChildTransform(0) == btTransform::getIdentity())
|
||||
{
|
||||
compoundShape = tmpShape->getChildShape(0);
|
||||
}
|
||||
|
||||
|
||||
int graphicsIndex;
|
||||
{
|
||||
B3_PROFILE("convertLinkVisualShapes");
|
||||
if (cachedLinkGraphicsShapesIn && cachedLinkGraphicsShapesIn->m_cachedUrdfLinkVisualShapeIndices.size() > (mbLinkIndex+1))
|
||||
if (cachedLinkGraphicsShapesIn && cachedLinkGraphicsShapesIn->m_cachedUrdfLinkVisualShapeIndices.size() > (mbLinkIndex + 1))
|
||||
{
|
||||
graphicsIndex = cachedLinkGraphicsShapesIn->m_cachedUrdfLinkVisualShapeIndices[mbLinkIndex+1];
|
||||
graphicsIndex = cachedLinkGraphicsShapesIn->m_cachedUrdfLinkVisualShapeIndices[mbLinkIndex + 1];
|
||||
UrdfMaterialColor matColor = cachedLinkGraphicsShapesIn->m_cachedUrdfLinkColors[mbLinkIndex + 1];
|
||||
u2b.setLinkColor2(urdfLinkIndex, matColor);
|
||||
}
|
||||
@@ -291,85 +271,77 @@ void ConvertURDF2BulletInternal(
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
if (compoundShape)
|
||||
{
|
||||
|
||||
|
||||
if (compoundShape)
|
||||
{
|
||||
UrdfMaterialColor matColor;
|
||||
btVector4 color2 = selectColor2();
|
||||
btVector3 specular(0.5,0.5,0.5);
|
||||
if (u2b.getLinkColor2(urdfLinkIndex,matColor))
|
||||
btVector4 color2 = selectColor2();
|
||||
btVector3 specular(0.5, 0.5, 0.5);
|
||||
if (u2b.getLinkColor2(urdfLinkIndex, matColor))
|
||||
{
|
||||
color2 = matColor.m_rgbaColor;
|
||||
specular = matColor.m_specularColor;
|
||||
}
|
||||
|
||||
/*
|
||||
/*
|
||||
if (visual->material.get())
|
||||
{
|
||||
color.setValue(visual->material->color.r,visual->material->color.g,visual->material->color.b);//,visual->material->color.a);
|
||||
}
|
||||
*/
|
||||
if (mass)
|
||||
{
|
||||
if (!(flags & CUF_USE_URDF_INERTIA))
|
||||
{
|
||||
compoundShape->calculateLocalInertia(mass, localInertiaDiagonal);
|
||||
btAssert(localInertiaDiagonal[0] < 1e10);
|
||||
btAssert(localInertiaDiagonal[1] < 1e10);
|
||||
btAssert(localInertiaDiagonal[2] < 1e10);
|
||||
}
|
||||
URDFLinkContactInfo contactInfo;
|
||||
u2b.getLinkContactInfo(urdfLinkIndex,contactInfo);
|
||||
//temporary inertia scaling until we load inertia from URDF
|
||||
if (contactInfo.m_flags & URDF_CONTACT_HAS_INERTIA_SCALING)
|
||||
{
|
||||
localInertiaDiagonal*=contactInfo.m_inertiaScaling;
|
||||
}
|
||||
}
|
||||
if (mass)
|
||||
{
|
||||
if (!(flags & CUF_USE_URDF_INERTIA))
|
||||
{
|
||||
compoundShape->calculateLocalInertia(mass, localInertiaDiagonal);
|
||||
btAssert(localInertiaDiagonal[0] < 1e10);
|
||||
btAssert(localInertiaDiagonal[1] < 1e10);
|
||||
btAssert(localInertiaDiagonal[2] < 1e10);
|
||||
}
|
||||
URDFLinkContactInfo contactInfo;
|
||||
u2b.getLinkContactInfo(urdfLinkIndex, contactInfo);
|
||||
//temporary inertia scaling until we load inertia from URDF
|
||||
if (contactInfo.m_flags & URDF_CONTACT_HAS_INERTIA_SCALING)
|
||||
{
|
||||
localInertiaDiagonal *= contactInfo.m_inertiaScaling;
|
||||
}
|
||||
}
|
||||
|
||||
btRigidBody* linkRigidBody = 0;
|
||||
btTransform inertialFrameInWorldSpace = linkTransformInWorldSpace*localInertialFrame;
|
||||
bool canSleep = (flags & CUF_ENABLE_SLEEPING)!=0;
|
||||
btRigidBody* linkRigidBody = 0;
|
||||
btTransform inertialFrameInWorldSpace = linkTransformInWorldSpace * localInertialFrame;
|
||||
bool canSleep = (flags & CUF_ENABLE_SLEEPING) != 0;
|
||||
|
||||
if (!createMultiBody)
|
||||
{
|
||||
btRigidBody* body = creation.allocateRigidBody(urdfLinkIndex, mass, localInertiaDiagonal, inertialFrameInWorldSpace, compoundShape);
|
||||
|
||||
if (!createMultiBody)
|
||||
{
|
||||
btRigidBody* body = creation.allocateRigidBody(urdfLinkIndex, mass, localInertiaDiagonal, inertialFrameInWorldSpace, compoundShape);
|
||||
|
||||
if (!canSleep)
|
||||
{
|
||||
body->forceActivationState(DISABLE_DEACTIVATION);
|
||||
}
|
||||
|
||||
linkRigidBody = body;
|
||||
linkRigidBody = body;
|
||||
|
||||
world1->addRigidBody(body);
|
||||
world1->addRigidBody(body);
|
||||
|
||||
|
||||
compoundShape->setUserIndex(graphicsIndex);
|
||||
compoundShape->setUserIndex(graphicsIndex);
|
||||
|
||||
URDFLinkContactInfo contactInfo;
|
||||
u2b.getLinkContactInfo(urdfLinkIndex, contactInfo);
|
||||
|
||||
processContactParameters(contactInfo, body);
|
||||
creation.createRigidBodyGraphicsInstance2(urdfLinkIndex, body, color2,specular, graphicsIndex);
|
||||
cache.registerRigidBody(urdfLinkIndex, body, inertialFrameInWorldSpace, mass, localInertiaDiagonal, compoundShape, localInertialFrame);
|
||||
|
||||
creation.createRigidBodyGraphicsInstance2(urdfLinkIndex, body, color2, specular, graphicsIndex);
|
||||
cache.registerRigidBody(urdfLinkIndex, body, inertialFrameInWorldSpace, mass, localInertiaDiagonal, compoundShape, localInertialFrame);
|
||||
|
||||
|
||||
//untested: u2b.convertLinkVisualShapes2(linkIndex,urdfLinkIndex,pathPrefix,localInertialFrame,body);
|
||||
} else
|
||||
{
|
||||
if (cache.m_bulletMultiBody==0)
|
||||
{
|
||||
|
||||
|
||||
bool isFixedBase = (mass==0);//todo: figure out when base is fixed
|
||||
int totalNumJoints = cache.m_totalNumJoints1;
|
||||
cache.m_bulletMultiBody = creation.allocateMultiBody(urdfLinkIndex, totalNumJoints,mass, localInertiaDiagonal, isFixedBase, canSleep);
|
||||
//untested: u2b.convertLinkVisualShapes2(linkIndex,urdfLinkIndex,pathPrefix,localInertialFrame,body);
|
||||
}
|
||||
else
|
||||
{
|
||||
if (cache.m_bulletMultiBody == 0)
|
||||
{
|
||||
bool isFixedBase = (mass == 0); //todo: figure out when base is fixed
|
||||
int totalNumJoints = cache.m_totalNumJoints1;
|
||||
cache.m_bulletMultiBody = creation.allocateMultiBody(urdfLinkIndex, totalNumJoints, mass, localInertiaDiagonal, isFixedBase, canSleep);
|
||||
if (flags & CUF_GLOBAL_VELOCITIES_MB)
|
||||
{
|
||||
cache.m_bulletMultiBody->useGlobalVelocities(true);
|
||||
@@ -378,21 +350,20 @@ void ConvertURDF2BulletInternal(
|
||||
{
|
||||
cache.m_bulletMultiBody->setBaseWorldTransform(linkTransformInWorldSpace);
|
||||
}
|
||||
|
||||
cache.registerMultiBody(urdfLinkIndex, cache.m_bulletMultiBody, inertialFrameInWorldSpace, mass, localInertiaDiagonal, compoundShape, localInertialFrame);
|
||||
}
|
||||
|
||||
}
|
||||
cache.registerMultiBody(urdfLinkIndex, cache.m_bulletMultiBody, inertialFrameInWorldSpace, mass, localInertiaDiagonal, compoundShape, localInertialFrame);
|
||||
}
|
||||
}
|
||||
|
||||
//create a joint if necessary
|
||||
if (hasParentJoint) {
|
||||
//create a joint if necessary
|
||||
if (hasParentJoint)
|
||||
{
|
||||
btTransform offsetInA, offsetInB;
|
||||
offsetInA = parentLocalInertialFrame.inverse() * parent2joint;
|
||||
offsetInB = localInertialFrame.inverse();
|
||||
btQuaternion parentRotToThis = offsetInB.getRotation() * offsetInA.inverse().getRotation();
|
||||
|
||||
btTransform offsetInA,offsetInB;
|
||||
offsetInA = parentLocalInertialFrame.inverse()*parent2joint;
|
||||
offsetInB = localInertialFrame.inverse();
|
||||
btQuaternion parentRotToThis = offsetInB.getRotation() * offsetInA.inverse().getRotation();
|
||||
|
||||
bool disableParentCollision = true;
|
||||
bool disableParentCollision = true;
|
||||
|
||||
if (createMultiBody && cache.m_bulletMultiBody)
|
||||
{
|
||||
@@ -404,103 +375,108 @@ void ConvertURDF2BulletInternal(
|
||||
cache.m_bulletMultiBody->getLink(mbLinkIndex).m_jointMaxVelocity = jointMaxVelocity;
|
||||
}
|
||||
|
||||
switch (jointType)
|
||||
{
|
||||
switch (jointType)
|
||||
{
|
||||
case URDFFloatingJoint:
|
||||
case URDFPlanarJoint:
|
||||
case URDFFixedJoint:
|
||||
{
|
||||
if ((jointType==URDFFloatingJoint)||(jointType==URDFPlanarJoint))
|
||||
case URDFFixedJoint:
|
||||
{
|
||||
if ((jointType == URDFFloatingJoint) || (jointType == URDFPlanarJoint))
|
||||
{
|
||||
printf("Warning: joint unsupported, creating a fixed joint instead.");
|
||||
}
|
||||
creation.addLinkMapping(urdfLinkIndex,mbLinkIndex);
|
||||
creation.addLinkMapping(urdfLinkIndex, mbLinkIndex);
|
||||
|
||||
if (createMultiBody)
|
||||
{
|
||||
//todo: adjust the center of mass transform and pivot axis properly
|
||||
cache.m_bulletMultiBody->setupFixed(mbLinkIndex, mass, localInertiaDiagonal, mbParentIndex,
|
||||
parentRotToThis, offsetInA.getOrigin(), -offsetInB.getOrigin());
|
||||
}
|
||||
else
|
||||
{
|
||||
//b3Printf("Fixed joint\n");
|
||||
|
||||
if (createMultiBody)
|
||||
{
|
||||
//todo: adjust the center of mass transform and pivot axis properly
|
||||
cache.m_bulletMultiBody->setupFixed(mbLinkIndex, mass, localInertiaDiagonal, mbParentIndex,
|
||||
parentRotToThis, offsetInA.getOrigin(),-offsetInB.getOrigin());
|
||||
|
||||
} else
|
||||
{
|
||||
//b3Printf("Fixed joint\n");
|
||||
|
||||
btGeneric6DofSpring2Constraint* dof6 = 0;
|
||||
|
||||
//backward compatibility
|
||||
if (flags & CUF_RESERVED )
|
||||
if (flags & CUF_RESERVED)
|
||||
{
|
||||
dof6 = creation.createFixedJoint(urdfLinkIndex,*parentRigidBody, *linkRigidBody, offsetInA, offsetInB);
|
||||
} else
|
||||
{
|
||||
dof6 = creation.createFixedJoint(urdfLinkIndex,*linkRigidBody, *parentRigidBody, offsetInB, offsetInA);
|
||||
dof6 = creation.createFixedJoint(urdfLinkIndex, *parentRigidBody, *linkRigidBody, offsetInA, offsetInB);
|
||||
}
|
||||
if (enableConstraints)
|
||||
world1->addConstraint(dof6,true);
|
||||
}
|
||||
break;
|
||||
}
|
||||
case URDFContinuousJoint:
|
||||
case URDFRevoluteJoint:
|
||||
{
|
||||
creation.addLinkMapping(urdfLinkIndex,mbLinkIndex);
|
||||
if (createMultiBody)
|
||||
{
|
||||
cache.m_bulletMultiBody->setupRevolute(mbLinkIndex, mass, localInertiaDiagonal, mbParentIndex,
|
||||
parentRotToThis, quatRotate(offsetInB.getRotation(),jointAxisInJointSpace), offsetInA.getOrigin(),//parent2joint.getOrigin(),
|
||||
-offsetInB.getOrigin(),
|
||||
disableParentCollision);
|
||||
|
||||
if (jointType == URDFRevoluteJoint && jointLowerLimit <= jointUpperLimit) {
|
||||
//std::string name = u2b.getLinkName(urdfLinkIndex);
|
||||
//printf("create btMultiBodyJointLimitConstraint for revolute link name=%s urdf link index=%d (low=%f, up=%f)\n", name.c_str(), urdfLinkIndex, jointLowerLimit, jointUpperLimit);
|
||||
btMultiBodyConstraint* con = new btMultiBodyJointLimitConstraint(cache.m_bulletMultiBody, mbLinkIndex, jointLowerLimit, jointUpperLimit);
|
||||
world1->addMultiBodyConstraint(con);
|
||||
}
|
||||
} else
|
||||
{
|
||||
else
|
||||
{
|
||||
dof6 = creation.createFixedJoint(urdfLinkIndex, *linkRigidBody, *parentRigidBody, offsetInB, offsetInA);
|
||||
}
|
||||
if (enableConstraints)
|
||||
world1->addConstraint(dof6, true);
|
||||
}
|
||||
break;
|
||||
}
|
||||
case URDFContinuousJoint:
|
||||
case URDFRevoluteJoint:
|
||||
{
|
||||
creation.addLinkMapping(urdfLinkIndex, mbLinkIndex);
|
||||
if (createMultiBody)
|
||||
{
|
||||
cache.m_bulletMultiBody->setupRevolute(mbLinkIndex, mass, localInertiaDiagonal, mbParentIndex,
|
||||
parentRotToThis, quatRotate(offsetInB.getRotation(), jointAxisInJointSpace), offsetInA.getOrigin(), //parent2joint.getOrigin(),
|
||||
-offsetInB.getOrigin(),
|
||||
disableParentCollision);
|
||||
|
||||
btGeneric6DofSpring2Constraint* dof6 = 0;
|
||||
if (jointType == URDFRevoluteJoint && jointLowerLimit <= jointUpperLimit)
|
||||
{
|
||||
//std::string name = u2b.getLinkName(urdfLinkIndex);
|
||||
//printf("create btMultiBodyJointLimitConstraint for revolute link name=%s urdf link index=%d (low=%f, up=%f)\n", name.c_str(), urdfLinkIndex, jointLowerLimit, jointUpperLimit);
|
||||
btMultiBodyConstraint* con = new btMultiBodyJointLimitConstraint(cache.m_bulletMultiBody, mbLinkIndex, jointLowerLimit, jointUpperLimit);
|
||||
world1->addMultiBodyConstraint(con);
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
btGeneric6DofSpring2Constraint* dof6 = 0;
|
||||
if (jointType == URDFRevoluteJoint && jointLowerLimit <= jointUpperLimit)
|
||||
{
|
||||
//backwards compatibility
|
||||
if (flags & CUF_RESERVED )
|
||||
if (flags & CUF_RESERVED)
|
||||
{
|
||||
dof6 = creation.createRevoluteJoint(urdfLinkIndex,*parentRigidBody, *linkRigidBody, offsetInA, offsetInB,jointAxisInJointSpace,jointLowerLimit, jointUpperLimit);
|
||||
} else
|
||||
{
|
||||
dof6 = creation.createRevoluteJoint(urdfLinkIndex,*linkRigidBody, *parentRigidBody, offsetInB, offsetInA,jointAxisInJointSpace,jointLowerLimit, jointUpperLimit);
|
||||
dof6 = creation.createRevoluteJoint(urdfLinkIndex, *parentRigidBody, *linkRigidBody, offsetInA, offsetInB, jointAxisInJointSpace, jointLowerLimit, jointUpperLimit);
|
||||
}
|
||||
} else
|
||||
{
|
||||
//disable joint limits
|
||||
if (flags & CUF_RESERVED )
|
||||
else
|
||||
{
|
||||
dof6 = creation.createRevoluteJoint(urdfLinkIndex,*parentRigidBody, *linkRigidBody, offsetInA, offsetInB,jointAxisInJointSpace,1,-1);
|
||||
} else
|
||||
{
|
||||
dof6 = creation.createRevoluteJoint(urdfLinkIndex,*linkRigidBody, *parentRigidBody, offsetInB, offsetInA,jointAxisInJointSpace,1,-1);
|
||||
dof6 = creation.createRevoluteJoint(urdfLinkIndex, *linkRigidBody, *parentRigidBody, offsetInB, offsetInA, jointAxisInJointSpace, jointLowerLimit, jointUpperLimit);
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
//disable joint limits
|
||||
if (flags & CUF_RESERVED)
|
||||
{
|
||||
dof6 = creation.createRevoluteJoint(urdfLinkIndex, *parentRigidBody, *linkRigidBody, offsetInA, offsetInB, jointAxisInJointSpace, 1, -1);
|
||||
}
|
||||
else
|
||||
{
|
||||
dof6 = creation.createRevoluteJoint(urdfLinkIndex, *linkRigidBody, *parentRigidBody, offsetInB, offsetInA, jointAxisInJointSpace, 1, -1);
|
||||
}
|
||||
}
|
||||
|
||||
if (enableConstraints)
|
||||
world1->addConstraint(dof6,true);
|
||||
//b3Printf("Revolute/Continuous joint\n");
|
||||
}
|
||||
break;
|
||||
}
|
||||
case URDFPrismaticJoint:
|
||||
{
|
||||
creation.addLinkMapping(urdfLinkIndex,mbLinkIndex);
|
||||
|
||||
if (createMultiBody)
|
||||
{
|
||||
cache.m_bulletMultiBody->setupPrismatic(mbLinkIndex, mass, localInertiaDiagonal, mbParentIndex,
|
||||
parentRotToThis, quatRotate(offsetInB.getRotation(),jointAxisInJointSpace), offsetInA.getOrigin(),//parent2joint.getOrigin(),
|
||||
-offsetInB.getOrigin(),
|
||||
disableParentCollision);
|
||||
|
||||
if (enableConstraints)
|
||||
world1->addConstraint(dof6, true);
|
||||
//b3Printf("Revolute/Continuous joint\n");
|
||||
}
|
||||
break;
|
||||
}
|
||||
case URDFPrismaticJoint:
|
||||
{
|
||||
creation.addLinkMapping(urdfLinkIndex, mbLinkIndex);
|
||||
|
||||
if (createMultiBody)
|
||||
{
|
||||
cache.m_bulletMultiBody->setupPrismatic(mbLinkIndex, mass, localInertiaDiagonal, mbParentIndex,
|
||||
parentRotToThis, quatRotate(offsetInB.getRotation(), jointAxisInJointSpace), offsetInA.getOrigin(), //parent2joint.getOrigin(),
|
||||
-offsetInB.getOrigin(),
|
||||
disableParentCollision);
|
||||
|
||||
if (jointLowerLimit <= jointUpperLimit)
|
||||
{
|
||||
//std::string name = u2b.getLinkName(urdfLinkIndex);
|
||||
@@ -509,38 +485,36 @@ void ConvertURDF2BulletInternal(
|
||||
btMultiBodyConstraint* con = new btMultiBodyJointLimitConstraint(cache.m_bulletMultiBody, mbLinkIndex, jointLowerLimit, jointUpperLimit);
|
||||
world1->addMultiBodyConstraint(con);
|
||||
}
|
||||
//printf("joint lower limit=%d, upper limit = %f\n", jointLowerLimit, jointUpperLimit);
|
||||
//printf("joint lower limit=%d, upper limit = %f\n", jointLowerLimit, jointUpperLimit);
|
||||
}
|
||||
else
|
||||
{
|
||||
btGeneric6DofSpring2Constraint* dof6 = creation.createPrismaticJoint(urdfLinkIndex, *parentRigidBody, *linkRigidBody, offsetInA, offsetInB, jointAxisInJointSpace, jointLowerLimit, jointUpperLimit);
|
||||
|
||||
} else
|
||||
{
|
||||
|
||||
btGeneric6DofSpring2Constraint* dof6 = creation.createPrismaticJoint(urdfLinkIndex,*parentRigidBody, *linkRigidBody, offsetInA, offsetInB,jointAxisInJointSpace,jointLowerLimit,jointUpperLimit);
|
||||
|
||||
if (enableConstraints)
|
||||
world1->addConstraint(dof6,true);
|
||||
if (enableConstraints)
|
||||
world1->addConstraint(dof6, true);
|
||||
|
||||
//b3Printf("Prismatic\n");
|
||||
}
|
||||
break;
|
||||
}
|
||||
default:
|
||||
{
|
||||
//b3Printf("Error: unsupported joint type in URDF (%d)\n", jointType);
|
||||
//b3Printf("Prismatic\n");
|
||||
}
|
||||
break;
|
||||
}
|
||||
default:
|
||||
{
|
||||
//b3Printf("Error: unsupported joint type in URDF (%d)\n", jointType);
|
||||
btAssert(0);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
if (createMultiBody)
|
||||
{
|
||||
//if (compoundShape->getNumChildShapes()>0)
|
||||
{
|
||||
btMultiBodyLinkCollider* col = creation.allocateMultiBodyLinkCollider(urdfLinkIndex, mbLinkIndex, cache.m_bulletMultiBody);
|
||||
|
||||
if (createMultiBody)
|
||||
{
|
||||
//if (compoundShape->getNumChildShapes()>0)
|
||||
{
|
||||
btMultiBodyLinkCollider* col= creation.allocateMultiBodyLinkCollider(urdfLinkIndex, mbLinkIndex, cache.m_bulletMultiBody);
|
||||
compoundShape->setUserIndex(graphicsIndex);
|
||||
|
||||
compoundShape->setUserIndex(graphicsIndex);
|
||||
|
||||
col->setCollisionShape(compoundShape);
|
||||
col->setCollisionShape(compoundShape);
|
||||
|
||||
if (compoundShape->getShapeType() == TRIANGLE_MESH_SHAPE_PROXYTYPE)
|
||||
{
|
||||
@@ -551,21 +525,21 @@ void ConvertURDF2BulletInternal(
|
||||
}
|
||||
}
|
||||
|
||||
btTransform tr;
|
||||
tr.setIdentity();
|
||||
tr = linkTransformInWorldSpace;
|
||||
//if we don't set the initial pose of the btCollisionObject, the simulator will do this
|
||||
//when syncing the btMultiBody link transforms to the btMultiBodyLinkCollider
|
||||
btTransform tr;
|
||||
tr.setIdentity();
|
||||
tr = linkTransformInWorldSpace;
|
||||
//if we don't set the initial pose of the btCollisionObject, the simulator will do this
|
||||
//when syncing the btMultiBody link transforms to the btMultiBodyLinkCollider
|
||||
|
||||
col->setWorldTransform(tr);
|
||||
|
||||
col->setWorldTransform(tr);
|
||||
|
||||
//base and fixed? -> static, otherwise flag as dynamic
|
||||
bool isDynamic = (mbLinkIndex<0 && cache.m_bulletMultiBody->hasFixedBase())? false : true;
|
||||
int collisionFilterGroup = isDynamic? int(btBroadphaseProxy::DefaultFilter) : int(btBroadphaseProxy::StaticFilter);
|
||||
int collisionFilterMask = isDynamic? int(btBroadphaseProxy::AllFilter) : int(btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter);
|
||||
bool isDynamic = (mbLinkIndex < 0 && cache.m_bulletMultiBody->hasFixedBase()) ? false : true;
|
||||
int collisionFilterGroup = isDynamic ? int(btBroadphaseProxy::DefaultFilter) : int(btBroadphaseProxy::StaticFilter);
|
||||
int collisionFilterMask = isDynamic ? int(btBroadphaseProxy::AllFilter) : int(btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter);
|
||||
|
||||
int colGroup=0, colMask=0;
|
||||
int collisionFlags = u2b.getCollisionGroupAndMask(urdfLinkIndex,colGroup, colMask);
|
||||
int colGroup = 0, colMask = 0;
|
||||
int collisionFlags = u2b.getCollisionGroupAndMask(urdfLinkIndex, colGroup, colMask);
|
||||
if (collisionFlags & URDF_HAS_COLLISION_GROUP)
|
||||
{
|
||||
collisionFilterGroup = colGroup;
|
||||
@@ -574,12 +548,12 @@ void ConvertURDF2BulletInternal(
|
||||
{
|
||||
collisionFilterMask = colMask;
|
||||
}
|
||||
world1->addCollisionObject(col,collisionFilterGroup,collisionFilterMask);
|
||||
world1->addCollisionObject(col, collisionFilterGroup, collisionFilterMask);
|
||||
|
||||
btVector4 color2 = selectColor2();//(0.0,0.0,0.5);
|
||||
btVector3 specularColor(1,1,1);
|
||||
btVector4 color2 = selectColor2(); //(0.0,0.0,0.5);
|
||||
btVector3 specularColor(1, 1, 1);
|
||||
UrdfMaterialColor matCol;
|
||||
if (u2b.getLinkColor2(urdfLinkIndex,matCol))
|
||||
if (u2b.getLinkColor2(urdfLinkIndex, matCol))
|
||||
{
|
||||
color2 = matCol.m_rgbaColor;
|
||||
specularColor = matCol.m_specularColor;
|
||||
@@ -593,75 +567,72 @@ void ConvertURDF2BulletInternal(
|
||||
u2b.convertLinkVisualShapes2(mbLinkIndex, urdfLinkIndex, pathPrefix, localInertialFrame, col, u2b.getBodyUniqueId());
|
||||
}
|
||||
URDFLinkContactInfo contactInfo;
|
||||
u2b.getLinkContactInfo(urdfLinkIndex,contactInfo);
|
||||
u2b.getLinkContactInfo(urdfLinkIndex, contactInfo);
|
||||
|
||||
processContactParameters(contactInfo, col);
|
||||
|
||||
if (mbLinkIndex>=0) //???? double-check +/- 1
|
||||
{
|
||||
|
||||
cache.m_bulletMultiBody->getLink(mbLinkIndex).m_collider=col;
|
||||
if (flags&CUF_USE_SELF_COLLISION_INCLUDE_PARENT)
|
||||
if (mbLinkIndex >= 0) //???? double-check +/- 1
|
||||
{
|
||||
cache.m_bulletMultiBody->getLink(mbLinkIndex).m_collider = col;
|
||||
if (flags & CUF_USE_SELF_COLLISION_INCLUDE_PARENT)
|
||||
{
|
||||
cache.m_bulletMultiBody->getLink(mbLinkIndex).m_flags &= ~BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION;
|
||||
}
|
||||
if (flags&CUF_USE_SELF_COLLISION_EXCLUDE_ALL_PARENTS)
|
||||
if (flags & CUF_USE_SELF_COLLISION_EXCLUDE_ALL_PARENTS)
|
||||
{
|
||||
cache.m_bulletMultiBody->getLink(mbLinkIndex).m_flags |= BT_MULTIBODYLINKFLAGS_DISABLE_ALL_PARENT_COLLISION;
|
||||
}
|
||||
} else
|
||||
{
|
||||
// if (canSleep)
|
||||
}
|
||||
else
|
||||
{
|
||||
// if (canSleep)
|
||||
{
|
||||
if (cache.m_bulletMultiBody->getBaseMass()==0)
|
||||
//&& cache.m_bulletMultiBody->getNumDofs()==0)
|
||||
if (cache.m_bulletMultiBody->getBaseMass() == 0)
|
||||
//&& cache.m_bulletMultiBody->getNumDofs()==0)
|
||||
{
|
||||
//col->setCollisionFlags(btCollisionObject::CF_KINEMATIC_OBJECT);
|
||||
col->setCollisionFlags(btCollisionObject::CF_STATIC_OBJECT);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
cache.m_bulletMultiBody->setBaseCollider(col);
|
||||
}
|
||||
}
|
||||
} else
|
||||
{
|
||||
int mbLinkIndex =cache.getMbIndexFromUrdfIndex(urdfLinkIndex);
|
||||
|
||||
cache.m_bulletMultiBody->setBaseCollider(col);
|
||||
}
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
int mbLinkIndex = cache.getMbIndexFromUrdfIndex(urdfLinkIndex);
|
||||
//u2b.convertLinkVisualShapes2(mbLinkIndex, urdfLinkIndex, pathPrefix, localInertialFrame, col, u2b.getBodyUniqueId());
|
||||
u2b.convertLinkVisualShapes2(-1,urdfLinkIndex,pathPrefix,localInertialFrame,linkRigidBody,u2b.getBodyUniqueId());
|
||||
}
|
||||
}
|
||||
u2b.convertLinkVisualShapes2(-1, urdfLinkIndex, pathPrefix, localInertialFrame, linkRigidBody, u2b.getBodyUniqueId());
|
||||
}
|
||||
}
|
||||
|
||||
btAlignedObjectArray<int> urdfChildIndices;
|
||||
u2b.getLinkChildIndices(urdfLinkIndex, urdfChildIndices);
|
||||
|
||||
btAlignedObjectArray<int> urdfChildIndices;
|
||||
u2b.getLinkChildIndices(urdfLinkIndex,urdfChildIndices);
|
||||
int numChildren = urdfChildIndices.size();
|
||||
|
||||
int numChildren = urdfChildIndices.size();
|
||||
|
||||
for (int i=0;i<numChildren;i++)
|
||||
{
|
||||
int urdfChildLinkIndex = urdfChildIndices[i];
|
||||
|
||||
ConvertURDF2BulletInternal(u2b,creation, cache,urdfChildLinkIndex,linkTransformInWorldSpace,world1,createMultiBody,pathPrefix,flags, cachedLinkGraphicsShapesIn, cachedLinkGraphicsShapesOut);
|
||||
}
|
||||
for (int i = 0; i < numChildren; i++)
|
||||
{
|
||||
int urdfChildLinkIndex = urdfChildIndices[i];
|
||||
|
||||
ConvertURDF2BulletInternal(u2b, creation, cache, urdfChildLinkIndex, linkTransformInWorldSpace, world1, createMultiBody, pathPrefix, flags, cachedLinkGraphicsShapesIn, cachedLinkGraphicsShapesOut);
|
||||
}
|
||||
}
|
||||
void ConvertURDF2Bullet(
|
||||
const URDFImporterInterface& u2b, MultiBodyCreationInterface& creation,
|
||||
const btTransform& rootTransformInWorldSpace,
|
||||
btMultiBodyDynamicsWorld* world1,
|
||||
bool createMultiBody, const char* pathPrefix, int flags, UrdfVisualShapeCache* cachedLinkGraphicsShapes)
|
||||
const URDFImporterInterface& u2b, MultiBodyCreationInterface& creation,
|
||||
const btTransform& rootTransformInWorldSpace,
|
||||
btMultiBodyDynamicsWorld* world1,
|
||||
bool createMultiBody, const char* pathPrefix, int flags, UrdfVisualShapeCache* cachedLinkGraphicsShapes)
|
||||
{
|
||||
|
||||
URDF2BulletCachedData cache;
|
||||
InitURDF2BulletCache(u2b,cache);
|
||||
int urdfLinkIndex = u2b.getRootLinkIndex();
|
||||
InitURDF2BulletCache(u2b, cache);
|
||||
int urdfLinkIndex = u2b.getRootLinkIndex();
|
||||
B3_PROFILE("ConvertURDF2Bullet");
|
||||
|
||||
|
||||
UrdfVisualShapeCache cachedLinkGraphicsShapesOut;
|
||||
|
||||
ConvertURDF2BulletInternal(u2b, creation, cache, urdfLinkIndex,rootTransformInWorldSpace,world1,createMultiBody,pathPrefix,flags, cachedLinkGraphicsShapes, &cachedLinkGraphicsShapesOut);
|
||||
ConvertURDF2BulletInternal(u2b, creation, cache, urdfLinkIndex, rootTransformInWorldSpace, world1, createMultiBody, pathPrefix, flags, cachedLinkGraphicsShapes, &cachedLinkGraphicsShapesOut);
|
||||
if (cachedLinkGraphicsShapes && cachedLinkGraphicsShapesOut.m_cachedUrdfLinkVisualShapeIndices.size() > cachedLinkGraphicsShapes->m_cachedUrdfLinkVisualShapeIndices.size())
|
||||
{
|
||||
*cachedLinkGraphicsShapes = cachedLinkGraphicsShapesOut;
|
||||
@@ -672,25 +643,24 @@ void ConvertURDF2Bullet(
|
||||
B3_PROFILE("Post process");
|
||||
btMultiBody* mb = cache.m_bulletMultiBody;
|
||||
|
||||
mb->setHasSelfCollision((flags&CUF_USE_SELF_COLLISION)!=0);
|
||||
|
||||
mb->setHasSelfCollision((flags & CUF_USE_SELF_COLLISION) != 0);
|
||||
|
||||
mb->finalizeMultiDof();
|
||||
|
||||
btTransform localInertialFrameRoot = cache.m_urdfLinkLocalInertialFrames[urdfLinkIndex];
|
||||
|
||||
if (flags & CUF_USE_MJCF)
|
||||
{
|
||||
} else
|
||||
}
|
||||
else
|
||||
{
|
||||
mb->setBaseWorldTransform(rootTransformInWorldSpace*localInertialFrameRoot);
|
||||
mb->setBaseWorldTransform(rootTransformInWorldSpace * localInertialFrameRoot);
|
||||
}
|
||||
btAlignedObjectArray<btQuaternion> scratch_q;
|
||||
btAlignedObjectArray<btVector3> scratch_m;
|
||||
mb->forwardKinematics(scratch_q,scratch_m);
|
||||
mb->updateCollisionObjectWorldTransforms(scratch_q,scratch_m);
|
||||
|
||||
mb->forwardKinematics(scratch_q, scratch_m);
|
||||
mb->updateCollisionObjectWorldTransforms(scratch_q, scratch_m);
|
||||
|
||||
world1->addMultiBody(mb);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
@@ -3,35 +3,34 @@
|
||||
#include "LinearMath/btAlignedObjectArray.h"
|
||||
#include "LinearMath/btTransform.h"
|
||||
#include <string>
|
||||
#include "URDFJointTypes.h"//for UrdfMaterialColor cache
|
||||
#include "URDFJointTypes.h" //for UrdfMaterialColor cache
|
||||
|
||||
class btVector3;
|
||||
class btTransform;
|
||||
class btMultiBodyDynamicsWorld;
|
||||
class btTransform;
|
||||
|
||||
|
||||
class URDFImporterInterface;
|
||||
class MultiBodyCreationInterface;
|
||||
|
||||
|
||||
//manually sync with eURDF_Flags in SharedMemoryPublic.h!
|
||||
enum ConvertURDFFlags {
|
||||
CUF_USE_SDF = 1,
|
||||
// Use inertia values in URDF instead of recomputing them from collision shape.
|
||||
CUF_USE_URDF_INERTIA = 2,
|
||||
CUF_USE_MJCF = 4,
|
||||
CUF_USE_SELF_COLLISION=8,
|
||||
CUF_USE_SELF_COLLISION_EXCLUDE_PARENT=16,
|
||||
CUF_USE_SELF_COLLISION_EXCLUDE_ALL_PARENTS=32,
|
||||
CUF_RESERVED=64,
|
||||
CUF_USE_IMPLICIT_CYLINDER=128,
|
||||
CUF_GLOBAL_VELOCITIES_MB=256,
|
||||
CUF_MJCF_COLORS_FROM_FILE=512,
|
||||
CUF_ENABLE_CACHED_GRAPHICS_SHAPES = 1024,
|
||||
CUF_ENABLE_SLEEPING=2048,
|
||||
CUF_INITIALIZE_SAT_FEATURES=4096,
|
||||
CUF_USE_SELF_COLLISION_INCLUDE_PARENT = 8192,
|
||||
enum ConvertURDFFlags
|
||||
{
|
||||
CUF_USE_SDF = 1,
|
||||
// Use inertia values in URDF instead of recomputing them from collision shape.
|
||||
CUF_USE_URDF_INERTIA = 2,
|
||||
CUF_USE_MJCF = 4,
|
||||
CUF_USE_SELF_COLLISION = 8,
|
||||
CUF_USE_SELF_COLLISION_EXCLUDE_PARENT = 16,
|
||||
CUF_USE_SELF_COLLISION_EXCLUDE_ALL_PARENTS = 32,
|
||||
CUF_RESERVED = 64,
|
||||
CUF_USE_IMPLICIT_CYLINDER = 128,
|
||||
CUF_GLOBAL_VELOCITIES_MB = 256,
|
||||
CUF_MJCF_COLORS_FROM_FILE = 512,
|
||||
CUF_ENABLE_CACHED_GRAPHICS_SHAPES = 1024,
|
||||
CUF_ENABLE_SLEEPING = 2048,
|
||||
CUF_INITIALIZE_SAT_FEATURES = 4096,
|
||||
CUF_USE_SELF_COLLISION_INCLUDE_PARENT = 8192,
|
||||
};
|
||||
|
||||
struct UrdfVisualShapeCache
|
||||
@@ -40,17 +39,13 @@ struct UrdfVisualShapeCache
|
||||
btAlignedObjectArray<int> m_cachedUrdfLinkVisualShapeIndices;
|
||||
};
|
||||
|
||||
|
||||
void ConvertURDF2Bullet(const URDFImporterInterface& u2b,
|
||||
MultiBodyCreationInterface& creationCallback,
|
||||
const btTransform& rootTransformInWorldSpace,
|
||||
btMultiBodyDynamicsWorld* world,
|
||||
bool createMultiBody,
|
||||
const char* pathPrefix,
|
||||
int flags = 0,
|
||||
UrdfVisualShapeCache* cachedLinkGraphicsShapes= 0
|
||||
);
|
||||
|
||||
|
||||
#endif //_URDF2BULLET_H
|
||||
MultiBodyCreationInterface& creationCallback,
|
||||
const btTransform& rootTransformInWorldSpace,
|
||||
btMultiBodyDynamicsWorld* world,
|
||||
bool createMultiBody,
|
||||
const char* pathPrefix,
|
||||
int flags = 0,
|
||||
UrdfVisualShapeCache* cachedLinkGraphicsShapes = 0);
|
||||
|
||||
#endif //_URDF2BULLET_H
|
||||
|
||||
@@ -9,93 +9,88 @@
|
||||
|
||||
class URDFImporterInterface
|
||||
{
|
||||
|
||||
public:
|
||||
|
||||
|
||||
virtual ~URDFImporterInterface() {}
|
||||
|
||||
virtual bool loadURDF(const char* fileName, bool forceFixedBase = false)=0;
|
||||
|
||||
virtual bool loadSDF(const char* fileName, bool forceFixedBase = false) { return false;}
|
||||
virtual bool loadURDF(const char* fileName, bool forceFixedBase = false) = 0;
|
||||
|
||||
virtual const char* getPathPrefix()=0;
|
||||
|
||||
///return >=0 for the root link index, -1 if there is no root link
|
||||
virtual int getRootLinkIndex() const = 0;
|
||||
|
||||
///pure virtual interfaces, precondition is a valid linkIndex (you can assert/terminate if the linkIndex is out of range)
|
||||
virtual std::string getLinkName(int linkIndex) const =0;
|
||||
virtual bool loadSDF(const char* fileName, bool forceFixedBase = false) { return false; }
|
||||
|
||||
virtual const char* getPathPrefix() = 0;
|
||||
|
||||
///return >=0 for the root link index, -1 if there is no root link
|
||||
virtual int getRootLinkIndex() const = 0;
|
||||
|
||||
///pure virtual interfaces, precondition is a valid linkIndex (you can assert/terminate if the linkIndex is out of range)
|
||||
virtual std::string getLinkName(int linkIndex) const = 0;
|
||||
|
||||
//various derived class in internal source code break with new pure virtual methods, so provide some default implementation
|
||||
virtual std::string getBodyName() const
|
||||
{
|
||||
return "";
|
||||
}
|
||||
|
||||
/// optional method to provide the link color. return true if the color is available and copied into colorRGBA, return false otherwise
|
||||
virtual bool getLinkColor(int linkIndex, btVector4& colorRGBA) const { return false;}
|
||||
|
||||
virtual bool getLinkColor2(int linkIndex, struct UrdfMaterialColor& matCol) const { return false;}
|
||||
/// optional method to provide the link color. return true if the color is available and copied into colorRGBA, return false otherwise
|
||||
virtual bool getLinkColor(int linkIndex, btVector4& colorRGBA) const { return false; }
|
||||
|
||||
virtual bool getLinkColor2(int linkIndex, struct UrdfMaterialColor& matCol) const { return false; }
|
||||
virtual void setLinkColor2(int linkIndex, struct UrdfMaterialColor& matCol) const {}
|
||||
|
||||
virtual int getCollisionGroupAndMask(int linkIndex, int& colGroup, int& colMask) const { return 0;}
|
||||
virtual int getCollisionGroupAndMask(int linkIndex, int& colGroup, int& colMask) const { return 0; }
|
||||
///this API will likely change, don't override it!
|
||||
virtual bool getLinkContactInfo(int linkIndex, URDFLinkContactInfo& contactInfo ) const { return false;}
|
||||
|
||||
virtual bool getLinkAudioSource(int linkIndex, SDFAudioSource& audioSource) const { return false;}
|
||||
virtual bool getLinkContactInfo(int linkIndex, URDFLinkContactInfo& contactInfo) const { return false; }
|
||||
|
||||
virtual std::string getJointName(int linkIndex) const = 0;
|
||||
virtual bool getLinkAudioSource(int linkIndex, SDFAudioSource& audioSource) const { return false; }
|
||||
|
||||
//fill mass and inertial data. If inertial data is missing, please initialize mass, inertia to sensitive values, and inertialFrame to identity.
|
||||
virtual void getMassAndInertia (int urdfLinkIndex, btScalar& mass,btVector3& localInertiaDiagonal, btTransform& inertialFrame) const =0;
|
||||
virtual void getMassAndInertia2(int urdfLinkIndex, btScalar& mass, btVector3& localInertiaDiagonal, btTransform& inertialFrame, int flags) const
|
||||
virtual std::string getJointName(int linkIndex) const = 0;
|
||||
|
||||
//fill mass and inertial data. If inertial data is missing, please initialize mass, inertia to sensitive values, and inertialFrame to identity.
|
||||
virtual void getMassAndInertia(int urdfLinkIndex, btScalar& mass, btVector3& localInertiaDiagonal, btTransform& inertialFrame) const = 0;
|
||||
virtual void getMassAndInertia2(int urdfLinkIndex, btScalar& mass, btVector3& localInertiaDiagonal, btTransform& inertialFrame, int flags) const
|
||||
{
|
||||
getMassAndInertia(urdfLinkIndex, mass, localInertiaDiagonal, inertialFrame);
|
||||
}
|
||||
|
||||
///fill an array of child link indices for this link, btAlignedObjectArray behaves like a std::vector so just use push_back and resize(0) if needed
|
||||
virtual void getLinkChildIndices(int urdfLinkIndex, btAlignedObjectArray<int>& childLinkIndices) const =0;
|
||||
|
||||
virtual bool getJointInfo(int urdfLinkIndex, btTransform& parent2joint, btTransform& linkTransformInWorld, btVector3& jointAxisInJointSpace, int& jointType, btScalar& jointLowerLimit, btScalar& jointUpperLimit, btScalar& jointDamping, btScalar& jointFriction) const =0;
|
||||
///fill an array of child link indices for this link, btAlignedObjectArray behaves like a std::vector so just use push_back and resize(0) if needed
|
||||
virtual void getLinkChildIndices(int urdfLinkIndex, btAlignedObjectArray<int>& childLinkIndices) const = 0;
|
||||
|
||||
virtual bool getJointInfo2(int urdfLinkIndex, btTransform& parent2joint, btTransform& linkTransformInWorld, btVector3& jointAxisInJointSpace, int& jointType, btScalar& jointLowerLimit, btScalar& jointUpperLimit, btScalar& jointDamping, btScalar& jointFriction, btScalar& jointMaxForce, btScalar& jointMaxVelocity) const
|
||||
virtual bool getJointInfo(int urdfLinkIndex, btTransform& parent2joint, btTransform& linkTransformInWorld, btVector3& jointAxisInJointSpace, int& jointType, btScalar& jointLowerLimit, btScalar& jointUpperLimit, btScalar& jointDamping, btScalar& jointFriction) const = 0;
|
||||
|
||||
virtual bool getJointInfo2(int urdfLinkIndex, btTransform& parent2joint, btTransform& linkTransformInWorld, btVector3& jointAxisInJointSpace, int& jointType, btScalar& jointLowerLimit, btScalar& jointUpperLimit, btScalar& jointDamping, btScalar& jointFriction, btScalar& jointMaxForce, btScalar& jointMaxVelocity) const
|
||||
{
|
||||
//backwards compatibility for custom file importers
|
||||
jointMaxForce = 0;
|
||||
jointMaxVelocity = 0;
|
||||
return getJointInfo(urdfLinkIndex, parent2joint, linkTransformInWorld, jointAxisInJointSpace, jointType, jointLowerLimit, jointUpperLimit, jointDamping, jointFriction);
|
||||
};
|
||||
|
||||
virtual bool getRootTransformInWorld(btTransform& rootTransformInWorld) const =0;
|
||||
virtual void setRootTransformInWorld(const btTransform& rootTransformInWorld){}
|
||||
|
||||
virtual bool getRootTransformInWorld(btTransform& rootTransformInWorld) const = 0;
|
||||
virtual void setRootTransformInWorld(const btTransform& rootTransformInWorld) {}
|
||||
|
||||
///quick hack: need to rethink the API/dependencies of this
|
||||
virtual int convertLinkVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& inertialFrame) const { return -1;}
|
||||
|
||||
virtual void convertLinkVisualShapes2(int linkIndex, int urdfIndex, const char* pathPrefix, const btTransform& inertialFrame, class btCollisionObject* colObj, int objectIndex) const { }
|
||||
virtual void setBodyUniqueId(int bodyId) {}
|
||||
virtual int getBodyUniqueId() const { return 0;}
|
||||
virtual int convertLinkVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& inertialFrame) const { return -1; }
|
||||
|
||||
//default implementation for backward compatibility
|
||||
virtual class btCompoundShape* convertLinkCollisionShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame) const = 0;
|
||||
virtual void convertLinkVisualShapes2(int linkIndex, int urdfIndex, const char* pathPrefix, const btTransform& inertialFrame, class btCollisionObject* colObj, int objectIndex) const {}
|
||||
virtual void setBodyUniqueId(int bodyId) {}
|
||||
virtual int getBodyUniqueId() const { return 0; }
|
||||
|
||||
//default implementation for backward compatibility
|
||||
virtual class btCompoundShape* convertLinkCollisionShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame) const = 0;
|
||||
virtual int getUrdfFromCollisionShape(const class btCollisionShape* collisionShape, struct UrdfCollision& collision) const
|
||||
{
|
||||
return 0;
|
||||
}
|
||||
|
||||
virtual int getNumAllocatedCollisionShapes() const { return 0;}
|
||||
virtual class btCollisionShape* getAllocatedCollisionShape(int /*index*/ ) {return 0;}
|
||||
virtual int getNumModels() const {return 0;}
|
||||
virtual void activateModel(int /*modelIndex*/) { }
|
||||
virtual int getNumAllocatedMeshInterfaces() const { return 0;}
|
||||
virtual int getNumAllocatedCollisionShapes() const { return 0; }
|
||||
virtual class btCollisionShape* getAllocatedCollisionShape(int /*index*/) { return 0; }
|
||||
virtual int getNumModels() const { return 0; }
|
||||
virtual void activateModel(int /*modelIndex*/) {}
|
||||
virtual int getNumAllocatedMeshInterfaces() const { return 0; }
|
||||
|
||||
virtual int getNumAllocatedTextures() const { return 0; }
|
||||
virtual int getAllocatedTexture(int index) const { return 0; }
|
||||
|
||||
virtual class btStridingMeshInterface* getAllocatedMeshInterface(int index) {return 0;}
|
||||
|
||||
virtual class btStridingMeshInterface* getAllocatedMeshInterface(int index) { return 0; }
|
||||
};
|
||||
|
||||
#endif //URDF_IMPORTER_INTERFACE_H
|
||||
|
||||
#endif //URDF_IMPORTER_INTERFACE_H
|
||||
|
||||
@@ -5,52 +5,52 @@
|
||||
|
||||
enum UrdfJointTypes
|
||||
{
|
||||
URDFRevoluteJoint=1,
|
||||
URDFPrismaticJoint,
|
||||
URDFContinuousJoint,
|
||||
URDFFloatingJoint,
|
||||
URDFPlanarJoint,
|
||||
URDFFixedJoint,
|
||||
URDFRevoluteJoint = 1,
|
||||
URDFPrismaticJoint,
|
||||
URDFContinuousJoint,
|
||||
URDFFloatingJoint,
|
||||
URDFPlanarJoint,
|
||||
URDFFixedJoint,
|
||||
};
|
||||
|
||||
enum URDF_LinkContactFlags
|
||||
{
|
||||
URDF_CONTACT_HAS_LATERAL_FRICTION=1,
|
||||
URDF_CONTACT_HAS_INERTIA_SCALING=2,
|
||||
URDF_CONTACT_HAS_CONTACT_CFM=4,
|
||||
URDF_CONTACT_HAS_CONTACT_ERP=8,
|
||||
URDF_CONTACT_HAS_STIFFNESS_DAMPING=16,
|
||||
URDF_CONTACT_HAS_ROLLING_FRICTION=32,
|
||||
URDF_CONTACT_HAS_SPINNING_FRICTION=64,
|
||||
URDF_CONTACT_HAS_RESTITUTION=128,
|
||||
URDF_CONTACT_HAS_FRICTION_ANCHOR=256,
|
||||
|
||||
URDF_CONTACT_HAS_LATERAL_FRICTION = 1,
|
||||
URDF_CONTACT_HAS_INERTIA_SCALING = 2,
|
||||
URDF_CONTACT_HAS_CONTACT_CFM = 4,
|
||||
URDF_CONTACT_HAS_CONTACT_ERP = 8,
|
||||
URDF_CONTACT_HAS_STIFFNESS_DAMPING = 16,
|
||||
URDF_CONTACT_HAS_ROLLING_FRICTION = 32,
|
||||
URDF_CONTACT_HAS_SPINNING_FRICTION = 64,
|
||||
URDF_CONTACT_HAS_RESTITUTION = 128,
|
||||
URDF_CONTACT_HAS_FRICTION_ANCHOR = 256,
|
||||
|
||||
};
|
||||
|
||||
struct URDFLinkContactInfo
|
||||
{
|
||||
btScalar m_lateralFriction;
|
||||
btScalar m_rollingFriction;
|
||||
btScalar m_spinningFriction;
|
||||
btScalar m_spinningFriction;
|
||||
btScalar m_restitution;
|
||||
btScalar m_inertiaScaling;
|
||||
btScalar m_inertiaScaling;
|
||||
btScalar m_contactCfm;
|
||||
btScalar m_contactErp;
|
||||
btScalar m_contactStiffness;
|
||||
btScalar m_contactDamping;
|
||||
|
||||
|
||||
int m_flags;
|
||||
|
||||
URDFLinkContactInfo()
|
||||
:m_lateralFriction(0.5),
|
||||
m_rollingFriction(0),
|
||||
m_spinningFriction(0),
|
||||
m_restitution(0),
|
||||
m_inertiaScaling(1),
|
||||
m_contactCfm(0),
|
||||
m_contactErp(0),
|
||||
m_contactStiffness(1e4),
|
||||
m_contactDamping(1)
|
||||
: m_lateralFriction(0.5),
|
||||
m_rollingFriction(0),
|
||||
m_spinningFriction(0),
|
||||
m_restitution(0),
|
||||
m_inertiaScaling(1),
|
||||
m_contactCfm(0),
|
||||
m_contactErp(0),
|
||||
m_contactStiffness(1e4),
|
||||
m_contactDamping(1)
|
||||
{
|
||||
m_flags = URDF_CONTACT_HAS_LATERAL_FRICTION;
|
||||
}
|
||||
@@ -58,9 +58,9 @@ struct URDFLinkContactInfo
|
||||
|
||||
enum UrdfCollisionFlags
|
||||
{
|
||||
URDF_FORCE_CONCAVE_TRIMESH=1,
|
||||
URDF_HAS_COLLISION_GROUP=2,
|
||||
URDF_HAS_COLLISION_MASK=4,
|
||||
URDF_FORCE_CONCAVE_TRIMESH = 1,
|
||||
URDF_HAS_COLLISION_GROUP = 2,
|
||||
URDF_HAS_COLLISION_MASK = 4,
|
||||
};
|
||||
|
||||
struct UrdfMaterialColor
|
||||
@@ -68,10 +68,10 @@ struct UrdfMaterialColor
|
||||
btVector4 m_rgbaColor;
|
||||
btVector3 m_specularColor;
|
||||
UrdfMaterialColor()
|
||||
:m_rgbaColor(0.8, 0.8, 0.8, 1),
|
||||
m_specularColor(0.4,0.4,0.4)
|
||||
: m_rgbaColor(0.8, 0.8, 0.8, 1),
|
||||
m_specularColor(0.4, 0.4, 0.4)
|
||||
{
|
||||
}
|
||||
};
|
||||
|
||||
#endif //URDF_JOINT_TYPES_H
|
||||
#endif //URDF_JOINT_TYPES_H
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
@@ -12,14 +12,12 @@
|
||||
|
||||
struct ErrorLogger
|
||||
{
|
||||
virtual ~ErrorLogger(){}
|
||||
virtual void reportError(const char* error)=0;
|
||||
virtual void reportWarning(const char* warning)=0;
|
||||
virtual void printMessage(const char* msg)=0;
|
||||
virtual ~ErrorLogger() {}
|
||||
virtual void reportError(const char* error) = 0;
|
||||
virtual void reportWarning(const char* warning) = 0;
|
||||
virtual void printMessage(const char* msg) = 0;
|
||||
};
|
||||
|
||||
|
||||
|
||||
struct UrdfMaterial
|
||||
{
|
||||
std::string m_name;
|
||||
@@ -37,38 +35,37 @@ struct UrdfInertia
|
||||
bool m_hasLinkLocalFrame;
|
||||
|
||||
double m_mass;
|
||||
double m_ixx,m_ixy,m_ixz,m_iyy,m_iyz,m_izz;
|
||||
|
||||
double m_ixx, m_ixy, m_ixz, m_iyy, m_iyz, m_izz;
|
||||
|
||||
UrdfInertia()
|
||||
{
|
||||
m_hasLinkLocalFrame = false;
|
||||
m_linkLocalFrame.setIdentity();
|
||||
m_mass = 0.f;
|
||||
m_ixx=m_ixy=m_ixz=m_iyy=m_iyz=m_izz=0.f;
|
||||
m_ixx = m_ixy = m_ixz = m_iyy = m_iyz = m_izz = 0.f;
|
||||
}
|
||||
};
|
||||
|
||||
enum UrdfGeomTypes
|
||||
{
|
||||
URDF_GEOM_SPHERE=2,
|
||||
URDF_GEOM_SPHERE = 2,
|
||||
URDF_GEOM_BOX,
|
||||
URDF_GEOM_CYLINDER,
|
||||
URDF_GEOM_MESH,
|
||||
URDF_GEOM_PLANE,
|
||||
URDF_GEOM_CAPSULE, //non-standard URDF
|
||||
URDF_GEOM_CDF,//signed-distance-field, non-standard URDF
|
||||
URDF_GEOM_UNKNOWN,
|
||||
URDF_GEOM_CAPSULE, //non-standard URDF
|
||||
URDF_GEOM_CDF, //signed-distance-field, non-standard URDF
|
||||
URDF_GEOM_UNKNOWN,
|
||||
};
|
||||
|
||||
|
||||
struct UrdfGeometry
|
||||
{
|
||||
UrdfGeomTypes m_type;
|
||||
|
||||
|
||||
double m_sphereRadius;
|
||||
|
||||
|
||||
btVector3 m_boxSize;
|
||||
|
||||
|
||||
double m_capsuleRadius;
|
||||
double m_capsuleHeight;
|
||||
int m_hasFromTo;
|
||||
@@ -76,42 +73,42 @@ struct UrdfGeometry
|
||||
btVector3 m_capsuleTo;
|
||||
|
||||
btVector3 m_planeNormal;
|
||||
|
||||
enum {
|
||||
FILE_STL =1,
|
||||
FILE_COLLADA =2,
|
||||
FILE_OBJ =3,
|
||||
|
||||
enum
|
||||
{
|
||||
FILE_STL = 1,
|
||||
FILE_COLLADA = 2,
|
||||
FILE_OBJ = 3,
|
||||
FILE_CDF = 4,
|
||||
|
||||
};
|
||||
int m_meshFileType;
|
||||
int m_meshFileType;
|
||||
std::string m_meshFileName;
|
||||
btVector3 m_meshScale;
|
||||
btVector3 m_meshScale;
|
||||
|
||||
UrdfMaterial m_localMaterial;
|
||||
bool m_hasLocalMaterial;
|
||||
|
||||
UrdfGeometry()
|
||||
:m_type(URDF_GEOM_UNKNOWN),
|
||||
m_sphereRadius(1),
|
||||
m_boxSize(1,1,1),
|
||||
m_capsuleRadius(1),
|
||||
m_capsuleHeight(1),
|
||||
m_hasFromTo(0),
|
||||
m_capsuleFrom(0,1,0),
|
||||
m_capsuleTo(1,0,0),
|
||||
m_planeNormal(0,0,1),
|
||||
m_meshFileType(0),
|
||||
m_meshScale(1,1,1),
|
||||
m_hasLocalMaterial(false)
|
||||
: m_type(URDF_GEOM_UNKNOWN),
|
||||
m_sphereRadius(1),
|
||||
m_boxSize(1, 1, 1),
|
||||
m_capsuleRadius(1),
|
||||
m_capsuleHeight(1),
|
||||
m_hasFromTo(0),
|
||||
m_capsuleFrom(0, 1, 0),
|
||||
m_capsuleTo(1, 0, 0),
|
||||
m_planeNormal(0, 0, 1),
|
||||
m_meshFileType(0),
|
||||
m_meshScale(1, 1, 1),
|
||||
m_hasLocalMaterial(false)
|
||||
{
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
bool findExistingMeshFile(const std::string& urdf_path, std::string fn,
|
||||
const std::string& error_message_prefix,
|
||||
std::string* out_found_filename, int* out_type); // intended to fill UrdfGeometry::m_meshFileName and Type, but can be used elsewhere
|
||||
const std::string& error_message_prefix,
|
||||
std::string* out_found_filename, int* out_type); // intended to fill UrdfGeometry::m_meshFileName and Type, but can be used elsewhere
|
||||
|
||||
struct UrdfShape
|
||||
{
|
||||
@@ -121,18 +118,18 @@ struct UrdfShape
|
||||
std::string m_name;
|
||||
};
|
||||
|
||||
struct UrdfVisual: UrdfShape
|
||||
struct UrdfVisual : UrdfShape
|
||||
{
|
||||
std::string m_materialName;
|
||||
};
|
||||
|
||||
struct UrdfCollision: UrdfShape
|
||||
struct UrdfCollision : UrdfShape
|
||||
{
|
||||
int m_flags;
|
||||
int m_collisionGroup;
|
||||
int m_collisionMask;
|
||||
UrdfCollision()
|
||||
:m_flags(0)
|
||||
: m_flags(0)
|
||||
{
|
||||
}
|
||||
};
|
||||
@@ -141,30 +138,29 @@ struct UrdfJoint;
|
||||
|
||||
struct UrdfLink
|
||||
{
|
||||
std::string m_name;
|
||||
UrdfInertia m_inertia;
|
||||
btTransform m_linkTransformInWorld;
|
||||
std::string m_name;
|
||||
UrdfInertia m_inertia;
|
||||
btTransform m_linkTransformInWorld;
|
||||
btArray<UrdfVisual> m_visualArray;
|
||||
btArray<UrdfCollision> m_collisionArray;
|
||||
UrdfLink* m_parentLink;
|
||||
UrdfJoint* m_parentJoint;
|
||||
|
||||
|
||||
btArray<UrdfJoint*> m_childJoints;
|
||||
btArray<UrdfLink*> m_childLinks;
|
||||
|
||||
|
||||
int m_linkIndex;
|
||||
|
||||
|
||||
URDFLinkContactInfo m_contactInfo;
|
||||
|
||||
SDFAudioSource m_audioSource;
|
||||
|
||||
UrdfLink()
|
||||
:m_parentLink(0),
|
||||
m_parentJoint(0),
|
||||
m_linkIndex(-2)
|
||||
: m_parentLink(0),
|
||||
m_parentJoint(0),
|
||||
m_linkIndex(-2)
|
||||
{
|
||||
}
|
||||
|
||||
};
|
||||
struct UrdfJoint
|
||||
{
|
||||
@@ -174,22 +170,22 @@ struct UrdfJoint
|
||||
std::string m_parentLinkName;
|
||||
std::string m_childLinkName;
|
||||
btVector3 m_localJointAxis;
|
||||
|
||||
|
||||
double m_lowerLimit;
|
||||
double m_upperLimit;
|
||||
|
||||
|
||||
double m_effortLimit;
|
||||
double m_velocityLimit;
|
||||
|
||||
double m_jointDamping;
|
||||
double m_jointFriction;
|
||||
UrdfJoint()
|
||||
:m_lowerLimit(0),
|
||||
m_upperLimit(-1),
|
||||
m_effortLimit(0),
|
||||
m_velocityLimit(0),
|
||||
m_jointDamping(0),
|
||||
m_jointFriction(0)
|
||||
: m_lowerLimit(0),
|
||||
m_upperLimit(-1),
|
||||
m_effortLimit(0),
|
||||
m_velocityLimit(0),
|
||||
m_jointDamping(0),
|
||||
m_jointFriction(0)
|
||||
{
|
||||
}
|
||||
};
|
||||
@@ -198,16 +194,16 @@ struct UrdfModel
|
||||
{
|
||||
std::string m_name;
|
||||
std::string m_sourceFile;
|
||||
btTransform m_rootTransformInWorld;
|
||||
btTransform m_rootTransformInWorld;
|
||||
btHashMap<btHashString, UrdfMaterial*> m_materials;
|
||||
btHashMap<btHashString, UrdfLink*> m_links;
|
||||
btHashMap<btHashString, UrdfJoint*> m_joints;
|
||||
|
||||
|
||||
btArray<UrdfLink*> m_rootLinks;
|
||||
bool m_overrideFixedBase;
|
||||
|
||||
UrdfModel()
|
||||
:m_overrideFixedBase(false)
|
||||
: m_overrideFixedBase(false)
|
||||
{
|
||||
m_rootTransformInWorld.setIdentity();
|
||||
}
|
||||
@@ -246,99 +242,96 @@ struct UrdfModel
|
||||
|
||||
namespace tinyxml2
|
||||
{
|
||||
class XMLElement;
|
||||
class XMLElement;
|
||||
};
|
||||
|
||||
class UrdfParser
|
||||
{
|
||||
protected:
|
||||
|
||||
UrdfModel m_urdf2Model;
|
||||
btAlignedObjectArray<UrdfModel*> m_sdfModels;
|
||||
btAlignedObjectArray<UrdfModel*> m_tmpModels;
|
||||
|
||||
bool m_parseSDF;
|
||||
int m_activeSdfModel;
|
||||
UrdfModel m_urdf2Model;
|
||||
btAlignedObjectArray<UrdfModel*> m_sdfModels;
|
||||
btAlignedObjectArray<UrdfModel*> m_tmpModels;
|
||||
|
||||
bool m_parseSDF;
|
||||
int m_activeSdfModel;
|
||||
|
||||
btScalar m_urdfScaling;
|
||||
bool parseTransform(btTransform& tr, tinyxml2::XMLElement* xml, ErrorLogger* logger, bool parseSDF = false);
|
||||
bool parseTransform(btTransform& tr, tinyxml2::XMLElement* xml, ErrorLogger* logger, bool parseSDF = false);
|
||||
bool parseInertia(UrdfInertia& inertia, tinyxml2::XMLElement* config, ErrorLogger* logger);
|
||||
bool parseGeometry(UrdfGeometry& geom, tinyxml2::XMLElement* g, ErrorLogger* logger);
|
||||
bool parseVisual(UrdfModel& model, UrdfVisual& visual, tinyxml2::XMLElement* config, ErrorLogger* logger);
|
||||
bool parseCollision(UrdfCollision& collision, tinyxml2::XMLElement* config, ErrorLogger* logger);
|
||||
bool initTreeAndRoot(UrdfModel& model, ErrorLogger* logger);
|
||||
bool parseMaterial(UrdfMaterial& material, tinyxml2::XMLElement *config, ErrorLogger* logger);
|
||||
bool parseMaterial(UrdfMaterial& material, tinyxml2::XMLElement* config, ErrorLogger* logger);
|
||||
bool parseJointLimits(UrdfJoint& joint, tinyxml2::XMLElement* config, ErrorLogger* logger);
|
||||
bool parseJointDynamics(UrdfJoint& joint, tinyxml2::XMLElement* config, ErrorLogger* logger);
|
||||
bool parseJoint(UrdfJoint& joint, tinyxml2::XMLElement *config, ErrorLogger* logger);
|
||||
bool parseLink(UrdfModel& model, UrdfLink& link, tinyxml2::XMLElement *config, ErrorLogger* logger);
|
||||
|
||||
|
||||
bool parseJointDynamics(UrdfJoint& joint, tinyxml2::XMLElement* config, ErrorLogger* logger);
|
||||
bool parseJoint(UrdfJoint& joint, tinyxml2::XMLElement* config, ErrorLogger* logger);
|
||||
bool parseLink(UrdfModel& model, UrdfLink& link, tinyxml2::XMLElement* config, ErrorLogger* logger);
|
||||
|
||||
public:
|
||||
|
||||
UrdfParser();
|
||||
virtual ~UrdfParser();
|
||||
|
||||
void setParseSDF(bool useSDF)
|
||||
{
|
||||
m_parseSDF = useSDF;
|
||||
}
|
||||
bool getParseSDF() const
|
||||
{
|
||||
return m_parseSDF;
|
||||
}
|
||||
void setParseSDF(bool useSDF)
|
||||
{
|
||||
m_parseSDF = useSDF;
|
||||
}
|
||||
bool getParseSDF() const
|
||||
{
|
||||
return m_parseSDF;
|
||||
}
|
||||
void setGlobalScaling(btScalar scaling)
|
||||
{
|
||||
m_urdfScaling = scaling;
|
||||
}
|
||||
|
||||
bool loadUrdf(const char* urdfText, ErrorLogger* logger, bool forceFixedBase);
|
||||
bool loadSDF(const char* sdfText, ErrorLogger* logger);
|
||||
|
||||
int getNumModels() const
|
||||
{
|
||||
//user should have loaded an SDF when calling this method
|
||||
if (m_parseSDF)
|
||||
{
|
||||
return m_sdfModels.size();
|
||||
}
|
||||
return 1;
|
||||
}
|
||||
|
||||
void activateModel(int modelIndex);
|
||||
|
||||
UrdfModel& getModelByIndex(int index)
|
||||
{
|
||||
//user should have loaded an SDF when calling this method
|
||||
btAssert(m_parseSDF);
|
||||
bool loadUrdf(const char* urdfText, ErrorLogger* logger, bool forceFixedBase);
|
||||
bool loadSDF(const char* sdfText, ErrorLogger* logger);
|
||||
|
||||
return *m_sdfModels[index];
|
||||
}
|
||||
|
||||
const UrdfModel& getModelByIndex(int index) const
|
||||
{
|
||||
//user should have loaded an SDF when calling this method
|
||||
btAssert(m_parseSDF);
|
||||
|
||||
return *m_sdfModels[index];
|
||||
}
|
||||
|
||||
const UrdfModel& getModel() const
|
||||
int getNumModels() const
|
||||
{
|
||||
if (m_parseSDF)
|
||||
{
|
||||
return *m_sdfModels[m_activeSdfModel];
|
||||
}
|
||||
//user should have loaded an SDF when calling this method
|
||||
if (m_parseSDF)
|
||||
{
|
||||
return m_sdfModels.size();
|
||||
}
|
||||
return 1;
|
||||
}
|
||||
|
||||
void activateModel(int modelIndex);
|
||||
|
||||
UrdfModel& getModelByIndex(int index)
|
||||
{
|
||||
//user should have loaded an SDF when calling this method
|
||||
btAssert(m_parseSDF);
|
||||
|
||||
return *m_sdfModels[index];
|
||||
}
|
||||
|
||||
const UrdfModel& getModelByIndex(int index) const
|
||||
{
|
||||
//user should have loaded an SDF when calling this method
|
||||
btAssert(m_parseSDF);
|
||||
|
||||
return *m_sdfModels[index];
|
||||
}
|
||||
|
||||
const UrdfModel& getModel() const
|
||||
{
|
||||
if (m_parseSDF)
|
||||
{
|
||||
return *m_sdfModels[m_activeSdfModel];
|
||||
}
|
||||
|
||||
return m_urdf2Model;
|
||||
}
|
||||
|
||||
|
||||
UrdfModel& getModel()
|
||||
{
|
||||
if (m_parseSDF)
|
||||
{
|
||||
return *m_sdfModels[m_activeSdfModel];
|
||||
}
|
||||
{
|
||||
if (m_parseSDF)
|
||||
{
|
||||
return *m_sdfModels[m_activeSdfModel];
|
||||
}
|
||||
return m_urdf2Model;
|
||||
}
|
||||
|
||||
@@ -351,4 +344,3 @@ public:
|
||||
};
|
||||
|
||||
#endif
|
||||
|
||||
|
||||
@@ -7,93 +7,90 @@ struct UrdfModel;
|
||||
///btTransform is a position and 3x3 matrix, as defined in Bullet/src/LinearMath/btTransform
|
||||
class btTransform;
|
||||
|
||||
///UrdfRenderingInterface is a simple rendering interface, mainly for URDF-based robots.
|
||||
///There is an implementation in
|
||||
///UrdfRenderingInterface is a simple rendering interface, mainly for URDF-based robots.
|
||||
///There is an implementation in
|
||||
///bullet3\examples\SharedMemory\plugins\tinyRendererPlugin\TinyRendererVisualShapeConverter.cpp
|
||||
struct UrdfRenderingInterface
|
||||
{
|
||||
virtual ~UrdfRenderingInterface() {}
|
||||
{
|
||||
virtual ~UrdfRenderingInterface() {}
|
||||
///given a URDF link, convert all visual shapes into internal renderer (loading graphics meshes, textures etc)
|
||||
///use the collisionObjectUid as a unique identifier to synchronize the world transform and to remove the visual shape.
|
||||
virtual void convertVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame, const UrdfLink* linkPtr, const UrdfModel* model, int collisionObjectUid, int bodyUniqueId) =0;
|
||||
|
||||
virtual void convertVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame, const UrdfLink* linkPtr, const UrdfModel* model, int collisionObjectUid, int bodyUniqueId) = 0;
|
||||
|
||||
///remove a visual shapes, based on the shape unique id (shapeUid)
|
||||
virtual void removeVisualShape(int collisionObjectUid)=0;
|
||||
|
||||
virtual void removeVisualShape(int collisionObjectUid) = 0;
|
||||
|
||||
///update the world transform + scaling of the visual shape, using the shapeUid
|
||||
virtual void syncTransform(int collisionObjectUid, const class btTransform& worldTransform, const class btVector3& localScaling)=0;
|
||||
|
||||
virtual void syncTransform(int collisionObjectUid, const class btTransform& worldTransform, const class btVector3& localScaling) = 0;
|
||||
|
||||
///return the number of visual shapes, for a particular body unique id
|
||||
virtual int getNumVisualShapes(int bodyUniqueId)=0;
|
||||
|
||||
virtual int getNumVisualShapes(int bodyUniqueId) = 0;
|
||||
|
||||
///return the visual shape information, for a particular body unique id and 'shape index'
|
||||
virtual int getVisualShapesData(int bodyUniqueId, int shapeIndex, struct b3VisualShapeData* shapeData)=0;
|
||||
|
||||
virtual int getVisualShapesData(int bodyUniqueId, int shapeIndex, struct b3VisualShapeData* shapeData) = 0;
|
||||
|
||||
///change the RGBA color for some visual shape.
|
||||
virtual void changeRGBAColor(int bodyUniqueId, int linkIndex, int shapeIndex, const double rgbaColor[4])=0;
|
||||
virtual void changeRGBAColor(int bodyUniqueId, int linkIndex, int shapeIndex, const double rgbaColor[4]) = 0;
|
||||
|
||||
///select a given texture for some visual shape.
|
||||
virtual void changeShapeTexture(int objectUniqueId, int linkIndex, int shapeIndex, int textureUniqueId)=0;
|
||||
virtual void changeShapeTexture(int objectUniqueId, int linkIndex, int shapeIndex, int textureUniqueId) = 0;
|
||||
|
||||
///pick the up-axis, either Y (1) or Z (2)
|
||||
virtual void setUpAxis(int axis)=0;
|
||||
|
||||
///compute the view matrix based on those parameters
|
||||
virtual void resetCamera(float camDist, float yaw, float pitch, float camPosX,float camPosY, float camPosZ)=0;
|
||||
|
||||
///clear the render buffer with a particular color.
|
||||
virtual void clearBuffers(struct TGAColor& clearColor)=0;
|
||||
|
||||
///remove all visual shapes.
|
||||
virtual void resetAll()=0;
|
||||
|
||||
///return the frame buffer width and height for the renderer
|
||||
virtual void getWidthAndHeight(int& width, int& height)=0;
|
||||
|
||||
///set the frame buffer width and height for the renderer
|
||||
virtual void setWidthAndHeight(int width, int height)=0;
|
||||
|
||||
///set the light direction, in world coordinates
|
||||
virtual void setLightDirection(float x, float y, float z)=0;
|
||||
|
||||
///set the ambient light color, in world coordinates
|
||||
virtual void setLightColor(float x, float y, float z)=0;
|
||||
|
||||
///set the light distance
|
||||
virtual void setLightDistance(float dist)=0;
|
||||
|
||||
///set the light ambient coefficient
|
||||
virtual void setLightAmbientCoeff(float ambientCoeff)=0;
|
||||
|
||||
///set the light diffuse coefficient
|
||||
virtual void setLightDiffuseCoeff(float diffuseCoeff)=0;
|
||||
|
||||
///set the light specular coefficient
|
||||
virtual void setLightSpecularCoeff(float specularCoeff)=0;
|
||||
|
||||
///enable or disable rendering of shadows
|
||||
virtual void setShadow(bool hasShadow)=0;
|
||||
|
||||
///some undocumented flags for experimentation (todo: document)
|
||||
virtual void setFlags(int flags)=0;
|
||||
|
||||
///provide the image pixels as a part of a stream.
|
||||
virtual void copyCameraImageData(unsigned char* pixelsRGBA, int rgbaBufferSizeInPixels, float* depthBuffer, int depthBufferSizeInPixels,int* segmentationMaskBuffer, int segmentationMaskSizeInPixels, int startPixelIndex, int* widthPtr, int* heightPtr, int* numPixelsCopied)=0;
|
||||
|
||||
///render an image, using some arbitraty view and projection matrix
|
||||
virtual void render()=0;
|
||||
|
||||
///render an image using the provided view and projection matrix
|
||||
virtual void render(const float viewMat[16], const float projMat[16])=0;
|
||||
|
||||
///load a texture from file, in png or other popular/supported format
|
||||
virtual int loadTextureFile(const char* filename)=0;
|
||||
|
||||
///register a texture using an in-memory pixel buffer of a given width and height
|
||||
virtual int registerTexture(unsigned char* texels, int width, int height)=0;
|
||||
|
||||
|
||||
virtual void setUpAxis(int axis) = 0;
|
||||
|
||||
///compute the view matrix based on those parameters
|
||||
virtual void resetCamera(float camDist, float yaw, float pitch, float camPosX, float camPosY, float camPosZ) = 0;
|
||||
|
||||
///clear the render buffer with a particular color.
|
||||
virtual void clearBuffers(struct TGAColor& clearColor) = 0;
|
||||
|
||||
///remove all visual shapes.
|
||||
virtual void resetAll() = 0;
|
||||
|
||||
///return the frame buffer width and height for the renderer
|
||||
virtual void getWidthAndHeight(int& width, int& height) = 0;
|
||||
|
||||
///set the frame buffer width and height for the renderer
|
||||
virtual void setWidthAndHeight(int width, int height) = 0;
|
||||
|
||||
///set the light direction, in world coordinates
|
||||
virtual void setLightDirection(float x, float y, float z) = 0;
|
||||
|
||||
///set the ambient light color, in world coordinates
|
||||
virtual void setLightColor(float x, float y, float z) = 0;
|
||||
|
||||
///set the light distance
|
||||
virtual void setLightDistance(float dist) = 0;
|
||||
|
||||
///set the light ambient coefficient
|
||||
virtual void setLightAmbientCoeff(float ambientCoeff) = 0;
|
||||
|
||||
///set the light diffuse coefficient
|
||||
virtual void setLightDiffuseCoeff(float diffuseCoeff) = 0;
|
||||
|
||||
///set the light specular coefficient
|
||||
virtual void setLightSpecularCoeff(float specularCoeff) = 0;
|
||||
|
||||
///enable or disable rendering of shadows
|
||||
virtual void setShadow(bool hasShadow) = 0;
|
||||
|
||||
///some undocumented flags for experimentation (todo: document)
|
||||
virtual void setFlags(int flags) = 0;
|
||||
|
||||
///provide the image pixels as a part of a stream.
|
||||
virtual void copyCameraImageData(unsigned char* pixelsRGBA, int rgbaBufferSizeInPixels, float* depthBuffer, int depthBufferSizeInPixels, int* segmentationMaskBuffer, int segmentationMaskSizeInPixels, int startPixelIndex, int* widthPtr, int* heightPtr, int* numPixelsCopied) = 0;
|
||||
|
||||
///render an image, using some arbitraty view and projection matrix
|
||||
virtual void render() = 0;
|
||||
|
||||
///render an image using the provided view and projection matrix
|
||||
virtual void render(const float viewMat[16], const float projMat[16]) = 0;
|
||||
|
||||
///load a texture from file, in png or other popular/supported format
|
||||
virtual int loadTextureFile(const char* filename) = 0;
|
||||
|
||||
///register a texture using an in-memory pixel buffer of a given width and height
|
||||
virtual int registerTexture(unsigned char* texels, int width, int height) = 0;
|
||||
};
|
||||
|
||||
#endif //LINK_VISUAL_SHAPES_CONVERTER_H
|
||||
#endif //LINK_VISUAL_SHAPES_CONVERTER_H
|
||||
|
||||
@@ -3,15 +3,11 @@
|
||||
|
||||
#include <stdlib.h>
|
||||
|
||||
|
||||
template <typename T> T urdfLexicalCast(const char* txt)
|
||||
template <typename T>
|
||||
T urdfLexicalCast(const char* txt)
|
||||
{
|
||||
double result = atof(txt);
|
||||
return result;
|
||||
};
|
||||
|
||||
|
||||
|
||||
|
||||
#endif
|
||||
|
||||
|
||||
@@ -8,33 +8,29 @@
|
||||
|
||||
#include "urdfStringSplit.h"
|
||||
|
||||
void urdfStringSplit( btAlignedObjectArray<std::string>&pieces, const std::string& vector_str, const btAlignedObjectArray<std::string>& separators)
|
||||
void urdfStringSplit(btAlignedObjectArray<std::string> &pieces, const std::string &vector_str, const btAlignedObjectArray<std::string> &separators)
|
||||
{
|
||||
assert(separators.size() == 1);
|
||||
if (separators.size() == 1)
|
||||
{
|
||||
assert(separators.size()==1);
|
||||
if (separators.size()==1)
|
||||
{
|
||||
char** strArray = urdfStrSplit(vector_str.c_str(),separators[0].c_str());
|
||||
int numSubStr = urdfStrArrayLen(strArray);
|
||||
for (int i=0;i<numSubStr;i++)
|
||||
pieces.push_back(std::string(strArray[i]));
|
||||
urdfStrArrayFree(strArray);
|
||||
}
|
||||
char **strArray = urdfStrSplit(vector_str.c_str(), separators[0].c_str());
|
||||
int numSubStr = urdfStrArrayLen(strArray);
|
||||
for (int i = 0; i < numSubStr; i++)
|
||||
pieces.push_back(std::string(strArray[i]));
|
||||
urdfStrArrayFree(strArray);
|
||||
}
|
||||
void urdfIsAnyOf(const char* seps, btAlignedObjectArray<std::string>& strArray)
|
||||
}
|
||||
void urdfIsAnyOf(const char *seps, btAlignedObjectArray<std::string> &strArray)
|
||||
{
|
||||
int numSeps = strlen(seps);
|
||||
for (int i = 0; i < numSeps; i++)
|
||||
{
|
||||
|
||||
int numSeps = strlen(seps);
|
||||
for (int i=0;i<numSeps;i++)
|
||||
{
|
||||
char sep2[2] = {0,0};
|
||||
|
||||
sep2[0] = seps[i];
|
||||
strArray.push_back(sep2);
|
||||
}
|
||||
char sep2[2] = {0, 0};
|
||||
|
||||
sep2[0] = seps[i];
|
||||
strArray.push_back(sep2);
|
||||
}
|
||||
|
||||
|
||||
|
||||
}
|
||||
|
||||
/* Append an item to a dynamically allocated array of strings. On failure,
|
||||
return NULL, in which case the original array is intact. The item
|
||||
@@ -42,206 +38,219 @@ void urdfStringSplit( btAlignedObjectArray<std::string>&pieces, const std::strin
|
||||
array. Otherwise, extend the array. Make sure the array is always
|
||||
NULL-terminated. Input string might not be '\0'-terminated. */
|
||||
char **urdfStrArrayAppend(char **array, size_t nitems, const char *item,
|
||||
size_t itemlen)
|
||||
size_t itemlen)
|
||||
{
|
||||
/* Make a dynamic copy of the item. */
|
||||
char *copy;
|
||||
if (item == NULL)
|
||||
copy = NULL;
|
||||
else {
|
||||
copy = (char*)malloc(itemlen + 1);
|
||||
if (copy == NULL)
|
||||
return NULL;
|
||||
memcpy(copy, item, itemlen);
|
||||
copy[itemlen] = '\0';
|
||||
}
|
||||
|
||||
/* Extend array with one element. Except extend it by two elements,
|
||||
/* Make a dynamic copy of the item. */
|
||||
char *copy;
|
||||
if (item == NULL)
|
||||
copy = NULL;
|
||||
else
|
||||
{
|
||||
copy = (char *)malloc(itemlen + 1);
|
||||
if (copy == NULL)
|
||||
return NULL;
|
||||
memcpy(copy, item, itemlen);
|
||||
copy[itemlen] = '\0';
|
||||
}
|
||||
|
||||
/* Extend array with one element. Except extend it by two elements,
|
||||
in case it did not yet exist. This might mean it is a teeny bit
|
||||
too big, but we don't care. */
|
||||
array = (char**)realloc(array, (nitems + 2) * sizeof(array[0]));
|
||||
if (array == NULL) {
|
||||
free(copy);
|
||||
return NULL;
|
||||
}
|
||||
|
||||
/* Add copy of item to array, and return it. */
|
||||
array[nitems] = copy;
|
||||
array[nitems+1] = NULL;
|
||||
return array;
|
||||
}
|
||||
array = (char **)realloc(array, (nitems + 2) * sizeof(array[0]));
|
||||
if (array == NULL)
|
||||
{
|
||||
free(copy);
|
||||
return NULL;
|
||||
}
|
||||
|
||||
/* Add copy of item to array, and return it. */
|
||||
array[nitems] = copy;
|
||||
array[nitems + 1] = NULL;
|
||||
return array;
|
||||
}
|
||||
|
||||
/* Free a dynamic array of dynamic strings. */
|
||||
void urdfStrArrayFree(char **array)
|
||||
{
|
||||
if (array == NULL)
|
||||
return;
|
||||
for (size_t i = 0; array[i] != NULL; ++i)
|
||||
free(array[i]);
|
||||
free(array);
|
||||
if (array == NULL)
|
||||
return;
|
||||
for (size_t i = 0; array[i] != NULL; ++i)
|
||||
free(array[i]);
|
||||
free(array);
|
||||
}
|
||||
|
||||
|
||||
/* Split a string into substrings. Return dynamic array of dynamically
|
||||
allocated substrings, or NULL if there was an error. Caller is
|
||||
expected to free the memory, for example with str_array_free. */
|
||||
char **urdfStrSplit(const char *input, const char *sep)
|
||||
{
|
||||
size_t nitems = 0;
|
||||
char **array = NULL;
|
||||
const char *start = input;
|
||||
const char *next = strstr(start, sep);
|
||||
size_t seplen = strlen(sep);
|
||||
const char *item;
|
||||
size_t itemlen;
|
||||
|
||||
for (;;) {
|
||||
next = strstr(start, sep);
|
||||
if (next == NULL) {
|
||||
/* Add the remaining string (or empty string, if input ends with
|
||||
separator. */
|
||||
char **newstr = urdfStrArrayAppend(array, nitems, start, strlen(start));
|
||||
if (newstr == NULL) {
|
||||
urdfStrArrayFree(array);
|
||||
return NULL;
|
||||
}
|
||||
array = newstr;
|
||||
++nitems;
|
||||
break;
|
||||
} else if (next == input) {
|
||||
/* Input starts with separator. */
|
||||
item = "";
|
||||
itemlen = 0;
|
||||
} else {
|
||||
item = start;
|
||||
itemlen = next - item;
|
||||
}
|
||||
char **newstr = urdfStrArrayAppend(array, nitems, item, itemlen);
|
||||
if (newstr == NULL) {
|
||||
urdfStrArrayFree(array);
|
||||
return NULL;
|
||||
}
|
||||
array = newstr;
|
||||
++nitems;
|
||||
start = next + seplen;
|
||||
}
|
||||
|
||||
if (nitems == 0) {
|
||||
/* Input does not contain separator at all. */
|
||||
assert(array == NULL);
|
||||
array = urdfStrArrayAppend(array, nitems, input, strlen(input));
|
||||
}
|
||||
|
||||
return array;
|
||||
}
|
||||
size_t nitems = 0;
|
||||
char **array = NULL;
|
||||
const char *start = input;
|
||||
const char *next = strstr(start, sep);
|
||||
size_t seplen = strlen(sep);
|
||||
const char *item;
|
||||
size_t itemlen;
|
||||
|
||||
for (;;)
|
||||
{
|
||||
next = strstr(start, sep);
|
||||
if (next == NULL)
|
||||
{
|
||||
/* Add the remaining string (or empty string, if input ends with
|
||||
separator. */
|
||||
char **newstr = urdfStrArrayAppend(array, nitems, start, strlen(start));
|
||||
if (newstr == NULL)
|
||||
{
|
||||
urdfStrArrayFree(array);
|
||||
return NULL;
|
||||
}
|
||||
array = newstr;
|
||||
++nitems;
|
||||
break;
|
||||
}
|
||||
else if (next == input)
|
||||
{
|
||||
/* Input starts with separator. */
|
||||
item = "";
|
||||
itemlen = 0;
|
||||
}
|
||||
else
|
||||
{
|
||||
item = start;
|
||||
itemlen = next - item;
|
||||
}
|
||||
char **newstr = urdfStrArrayAppend(array, nitems, item, itemlen);
|
||||
if (newstr == NULL)
|
||||
{
|
||||
urdfStrArrayFree(array);
|
||||
return NULL;
|
||||
}
|
||||
array = newstr;
|
||||
++nitems;
|
||||
start = next + seplen;
|
||||
}
|
||||
|
||||
if (nitems == 0)
|
||||
{
|
||||
/* Input does not contain separator at all. */
|
||||
assert(array == NULL);
|
||||
array = urdfStrArrayAppend(array, nitems, input, strlen(input));
|
||||
}
|
||||
|
||||
return array;
|
||||
}
|
||||
|
||||
/* Return length of a NULL-delimited array of strings. */
|
||||
size_t urdfStrArrayLen(char **array)
|
||||
{
|
||||
size_t len;
|
||||
|
||||
for (len = 0; array[len] != NULL; ++len)
|
||||
continue;
|
||||
return len;
|
||||
size_t len;
|
||||
|
||||
for (len = 0; array[len] != NULL; ++len)
|
||||
continue;
|
||||
return len;
|
||||
}
|
||||
|
||||
#ifdef UNIT_TEST_STRING_SPLIT
|
||||
|
||||
#define MAX_OUTPUT 20
|
||||
|
||||
|
||||
int main(void)
|
||||
{
|
||||
struct {
|
||||
const char *input;
|
||||
const char *sep;
|
||||
char *output[MAX_OUTPUT];
|
||||
} tab[] = {
|
||||
/* Input is empty string. Output should be a list with an empty
|
||||
struct
|
||||
{
|
||||
const char *input;
|
||||
const char *sep;
|
||||
char *output[MAX_OUTPUT];
|
||||
} tab[] = {
|
||||
/* Input is empty string. Output should be a list with an empty
|
||||
string. */
|
||||
{
|
||||
"",
|
||||
"and",
|
||||
{
|
||||
"",
|
||||
NULL,
|
||||
},
|
||||
},
|
||||
/* Input is exactly the separator. Output should be two empty
|
||||
{
|
||||
"",
|
||||
"and",
|
||||
{
|
||||
"",
|
||||
NULL,
|
||||
},
|
||||
},
|
||||
/* Input is exactly the separator. Output should be two empty
|
||||
strings. */
|
||||
{
|
||||
"and",
|
||||
"and",
|
||||
{
|
||||
"",
|
||||
"",
|
||||
NULL,
|
||||
},
|
||||
},
|
||||
/* Input is non-empty, but does not have separator. Output should
|
||||
{
|
||||
"and",
|
||||
"and",
|
||||
{
|
||||
"",
|
||||
"",
|
||||
NULL,
|
||||
},
|
||||
},
|
||||
/* Input is non-empty, but does not have separator. Output should
|
||||
be the same string. */
|
||||
{
|
||||
"foo",
|
||||
"and",
|
||||
{
|
||||
"foo",
|
||||
NULL,
|
||||
},
|
||||
},
|
||||
/* Input is non-empty, and does have separator. */
|
||||
{
|
||||
"foo bar 1 and foo bar 2",
|
||||
" and ",
|
||||
{
|
||||
"foo bar 1",
|
||||
"foo bar 2",
|
||||
NULL,
|
||||
},
|
||||
},
|
||||
};
|
||||
const int tab_len = sizeof(tab) / sizeof(tab[0]);
|
||||
bool errors;
|
||||
|
||||
errors = false;
|
||||
|
||||
for (int i = 0; i < tab_len; ++i) {
|
||||
printf("test %d\n", i);
|
||||
|
||||
char **output = str_split(tab[i].input, tab[i].sep);
|
||||
if (output == NULL) {
|
||||
fprintf(stderr, "output is NULL\n");
|
||||
errors = true;
|
||||
break;
|
||||
}
|
||||
size_t num_output = str_array_len(output);
|
||||
printf("num_output %lu\n", (unsigned long) num_output);
|
||||
|
||||
size_t num_correct = str_array_len(tab[i].output);
|
||||
if (num_output != num_correct) {
|
||||
fprintf(stderr, "wrong number of outputs (%lu, not %lu)\n",
|
||||
(unsigned long) num_output, (unsigned long) num_correct);
|
||||
errors = true;
|
||||
} else {
|
||||
for (size_t j = 0; j < num_output; ++j) {
|
||||
if (strcmp(tab[i].output[j], output[j]) != 0) {
|
||||
fprintf(stderr, "output[%lu] is '%s' not '%s'\n",
|
||||
(unsigned long) j, output[j], tab[i].output[j]);
|
||||
errors = true;
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
str_array_free(output);
|
||||
printf("\n");
|
||||
}
|
||||
|
||||
if (errors)
|
||||
return EXIT_FAILURE;
|
||||
return 0;
|
||||
{
|
||||
"foo",
|
||||
"and",
|
||||
{
|
||||
"foo",
|
||||
NULL,
|
||||
},
|
||||
},
|
||||
/* Input is non-empty, and does have separator. */
|
||||
{
|
||||
"foo bar 1 and foo bar 2",
|
||||
" and ",
|
||||
{
|
||||
"foo bar 1",
|
||||
"foo bar 2",
|
||||
NULL,
|
||||
},
|
||||
},
|
||||
};
|
||||
const int tab_len = sizeof(tab) / sizeof(tab[0]);
|
||||
bool errors;
|
||||
|
||||
errors = false;
|
||||
|
||||
for (int i = 0; i < tab_len; ++i)
|
||||
{
|
||||
printf("test %d\n", i);
|
||||
|
||||
char **output = str_split(tab[i].input, tab[i].sep);
|
||||
if (output == NULL)
|
||||
{
|
||||
fprintf(stderr, "output is NULL\n");
|
||||
errors = true;
|
||||
break;
|
||||
}
|
||||
size_t num_output = str_array_len(output);
|
||||
printf("num_output %lu\n", (unsigned long)num_output);
|
||||
|
||||
size_t num_correct = str_array_len(tab[i].output);
|
||||
if (num_output != num_correct)
|
||||
{
|
||||
fprintf(stderr, "wrong number of outputs (%lu, not %lu)\n",
|
||||
(unsigned long)num_output, (unsigned long)num_correct);
|
||||
errors = true;
|
||||
}
|
||||
else
|
||||
{
|
||||
for (size_t j = 0; j < num_output; ++j)
|
||||
{
|
||||
if (strcmp(tab[i].output[j], output[j]) != 0)
|
||||
{
|
||||
fprintf(stderr, "output[%lu] is '%s' not '%s'\n",
|
||||
(unsigned long)j, output[j], tab[i].output[j]);
|
||||
errors = true;
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
str_array_free(output);
|
||||
printf("\n");
|
||||
}
|
||||
|
||||
if (errors)
|
||||
return EXIT_FAILURE;
|
||||
return 0;
|
||||
}
|
||||
#endif//UNIT_TEST_STRING_SPLIT
|
||||
|
||||
|
||||
#endif //UNIT_TEST_STRING_SPLIT
|
||||
|
||||
@@ -8,33 +8,32 @@
|
||||
#include <string>
|
||||
#include "LinearMath/btAlignedObjectArray.h"
|
||||
|
||||
void urdfStringSplit( btAlignedObjectArray<std::string>&pieces, const std::string& vector_str, const btAlignedObjectArray<std::string>& separators);
|
||||
void urdfStringSplit(btAlignedObjectArray<std::string>& pieces, const std::string& vector_str, const btAlignedObjectArray<std::string>& separators);
|
||||
|
||||
void urdfIsAnyOf(const char* seps, btAlignedObjectArray<std::string>& strArray);
|
||||
#endif
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
extern "C"
|
||||
{
|
||||
#endif
|
||||
|
||||
///The string split C code is by Lars Wirzenius
|
||||
///See http://stackoverflow.com/questions/2531605/how-to-split-a-string-with-a-delimiter-larger-than-one-single-char
|
||||
///The string split C code is by Lars Wirzenius
|
||||
///See http://stackoverflow.com/questions/2531605/how-to-split-a-string-with-a-delimiter-larger-than-one-single-char
|
||||
|
||||
|
||||
/* Split a string into substrings. Return dynamic array of dynamically
|
||||
/* Split a string into substrings. Return dynamic array of dynamically
|
||||
allocated substrings, or NULL if there was an error. Caller is
|
||||
expected to free the memory, for example with str_array_free. */
|
||||
char** urdfStrSplit(const char* input, const char* sep);
|
||||
char** urdfStrSplit(const char* input, const char* sep);
|
||||
|
||||
/* Free a dynamic array of dynamic strings. */
|
||||
void urdfStrArrayFree(char** array);
|
||||
/* Free a dynamic array of dynamic strings. */
|
||||
void urdfStrArrayFree(char** array);
|
||||
|
||||
/* Return length of a NULL-delimited array of strings. */
|
||||
size_t urdfStrArrayLen(char** array);
|
||||
/* Return length of a NULL-delimited array of strings. */
|
||||
size_t urdfStrArrayLen(char** array);
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif //STRING_SPLIT_H
|
||||
|
||||
#endif //STRING_SPLIT_H
|
||||
|
||||
@@ -3,29 +3,28 @@
|
||||
|
||||
#define MSTRINGIFY(A) #A
|
||||
|
||||
|
||||
const char* urdf_char2 = 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>);
|
||||
<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_char1 = MSTRINGIFY(
|
||||
<?xml version="1.0"?>
|
||||
@@ -821,5 +820,4 @@ const char* urdf_char = MSTRINGIFY(
|
||||
|
||||
);
|
||||
|
||||
#endif //URDF_SAMPLES_H
|
||||
|
||||
#endif //URDF_SAMPLES_H
|
||||
|
||||
Reference in New Issue
Block a user