diff --git a/Demos/CommonMultiBodySetup.h b/Demos/CommonMultiBodySetup.h index f9e61364f..03553e3c2 100644 --- a/Demos/CommonMultiBodySetup.h +++ b/Demos/CommonMultiBodySetup.h @@ -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; + } } diff --git a/Demos3/AllBullet2Demos/main.cpp b/Demos3/AllBullet2Demos/main.cpp index 63394bd0a..9b38383a7 100644 --- a/Demos3/AllBullet2Demos/main.cpp +++ b/Demos3/AllBullet2Demos/main.cpp @@ -24,9 +24,11 @@ static b3AlignedObjectArray allNames; bool drawGUI=true; extern bool useShadowMap; static bool wireframe=false; -static bool pauseSimulation=false; +static bool pauseSimulation=false;//true; int midiBaseIndex = 176; +//#include +//unsigned int fp_control_state = _controlfp(_EM_INEXACT, _MCW_EM); #ifdef B3_USE_MIDI #include "../../btgui/MidiTest/RtMidi.h" diff --git a/Demos3/ImportSTLDemo/ImportSTLSetup.cpp b/Demos3/ImportSTLDemo/ImportSTLSetup.cpp index e67526150..bcdb0be3a 100644 --- a/Demos3/ImportSTLDemo/ImportSTLSetup.cpp +++ b/Demos3/ImportSTLDemo/ImportSTLSetup.cpp @@ -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* m_vertices; -// int m_numvertices; -// b3AlignedObjectArray* 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(); - shape->m_vertices = new b3AlignedObjectArray(); - for (int i=0;ivertex0[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) diff --git a/Demos3/ImportSTLDemo/LoadMeshFromSTL.h b/Demos3/ImportSTLDemo/LoadMeshFromSTL.h new file mode 100644 index 000000000..f0fe15954 --- /dev/null +++ b/Demos3/ImportSTLDemo/LoadMeshFromSTL.h @@ -0,0 +1,101 @@ + +#ifndef LOAD_MESH_FROM_STL_H +#define LOAD_MESH_FROM_STL_H + +#include "OpenGLWindow/GLInstanceGraphicsShape.h" +#include //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* m_vertices; +// int m_numvertices; +// b3AlignedObjectArray* 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(); + shape->m_vertices = new b3AlignedObjectArray(); + for (int i=0;ivertex0[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 diff --git a/Demos3/ImportURDFDemo/ImportURDFSetup.cpp b/Demos3/ImportURDFDemo/ImportURDFSetup.cpp index be0396bd0..6d42aca7b 100644 --- a/Demos3/ImportURDFDemo/ImportURDFSetup.cpp +++ b/Demos3/ImportURDFDemo/ImportURDFSetup.cpp @@ -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 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 m_link2rigidbody; btHashMap m_joint2Constraint; + + btAlignedObjectArray m_linkLocalInertiaTransforms;//Body transform is in center of mass, aligned with Principal Moment Of Inertia; + btAlignedObjectArray m_linkMasses; + btAlignedObjectArray m_linkLocalDiagonalInertiaTensors; + btAlignedObjectArray m_jointTransforms;//for root, it is identity + btAlignedObjectArray m_parentIndices;//for root, it is identity + btAlignedObjectArray m_jointAxisArray; + btAlignedObjectArray m_jointOffsetInParent; + btAlignedObjectArray m_jointOffsetInChild; + btAlignedObjectArray m_jointTypeArray; + }; -void URDFvisual2BulletCollisionShape(my_shared_ptr 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 vertices; + //int numVerts = sizeof(barrel_vertices)/(9*sizeof(float)); + int numSteps = 32; + for (int i=0;iradius*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 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 >::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 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 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 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 vertices; - //int numVerts = sizeof(barrel_vertices)/(9*sizeof(float)); - int numSteps = 32; - for (int i=0;iradius*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 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 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 && isetHasSelfCollision(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.); + } +} \ No newline at end of file diff --git a/Demos3/ImportURDFDemo/ImportURDFSetup.h b/Demos3/ImportURDFDemo/ImportURDFSetup.h index 34ae23cf5..5d5e9f900 100644 --- a/Demos3/ImportURDFDemo/ImportURDFSetup.h +++ b/Demos3/ImportURDFDemo/ImportURDFSetup.h @@ -11,6 +11,7 @@ public: virtual ~ImportUrdfDemo(); virtual void initPhysics(GraphicsPhysicsBridge& gfxBridge); + virtual void stepSimulation(float deltaTime); }; #endif //IMPORT_URDF_SETUP_H diff --git a/Demos3/bullet2/BasicDemo/Bullet2RigidBodyDemo.cpp b/Demos3/bullet2/BasicDemo/Bullet2RigidBodyDemo.cpp index c7f2f6664..f54aebf34 100644 --- a/Demos3/bullet2/BasicDemo/Bullet2RigidBodyDemo.cpp +++ b/Demos3/bullet2/BasicDemo/Bullet2RigidBodyDemo.cpp @@ -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()) { diff --git a/Demos3/bullet2/MultiBodyDemo/TestJointTorqueSetup.cpp b/Demos3/bullet2/MultiBodyDemo/TestJointTorqueSetup.cpp index e6765627f..f253d6f37 100644 --- a/Demos3/bullet2/MultiBodyDemo/TestJointTorqueSetup.cpp +++ b/Demos3/bullet2/MultiBodyDemo/TestJointTorqueSetup.cpp @@ -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); diff --git a/btgui/Gwen/Controls/ScrollControl.cpp b/btgui/Gwen/Controls/ScrollControl.cpp index 023ade942..4a0a95231 100644 --- a/btgui/Gwen/Controls/ScrollControl.cpp +++ b/btgui/Gwen/Controls/ScrollControl.cpp @@ -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 ); diff --git a/btgui/Gwen/Controls/TreeControl.cpp b/btgui/Gwen/Controls/TreeControl.cpp index 5a6f71477..bb53413fe 100644 --- a/btgui/Gwen/Controls/TreeControl.cpp +++ b/btgui/Gwen/Controls/TreeControl.cpp @@ -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 (newAmountm_VerticalScrollBar->SetScrolledAmount(newAmount,true); + float newAmount = float(minCoordSelectedItem)/(contSize-viewSize); + if (newAmountm_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 (newAmountm_VerticalScrollBar->SetScrolledAmount(newAmount,true); + float newAmount = float(minCoordSelectedItem)/(contSize-viewSize); + if (newAmountm_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 (newAmountm_VerticalScrollBar->SetScrolledAmount(newAmount,true); + float newAmount = float(minCoordSelectedItem)/(contSize-viewSize); + if (newAmountm_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 (newAmountm_VerticalScrollBar->SetScrolledAmount(newAmount,true); + float newAmount = float(minCoordSelectedItem)/(contSize-viewSize); + if (newAmountm_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 diff --git a/btgui/urdf/urdfdom/urdf_parser/src/model.cpp b/btgui/urdf/urdfdom/urdf_parser/src/model.cpp index 7c9d7e9c7..e8562d09b 100644 --- a/btgui/urdf/urdfdom/urdf_parser/src/model.cpp +++ b/btgui/urdf/urdfdom/urdf_parser/src/model.cpp @@ -116,6 +116,7 @@ my_shared_ptr parseURDF(const std::string &xml_string) { my_shared_ptr link; link.reset(new Link); + model->m_numLinks++; try { parseLink(*link, link_xml); @@ -176,6 +177,7 @@ my_shared_ptr parseURDF(const std::string &xml_string) { my_shared_ptr joint; joint.reset(new Joint); + model->m_numJoints++; if (parseJoint(*joint, joint_xml)) { diff --git a/btgui/urdf/urdfdom_headers/urdf_model/include/urdf_model/link.h b/btgui/urdf/urdfdom_headers/urdf_model/include/urdf_model/link.h index 67a1647ec..22e64cf5a 100644 --- a/btgui/urdf/urdfdom_headers/urdf_model/include/urdf_model/link.h +++ b/btgui/urdf/urdfdom_headers/urdf_model/include/urdf_model/link.h @@ -223,6 +223,8 @@ public: std::vector > child_joints; std::vector > 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: diff --git a/btgui/urdf/urdfdom_headers/urdf_model/include/urdf_model/model.h b/btgui/urdf/urdfdom_headers/urdf_model/include/urdf_model/model.h index 5bd11a6d6..64c74de4e 100644 --- a/btgui/urdf/urdfdom_headers/urdf_model/include/urdf_model/model.h +++ b/btgui/urdf/urdfdom_headers/urdf_model/include/urdf_model/model.h @@ -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 root_link_; + int m_numLinks;//includes parent + int m_numJoints; }; diff --git a/data/r2d2.urdf b/data/r2d2.urdf new file mode 100644 index 000000000..aa6b9aede --- /dev/null +++ b/data/r2d2.urdf @@ -0,0 +1,414 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/Bullet3Common/b3FileUtils.h b/src/Bullet3Common/b3FileUtils.h new file mode 100644 index 000000000..9e43e623e --- /dev/null +++ b/src/Bullet3Common/b3FileUtils.h @@ -0,0 +1,105 @@ +#ifndef B3_FILE_UTILS_H +#define B3_FILE_UTILS_H + +#include +#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 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; } diff --git a/src/BulletDynamics/Featherstone/btMultiBody.h b/src/BulletDynamics/Featherstone/btMultiBody.h index cd029c661..2cf4bf7f2 100644 --- a/src/BulletDynamics/Featherstone/btMultiBody.h +++ b/src/BulletDynamics/Featherstone/btMultiBody.h @@ -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, diff --git a/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h b/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h index ad57a346d..79b7758b1 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h +++ b/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h @@ -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 diff --git a/src/BulletDynamics/Featherstone/btMultiBodyLink.h b/src/BulletDynamics/Featherstone/btMultiBodyLink.h index af95cd992..a793f510e 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyLink.h +++ b/src/BulletDynamics/Featherstone/btMultiBodyLink.h @@ -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