mirror of
https://github.com/bulletphysics/bullet3
synced 2025-01-07 08:10:08 +00:00
prepare to create coordinate frame demo, minor cleanup for create funcs in demo entries,
fix r2d2.urdf inertia
This commit is contained in:
erwin coumans
parent
cf2b4e03b5
commit
122fabac87
@ -4,6 +4,9 @@
|
||||
|
||||
#include "Bullet3AppSupport/BulletDemoInterface.h"
|
||||
#include "../bullet2/BasicDemo/BasicDemo.h"
|
||||
|
||||
#include "../bullet2/CoordinateFramesDemo/CoordinateFrameDemoPhysicsSetup.h"
|
||||
|
||||
#include "../bullet2/BasicDemo/HingeDemo.h"
|
||||
#include "../bullet2/BasicDemo/HingeDemo.h"
|
||||
|
||||
@ -29,59 +32,33 @@
|
||||
#include "../../Demos3/FiniteElementMethod/FiniteElementDemo.h"
|
||||
//#include "../../Demos3/bullet2/SoftDemo/SoftDemo.h"
|
||||
|
||||
static BulletDemoInterface* TestJointTorqueCreateFunc(CommonGraphicsApp* app)
|
||||
{
|
||||
CommonPhysicsSetup* physicsSetup = new TestJointTorqueSetup();
|
||||
return new BasicDemo(app, physicsSetup);
|
||||
}
|
||||
static BulletDemoInterface* MultiBodyVehicleCreateFunc(CommonGraphicsApp* app)
|
||||
{
|
||||
CommonPhysicsSetup* physicsSetup = new MultiBodyVehicleSetup();
|
||||
return new BasicDemo(app, physicsSetup);
|
||||
}
|
||||
static BulletDemoInterface* LuaDemoCreateFunc(CommonGraphicsApp* app)
|
||||
{
|
||||
CommonPhysicsSetup* physicsSetup = new LuaPhysicsSetup(app);
|
||||
return new BasicDemo(app, physicsSetup);
|
||||
|
||||
#define MYCREATEFUNC(func) \
|
||||
static BulletDemoInterface* func##CreateFunc(CommonGraphicsApp* app)\
|
||||
{\
|
||||
CommonPhysicsSetup* physicsSetup = new func##Setup();\
|
||||
return new BasicDemo(app, physicsSetup);\
|
||||
}
|
||||
|
||||
static BulletDemoInterface* MyCcdPhysicsDemoCreateFunc(CommonGraphicsApp* app)
|
||||
{
|
||||
CommonPhysicsSetup* physicsSetup = new CcdPhysicsSetup();
|
||||
return new BasicDemo(app, physicsSetup);
|
||||
#define MYCREATEFUNC2(func,setup) \
|
||||
static BulletDemoInterface* func(CommonGraphicsApp* app)\
|
||||
{\
|
||||
CommonPhysicsSetup* physicsSetup = new setup(app);\
|
||||
return new BasicDemo(app, physicsSetup);\
|
||||
}
|
||||
|
||||
static BulletDemoInterface* MyKinematicObjectCreateFunc(CommonGraphicsApp* app)
|
||||
{
|
||||
CommonPhysicsSetup* physicsSetup = new KinematicObjectSetup();
|
||||
return new BasicDemo(app, physicsSetup);
|
||||
}
|
||||
static BulletDemoInterface* MySerializeCreateFunc(CommonGraphicsApp* app)
|
||||
{
|
||||
CommonPhysicsSetup* physicsSetup = new SerializeSetup();
|
||||
return new BasicDemo(app, physicsSetup);
|
||||
}
|
||||
static BulletDemoInterface* MyConstraintCreateFunc(CommonGraphicsApp* app)
|
||||
{
|
||||
CommonPhysicsSetup* physicsSetup = new ConstraintPhysicsSetup();
|
||||
return new BasicDemo(app, physicsSetup);
|
||||
}
|
||||
MYCREATEFUNC(TestJointTorque);
|
||||
MYCREATEFUNC(MultiBodyVehicle);
|
||||
MYCREATEFUNC2(LuaDemoCreateFunc,LuaPhysicsSetup);
|
||||
MYCREATEFUNC(Serialize);
|
||||
MYCREATEFUNC(CcdPhysics);
|
||||
MYCREATEFUNC(KinematicObject);
|
||||
MYCREATEFUNC(ConstraintPhysics);
|
||||
MYCREATEFUNC(ImportUrdf);
|
||||
MYCREATEFUNC2(ImportObjCreateFunc,ImportObjSetup);
|
||||
MYCREATEFUNC2(ImportSTLCreateFunc,ImportSTLSetup);
|
||||
MYCREATEFUNC(CoordinateFrameDemoPhysics);
|
||||
|
||||
static BulletDemoInterface* MyImportUrdfCreateFunc(CommonGraphicsApp* app)
|
||||
{
|
||||
CommonPhysicsSetup* physicsSetup = new ImportUrdfDemo();
|
||||
return new BasicDemo(app, physicsSetup);
|
||||
}
|
||||
static BulletDemoInterface* MyImportObjCreateFunc(CommonGraphicsApp* app)
|
||||
{
|
||||
CommonPhysicsSetup* physicsSetup = new ImportObjDemo(app);
|
||||
return new BasicDemo(app, physicsSetup);
|
||||
}
|
||||
static BulletDemoInterface* MyImportSTLCreateFunc(CommonGraphicsApp* app)
|
||||
{
|
||||
CommonPhysicsSetup* physicsSetup = new ImportSTLDemo(app);
|
||||
return new BasicDemo(app, physicsSetup);
|
||||
}
|
||||
|
||||
static BulletDemoInterface* MyImportColladaCreateFunc(CommonGraphicsApp* app)
|
||||
{
|
||||
@ -92,7 +69,6 @@ static BulletDemoInterface* MyImportColladaCreateFunc(CommonGraphicsApp* app)
|
||||
|
||||
|
||||
|
||||
|
||||
struct BulletDemoEntry
|
||||
{
|
||||
int m_menuLevel;
|
||||
@ -107,21 +83,22 @@ static BulletDemoEntry allDemos[]=
|
||||
{0,"Basic Concepts",0},
|
||||
{1,"Basis Frame", &CoordinateSystemDemo::CreateFunc},
|
||||
{1,"SupportFunc", &MySupportFuncDemo::CreateFunc},
|
||||
{1,"Coordinate Frames", CoordinateFrameDemoPhysicsCreateFunc},
|
||||
//{"emptydemo",EmptyBulletDemo::MyCreateFunc},
|
||||
{0,"API Demos", 0},
|
||||
|
||||
{1,"BasicDemo",BasicDemo::MyCreateFunc},
|
||||
{ 1, "CcdDemo", MyCcdPhysicsDemoCreateFunc },
|
||||
{ 1, "Kinematic", MyKinematicObjectCreateFunc },
|
||||
{ 1, "Constraints", MyConstraintCreateFunc },
|
||||
{ 1, "CcdDemo", CcdPhysicsCreateFunc },
|
||||
{ 1, "Kinematic", KinematicObjectCreateFunc },
|
||||
{ 1, "Constraints", ConstraintPhysicsCreateFunc },
|
||||
{ 1, "LuaDemo",LuaDemoCreateFunc},
|
||||
|
||||
{0,"File Formats", 0},
|
||||
|
||||
{ 1, ".bullet",MySerializeCreateFunc},
|
||||
{ 1, "Wavefront Obj", MyImportObjCreateFunc},
|
||||
{ 1, "URDF", MyImportUrdfCreateFunc },
|
||||
{ 1, "STL", MyImportSTLCreateFunc},
|
||||
{ 1, ".bullet",SerializeCreateFunc},
|
||||
{ 1, "Wavefront Obj", ImportObjCreateFunc},
|
||||
{ 1, "URDF", ImportUrdfCreateFunc },
|
||||
{ 1, "STL", ImportSTLCreateFunc},
|
||||
{ 1, "COLLADA", MyImportColladaCreateFunc},
|
||||
{0,"Experiments", 0},
|
||||
{1, "Finite Element Demo", FiniteElementDemo::CreateFunc},
|
||||
|
||||
@ -321,7 +321,7 @@ void openURDFDemo(const char* filename)
|
||||
|
||||
s_parameterInterface->removeAllParameters();
|
||||
|
||||
ImportUrdfDemo* physicsSetup = new ImportUrdfDemo();
|
||||
ImportUrdfSetup* physicsSetup = new ImportUrdfSetup();
|
||||
physicsSetup->setFileName(filename);
|
||||
|
||||
sCurrentDemo = new BasicDemo(app, physicsSetup);
|
||||
|
||||
@ -7,13 +7,13 @@
|
||||
#include "OpenGLWindow/SimpleOpenGL3App.h"
|
||||
#include "Wavefront2GLInstanceGraphicsShape.h"
|
||||
|
||||
ImportObjDemo::ImportObjDemo(CommonGraphicsApp* app)
|
||||
ImportObjSetup::ImportObjSetup(CommonGraphicsApp* app)
|
||||
:m_app(app)
|
||||
{
|
||||
|
||||
}
|
||||
|
||||
ImportObjDemo::~ImportObjDemo()
|
||||
ImportObjSetup::~ImportObjSetup()
|
||||
{
|
||||
|
||||
}
|
||||
@ -24,7 +24,7 @@ ImportObjDemo::~ImportObjDemo()
|
||||
|
||||
|
||||
|
||||
void ImportObjDemo::initPhysics(GraphicsPhysicsBridge& gfxBridge)
|
||||
void ImportObjSetup::initPhysics(GraphicsPhysicsBridge& gfxBridge)
|
||||
{
|
||||
gfxBridge.setUpAxis(2);
|
||||
this->createEmptyDynamicsWorld();
|
||||
|
||||
@ -4,12 +4,12 @@
|
||||
|
||||
#include "Bullet3AppSupport/CommonRigidBodySetup.h"
|
||||
|
||||
class ImportObjDemo : public CommonRigidBodySetup
|
||||
class ImportObjSetup : public CommonRigidBodySetup
|
||||
{
|
||||
struct CommonGraphicsApp* m_app;
|
||||
public:
|
||||
ImportObjDemo(CommonGraphicsApp* app);
|
||||
virtual ~ImportObjDemo();
|
||||
ImportObjSetup(CommonGraphicsApp* app);
|
||||
virtual ~ImportObjSetup();
|
||||
|
||||
virtual void initPhysics(GraphicsPhysicsBridge& gfxBridge);
|
||||
};
|
||||
|
||||
@ -6,20 +6,20 @@
|
||||
#include "OpenGLWindow/SimpleOpenGL3App.h"
|
||||
#include "LoadMeshFromSTL.h"
|
||||
|
||||
ImportSTLDemo::ImportSTLDemo(CommonGraphicsApp* app)
|
||||
ImportSTLSetup::ImportSTLSetup(CommonGraphicsApp* app)
|
||||
:m_app(app)
|
||||
{
|
||||
|
||||
}
|
||||
|
||||
ImportSTLDemo::~ImportSTLDemo()
|
||||
ImportSTLSetup::~ImportSTLSetup()
|
||||
{
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
void ImportSTLDemo::initPhysics(GraphicsPhysicsBridge& gfxBridge)
|
||||
void ImportSTLSetup::initPhysics(GraphicsPhysicsBridge& gfxBridge)
|
||||
{
|
||||
gfxBridge.setUpAxis(2);
|
||||
this->createEmptyDynamicsWorld();
|
||||
|
||||
@ -4,12 +4,12 @@
|
||||
|
||||
#include "Bullet3AppSupport/CommonRigidBodySetup.h"
|
||||
|
||||
class ImportSTLDemo : public CommonRigidBodySetup
|
||||
class ImportSTLSetup : public CommonRigidBodySetup
|
||||
{
|
||||
struct CommonGraphicsApp* m_app;
|
||||
public:
|
||||
ImportSTLDemo(CommonGraphicsApp* app);
|
||||
virtual ~ImportSTLDemo();
|
||||
ImportSTLSetup(CommonGraphicsApp* app);
|
||||
virtual ~ImportSTLSetup();
|
||||
|
||||
virtual void initPhysics(GraphicsPhysicsBridge& gfxBridge);
|
||||
};
|
||||
|
||||
@ -12,12 +12,12 @@ static int bodyCollisionFilterMask=btBroadphaseProxy::AllFilter&(~btBroadphasePr
|
||||
static bool enableConstraints = true;//false;
|
||||
|
||||
|
||||
ImportUrdfDemo::ImportUrdfDemo()
|
||||
ImportUrdfSetup::ImportUrdfSetup()
|
||||
{
|
||||
sprintf(m_fileName,"r2d2.urdf");
|
||||
sprintf(m_fileName,"r2d2.urdf");//sphere2.urdf");//
|
||||
}
|
||||
|
||||
ImportUrdfDemo::~ImportUrdfDemo()
|
||||
ImportUrdfSetup::~ImportUrdfSetup()
|
||||
{
|
||||
|
||||
}
|
||||
@ -41,7 +41,7 @@ btVector3 selectColor()
|
||||
return color;
|
||||
}
|
||||
|
||||
void ImportUrdfDemo::setFileName(const char* urdfFileName)
|
||||
void ImportUrdfSetup::setFileName(const char* urdfFileName)
|
||||
{
|
||||
memcpy(m_fileName,urdfFileName,strlen(urdfFileName)+1);
|
||||
}
|
||||
@ -544,7 +544,260 @@ void URDFvisual2BulletCollisionShape(my_shared_ptr<const Link> link, GraphicsPhy
|
||||
|
||||
|
||||
}
|
||||
void ImportUrdfDemo::initPhysics(GraphicsPhysicsBridge& gfxBridge)
|
||||
|
||||
|
||||
|
||||
btMultiBody* URDF2BulletMultiBody(my_shared_ptr<const Link> link, GraphicsPhysicsBridge& gfxBridge, const btTransform& parentTransformInWorldSpace, btMultiBodyDynamicsWorld* world, URDF2BulletMappings& mappings, const char* pathPrefix, btMultiBody* mb, int totalNumJoints)
|
||||
{
|
||||
|
||||
btScalar mass = 0.f;
|
||||
btTransform localInertialTransform; localInertialTransform.setIdentity();
|
||||
btVector3 localInertiaDiagonal(0,0,0);
|
||||
|
||||
{
|
||||
|
||||
if ((*link).inertial)
|
||||
{
|
||||
mass = (*link).inertial->mass;
|
||||
btMatrix3x3 inertiaMat;
|
||||
inertiaMat.setIdentity();
|
||||
inertiaMat.setValue(
|
||||
(*link).inertial->ixx,(*link).inertial->ixy,(*link).inertial->ixz,
|
||||
(*link).inertial->ixy,(*link).inertial->iyy,(*link).inertial->iyz,
|
||||
(*link).inertial->ixz,(*link).inertial->iyz,(*link).inertial->izz);
|
||||
|
||||
btScalar threshold = 0.00001f;
|
||||
int maxSteps=20;
|
||||
btMatrix3x3 inertia2PrincipalAxis;
|
||||
inertiaMat.diagonalize(inertia2PrincipalAxis,threshold,maxSteps);
|
||||
localInertiaDiagonal.setValue(inertiaMat[0][0],inertiaMat[1][1],inertiaMat[2][2]);
|
||||
|
||||
btVector3 inertiaLocalCOM((*link).inertial->origin.position.x,(*link).inertial->origin.position.y,(*link).inertial->origin.position.z);
|
||||
localInertialTransform.setOrigin(inertiaLocalCOM);
|
||||
btQuaternion inertiaOrn((*link).inertial->origin.rotation.x,(*link).inertial->origin.rotation.y,(*link).inertial->origin.rotation.z,(*link).inertial->origin.rotation.w);
|
||||
btMatrix3x3 inertiaOrnMat(inertiaOrn);
|
||||
|
||||
if (mass > 0 && (localInertiaDiagonal[0]==0.f || localInertiaDiagonal[1] == 0.f
|
||||
|| localInertiaDiagonal[2] == 0.f))
|
||||
{
|
||||
b3Warning("Error: inertia should not be zero if mass is positive\n");
|
||||
localInertiaDiagonal.setMax(btVector3(0.1,0.1,0.1));
|
||||
localInertialTransform.setIdentity();//.setBasis(inertiaOrnMat);
|
||||
}
|
||||
else
|
||||
{
|
||||
localInertialTransform.setBasis(inertiaOrnMat*inertia2PrincipalAxis);
|
||||
}
|
||||
}
|
||||
}
|
||||
btTransform linkTransformInWorldSpace;
|
||||
int parentIndex = -1;
|
||||
|
||||
const Link* parentLink = (*link).getParent();
|
||||
if (parentLink)
|
||||
{
|
||||
parentIndex = parentLink->m_link_index;
|
||||
btAssert(parentIndex>=0);
|
||||
}
|
||||
int linkIndex = mappings.m_linkMasses.size();
|
||||
|
||||
btTransform parent2joint;
|
||||
|
||||
if ((*link).parent_joint)
|
||||
{
|
||||
const urdf::Vector3 pos = (*link).parent_joint->parent_to_joint_origin_transform.position;
|
||||
const urdf::Rotation orn = (*link).parent_joint->parent_to_joint_origin_transform.rotation;
|
||||
parent2joint.setOrigin(btVector3(pos.x,pos.y,pos.z));
|
||||
parent2joint.setRotation(btQuaternion(orn.x,orn.y,orn.z,orn.w));
|
||||
linkTransformInWorldSpace =parentTransformInWorldSpace*parent2joint;
|
||||
} else
|
||||
{
|
||||
linkTransformInWorldSpace = parentTransformInWorldSpace;
|
||||
|
||||
btAssert(mb==0);
|
||||
|
||||
bool multiDof = true;
|
||||
bool canSleep = false;
|
||||
bool isFixedBase = (mass==0);//todo: figure out when base is fixed
|
||||
|
||||
mb = new btMultiBody(totalNumJoints,mass, localInertiaDiagonal, isFixedBase, canSleep, multiDof);
|
||||
|
||||
|
||||
}
|
||||
|
||||
btAssert(mb);
|
||||
|
||||
(*link).m_link_index = linkIndex;
|
||||
|
||||
//compute this links center of mass transform, aligned with the principal axis of inertia
|
||||
|
||||
|
||||
{
|
||||
//btTransform rigidBodyFrameInWorldSpace =linkTransformInWorldSpace*inertialFrame;
|
||||
|
||||
mappings.m_linkMasses.push_back(mass);
|
||||
mappings.m_linkLocalDiagonalInertiaTensors.push_back(localInertiaDiagonal);
|
||||
mappings.m_linkLocalInertiaTransforms.push_back(localInertialTransform);
|
||||
|
||||
|
||||
|
||||
if ((*link).parent_joint)
|
||||
{
|
||||
btTransform offsetInA,offsetInB;
|
||||
offsetInA.setIdentity();
|
||||
//offsetInA = mappings.m_linkLocalInertiaTransforms[parentIndex].inverse()*parent2joint;
|
||||
offsetInA = parent2joint;
|
||||
offsetInB.setIdentity();
|
||||
//offsetInB = localInertialTransform.inverse();
|
||||
|
||||
const Joint* pj = (*link).parent_joint.get();
|
||||
//btVector3 jointAxis(0,0,1);//pj->axis.x,pj->axis.y,pj->axis.z);
|
||||
btVector3 jointAxis(pj->axis.x,pj->axis.y,pj->axis.z);
|
||||
mappings.m_jointAxisArray.push_back(jointAxis);
|
||||
mappings.m_jointOffsetInParent.push_back(offsetInA);
|
||||
mappings.m_jointOffsetInChild.push_back(offsetInB);
|
||||
mappings.m_jointTypeArray.push_back(pj->type);
|
||||
|
||||
switch (pj->type)
|
||||
{
|
||||
case Joint::FIXED:
|
||||
{
|
||||
printf("Fixed joint\n");
|
||||
mb->setupFixed(linkIndex-1,mass,localInertiaDiagonal,parentIndex-1,offsetInA.getRotation(),offsetInA.getOrigin(),offsetInB.getOrigin());
|
||||
|
||||
break;
|
||||
}
|
||||
case Joint::CONTINUOUS:
|
||||
case Joint::REVOLUTE:
|
||||
{
|
||||
printf("Revolute joint\n");
|
||||
mb->setupRevolute(linkIndex-1,mass,localInertiaDiagonal,parentIndex-1,offsetInA.getRotation(),jointAxis,offsetInA.getOrigin(),offsetInB.getOrigin(),true);
|
||||
mb->finalizeMultiDof();
|
||||
//mb->setJointVel(linkIndex-1,1);
|
||||
|
||||
break;
|
||||
}
|
||||
case Joint::PRISMATIC:
|
||||
{
|
||||
mb->setupPrismatic(linkIndex-1,mass,localInertiaDiagonal,parentIndex-1,offsetInA.getRotation(),jointAxis,offsetInB.getOrigin(),true);
|
||||
printf("Prismatic joint\n");
|
||||
break;
|
||||
}
|
||||
default:
|
||||
{
|
||||
printf("Unknown joint\n");
|
||||
btAssert(0);
|
||||
}
|
||||
};
|
||||
|
||||
|
||||
|
||||
|
||||
} else
|
||||
{
|
||||
mappings.m_jointAxisArray.push_back(btVector3(0,0,0));
|
||||
btTransform ident;
|
||||
ident.setIdentity();
|
||||
mappings.m_jointOffsetInParent.push_back(ident);
|
||||
mappings.m_jointOffsetInChild.push_back(ident);
|
||||
mappings.m_jointTypeArray.push_back(-1);
|
||||
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
//btCompoundShape* compoundShape = new btCompoundShape();
|
||||
btCollisionShape* shape = 0;
|
||||
|
||||
for (int v=0;v<(int)link->collision_array.size();v++)
|
||||
{
|
||||
const Collision* visual = link->collision_array[v].get();
|
||||
|
||||
shape = convertVisualToCollisionShape(visual,pathPrefix);
|
||||
|
||||
if (shape)//childShape)
|
||||
{
|
||||
gfxBridge.createCollisionShapeGraphicsObject(shape);//childShape);
|
||||
|
||||
//btVector3 color = selectColor();
|
||||
/*
|
||||
if (visual->material.get())
|
||||
{
|
||||
color.setValue(visual->material->color.r,visual->material->color.g,visual->material->color.b);//,visual->material->color.a);
|
||||
}
|
||||
*/
|
||||
btVector3 localInertia(0,0,0);
|
||||
if (mass)
|
||||
{
|
||||
shape->calculateLocalInertia(mass,localInertia);
|
||||
}
|
||||
//btRigidBody::btRigidBodyConstructionInfo rbci(mass,0,shape,localInertia);
|
||||
|
||||
|
||||
btVector3 visual_pos(visual->origin.position.x,visual->origin.position.y,visual->origin.position.z);
|
||||
btQuaternion visual_orn(visual->origin.rotation.x,visual->origin.rotation.y,visual->origin.rotation.z,visual->origin.rotation.w);
|
||||
btTransform visual_frame;
|
||||
visual_frame.setOrigin(visual_pos);
|
||||
visual_frame.setRotation(visual_orn);
|
||||
btTransform childTransform;
|
||||
childTransform.setIdentity();//TODO(erwincoumans): compute relative visual/inertial transform
|
||||
// compoundShape->addChildShape(childTransform,childShape);
|
||||
}
|
||||
}
|
||||
|
||||
if (shape)//compoundShape->getNumChildShapes()>0)
|
||||
{
|
||||
btMultiBodyLinkCollider* col= new btMultiBodyLinkCollider(mb, linkIndex-1);
|
||||
col->setCollisionShape(shape);
|
||||
|
||||
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
|
||||
|
||||
//tr.setOrigin(local_origin[0]);
|
||||
//tr.setRotation(btQuaternion(quat[0],quat[1],quat[2],quat[3]));
|
||||
col->setWorldTransform(tr);
|
||||
|
||||
bool isDynamic = true;
|
||||
short collisionFilterGroup = isDynamic? short(btBroadphaseProxy::DefaultFilter) : short(btBroadphaseProxy::StaticFilter);
|
||||
short collisionFilterMask = isDynamic? short(btBroadphaseProxy::AllFilter) : short(btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter);
|
||||
|
||||
world->addCollisionObject(col,collisionFilterGroup,collisionFilterMask);
|
||||
|
||||
btVector3 color = selectColor();//(0.0,0.0,0.5);
|
||||
gfxBridge.createCollisionObjectGraphicsObject(col,color);
|
||||
btScalar friction = 0.5f;
|
||||
|
||||
col->setFriction(friction);
|
||||
|
||||
if (parentIndex>=0)
|
||||
{
|
||||
mb->getLink(linkIndex-1).m_collider=col;
|
||||
} else
|
||||
{
|
||||
mb->setBaseCollider(col);
|
||||
}
|
||||
}
|
||||
|
||||
for (std::vector<my_shared_ptr<Link> >::const_iterator child = link->child_links.begin(); child != link->child_links.end(); child++)
|
||||
{
|
||||
if (*child)
|
||||
{
|
||||
URDF2BulletMultiBody(*child,gfxBridge, linkTransformInWorldSpace, world,mappings,pathPrefix,mb,totalNumJoints);
|
||||
|
||||
}
|
||||
else
|
||||
{
|
||||
std::cout << "root link: " << link->name << " has a null child!" << *child << std::endl;
|
||||
}
|
||||
}
|
||||
return mb;
|
||||
}
|
||||
|
||||
|
||||
void ImportUrdfSetup::initPhysics(GraphicsPhysicsBridge& gfxBridge)
|
||||
{
|
||||
|
||||
int upAxis = 2;
|
||||
@ -620,15 +873,16 @@ void ImportUrdfDemo::initPhysics(GraphicsPhysicsBridge& gfxBridge)
|
||||
|
||||
int numJoints = (*robot).m_numJoints;
|
||||
|
||||
if (1)
|
||||
static bool useFeatherstone = false;
|
||||
if (!useFeatherstone)
|
||||
{
|
||||
URDF2BulletMappings mappings;
|
||||
URDFvisual2BulletCollisionShape(root_link, gfxBridge, identityTrans,m_dynamicsWorld,mappings,pathPrefix);
|
||||
}
|
||||
|
||||
//the btMultiBody support is work-in-progress :-)
|
||||
#if 0
|
||||
if (0)
|
||||
#if 1
|
||||
else
|
||||
{
|
||||
URDF2BulletMappings mappings;
|
||||
|
||||
@ -641,7 +895,7 @@ void ImportUrdfDemo::initPhysics(GraphicsPhysicsBridge& gfxBridge)
|
||||
|
||||
}
|
||||
#endif//
|
||||
|
||||
useFeatherstone = !useFeatherstone;
|
||||
printf("numJoints/DOFS = %d\n", numJoints);
|
||||
|
||||
if (0)
|
||||
@ -681,7 +935,7 @@ void ImportUrdfDemo::initPhysics(GraphicsPhysicsBridge& gfxBridge)
|
||||
|
||||
}
|
||||
|
||||
void ImportUrdfDemo::stepSimulation(float deltaTime)
|
||||
void ImportUrdfSetup::stepSimulation(float deltaTime)
|
||||
{
|
||||
if (m_dynamicsWorld)
|
||||
{
|
||||
|
||||
@ -4,13 +4,13 @@
|
||||
|
||||
#include "Bullet3AppSupport/CommonMultiBodySetup.h"
|
||||
|
||||
class ImportUrdfDemo : public CommonMultiBodySetup
|
||||
class ImportUrdfSetup : public CommonMultiBodySetup
|
||||
{
|
||||
char m_fileName[1024];
|
||||
|
||||
public:
|
||||
ImportUrdfDemo();
|
||||
virtual ~ImportUrdfDemo();
|
||||
ImportUrdfSetup();
|
||||
virtual ~ImportUrdfSetup();
|
||||
|
||||
virtual void initPhysics(GraphicsPhysicsBridge& gfxBridge);
|
||||
virtual void stepSimulation(float deltaTime);
|
||||
|
||||
@ -0,0 +1,90 @@
|
||||
|
||||
|
||||
#include "CoordinateFrameDemoPhysicsSetup.h"
|
||||
#include "btBulletDynamicsCommon.h"
|
||||
#define ARRAY_SIZE_Y 5
|
||||
#define ARRAY_SIZE_X 5
|
||||
#define ARRAY_SIZE_Z 5
|
||||
|
||||
void CoordinateFrameDemoPhysicsSetup::initPhysics(GraphicsPhysicsBridge& gfxBridge)
|
||||
{
|
||||
createEmptyDynamicsWorld();
|
||||
gfxBridge.createPhysicsDebugDrawer(m_dynamicsWorld);
|
||||
m_dynamicsWorld->getDebugDrawer()->setDebugMode(btIDebugDraw::DBG_DrawWireframe+btIDebugDraw::DBG_DrawContactPoints);
|
||||
|
||||
///create a few basic rigid bodies
|
||||
btBoxShape* groundShape = createBoxShape(btVector3(btScalar(50.),btScalar(50.),btScalar(50.)));
|
||||
gfxBridge.createCollisionShapeGraphicsObject(groundShape);
|
||||
|
||||
//groundShape->initializePolyhedralFeatures();
|
||||
// btCollisionShape* groundShape = new btStaticPlaneShape(btVector3(0,1,0),50);
|
||||
|
||||
m_collisionShapes.push_back(groundShape);
|
||||
|
||||
btTransform groundTransform;
|
||||
groundTransform.setIdentity();
|
||||
groundTransform.setOrigin(btVector3(0,-50,0));
|
||||
|
||||
{
|
||||
btScalar mass(0.);
|
||||
btRigidBody* body = createRigidBody(mass,groundTransform,groundShape, btVector4(0,0,1,1));
|
||||
gfxBridge.createRigidBodyGraphicsObject(body, btVector3(0, 1, 0));
|
||||
}
|
||||
|
||||
|
||||
{
|
||||
//create a few dynamic rigidbodies
|
||||
// Re-using the same collision is better for memory usage and performance
|
||||
|
||||
btBoxShape* colShape = createBoxShape(btVector3(1,1,1));
|
||||
gfxBridge.createCollisionShapeGraphicsObject(colShape);
|
||||
|
||||
//btCollisionShape* colShape = new btSphereShape(btScalar(1.));
|
||||
m_collisionShapes.push_back(colShape);
|
||||
|
||||
/// Create Dynamic Objects
|
||||
btTransform startTransform;
|
||||
startTransform.setIdentity();
|
||||
|
||||
btScalar mass(1.f);
|
||||
|
||||
//rigidbody is dynamic if and only if mass is non zero, otherwise static
|
||||
bool isDynamic = (mass != 0.f);
|
||||
|
||||
btVector3 localInertia(0,0,0);
|
||||
if (isDynamic)
|
||||
colShape->calculateLocalInertia(mass,localInertia);
|
||||
|
||||
|
||||
for (int k=0;k<ARRAY_SIZE_Y;k++)
|
||||
{
|
||||
for (int i=0;i<ARRAY_SIZE_X;i++)
|
||||
{
|
||||
for(int j = 0;j<ARRAY_SIZE_Z;j++)
|
||||
{
|
||||
startTransform.setOrigin(btVector3(
|
||||
btScalar(2.0*i),
|
||||
btScalar(20+2.0*k),
|
||||
btScalar(2.0*j)));
|
||||
|
||||
|
||||
btRigidBody* body = createRigidBody(mass,startTransform,colShape);
|
||||
gfxBridge.createRigidBodyGraphicsObject(body, btVector3(1, 1, 0));
|
||||
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
@ -0,0 +1,28 @@
|
||||
#ifndef COORDINATE_FRAME_DEMO_PHYSICS_SETUP_H
|
||||
#define COORDINATE_FRAME_DEMO_PHYSICS_SETUP_H
|
||||
|
||||
class btRigidBody;
|
||||
class btCollisionShape;
|
||||
class btBroadphaseInterface;
|
||||
class btConstraintSolver;
|
||||
class btCollisionDispatcher;
|
||||
class btDefaultCollisionConfiguration;
|
||||
class btDiscreteDynamicsWorld;
|
||||
class btTransform;
|
||||
class btVector3;
|
||||
class btBoxShape;
|
||||
#include "LinearMath/btVector3.h"
|
||||
|
||||
#include "LinearMath/btAlignedObjectArray.h"
|
||||
|
||||
|
||||
#include "Bullet3AppSupport/CommonRigidBodySetup.h"
|
||||
|
||||
struct CoordinateFrameDemoPhysicsSetup : public CommonRigidBodySetup
|
||||
{
|
||||
|
||||
virtual void initPhysics(GraphicsPhysicsBridge& gfxBridge);
|
||||
|
||||
};
|
||||
|
||||
#endif //COORDINATE_FRAME_DEMO_PHYSICS_SETUP_H
|
||||
@ -38,6 +38,7 @@
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="10"/>
|
||||
<origin rpy="0 1.57075 0" xyz="0 0 -0.3"/>
|
||||
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
|
||||
</inertial>
|
||||
</link>
|
||||
@ -145,6 +146,7 @@
|
||||
<inertial>
|
||||
<mass value="10"/>
|
||||
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
|
||||
<origin rpy="0 1.57075 0" xyz="0 0 -0.3"/>
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
|
||||
Loading…
Reference in New Issue
Block a user