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

Merge pull request #744 from erwincoumans/master

Browse Source 
Fix some inconsistencies in URDF file handling between btRigidBody an…
This commit is contained in:
erwincoumans
2016-08-18 10:10:52 -07:00
committed by GitHub
parent 8a780fa0a4 6005e54aa1
commit d6547f6ee4
3 changed files with 29 additions and 16 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -36,7 +36,8 @@ class ImportUrdfSetup : public CommonMultiBodyBase
struct ImportUrdfInternalData* m_data; struct ImportUrdfInternalData* m_data;
bool m_useMultiBody; bool m_useMultiBody;
btAlignedObjectArray<std::string* > m_nameMemory; btAlignedObjectArray<std::string* > m_nameMemory;
btScalar m_grav;
int m_upAxis;
public: public:
ImportUrdfSetup(struct GUIHelperInterface* helper, int option, const char* fileName); ImportUrdfSetup(struct GUIHelperInterface* helper, int option, const char* fileName);
virtual ~ImportUrdfSetup(); virtual ~ImportUrdfSetup();
@@ -87,7 +88,9 @@ struct ImportUrdfInternalData
ImportUrdfSetup::ImportUrdfSetup(struct GUIHelperInterface* helper, int option, const char* fileName) ImportUrdfSetup::ImportUrdfSetup(struct GUIHelperInterface* helper, int option, const char* fileName)
:CommonMultiBodyBase(helper) :CommonMultiBodyBase(helper),
m_grav(0),
m_upAxis(2)
{ {
m_data = new ImportUrdfInternalData; m_data = new ImportUrdfInternalData;
@@ -186,9 +189,9 @@ void ImportUrdfSetup::setFileName(const char* urdfFileName)
void ImportUrdfSetup::initPhysics() void ImportUrdfSetup::initPhysics()
{ {
int upAxis = 2;
m_guiHelper->setUpAxis(upAxis); m_guiHelper->setUpAxis(m_upAxis);
this->createEmptyDynamicsWorld(); this->createEmptyDynamicsWorld();
//m_dynamicsWorld->getSolverInfo().m_numIterations = 100; //m_dynamicsWorld->getSolverInfo().m_numIterations = 100;
m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld); m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
@@ -199,10 +202,14 @@ void ImportUrdfSetup::initPhysics()
);//+btIDebugDraw::DBG_DrawConstraintLimits); );//+btIDebugDraw::DBG_DrawConstraintLimits);
btVector3 gravity(0,0,0); if (m_guiHelper->getParameterInterface())
gravity[upAxis]=-9.8; {
SliderParams slider("Gravity", &m_grav);
m_dynamicsWorld->setGravity(gravity); slider.m_minVal = -10;
slider.m_maxVal = 10;
m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
}
BulletURDFImporter u2b(m_guiHelper, 0); BulletURDFImporter u2b(m_guiHelper, 0);
@@ -350,7 +357,7 @@ void ImportUrdfSetup::initPhysics()
if (createGround) if (createGround)
{ {
btVector3 groundHalfExtents(20,20,20); btVector3 groundHalfExtents(20,20,20);
groundHalfExtents[upAxis]=1.f; groundHalfExtents[m_upAxis]=1.f;
btBoxShape* box = new btBoxShape(groundHalfExtents); btBoxShape* box = new btBoxShape(groundHalfExtents);
m_collisionShapes.push_back(box); m_collisionShapes.push_back(box);
box->initializePolyhedralFeatures(); box->initializePolyhedralFeatures();
@@ -358,7 +365,7 @@ void ImportUrdfSetup::initPhysics()
m_guiHelper->createCollisionShapeGraphicsObject(box); m_guiHelper->createCollisionShapeGraphicsObject(box);
btTransform start; start.setIdentity(); btTransform start; start.setIdentity();
btVector3 groundOrigin(0,0,0); btVector3 groundOrigin(0,0,0);
groundOrigin[upAxis]=-2.5; groundOrigin[m_upAxis]=-2.5;
start.setOrigin(groundOrigin); start.setOrigin(groundOrigin);
btRigidBody* body = createRigidBody(0,start,box); btRigidBody* body = createRigidBody(0,start,box);
//m_dynamicsWorld->removeRigidBody(body); //m_dynamicsWorld->removeRigidBody(body);
@@ -388,6 +395,10 @@ void ImportUrdfSetup::stepSimulation(float deltaTime)
{ {
if (m_dynamicsWorld) if (m_dynamicsWorld)
{ {
btVector3 gravity(0, 0, 0);
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]) if (m_data->m_jointMotors[i])

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

@@ -299,7 +299,7 @@ void ConvertURDF2BulletInternal(const URDFImporterInterface& u2b, MultiBodyCreat
{ {
//b3Printf("Fixed joint\n"); //b3Printf("Fixed joint\n");
btGeneric6DofSpring2Constraint* dof6 = creation.createFixedJoint(urdfLinkIndex,*parentRigidBody, *linkRigidBody, offsetInA, offsetInB); btGeneric6DofSpring2Constraint* dof6 = creation.createFixedJoint(urdfLinkIndex,*linkRigidBody, *parentRigidBody, offsetInB, offsetInA);
if (enableConstraints) if (enableConstraints)
world1->addConstraint(dof6,true); world1->addConstraint(dof6,true);
@@ -324,7 +324,7 @@ void ConvertURDF2BulletInternal(const URDFImporterInterface& u2b, MultiBodyCreat
} else } else
{ {
btGeneric6DofSpring2Constraint* dof6 = creation.createRevoluteJoint(urdfLinkIndex,*parentRigidBody, *linkRigidBody, offsetInA, offsetInB,jointAxisInJointSpace,jointLowerLimit, jointUpperLimit); btGeneric6DofSpring2Constraint* dof6 = creation.createRevoluteJoint(urdfLinkIndex,*linkRigidBody, *parentRigidBody, offsetInB, offsetInA,jointAxisInJointSpace,jointLowerLimit, jointUpperLimit);
if (enableConstraints) if (enableConstraints)
world1->addConstraint(dof6,true); world1->addConstraint(dof6,true);
@@ -350,7 +350,7 @@ void ConvertURDF2BulletInternal(const URDFImporterInterface& u2b, MultiBodyCreat
} else } else
{ {
btGeneric6DofSpring2Constraint* dof6 = creation.createPrismaticJoint(urdfLinkIndex,*parentRigidBody, *linkRigidBody, offsetInA, offsetInB,jointAxisInJointSpace,jointLowerLimit,jointUpperLimit); btGeneric6DofSpring2Constraint* dof6 = creation.createPrismaticJoint(urdfLinkIndex,*linkRigidBody, *parentRigidBody, offsetInB, offsetInA,jointAxisInJointSpace,jointLowerLimit,jointUpperLimit);
if (enableConstraints) if (enableConstraints)
@@ -455,7 +455,9 @@ void ConvertURDF2Bullet(const URDFImporterInterface& u2b, MultiBodyCreationInter
mb->setHasSelfCollision(false); mb->setHasSelfCollision(false);
mb->finalizeMultiDof(); mb->finalizeMultiDof();
mb->setBaseWorldTransform(rootTransformInWorldSpace); btTransform localInertialFrameRoot = cache.m_urdfLinkLocalInertialFrames[urdfLinkIndex];
mb->setBaseWorldTransform(rootTransformInWorldSpace*localInertialFrameRoot);
btAlignedObjectArray<btQuaternion> scratch_q; btAlignedObjectArray<btQuaternion> scratch_q;
btAlignedObjectArray<btVector3> scratch_m; btAlignedObjectArray<btVector3> scratch_m;
mb->forwardKinematics(scratch_q,scratch_m); mb->forwardKinematics(scratch_q,scratch_m);

2
examples/SharedMemory/PhysicsClientExample.cpp
View File

@@ -239,7 +239,7 @@ void PhysicsClientExample::prepareAndSubmitCommand(int commandId)
b3SharedMemoryCommandHandle commandHandle = b3LoadUrdfCommandInit(m_physicsClientHandle, "kuka_iiwa/model.urdf"); b3SharedMemoryCommandHandle commandHandle = b3LoadUrdfCommandInit(m_physicsClientHandle, "kuka_iiwa/model.urdf");
//setting the initial position, orientation and other arguments are optional //setting the initial position, orientation and other arguments are optional
double startPosX = 0; double startPosX = 0;
static double startPosY = 1; static double startPosY = 0;
double startPosZ = 0; double startPosZ = 0;
b3LoadUrdfCommandSetStartPosition(commandHandle, startPosX,startPosY,startPosZ); b3LoadUrdfCommandSetStartPosition(commandHandle, startPosX,startPosY,startPosZ);
startPosY += 2.f; startPosY += 2.f;