AuroraMiddleware/bullet3
1
0
Fork 0
mirror of https://github.com/bulletphysics/bullet3 synced 2024-12-13 21:30:09 +00:00
Code Issues Releases Wiki Activity

add 'fixed' joint for btMultiBody

Browse Source 
improve btMultiBody version of URDF reader (still work-in-progress)
enabled planar joint for btMultiBody (untested)
enable loading from relative path for .stl meshes
This commit is contained in:
erwin coumans 2014-08-28 18:42:08 -07:00
parent 3c558ec995
commit 89addd438e
19 changed files with 1276 additions and 304 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

65
Demos/CommonMultiBodySetup.h
View File

@ -7,6 +7,8 @@
#include "CommonPhysicsSetup.h"
#include "BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h"
#include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h"
#include "BulletDynamics/Featherstone/btMultiBodyPoint2Point.h"
#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h"
struct CommonMultiBodySetup : public CommonPhysicsSetup
{
@ -21,9 +23,12 @@ struct CommonMultiBodySetup : public CommonPhysicsSetup
//data for picking objects
class btRigidBody* m_pickedBody;
class btTypedConstraint* m_pickedConstraint;
class btMultiBodyPoint2Point* m_pickingMultiBodyPoint2Point;
btVector3 m_oldPickingPos;
btVector3 m_hitPos;
btScalar m_oldPickingDist;
bool m_prevCanSleep;
CommonMultiBodySetup()
:m_broadphase(0),
@ -32,7 +37,9 @@ struct CommonMultiBodySetup : public CommonPhysicsSetup
m_collisionConfiguration(0),
m_dynamicsWorld(0),
m_pickedBody(0),
m_pickedConstraint(0)
m_pickedConstraint(0),
m_pickingMultiBodyPoint2Point(0),
m_prevCanSleep(false)
{
}
@ -157,9 +164,33 @@ struct CommonMultiBodySetup : public CommonPhysicsSetup
//very weak constraint for picking
p2p->m_setting.m_tau = 0.001f;
}
} else
{
btMultiBodyLinkCollider* multiCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(rayCallback.m_collisionObject);
if (multiCol && multiCol->m_multiBody)
{
m_prevCanSleep = multiCol->m_multiBody->getCanSleep();
multiCol->m_multiBody->setCanSleep(false);
btVector3 pivotInA = multiCol->m_multiBody->worldPosToLocal(multiCol->m_link, pickPos);
btMultiBodyPoint2Point* p2p = new btMultiBodyPoint2Point(multiCol->m_multiBody,multiCol->m_link,0,pivotInA,pickPos);
//if you add too much energy to the system, causing high angular velocities, simulation 'explodes'
//see also http://www.bulletphysics.org/Bullet/phpBB3/viewtopic.php?f=4&t=949
//so we try to avoid it by clamping the maximum impulse (force) that the mouse pick can apply
//it is not satisfying, hopefully we find a better solution (higher order integrator, using joint friction using a zero-velocity target motor with limited force etc?)
btScalar scaling=1;
p2p->setMaxAppliedImpulse(2*scaling);
btMultiBodyDynamicsWorld* world = (btMultiBodyDynamicsWorld*) m_dynamicsWorld;
world->addMultiBodyConstraint(p2p);
m_pickingMultiBodyPoint2Point =p2p;
}
}
// pickObject(pickPos, rayCallback.m_collisionObject);
m_oldPickingPos = rayToWorld;
m_hitPos = pickPos;
@ -177,18 +208,30 @@ struct CommonMultiBodySetup : public CommonPhysicsSetup
if (pickCon)
{
//keep it at the same picking distance
btVector3 newPivotB;
btVector3 dir = rayToWorld - rayFromWorld;
btVector3 dir = rayToWorld-rayFromWorld;
dir.normalize();
dir *= m_oldPickingDist;
newPivotB = rayFromWorld + dir;
btVector3 newPivotB = rayFromWorld + dir;
pickCon->setPivotB(newPivotB);
return true;
}
}
if (m_pickingMultiBodyPoint2Point)
{
//keep it at the same picking distance
btVector3 dir = rayToWorld-rayFromWorld;
dir.normalize();
dir *= m_oldPickingDist;
btVector3 newPivotB = rayFromWorld + dir;
m_pickingMultiBodyPoint2Point->setPivotInB(newPivotB);
}
return false;
}
virtual void removePickingConstraint()
@ -200,6 +243,14 @@ struct CommonMultiBodySetup : public CommonPhysicsSetup
m_pickedConstraint = 0;
m_pickedBody = 0;
}
if (m_pickingMultiBodyPoint2Point)
{
m_pickingMultiBodyPoint2Point->getMultiBodyA()->setCanSleep(m_prevCanSleep);
btMultiBodyDynamicsWorld* world = (btMultiBodyDynamicsWorld*) m_dynamicsWorld;
world->removeMultiBodyConstraint(m_pickingMultiBodyPoint2Point);
delete m_pickingMultiBodyPoint2Point;
m_pickingMultiBodyPoint2Point = 0;
}
}

4
Demos3/AllBullet2Demos/main.cpp
View File

@ -24,9 +24,11 @@ static b3AlignedObjectArray<const char*> allNames;
bool drawGUI=true;
extern bool useShadowMap;
static bool wireframe=false;
static bool pauseSimulation=false;
static bool pauseSimulation=false;//true;
int midiBaseIndex = 176;
//#include <float.h>
//unsigned int fp_control_state = _controlfp(_EM_INEXACT, _MCW_EM);
#ifdef B3_USE_MIDI
#include "../../btgui/MidiTest/RtMidi.h"

92
Demos3/ImportSTLDemo/ImportSTLSetup.cpp
View File

@ -4,6 +4,7 @@
#include "OpenGLWindow/GLInstanceGraphicsShape.h"
#include "btBulletDynamicsCommon.h"
#include "OpenGLWindow/SimpleOpenGL3App.h"
#include "LoadMeshFromSTL.h"
ImportSTLDemo::ImportSTLDemo(SimpleOpenGL3App* app)
:m_app(app)
@ -16,97 +17,6 @@ ImportSTLDemo::~ImportSTLDemo()
}
struct MySTLTriangle
{
float normal[3];
float vertex0[3];
float vertex1[3];
float vertex2[3];
};
GLInstanceGraphicsShape* LoadMeshFromSTL(const char* relativeFileName)
{
GLInstanceGraphicsShape* shape = 0;
FILE* file = fopen(relativeFileName,"rb");
if (file)
{
int size=0;
if (fseek(file, 0, SEEK_END) || (size = ftell(file)) == EOF || fseek(file, 0, SEEK_SET))
{
printf("Error: Cannot access file to determine size of %s\n", relativeFileName);
} else
{
if (size)
{
printf("Open STL file of %d bytes\n",size);
char* memoryBuffer = new char[size+1];
int actualBytesRead = fread(memoryBuffer,1,size,file);
if (actualBytesRead!=size)
{
printf("Error reading from file %s",relativeFileName);
} else
{
int numTriangles = *(int*)&memoryBuffer[80];
if (numTriangles)
{
shape = new GLInstanceGraphicsShape;
// b3AlignedObjectArray<GLInstanceVertex>* m_vertices;
// int m_numvertices;
// b3AlignedObjectArray<int>* m_indices;
// int m_numIndices;
// float m_scaling[4];
shape->m_scaling[0] = 1;
shape->m_scaling[1] = 1;
shape->m_scaling[2] = 1;
shape->m_scaling[3] = 1;
int index = 0;
shape->m_indices = new b3AlignedObjectArray<int>();
shape->m_vertices = new b3AlignedObjectArray<GLInstanceVertex>();
for (int i=0;i<numTriangles;i++)
{
char* curPtr = &memoryBuffer[84+i*50];
MySTLTriangle* tri = (MySTLTriangle*) curPtr;
GLInstanceVertex v0,v1,v2;
if (i==numTriangles-2)
{
printf("!\n");
}
v0.uv[0] = v1.uv[0] = v2.uv[0] = 0.5;
v0.uv[1] = v1.uv[1] = v2.uv[1] = 0.5;
for (int v=0;v<3;v++)
{
v0.xyzw[v] = tri->vertex0[v];
v1.xyzw[v] = tri->vertex1[v];
v2.xyzw[v] = tri->vertex2[v];
v0.normal[v] = v1.normal[v] = v2.normal[v] = tri->normal[v];
}
v0.xyzw[3] = v1.xyzw[3] = v2.xyzw[3] = 0.f;
shape->m_vertices->push_back(v0);
shape->m_vertices->push_back(v1);
shape->m_vertices->push_back(v2);
shape->m_indices->push_back(index++);
shape->m_indices->push_back(index++);
shape->m_indices->push_back(index++);
}
}
}
delete[] memoryBuffer;
}
}
fclose(file);
}
shape->m_numIndices = shape->m_indices->size();
shape->m_numvertices = shape->m_vertices->size();
return shape;
}
void ImportSTLDemo::initPhysics(GraphicsPhysicsBridge& gfxBridge)

101
Demos3/ImportSTLDemo/LoadMeshFromSTL.h Normal file
View File

@ -0,0 +1,101 @@
#ifndef LOAD_MESH_FROM_STL_H
#define LOAD_MESH_FROM_STL_H
#include "OpenGLWindow/GLInstanceGraphicsShape.h"
#include <stdio.h> //fopen
#include "Bullet3Common/b3AlignedObjectArray.h"
struct MySTLTriangle
{
float normal[3];
float vertex0[3];
float vertex1[3];
float vertex2[3];
};
static GLInstanceGraphicsShape* LoadMeshFromSTL(const char* relativeFileName)
{
GLInstanceGraphicsShape* shape = 0;
FILE* file = fopen(relativeFileName,"rb");
if (file)
{
int size=0;
if (fseek(file, 0, SEEK_END) || (size = ftell(file)) == EOF || fseek(file, 0, SEEK_SET))
{
printf("Error: Cannot access file to determine size of %s\n", relativeFileName);
} else
{
if (size)
{
printf("Open STL file of %d bytes\n",size);
char* memoryBuffer = new char[size+1];
int actualBytesRead = fread(memoryBuffer,1,size,file);
if (actualBytesRead!=size)
{
printf("Error reading from file %s",relativeFileName);
} else
{
int numTriangles = *(int*)&memoryBuffer[80];
if (numTriangles)
{
shape = new GLInstanceGraphicsShape;
// b3AlignedObjectArray<GLInstanceVertex>* m_vertices;
// int m_numvertices;
// b3AlignedObjectArray<int>* m_indices;
// int m_numIndices;
// float m_scaling[4];
shape->m_scaling[0] = 1;
shape->m_scaling[1] = 1;
shape->m_scaling[2] = 1;
shape->m_scaling[3] = 1;
int index = 0;
shape->m_indices = new b3AlignedObjectArray<int>();
shape->m_vertices = new b3AlignedObjectArray<GLInstanceVertex>();
for (int i=0;i<numTriangles;i++)
{
char* curPtr = &memoryBuffer[84+i*50];
MySTLTriangle* tri = (MySTLTriangle*) curPtr;
GLInstanceVertex v0,v1,v2;
if (i==numTriangles-2)
{
printf("!\n");
}
v0.uv[0] = v1.uv[0] = v2.uv[0] = 0.5;
v0.uv[1] = v1.uv[1] = v2.uv[1] = 0.5;
for (int v=0;v<3;v++)
{
v0.xyzw[v] = tri->vertex0[v];
v1.xyzw[v] = tri->vertex1[v];
v2.xyzw[v] = tri->vertex2[v];
v0.normal[v] = v1.normal[v] = v2.normal[v] = tri->normal[v];
}
v0.xyzw[3] = v1.xyzw[3] = v2.xyzw[3] = 0.f;
shape->m_vertices->push_back(v0);
shape->m_vertices->push_back(v1);
shape->m_vertices->push_back(v2);
shape->m_indices->push_back(index++);
shape->m_indices->push_back(index++);
shape->m_indices->push_back(index++);
}
}
}
delete[] memoryBuffer;
}
}
fclose(file);
}
shape->m_numIndices = shape->m_indices->size();
shape->m_numvertices = shape->m_vertices->size();
return shape;
}
#endif //LOAD_MESH_FROM_STL_H

546
Demos3/ImportURDFDemo/ImportURDFSetup.cpp
View File

@ -1,6 +1,9 @@
#include "ImportURDFSetup.h"
#include "BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h"
#include "Bullet3Common/b3FileUtils.h"
#include "../ImportSTLDemo/LoadMeshFromSTL.h"
#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h"
static int bodyCollisionFilterGroup=btBroadphaseProxy::CharacterFilter;
static int bodyCollisionFilterMask=btBroadphaseProxy::AllFilter&(~btBroadphaseProxy::CharacterFilter);
@ -59,6 +62,8 @@ void printTree(my_shared_ptr<const Link> link,int level = 0)
struct URDF_LinkInformation
{
const Link* m_thisLink;
int m_linkIndex;
int m_parentIndex;
btTransform m_localInertialFrame;
btTransform m_localVisualFrame;
@ -80,9 +85,367 @@ struct URDF2BulletMappings
{
btHashMap<btHashPtr /*to Link*/, URDF_LinkInformation*> m_link2rigidbody;
btHashMap<btHashPtr /*to Joint*/, btTypedConstraint*> m_joint2Constraint;
btAlignedObjectArray<btTransform> m_linkLocalInertiaTransforms;//Body transform is in center of mass, aligned with Principal Moment Of Inertia;
btAlignedObjectArray<btScalar> m_linkMasses;
btAlignedObjectArray<btVector3> m_linkLocalDiagonalInertiaTensors;
btAlignedObjectArray<btTransform> m_jointTransforms;//for root, it is identity
btAlignedObjectArray<int> m_parentIndices;//for root, it is identity
btAlignedObjectArray<btVector3> m_jointAxisArray;
btAlignedObjectArray<btTransform> m_jointOffsetInParent;
btAlignedObjectArray<btTransform> m_jointOffsetInChild;
btAlignedObjectArray<int> m_jointTypeArray;
};
void URDFvisual2BulletCollisionShape(my_shared_ptr<const Link> link, GraphicsPhysicsBridge& gfxBridge, const btTransform& parentTransformInWorldSpace, btDiscreteDynamicsWorld* world, URDF2BulletMappings& mappings)
btCollisionShape* convertVisualToCollisionShape(const Visual* visual, const char* pathPrefix)
{
btCollisionShape* shape = 0;
switch (visual->geometry->type)
{
case Geometry::CYLINDER:
{
printf("processing a cylinder\n");
urdf::Cylinder* cyl = (urdf::Cylinder*)visual->geometry.get();
btAlignedObjectArray<btVector3> vertices;
//int numVerts = sizeof(barrel_vertices)/(9*sizeof(float));
int numSteps = 32;
for (int i=0;i<numSteps;i++)
{
btVector3 vert(cyl->radius*btSin(SIMD_2_PI*(float(i)/numSteps)),cyl->radius*btCos(SIMD_2_PI*(float(i)/numSteps)),cyl->length/2.);
vertices.push_back(vert);
vert[2] = -cyl->length/2.;
vertices.push_back(vert);
}
btConvexHullShape* cylZShape = new btConvexHullShape(&vertices[0].x(), vertices.size(), sizeof(btVector3));
cylZShape->initializePolyhedralFeatures();
//btVector3 halfExtents(cyl->radius,cyl->radius,cyl->length/2.);
//btCylinderShapeZ* cylZShape = new btCylinderShapeZ(halfExtents);
cylZShape->setMargin(0.001);
shape = cylZShape;
break;
}
case Geometry::BOX:
{
printf("processing a box\n");
urdf::Box* box = (urdf::Box*)visual->geometry.get();
btVector3 extents(box->dim.x,box->dim.y,box->dim.z);
btBoxShape* boxShape = new btBoxShape(extents*0.5f);
shape = boxShape;
shape ->setMargin(0.001);
break;
}
case Geometry::SPHERE:
{
printf("processing a sphere\n");
urdf::Sphere* sphere = (urdf::Sphere*)visual->geometry.get();
btScalar radius = sphere->radius;
btSphereShape* sphereShape = new btSphereShape(radius);
shape = sphereShape;
shape ->setMargin(0.001);
break;
break;
}
case Geometry::MESH:
{
if (visual->name.length())
{
printf("visual->name=%s\n",visual->name.c_str());
}
if (visual->geometry)
{
const urdf::Mesh* mesh = (const urdf::Mesh*) visual->geometry.get();
if (mesh->filename.length())
{
const char* filename = mesh->filename.c_str();
printf("mesh->filename=%s\n",filename);
char fullPath[1024];
sprintf(fullPath,"%s%s",pathPrefix,filename);
FILE* f = fopen(fullPath,"rb");
if (f)
{
fclose(f);
GLInstanceGraphicsShape* glmesh = LoadMeshFromSTL(fullPath);
if (glmesh && (glmesh->m_numvertices>0))
{
printf("extracted %d verticed from STL file %s\n", glmesh->m_numvertices,fullPath);
//int shapeId = m_glApp->m_instancingRenderer->registerShape(&gvertices[0].pos[0],gvertices.size(),&indices[0],indices.size());
//convex->setUserIndex(shapeId);
btConvexHullShape* cylZShape = new btConvexHullShape(&glmesh->m_vertices->at(0).xyzw[0], glmesh->m_numvertices, sizeof(GLInstanceVertex));
//cylZShape->initializePolyhedralFeatures();
//btVector3 halfExtents(cyl->radius,cyl->radius,cyl->length/2.);
//btCylinderShapeZ* cylZShape = new btCylinderShapeZ(halfExtents);
cylZShape->setMargin(0.001);
shape = cylZShape;
} else
{
printf("issue extracting mesh from STL file %s\n", fullPath);
}
} else
{
printf("mesh geometry not found %s\n",fullPath);
}
}
}
break;
}
default:
{
printf("Error: unknown visual geometry type\n");
}
}
return shape;
}
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);
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->visual_array.size();v++)
{
const Visual* visual = link->visual_array[v].get();
shape = convertVisualToCollisionShape(visual,pathPrefix);
if (shape)//childShape)
{
gfxBridge.createCollisionShapeGraphicsObject(shape);//childShape);
btVector3 color(0,0,1);
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(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 URDFvisual2BulletCollisionShape(my_shared_ptr<const Link> link, GraphicsPhysicsBridge& gfxBridge, const btTransform& parentTransformInWorldSpace, btDiscreteDynamicsWorld* world, URDF2BulletMappings& mappings, const char* pathPrefix)
{
btCollisionShape* shape = 0;
@ -112,11 +475,13 @@ void URDFvisual2BulletCollisionShape(my_shared_ptr<const Link> link, GraphicsPhy
inertialFrame.setRotation(btQuaternion((*link).inertial->origin.rotation.x,(*link).inertial->origin.rotation.y,(*link).inertial->origin.rotation.z,(*link).inertial->origin.rotation.w));
}
btTransform parent2joint;
btTransform parent2joint;
parent2joint.setIdentity();
if ((*link).parent_joint)
{
btTransform p2j;
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;
@ -130,72 +495,13 @@ void URDFvisual2BulletCollisionShape(my_shared_ptr<const Link> link, GraphicsPhy
{
printf("converting link %s",link->name.c_str());
printf("converting visuals of link %s",link->name.c_str());
for (int v=0;v<(int)link->visual_array.size();v++)
{
const Visual* visual = link->visual_array[v].get();
switch (visual->geometry->type)
{
case Geometry::CYLINDER:
{
printf("processing a cylinder\n");
urdf::Cylinder* cyl = (urdf::Cylinder*)visual->geometry.get();
btAlignedObjectArray<btVector3> vertices;
//int numVerts = sizeof(barrel_vertices)/(9*sizeof(float));
int numSteps = 32;
for (int i=0;i<numSteps;i++)
{
btVector3 vert(cyl->radius*btSin(SIMD_2_PI*(float(i)/numSteps)),cyl->radius*btCos(SIMD_2_PI*(float(i)/numSteps)),cyl->length/2.);
vertices.push_back(vert);
vert[2] = -cyl->length/2.;
vertices.push_back(vert);
}
btConvexHullShape* cylZShape = new btConvexHullShape(&vertices[0].x(), vertices.size(), sizeof(btVector3));
cylZShape->initializePolyhedralFeatures();
//btVector3 halfExtents(cyl->radius,cyl->radius,cyl->length/2.);
//btCylinderShapeZ* cylZShape = new btCylinderShapeZ(halfExtents);
cylZShape->setMargin(0.001);
shape = cylZShape;
break;
}
case Geometry::BOX:
{
printf("processing a box\n");
urdf::Box* box = (urdf::Box*)visual->geometry.get();
btVector3 extents(box->dim.x,box->dim.y,box->dim.z);
btBoxShape* boxShape = new btBoxShape(extents*0.5f);
shape = boxShape;
break;
}
case Geometry::SPHERE:
{
printf("processing a sphere\n");
urdf::Sphere* sphere = (urdf::Sphere*)visual->geometry.get();
btScalar radius = sphere->radius*0.8;
btSphereShape* sphereShape = new btSphereShape(radius);
shape = sphereShape;
break;
break;
}
case Geometry::MESH:
{
break;
}
default:
{
printf("Error: unknown visual geometry type\n");
}
}
shape = convertVisualToCollisionShape(visual,pathPrefix);
if (shape)
{
gfxBridge.createCollisionShapeGraphicsObject(shape);
@ -245,21 +551,10 @@ void URDFvisual2BulletCollisionShape(my_shared_ptr<const Link> link, GraphicsPhy
const Joint* pj = (*link).parent_joint.get();
btTransform offsetInA,offsetInB;
btTransform p2j; p2j.setIdentity();
btVector3 p2jPos; p2jPos.setValue(pj->parent_to_joint_origin_transform.position.x,
pj->parent_to_joint_origin_transform.position.y,
pj->parent_to_joint_origin_transform.position.z);
btQuaternion p2jOrn;p2jOrn.setValue(pj->parent_to_joint_origin_transform.rotation.x,
pj->parent_to_joint_origin_transform.rotation.y,
pj->parent_to_joint_origin_transform.rotation.z,
pj->parent_to_joint_origin_transform.rotation.w);
p2j.setOrigin(p2jPos);
p2j.setRotation(p2jOrn);
offsetInA.setIdentity();
offsetInA = pp->m_localVisualFrame.inverse()*p2j;
offsetInA = pp->m_localVisualFrame.inverse()*parent2joint;
offsetInB.setIdentity();
offsetInB = visual_frame.inverse();
@ -331,7 +626,7 @@ void URDFvisual2BulletCollisionShape(my_shared_ptr<const Link> link, GraphicsPhy
{
if (*child)
{
URDFvisual2BulletCollisionShape(*child,gfxBridge, linkTransformInWorldSpace, world,mappings);
URDFvisual2BulletCollisionShape(*child,gfxBridge, linkTransformInWorldSpace, world,mappings,pathPrefix);
}
else
@ -353,9 +648,9 @@ void ImportUrdfDemo::initPhysics(GraphicsPhysicsBridge& gfxBridge)
this->createEmptyDynamicsWorld();
gfxBridge.createPhysicsDebugDrawer(m_dynamicsWorld);
m_dynamicsWorld->getDebugDrawer()->setDebugMode(
//btIDebugDraw::DBG_DrawConstraints
btIDebugDraw::DBG_DrawConstraints
+btIDebugDraw::DBG_DrawContactPoints
//+btIDebugDraw::DBG_DrawAabb
+btIDebugDraw::DBG_DrawAabb
);//+btIDebugDraw::DBG_DrawConstraintLimits);
@ -365,38 +660,29 @@ void ImportUrdfDemo::initPhysics(GraphicsPhysicsBridge& gfxBridge)
m_dynamicsWorld->setGravity(gravity);
//int argc=0;
const char* someFileName="r2d2.urdf";
const char* someFileName="r2d2.urdf";
char relativeFileName[1024];
b3FileUtils fu;
bool fileFound = fu.findFile(someFileName, relativeFileName, 1024);
std::string xml_string;
const char* prefix[]={"./","./data/","../data/","../../data/","../../../data/","../../../../data/"};
int numPrefixes = sizeof(prefix)/sizeof(const char*);
char relativeFileName[1024];
FILE* f=0;
bool fileFound = false;
int result = 0;
for (int i=0;!f && i<numPrefixes;i++)
{
sprintf(relativeFileName,"%s%s",prefix[i],someFileName);
f = fopen(relativeFileName,"rb");
if (f)
{
fileFound = true;
break;
}
}
if (f)
{
fclose(f);
}
std::string xml_string;
char pathPrefix[1024];
pathPrefix[0] = 0;
if (!fileFound){
std::cerr << "URDF file not found, using a dummy test URDF" << std::endl;
xml_string = std::string(urdf_char);
} else
{
int maxPathLen = 1024;
fu.extractPath(relativeFileName,pathPrefix,maxPathLen);
std::fstream xml_file(relativeFileName, std::fstream::in);
while ( xml_file.good() )
{
@ -426,11 +712,46 @@ void ImportUrdfDemo::initPhysics(GraphicsPhysicsBridge& gfxBridge)
printTree(root_link);
btTransform worldTrans;
worldTrans.setIdentity();
int numJoints = (*robot).m_numJoints;
if (1)
{
URDF2BulletMappings mappings;
URDFvisual2BulletCollisionShape(root_link, gfxBridge, worldTrans,m_dynamicsWorld,mappings);
URDFvisual2BulletCollisionShape(root_link, gfxBridge, worldTrans,m_dynamicsWorld,mappings,pathPrefix);
}
//the btMultiBody support is work-in-progress :-)
if (0)
{
URDF2BulletMappings mappings;
btMultiBody* mb = URDF2BulletMultiBody(root_link, gfxBridge, worldTrans,m_dynamicsWorld,mappings,pathPrefix, 0,numJoints);
mb->setHasSelfCollision(false);
mb->finalizeMultiDof();
m_dynamicsWorld->addMultiBody(mb);
//m_dynamicsWorld->integrateTransforms(0.f);
}
printf("numJoints/DOFS = %d\n", numJoints);
if (0)
{
btVector3 halfExtents(1,1,1);
btBoxShape* box = new btBoxShape(halfExtents);
box->initializePolyhedralFeatures();
gfxBridge.createCollisionShapeGraphicsObject(box);
btTransform start; start.setIdentity();
btVector3 origin(0,0,0);
origin[upAxis]=5;
start.setOrigin(origin);
btRigidBody* body = createRigidBody(1,start,box);
btVector3 color(0.5,0.5,0.5);
gfxBridge.createRigidBodyGraphicsObject(body,color);
}
{
@ -442,7 +763,7 @@ void ImportUrdfDemo::initPhysics(GraphicsPhysicsBridge& gfxBridge)
gfxBridge.createCollisionShapeGraphicsObject(box);
btTransform start; start.setIdentity();
btVector3 groundOrigin(0,0,0);
groundOrigin[upAxis]=-1.5;
groundOrigin[upAxis]=-2.5;
start.setOrigin(groundOrigin);
btRigidBody* body = createRigidBody(0,start,box);
btVector3 color(0.5,0.5,0.5);
@ -451,3 +772,12 @@ void ImportUrdfDemo::initPhysics(GraphicsPhysicsBridge& gfxBridge)
}
void ImportUrdfDemo::stepSimulation(float deltaTime)
{
if (m_dynamicsWorld)
{
//the maximal coordinates/iterative MLCP solver requires a smallish timestep to converge
m_dynamicsWorld->stepSimulation(deltaTime,10,1./240.);
}
}

1
Demos3/ImportURDFDemo/ImportURDFSetup.h
View File

@ -11,6 +11,7 @@ public:
virtual ~ImportUrdfDemo();
virtual void initPhysics(GraphicsPhysicsBridge& gfxBridge);
virtual void stepSimulation(float deltaTime);
};
#endif //IMPORT_URDF_SETUP_H

4
Demos3/bullet2/BasicDemo/Bullet2RigidBodyDemo.cpp
View File

@ -36,6 +36,10 @@ struct MyGraphicsPhysicsBridge : public GraphicsPhysicsBridge
}
virtual void createCollisionShapeGraphicsObject(btCollisionShape* collisionShape)
{
//already has a graphics object?
if (collisionShape->getUserIndex()>=0)
return;
//todo: support all collision shape types
switch (collisionShape->getShapeType())
{

3
Demos3/bullet2/MultiBodyDemo/TestJointTorqueSetup.cpp
View File

@ -184,6 +184,9 @@ void TestJointTorqueSetup::initPhysics(GraphicsPhysicsBridge& gfxBridge)
btTransform tr;
tr.setIdentity();
//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);

10
btgui/Gwen/Controls/ScrollControl.cpp
View File

@ -134,8 +134,14 @@ void ScrollControl::UpdateScrollBars()
m_InnerPanel->SetSize( Utility::Max(Width(), childrenWidth), Utility::Max(Height(), childrenHeight));
float wPercent = (float)Width() / (float)(childrenWidth + (m_VerticalScrollBar->Hidden() ? 0 : m_VerticalScrollBar->Width()));
float hPercent = (float)Height() / (float)(childrenHeight + (m_HorizontalScrollBar->Hidden() ? 0 : m_HorizontalScrollBar->Height()));
float hg = (float)(childrenWidth + (m_VerticalScrollBar->Hidden() ? 0 : m_VerticalScrollBar->Width()));
if (hg==0.f)
hg = 0.00001f;
float wPercent = (float)Width() / hg;
hg = (float)(childrenHeight + (m_HorizontalScrollBar->Hidden() ? 0 : m_HorizontalScrollBar->Height()));
if (hg==0.f)
hg = 0.00001f;
float hPercent = (float)Height() / hg;
if ( m_bCanScrollV )
SetVScrollRequired( hPercent >= 1 );

95
btgui/Gwen/Controls/TreeControl.cpp
View File

@ -130,20 +130,23 @@ bool TreeControl::OnKeyUp( bool bDown )
//float maxCoordViewableWindow = minCoordViewableWindow+viewSize;
float minCoordSelectedItem = curItem*16.f;
// float maxCoordSelectedItem = (curItem+1)*16.f;
if (contSize!=viewSize)
{
float newAmount = float(minCoordSelectedItem)/(contSize-viewSize);
if (newAmount<curAmount)
{
m_ScrollControl->m_VerticalScrollBar->SetScrolledAmount(newAmount,true);
float newAmount = float(minCoordSelectedItem)/(contSize-viewSize);
if (newAmount<curAmount)
{
m_ScrollControl->m_VerticalScrollBar->SetScrolledAmount(newAmount,true);
}
}
}
{
int numItems = (viewSize)/16-1;
float newAmount = float((curItem-numItems)*16)/(contSize-viewSize);
if (newAmount>curAmount)
{
m_ScrollControl->m_VerticalScrollBar->SetScrolledAmount(newAmount,true);
int numItems = (viewSize)/16-1;
float newAmount = float((curItem-numItems)*16)/(contSize-viewSize);
if (newAmount>curAmount)
{
m_ScrollControl->m_VerticalScrollBar->SetScrolledAmount(newAmount,true);
}
}
}
}
@ -186,20 +189,23 @@ bool TreeControl::OnKeyDown( bool bDown )
//float maxCoordViewableWindow = minCoordViewableWindow+viewSize;
float minCoordSelectedItem = curItem*16.f;
//float maxCoordSelectedItem = (curItem+1)*16.f;
if (contSize!=viewSize)
{
float newAmount = float(minCoordSelectedItem)/(contSize-viewSize);
if (newAmount<curAmount)
{
m_ScrollControl->m_VerticalScrollBar->SetScrolledAmount(newAmount,true);
float newAmount = float(minCoordSelectedItem)/(contSize-viewSize);
if (newAmount<curAmount)
{
m_ScrollControl->m_VerticalScrollBar->SetScrolledAmount(newAmount,true);
}
}
}
{
int numItems = (viewSize)/16-1;
float newAmount = float((curItem-numItems)*16)/(contSize-viewSize);
if (newAmount>curAmount)
{
m_ScrollControl->m_VerticalScrollBar->SetScrolledAmount(newAmount,true);
int numItems = (viewSize)/16-1;
float newAmount = float((curItem-numItems)*16)/(contSize-viewSize);
if (newAmount>curAmount)
{
m_ScrollControl->m_VerticalScrollBar->SetScrolledAmount(newAmount,true);
}
}
}
}
@ -228,20 +234,23 @@ bool TreeControl::OnKeyRight( bool bDown )
// float maxCoordViewableWindow = minCoordViewableWindow+viewSize;
float minCoordSelectedItem = curItem*16.f;
// float maxCoordSelectedItem = (curItem+1)*16.f;
if (contSize!=viewSize)
{
float newAmount = float(minCoordSelectedItem)/(contSize-viewSize);
if (newAmount<curAmount)
{
m_ScrollControl->m_VerticalScrollBar->SetScrolledAmount(newAmount,true);
float newAmount = float(minCoordSelectedItem)/(contSize-viewSize);
if (newAmount<curAmount)
{
m_ScrollControl->m_VerticalScrollBar->SetScrolledAmount(newAmount,true);
}
}
}
{
int numItems = (viewSize)/16-1;
float newAmount = float((curItem-numItems)*16)/(contSize-viewSize);
if (newAmount>curAmount)
{
m_ScrollControl->m_VerticalScrollBar->SetScrolledAmount(newAmount,true);
int numItems = (viewSize)/16-1;
float newAmount = float((curItem-numItems)*16)/(contSize-viewSize);
if (newAmount>curAmount)
{
m_ScrollControl->m_VerticalScrollBar->SetScrolledAmount(newAmount,true);
}
}
}
Invalidate();
@ -273,23 +282,25 @@ bool TreeControl::OnKeyLeft( bool bDown )
// float maxCoordViewableWindow = minCoordViewableWindow+viewSize;
float minCoordSelectedItem = curItem*16.f;
// float maxCoordSelectedItem = (curItem+1)*16.f;
if (contSize!=viewSize)
{
float newAmount = float(minCoordSelectedItem)/(contSize-viewSize);
if (newAmount<curAmount)
{
m_ScrollControl->m_VerticalScrollBar->SetScrolledAmount(newAmount,true);
float newAmount = float(minCoordSelectedItem)/(contSize-viewSize);
if (newAmount<curAmount)
{
m_ScrollControl->m_VerticalScrollBar->SetScrolledAmount(newAmount,true);
}
}
}
{
int numItems = (viewSize)/16-1;
float newAmount = float((curItem-numItems)*16)/(contSize-viewSize);
{
int numItems = (viewSize)/16-1;
float newAmount = float((curItem-numItems)*16)/(contSize-viewSize);
if (newAmount>curAmount)
{
m_ScrollControl->m_VerticalScrollBar->SetScrolledAmount(newAmount,true);
if (newAmount>curAmount)
{
m_ScrollControl->m_VerticalScrollBar->SetScrolledAmount(newAmount,true);
}
Invalidate();
}
Invalidate();
}
//viewSize/contSize

2
btgui/urdf/urdfdom/urdf_parser/src/model.cpp
View File

@ -116,6 +116,7 @@ my_shared_ptr<ModelInterface> parseURDF(const std::string &xml_string)
{
my_shared_ptr<Link> link;
link.reset(new Link);
model->m_numLinks++;
try {
parseLink(*link, link_xml);
@ -176,6 +177,7 @@ my_shared_ptr<ModelInterface> parseURDF(const std::string &xml_string)
{
my_shared_ptr<Joint> joint;
joint.reset(new Joint);
model->m_numJoints++;
if (parseJoint(*joint, joint_xml))
{

4
btgui/urdf/urdfdom_headers/urdf_model/include/urdf_model/link.h
View File

@ -223,6 +223,8 @@ public:
std::vector<my_shared_ptr<Joint> > child_joints;
std::vector<my_shared_ptr<Link> > child_links;
mutable int m_link_index;
const Link* getParent() const
{return parent_link_;}
@ -242,6 +244,8 @@ public:
this->child_links.clear();
this->collision_array.clear();
this->visual_array.clear();
this->m_link_index=-1;
this->parent_link_ = NULL;
};
private:

5
btgui/urdf/urdfdom_headers/urdf_model/include/urdf_model/model.h
View File

@ -82,6 +82,8 @@ public:
void clear()
{
m_numLinks=0;
m_numJoints = 0;
name_.clear();
this->links_.clear();
this->joints_.clear();
@ -132,6 +134,7 @@ public:
this->getLink(child_link_name, child_link);
if (!child_link)
{
printf("Error: child link [%s] of joint [%s] not found\n", child_link_name,joint->first );
assert(0);
// throw ParseError("child link [" + child_link_name + "] of joint [" + joint->first + "] not found");
}
@ -206,6 +209,8 @@ public:
/// \brief The root is always a link (the parent of the tree describing the robot)
my_shared_ptr<Link> root_link_;
int m_numLinks;//includes parent
int m_numJoints;
};

414
data/r2d2.urdf Normal file
View File

@ -0,0 +1,414 @@
<?xml version="1.0"?>
<robot name="physics">
<link name="base_link">
<visual>
<geometry>
<cylinder length="0.6" radius="0.2"/>
</geometry>
<material name="blue">
<color rgba="0 0 .8 1"/>
</material>
</visual>
<collision>
<geometry>
<cylinder length="0.6" radius="0.2"/>
</geometry>
</collision>
<inertial>
<mass value="10"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<link name="right_leg">
<visual>
<geometry>
<box size="0.6 .2 .1"/>
</geometry>
<origin rpy="0 1.57075 0" xyz="0 0 -0.3"/>
<material name="white">
<color rgba="1 1 1 1"/>
</material>
</visual>
<collision>
<geometry>
<box size="0.6 .2 .1"/>
</geometry>
<origin rpy="0 1.57075 0" xyz="0 0 -0.3"/>
</collision>
<inertial>
<mass value="10"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<joint name="base_to_right_leg" type="fixed">
<parent link="base_link"/>
<child link="right_leg"/>
<origin xyz="0.22 0 .25"/>
</joint>
<link name="right_base">
<visual>
<geometry>
<box size=".1 0.4 .1"/>
</geometry>
<material name="white"/>
</visual>
<collision>
<geometry>
<box size=".1 0.4 .1"/>
</geometry>
</collision>
<inertial>
<mass value="10"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<joint name="right_base_joint" type="fixed">
<parent link="right_leg"/>
<child link="right_base"/>
<origin xyz="0 0 -0.6"/>
</joint>
<link name="right_front_wheel">
<visual>
<geometry>
<cylinder length=".1" radius="0.035"/>
</geometry>
<material name="black">
<color rgba="0 0 0 1"/>
</material>
</visual>
<collision>
<geometry>
<cylinder length=".1" radius="0.035"/>
</geometry>
</collision>
<inertial>
<mass value="1"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<joint name="right_front_wheel_joint" type="continuous">
<axis xyz="0 0 1"/>
<parent link="right_base"/>
<child link="right_front_wheel"/>
<origin rpy="0 1.57075 0" xyz="0 0.133333333333 -0.085"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>
<link name="right_back_wheel">
<visual>
<geometry>
<cylinder length=".1" radius="0.035"/>
</geometry>
<material name="black"/>
</visual>
<collision>
<geometry>
<cylinder length=".1" radius="0.035"/>
</geometry>
</collision>
<inertial>
<mass value="1"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<joint name="right_back_wheel_joint" type="continuous">
<axis xyz="0 0 1"/>
<parent link="right_base"/>
<child link="right_back_wheel"/>
<origin rpy="0 1.57075 0" xyz="0 -0.133333333333 -0.085"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>
<link name="left_leg">
<visual>
<geometry>
<box size="0.6 .2 .1"/>
</geometry>
<origin rpy="0 1.57075 0" xyz="0 0 -0.3"/>
<material name="white"/>
</visual>
<collision>
<geometry>
<box size="0.6 .2 .1"/>
</geometry>
<origin rpy="0 1.57075 0" xyz="0 0 -0.3"/>
</collision>
<inertial>
<mass value="10"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<joint name="base_to_left_leg" type="fixed">
<parent link="base_link"/>
<child link="left_leg"/>
<origin xyz="-0.22 0 .25"/>
</joint>
<link name="left_base">
<visual>
<geometry>
<box size=".1 0.4 .1"/>
</geometry>
<material name="white"/>
</visual>
<collision>
<geometry>
<box size=".1 0.4 .1"/>
</geometry>
</collision>
<inertial>
<mass value="10"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<joint name="left_base_joint" type="fixed">
<parent link="left_leg"/>
<child link="left_base"/>
<origin xyz="0 0 -0.6"/>
</joint>
<link name="left_front_wheel">
<visual>
<geometry>
<cylinder length=".1" radius="0.035"/>
</geometry>
<material name="black"/>
</visual>
<collision>
<geometry>
<cylinder length=".1" radius="0.035"/>
</geometry>
</collision>
<inertial>
<mass value="1"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<joint name="left_front_wheel_joint" type="continuous">
<axis xyz="0 0 1"/>
<parent link="left_base"/>
<child link="left_front_wheel"/>
<origin rpy="0 1.57075 0" xyz="0 0.133333333333 -0.085"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>
<link name="left_back_wheel">
<visual>
<geometry>
<cylinder length=".1" radius="0.035"/>
</geometry>
<material name="black"/>
</visual>
<collision>
<geometry>
<cylinder length=".1" radius="0.035"/>
</geometry>
</collision>
<inertial>
<mass value="1"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<joint name="left_back_wheel_joint" type="continuous">
<axis xyz="0 0 1"/>
<parent link="left_base"/>
<child link="left_back_wheel"/>
<origin rpy="0 1.57075 0" xyz="0 -0.133333333333 -0.085"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>
<joint name="gripper_extension" type="prismatic">
<parent link="base_link"/>
<child link="gripper_pole"/>
<limit effort="1000.0" lower="-0.38" upper="0" velocity="0.5"/>
<origin rpy="0 0 1.57075" xyz="0 0.19 .2"/>
</joint>
<link name="gripper_pole">
<visual>
<geometry>
<cylinder length="0.2" radius=".01"/>
</geometry>
<origin rpy="0 1.57075 0 " xyz="0.1 0 0"/>
<material name="Gray">
<color rgba=".7 .7 .7 1"/>
</material>
</visual>
<collision>
<geometry>
<cylinder length="0.2" radius=".01"/>
</geometry>
<origin rpy="0 1.57075 0 " xyz="0.1 0 0"/>
</collision>
<inertial>
<mass value="0.05"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<joint name="left_gripper_joint" type="revolute">
<axis xyz="0 0 1"/>
<limit effort="1000.0" lower="0.0" upper="0.548" velocity="0.5"/>
<origin rpy="0 0 0" xyz="0.2 0.01 0"/>
<parent link="gripper_pole"/>
<child link="left_gripper"/>
</joint>
<link name="left_gripper">
<visual>
<origin rpy="0.0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="l_finger.stl"/>
</geometry>
</visual>
<collision>
<geometry>
<mesh filename="l_finger.stl"/>
</geometry>
<origin rpy="0.0 0 0" xyz="0 0 0"/>
</collision>
<inertial>
<mass value="0.05"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<joint name="left_tip_joint" type="fixed">
<parent link="left_gripper"/>
<child link="left_tip"/>
</joint>
<link name="left_tip">
<visual>
<origin rpy="0.0 0 0" xyz="0.09137 0.00495 0"/>
<geometry>
<mesh filename="l_finger_tip.stl"/>
</geometry>
</visual>
<collision>
<geometry>
<mesh filename="l_finger_tip.stl"/>
</geometry>
<origin rpy="0.0 0 0" xyz="0.09137 0.00495 0"/>
</collision>
<inertial>
<mass value="0.05"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<joint name="right_gripper_joint" type="revolute">
<axis xyz="0 0 -1"/>
<limit effort="1000.0" lower="0.0" upper="0.548" velocity="0.5"/>
<origin rpy="0 0 0" xyz="0.2 -0.01 0"/>
<parent link="gripper_pole"/>
<child link="right_gripper"/>
</joint>
<link name="right_gripper">
<visual>
<origin rpy="-3.1415 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="l_finger.stl"/>
</geometry>
</visual>
<collision>
<geometry>
<mesh filename="l_finger.stl"/>
</geometry>
<origin rpy="-3.1415 0 0" xyz="0 0 0"/>
</collision>
<inertial>
<mass value="0.05"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<joint name="right_tip_joint" type="fixed">
<parent link="right_gripper"/>
<child link="right_tip"/>
</joint>
<link name="right_tip">
<visual>
<origin rpy="-3.1415 0 0" xyz="0.09137 0.00495 0"/>
<geometry>
<mesh filename="l_finger_tip.stl"/>
</geometry>
</visual>
<collision>
<geometry>
<mesh filename="l_finger_tip.stl"/>
</geometry>
<origin rpy="-3.1415 0 0" xyz="0.09137 0.00495 0"/>
</collision>
<inertial>
<mass value="0.05"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<link name="head">
<visual>
<geometry>
<sphere radius="0.2"/>
</geometry>
<material name="white"/>
</visual>
<collision>
<geometry>
<sphere radius="0.2"/>
</geometry>
</collision>
<inertial>
<mass value="10"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<joint name="head_swivel" type="continuous">
<parent link="base_link"/>
<child link="head"/>
<axis xyz="0 0 1"/>
<origin xyz="0 0 0.3"/>
</joint>
<link name="box">
<visual>
<geometry>
<box size=".08 .08 .08"/>
</geometry>
<material name="blue"/>
</visual>
<collision>
<geometry>
<box size=".08 .08 .08"/>
</geometry>
</collision>
<inertial>
<mass value="1"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<joint name="tobox" type="fixed">
<parent link="head"/>
<child link="box"/>
<origin xyz="0 0.1414 0.1414"/>
</joint>
</robot>

105
src/Bullet3Common/b3FileUtils.h Normal file
View File

@ -0,0 +1,105 @@
#ifndef B3_FILE_UTILS_H
#define B3_FILE_UTILS_H
#include <stdio.h>
#include "b3Scalar.h"
struct b3FileUtils
{
b3FileUtils()
{
}
virtual ~b3FileUtils()
{
}
bool findFile(const char* orgFileName, char* relativeFileName, int maxRelativeFileNameMaxLen)
{
const char* prefix[]={"","./","./data/","../data/","../../data/","../../../data/","../../../../data/"};
int numPrefixes = sizeof(prefix)/sizeof(const char*);
FILE* f=0;
bool fileFound = false;
int result = 0;
for (int i=0;!f && i<numPrefixes;i++)
{
#ifdef _WIN32
sprintf_s(relativeFileName,maxRelativeFileNameMaxLen,"%s%s",prefix[i],orgFileName);
#else
sprintf(relativeFileName,"%s%s",prefix[i],orgFileName);
#endif
f = fopen(relativeFileName,"rb");
if (f)
{
fileFound = true;
break;
}
}
if (f)
{
fclose(f);
}
return fileFound;
}
static const char* strip2(const char* name, const char* pattern)
{
size_t const patlen = strlen(pattern);
size_t patcnt = 0;
const char * oriptr;
const char * patloc;
// find how many times the pattern occurs in the original string
for (oriptr = name; patloc = strstr(oriptr, pattern); oriptr = patloc + patlen)
{
patcnt++;
}
return oriptr;
}
void extractPath(const char* fileName, char* path, int maxPathLength)
{
const char* stripped = strip2(fileName, "/");
stripped = strip2(stripped, "\\");
ptrdiff_t len = stripped-fileName;
b3Assert((len+1)<maxPathLength);
if (len && ((len+1)<maxPathLength))
{
for (int i=0;i<len;i++)
{
path[i] = fileName[i];
}
path[len]=0;
} else
{
#ifdef _WIN32
sprintf_s(path, maxPathLength,"");
#else
sprintf(path, maxPathLength,"");
#endif
}
}
/*static const char* strip2(const char* name, const char* pattern)
{
size_t const patlen = strlen(pattern);
size_t patcnt = 0;
const char * oriptr;
const char * patloc;
// find how many times the pattern occurs in the original string
for (oriptr = name; patloc = strstr(oriptr, pattern); oriptr = patloc + patlen)
{
patcnt++;
}
return oriptr;
}
*/
};
#endif //B3_FILE_UTILS_H

94
src/BulletDynamics/Featherstone/btMultiBody.cpp
View File

@ -132,6 +132,38 @@ btMultiBody::~btMultiBody()
{
}
void btMultiBody::setupFixed(int i,
btScalar mass,
const btVector3 &inertia,
int parent,
const btQuaternion &rotParentToThis,
const btVector3 &parentComToThisComOffset,
bool disableParentCollision)
{
btAssert(m_isMultiDof);
m_links[i].m_mass = mass;
m_links[i].m_inertia = inertia;
m_links[i].m_parent = parent;
m_links[i].m_zeroRotParentToThis = rotParentToThis;
m_links[i].m_eVector = parentComToThisComOffset;
m_links[i].m_jointType = btMultibodyLink::eFixed;
m_links[i].m_dofCount = 0;
m_links[i].m_posVarCount = 0;
if (disableParentCollision)
m_links[i].m_flags |=BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION;
//
m_links[i].updateCacheMultiDof();
//
//if(m_isMultiDof)
// resizeInternalMultiDofBuffers();
//
updateLinksDofOffsets();
}
void btMultiBody::setupPrismatic(int i,
btScalar mass,
const btVector3 &inertia,
@ -290,12 +322,15 @@ void btMultiBody::setupPlanar(int i,
m_links[i].m_jointType = btMultibodyLink::ePlanar;
m_links[i].m_dofCount = 3;
m_links[i].m_posVarCount = 3;
m_links[i].getAxisTop(0) = rotationAxis.normalized();
m_links[i].getAxisTop(1).setZero();
m_links[i].getAxisTop(2).setZero();
m_links[i].getAxisBottom(0).setZero();
m_links[i].getAxisBottom(1) = m_links[i].getAxisTop(0).cross(vecNonParallelToRotAxis);
m_links[i].getAxisBottom(2) = m_links[i].getAxisBottom(1).cross(m_links[i].getAxisTop(0));
btVector3 n=rotationAxis.normalized();
m_links[i].setAxisTop(0, n[0],n[1],n[2]);
m_links[i].setAxisTop(1,0,0,0);
m_links[i].setAxisTop(2,0,0,0);
m_links[i].setAxisBottom(0,0,0,0);
btVector3 cr = m_links[i].getAxisTop(0).cross(vecNonParallelToRotAxis);
m_links[i].setAxisBottom(1,cr[0],cr[1],cr[2]);
cr = m_links[i].getAxisBottom(1).cross(m_links[i].getAxisTop(0));
m_links[i].setAxisBottom(2,cr[0],cr[1],cr[2]);
m_links[i].m_jointPos[0] = m_links[i].m_jointPos[1] = m_links[i].m_jointPos[2] = 0.f;
m_links[i].m_jointTorque[0] = m_links[i].m_jointTorque[1] = m_links[i].m_jointTorque[2] = 0.f;
@ -1171,41 +1206,7 @@ void btMultiBody::stepVelocitiesMultiDof(btScalar dt,
fromParent.m_rotMat = rot_from_parent[i+1]; fromParent.m_trnVec = m_links[i].m_cachedRVector;
fromWorld.m_rotMat = rot_from_world[i+1];
////clamp parent's omega
//btScalar parOmegaMod = spatVel[parent+1].getAngular().length();
//btScalar parOmegaModMax = 0.1;
//if(parOmegaMod > parOmegaModMax)
//{
// //btSpatialMotionVector clampedParVel(spatVel[parent+1].getAngular() * parOmegaModMax / parOmegaMod, spatVel[parent+1].getLinear());
// btSpatialMotionVector clampedParVel; clampedParVel = spatVel[parent+1] * (parOmegaModMax / parOmegaMod);
// fromParent.transform(clampedParVel, spatVel[i+1]);
// spatVel[parent+1] *= (parOmegaModMax / parOmegaMod);
//}
//else
{
// vhat_i = i_xhat_p(i) * vhat_p(i)
fromParent.transform(spatVel[parent+1], spatVel[i+1]);
//nice alternative below (using operator *) but it generates temps
}
//////////////////////////////////////////////////////////////
//if(m_links[i].m_jointType == btMultibodyLink::eRevolute || m_links[i].m_jointType == btMultibodyLink::eSpherical)
//{
// btScalar mod2 = 0;
// for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
// mod2 += getJointVelMultiDof(i)[dof]*getJointVelMultiDof(i)[dof];
// btScalar angvel = sqrt(mod2);
// btScalar maxAngVel = 6;//SIMD_HALF_PI * 0.075;
// btScalar step = 1; //dt
// if (angvel*step > maxAngVel)
// {
// btScalar * qd = getJointVelMultiDof(i);
// for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
// qd[dof] *= (maxAngVel/step) /angvel;
// }
//}
fromParent.transform(spatVel[parent+1], spatVel[i+1]);
// now set vhat_i to its true value by doing
// vhat_i += qidot * shat_i
@ -1717,13 +1718,14 @@ void btMultiBody::stepVelocities(btScalar dt,
h_bottom[i] = inertia_bottom_left[i+1] * m_links[i].getAxisTop(0) + inertia_top_left[i+1].transpose() * m_links[i].getAxisBottom(0);
btScalar val = SpatialDotProduct(m_links[i].getAxisTop(0), m_links[i].getAxisBottom(0), h_top[i], h_bottom[i]);
D[i] = val;
Y[i] = m_links[i].m_jointTorque[0]
- SpatialDotProduct(m_links[i].getAxisTop(0), m_links[i].getAxisBottom(0), zero_acc_top_angular[i+1], zero_acc_bottom_linear[i+1])
- SpatialDotProduct(h_top[i], h_bottom[i], coriolis_top_angular[i], coriolis_bottom_linear[i]);
const int parent = m_links[i].m_parent;
btAssert(D[i]!=0.f);
// Ip += pXi * (Ii - hi hi' / Di) * iXp
const btScalar one_over_di = 1.0f / D[i];
@ -2637,7 +2639,7 @@ void btMultiBody::filConstraintJacobianMultiDof(int link,
btAssert(v_ptr - &scratch_v[0] == scratch_v.size());
scratch_r.resize(m_dofCount);
btScalar * results = num_links > 0 ? &scratch_r[0] : 0;
btScalar * results = m_dofCount > 0 ? &scratch_r[0] : 0;
btMatrix3x3 * rot_from_world = &scratch_m[0];
@ -2717,9 +2719,9 @@ void btMultiBody::filConstraintJacobianMultiDof(int link,
#ifdef BT_MULTIBODYLINK_INCLUDE_PLANAR_JOINTS
case btMultibodyLink::ePlanar:
{
results[m_links[i].m_dofOffset + 0] = n_local[i+1].dot(m_links[i].getAxisTop(0).cross(p_minus_com_local[i+1]));// + m_links[i].getAxisBottom(0));
results[m_links[i].m_dofOffset + 1] = n_local[i+1].dot(m_links[i].getAxisBottom(1));
results[m_links[i].m_dofOffset + 2] = n_local[i+1].dot(m_links[i].getAxisBottom(2));
results[m_links[i].m_dofOffset + 0] = n_local_lin[i+1].dot(m_links[i].getAxisTop(0).cross(p_minus_com_local[i+1]));// + m_links[i].getAxisBottom(0));
results[m_links[i].m_dofOffset + 1] = n_local_lin[i+1].dot(m_links[i].getAxisBottom(1));
results[m_links[i].m_dofOffset + 2] = n_local_lin[i+1].dot(m_links[i].getAxisBottom(2));
break;
}

17
src/BulletDynamics/Featherstone/btMultiBody.h
View File

@ -56,7 +56,16 @@ public:
~btMultiBody();
void setupPrismatic(int i, // 0 to num_links-1
void btMultiBody::setupFixed(int linkIndex,
btScalar mass,
const btVector3 &inertia,
int parent,
const btQuaternion &rotParentToThis,
const btVector3 &parentComToThisComOffset,
bool disableParentCollision);
void setupPrismatic(int linkIndex, // 0 to num_links-1
btScalar mass,
const btVector3 &inertia, // in my frame; assumed diagonal
int parent,
@ -66,17 +75,17 @@ public:
bool disableParentCollision=false
);
void setupRevolute(int i, // 0 to num_links-1
void setupRevolute(int linkIndex, // 0 to num_links-1
btScalar mass,
const btVector3 &inertia,
int parent,
int parentIndex,
const btQuaternion &rotParentToThis, // rotate points in parent frame to this frame, when q = 0
const btVector3 &jointAxis, // in my frame
const btVector3 &parentComToThisPivotOffset, // vector from parent COM to joint axis, in PARENT frame
const btVector3 &thisPivotToThisComOffset, // vector from joint axis to my COM, in MY frame
bool disableParentCollision=false);
void setupSpherical(int i, // 0 to num_links-1
void setupSpherical(int linkIndex, // 0 to num_links-1
btScalar mass,
const btVector3 &inertia,
int parent,

4
src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h
View File

@ -38,7 +38,7 @@ protected:
virtual void calculateSimulationIslands();
virtual void updateActivationState(btScalar timeStep);
virtual void solveConstraints(btContactSolverInfo& solverInfo);
virtual void integrateTransforms(btScalar timeStep);
public:
btMultiBodyDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btMultiBodyConstraintSolver* constraintSolver,btCollisionConfiguration* collisionConfiguration);
@ -52,5 +52,7 @@ public:
virtual void addMultiBodyConstraint( btMultiBodyConstraint* constraint);
virtual void removeMultiBodyConstraint( btMultiBodyConstraint* constraint);
virtual void integrateTransforms(btScalar timeStep);
};
#endif //BT_MULTIBODY_DYNAMICS_WORLD_H

14
src/BulletDynamics/Featherstone/btMultiBodyLink.h
View File

@ -25,7 +25,7 @@ enum btMultiBodyLinkFlags
BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION = 1
};
//#define BT_MULTIBODYLINK_INCLUDE_PLANAR_JOINTS
#define BT_MULTIBODYLINK_INCLUDE_PLANAR_JOINTS
#define TEST_SPATIAL_ALGEBRA_LAYER
//
@ -368,7 +368,9 @@ struct btMultibodyLink
// revolute: vector from parent's COM to the pivot point, in PARENT's frame.
btVector3 m_eVector;
#ifdef TEST_SPATIAL_ALGEBRA_LAYER
btSpatialMotionVector m_absFrameTotVelocity, m_absFrameLocVelocity;
#endif
enum eFeatherstoneJointType
{
@ -378,6 +380,7 @@ struct btMultibodyLink
#ifdef BT_MULTIBODYLINK_INCLUDE_PLANAR_JOINTS
ePlanar = 3,
#endif
eFixed = 4,
eInvalid
};
@ -505,11 +508,18 @@ struct btMultibodyLink
case ePlanar:
{
m_cachedRotParentToThis = btQuaternion(getAxisTop(0),-pJointPos[0]) * m_zeroRotParentToThis;
m_cachedRVector = quatRotate(btQuaternion(getAxisTop(0),-pJointPos[0]), pJointPos[1] * m_axesBottom[1] + pJointPos[2] * m_axesBottom[2]) + quatRotate(m_cachedRotParentToThis,m_eVector);
m_cachedRVector = quatRotate(btQuaternion(getAxisTop(0),-pJointPos[0]), pJointPos[1] * getAxisBottom(1) + pJointPos[2] * getAxisBottom(2)) + quatRotate(m_cachedRotParentToThis,m_eVector);
break;
}
#endif
case eFixed:
{
m_cachedRotParentToThis = m_zeroRotParentToThis;
m_cachedRVector = quatRotate(m_cachedRotParentToThis,m_eVector);
break;
}
default:
{
//invalid type